예제 #1
0
        public void Reset()
        {
            m_relpos1CrossNormal = IndexedVector3.Zero;
            m_contactNormal = IndexedVector3.Zero;
            m_relpos2CrossNormal = IndexedVector3.Zero;
            m_angularComponentA = IndexedVector3.Zero;
            m_angularComponentB = IndexedVector3.Zero;
            m_appliedPushImpulse = 0f;
            m_appliedImpulse = 0f;
            m_friction = 0f;
            m_jacDiagABInv = 0f;
            m_numConsecutiveRowsPerKernel = 0;
            m_frictionIndex = 0;
            m_solverBodyA = null;
            m_companionIdA = 0;
            m_solverBodyB = null;
            m_companionIdB = 0;
            m_originalContactPoint = null;
            //m_originalContactPointConstraint = null;
            m_rhs = 0f;
            m_cfm = 0;
            m_lowerLimit = 0f;
            m_upperLimit = 0f;
            m_rhsPenetration = 0f;
            m_overrideNumSolverIterations = -1;

        }
 public RaycastVehicle(VehicleTuning tuning, RigidBody chassis, IVehicleRaycaster raycaster)
 {
     m_vehicleRaycaster = raycaster;
     m_pitchControl = 0f;
     m_chassisBody = chassis;
     m_indexRightAxis = 0;
     m_indexUpAxis = 1;
     m_indexForwardAxis = 2;
     DefaultInit(ref tuning);
 }
예제 #3
0
		public Generic6DofConstraint(RigidBody rbA, RigidBody rbB, ref IndexedMatrix frameInA, ref IndexedMatrix frameInB, bool useLinearReferenceFrameA)
			: base(TypedConstraintType.D6_CONSTRAINT_TYPE, rbA, rbB)
		{
			m_frameInA = frameInA;
			m_frameInB = frameInB;
			m_useLinearReferenceFrameA = useLinearReferenceFrameA;
			m_useOffsetForConstraintFrame = D6_USE_FRAME_OFFSET;
			m_linearLimits = new TranslationalLimitMotor();
			m_angularLimits[0] = new RotationalLimitMotor();
			m_angularLimits[1] = new RotationalLimitMotor();
			m_angularLimits[2] = new RotationalLimitMotor();
			CalculateTransforms();
		}
