C# btVector3.Add方法代码示例

		public static void AabbExpand( ref btVector3 aabbMin,
										   ref btVector3 aabbMax,
										   ref btVector3 expansionMin,
										   ref btVector3 expansionMax )
		{
			aabbMin.Add( ref expansionMin, out aabbMin );
			aabbMin.Add( ref expansionMax, out aabbMin );
		}

		public override void drawContactPoint( ref btVector3 PointOnB, ref btVector3 normalOnB, double distance, int lifeTime, ref btVector3 color )
		{
			btVector3 tmpD;
			PointOnB.Add( ref normalOnB, out tmpD );
			drawLine( ref PointOnB, ref tmpD, ref color, ref color );

		}

		public override void setMargin( double collisionMargin )
		{
			//correct the m_implicitShapeDimensions for the margin
			btVector3 oldMargin = new btVector3( getMargin(), getMargin(), getMargin() );
			btVector3 implicitShapeDimensionsWithMargin; oldMargin.Add( ref m_implicitShapeDimensions, out implicitShapeDimensionsWithMargin );

			base.setMargin( collisionMargin );
			btVector3 newMargin = new btVector3( getMargin(), getMargin(), getMargin() );
			implicitShapeDimensionsWithMargin.Sub( ref newMargin, out m_implicitShapeDimensions );

		}

		public override void getAabb( ref btTransform t, out btVector3 aabbMin, out btVector3 aabbMax )
		{
			btVector3 halfExtents = new btVector3( getRadius(), getRadius(), getRadius() );
			btVector3 tmp = halfExtents;
			halfExtents[m_upAxis] = getRadius() + getHalfHeight();

			halfExtents.Add( ref tmp, out halfExtents );

			btMatrix3x3 abs_b; t.m_basis.absolute( out abs_b );
			btVector3 extent; halfExtents.dot3( ref abs_b.m_el0, ref abs_b.m_el1, ref abs_b.m_el2, out extent );

			t.m_origin.Sub( ref extent, out aabbMin );
			t.m_origin.Add( ref extent, out aabbMax );
		}

		public override void localGetSupportingVertex( ref btVector3 vec, out btVector3 result )
		{
			coneLocalSupport( ref vec, out result );
			if( getMargin() != btScalar.BT_ZERO )
			{
				btVector3 vecnorm = vec;
				if( vecnorm.length2() < ( btScalar.SIMD_EPSILON * btScalar.SIMD_EPSILON ) )
				{
					vecnorm = btVector3.NegOne;
				}
				vecnorm.normalize();
				vecnorm.Mult( getMargin(), out vecnorm );
				result.Add( ref vecnorm, out result );
			}
		}

