示例#1
0
        // constructor
	    // anchor, axis1 and axis2 are in world coordinate system
	    // axis1 must be orthogonal to axis2
        public UniversalConstraint(RigidBody rbA, RigidBody rbB, ref Vector3 anchor, ref Vector3 axis1, ref Vector3 axis2)
            : base(rbA, rbB, ref BulletGlobals.IdentityMatrix, ref BulletGlobals.IdentityMatrix, true)
        {
            m_anchor = anchor;
            m_axis1 = axis1;
            m_axis2 = axis2;
        
        
        	// build frame basis
	        // 6DOF constraint uses Euler angles and to define limits
	        // it is assumed that rotational order is :
	        // Z - first, allowed limits are (-PI,PI);
	        // new position of Y - second (allowed limits are (-PI/2 + epsilon, PI/2 - epsilon), where epsilon is a small positive number 
	        // used to prevent constraint from instability on poles;
	        // new position of X, allowed limits are (-PI,PI);
	        // So to simulate ODE Universal joint we should use parent axis as Z, child axis as Y and limit all other DOFs
	        // Build the frame in world coordinate system first
	        Vector3 zAxis = Vector3.Normalize(axis1);
	        Vector3 yAxis = Vector3.Normalize(axis2);
	        Vector3 xAxis = Vector3.Cross(yAxis,zAxis); // we want right coordinate system
	        Matrix frameInW = Matrix.Identity;
            MathUtil.SetBasis(ref frameInW,ref xAxis,ref yAxis,ref zAxis);
	        frameInW.Translation = anchor;
	        // now get constraint frame in local coordinate systems
			//m_frameInA = MathUtil.inverseTimes(rbA.getCenterOfMassTransform(),frameInW);
			//m_frameInB = MathUtil.inverseTimes(rbB.getCenterOfMassTransform(),frameInW);
			m_frameInA = MathUtil.BulletMatrixMultiply(Matrix.Invert(rbA.GetCenterOfMassTransform()), frameInW);
			m_frameInB = MathUtil.BulletMatrixMultiply(Matrix.Invert(rbB.GetCenterOfMassTransform()), frameInW);

	        // sei limits
	        SetLinearLowerLimit(Vector3.Zero);
	        SetLinearUpperLimit(Vector3.Zero);
	        SetAngularLowerLimit(new Vector3(0.0f, -MathUtil.SIMD_HALF_PI + UNIV_EPS, -MathUtil.SIMD_PI + UNIV_EPS));
	        SetAngularUpperLimit(new Vector3(0.0f,  MathUtil.SIMD_HALF_PI - UNIV_EPS,  MathUtil.SIMD_PI - UNIV_EPS));
        }
示例#2
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 Generic6DofConstraint(RigidBody rbB, ref Matrix frameInB, bool useLinearReferenceFrameB) : base(TypedConstraintType.D6_CONSTRAINT_TYPE,GetFixedBody(),rbB)
        {
            m_frameInB = frameInB;
            m_useLinearReferenceFrameA = useLinearReferenceFrameB;
            m_useOffsetForConstraintFrame = D6_USE_FRAME_OFFSET;
            m_flags = 0;
            m_linearLimits = new TranslationalLimitMotor();
            m_angularLimits[0] = new RotationalLimitMotor();
            m_angularLimits[1] = new RotationalLimitMotor();
            m_angularLimits[2] = new RotationalLimitMotor();

            ///not providing rigidbody A means implicitly using worldspace for body A
	        m_frameInA = MathUtil.BulletMatrixMultiply(rbB.GetCenterOfMassTransform(),m_frameInB);

            CalculateTransforms();
        }