예제 #4
0
		public Generic6DofConstraint(RigidBody rbB, ref IndexedMatrix frameInB, bool useLinearReferenceFrameB)
			: base(TypedConstraintType.D6_CONSTRAINT_TYPE, GetFixedBody(), rbB)
		{
			m_frameInB = frameInB;
			m_useLinearReferenceFrameA = useLinearReferenceFrameB;
			m_useOffsetForConstraintFrame = D6_USE_FRAME_OFFSET;
			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 = rbB.GetCenterOfMassTransform() * m_frameInB;

			CalculateTransforms();
		}
		//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;
		}
        // constructor
    	// anchor, axis1 and axis2 are in world coordinate system
	    // axis1 must be orthogonal to axis2
        public Hinge2Constraint(RigidBody rbA, RigidBody rbB, ref IndexedVector3 anchor, ref IndexedVector3 axis1, ref IndexedVector3 axis2) : base(rbA,rbB,IndexedMatrix.Identity,IndexedMatrix.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
            IndexedVector3 zAxis = IndexedVector3.Normalize(axis1);
            IndexedVector3 xAxis = IndexedVector3.Normalize(axis2);
            IndexedVector3 yAxis = IndexedVector3.Cross(zAxis,xAxis); // we want right coordinate system

            IndexedMatrix frameInW = IndexedMatrix.Identity;
            frameInW._basis = new IndexedBasisMatrix(xAxis.X, yAxis.X, zAxis.X,
                                    xAxis.Y, yAxis.Y, zAxis.Y,
                                   xAxis.Z, yAxis.Z, zAxis.Z);
            frameInW._origin = anchor;

            // now get constraint frame in local coordinate systems
            m_frameInA = rbA.GetCenterOfMassTransform().Inverse() * frameInW;
            m_frameInB = rbB.GetCenterOfMassTransform().Inverse() * frameInW;
            // sei limits
            SetLinearLowerLimit(new IndexedVector3(0.0f, 0.0f, -1.0f));
            SetLinearUpperLimit(new IndexedVector3(0.0f, 0.0f, 1.0f));
            // like front wheels of a car
            SetAngularLowerLimit(new IndexedVector3(1.0f, 0.0f, -MathUtil.SIMD_HALF_PI * 0.5f));
            SetAngularUpperLimit(new IndexedVector3(-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();

        }
예제 #7
0
        // constructor
	    // anchor, axis1 and axis2 are in world coordinate system
	    // axis1 must be orthogonal to axis2
        public UniversalConstraint(RigidBody rbA, RigidBody rbB, ref IndexedVector3 anchor, ref IndexedVector3 axis1, ref IndexedVector3 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
	        IndexedVector3 zAxis = IndexedVector3.Normalize(m_axis1);
	        IndexedVector3 yAxis = IndexedVector3.Normalize(m_axis2);
	        IndexedVector3 xAxis = IndexedVector3.Cross(yAxis,zAxis); // we want right coordinate system
	        IndexedMatrix frameInW = IndexedMatrix.Identity;
            frameInW._basis = new IndexedBasisMatrix(xAxis.X, yAxis.X, zAxis.X,
                                    xAxis.Y, yAxis.Y, zAxis.Y,
                                    xAxis.Z, yAxis.Z, zAxis.Z);
            frameInW._origin = 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 = rbA.GetCenterOfMassTransform().Inverse() * frameInW;
			m_frameInB = rbB.GetCenterOfMassTransform().Inverse() * frameInW;

	        // sei limits
	        SetLinearLowerLimit(IndexedVector3.Zero);
	        SetLinearUpperLimit(IndexedVector3.Zero);
	        SetAngularLowerLimit(new IndexedVector3(0.0f, -MathUtil.SIMD_HALF_PI + UNIV_EPS, -MathUtil.SIMD_PI + UNIV_EPS));
	        SetAngularUpperLimit(new IndexedVector3(0.0f,  MathUtil.SIMD_HALF_PI - UNIV_EPS,  MathUtil.SIMD_PI - UNIV_EPS));
        }
    	///internal method used by the constraint solver, don't use them directly
	    public virtual	void SolveConstraintObsolete(RigidBody bodyA,RigidBody bodyB,float timeStep) 
        {

        }
		public TypedConstraint(TypedConstraintType type, RigidBody rbA, RigidBody rbB)
			: base((int)type)
		{
			m_userConstraintType = -1;
			m_userConstraintId = -1;
			m_constraintType = type;
			m_rbA = rbA;
			m_rbB = rbB;
			m_appliedImpulse = 0f;
			m_breakingImpulseThreshold = MathUtil.SIMD_INFINITY;
			m_isEnabled = true;
			m_dbgDrawSize = DEFAULT_DEBUGDRAW_SIZE;
			{
				GetFixedBody().SetMassProps(0f, IndexedVector3.Zero);
			}

		}
예제 #10
0
		public SliderConstraint(RigidBody rbB, ref IndexedMatrix frameInB, bool useLinearReferenceFrameA)
			: base(TypedConstraintType.SLIDER_CONSTRAINT_TYPE, GetFixedBody(), rbB)
		{
			m_frameInB = frameInB;
			m_frameInA = rbB.GetCenterOfMassTransform() *  m_frameInB;
			InitParams();
		}
예제 #11
0
        public void BuildCollisionData(Color[] map, int width, int height, Vector3 bottomLeft)
        {
            Vector2 dim   = Vector2.One;
            Vector3 scale = new Vector3(dim, 1);

            BoxShape collisionBoxShape = new BoxShape(new IndexedVector3(0.5f));

            for (int x = 0; x < width; x++)
            {
                for (int y = 0; y < height; y++)
                {
                    int currentIndex = x + y * width;
                    if (map[currentIndex] == Color.White)
                    {
                        float   yOffset  = -dim.Y;//0f;// -(dim.Y / 2f);
                        Vector3 position = new Vector3(x - bottomLeft.X, (bottomLeft.Y - y) + yOffset, bottomLeft.Z);
                        m_waterLocations.Add(position);
                    }

                    if (map[currentIndex] == Color.White)
                    {
                        // check the 4 ordinals , if we're surrounded by other solid blocks
                        // then we don't need a block here.
                        bool upSet    = false;
                        bool downSet  = false;
                        bool leftSet  = false;
                        bool rightSet = false;

                        if (x >= 1 && x < width - 1)
                        {
                            if (map[currentIndex - 1] == Color.White)
                            {
                                leftSet = true;
                            }
                            if (map[currentIndex + 1] == Color.White)
                            {
                                rightSet = true;
                            }
                        }

                        if (y >= 1 && y < height - 1)
                        {
                            if (map[currentIndex - height] == Color.White)
                            {
                                upSet = true;
                            }
                            if (map[currentIndex + height] == Color.White)
                            {
                                downSet = true;
                            }
                        }

                        // if we're not surrounded by blocks then add in.
                        if (!(upSet && downSet && leftSet && rightSet))
                        {
                            Object  rigifdBody;
                            float   yOffset  = -dim.Y;//0f;// -(dim.Y / 2f);
                            Vector3 position = new Vector3(x - bottomLeft.X, (bottomLeft.Y - y) + yOffset, bottomLeft.Z);

                            RigidBodyConstructionInfo constructionInfo = new BulletXNA.BulletDynamics.RigidBodyConstructionInfo(0f, null, (BulletXNA.BulletCollision.CollisionShape)collisionBoxShape);
                            RigidBody rigidBody = new BulletXNA.BulletDynamics.RigidBody(constructionInfo);
                            Matrix    bsm       = Matrix.CreateTranslation(position);
                            rigidBody.SetWorldTransform(bsm);
                            // FIXME MAN - setup some collision flags on these bodies...
                            BulletXNA.BulletCollision.CollisionFilterGroups flags = (BulletXNA.BulletCollision.CollisionFilterGroups)(1 << 8);
                            BulletXNA.BulletCollision.CollisionFilterGroups mask  = (BulletXNA.BulletCollision.CollisionFilterGroups)(1 << 9);
                            //rigidBody.CollisionFlags |= (BulletSharp.CollisionFlags)CollisionObjectType.Ground;
                            m_dynamicsWorld.AddRigidBody(rigidBody, flags, mask);
                        }
                    }

                    // Build water ghost objects.
                    foreach (Vector3 pos in m_waterLocations)
                    {
                        GhostObject ghostObject = new GhostObject();

                        ghostObject.SetCollisionShape((BulletXNA.BulletCollision.CollisionShape)collisionBoxShape);

                        CollisionFilterGroups flags = (CollisionFilterGroups)(1 << 10);
                        CollisionFilterGroups mask  = (CollisionFilterGroups)(1 << 9);

                        ghostObject.SetCollisionFlags(CollisionFlags.CF_NO_CONTACT_RESPONSE | CollisionFlags.CF_STATIC_OBJECT);         // We can choose to make it "solid" if we want...
                        ghostObject.SetWorldTransform(BulletXNA.LinearMath.IndexedMatrix.CreateTranslation(pos));
                        m_dynamicsWorld.AddCollisionObject(ghostObject, flags, mask);
                        break;
                    }
                }
            }
        }
        public HingeConstraint(RigidBody rbA, RigidBody rbB, ref IndexedVector3 pivotInA, ref IndexedVector3 pivotInB, ref IndexedVector3 axisInA, ref IndexedVector3 axisInB)
			: this(rbA, rbB, ref pivotInA, ref pivotInB, ref axisInA, ref axisInB, false)
		{
		}
		public HingeConstraint(RigidBody rbA, ref IndexedMatrix rbAFrame)
			: this(rbA, ref rbAFrame, false)
		{
		}
예제 #14
0
 public Point2PointConstraint(RigidBody rbA, ref IndexedVector3 pivotInA) : base(TypedConstraintType.POINT2POINT_CONSTRAINT_TYPE, rbA)
 {
     m_pivotInA = pivotInA;
     m_pivotInB = rbA.GetCenterOfMassTransform() * pivotInA;
 }
예제 #15
0
    public override BulletBody CreateBodyWithDefaultMotionState( BulletShape pShape, uint pLocalID, Vector3 pRawPosition, Quaternion pRawOrientation)
    {

        IndexedMatrix mat =
            IndexedMatrix.CreateFromQuaternion(new IndexedQuaternion(pRawOrientation.X, pRawOrientation.Y,
                                                                     pRawOrientation.Z, pRawOrientation.W));
        mat._origin = new IndexedVector3(pRawPosition.X, pRawPosition.Y, pRawPosition.Z);

        CollisionShape shape = (pShape as BulletShapeXNA).shape;

        // TODO: Feed Update array into null
        RigidBody body = new RigidBody(0, new DefaultMotionState( mat, IndexedMatrix.Identity), shape, IndexedVector3.Zero);
        body.SetWorldTransform(mat);
        body.SetUserPointer(pLocalID);
        return new BulletBodyXNA(pLocalID, body);
    }
예제 #16
0
 public Point2PointConstraint(RigidBody rbA, RigidBody rbB, ref IndexedVector3 pivotInA, ref IndexedVector3 pivotInB)
     : base(TypedConstraintType.POINT2POINT_CONSTRAINT_TYPE, rbA, rbB)
 {
     m_pivotInA = pivotInA;
     m_pivotInB = pivotInB;
 }
		protected void ResolveSingleConstraintRowGeneric(RigidBody body1, RigidBody body2, ref SolverConstraint c)
		{
            m_genericCount++;
                float deltaImpulse = c.m_rhs - c.m_appliedImpulse * c.m_cfm;

                float deltaVel1Dotn = (c.m_contactNormal.X * body1.m_deltaLinearVelocity.X) + (c.m_contactNormal.Y * body1.m_deltaLinearVelocity.Y) + (c.m_contactNormal.Z * body1.m_deltaLinearVelocity.Z) +
                (c.m_relpos1CrossNormal.X * body1.m_deltaAngularVelocity.X) + (c.m_relpos1CrossNormal.Y * body1.m_deltaAngularVelocity.Y) + (c.m_relpos1CrossNormal.Z * body1.m_deltaAngularVelocity.Z);

                float deltaVel2Dotn = -((c.m_contactNormal.X * body2.m_deltaLinearVelocity.X) + (c.m_contactNormal.Y * body2.m_deltaLinearVelocity.Y) + (c.m_contactNormal.Z * body2.m_deltaLinearVelocity.Z)) +
                (c.m_relpos2CrossNormal.X * body2.m_deltaAngularVelocity.X) + (c.m_relpos2CrossNormal.Y * body2.m_deltaAngularVelocity.Y) + (c.m_relpos2CrossNormal.Z * body2.m_deltaAngularVelocity.Z);


                float originalDeltaImpulse = deltaImpulse;

                deltaImpulse -= deltaVel1Dotn * c.m_jacDiagABInv;
                deltaImpulse -= deltaVel2Dotn * c.m_jacDiagABInv;

#if DEBUG
                if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugSolver && false)
                {
                    BulletGlobals.g_streamWriter.WriteLine("ResolveSingleConstraintRowGeneric start [{0}][{1}][{2}][{3}].", originalDeltaImpulse, deltaVel1Dotn, deltaVel2Dotn, c.m_jacDiagABInv);
                }
#endif
                float sum = c.m_appliedImpulse + deltaImpulse;
                if (sum < c.m_lowerLimit)
                {
                    deltaImpulse = c.m_lowerLimit - c.m_appliedImpulse;
                    c.m_appliedImpulse = c.m_lowerLimit;
                }
                else if (sum > c.m_upperLimit)
                {
                    deltaImpulse = c.m_upperLimit - c.m_appliedImpulse;
                    c.m_appliedImpulse = c.m_upperLimit;
                }
                else
                {
                    c.m_appliedImpulse = sum;
                }

                IndexedVector3 temp = new IndexedVector3(c.m_contactNormal.X * body1.m_invMass.X, c.m_contactNormal.Y * body1.m_invMass.Y, c.m_contactNormal.Z * body1.m_invMass.Z);

                body1.InternalApplyImpulse(ref temp, ref c.m_angularComponentA, deltaImpulse, "ResolveSingleConstraintGeneric-body1");

                temp = new IndexedVector3(-c.m_contactNormal.X * body2.m_invMass.X, -c.m_contactNormal.Y * body2.m_invMass.Y, -c.m_contactNormal.Z * body2.m_invMass.Z);

                body2.InternalApplyImpulse(ref temp, ref c.m_angularComponentB, deltaImpulse, "ResolveSingleConstraintGeneric-body2");
		}
		public HingeConstraint(RigidBody rbA, ref IndexedVector3 pivotInA, ref IndexedVector3 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;

#if	_BT_USE_CENTER_LIMIT_
			m_limit = new AngularLimit();
#endif

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

			m_rbAFrame._origin = pivotInA;
            //m_rbAFrame._basis = new IndexedBasisMatrix(ref rbAxisA1, ref rbAxisA2, ref axisInA);
            m_rbAFrame._basis = new IndexedBasisMatrix(rbAxisA1.X, rbAxisA2.X, axisInA.X,
                                                rbAxisA1.Y, rbAxisA2.Y, axisInA.Y,
                                                rbAxisA1.Z, rbAxisA2.Z, axisInA.Z);

            IndexedVector3 axisInB = rbA.GetCenterOfMassTransform()._basis * axisInA;

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

            m_rbBFrame._origin = rbA.GetCenterOfMassTransform() * pivotInA;
            //m_rbBFrame._basis = new IndexedBasisMatrix(ref rbAxisB1, ref rbAxisB2, ref axisInB);
            m_rbBFrame._basis = new IndexedBasisMatrix(rbAxisB1.X, rbAxisB2.X, axisInB.X,
                        rbAxisB1.Y, rbAxisB2.Y, axisInB.Y,
                        rbAxisB1.Z, rbAxisB2.Z, axisInB.Z);

			//start with free
#if!	_BT_USE_CENTER_LIMIT_

            m_lowerLimit = 1f;
            m_upperLimit = -1f;
            m_biasFactor = 0.3f;
            m_relaxationFactor = 1.0f;
            m_limitSoftness = 0.9f;
            m_solveLimit = false;
#endif
			m_referenceSign = m_useReferenceFrameA ? -1.0f : 1.0f;
		}
		protected void ResolveSingleConstraintRowLowerLimit(RigidBody body1, RigidBody body2, ref SolverConstraint c)
		{
            m_lowerLimitCount++;
			//check magniture of applied impulse from SolverConstraint 
			float deltaImpulse = c.m_rhs - c.m_appliedImpulse * c.m_cfm;

            if (deltaImpulse > 200)
            {
                int ibreak = 0;
            }

            float deltaVel1Dotn = (c.m_contactNormal.X * body1.m_deltaLinearVelocity.X) + (c.m_contactNormal.Y * body1.m_deltaLinearVelocity.Y) + (c.m_contactNormal.Z * body1.m_deltaLinearVelocity.Z) +
            (c.m_relpos1CrossNormal.X * body1.m_deltaAngularVelocity.X) + (c.m_relpos1CrossNormal.Y * body1.m_deltaAngularVelocity.Y) + (c.m_relpos1CrossNormal.Z * body1.m_deltaAngularVelocity.Z);

            float deltaVel2Dotn = -((c.m_contactNormal.X * body2.m_deltaLinearVelocity.X) + (c.m_contactNormal.Y * body2.m_deltaLinearVelocity.Y) + (c.m_contactNormal.Z * body2.m_deltaLinearVelocity.Z)) +
            (c.m_relpos2CrossNormal.X * body2.m_deltaAngularVelocity.X) + (c.m_relpos2CrossNormal.Y * body2.m_deltaAngularVelocity.Y) + (c.m_relpos2CrossNormal.Z * body2.m_deltaAngularVelocity.Z);

			deltaImpulse -= deltaVel1Dotn * c.m_jacDiagABInv;
			deltaImpulse -= deltaVel2Dotn * c.m_jacDiagABInv;

			float sum = c.m_appliedImpulse + deltaImpulse;

			if (sum < c.m_lowerLimit)
			{
				deltaImpulse = c.m_lowerLimit - c.m_appliedImpulse;
				c.m_appliedImpulse = c.m_lowerLimit;
			}
			else
			{
				c.m_appliedImpulse = sum;
			}

            IndexedVector3 temp = new IndexedVector3(c.m_contactNormal.X * body1.m_invMass.X,c.m_contactNormal.Y * body1.m_invMass.Y,c.m_contactNormal.Z * body1.m_invMass.Z);
            body1.InternalApplyImpulse(ref temp, ref c.m_angularComponentA, deltaImpulse, "ResolveSingleConstraintRowLowerLimit-body1");

            temp = new IndexedVector3(-c.m_contactNormal.X * body2.m_invMass.X, -c.m_contactNormal.Y * body2.m_invMass.Y, -c.m_contactNormal.Z * body2.m_invMass.Z);
            body2.InternalApplyImpulse(ref temp, ref c.m_angularComponentB, deltaImpulse, "ResolveSingleConstraintRowLowerLimit-body2");

		}
		public HingeConstraint(RigidBody rbA, ref IndexedMatrix rbAFrame, bool useReferenceFrameA)
			: base(TypedConstraintType.HINGE_CONSTRAINT_TYPE, rbA)
		{
			m_rbAFrame = rbAFrame;
			m_rbBFrame = rbAFrame;
			m_angularOnly = false;
			m_enableAngularMotor = false;
			m_useOffsetForConstraintFrame = HINGE_USE_FRAME_OFFSET;

			m_useReferenceFrameA = useReferenceFrameA;
			m_flags = 0;

            m_rbBFrame._origin = m_rbA.GetCenterOfMassTransform() * (m_rbAFrame._origin);

#if	_BT_USE_CENTER_LIMIT_
			m_limit = new AngularLimit();
#else

            //start with free
            m_lowerLimit = 1f;
            m_upperLimit = -1f;
            m_biasFactor = 0.3f;
            m_relaxationFactor = 1.0f;
            m_limitSoftness = 0.9f;
            m_solveLimit = false;
#endif
			m_referenceSign = m_useReferenceFrameA ? -1.0f : 1.0f;

		}
		protected void ResolveSplitPenetrationImpulseCacheFriendly(
			RigidBody body1,
			RigidBody body2,
			ref SolverConstraint c)
		{
			if (c.m_rhsPenetration != 0f)
			{
				gNumSplitImpulseRecoveries++;
				float deltaImpulse = c.m_rhsPenetration - (c.m_appliedPushImpulse * c.m_cfm);
				float deltaVel1Dotn = IndexedVector3.Dot(c.m_contactNormal, body1.InternalGetPushVelocity()) + IndexedVector3.Dot(c.m_relpos1CrossNormal, body1.InternalGetTurnVelocity());
				float deltaVel2Dotn = -IndexedVector3.Dot(c.m_contactNormal, body2.InternalGetPushVelocity()) + IndexedVector3.Dot(c.m_relpos2CrossNormal, body2.InternalGetTurnVelocity());

				deltaImpulse -= deltaVel1Dotn * c.m_jacDiagABInv;
				deltaImpulse -= deltaVel2Dotn * c.m_jacDiagABInv;
				float sum = c.m_appliedPushImpulse + deltaImpulse;
				if (sum < c.m_lowerLimit)
				{
					deltaImpulse = c.m_lowerLimit - c.m_appliedPushImpulse;
					c.m_appliedPushImpulse = c.m_lowerLimit;
				}
				else
				{
					c.m_appliedPushImpulse = sum;
				}
				body1.InternalApplyPushImpulse(c.m_contactNormal * body1.InternalGetInvMass(), c.m_angularComponentA, deltaImpulse);
				body2.InternalApplyPushImpulse(-c.m_contactNormal * body2.InternalGetInvMass(), c.m_angularComponentB, deltaImpulse);
			}
		}
		public HingeConstraint(RigidBody rbA, RigidBody rbB, ref IndexedVector3 pivotInA, ref IndexedVector3 pivotInB, ref IndexedVector3 axisInA, ref IndexedVector3 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._origin = pivotInA;
#if	_BT_USE_CENTER_LIMIT_
			m_limit = new AngularLimit();
#endif


			m_flags = 0;

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

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

            
            //m_rbAFrame._basis = new IndexedBasisMatrix(ref rbAxisA1, ref rbAxisA2, ref axisInA);
            m_rbAFrame._basis = new IndexedBasisMatrix(rbAxisA1.X, rbAxisA2.X, axisInA.X,
                                    rbAxisA1.Y, rbAxisA2.Y, axisInA.Y,
                                    rbAxisA1.Z, rbAxisA2.Z, axisInA.Z);

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

			m_rbBFrame._origin = pivotInB;
            //m_rbBFrame._basis = new IndexedBasisMatrix(ref rbAxisB1, ref rbAxisB2, ref axisInB);
            m_rbBFrame._basis = new IndexedBasisMatrix(rbAxisB1.X, rbAxisB2.X, axisInB.X,
                                    rbAxisB1.Y, rbAxisB2.Y, axisInB.Y,
                                    rbAxisB1.Z, rbAxisB2.Z, axisInB.Z);

#if!	_BT_USE_CENTER_LIMIT_
	//start with free
	m_lowerLimit = float(1.0f);
	m_upperLimit = float(-1.0f);
	m_biasFactor = 0.3f;
	m_relaxationFactor = 1.0f;
	m_limitSoftness = 0.9f;
	m_solveLimit = false;
#endif
			m_referenceSign = m_useReferenceFrameA ? -1f : 1f;
		}
		public static RigidBody GetFixedBody()
		{
			if (s_fixed == null)
			{
				s_fixed = new RigidBody(0f, null, null, IndexedVector3.Zero);
                s_fixed.SetUserPointer("SICS:Fixed");
			}
			s_fixed.SetMassProps(0f, IndexedVector3.Zero);
			return s_fixed;
		}
예제 #24
0
		//------------------------    

		// constructors
		public SliderConstraint(RigidBody rbA, RigidBody rbB, ref IndexedMatrix frameInA, ref IndexedMatrix frameInB, bool useLinearReferenceFrameA)
			: base(TypedConstraintType.SLIDER_CONSTRAINT_TYPE, rbA, rbB)
		{
			m_frameInA = frameInA;
			m_frameInB = frameInB;
			m_useLinearReferenceFrameA = useLinearReferenceFrameA;
			InitParams();

		}
		public void SetupFrictionConstraint(ref SolverConstraint solverConstraint, ref IndexedVector3 normalAxis, RigidBody solverBodyA, RigidBody solverBodyB,
						ManifoldPoint cp, ref IndexedVector3 rel_pos1, ref IndexedVector3 rel_pos2,
						CollisionObject colObj0, CollisionObject colObj1, float relaxation)
		{
			SetupFrictionConstraint(ref solverConstraint, ref normalAxis, solverBodyA, solverBodyB, cp, ref rel_pos1, ref rel_pos2, colObj0, colObj1, relaxation, 0f, 0f);
		}
예제 #26
0
        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;
	        }
        }
		public void SetupFrictionConstraint(ref SolverConstraint solverConstraint, ref IndexedVector3 normalAxis, RigidBody solverBodyA, RigidBody solverBodyB,
								ManifoldPoint cp, ref IndexedVector3 rel_pos1, ref IndexedVector3 rel_pos2,
								CollisionObject colObj0, CollisionObject colObj1, float relaxation,
								float desiredVelocity, float cfmSlip)
		{
			RigidBody body0 = RigidBody.Upcast(colObj0);
			RigidBody body1 = RigidBody.Upcast(colObj1);

			solverConstraint.m_contactNormal = normalAxis;

			solverConstraint.m_solverBodyA = body0 != null ? body0 : GetFixedBody();
			solverConstraint.m_solverBodyB = body1 != null ? body1 : GetFixedBody();

			solverConstraint.m_friction = cp.GetCombinedFriction();
#if DEBUG
            if (BulletGlobals.g_streamWriter != null && (body0 != null  || body1 != null) && BulletGlobals.debugSolver)
            {
                BulletGlobals.g_streamWriter.WriteLine("SetupFrictionConstraint[{0}][{1}]", (String)solverConstraint.m_solverBodyA.GetUserPointer(), (String)solverConstraint.m_solverBodyB.GetUserPointer());
                MathUtil.PrintContactPoint(BulletGlobals.g_streamWriter, cp);
            }
#endif

			solverConstraint.m_originalContactPoint = null;
            //solverConstraint.m_originalContactPointConstraint = null;
			solverConstraint.m_appliedImpulse = 0f;
			solverConstraint.m_appliedPushImpulse = 0f;




			{
				IndexedVector3 ftorqueAxis1 = IndexedVector3.Cross(rel_pos1, solverConstraint.m_contactNormal);
				solverConstraint.m_relpos1CrossNormal = ftorqueAxis1;
                solverConstraint.m_angularComponentA = body0 != null ? body0.GetInvInertiaTensorWorld() * ftorqueAxis1 * body0.GetAngularFactor() : IndexedVector3.Zero;
            }
			{
				IndexedVector3 ftorqueAxis1 = IndexedVector3.Cross(rel_pos2, -solverConstraint.m_contactNormal);
				solverConstraint.m_relpos2CrossNormal = ftorqueAxis1;
                solverConstraint.m_angularComponentB = body1 != null ? body1.GetInvInertiaTensorWorld() * ftorqueAxis1 * body1.GetAngularFactor() : IndexedVector3.Zero;
            }

#if COMPUTE_IMPULSE_DENOM
	        float denom0 = rb0.computeImpulseDenominator(pos1,solverConstraint.m_contactNormal);
	        float denom1 = rb1.computeImpulseDenominator(pos2,solverConstraint.m_contactNormal);
#else
			IndexedVector3 vec;
			float denom0 = 0f;
			float denom1 = 0f;
			if (body0 != null)
			{
				vec = IndexedVector3.Cross(solverConstraint.m_angularComponentA, rel_pos1);
				denom0 = body0.GetInvMass() + IndexedVector3.Dot(normalAxis, vec);
			}
			if (body1 != null)
			{
				vec = IndexedVector3.Cross(-solverConstraint.m_angularComponentB, rel_pos2);
				denom1 = body1.GetInvMass() + IndexedVector3.Dot(normalAxis, vec);
			}


#endif //COMPUTE_IMPULSE_DENOM
			float denom = relaxation / (denom0 + denom1);
			solverConstraint.m_jacDiagABInv = denom;
			MathUtil.SanityCheckFloat(solverConstraint.m_jacDiagABInv);
#if _USE_JACOBIAN
	        solverConstraint.m_jac =  new JacobianEntry (
		        ref rel_pos1,ref rel_pos2,ref solverConstraint.m_contactNormal,
		        body0.getInvInertiaDiagLocal(),
		        body0.getInvMass(),
		        body1.getInvInertiaDiagLocal(),
		        body1.getInvMass());
#endif //_USE_JACOBIAN


			{
				float rel_vel;
				float vel1Dotn = IndexedVector3.Dot(solverConstraint.m_contactNormal, body0 != null ? body0.GetLinearVelocity() : IndexedVector3.Zero)
					+ IndexedVector3.Dot(solverConstraint.m_relpos1CrossNormal, body0 != null ? body0.GetAngularVelocity() : IndexedVector3.Zero);
				float vel2Dotn = -IndexedVector3.Dot(solverConstraint.m_contactNormal, body1 != null ? body1.GetLinearVelocity() : IndexedVector3.Zero)
					+ IndexedVector3.Dot(solverConstraint.m_relpos2CrossNormal, body1 != null ? body1.GetAngularVelocity() : IndexedVector3.Zero);

				rel_vel = vel1Dotn + vel2Dotn;

				//float positionalError = 0f;

				float velocityError = desiredVelocity - rel_vel;
				float damper = 1f;
				float velocityImpulse = (velocityError * solverConstraint.m_jacDiagABInv) * damper;
				solverConstraint.m_rhs = velocityImpulse;
				solverConstraint.m_cfm = cfmSlip;
				solverConstraint.m_lowerLimit = 0;
				solverConstraint.m_upperLimit = 1e10f;
			}
		}
		public static RigidBody GetFixedBody()
		{
			//static btRigidBody s_fixed(0, 0,0);
			//s_fixed.setMassProps(float(0.),btVector3(float(0.),float(0.),float(0.)));
			if (s_fixed == null)
			{
				s_fixed = new RigidBody(0f, null, null, IndexedVector3.Zero);
			}
			IndexedVector3 inertia = IndexedVector3.Zero;
			s_fixed.SetMassProps(0f, ref inertia);
			return s_fixed;
		}
		public override void AddRigidBody(RigidBody body)
		{
			body.SetGravity(ref m_gravity);

			if (body.GetCollisionShape() != null)
			{
				AddCollisionObject(body);
			}

		}
