protected virtual int SetAngularLimits(ConstraintInfo2 info, int row_offset,ref Matrix transA,ref Matrix transB,ref Vector3 linVelA,ref Vector3 linVelB,ref Vector3 angVelA,ref Vector3 angVelB)
{
Generic6DofConstraint d6constraint = this;
int row = row_offset;
//solve angular limits
for (int i=0;i<3 ;i++ )
{
if(d6constraint.GetRotationalLimitMotor(i).NeedApplyTorques())
{
Vector3 axis = d6constraint.GetAxis(i);
int tempFlags = ((int)m_flags) >> ((i + 3) * BT_6DOF_FLAGS_AXIS_SHIFT);
SixDofFlags flags = (SixDofFlags)tempFlags;
if (0 == (flags & SixDofFlags.BT_6DOF_FLAGS_CFM_NORM))
{
m_angularLimits[i].m_normalCFM = info.m_solverConstraints[0].m_cfm;
}
if (0 == (flags & SixDofFlags.BT_6DOF_FLAGS_CFM_STOP))
{
m_angularLimits[i].m_stopCFM = info.m_solverConstraints[0].m_cfm;
}
if (0 == (flags & SixDofFlags.BT_6DOF_FLAGS_ERP_STOP))
{
m_angularLimits[i].m_stopERP = info.erp;
}
row += GetLimitMotorInfo2(d6constraint.GetRotationalLimitMotor(i),
ref transA,ref transB,ref linVelA,ref linVelB,ref angVelA,ref angVelB, info,row,ref axis,1,false);
}
}
return row;
}
protected void InternalUpdateSprings(ConstraintInfo2 info)
{
// it is assumed that calculateTransforms() have been called before this call
Vector3 relVel = m_rbB.GetLinearVelocity() - m_rbA.GetLinearVelocity();
for (int i = 0; i < 3; i++)
{
if (m_springEnabled[i])
{
// get current position of constraint
float currPos = MathUtil.VectorComponent(ref m_calculatedLinearDiff,i);
// calculate difference
float delta = currPos - m_equilibriumPoint[i];
// spring force is (delta * m_stiffness) according to Hooke's Law
float force = delta * m_springStiffness[i];
float velFactor = info.fps * m_springDamping[i] / (float)info.m_numIterations;
MathUtil.VectorComponent(ref m_linearLimits.m_targetVelocity,i,velFactor * force);
MathUtil.VectorComponent(ref m_linearLimits.m_maxMotorForce,i,System.Math.Abs(force) / info.fps);
}
}
for (int i = 0; i < 3; i++)
{
if (m_springEnabled[i + 3])
{
// get current position of constraint
float currPos = MathUtil.VectorComponent(ref m_calculatedAxisAngleDiff,i);
// calculate difference
float delta = currPos - m_equilibriumPoint[i + 3];
// spring force is (-delta * m_stiffness) according to Hooke's Law
float force = -delta * m_springStiffness[i + 3];
float velFactor = info.fps * m_springDamping[i + 3] / (float)info.m_numIterations;
m_angularLimits[i].m_targetVelocity = velFactor * force;
m_angularLimits[i].m_maxMotorForce = System.Math.Abs(force) / info.fps;
}
}
}
public virtual int GetLimitMotorInfo2(RotationalLimitMotor limot,
ref Matrix transA,ref Matrix transB,ref Vector3 linVelA,
ref Vector3 linVelB,ref Vector3 angVelA,ref Vector3 angVelB,
ConstraintInfo2 info, int row, ref Vector3 ax1, int rotational,bool rotAllowed)
{
bool powered = limot.m_enableMotor;
int limit = limot.m_currentLimit;
if (powered || limit != 0)
{
// if the joint is powered, or has joint limits, add in the extra row
//btScalar* J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis;
//btScalar* J2 = rotational ? info->m_J2angularAxis : 0;
//info2.m_J1linearAxis = currentConstraintRow->m_contactNormal;
//info2.m_J1angularAxis = currentConstraintRow->m_relpos1CrossNormal;
//info2.m_J2linearAxis = 0;
//info2.m_J2angularAxis = currentConstraintRow->m_relpos2CrossNormal;
if(rotational != 0)
{
info.m_solverConstraints[row].m_relpos1CrossNormal = ax1;
MathUtil.ZeroCheckVector(info.m_solverConstraints[row].m_relpos1CrossNormal);
}
else
{
info.m_solverConstraints[row].m_contactNormal = ax1;
MathUtil.ZeroCheckVector(info.m_solverConstraints[row].m_contactNormal);
}
if (rotational != 0)
{
info.m_solverConstraints[row].m_relpos2CrossNormal = -ax1;
}
//MathUtil.zeroCheckVector(info.m_solverConstraints[row].m_relpos2CrossNormal);
if (rotational == 0)
{
if (m_useOffsetForConstraintFrame)
{
Vector3 tmpA = Vector3.Zero, tmpB= Vector3.Zero, relA= Vector3.Zero, relB= Vector3.Zero;
// get vector from bodyB to frameB in WCS
relB = m_calculatedTransformB.Translation - transB.Translation;
// get its projection to constraint axis
Vector3 projB = ax1 * Vector3.Dot(relB,ax1);
// get vector directed from bodyB to constraint axis (and orthogonal to it)
Vector3 orthoB = relB - projB;
// same for bodyA
relA = m_calculatedTransformA.Translation - transA.Translation;
Vector3 projA = ax1 * Vector3.Dot(relA,ax1);
Vector3 orthoA = relA - projA;
// get desired offset between frames A and B along constraint axis
float desiredOffs = limot.m_currentPosition - limot.m_currentLimitError;
// desired vector from projection of center of bodyA to projection of center of bodyB to constraint axis
Vector3 totalDist = projA + ax1 * desiredOffs - projB;
// get offset vectors relA and relB
relA = orthoA + totalDist * m_factA;
relB = orthoB - totalDist * m_factB;
tmpA = Vector3.Cross(relA,ax1);
tmpB = Vector3.Cross(relB,ax1);
if(m_hasStaticBody && (!rotAllowed))
{
tmpA *= m_factA;
tmpB *= m_factB;
}
info.m_solverConstraints[row].m_relpos1CrossNormal = tmpA;
MathUtil.ZeroCheckVector(ref tmpA);
info.m_solverConstraints[row].m_relpos2CrossNormal = -tmpB;
MathUtil.ZeroCheckVector(ref tmpB);
}
else
{
Vector3 ltd; // Linear Torque Decoupling vector
Vector3 c = m_calculatedTransformB.Translation - transA.Translation;
ltd = Vector3.Cross(c,ax1);
info.m_solverConstraints[row].m_relpos1CrossNormal = ltd;
MathUtil.ZeroCheckVector(info.m_solverConstraints[row].m_relpos1CrossNormal);
c = m_calculatedTransformB.Translation - transB.Translation;
ltd = -Vector3.Cross(c,ax1);
info.m_solverConstraints[row].m_relpos2CrossNormal = ltd;
MathUtil.ZeroCheckVector(info.m_solverConstraints[row].m_relpos2CrossNormal);
}
}
// if we're limited low and high simultaneously, the joint motor is
// ineffective
if (limit != 0 && (MathUtil.CompareFloat(limot.m_loLimit,limot.m_hiLimit)))
{
powered = false;
}
info.m_solverConstraints[row].m_rhs = 0f;
if (powered)
{
info.m_solverConstraints[row].m_cfm = limot.m_normalCFM;
if (limit == 0)
{
float tag_vel = (rotational != 0)? limot.m_targetVelocity : -limot.m_targetVelocity;
float mot_fact = GetMotorFactor(limot.m_currentPosition,
limot.m_loLimit,
limot.m_hiLimit,
tag_vel,
info.fps * limot.m_stopERP);
info.m_solverConstraints[row].m_rhs += mot_fact * limot.m_targetVelocity;
info.m_solverConstraints[row].m_lowerLimit = -limot.m_maxMotorForce;
info.m_solverConstraints[row].m_upperLimit = limot.m_maxMotorForce;
}
}
if (limit != 0)
{
float k = info.fps * limot.m_stopERP;
if (rotational == 0)
{
info.m_solverConstraints[row].m_rhs += k * limot.m_currentLimitError;
}
else
{
info.m_solverConstraints[row].m_rhs += -k * limot.m_currentLimitError;
}
info.m_solverConstraints[row].m_cfm = limot.m_stopCFM;
if (MathUtil.CompareFloat(limot.m_loLimit,limot.m_hiLimit))
{ // limited low and high simultaneously
info.m_solverConstraints[row].m_lowerLimit = -MathUtil.SIMD_INFINITY;
info.m_solverConstraints[row].m_upperLimit = MathUtil.SIMD_INFINITY;
}
else
{
if (limit == 1)
{
info.m_solverConstraints[row].m_lowerLimit = 0;
info.m_solverConstraints[row].m_upperLimit = MathUtil.SIMD_INFINITY;
}
else
{
info.m_solverConstraints[row].m_lowerLimit = -MathUtil.SIMD_INFINITY;
info.m_solverConstraints[row].m_upperLimit = 0;
}
// deal with bounce
if (limot.m_bounce > 0)
{
// calculate joint velocity
float vel;
if (rotational != 0)
{
vel = Vector3.Dot(angVelA,ax1);
vel -= Vector3.Dot(angVelB,ax1);
}
else
{
vel = Vector3.Dot(linVelA,ax1);
vel -= Vector3.Dot(linVelB,ax1);
}
// only apply bounce if the velocity is incoming, and if the
// resulting c[] exceeds what we already have.
if (limit == 1)
{
if (vel < 0)
{
float newc = -limot.m_bounce * vel;
if (newc > info.m_solverConstraints[row].m_rhs)
{
info.m_solverConstraints[row].m_rhs = newc;
}
}
}
else
{
if (vel > 0)
{
float newc = -limot.m_bounce * vel;
if (newc < info.m_solverConstraints[row].m_rhs)
{
info.m_solverConstraints[row].m_rhs = newc;
}
}
}
}
}
}
return 1;
}
else return 0;
}
public void GetInfo2NonVirtual(ConstraintInfo2 info,Matrix transA,Matrix transB,Vector3 linVelA,Vector3 linVelB,Vector3 angVelA,Vector3 angVelB)
{
//prepare constraint
CalculateTransforms(ref transA, ref transB);
if(m_useOffsetForConstraintFrame)
{ // for stability better to solve angular limits first
int row = SetAngularLimits(info, 0, ref transA, ref transB, ref linVelA, ref linVelB, ref angVelA, ref angVelB);
SetLinearLimits(info, row, ref transA, ref transB, ref linVelA, ref linVelB, ref angVelA, ref angVelB);
}
else
{ // leave old version for compatibility
int row = SetLinearLimits(info, 0, ref transA, ref transB, ref linVelA, ref linVelB, ref angVelA, ref angVelB);
SetAngularLimits(info, row, ref transA, ref transB, ref linVelA, ref linVelB, ref angVelA, ref angVelB);
}
}
public void GetInfo2InternalUsingFrameOffset(ConstraintInfo2 info, ref Matrix transA, ref Matrix transB, ref Vector3 angVelA, ref Vector3 angVelB)
{
// transforms in world space
if (BulletGlobals.g_streamWriter != null && debugHingeConstrainst)
{
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);
}
Matrix trA = MathUtil.BulletMatrixMultiply(transA, m_rbAFrame);
Matrix trB = MathUtil.BulletMatrixMultiply(transB, m_rbBFrame);
if (BulletGlobals.g_streamWriter != null && debugHingeConstrainst)
{
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "trA", trA);
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "trB", trB);
}
// pivot point
Vector3 pivotAInW = trA.Translation;
Vector3 pivotBInW = trB.Translation;
#if true
// difference between frames in WCS
Vector3 ofs = trB.Translation - trA.Translation;
// 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
Vector3 ax1A = MathUtil.MatrixColumn(ref trA,2);
Vector3 ax1B = MathUtil.MatrixColumn(ref trB,2);
Vector3 ax1 = ax1A * factA + ax1B * factB;
ax1.Normalize();
// fill first 3 rows
// we want: velA + wA x relA == velB + wB x relB
Matrix bodyA_trans = transA;
Matrix bodyB_trans = transB;
int s0 = 0;
int s1 = 1;
int s2 = 2;
int nrow = 2; // last filled row
Vector3 tmpA, tmpB, relA, relB, p, q;
// get vector from bodyB to frameB in WCS
relB = trB.Translation - bodyB_trans.Translation;
// get its projection to hinge axis
Vector3 projB = ax1 * Vector3.Dot(relB,ax1);
// get vector directed from bodyB to hinge axis (and orthogonal to it)
Vector3 orthoB = relB - projB;
// same for bodyA
relA = trA.Translation - bodyA_trans.Translation;
Vector3 projA = ax1 * Vector3.Dot(relA,ax1);
Vector3 orthoA = relA - projA;
Vector3 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 = MathUtil.MatrixColumn(ref trA,1);
}
// make one more ortho
q = Vector3.Cross(ax1,p);
// fill three rows
tmpA = Vector3.Cross(relA,p);
tmpB = Vector3.Cross(relB,p);
info.m_solverConstraints[s0].m_relpos1CrossNormal = tmpA;
info.m_solverConstraints[s0].m_relpos2CrossNormal = -tmpB;
tmpA = Vector3.Cross(relA,q);
tmpB = Vector3.Cross(relB,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 = Vector3.Cross(relA,ax1);
tmpB = Vector3.Cross(relB,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;
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 k = info.fps * info.erp;
float rhs = k * Vector3.Dot(p,ofs);
info.m_solverConstraints[s0].m_rhs = rhs;
rhs = k * Vector3.Dot(q,ofs);
info.m_solverConstraints[s1].m_rhs = rhs;
rhs = k * Vector3.Dot(ax1,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;
Vector3 u = Vector3.Cross(ax1A,ax1B);
info.m_solverConstraints[s3].m_rhs = k * Vector3.Dot(u,p);
info.m_solverConstraints[s4].m_rhs = k * Vector3.Dot(u,q);
#endif
// check angular limits
nrow = 4; // last filled row
int srow;
float limit_err = 0f;
int limit = 0;
if(GetSolveLimit())
{
limit_err = m_correction * m_referenceSign;
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)
float bounce = m_relaxationFactor;
if(bounce > 0f)
{
float vel = Vector3.Dot(angVelA,ax1);
vel -= Vector3.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[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;
}
}
}
}
info.m_solverConstraints[srow].m_rhs *= m_biasFactor;
} // if(limit)
} // if angular limit or powered
if (BulletGlobals.g_streamWriter != null && debugHingeConstrainst)
{
PrintInfo2(BulletGlobals.g_streamWriter, this, info);
}
}
public void GetInfo2InternalUsingFrameOffset(ConstraintInfo2 info, Matrix transA, Matrix transB, Vector3 angVelA, Vector3 angVelB)
{
GetInfo2InternalUsingFrameOffset(info, ref transA, ref transB, ref angVelA, ref angVelB);
}
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_solverConstraints.Length,info2.fps,info2.erp,info2.findex,info2.m_numIterations));
for(int i=0;i<info2.m_solverConstraints.Length;++i)
{
writer.WriteLine(String.Format("SolverConstaint[{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 void GetInfo2NonVirtual(ConstraintInfo2 info,Matrix body0_trans,Matrix 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;
Vector3 a1 = Vector3.TransformNormal(GetPivotInA(),body0_trans);
{
Vector3 angular0 = info.m_solverConstraints[0].m_relpos1CrossNormal;
Vector3 angular1 = info.m_solverConstraints[1].m_relpos1CrossNormal;
Vector3 angular2 = info.m_solverConstraints[2].m_relpos1CrossNormal;
Vector3 a1neg = -a1;
MathUtil.GetSkewSymmetricMatrix(ref a1neg, ref angular0, ref angular1, ref angular2);
}
/*info->m_J2linearAxis[0] = -1;
info->m_J2linearAxis[s+1] = -1;
info->m_J2linearAxis[2*s+2] = -1;
*/
Vector3 a2 = Vector3.TransformNormal(GetPivotInB(),body1_trans);
{
Vector3 a2n = -a2;
Vector3 angular0 = info.m_solverConstraints[0].m_relpos2CrossNormal;
Vector3 angular1 = info.m_solverConstraints[1].m_relpos2CrossNormal;
Vector3 angular2 = info.m_solverConstraints[2].m_relpos2CrossNormal;
MathUtil.GetSkewSymmetricMatrix(ref a2, ref angular0, ref angular1, ref angular2);
}
// 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;
Vector3 body0Origin = body0_trans.Translation;
Vector3 body1Origin = body1_trans.Translation;
for (j = 0; j < 3; j++)
{
info.m_solverConstraints[j].m_rhs = k * (MathUtil.VectorComponent(ref a2,j) + MathUtil.VectorComponent(ref body1Origin,j) - MathUtil.VectorComponent(ref a1,j) - MathUtil.VectorComponent(ref 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;
}
}
}
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 void GetInfo2NonVirtual(ConstraintInfo2 info, Matrix transA, Matrix transB, Matrix invInertiaWorldA, Matrix invInertiaWorldB)
{
CalcAngleInfo2(ref transA, ref transB, ref invInertiaWorldA, ref invInertiaWorldB);
// set jacobian
info.m_solverConstraints[0].m_contactNormal.X = 1f;
info.m_solverConstraints[1].m_contactNormal.Y = 1f;
info.m_solverConstraints[2].m_contactNormal.Z = 1f;
Vector3 a1 = Vector3.TransformNormal(m_rbAFrame.Translation, transA);
{
Vector3 a1neg = -a1;
MathUtil.GetSkewSymmetricMatrix(ref a1neg, ref info.m_solverConstraints[0].m_relpos1CrossNormal, ref info.m_solverConstraints[1].m_relpos1CrossNormal, ref info.m_solverConstraints[2].m_relpos1CrossNormal);
}
Vector3 a2 = Vector3.TransformNormal(m_rbBFrame.Translation, transB);
{
MathUtil.GetSkewSymmetricMatrix(ref a2, ref info.m_solverConstraints[0].m_relpos2CrossNormal, ref info.m_solverConstraints[1].m_relpos2CrossNormal, ref info.m_solverConstraints[2].m_relpos2CrossNormal);
}
// set right hand side
float linERP = ((m_flags & (int)ConeTwistFlags.BT_CONETWIST_FLAGS_LIN_ERP) != 0) ? m_linERP : info.erp;
float k = info.fps * linERP;
for (int j = 0; j < 3; j++)
{
info.m_solverConstraints[j].m_rhs = k * (MathUtil.VectorComponent(ref a2, j) + MathUtil.VectorComponent(transB.Translation, j) - MathUtil.VectorComponent(ref a1, j) - MathUtil.VectorComponent(transA.Translation, j));
info.m_solverConstraints[j].m_lowerLimit = -MathUtil.SIMD_INFINITY;
info.m_solverConstraints[j].m_upperLimit = MathUtil.SIMD_INFINITY;
if ((m_flags & (int)ConeTwistFlags.BT_CONETWIST_FLAGS_LIN_CFM) != 0)
{
info.m_solverConstraints[j].m_cfm = m_linCFM;
}
}
int row = 3;
Vector3 ax1;
// angular limits
if (m_solveSwingLimit)
{
if ((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh))
{
Matrix trA = MathUtil.BulletMatrixMultiply(transA, m_rbAFrame);
Vector3 p = MathUtil.MatrixColumn(ref trA, 1);
Vector3 q = MathUtil.MatrixColumn(ref trA, 2);
info.m_solverConstraints[row].m_relpos1CrossNormal = p;
info.m_solverConstraints[row + 1].m_relpos1CrossNormal = q;
info.m_solverConstraints[row].m_relpos2CrossNormal = -p;
info.m_solverConstraints[row + 1].m_relpos2CrossNormal = -q;
float fact = info.fps * m_relaxationFactor;
info.m_solverConstraints[row].m_rhs = fact * Vector3.Dot(m_swingAxis, p);
info.m_solverConstraints[row + 1].m_rhs = fact * Vector3.Dot(m_swingAxis, q);
info.m_solverConstraints[row].m_lowerLimit = -MathUtil.SIMD_INFINITY;
info.m_solverConstraints[row].m_upperLimit = MathUtil.SIMD_INFINITY;
info.m_solverConstraints[row + 1].m_lowerLimit = -MathUtil.SIMD_INFINITY;
info.m_solverConstraints[row + 1].m_upperLimit = MathUtil.SIMD_INFINITY;
row += 2;
}
else
{
ax1 = m_swingAxis * m_relaxationFactor * m_relaxationFactor;
info.m_solverConstraints[row].m_relpos1CrossNormal = ax1;
info.m_solverConstraints[row].m_relpos2CrossNormal = -ax1;
float k1 = info.fps * m_biasFactor;
info.m_solverConstraints[row].m_rhs = k1 * m_swingCorrection;
if ((m_flags & (int)ConeTwistFlags.BT_CONETWIST_FLAGS_ANG_CFM) != 0)
{
info.m_solverConstraints[row].m_cfm = m_angCFM;
}
// m_swingCorrection is always positive or 0
info.m_solverConstraints[row].m_lowerLimit = 0;
info.m_solverConstraints[row].m_upperLimit = MathUtil.SIMD_INFINITY;
++row;
}
}
if (m_solveTwistLimit)
{
ax1 = m_twistAxis * m_relaxationFactor * m_relaxationFactor;
info.m_solverConstraints[row].m_relpos1CrossNormal = ax1;
info.m_solverConstraints[row].m_relpos2CrossNormal = -ax1;
float k1 = info.fps * m_biasFactor;
info.m_solverConstraints[row].m_rhs = k1 * m_twistCorrection;
if ((m_flags & (int)ConeTwistFlags.BT_CONETWIST_FLAGS_ANG_CFM) != 0)
{
info.m_solverConstraints[row].m_cfm = m_angCFM;
}
if (m_twistSpan > 0.0f)
{
if (m_twistCorrection > 0.0f)
{
info.m_solverConstraints[row].m_lowerLimit = 0;
info.m_solverConstraints[row].m_upperLimit = MathUtil.SIMD_INFINITY;
}
else
{
info.m_solverConstraints[row].m_lowerLimit = -MathUtil.SIMD_INFINITY;
info.m_solverConstraints[row].m_upperLimit = 0;
}
}
else
{
info.m_solverConstraints[row].m_lowerLimit = -MathUtil.SIMD_INFINITY;
info.m_solverConstraints[row].m_upperLimit = MathUtil.SIMD_INFINITY;
}
++row;
}
if (BulletGlobals.g_streamWriter != null && debugConstraint)
{
PrintInfo2(BulletGlobals.g_streamWriter, this, info);
}
}
public override void GetInfo2 (ConstraintInfo2 info)
{
InternalUpdateSprings(info);
base.GetInfo2(info);
}
public void TestLinLimits2(ConstraintInfo2 info)
{
}
public void GetInfo2NonVirtual(ConstraintInfo2 info, Matrix transA, Matrix transB, Vector3 linVelA, Vector3 linVelB, float rbAinvMass, float rbBinvMass)
{
Matrix trA = GetCalculatedTransformA();
Matrix trB = GetCalculatedTransformB();
Debug.Assert(!m_useSolveConstraintObsolete);
int i, s = 1;
float signFact = m_useLinearReferenceFrameA ? 1.0f : -1.0f;
// difference between frames in WCS
Vector3 ofs = trB.Translation - trA.Translation;
// 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;
Vector3 ax1 = Vector3.Zero, p= Vector3.Zero, q= Vector3.Zero;
Vector3 ax1A = MathUtil.MatrixColumn(ref trA,0);
Vector3 ax1B = MathUtil.MatrixColumn(ref trB,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, ref p, ref q);
}
else
{ // old way - use frameA
ax1 = MathUtil.MatrixColumn(ref trA,0);
// get 2 orthos to slider axis (Y, Z)
p = MathUtil.MatrixColumn(ref trA,1);
q = MathUtil.MatrixColumn(ref trA,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;
Vector3 u = Vector3.Cross(ax1A,ax1B);
info.m_solverConstraints[0].m_rhs = k * Vector3.Dot(u,p);
info.m_solverConstraints[s].m_rhs = k * Vector3.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
Matrix bodyA_trans = transA;
Matrix bodyB_trans = transB;
nrow++;
int s2 = nrow * s;
nrow++;
int s3 = nrow * s;
Vector3 tmpA = Vector3.Zero, tmpB = Vector3.Zero, relA = Vector3.Zero, relB = Vector3.Zero, c = Vector3.Zero;
if(m_useOffsetForConstraintFrame)
{
// get vector from bodyB to frameB in WCS
relB = trB.Translation - bodyB_trans.Translation;
// get its projection to slider axis
Vector3 projB = ax1 * Vector3.Dot(relB,ax1);
// get vector directed from bodyB to slider axis (and orthogonal to it)
Vector3 orthoB = relB - projB;
// same for bodyA
relA = trA.Translation - bodyA_trans.Translation;
Vector3 projA = ax1 * Vector3.Dot(relA,ax1);
Vector3 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
Vector3 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 = MathUtil.MatrixColumn(ref trA,1);
}
// make one more ortho
q = Vector3.Cross(ax1,p);
// fill two rows
tmpA = Vector3.Cross(relA,p);
tmpB = Vector3.Cross(relB,p);
info.m_solverConstraints[s2].m_relpos1CrossNormal = tmpA;
info.m_solverConstraints[s2].m_relpos2CrossNormal = -tmpB;
tmpA = Vector3.Cross(relA,q);
tmpB = Vector3.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
Vector3 tmp = Vector3.Cross(c, p);
info.m_solverConstraints[s2].m_relpos1CrossNormal = factA * tmp;
info.m_solverConstraints[s2].m_relpos2CrossNormal = factB * tmp;
tmp = Vector3.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 * Vector3.Dot(p,ofs);
info.m_solverConstraints[s2].m_rhs = rhs;
rhs = k * Vector3.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 = Vector3.Cross(relA,ax1);
tmpB = Vector3.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
Vector3 ltd = Vector3.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 = System.Math.Abs(1.0f - GetDampingLimLin());
if(bounce > 0.0f)
{
float vel = Vector3.Dot(linVelA,ax1);
vel -= Vector3.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 = System.Math.Abs(1.0f - GetDampingLimAng());
if (bounce > 0.0f)
{
float vel = Vector3.Dot(m_rbA.GetAngularVelocity(), ax1);
vel -= Vector3.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 (BulletGlobals.g_streamWriter != null && debugConstraint)
{
PrintInfo2(BulletGlobals.g_streamWriter, this, info);
}
}
//protected float solveSingleIteration(int iteration, ObjectArray<CollisionObject> bodies, int numBodies, ObjectArray<PersistentManifold> manifold, int numManifolds, ObjectArray<TypedConstraint> constraints, int numConstraints, ContactSolverInfo infoGlobal, IDebugDraw debugDrawer, IDispatcher dispatcher)
//protected float solveSingleIteration(int iteration, ObjectArray<CollisionObject> bodies, int numBodies, ObjectArray<PersistentManifold> manifold, int numManifolds, ObjectArray<TypedConstraint> constraints, int numConstraints, ContactSolverInfo infoGlobal, IDebugDraw debugDrawer)
//{
// return 0f;
//}
protected virtual float SolveGroupCacheFriendlySetup(ObjectArray<CollisionObject> bodies, int numBodies, ObjectArray<PersistentManifold> manifold, int numManifolds, ObjectArray<TypedConstraint> constraints, int numConstraints, ContactSolverInfo infoGlobal, IDebugDraw debugDrawer, IDispatcher dispatcher)
{
//BT_PROFILE("solveGroupCacheFriendlySetup");
m_counter++;
if ((numConstraints + numManifolds) == 0)
{
// printf("empty\n");
return 0f;
}
//if (true)
//{
// for (int j=0;j<numConstraints;j++)
// {
// TypedConstraint constraint = constraints[j];
// constraint.buildJacobian();
// }
//}
//btRigidBody* rb0=0,*rb1=0;
//if (1)
{
{
int totalNumRows = 0;
//calculate the total number of contraint rows
m_tmpConstraintSizesPool.Clear();
for (int i = 0; i < numConstraints; i++)
{
ConstraintInfo1 info1 = new ConstraintInfo1();
m_tmpConstraintSizesPool.Add(info1);
constraints[i].GetInfo1(info1);
totalNumRows += info1.m_numConstraintRows;
}
ResizeSolverConstraintList(m_tmpSolverNonContactConstraintPool, totalNumRows);
///setup the btSolverConstraints
int currentRow = 0;
for (int i = 0; i < numConstraints; i++)
{
ConstraintInfo1 info1a = m_tmpConstraintSizesPool[i];
if (info1a.m_numConstraintRows != 0)
{
Debug.Assert(currentRow < totalNumRows);
//SolverConstraint currentConstraintRow = m_tmpSolverNonContactConstraintPool[currentRow];
TypedConstraint constraint = constraints[i];
RigidBody rbA = constraint.GetRigidBodyA();
RigidBody rbB = constraint.GetRigidBodyB();
for (int j = currentRow; j < (currentRow + info1a.m_numConstraintRows); j++)
{
SolverConstraint solverConstraint = new SolverConstraint();
solverConstraint.m_lowerLimit = float.MinValue;
solverConstraint.m_upperLimit = float.MaxValue;
solverConstraint.m_appliedImpulse = 0f;
solverConstraint.m_appliedPushImpulse = 0f;
solverConstraint.m_solverBodyA = rbA;
solverConstraint.m_solverBodyB = rbB;
m_tmpSolverNonContactConstraintPool[j] = solverConstraint;
}
Vector3 zero = Vector3.Zero;
rbA.InternalSetDeltaLinearVelocity(ref zero);
rbA.InternalSetDeltaAngularVelocity(ref zero);
rbB.InternalSetDeltaLinearVelocity(ref zero);
rbB.InternalSetDeltaAngularVelocity(ref zero);
ConstraintInfo2 info2 = new ConstraintInfo2();
info2.m_solverConstraints = new SolverConstraint[info1a.m_numConstraintRows];
// MAN - copy the data into the info block for passing to the constraints
for (int j = 0; j < info1a.m_numConstraintRows; ++j)
{
info2.m_solverConstraints[j] = m_tmpSolverNonContactConstraintPool[currentRow + j];
}
info2.fps = 1f / infoGlobal.m_timeStep;
info2.erp = infoGlobal.m_erp;
info2.m_numIterations = infoGlobal.m_numIterations;
constraints[i].GetInfo2(info2);
///finalize the constraint setup
///
for (int j = 0; j < info1a.m_numConstraintRows; ++j)
{
m_tmpSolverNonContactConstraintPool[currentRow + j] = info2.m_solverConstraints[j];
}
//FIXME - log the output of the solverconstraints for comparison.
for (int j = 0; j < (info1a.m_numConstraintRows); j++)
{
SolverConstraint solverConstraint = m_tmpSolverNonContactConstraintPool[currentRow + j];
solverConstraint.m_originalContactPoint = constraint;
{
Vector3 ftorqueAxis1 = solverConstraint.m_relpos1CrossNormal;
solverConstraint.m_angularComponentA = Vector3.TransformNormal(ftorqueAxis1, constraint.GetRigidBodyA().GetInvInertiaTensorWorld()) * constraint.GetRigidBodyA().GetAngularFactor();
}
{
Vector3 ftorqueAxis2 = solverConstraint.m_relpos2CrossNormal;
solverConstraint.m_angularComponentB = Vector3.TransformNormal(ftorqueAxis2, constraint.GetRigidBodyB().GetInvInertiaTensorWorld()) * constraint.GetRigidBodyB().GetAngularFactor();
}
{
Vector3 iMJlA = solverConstraint.m_contactNormal * rbA.GetInvMass();
Vector3 iMJaA = Vector3.TransformNormal(solverConstraint.m_relpos1CrossNormal, rbA.GetInvInertiaTensorWorld());
Vector3 iMJlB = solverConstraint.m_contactNormal * rbB.GetInvMass();//sign of normal?
Vector3 iMJaB = Vector3.TransformNormal(solverConstraint.m_relpos2CrossNormal, rbB.GetInvInertiaTensorWorld());
float sum = Vector3.Dot(iMJlA, solverConstraint.m_contactNormal);
float a = Vector3.Dot(iMJaA, solverConstraint.m_relpos1CrossNormal);
float b = Vector3.Dot(iMJlB, solverConstraint.m_contactNormal);
float c = Vector3.Dot(iMJaB, solverConstraint.m_relpos2CrossNormal);
sum += a;
sum += b;
sum += c;
solverConstraint.m_jacDiagABInv = 1f / sum;
MathUtil.SanityCheckFloat(solverConstraint.m_jacDiagABInv);
}
///fix rhs
///todo: add force/torque accelerators
{
float rel_vel;
float vel1Dotn = Vector3.Dot(solverConstraint.m_contactNormal, rbA.GetLinearVelocity()) + Vector3.Dot(solverConstraint.m_relpos1CrossNormal, rbA.GetAngularVelocity());
float vel2Dotn = -Vector3.Dot(solverConstraint.m_contactNormal, rbB.GetLinearVelocity()) + Vector3.Dot(solverConstraint.m_relpos2CrossNormal, rbB.GetAngularVelocity());
rel_vel = vel1Dotn + vel2Dotn;
float restitution = 0f;
float positionalError = solverConstraint.m_rhs;//already filled in by getConstraintInfo2
float velocityError = restitution - rel_vel;// * damping;
float penetrationImpulse = positionalError * solverConstraint.m_jacDiagABInv;
float velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv;
solverConstraint.m_rhs = penetrationImpulse + velocityImpulse;
solverConstraint.m_appliedImpulse = 0f;
}
if (BulletGlobals.g_streamWriter != null && debugSolver)
{
TypedConstraint.PrintSolverConstraint(BulletGlobals.g_streamWriter, solverConstraint, j);
}
m_tmpSolverNonContactConstraintPool[currentRow + j] = solverConstraint;
}
}
currentRow += m_tmpConstraintSizesPool[i].m_numConstraintRows;
}
}
{
PersistentManifold manifold2 = null;
CollisionObject colObj0 = null, colObj1 = null;
for (int i = 0; i < numManifolds; i++)
{
manifold2 = manifold[i];
ConvertContact(manifold2, infoGlobal);
}
}
}
ContactSolverInfo info = infoGlobal;
int numConstraintPool = m_tmpSolverContactConstraintPool.Count;
int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.Count;
///@todo: use stack allocator for such temporarily memory, same for solver bodies/constraints
//m_orderTmpConstraintPool.Capacity = numConstraintPool;
//m_orderFrictionConstraintPool.Capacity = numFrictionPool;
m_orderTmpConstraintPool.Clear();
m_orderFrictionConstraintPool.Clear();
{
for (int i = 0; i < numConstraintPool; i++)
{
m_orderTmpConstraintPool.Add(i);
}
for (int i = 0; i < numFrictionPool; i++)
{
m_orderFrictionConstraintPool.Add(i);
}
}
return 0f;
}
protected virtual int SetLinearLimits(ConstraintInfo2 info, int row, ref Matrix transA,ref Matrix transB,ref Vector3 linVelA,ref Vector3 linVelB,ref Vector3 angVelA,ref Vector3 angVelB)
{
//solve linear limits
RotationalLimitMotor limot = new RotationalLimitMotor();
for (int i = 0; i < 3; i++)
{
if (m_linearLimits.NeedApplyForce(i))
{ // re-use rotational motor code
limot.m_bounce = 0f;
limot.m_currentLimit = m_linearLimits.m_currentLimit[i];
limot.m_currentPosition = MathUtil.VectorComponent(ref m_linearLimits.m_currentLinearDiff,i);
limot.m_currentLimitError = MathUtil.VectorComponent(ref m_linearLimits.m_currentLimitError,i);
limot.m_damping = m_linearLimits.m_damping;
limot.m_enableMotor = m_linearLimits.m_enableMotor[i];
limot.m_hiLimit = MathUtil.VectorComponent(m_linearLimits.m_upperLimit,i);
limot.m_limitSoftness = m_linearLimits.m_limitSoftness;
limot.m_loLimit = MathUtil.VectorComponent(m_linearLimits.m_lowerLimit,i);
limot.m_maxLimitForce = 0f;
limot.m_maxMotorForce = MathUtil.VectorComponent(m_linearLimits.m_maxMotorForce,i);
limot.m_targetVelocity = MathUtil.VectorComponent(m_linearLimits.m_targetVelocity,i);
Vector3 axis = MathUtil.MatrixColumn(m_calculatedTransformA,i);
int tempFlags = (((int)m_flags) >> (i * BT_6DOF_FLAGS_AXIS_SHIFT));
SixDofFlags flags = (SixDofFlags)tempFlags;
limot.m_normalCFM = ((flags & SixDofFlags.BT_6DOF_FLAGS_CFM_NORM) != 0) ? MathUtil.VectorComponent(ref m_linearLimits.m_normalCFM,i) : info.m_solverConstraints[0].m_cfm;
limot.m_stopCFM = ((flags & SixDofFlags.BT_6DOF_FLAGS_CFM_STOP) != 0) ? MathUtil.VectorComponent(ref m_linearLimits.m_stopCFM,i) : info.m_solverConstraints[0].m_cfm;
limot.m_stopERP = ((flags & SixDofFlags.BT_6DOF_FLAGS_ERP_STOP) != 0) ? MathUtil.VectorComponent(ref m_linearLimits.m_stopERP,i) : info.erp;
if(m_useOffsetForConstraintFrame)
{
int indx1 = (i + 1) % 3;
int indx2 = (i + 2) % 3;
bool rotAllowed = true; // rotations around orthos to current axis
if(m_angularLimits[indx1].m_currentLimit != 0 && m_angularLimits[indx2].m_currentLimit != 0)
{
rotAllowed = false;
}
row += GetLimitMotorInfo2(limot, ref transA,ref transB,ref linVelA,ref linVelB,ref angVelA,ref angVelB, info, row, ref axis, 0, rotAllowed);
}
else
{
row += GetLimitMotorInfo2(limot, ref transA,ref transB,ref linVelA,ref linVelB,ref angVelA,ref angVelB, info, row, ref axis, 0,false);
}
}
}
return row;
}
public void GetInfo2NonVirtual(ConstraintInfo2 info,Matrix transA,Matrix transB,Vector3 angVelA,Vector3 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)
{
GetInfo2NonVirtual(info, m_rbA.GetCenterOfMassTransform(), m_rbB.GetCenterOfMassTransform());
}
public void GetInfo2Internal(ConstraintInfo2 info,Matrix transA,Matrix transB,Vector3 angVelA,Vector3 angVelB)
{
// transforms in world space
Matrix trA = MathUtil.BulletMatrixMultiply(ref transA,ref m_rbAFrame);
Matrix trB = MathUtil.BulletMatrixMultiply(ref transB,ref m_rbBFrame);
// pivot point
Vector3 pivotAInW = trA.Translation;
Vector3 pivotBInW = trB.Translation;
// linear (all fixed)
//info.m_J1linearAxis[0] = 1;
//info.m_J1linearAxis[s + 1] = 1;
//info.m_J1linearAxis[2 * s + 2] = 1;
info.m_solverConstraints[0].m_contactNormal.X = 1f;
info.m_solverConstraints[1].m_contactNormal.Y = 1f;
info.m_solverConstraints[2].m_contactNormal.Z = 1f;
Vector3 a1 = pivotAInW - transA.Translation;
{
Vector3 a1neg = -a1;
MathUtil.GetSkewSymmetricMatrix(ref a1neg, ref info.m_solverConstraints[0].m_relpos1CrossNormal, ref info.m_solverConstraints[1].m_relpos1CrossNormal, ref info.m_solverConstraints[2].m_relpos1CrossNormal);
//if (info.m_solverConstraints[0].m_relpos1CrossNormal.X == 0.15)
//{
// int ibreak = 0;
//}
int ibreak = 0;
}
Vector3 a2 = pivotBInW - transB.Translation;
{
MathUtil.GetSkewSymmetricMatrix(ref a2, ref info.m_solverConstraints[0].m_relpos2CrossNormal, ref info.m_solverConstraints[1].m_relpos2CrossNormal, ref info.m_solverConstraints[2].m_relpos2CrossNormal);
}
// linear RHS
float k = info.fps * info.erp;
for (int i = 0; i < 3; i++)
{
float val = k * (MathUtil.VectorComponent(ref pivotBInW,i) - MathUtil.VectorComponent(ref 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)
Vector3 ax1 = MathUtil.MatrixColumn(ref trA,2);
// get 2 orthos to hinge axis (X, Y)
Vector3 p = MathUtil.MatrixColumn(ref trA,0);
Vector3 q = MathUtil.MatrixColumn(ref trA,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.
Vector3 ax2 = MathUtil.MatrixColumn(ref trB,2);
Vector3 u = Vector3.Cross(ax1,ax2);
info.m_solverConstraints[s3].m_rhs = k * Vector3.Dot(u, p);
info.m_solverConstraints[s4].m_rhs = k * Vector3.Dot(u, q);
// check angular limits
int nrow = 4; // last filled row
float limit_err = 0.0f;
int limit = 0;
if (GetSolveLimit())
{
limit_err = m_correction * m_referenceSign;
limit = (limit_err >0.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++;
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 * info.erp);
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)
float bounce = m_relaxationFactor;
if (bounce > 0f)
{
float vel = Vector3.Dot(angVelA,ax1);
vel -= Vector3.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;
}
}
}
}
info.m_solverConstraints[nrow].m_rhs *= m_biasFactor;
} // if(limit)
} // if angular limit or powered
if (BulletGlobals.g_streamWriter != null && debugHingeConstrainst)
{
PrintInfo2(BulletGlobals.g_streamWriter, this, info);
}
}
public override void GetInfo2(ConstraintInfo2 info)
{
}
public virtual void GetInfo2(ConstraintInfo2 info)
{
}
