C# RigidBody.GetCenterOfMassPosition方法代码示例

		public float SolveLinearAxis(
			float timeStep,
			float jacDiagABInv,
			RigidBody body1, ref IndexedVector3 pointInA,
			RigidBody body2, ref IndexedVector3 pointInB,
			int limit_index,
			ref IndexedVector3 axis_normal_on_a,
			ref IndexedVector3 anchorPos)
		{
			///find relative velocity
			//    IndexedVector3 rel_pos1 = pointInA - body1.getCenterOfMassPosition();
			//    IndexedVector3 rel_pos2 = pointInB - body2.getCenterOfMassPosition();
			IndexedVector3 rel_pos1 = anchorPos - body1.GetCenterOfMassPosition();
			IndexedVector3 rel_pos2 = anchorPos - body2.GetCenterOfMassPosition();

			IndexedVector3 vel1 = IndexedVector3.Zero;
			body1.InternalGetVelocityInLocalPointObsolete(ref rel_pos1, ref vel1);
			IndexedVector3 vel2 = IndexedVector3.Zero; ;
			body2.InternalGetVelocityInLocalPointObsolete(ref rel_pos2, ref vel2);
			IndexedVector3 vel = vel1 - vel2;

			float rel_vel = IndexedVector3.Dot(axis_normal_on_a, vel);

			/// apply displacement correction

			//positional error (zeroth order error)
			float depth = -IndexedVector3.Dot((pointInA - pointInB), axis_normal_on_a);
			float lo = float.MinValue;
			float hi = float.MaxValue;

			float minLimit = m_lowerLimit[limit_index];
			float maxLimit = m_upperLimit[limit_index];

			//handle the limits
			if (minLimit < maxLimit)
			{
				{
					if (depth > maxLimit)
					{
						depth -= maxLimit;
						lo = 0f;

					}
					else
					{
						if (depth < minLimit)
						{
							depth -= minLimit;
							hi = 0f;
						}
						else
						{
							return 0.0f;
						}
					}
				}
			}

			float normalImpulse = m_limitSoftness * (m_restitution * depth / timeStep - m_damping * rel_vel) * jacDiagABInv;

			float oldNormalImpulse = m_accumulatedImpulse[limit_index];
			float sum = oldNormalImpulse + normalImpulse;
			m_accumulatedImpulse[limit_index] =  (sum > hi ? 0f : sum < lo ? 0f : sum);
			normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse;

			IndexedVector3 impulse_vector = axis_normal_on_a * normalImpulse;
			//body1.applyImpulse( impulse_vector, rel_pos1);
			//body2.applyImpulse(-impulse_vector, rel_pos2);

			IndexedVector3 ftorqueAxis1 = IndexedVector3.Cross(rel_pos1, axis_normal_on_a);
			IndexedVector3 ftorqueAxis2 = IndexedVector3.Cross(rel_pos2, axis_normal_on_a);
            body1.InternalApplyImpulse(axis_normal_on_a * body1.GetInvMass(), body1.GetInvInertiaTensorWorld() * ftorqueAxis1, normalImpulse, "Generic6DoF body1");
            body2.InternalApplyImpulse(axis_normal_on_a * body2.GetInvMass(), body2.GetInvInertiaTensorWorld() * ftorqueAxis2, -normalImpulse, "Generic6DoF body2");

			return normalImpulse;

		}

        public void UpdateWheel(RigidBody chassis, ref WheelRaycastInfo raycastInfo)
        {
	        if (m_raycastInfo.m_isInContact)
	        {
		        float project= IndexedVector3.Dot(m_raycastInfo.m_contactNormalWS,m_raycastInfo.m_wheelDirectionWS );
		        IndexedVector3	chassis_velocity_at_contactPoint;
		        IndexedVector3 relpos = m_raycastInfo.m_contactPointWS - chassis.GetCenterOfMassPosition();
		        chassis_velocity_at_contactPoint = chassis.GetVelocityInLocalPoint( ref relpos );
		        float projVel = IndexedVector3.Dot(m_raycastInfo.m_contactNormalWS,chassis_velocity_at_contactPoint );
		        if ( project >= -0.1f)
		        {
			        m_suspensionRelativeVelocity = 0f;
			        m_clippedInvContactDotSuspension = 1.0f / 0.1f;
		        }
		        else
		        {
			        float inv = -1f / project;
			        m_suspensionRelativeVelocity = projVel * inv;
			        m_clippedInvContactDotSuspension = inv;
		        }
	        }
	        else	// Not in contact : position wheel in a nice (rest length) position
	        {
		        m_raycastInfo.m_suspensionLength = this.GetSuspensionRestLength();
		        m_suspensionRelativeVelocity = 0f;
		        m_raycastInfo.m_contactNormalWS = -m_raycastInfo.m_wheelDirectionWS;
		        m_clippedInvContactDotSuspension = 1f;
	        }
        }

		///bilateral constraint between two dynamic objects
		///positive distance = separation, negative distance = penetration
		public static void ResolveSingleBilateral(RigidBody body1, ref IndexedVector3 pos1,
							  RigidBody body2, ref IndexedVector3 pos2,
							  float distance, ref IndexedVector3 normal, ref float impulse, float timeStep)
		{
			float normalLenSqr = normal.LengthSquared();
			Debug.Assert(Math.Abs(normalLenSqr) < 1.1f);
			if (normalLenSqr > 1.1f)
			{
				impulse = 0f;
				return;
			}
			IndexedVector3 rel_pos1 = pos1 - body1.GetCenterOfMassPosition();
			IndexedVector3 rel_pos2 = pos2 - body2.GetCenterOfMassPosition();
			//this jacobian entry could be re-used for all iterations

			IndexedVector3 vel1 = body1.GetVelocityInLocalPoint(ref rel_pos1);
			IndexedVector3 vel2 = body2.GetVelocityInLocalPoint(ref rel_pos2);
			IndexedVector3 vel = vel1 - vel2;

            IndexedBasisMatrix m1 = body1.GetCenterOfMassTransform()._basis.Transpose();
            IndexedBasisMatrix m2 = body2.GetCenterOfMassTransform()._basis.Transpose();


			JacobianEntry jac = new JacobianEntry(m1, m2, rel_pos1, rel_pos2, normal,
				body1.GetInvInertiaDiagLocal(), body1.GetInvMass(),
				body2.GetInvInertiaDiagLocal(), body2.GetInvMass());

			float jacDiagAB = jac.GetDiagonal();
			float jacDiagABInv = 1f / jacDiagAB;


			float rel_vel = jac.GetRelativeVelocity(
				body1.GetLinearVelocity(),
                body1.GetCenterOfMassTransform()._basis.Transpose() * body1.GetAngularVelocity(),
                body2.GetLinearVelocity(),
                body2.GetCenterOfMassTransform()._basis.Transpose() * body2.GetAngularVelocity());
            float a = jacDiagABInv;

			rel_vel = normal.Dot(ref vel);

			//todo: move this into proper structure
			float contactDamping = 0.2f;

			if (ONLY_USE_LINEAR_MASS)
			{
				float massTerm = 1f / (body1.GetInvMass() + body2.GetInvMass());
				impulse = -contactDamping * rel_vel * massTerm;
			}
			else
			{
				float velocityImpulse = -contactDamping * rel_vel * jacDiagABInv;
				impulse = velocityImpulse;
			}
		}