예제 #30
0
    //(sim.ptr, shape.ptr, prim.LocalID, prim.RawPosition, prim.RawOrientation);

    public override BulletBody CreateBodyFromShape(BulletWorld pWorld, BulletShape pShape, uint pLocalID, Vector3 pRawPosition, Quaternion pRawOrientation)
    {
        CollisionWorld world = (pWorld as BulletWorldXNA).world;
        IndexedMatrix mat =
            IndexedMatrix.CreateFromQuaternion(new IndexedQuaternion(pRawOrientation.X, pRawOrientation.Y,
                                                                     pRawOrientation.Z, pRawOrientation.W));
        mat._origin = new IndexedVector3(pRawPosition.X, pRawPosition.Y, pRawPosition.Z);
        CollisionShape shape = (pShape as BulletShapeXNA).shape;
        //UpdateSingleAabb(world, shape);
        // TODO: Feed Update array into null
        SimMotionState motionState = new SimMotionState(this, pLocalID, mat, null);
        RigidBody body = new RigidBody(0,motionState,shape,IndexedVector3.Zero);
        RigidBodyConstructionInfo constructionInfo = new RigidBodyConstructionInfo(0, motionState, shape, IndexedVector3.Zero)
                                                         {
                                                             m_mass = 0
                                                         };
        /*
            m_mass = mass;
			m_motionState =motionState;
			m_collisionShape = collisionShape;
			m_localInertia = localInertia;
			m_linearDamping = 0f;
			m_angularDamping = 0f;
			m_friction = 0.5f;
			m_restitution = 0f;
			m_linearSleepingThreshold = 0.8f;
			m_angularSleepingThreshold = 1f;
			m_additionalDamping = false;
			m_additionalDampingFactor = 0.005f;
			m_additionalLinearDampingThresholdSqr = 0.01f;
			m_additionalAngularDampingThresholdSqr = 0.01f;
			m_additionalAngularDampingFactor = 0.01f;
            m_startWorldTransform = IndexedMatrix.Identity;
        */
        body.SetUserPointer(pLocalID);

        return new BulletBodyXNA(pLocalID, body);
    }
		public override void AddRigidBody(RigidBody body, CollisionFilterGroups group, CollisionFilterGroups mask)
		{
			body.SetGravity(ref m_gravity);

			if (body.GetCollisionShape() != null)
			{
				AddCollisionObject(body, group, mask);
			}
		}
예제 #32
0
 public override void SetRigidBody(RigidBody body)
 {
     Rigidbody = body;
 }
		public override void RemoveRigidBody(RigidBody body)
		{
			base.RemoveCollisionObject(body);
		}