示例#4
0
        // constructor
    	// anchor, axis1 and axis2 are in world coordinate system
	    // axis1 must be orthogonal to axis2
        public Hinge2Constraint(RigidBody rbA, RigidBody rbB, ref Vector3 anchor, ref Vector3 axis1, ref Vector3 axis2) : base(rbA,rbB,Matrix.Identity,Matrix.Identity,true)
        {
            m_anchor = anchor;
            m_axis1 = axis1;
            m_axis2 = axis2;
            // build frame basis
            // 6DOF constraint uses Euler angles and to define limits
            // it is assumed that rotational order is :
            // Z - first, allowed limits are (-PI,PI);
            // new position of Y - second (allowed limits are (-PI/2 + epsilon, PI/2 - epsilon), where epsilon is a small positive number 
            // used to prevent constraint from instability on poles;
            // new position of X, allowed limits are (-PI,PI);
            // So to simulate ODE Universal joint we should use parent axis as Z, child axis as Y and limit all other DOFs
            // Build the frame in world coordinate system first
            Vector3 zAxis = Vector3.Normalize(axis1);
            Vector3 xAxis = Vector3.Normalize(axis2);
            Vector3 yAxis = Vector3.Cross(zAxis,xAxis); // we want right coordinate system

            Matrix frameInW = Matrix.Identity;
            MathUtil.SetBasis(ref frameInW, ref xAxis, ref yAxis, ref zAxis);
            frameInW.Translation = anchor;

            // now get constraint frame in local coordinate systems
            m_frameInA = MathUtil.InverseTimes(rbA.GetCenterOfMassTransform(),frameInW);
            m_frameInB = MathUtil.InverseTimes(rbB.GetCenterOfMassTransform(), frameInW);
            // sei limits
            SetLinearLowerLimit(new Vector3(0.0f, 0.0f, -1.0f));
            SetLinearUpperLimit(new Vector3(0.0f, 0.0f, 1.0f));
            // like front wheels of a car
            SetAngularLowerLimit(new Vector3(1.0f, 0.0f, -MathUtil.SIMD_HALF_PI * 0.5f));
            SetAngularUpperLimit(new Vector3(-1.0f, 0.0f, MathUtil.SIMD_HALF_PI * 0.5f));
            // enable suspension
            EnableSpring(2, true);
            SetStiffness(2, MathUtil.SIMD_PI * MathUtil.SIMD_PI * 4.0f); // period 1 sec for 1 kilogramm weel :-)
            SetDamping(2, 0.01f);
            SetEquilibriumPoint();

        }
	    public Point2PointConstraint(RigidBody rbA,ref Vector3 pivotInA) : base(TypedConstraintType.POINT2POINT_CONSTRAINT_TYPE,rbA)
        {
            m_pivotInA = pivotInA;
            m_pivotInB = Vector3.Transform(pivotInA, rbA.GetCenterOfMassTransform());
        }
