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