//.........这里部分代码省略.........
				do
				{
					uint next = 1 - m_current;
					sSimplex cs = m_current==0?m_simplices0:m_simplices1;
					sSimplex ns = next==0?m_simplices0:m_simplices1;
					/* Check zero							*/
					double rl = m_ray.length();
					if( rl < GJK_MIN_DISTANCE )
					{/* Touching or inside				*/
						m_status = eStatus._.Inside;
						break;
					}
					/* Append new vertice in -'v' direction	*/
					m_ray.Invert( out tmp );
					appendvertice( cs, ref tmp );
					btVector3 w = cs.c[cs.rank - 1].w;
					bool found = false;
					for( uint i = 0; i < 4; ++i )
					{
						w.Sub( ref lastw[i], out tmp );
						if( tmp.length2() < GJK_DUPLICATED_EPS )
						{ found = true; break; }
					}
					if( found )
					{/* Return old simplex				*/
						removevertice( cs );
						break;
					}
					else
					{/* Update lastw					*/
						lastw[clastw = ( clastw + 1 ) & 3] = w;
					}
					/* Check for termination				*/
					double omega = btVector3.btDot( ref m_ray, ref w ) / rl;
					alpha = btScalar.btMax( omega, alpha );
					if( ( ( rl - alpha ) - ( GJK_ACCURARY * rl ) ) <= 0 )
					{/* Return old simplex				*/
						removevertice( cs );
						break;
					}
					/* Reduce simplex						*/
					double[] weights = new double[4];
					uint mask = 0;
					switch( cs.rank )
					{
						case 2:
							sqdist = projectorigin( ref cs.c[0].w,
								ref cs.c[1].w,
								weights, out mask ); break;
						case 3:
							sqdist = projectorigin( ref cs.c[0].w,
								ref cs.c[1].w,
								ref cs.c[2].w,
								weights, out mask ); break;
						case 4:
							sqdist = projectorigin( ref cs.c[0].w,
								ref cs.c[1].w,
								ref cs.c[2].w,
								ref cs.c[3].w,
								weights, out mask ); break;
					}
					if( sqdist >= 0 )
					{/* Valid	*/
						ns.rank = 0;
						m_ray = btVector3.Zero;
						m_current = next;
						for( int i = 0, ni = (int)cs.rank; i < ni; ++i )
						{
							if( ( mask & ( (uint)1 << i ) ) != 0 )
							{
								ns.c[ns.rank] = cs.c[i];
								ns.p[ns.rank++] = weights[i];
								btVector3 tmp2;
								cs.c[i].w.Mult( weights[i], out tmp2 );
								m_ray.Add( ref tmp2, out m_ray );
							}
							else
							{
								m_free[m_nfree++] = cs.c[i];
							}
						}
						if( mask == 15 ) m_status = eStatus._.Inside;
					}
					else
					{/* Return old simplex				*/
						removevertice( cs );
						break;
					}
					m_status = ( ( ++iterations ) < GJK_MAX_ITERATIONS ) ? m_status : eStatus._.Failed;
				} while( m_status == eStatus._.Valid );
				m_simplex = m_current==0?m_simplices0:m_simplices1;
				switch( m_status )
				{
					case eStatus._.Valid: m_distance = m_ray.length(); break;
					case eStatus._.Inside: m_distance = 0; break;
					default:
						break;
				}
				return ( m_status );
			}

		public void getAabbNonVirtual( ref btTransform t, out btVector3 aabbMin, out btVector3 aabbMax )
		{
			switch( m_shapeType )
			{
				case BroadphaseNativeTypes.SPHERE_SHAPE_PROXYTYPE:
					{
						btSphereShape sphereShape = (btSphereShape)this;
						double radius = sphereShape.getImplicitShapeDimensions().x;// * convexShape.getLocalScaling().x;
						double margin = radius + sphereShape.getMarginNonVirtual();
						btVector3 extent = new btVector3( margin, margin, margin );
						t.m_origin.Sub( ref extent, out aabbMin );
						t.m_origin.Add( ref extent, out aabbMax );
					}
					break;
				case BroadphaseNativeTypes.CYLINDER_SHAPE_PROXYTYPE:
				/* fall through */
				case BroadphaseNativeTypes.BOX_SHAPE_PROXYTYPE:
					{
						btBoxShape convexShape = (btBoxShape)this;
						double margin = convexShape.getMarginNonVirtual();
						btVector3 halfExtents = convexShape.getImplicitShapeDimensions();
						btVector3 tmp = new btVector3( margin, margin, margin );
						halfExtents.Add( ref tmp, out halfExtents );
						btMatrix3x3 abs_b; t.m_basis.absolute( out abs_b );
						btVector3 extent; halfExtents.dot3( ref abs_b.m_el0, ref abs_b.m_el1, ref abs_b.m_el2, out extent );

						t.m_origin.Sub( ref extent, out aabbMin );
						t.m_origin.Add( ref extent, out aabbMax );
						break;
					}
				case BroadphaseNativeTypes.TRIANGLE_SHAPE_PROXYTYPE:
					{
						btTriangleShape triangleShape = (btTriangleShape)this;
						double margin = triangleShape.getMarginNonVirtual();
						aabbMax = aabbMin = btVector3.Zero;
						for( int i = 0; i < 3; i++ )
						{
							btVector3 vec = btVector3.Zero;
							vec[i] = (double)( 1.0 );
							btVector3 tmp;
							t.m_basis.ApplyInverse( ref vec, out tmp );
							btVector3 sv; localGetSupportVertexWithoutMarginNonVirtual( ref tmp, out sv );

							t.Apply( ref sv, out tmp );

							aabbMax[i] = tmp[i] + margin;
							vec[i] = (double)( -1.0);

							t.m_basis.ApplyInverse( ref vec, out tmp );
                            localGetSupportVertexWithoutMarginNonVirtual( ref tmp, out sv );
							t.Apply( ref sv, out tmp );
							aabbMin[i] = tmp[i] - margin;
						}
					}
					break;
				case BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE:
					{
						btCapsuleShape capsuleShape = (btCapsuleShape)this;
						btVector3 halfExtents = new btVector3( capsuleShape.getRadius(), capsuleShape.getRadius(), capsuleShape.getRadius());
						int m_upAxis = capsuleShape.getUpAxis();
						halfExtents[m_upAxis] = capsuleShape.getRadius() + capsuleShape.getHalfHeight();
						btVector3 tmp = new btVector3( capsuleShape.getMarginNonVirtual(), capsuleShape.getMarginNonVirtual(), capsuleShape.getMarginNonVirtual() );
                        halfExtents.Add( ref tmp, out halfExtents );
						btMatrix3x3 abs_b; t.m_basis.absolute( out abs_b );
						btVector3 extent; halfExtents.dot3( ref abs_b.m_el0, ref abs_b.m_el1, ref abs_b.m_el2, out extent );
						t.m_origin.Sub( ref extent, out aabbMin );
						t.m_origin.Add( ref extent, out aabbMax );
					}
					break;
				case BroadphaseNativeTypes.CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE:
				case BroadphaseNativeTypes.CONVEX_HULL_SHAPE_PROXYTYPE:
					{
						btPolyhedralConvexAabbCachingShape convexHullShape = (btPolyhedralConvexAabbCachingShape)this;
						double margin = convexHullShape.getMarginNonVirtual();
						convexHullShape.getNonvirtualAabb( ref t, out aabbMin, out aabbMax, margin );
					}
					break;
				default:
					getAabb( ref t, out aabbMin, out aabbMax );
					break;
			}

			// should never reach here
			Debug.Assert( false );
			aabbMin = aabbMax = btVector3.Zero;
		}

		public void localGetSupportVertexNonVirtual( ref btVector3 localDir, out btVector3 result )
		{
			btVector3 localDirNorm = localDir;
			if( localDirNorm.length2() < ( btScalar.SIMD_EPSILON * btScalar.SIMD_EPSILON ) )
			{
				localDirNorm = btVector3.NegOne;
			}
			localDirNorm.normalize();
			localDirNorm.Mult( getMarginNonVirtual(), out localDirNorm );
			localGetSupportVertexWithoutMarginNonVirtual( ref localDirNorm, out result );
			result.Add( ref localDirNorm, out result );
		}

		public void getHalfExtentsWithMargin( out btVector3 halfExtents )
		{
			getHalfExtentsWithoutMargin( out halfExtents );
			btVector3 margin = new btVector3( getMargin(), getMargin(), getMargin() );
			halfExtents.Add( ref margin, out halfExtents );
		}

		public void PlaneLineIntersection( ref btVector3 p0, ref btVector3 p1, out btVector3 result )
		{
			// returns the point where the line p0-p1 intersects the plane n&d
			btVector3 dif;
			p1.Sub( ref p0, out dif );
			double dn = normal.dot( ref dif );
			double t = -( dist + normal.dot( ref p0 ) ) / dn;
			dif.Mult( t, out dif );
			p0.Add( ref dif, out result );
		}

		public static double DistanceBetweenLines( ref btVector3 ustart, ref btVector3 udir
			, ref btVector3 vstart, ref btVector3 vdir
			, out btVector3 upoint, out btVector3 vpoint )
		{
			btVector3 cp;
			udir.cross( ref vdir, out cp );
			cp.normalize();

			double distu = -cp.dot( ref ustart );
			double distv = -cp.dot( ref vstart );
			double dist = btScalar.btFabs( distu - distv );
			btPlane plane;
			btVector3 tmp;
			//if( upoint )
			{
				vdir.cross( ref cp, out plane.normal );
				plane.normal.normalize();
				plane.dist = -plane.normal.dot( ref vstart );
				ustart.Add( ref udir, out tmp );
				plane.PlaneLineIntersection( ref ustart, ref tmp, out upoint );
			}
			//if( vpoint )
			{
				udir.cross( ref cp, out plane.normal );
				plane.normal.normalize();
				plane.dist = -plane.normal.dot( ref ustart );
				ustart.Add( ref vdir, out tmp );
				plane.PlaneLineIntersection( ref vstart, ref tmp, out vpoint );
			}
			return dist;
		}

		public bool closestPtPointTriangle( ref btVector3 p, ref btVector3 a, ref btVector3 b, ref btVector3 c, ref btSubSimplexClosestResult result )
		{
			result.m_usedVertices = 0;//.reset();

			// Check if P in vertex region outside A
			btVector3 ab; b.Sub( ref a, out ab );
			btVector3 ac; c.Sub( ref a, out ac );
			btVector3 ap; p.Sub( ref a, out ap );
			double d1 = ab.dot( ref ap );
			double d2 = ac.dot( ref ap );
			if( d1 <= (double)( 0.0 ) && d2 <= (double)( 0.0 ) )
			{
				result.m_closestPointOnSimplex = a;
				result.m_usedVertices |= btUsageBitfield.usedVertexA;
				result.setBarycentricCoordinates( 1, 0, 0 );
				return true;// a; // barycentric coordinates (1,0,0)
			}

			// Check if P in vertex region outside B
			btVector3 bp; p.Sub( ref b, out bp );
			double d3 = ab.dot( ref bp );
			double d4 = ac.dot( ref bp );
			if( d3 >= (double)( 0.0 ) && d4 <= d3 )
			{
				result.m_closestPointOnSimplex = b;
				result.m_usedVertices |= btUsageBitfield.usedVertexB;
				result.setBarycentricCoordinates( 0, 1, 0 );

				return true; // b; // barycentric coordinates (0,1,0)
			}
			// Check if P in edge region of AB, if so return projection of P onto AB
			double vc = d1 * d4 - d3 * d2;
			btVector3 tmp;
			if( vc <= (double)( 0.0 ) && d1 >= (double)( 0.0 ) && d3 <= (double)( 0.0 ) )
			{
				double v = d1 / ( d1 - d3 );
				ab.Mult( v, out tmp );
				a.Add( ref tmp, out result.m_closestPointOnSimplex );
				result.m_usedVertices |= btUsageBitfield.usedVertexA;
				result.m_usedVertices |= btUsageBitfield.usedVertexB;
				result.setBarycentricCoordinates( 1 - v, v, 0 );
				return true;
				//return a + v * ab; // barycentric coordinates (1-v,v,0)
			}

			// Check if P in vertex region outside C
			btVector3 cp; p.Sub( ref c, out cp );
			double d5 = ab.dot( ref cp );
			double d6 = ac.dot( ref cp );
			if( d6 >= (double)( 0.0 ) && d5 <= d6 )
			{
				result.m_closestPointOnSimplex = c;
				result.m_usedVertices |= btUsageBitfield.usedVertexC;
				result.setBarycentricCoordinates( 0, 0, 1 );
				return true;//c; // barycentric coordinates (0,0,1)
			}

			// Check if P in edge region of AC, if so return projection of P onto AC
			double vb = d5 * d2 - d1 * d6;
			if( vb <= (double)( 0.0 ) && d2 >= (double)( 0.0 ) && d6 <= (double)( 0.0 ) )
			{
				double w = d2 / ( d2 - d6 );
				ac.Mult( w, out tmp );
				a.Add( ref tmp, out result.m_closestPointOnSimplex );
				result.m_usedVertices |= btUsageBitfield.usedVertexA;
				result.m_usedVertices |= btUsageBitfield.usedVertexC;
				result.setBarycentricCoordinates( 1 - w, 0, w );
				return true;
				//return a + w * ac; // barycentric coordinates (1-w,0,w)
			}

			// Check if P in edge region of BC, if so return projection of P onto BC
			double va = d3 * d6 - d5 * d4;
			if( va <= (double)( 0.0 ) && ( d4 - d3 ) >= (double)( 0.0 ) && ( d5 - d6 ) >= (double)( 0.0 ) )
			{
				double w = ( d4 - d3 ) / ( ( d4 - d3 ) + ( d5 - d6 ) );
				btVector3 tmp2;
				c.Sub( ref b, out tmp2 );
				tmp2.Mult( w, out tmp );
				b.Add( ref tmp, out result.m_closestPointOnSimplex );
				//result.m_closestPointOnSimplex = b + w * ( c - b );
				result.m_usedVertices |= btUsageBitfield.usedVertexB;
				result.m_usedVertices |= btUsageBitfield.usedVertexC;
				result.setBarycentricCoordinates( 0, 1 - w, w );
				return true;
				// return b + w * (c - b); // barycentric coordinates (0,1-w,w)
			}

			// P inside face region. Compute Q through its barycentric coordinates (u,v,w)
			{
				double denom = (double)( 1.0 ) / ( va + vb + vc );
				double v = vb * denom;
				double w = vc * denom;
				ab.Mult( v, out ab );
				ac.Mult( w, out ac );
				a.Add( ref ab, out tmp );
				tmp.Add( ref ac, out result.m_closestPointOnSimplex );

				//result.m_closestPointOnSimplex = a + ab * v + ac * w;
				result.m_usedVertices |= btUsageBitfield.usedVertexA;
//.........这里部分代码省略.........

		///calculateTemporalAabb calculates the enclosing aabb for the moving object over interval [0..timeStep)
		///result is conservative
		public void calculateTemporalAabb( ref btTransform curTrans, ref btVector3 linvel, ref btVector3 angvel, double timeStep
			, out btVector3 temporalAabbMin, out btVector3 temporalAabbMax )
		{
			//start with static aabb
			getAabb( ref curTrans, out temporalAabbMin, out temporalAabbMax );

			double temporalAabbMaxx = temporalAabbMax.x;
			double temporalAabbMaxy = temporalAabbMax.y;
			double temporalAabbMaxz = temporalAabbMax.z;
			double temporalAabbMinx = temporalAabbMin.x;
			double temporalAabbMiny = temporalAabbMin.y;
			double temporalAabbMinz = temporalAabbMin.z;

			// add linear motion
			btVector3 linMotion;
			linvel.Mult( timeStep, out linMotion );
			///@todo: simd would have a vector max/min operation, instead of per-element access
			if( linMotion.x > btScalar.BT_ZERO )
				temporalAabbMaxx += linMotion.x;
			else
				temporalAabbMinx += linMotion.x;
			if( linMotion.y > btScalar.BT_ZERO )
				temporalAabbMaxy += linMotion.y;
			else
				temporalAabbMiny += linMotion.y;
			if( linMotion.z > btScalar.BT_ZERO )
				temporalAabbMaxz += linMotion.z;
			else
				temporalAabbMinz += linMotion.z;

			//add conservative angular motion
			double angularMotion = angvel.length() * getAngularMotionDisc() * timeStep;
			btVector3 angularMotion3d = new btVector3( angularMotion, angularMotion, angularMotion );
			temporalAabbMin = new btVector3( temporalAabbMinx, temporalAabbMiny, temporalAabbMinz );
			temporalAabbMax = new btVector3( temporalAabbMaxx, temporalAabbMaxy, temporalAabbMaxz );
			temporalAabbMin.Sub( ref angularMotion3d, out temporalAabbMin );
			temporalAabbMax.Add( ref angularMotion3d, out temporalAabbMax );
		}