C# RigidBody.GetVelocityInLocalPoint方法代码示例

		//response  between two dynamic objects without friction, assuming 0 penetration depth
		public static float ResolveSingleCollision(
				RigidBody body1,
				CollisionObject colObj2,
				ref IndexedVector3 contactPositionWorld,
				ref IndexedVector3 contactNormalOnB,
				ContactSolverInfo solverInfo,
				float distance)
		{
			RigidBody body2 = RigidBody.Upcast(colObj2);


			IndexedVector3 normal = contactNormalOnB;

			IndexedVector3 rel_pos1 = contactPositionWorld - body1.GetWorldTransform()._origin;
			IndexedVector3 rel_pos2 = contactPositionWorld - colObj2.GetWorldTransform()._origin;

			IndexedVector3 vel1 = body1.GetVelocityInLocalPoint(ref rel_pos1);
			IndexedVector3 vel2 = body2 != null ? body2.GetVelocityInLocalPoint(ref rel_pos2) : IndexedVector3.Zero;
			IndexedVector3 vel = vel1 - vel2;
			float rel_vel = normal.Dot(ref vel);

			float combinedRestitution = body1.GetRestitution() * colObj2.GetRestitution();
			float restitution = combinedRestitution * -rel_vel;

			float positionalError = solverInfo.m_erp * -distance / solverInfo.m_timeStep;
			float velocityError = -(1.0f + restitution) * rel_vel;// * damping;
			float denom0 = body1.ComputeImpulseDenominator(ref contactPositionWorld, ref normal);
			float denom1 = body2 != null ? body2.ComputeImpulseDenominator(ref contactPositionWorld, ref normal) : 0.0f;
			float relaxation = 1.0f;
			float jacDiagABInv = relaxation / (denom0 + denom1);

			float penetrationImpulse = positionalError * jacDiagABInv;
			float velocityImpulse = velocityError * jacDiagABInv;

			float normalImpulse = penetrationImpulse + velocityImpulse;
			normalImpulse = 0.0f > normalImpulse ? 0.0f : normalImpulse;

			body1.ApplyImpulse(normal * (normalImpulse), rel_pos1);
			if (body2 != null)
			{
				body2.ApplyImpulse(-normal * (normalImpulse), rel_pos2);
			}

			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;
	        }
        }

	    //
	    // solve unilateral raint (equality, direct method)
	    //
        public void ResolveUnilateralPairConstraint(RigidBody body0, RigidBody body1, ref IndexedBasisMatrix world2A,
                            ref IndexedBasisMatrix world2B,
                            ref IndexedVector3 invInertiaADiag,
                            float invMassA,
                            ref IndexedVector3 linvelA, ref IndexedVector3 angvelA,
                            ref IndexedVector3 rel_posA1,
                            ref IndexedVector3 invInertiaBDiag,
                            float invMassB,
                            ref IndexedVector3 linvelB, ref IndexedVector3 angvelB,
                            ref IndexedVector3 rel_posA2,
                            float depthA, ref IndexedVector3 normalA,
                            ref IndexedVector3 rel_posB1, ref IndexedVector3 rel_posB2,
                            float depthB, ref IndexedVector3 normalB,
                            out float imp0, out float imp1)
        {
            //(void)linvelA;
            //(void)linvelB;
            //(void)angvelB;
            //(void)angvelA;

	        imp0 = 0f;
	        imp1 = 0f;

	        float len = Math.Abs(normalA.Length()) - 1f;
	        if (Math.Abs(len) >= MathUtil.SIMD_EPSILON)
		        return;

	        Debug.Assert(len < MathUtil.SIMD_EPSILON);

	        //this jacobian entry could be re-used for all iterations
	        JacobianEntry jacA = new JacobianEntry(ref world2A,ref world2B,ref rel_posA1,ref rel_posA2,ref normalA,ref invInertiaADiag,invMassA,
		        ref invInertiaBDiag,invMassB);
	        JacobianEntry jacB = new JacobianEntry(ref world2A,ref world2B,ref rel_posB1,ref rel_posB2,ref normalB,ref invInertiaADiag,invMassA,
		        ref invInertiaBDiag,invMassB);
        	
	        // float vel0 = jacA.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
	        // float vel1 = jacB.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);

	        float vel0 = IndexedVector3.Dot(normalA,(body0.GetVelocityInLocalPoint(ref rel_posA1)-body1.GetVelocityInLocalPoint(ref rel_posA1)));
	        float vel1 = IndexedVector3.Dot(normalB,(body0.GetVelocityInLocalPoint(ref rel_posB1)-body1.GetVelocityInLocalPoint(ref rel_posB1)));

        //	float penetrationImpulse = (depth*contactTau*timeCorrection)  * massTerm;//jacDiagABInv
	        float massTerm = 1f / (invMassA + invMassB);


	        // calculate rhs (or error) terms
	        float dv0 = depthA  * m_tau * massTerm - vel0 * m_damping;
	        float dv1 = depthB  * m_tau * massTerm - vel1 * m_damping;


	        // dC/dv * dv = -C
        	
	        // jacobian * impulse = -error
	        //

	        //impulse = jacobianInverse * -error

	        // inverting 2x2 symmetric system (offdiagonal are equal!)
	        // 


	        float nonDiag = jacA.GetNonDiagonal(jacB,invMassA,invMassB);
	        float invDet = 1f / (jacA.GetDiagonal() * jacB.GetDiagonal() - nonDiag * nonDiag );
        	
	        //imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet;
	        //imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet;

	        imp0 = dv0 * jacA.GetDiagonal() * invDet + dv1 * -nonDiag * invDet;
	        imp1 = dv1 * jacB.GetDiagonal() * invDet + dv0 * - nonDiag * invDet;

	        //[a b]								  [d -c]
	        //[c d] inverse = (1 / determinant) * [-b a] where determinant is (ad - bc)

	        //[jA nD] * [imp0] = [dv0]
	        //[nD jB]   [imp1]   [dv1]
        }

		///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;
			}
		}