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

            Matrix m1 = MathUtil.TransposeBasis(body1.GetCenterOfMassTransform());
            Matrix m2 = MathUtil.TransposeBasis(body2.GetCenterOfMassTransform());


            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(),Vector3.TransformNormal(body1.GetAngularVelocity(),m1),
                body2.GetLinearVelocity(),Vector3.TransformNormal(body2.GetAngularVelocity(),m2));
	        float a = jacDiagABInv;

            rel_vel = Vector3.Dot(normal,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;
            }
        }
        protected virtual void BuildAngularJacobian(ref JacobianEntry jacAngular, ref Vector3 jointAxisW)
        {
	        jacAngular = new JacobianEntry(jointAxisW,
                                      MathUtil.TransposeBasis(m_rbA.GetCenterOfMassTransform()),
                                      MathUtil.TransposeBasis(m_rbB.GetCenterOfMassTransform()),
                                      m_rbA.GetInvInertiaDiagLocal(),
                                      m_rbB.GetInvInertiaDiagLocal());

        }
        protected virtual void BuildLinearJacobian(
            ref JacobianEntry jacLinear, ref Vector3 normalWorld,
            ref Vector3 pivotAInW, ref Vector3 pivotBInW)
        {
	        jacLinear = new JacobianEntry(
                MathUtil.TransposeBasis(m_rbA.GetCenterOfMassTransform()),
                MathUtil.TransposeBasis(m_rbB.GetCenterOfMassTransform()),
                pivotAInW - m_rbA.GetCenterOfMassPosition(),
                pivotBInW - m_rbB.GetCenterOfMassPosition(),
                normalWorld,
                m_rbA.GetInvInertiaDiagLocal(),
                m_rbA.GetInvMass(),
                m_rbB.GetInvInertiaDiagLocal(),
                m_rbB.GetInvMass());
        }
	    //
	    // solve unilateral raint (equality, direct method)
	    //
        public void ResolveUnilateralPairConstraint(RigidBody body0, RigidBody body1, ref Matrix world2A,
                            ref Matrix world2B,
                            ref Vector3 invInertiaADiag,
                            float invMassA,
                            ref Vector3 linvelA, ref Vector3 angvelA,
                            ref Vector3 rel_posA1,
                            ref Vector3 invInertiaBDiag,
                            float invMassB,
                            ref Vector3 linvelB, ref Vector3 angvelB,
                            ref Vector3 rel_posA2,
                            float depthA, ref Vector3 normalA,
                            ref Vector3 rel_posB1, ref Vector3 rel_posB2,
                            float depthB, ref Vector3 normalB,
                            ref float imp0, ref float imp1)
        {
            //(void)linvelA;
            //(void)linvelB;
            //(void)angvelB;
            //(void)angvelA;

	        imp0 = 0f;
	        imp1 = 0f;

	        float len = System.Math.Abs(normalA.Length()) - 1f;
	        if (System.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 = Vector3.Dot(normalA,(body0.GetVelocityInLocalPoint(ref rel_posA1)-body1.GetVelocityInLocalPoint(ref rel_posA1)));
	        float vel1 = Vector3.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]
        }
Example #5
0
	// for two constraints on sharing two same rigidbodies (for example two contact points between two rigidbodies)
	public float GetNonDiagonal(JacobianEntry jacB,float massInvA,float massInvB)
	{
		JacobianEntry jacA = this;
		Vector3 lin = jacA.m_linearJointAxis * jacB.m_linearJointAxis;
		Vector3 ang0 = jacA.m_0MinvJt * jacB.m_aJ;
		Vector3 ang1 = jacA.m_1MinvJt * jacB.m_bJ;
		Vector3 lin0 = massInvA * lin ;
		Vector3 lin1 = massInvB * lin;
		Vector3 sum = ang0+ang1+lin0+lin1;
		return sum.X+sum.Y+sum.Z;
	}
Example #6
0
	// for two constraints on the same rigidbody (for example vehicle friction)
	public float GetNonDiagonal(JacobianEntry jacB, float massInvA)
	{
		JacobianEntry jacA = this;
		float lin = massInvA * Vector3.Dot(jacA.m_linearJointAxis,jacB.m_linearJointAxis);
		float ang = Vector3.Dot(jacA.m_0MinvJt,jacB.m_aJ);
		return lin + ang;
	}