示例#6
0
        public HingeConstraint(RigidBody rbA, RigidBody rbB, ref Vector3 pivotInA, ref Vector3 pivotInB, ref Vector3 axisInA, ref Vector3 axisInB, bool useReferenceFrameA) : base(TypedConstraintType.HINGE_CONSTRAINT_TYPE,rbA,rbB)
        {
            m_angularOnly = false;
            m_enableAngularMotor = false;
            m_useOffsetForConstraintFrame = HINGE_USE_FRAME_OFFSET;

            m_useReferenceFrameA = useReferenceFrameA;
            
            m_rbAFrame.Translation = pivotInA;

			m_flags = 0;

            // since no frame is given, assume this to be zero angle and just pick rb transform axis
            Vector3 rbAxisA1 = MathUtil.MatrixColumn(rbA.GetCenterOfMassTransform(),0);

            Vector3 rbAxisA2 = Vector3.Zero;
            float projection = Vector3.Dot(axisInA,rbAxisA1);
            if (projection >= 1.0f - MathUtil.SIMD_EPSILON)
            {
                rbAxisA1 = -MathUtil.MatrixColumn(rbA.GetCenterOfMassTransform(),2);
                rbAxisA2 = MathUtil.MatrixColumn(rbA.GetCenterOfMassTransform(),1);
            }
            else if (projection <= -1.0f + MathUtil.SIMD_EPSILON)
            {
                rbAxisA1 = MathUtil.MatrixColumn(rbA.GetCenterOfMassTransform(),2);
                rbAxisA2 = MathUtil.MatrixColumn(rbA.GetCenterOfMassTransform(),1);
            }
            else
            {
                rbAxisA2 = Vector3.Cross(axisInA,rbAxisA1);
                rbAxisA1 = Vector3.Cross(rbAxisA2,axisInA);
            }

            MathUtil.SetBasis(ref m_rbAFrame,ref rbAxisA1,ref rbAxisA2,ref axisInA);

            Quaternion rotationArc = MathUtil.ShortestArcQuat(ref axisInA, ref axisInB);
            Vector3 rbAxisB1 = MathUtil.QuatRotate(ref rotationArc, ref rbAxisA1);
            Vector3 rbAxisB2 = Vector3.Cross(axisInB,rbAxisB1);

            m_rbBFrame.Translation = pivotInB;
            MathUtil.SetBasis(ref m_rbBFrame,ref rbAxisB1,ref rbAxisB2,ref axisInB);

            //start with free
            m_lowerLimit = 1f;
            m_upperLimit = -1f;
            m_biasFactor = 0.3f;
            m_relaxationFactor = 1.0f;
            m_limitSoftness = 0.9f;
            m_solveLimit = false;
            m_referenceSign = m_useReferenceFrameA ? -1f : 1f;
        }
示例#7
0
        public HingeConstraint(RigidBody rbA, ref Vector3 pivotInA, ref Vector3 axisInA, bool useReferenceFrameA) : base(TypedConstraintType.HINGE_CONSTRAINT_TYPE,rbA)
        {
            m_angularOnly = false;
            m_enableAngularMotor = false;
            m_useReferenceFrameA = useReferenceFrameA;
            m_useOffsetForConstraintFrame = HINGE_USE_FRAME_OFFSET;
			m_flags = 0;

            // since no frame is given, assume this to be zero angle and just pick rb transform axis
            // fixed axis in worldspace
            Vector3 rbAxisA1 = Vector3.Zero, rbAxisA2 = Vector3.Zero;
            TransformUtil.PlaneSpace1(ref axisInA, ref rbAxisA1, ref rbAxisA2); 

            m_rbAFrame.Translation = pivotInA;
            MathUtil.SetBasis(ref m_rbAFrame,ref rbAxisA1,ref rbAxisA2,ref axisInA);

            Vector3 axisInB = Vector3.TransformNormal(axisInA,rbA.GetCenterOfMassTransform());

            Quaternion rotationArc = MathUtil.ShortestArcQuat(ref axisInA, ref axisInB);
            Vector3 rbAxisB1 = MathUtil.QuatRotate(ref rotationArc, ref rbAxisA1);
            Vector3 rbAxisB2 = Vector3.Cross(axisInB,rbAxisB1);

            m_rbBFrame.Translation = Vector3.Transform(pivotInA,rbA.GetCenterOfMassTransform());
            MathUtil.SetBasis(ref m_rbBFrame, ref rbAxisB1, ref rbAxisB2, ref axisInB);

            //start with free
            m_lowerLimit = 1f;
            m_upperLimit = -1f;
            m_biasFactor = 0.3f;
            m_relaxationFactor = 1.0f;
            m_limitSoftness = 0.9f;
            m_solveLimit = false;
            m_referenceSign = m_useReferenceFrameA ? -1.0f : 1.0f;
        }
示例#8
0
 public SliderConstraint(RigidBody rbB, ref Matrix frameInB, bool useLinearReferenceFrameA)
     : base(TypedConstraintType.SLIDER_CONSTRAINT_TYPE, GetFixedBody(), rbB)
 {
     m_frameInB = frameInB;
     m_frameInA = MathUtil.BulletMatrixMultiply(rbB.GetCenterOfMassTransform(),m_frameInB);
     InitParams();
 }