public override void GetInfo2(ConstraintInfo2 info) { if (m_useOffsetForConstraintFrame) { GetInfo2InternalUsingFrameOffset(info, m_rbA.GetCenterOfMassTransform(), m_rbB.GetCenterOfMassTransform(), m_rbA.GetAngularVelocity(), m_rbB.GetAngularVelocity()); } else { GetInfo2Internal(info, m_rbA.GetCenterOfMassTransform(), m_rbB.GetCenterOfMassTransform(), m_rbA.GetAngularVelocity(), m_rbB.GetAngularVelocity()); } }
public static void PrintInfo2(StreamWriter writer, TypedConstraint constraint, ConstraintInfo2 info2) { if (writer != null) { writer.WriteLine(String.Format("getInfo2 [{0}] [{1}] [{2}] [{3}]", constraint.m_userConstraintId, constraint.GetObjectType(), (string)constraint.GetRigidBodyA().GetUserPointer(), (string)constraint.GetRigidBodyB().GetUserPointer())); writer.WriteLine(String.Format("numRows [{0}] fps[{1:0.00000000}] erp[{2:0.00000000}] findex[{3}] numIter[{4}]", info2.m_numRows, info2.fps, info2.erp, info2.findex, info2.m_numIterations)); for (int i = 0; i < info2.m_numRows; ++i) { writer.WriteLine(String.Format("TypedConstraint[{0}]", i)); writer.WriteLine("ContactNormal"); MathUtil.PrintVector3(writer, info2.m_solverConstraints[i].m_contactNormal); writer.WriteLine("rel1pos1CrossNormal"); MathUtil.PrintVector3(writer, info2.m_solverConstraints[i].m_relpos1CrossNormal); writer.WriteLine("rel1pos2CrossNormal"); MathUtil.PrintVector3(writer, info2.m_solverConstraints[i].m_relpos2CrossNormal); } } }
public virtual void GetInfo2(ConstraintInfo2 info) { }
public void GetInfo2NonVirtual(ConstraintInfo2 info, IndexedMatrix transA, IndexedMatrix transB, IndexedVector3 linVelA, IndexedVector3 linVelB, float rbAinvMass, float rbBinvMass) { IndexedMatrix trA = GetCalculatedTransformA(); IndexedMatrix trB = GetCalculatedTransformB(); Debug.Assert(!m_useSolveConstraintObsolete); int i, s = 1; float signFact = m_useLinearReferenceFrameA ? 1.0f : -1.0f; // difference between frames in WCS IndexedVector3 ofs = trB._origin - trA._origin; // now get weight factors depending on masses float miA = rbAinvMass; float miB = rbBinvMass; bool hasStaticBody = (miA < MathUtil.SIMD_EPSILON) || (miB < MathUtil.SIMD_EPSILON); float miS = miA + miB; float factA, factB; if (miS > 0.0f) { factA = miB / miS; } else { factA = 0.5f; } factB = 1.0f - factA; IndexedVector3 ax1 = IndexedVector3.Zero, p, q; IndexedVector3 ax1A = trA._basis.GetColumn(0); IndexedVector3 ax1B = trB._basis.GetColumn(0); if (m_useOffsetForConstraintFrame) { // get the desired direction of slider axis // as weighted sum of X-orthos of frameA and frameB in WCS ax1 = ax1A * factA + ax1B * factB; ax1.Normalize(); // construct two orthos to slider axis TransformUtil.PlaneSpace1(ref ax1, out p, out q); } else { // old way - use frameA ax1 = trA._basis.GetColumn(0); // get 2 orthos to slider axis (Y, Z) p = trA._basis.GetColumn(1); q = trA._basis.GetColumn(2); } // make rotations around these orthos equal // the slider axis should be the only unconstrained // rotational axis, the angular velocity of the two bodies perpendicular to // the slider axis should be equal. thus the constraint equations are // p*w1 - p*w2 = 0 // q*w1 - q*w2 = 0 // where p and q are unit vectors normal to the slider axis, and w1 and w2 // are the angular velocity vectors of the two bodies. info.m_solverConstraints[0].m_relpos1CrossNormal = p; info.m_solverConstraints[s].m_relpos1CrossNormal = q; info.m_solverConstraints[0].m_relpos2CrossNormal = -p; info.m_solverConstraints[s].m_relpos2CrossNormal = -q; // compute the right hand side of the constraint equation. set relative // body velocities along p and q to bring the slider back into alignment. // if ax1A,ax1B are the unit length slider axes as computed from bodyA and // bodyB, we need to rotate both bodies along the axis u = (ax1 x ax2). // if "theta" is the angle between ax1 and ax2, we need an angular velocity // along u to cover angle erp*theta in one step : // |angular_velocity| = angle/time = erp*theta / stepsize // = (erp*fps) * theta // angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2| // = (erp*fps) * theta * (ax1 x ax2) / sin(theta) // ...as ax1 and ax2 are unit length. if theta is smallish, // theta ~= sin(theta), so // angular_velocity = (erp*fps) * (ax1 x ax2) // ax1 x ax2 is in the plane space of ax1, so we project the angular // velocity to p and q to find the right hand side. // float k = info.fps * info.erp * getSoftnessOrthoAng(); float currERP = ((m_flags & (int)SliderFlags.BT_SLIDER_FLAGS_ERP_ORTANG) != 0) ? m_softnessOrthoAng : m_softnessOrthoAng * info.erp; float k = info.fps * currERP; IndexedVector3 u = IndexedVector3.Cross(ax1A, ax1B); info.m_solverConstraints[0].m_rhs = k * IndexedVector3.Dot(u, p); info.m_solverConstraints[s].m_rhs = k * IndexedVector3.Dot(u, q); if ((m_flags & (int)SliderFlags.BT_SLIDER_FLAGS_CFM_ORTANG) != 0) { info.m_solverConstraints[0].m_cfm = m_cfmOrthoAng; info.m_solverConstraints[s].m_cfm = m_cfmOrthoAng; } int nrow = 1; // last filled row int srow = nrow; float limit_err; int limit; bool powered; // next two rows. // we want: velA + wA x relA == velB + wB x relB ... but this would // result in three equations, so we project along two orthos to the slider axis IndexedMatrix bodyA_trans = transA; IndexedMatrix bodyB_trans = transB; nrow++; int s2 = nrow * s; nrow++; int s3 = nrow * s; IndexedVector3 tmpA = IndexedVector3.Zero, tmpB = IndexedVector3.Zero, relA = IndexedVector3.Zero, relB = IndexedVector3.Zero, c = IndexedVector3.Zero; if (m_useOffsetForConstraintFrame) { // get vector from bodyB to frameB in WCS relB = trB._origin - bodyB_trans._origin; // get its projection to slider axis IndexedVector3 projB = ax1 * IndexedVector3.Dot(relB, ax1); // get vector directed from bodyB to slider axis (and orthogonal to it) IndexedVector3 orthoB = relB - projB; // same for bodyA relA = trA._origin - bodyA_trans._origin; IndexedVector3 projA = ax1 * IndexedVector3.Dot(relA, ax1); IndexedVector3 orthoA = relA - projA; // get desired offset between frames A and B along slider axis float sliderOffs = m_linPos - m_depth.X; // desired vector from projection of center of bodyA to projection of center of bodyB to slider axis IndexedVector3 totalDist = projA + ax1 * sliderOffs - projB; // get offset vectors relA and relB relA = orthoA + totalDist * factA; relB = orthoB - totalDist * factB; // now choose average ortho to slider axis p = orthoB * factA + orthoA * factB; float len2 = p.LengthSquared(); if (len2 > MathUtil.SIMD_EPSILON) { p.Normalize(); } else { p = trA._basis.GetColumn(1); } // make one more ortho q = IndexedVector3.Cross(ax1, p); // fill two rows tmpA = IndexedVector3.Cross(relA, p); tmpB = IndexedVector3.Cross(relB, p); info.m_solverConstraints[s2].m_relpos1CrossNormal = tmpA; info.m_solverConstraints[s2].m_relpos2CrossNormal = -tmpB; tmpA = IndexedVector3.Cross(relA, q); tmpB = IndexedVector3.Cross(relB, q); if (hasStaticBody && GetSolveAngLimit()) { // to make constraint between static and dynamic objects more rigid // remove wA (or wB) from equation if angular limit is hit tmpB *= factB; tmpA *= factA; } info.m_solverConstraints[s3].m_relpos1CrossNormal = tmpA; info.m_solverConstraints[s3].m_relpos2CrossNormal = -tmpB; info.m_solverConstraints[s2].m_contactNormal = p; info.m_solverConstraints[s3].m_contactNormal = q; } else { // old way - maybe incorrect if bodies are not on the slider axis // see discussion "Bug in slider constraint" http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?f=9&t=4024&start=0 IndexedVector3 tmp = IndexedVector3.Cross(c, p); info.m_solverConstraints[s2].m_relpos1CrossNormal = factA * tmp; info.m_solverConstraints[s2].m_relpos2CrossNormal = factB * tmp; tmp = IndexedVector3.Cross(c, q); info.m_solverConstraints[s3].m_relpos1CrossNormal = factA * tmp; info.m_solverConstraints[s3].m_relpos2CrossNormal = factB * tmp; info.m_solverConstraints[s2].m_contactNormal = p; info.m_solverConstraints[s3].m_contactNormal = q; } // compute two elements of right hand side // k = info.fps * info.erp * getSoftnessOrthoLin(); currERP = ((m_flags & (int)SliderFlags.BT_SLIDER_FLAGS_ERP_ORTLIN) != 0) ? m_softnessOrthoLin : m_softnessOrthoLin * info.erp; k = info.fps * currERP; float rhs = k * IndexedVector3.Dot(p, ofs); info.m_solverConstraints[s2].m_rhs = rhs; rhs = k * IndexedVector3.Dot(q, ofs); info.m_solverConstraints[s3].m_rhs = rhs; if ((m_flags & (int)SliderFlags.BT_SLIDER_FLAGS_CFM_ORTLIN) != 0) { info.m_solverConstraints[s2].m_cfm = m_cfmOrthoLin; info.m_solverConstraints[s3].m_cfm = m_cfmOrthoLin; } // check linear limits limit_err = 0.0f; limit = 0; if (GetSolveLinLimit()) { limit_err = GetLinDepth() * signFact; limit = (limit_err > 0f) ? 2 : 1; } powered = false; if (GetPoweredLinMotor()) { powered = true; } // if the slider has joint limits or motor, add in the extra row if (limit != 0 || powered) { nrow++; srow = nrow; info.m_solverConstraints[srow].m_contactNormal = ax1; // linear torque decoupling step: // // we have to be careful that the linear constraint forces (+/- ax1) applied to the two bodies // do not create a torque couple. in other words, the points that the // constraint force is applied at must lie along the same ax1 axis. // a torque couple will result in limited slider-jointed free // bodies from gaining angular momentum. if (m_useOffsetForConstraintFrame) { // this is needed only when bodyA and bodyB are both dynamic. if (!hasStaticBody) { tmpA = IndexedVector3.Cross(relA, ax1); tmpB = IndexedVector3.Cross(relB, ax1); info.m_solverConstraints[srow].m_relpos1CrossNormal = tmpA; info.m_solverConstraints[srow].m_relpos2CrossNormal = -tmpB; } } else { // The old way. May be incorrect if bodies are not on the slider axis IndexedVector3 ltd = IndexedVector3.Cross(c, ax1); // Linear Torque Decoupling vector (a torque) info.m_solverConstraints[nrow].m_relpos1CrossNormal = factA * ltd; info.m_solverConstraints[nrow].m_relpos2CrossNormal = factB * ltd; } // right-hand part float lostop = GetLowerLinLimit(); float histop = GetUpperLinLimit(); if (limit != 0 && (MathUtil.CompareFloat(lostop, histop))) { // the joint motor is ineffective powered = false; } info.m_solverConstraints[nrow].m_rhs = 0f; info.m_solverConstraints[nrow].m_lowerLimit = 0f; info.m_solverConstraints[nrow].m_upperLimit = 0f; currERP = ((m_flags & (int)SliderFlags.BT_SLIDER_FLAGS_ERP_LIMLIN) != 0) ? m_softnessLimLin : info.erp; if (powered) { if ((m_flags & (int)SliderFlags.BT_SLIDER_FLAGS_CFM_DIRLIN) != 0) { info.m_solverConstraints[nrow].m_cfm = m_cfmDirLin; } float tag_vel = GetTargetLinMotorVelocity(); float mot_fact = GetMotorFactor(m_linPos, m_lowerLinLimit, m_upperLinLimit, tag_vel, info.fps * currERP); info.m_solverConstraints[nrow].m_rhs -= signFact * mot_fact * GetTargetLinMotorVelocity(); info.m_solverConstraints[nrow].m_lowerLimit += -GetMaxLinMotorForce() * info.fps; info.m_solverConstraints[nrow].m_upperLimit += GetMaxLinMotorForce() * info.fps; } if (limit != 0) { k = info.fps * currERP; info.m_solverConstraints[nrow].m_rhs += k * limit_err; if ((m_flags & (int)SliderFlags.BT_SLIDER_FLAGS_CFM_LIMLIN) != 0) { info.m_solverConstraints[nrow].m_cfm = m_cfmLimLin; } if (MathUtil.CompareFloat(lostop, histop)) { // limited low and high simultaneously info.m_solverConstraints[nrow].m_lowerLimit = -MathUtil.SIMD_INFINITY; info.m_solverConstraints[nrow].m_upperLimit = MathUtil.SIMD_INFINITY; } else if (limit == 1) { // low limit info.m_solverConstraints[nrow].m_lowerLimit = -MathUtil.SIMD_INFINITY; info.m_solverConstraints[nrow].m_upperLimit = 0f; } else { // high limit info.m_solverConstraints[nrow].m_lowerLimit = 0f; info.m_solverConstraints[nrow].m_upperLimit = MathUtil.SIMD_INFINITY; } // bounce (we'll use slider parameter abs(1.0 - m_dampingLimLin) for that) float bounce = Math.Abs(1.0f - GetDampingLimLin()); if (bounce > 0.0f) { float vel = IndexedVector3.Dot(linVelA, ax1); vel -= IndexedVector3.Dot(linVelB, ax1); vel *= signFact; // only apply bounce if the velocity is incoming, and if the // resulting c[] exceeds what we already have. if (limit == 1) { // low limit if (vel < 0) { float newc = -bounce * vel; if (newc > info.m_solverConstraints[nrow].m_rhs) { info.m_solverConstraints[nrow].m_rhs = newc; } } } else { // high limit - all those computations are reversed if (vel > 0) { float newc = -bounce * vel; if (newc < info.m_solverConstraints[nrow].m_rhs) { info.m_solverConstraints[nrow].m_rhs = newc; } } } } info.m_solverConstraints[nrow].m_rhs *= GetSoftnessLimLin(); } // if(limit) } // if linear limit // check angular limits limit_err = 0.0f; limit = 0; if (GetSolveAngLimit()) { limit_err = GetAngDepth(); limit = (limit_err > 0.0f) ? 1 : 2; } // if the slider has joint limits, add in the extra row powered = false; if (GetPoweredAngMotor()) { powered = true; } if (limit != 0 || powered) { nrow++; srow = nrow; info.m_solverConstraints[srow].m_relpos1CrossNormal = ax1; info.m_solverConstraints[srow].m_relpos2CrossNormal = -ax1; float lostop = GetLowerAngLimit(); float histop = GetUpperAngLimit(); if (limit != 0 && (MathUtil.CompareFloat(lostop, histop))) { // the joint motor is ineffective powered = false; } currERP = ((m_flags & (int)SliderFlags.BT_SLIDER_FLAGS_ERP_LIMANG) != 0) ? m_softnessLimAng : info.erp; if (powered) { if ((m_flags & (int)SliderFlags.BT_SLIDER_FLAGS_CFM_DIRANG) != 0) { info.m_solverConstraints[nrow].m_cfm = m_cfmDirAng; } float mot_fact = GetMotorFactor(m_angPos, m_lowerAngLimit, m_upperAngLimit, GetTargetAngMotorVelocity(), info.fps * currERP); info.m_solverConstraints[nrow].m_rhs = mot_fact * GetTargetAngMotorVelocity(); info.m_solverConstraints[nrow].m_lowerLimit = -GetMaxAngMotorForce() * info.fps; info.m_solverConstraints[nrow].m_upperLimit = GetMaxAngMotorForce() * info.fps; } if (limit != 0) { k = info.fps * currERP; info.m_solverConstraints[nrow].m_rhs += k * limit_err; if ((m_flags & (int)SliderFlags.BT_SLIDER_FLAGS_CFM_LIMANG) != 0) { info.m_solverConstraints[nrow].m_cfm = m_cfmLimAng; } if (MathUtil.CompareFloat(lostop, histop)) { // limited low and high simultaneously info.m_solverConstraints[nrow].m_lowerLimit = -MathUtil.SIMD_INFINITY; info.m_solverConstraints[nrow].m_upperLimit = MathUtil.SIMD_INFINITY; } else if (limit == 1) { // low limit info.m_solverConstraints[nrow].m_lowerLimit = 0; info.m_solverConstraints[nrow].m_upperLimit = MathUtil.SIMD_INFINITY; } else { // high limit info.m_solverConstraints[nrow].m_lowerLimit = -MathUtil.SIMD_INFINITY; info.m_solverConstraints[nrow].m_upperLimit = 0; } // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that) float bounce = Math.Abs(1.0f - GetDampingLimAng()); if (bounce > 0.0f) { float vel = IndexedVector3.Dot(m_rbA.GetAngularVelocity(), ax1); vel -= IndexedVector3.Dot(m_rbB.GetAngularVelocity(), ax1); // only apply bounce if the velocity is incoming, and if the // resulting c[] exceeds what we already have. if (limit == 1) { // low limit if (vel < 0) { float newc = -bounce * vel; if (newc > info.m_solverConstraints[nrow].m_rhs) { info.m_solverConstraints[nrow].m_rhs = newc; } } } else { // high limit - all those computations are reversed if (vel > 0) { float newc = -bounce * vel; if (newc < info.m_solverConstraints[nrow].m_rhs) { info.m_solverConstraints[nrow].m_rhs = newc; } } } } info.m_solverConstraints[nrow].m_rhs *= GetSoftnessLimAng(); } // if(limit) } // if angular limit or powered #if DEBUG if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugConstraints) { PrintInfo2(BulletGlobals.g_streamWriter, this, info); } #endif }
public override void GetInfo2(ConstraintInfo2 info) { GetInfo2NonVirtual(info, m_rbA.GetCenterOfMassTransform(), m_rbB.GetCenterOfMassTransform(), m_rbA.GetLinearVelocity(), m_rbB.GetLinearVelocity(), m_rbA.GetInvMass(), m_rbB.GetInvMass()); }
public void TestLinLimits2(ConstraintInfo2 info) { }
public void GetInfo2InternalUsingFrameOffset(ConstraintInfo2 info, ref IndexedMatrix transA, ref IndexedMatrix transB, ref IndexedVector3 angVelA, ref IndexedVector3 angVelB) { // transforms in world space #if DEBUG if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugConstraints) { MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "rbAFrame", m_rbAFrame); MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "rbBFrame", m_rbBFrame); MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "transA", transA); MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "transB", transB); } #endif IndexedMatrix trA = transA * m_rbAFrame; IndexedMatrix trB = transB * m_rbBFrame; #if DEBUG if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugConstraints) { MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "trA", trA); MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "trB", trB); } #endif // pivot point IndexedVector3 pivotAInW = trA._origin; IndexedVector3 pivotBInW = trB._origin; #if true // difference between frames in WCS IndexedVector3 ofs = trB._origin - trA._origin; // now get weight factors depending on masses float miA = GetRigidBodyA().GetInvMass(); float miB = GetRigidBodyB().GetInvMass(); bool hasStaticBody = (miA < MathUtil.SIMD_EPSILON) || (miB < MathUtil.SIMD_EPSILON); float miS = miA + miB; float factA, factB; if (miS > 0.0f) { factA = miB / miS; } else { factA = 0.5f; } factB = 1.0f - factA; // get the desired direction of hinge axis // as weighted sum of Z-orthos of frameA and frameB in WCS IndexedVector3 ax1A = trA._basis.GetColumn(2); IndexedVector3 ax1B = trB._basis.GetColumn(2); IndexedVector3 ax1 = ax1A * factA + ax1B * factB; ax1.Normalize(); // fill first 3 rows // we want: velA + wA x relA == velB + wB x relB IndexedMatrix bodyA_trans = transA; IndexedMatrix bodyB_trans = transB; int s0 = 0; int s1 = 1; int s2 = 2; int nrow = 2; // last filled row IndexedVector3 tmpA, tmpB, relA, relB, p, q; // get vector from bodyB to frameB in WCS relB = trB._origin - bodyB_trans._origin; // get its projection to hinge axis IndexedVector3 projB = ax1 * IndexedVector3.Dot(relB, ax1); // get vector directed from bodyB to hinge axis (and orthogonal to it) IndexedVector3 orthoB = relB - projB; // same for bodyA relA = trA._origin - bodyA_trans._origin; IndexedVector3 projA = ax1 * IndexedVector3.Dot(relA, ax1); IndexedVector3 orthoA = relA - projA; IndexedVector3 totalDist = projA - projB; // get offset vectors relA and relB relA = orthoA + totalDist * factA; relB = orthoB - totalDist * factB; // now choose average ortho to hinge axis p = orthoB * factA + orthoA * factB; float len2 = p.LengthSquared(); if (len2 > MathUtil.SIMD_EPSILON) { p.Normalize(); } else { p = trA._basis.GetColumn(1); } // make one more ortho q = IndexedVector3.Cross(ax1, p); // fill three rows tmpA = IndexedVector3.Cross(relA, p); tmpB = IndexedVector3.Cross(relB, p); info.m_solverConstraints[s0].m_relpos1CrossNormal = tmpA; info.m_solverConstraints[s0].m_relpos2CrossNormal = -tmpB; tmpA = IndexedVector3.Cross(ref relA, ref q); tmpB = IndexedVector3.Cross(ref relB, ref q); if (hasStaticBody && GetSolveLimit()) { // to make constraint between static and dynamic objects more rigid // remove wA (or wB) from equation if angular limit is hit tmpB *= factB; tmpA *= factA; } info.m_solverConstraints[s1].m_relpos1CrossNormal = tmpA; info.m_solverConstraints[s1].m_relpos2CrossNormal = -tmpB; tmpA = IndexedVector3.Cross(ref relA, ref ax1); tmpB = IndexedVector3.Cross(ref relB, ref ax1); if (hasStaticBody) { // to make constraint between static and dynamic objects more rigid // remove wA (or wB) from equation tmpB *= factB; tmpA *= factA; } info.m_solverConstraints[s2].m_relpos1CrossNormal = tmpA; info.m_solverConstraints[s2].m_relpos2CrossNormal = -tmpB; float k = info.fps * info.erp; if (!m_angularOnly) { info.m_solverConstraints[s0].m_contactNormal = p; info.m_solverConstraints[s1].m_contactNormal = q; info.m_solverConstraints[s2].m_contactNormal = ax1; // compute three elements of right hand side float rhs = k * IndexedVector3.Dot(ref p, ref ofs); info.m_solverConstraints[s0].m_rhs = rhs; rhs = k * IndexedVector3.Dot(ref q, ref ofs); info.m_solverConstraints[s1].m_rhs = rhs; rhs = k * IndexedVector3.Dot(ref ax1, ref ofs); info.m_solverConstraints[s2].m_rhs = rhs; } // the hinge axis should be the only unconstrained // rotational axis, the angular velocity of the two bodies perpendicular to // the hinge axis should be equal. thus the constraint equations are // p*w1 - p*w2 = 0 // q*w1 - q*w2 = 0 // where p and q are unit vectors normal to the hinge axis, and w1 and w2 // are the angular velocity vectors of the two bodies. int s3 = 3; int s4 = 4; info.m_solverConstraints[s3].m_relpos1CrossNormal = p; info.m_solverConstraints[s4].m_relpos1CrossNormal = q; info.m_solverConstraints[s3].m_relpos2CrossNormal = -p; info.m_solverConstraints[s4].m_relpos2CrossNormal = -q; // compute the right hand side of the constraint equation. set relative // body velocities along p and q to bring the hinge back into alignment. // if ax1A,ax1B are the unit length hinge axes as computed from bodyA and // bodyB, we need to rotate both bodies along the axis u = (ax1 x ax2). // if "theta" is the angle between ax1 and ax2, we need an angular velocity // along u to cover angle erp*theta in one step : // |angular_velocity| = angle/time = erp*theta / stepsize // = (erp*fps) * theta // angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2| // = (erp*fps) * theta * (ax1 x ax2) / sin(theta) // ...as ax1 and ax2 are unit length. if theta is smallish, // theta ~= sin(theta), so // angular_velocity = (erp*fps) * (ax1 x ax2) // ax1 x ax2 is in the plane space of ax1, so we project the angular // velocity to p and q to find the right hand side. k = info.fps * info.erp; IndexedVector3 u = IndexedVector3.Cross(ref ax1A, ref ax1B); info.m_solverConstraints[s3].m_rhs = k * IndexedVector3.Dot(u, p); info.m_solverConstraints[s4].m_rhs = k * IndexedVector3.Dot(u, q); #endif // check angular limits nrow = 4; // last filled row int srow; float limit_err = 0f; int limit = 0; if (GetSolveLimit()) { #if _BT_USE_CENTER_LIMIT_ limit_err = m_limit.GetCorrection() * m_referenceSign; #else limit_err = m_correction * m_referenceSign; #endif limit = (limit_err > 0f) ? 1 : 2; } // if the hinge has joint limits or motor, add in the extra row bool powered = false; if (GetEnableAngularMotor()) { powered = true; } if (limit != 0 || powered) { nrow++; srow = nrow; info.m_solverConstraints[srow].m_relpos1CrossNormal = ax1; info.m_solverConstraints[srow].m_relpos2CrossNormal = -ax1; float lostop = GetLowerLimit(); float histop = GetUpperLimit(); if (limit != 0 && (MathUtil.CompareFloat(lostop, histop))) { // the joint motor is ineffective powered = false; } info.m_solverConstraints[srow].m_rhs = 0f; float currERP = ((m_flags & (int)HingeFlags.BT_HINGE_FLAGS_ERP_STOP) != 0) ? m_stopERP : info.erp; if (powered) { if ((m_flags & (int)HingeFlags.BT_HINGE_FLAGS_CFM_NORM) != 0) { info.m_solverConstraints[srow].m_cfm = m_normalCFM; } float mot_fact = GetMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info.fps * currERP); info.m_solverConstraints[srow].m_rhs += mot_fact * m_motorTargetVelocity * m_referenceSign; info.m_solverConstraints[srow].m_lowerLimit = -m_maxMotorImpulse; info.m_solverConstraints[srow].m_upperLimit = m_maxMotorImpulse; } if (limit != 0) { k = info.fps * currERP; info.m_solverConstraints[srow].m_rhs += k * limit_err; if ((m_flags & (int)HingeFlags.BT_HINGE_FLAGS_CFM_STOP) != 0) { info.m_solverConstraints[srow].m_cfm = m_stopCFM; } if (MathUtil.CompareFloat(lostop, histop)) { // limited low and high simultaneously info.m_solverConstraints[srow].m_lowerLimit = -MathUtil.SIMD_INFINITY; info.m_solverConstraints[srow].m_upperLimit = MathUtil.SIMD_INFINITY; } else if (limit == 1) { // low limit info.m_solverConstraints[srow].m_lowerLimit = 0; info.m_solverConstraints[srow].m_upperLimit = MathUtil.SIMD_INFINITY; } else { // high limit info.m_solverConstraints[srow].m_lowerLimit = -MathUtil.SIMD_INFINITY; info.m_solverConstraints[srow].m_upperLimit = 0; } // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that) #if _BT_USE_CENTER_LIMIT_ float bounce = m_limit.GetRelaxationFactor(); #else float bounce = m_relaxationFactor; #endif if (bounce > 0f) { float vel = IndexedVector3.Dot(ref angVelA, ref ax1); vel -= IndexedVector3.Dot(ref angVelB, ref ax1); // only apply bounce if the velocity is incoming, and if the // resulting c[] exceeds what we already have. if (limit == 1) { // low limit if (vel < 0) { float newc = -bounce * vel; if (newc > info.m_solverConstraints[srow].m_rhs) { info.m_solverConstraints[srow].m_rhs = newc; } } } else { // high limit - all those computations are reversed if (vel > 0) { float newc = -bounce * vel; if (newc < info.m_solverConstraints[srow].m_rhs) { info.m_solverConstraints[srow].m_rhs = newc; } } } } #if _BT_USE_CENTER_LIMIT_ info.m_solverConstraints[srow].m_rhs *= m_limit.GetBiasFactor(); #else info.m_solverConstraints[srow].m_rhs *= m_biasFactor; #endif } // if(limit) } // if angular limit or powered #if DEBUG if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugConstraints) { PrintInfo2(BulletGlobals.g_streamWriter, this, info); } #endif }
public void GetInfo2InternalUsingFrameOffset(ConstraintInfo2 info, IndexedMatrix transA, IndexedMatrix transB, IndexedVector3 angVelA, IndexedVector3 angVelB) { GetInfo2InternalUsingFrameOffset(info, ref transA, ref transB, ref angVelA, ref angVelB); }
public void GetInfo2Internal(ConstraintInfo2 info, IndexedMatrix transA, IndexedMatrix transB, IndexedVector3 angVelA, IndexedVector3 angVelB) { // transforms in world space IndexedMatrix trA = transA * m_rbAFrame; IndexedMatrix trB = transB * m_rbBFrame; // pivot point IndexedVector3 pivotAInW = trA._origin; IndexedVector3 pivotBInW = trB._origin; // linear (all fixed) //info.m_J1linearAxis[0] = 1; //info.m_J1linearAxis[s + 1] = 1; //info.m_J1linearAxis[2 * s + 2] = 1; if (!m_angularOnly) { info.m_solverConstraints[0].m_contactNormal.X = 1f; info.m_solverConstraints[1].m_contactNormal.Y = 1f; info.m_solverConstraints[2].m_contactNormal.Z = 1f; } IndexedVector3 a1 = pivotAInW - transA._origin; { IndexedVector3 a1neg = -a1; MathUtil.GetSkewSymmetricMatrix(ref a1neg, out info.m_solverConstraints[0].m_relpos1CrossNormal, out info.m_solverConstraints[1].m_relpos1CrossNormal, out info.m_solverConstraints[2].m_relpos1CrossNormal); //if (info.m_solverConstraints[0].m_relpos1CrossNormal.X == 0.15) //{ // int ibreak = 0; //} int ibreak = 0; } IndexedVector3 a2 = pivotBInW - transB._origin; { MathUtil.GetSkewSymmetricMatrix(ref a2, out info.m_solverConstraints[0].m_relpos2CrossNormal, out info.m_solverConstraints[1].m_relpos2CrossNormal, out info.m_solverConstraints[2].m_relpos2CrossNormal); } // linear RHS float k = info.fps * info.erp; if (!m_angularOnly) { for (int i = 0; i < 3; i++) { float val = k * (pivotBInW[i] - pivotAInW[i]); info.m_solverConstraints[i].m_rhs = val; } } // make rotations around X and Y equal // the hinge axis should be the only unconstrained // rotational axis, the angular velocity of the two bodies perpendicular to // the hinge axis should be equal. thus the constraint equations are // p*w1 - p*w2 = 0 // q*w1 - q*w2 = 0 // where p and q are unit vectors normal to the hinge axis, and w1 and w2 // are the angular velocity vectors of the two bodies. // get hinge axis (Z) IndexedVector3 ax1 = trA._basis.GetColumn(2); // get 2 orthos to hinge axis (X, Y) IndexedVector3 p = trA._basis.GetColumn(0); IndexedVector3 q = trA._basis.GetColumn(1); // set the two hinge angular rows MathUtil.SanityCheckVector(ax1); MathUtil.SanityCheckVector(p); MathUtil.SanityCheckVector(q); int s3 = 3; int s4 = 4; info.m_solverConstraints[s3].m_relpos1CrossNormal = p; info.m_solverConstraints[s4].m_relpos1CrossNormal = q; info.m_solverConstraints[s3].m_relpos2CrossNormal = -p; info.m_solverConstraints[s4].m_relpos2CrossNormal = -q; // compute the right hand side of the constraint equation. set relative // body velocities along p and q to bring the hinge back into alignment. // if ax1,ax2 are the unit length hinge axes as computed from body1 and // body2, we need to rotate both bodies along the axis u = (ax1 x ax2). // if `theta' is the angle between ax1 and ax2, we need an angular velocity // along u to cover angle erp*theta in one step : // |angular_velocity| = angle/time = erp*theta / stepsize // = (erp*fps) * theta // angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2| // = (erp*fps) * theta * (ax1 x ax2) / sin(theta) // ...as ax1 and ax2 are unit length. if theta is smallish, // theta ~= sin(theta), so // angular_velocity = (erp*fps) * (ax1 x ax2) // ax1 x ax2 is in the plane space of ax1, so we project the angular // velocity to p and q to find the right hand side. IndexedVector3 ax2 = trB._basis.GetColumn(2); IndexedVector3 u = IndexedVector3.Cross(ax1, ax2); info.m_solverConstraints[s3].m_rhs = k * IndexedVector3.Dot(u, p); info.m_solverConstraints[s4].m_rhs = k * IndexedVector3.Dot(u, q); // check angular limits int nrow = 4; // last filled row float limit_err = 0.0f; int limit = 0; if (GetSolveLimit()) { #if _BT_USE_CENTER_LIMIT_ limit_err = m_limit.GetCorrection() * m_referenceSign; #else limit_err = m_correction * m_referenceSign; #endif } // if the hinge has joint limits or motor, add in the extra row bool powered = false; if (GetEnableAngularMotor()) { powered = true; } if (limit != 0 || powered) { nrow++; info.m_solverConstraints[nrow].m_relpos1CrossNormal = ax1; info.m_solverConstraints[nrow].m_relpos2CrossNormal = -ax1; float lostop = GetLowerLimit(); float histop = GetUpperLimit(); if (limit != 0 && (MathUtil.CompareFloat(lostop, histop))) { // the joint motor is ineffective powered = false; } info.m_solverConstraints[nrow].m_rhs = 0.0f; float currERP = ((m_flags & (int)HingeFlags.BT_HINGE_FLAGS_ERP_STOP) != 0) ? m_stopERP : info.erp; if (powered) { if ((m_flags & (int)HingeFlags.BT_HINGE_FLAGS_CFM_NORM) != 0) { info.m_solverConstraints[nrow].m_cfm = m_normalCFM; } float mot_fact = GetMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info.fps * currERP); info.m_solverConstraints[nrow].m_rhs += mot_fact * m_motorTargetVelocity * m_referenceSign; info.m_solverConstraints[nrow].m_lowerLimit = -m_maxMotorImpulse; info.m_solverConstraints[nrow].m_upperLimit = m_maxMotorImpulse; } if (limit != 0) { k = info.fps * currERP; info.m_solverConstraints[nrow].m_rhs += k * limit_err; if ((m_flags & (int)HingeFlags.BT_HINGE_FLAGS_CFM_STOP) != 0) { info.m_solverConstraints[nrow].m_cfm = m_stopCFM; } if (MathUtil.CompareFloat(lostop, histop)) { // limited low and high simultaneously info.m_solverConstraints[nrow].m_lowerLimit = -MathUtil.SIMD_INFINITY; info.m_solverConstraints[nrow].m_upperLimit = MathUtil.SIMD_INFINITY; } else if (limit == 1) { // low limit info.m_solverConstraints[nrow].m_lowerLimit = 0f; info.m_solverConstraints[nrow].m_upperLimit = MathUtil.SIMD_INFINITY; } else { // high limit info.m_solverConstraints[nrow].m_lowerLimit = -MathUtil.SIMD_INFINITY; info.m_solverConstraints[nrow].m_upperLimit = 0f; } // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that) #if _BT_USE_CENTER_LIMIT_ float bounce = m_limit.GetRelaxationFactor(); #else float bounce = m_relaxationFactor; #endif if (bounce > 0f) { float vel = IndexedVector3.Dot(angVelA, ax1); vel -= IndexedVector3.Dot(angVelB, ax1); // only apply bounce if the velocity is incoming, and if the // resulting c[] exceeds what we already have. if (limit == 1) { // low limit if (vel < 0) { float newc = -bounce * vel; if (newc > info.m_solverConstraints[nrow].m_rhs) { info.m_solverConstraints[nrow].m_rhs = newc; } } } else { // high limit - all those computations are reversed if (vel > 0) { float newc = -bounce * vel; if (newc < info.m_solverConstraints[nrow].m_rhs) { info.m_solverConstraints[nrow].m_rhs = newc; } } } } #if _BT_USE_CENTER_LIMIT_ info.m_solverConstraints[nrow].m_rhs *= m_limit.GetBiasFactor(); #else info.m_solverConstraints[nrow].m_rhs *= m_biasFactor; #endif } // if(limit) } // if angular limit or powered #if DEBUG if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugConstraints) { PrintInfo2(BulletGlobals.g_streamWriter, this, info); } #endif }
public void GetInfo2NonVirtual(ConstraintInfo2 info, IndexedMatrix transA, IndexedMatrix transB, IndexedVector3 angVelA, IndexedVector3 angVelB) { ///the regular (virtual) implementation getInfo2 already performs 'testLimit' during getInfo1, so we need to do it now TestLimit(ref transA, ref transB); GetInfo2Internal(info, transA, transB, angVelA, angVelB); }
public override void GetInfo2(ConstraintInfo2 info) { }
public void GetInfo2NonVirtual(ConstraintInfo2 info, IndexedMatrix body0_trans, IndexedMatrix body1_trans) { // anchor points in global coordinates with respect to body PORs. // set jacobian info.m_solverConstraints[0].m_contactNormal.X = 1; info.m_solverConstraints[1].m_contactNormal.Y = 1; info.m_solverConstraints[2].m_contactNormal.Z = 1; IndexedVector3 a1 = body0_trans._basis * GetPivotInA(); { IndexedVector3 a1neg = -a1; MathUtil.GetSkewSymmetricMatrix(ref a1neg, out info.m_solverConstraints[0].m_relpos1CrossNormal, out info.m_solverConstraints[1].m_relpos1CrossNormal, out info.m_solverConstraints[2].m_relpos1CrossNormal); } /*info->m_J2linearAxis[0] = -1; * info->m_J2linearAxis[s+1] = -1; * info->m_J2linearAxis[2*s+2] = -1; */ IndexedVector3 a2 = body1_trans._basis * GetPivotInB(); { IndexedVector3 a2n = -a2; MathUtil.GetSkewSymmetricMatrix(ref a2, out info.m_solverConstraints[0].m_relpos2CrossNormal, out info.m_solverConstraints[1].m_relpos2CrossNormal, out info.m_solverConstraints[2].m_relpos2CrossNormal); } // set right hand side float currERP = ((m_flags & Point2PointFlags.BT_P2P_FLAGS_ERP) != 0) ? m_erp : info.erp; float k = info.fps * currERP; int j; IndexedVector3 body0Origin = body0_trans._origin; IndexedVector3 body1Origin = body1_trans._origin; for (j = 0; j < 3; j++) { info.m_solverConstraints[j].m_rhs = k * (a2[j] + body1Origin[j] - a1[j] - body0Origin[j]); //printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]); } if ((m_flags & Point2PointFlags.BT_P2P_FLAGS_CFM) != 0) { for (j = 0; j < 3; j++) { info.m_solverConstraints[j].m_cfm = m_cfm; } } float impulseClamp = m_setting.m_impulseClamp;// for (j = 0; j < 3; j++) { if (m_setting.m_impulseClamp > 0) { info.m_solverConstraints[j].m_lowerLimit = -impulseClamp; info.m_solverConstraints[j].m_upperLimit = impulseClamp; } } info.m_damping = m_setting.m_damping; }