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;
}
