/// <summary>
/// Draws the specified constraint.
/// </summary>
/// <param name="constraint">The constraint.</param>
/// <param name="debugDraw">The debug draw.</param>
public override void Draw(TypedConstraint constraint, IDebugDraw debugDraw)
{
var p6DOF = (Generic6DofConstraint)constraint;
Matrix tr = p6DOF.GetCalculatedTransformA();
if (DrawFrames)
{
debugDraw.DrawTransform(ref tr, DrawSize);
}
tr = p6DOF.GetCalculatedTransformB();
if (DrawFrames)
{
debugDraw.DrawTransform(ref tr, DrawSize);
}
Vector3 zero = Vector3.Zero;
if (DrawLimits)
{
tr = p6DOF.GetCalculatedTransformA();
Vector3 center = p6DOF.GetCalculatedTransformB().Translation;
// up is axis 1 not 2 ?
Vector3 up = MathUtil.MatrixColumn(ref tr, 1);
Vector3 axis = MathUtil.MatrixColumn(ref tr, 0);
float minTh = p6DOF.GetRotationalLimitMotor(1).m_loLimit;
float maxTh = p6DOF.GetRotationalLimitMotor(1).m_hiLimit;
float minPs = p6DOF.GetRotationalLimitMotor(2).m_loLimit;
float maxPs = p6DOF.GetRotationalLimitMotor(2).m_hiLimit;
debugDraw.DrawSpherePatch(ref center, ref up, ref axis, DrawSize * .9f, minTh, maxTh, minPs, maxPs, ref zero);
axis = MathUtil.MatrixColumn(ref tr, 1);
float ay = p6DOF.GetAngle(1);
float az = p6DOF.GetAngle(2);
var cy = (float)System.Math.Cos(ay);
var sy = (float)System.Math.Sin(ay);
var cz = (float)System.Math.Cos(az);
var sz = (float)System.Math.Sin(az);
var ref1 = new Vector3();
ref1.X = cy * cz * axis.X + cy * sz * axis.Y - sy * axis.Z;
ref1.Y = -sz * axis.X + cz * axis.Y;
ref1.Z = cz * sy * axis.X + sz * sy * axis.Y + cy * axis.Z;
tr = p6DOF.GetCalculatedTransformB();
Vector3 normal = -MathUtil.MatrixColumn(ref tr, 0);
float minFi = p6DOF.GetRotationalLimitMotor(0).m_loLimit;
float maxFi = p6DOF.GetRotationalLimitMotor(0).m_hiLimit;
if (minFi > maxFi)
{
debugDraw.DrawArc(ref center, ref normal, ref ref1, DrawSize, DrawSize, -MathUtil.SIMD_PI, MathUtil.SIMD_PI,
ref zero, false);
}
else if (minFi < maxFi)
{
debugDraw.DrawArc(ref center, ref normal, ref ref1, DrawSize, DrawSize, minFi, maxFi, ref zero, false);
}
tr = p6DOF.GetCalculatedTransformA();
Vector3 bbMin = p6DOF.GetTranslationalLimitMotor().m_lowerLimit;
Vector3 bbMax = p6DOF.GetTranslationalLimitMotor().m_upperLimit;
debugDraw.DrawBox(ref bbMin, ref bbMax, ref tr, ref zero);
}
}
public override void Draw(TypedConstraint constraint, IDebugDraw debugDraw)
{
var pCT = (ConeTwistConstraint)constraint;
Matrix tr = MathUtil.BulletMatrixMultiply(pCT.GetRigidBodyA().GetCenterOfMassTransform(), pCT.GetAFrame());
if (DrawFrames) debugDraw.DrawTransform(ref tr, DrawSize);
tr = MathUtil.BulletMatrixMultiply(pCT.GetRigidBodyB().GetCenterOfMassTransform(), pCT.GetBFrame());
if (DrawFrames) debugDraw.DrawTransform(ref tr, DrawSize);
Vector3 zero = Vector3.Zero;
if (DrawLimits)
{
//const float length = float(5);
float length = DrawSize;
int nSegments = 8 * 4;
float fAngleInRadians = MathUtil.SIMD_2_PI * (nSegments - 1) / nSegments;
Vector3 pPrev = pCT.GetPointForAngle(fAngleInRadians, length);
pPrev = Vector3.Transform(pPrev, tr);
for (int i = 0; i < nSegments; i++)
{
fAngleInRadians = MathUtil.SIMD_2_PI * i / nSegments;
Vector3 pCur = pCT.GetPointForAngle(fAngleInRadians, length);
pCur = Vector3.Transform(pCur, tr);
debugDraw.DrawLine(ref pPrev, ref pCur, ref zero);
if (i % (nSegments / 8) == 0)
{
Vector3 origin = tr.Translation;
debugDraw.DrawLine(ref origin, ref pCur, ref zero);
}
pPrev = pCur;
}
float tws = pCT.GetTwistSpan();
float twa = pCT.GetTwistAngle();
bool useFrameB = (pCT.GetRigidBodyB().GetInvMass() > 0f);
if (useFrameB)
{
tr = MathUtil.BulletMatrixMultiply(pCT.GetRigidBodyB().GetCenterOfMassTransform(), pCT.GetBFrame());
}
else
{
tr = MathUtil.BulletMatrixMultiply(pCT.GetRigidBodyA().GetCenterOfMassTransform(), pCT.GetAFrame());
}
Vector3 pivot = tr.Translation;
Vector3 normal = MathUtil.MatrixColumn(ref tr, 0);
Vector3 axis1 = MathUtil.MatrixColumn(ref tr, 1);
debugDraw.DrawArc(ref pivot, ref normal, ref axis1, DrawSize, DrawSize, -twa - tws, -twa + tws, ref zero, true);
}
}
public override void Draw(TypedConstraint constraint, IDebugDraw debugDraw)
{
var p2pC = (Point2PointConstraint)constraint;
Matrix tr = Matrix.Identity;
Vector3 pivot = p2pC.GetPivotInA();
pivot = Vector3.Transform(pivot, p2pC.GetRigidBodyA().GetCenterOfMassTransform());
tr.Translation = pivot;
debugDraw.DrawTransform(ref tr, DrawSize);
// that ideally should draw the same frame
pivot = p2pC.GetPivotInB();
pivot = Vector3.Transform(pivot, p2pC.GetRigidBodyB().GetCenterOfMassTransform());
tr.Translation = pivot;
if (DrawFrames)
debugDraw.DrawTransform(ref tr, DrawSize);
}
public override void Draw(TypedConstraint constraint, IDebugDraw debugDraw)
{
var pSlider = (SliderConstraint)constraint;
Matrix tr = pSlider.GetCalculatedTransformA();
if (DrawFrames) debugDraw.DrawTransform(ref tr, DrawSize);
tr = pSlider.GetCalculatedTransformB();
if (DrawFrames) debugDraw.DrawTransform(ref tr, DrawSize);
Vector3 zero = Vector3.Zero;
if (DrawLimits)
{
Matrix tr2 = pSlider.GetCalculatedTransformA();
Vector3 li_min = Vector3.Transform(new Vector3(pSlider.GetLowerLinLimit(), 0f, 0f), tr2);
Vector3 li_max = Vector3.Transform(new Vector3(pSlider.GetUpperLinLimit(), 0f, 0f), tr2);
debugDraw.DrawLine(ref li_min, ref li_max, ref zero);
Vector3 normal = MathUtil.MatrixColumn(ref tr, 0);
Vector3 axis = MathUtil.MatrixColumn(ref tr, 1);
float a_min = pSlider.GetLowerAngLimit();
float a_max = pSlider.GetUpperAngLimit();
Vector3 center = pSlider.GetCalculatedTransformB().Translation;
debugDraw.DrawArc(ref center, ref normal, ref axis, DrawSize, DrawSize, a_min, a_max, ref zero, true);
}
}
public override void Draw(TypedConstraint constraint, IDebugDraw debugDraw)
{
var pHinge = (HingeConstraint)constraint;
Matrix tr = MathUtil.BulletMatrixMultiply(pHinge.GetRigidBodyA().GetCenterOfMassTransform(), pHinge.GetAFrame());
if (DrawFrames)
debugDraw.DrawTransform(ref tr, DrawSize);
tr = MathUtil.BulletMatrixMultiply(pHinge.GetRigidBodyB().GetCenterOfMassTransform(), pHinge.GetBFrame());
if (DrawFrames)
debugDraw.DrawTransform(ref tr, DrawSize);
float minAng = pHinge.GetLowerLimit();
float maxAng = pHinge.GetUpperLimit();
if (minAng == maxAng)
return;
bool drawSect = true;
if (minAng > maxAng)
{
minAng = 0f;
maxAng = MathUtil.SIMD_2_PI;
drawSect = false;
}
if (DrawLimits)
{
Vector3 center = tr.Translation;
Vector3 normal = MathUtil.MatrixColumn(ref tr, 2);
Vector3 axis = MathUtil.MatrixColumn(ref tr, 0);
Vector3 zero = Vector3.Zero;
debugDraw.DrawArc(ref center, ref normal, ref axis, DrawSize, DrawSize, minAng, maxAng, ref zero, drawSect);
}
}
public virtual void RemoveConstraint(TypedConstraint constraint)
{
//(void)constraint;
}
public virtual void AddConstraint(TypedConstraint constraint, bool disableCollisionsBetweenLinkedBodies)
{
//(void)constraint;
//(void)disableCollisionsBetweenLinkedBodies;
}
public virtual void AddConstraint(TypedConstraint constraint)
{
AddConstraint(constraint, false);
}
public static int GetConstraintIslandId(TypedConstraint lhs)
{
int islandId;
CollisionObject rcolObj0 = lhs.GetRigidBodyA();
CollisionObject rcolObj1 = lhs.GetRigidBodyB();
islandId = rcolObj0.GetIslandTag() >= 0 ? rcolObj0.GetIslandTag() : rcolObj1.GetIslandTag();
return islandId;
}
public override void RemoveConstraint(TypedConstraint constraint)
{
m_constraints.Remove(constraint);
constraint.GetRigidBodyA().RemoveConstraintRef(constraint);
constraint.GetRigidBodyB().RemoveConstraintRef(constraint);
}
public override void AddConstraint(TypedConstraint constraint, bool disableCollisionsBetweenLinkedBodies)
{
//if (constraint is ConeTwistConstraint)
//if (constraint is HingeConstraint)
//{
// return;
//}
m_constraints.Add(constraint);
if (disableCollisionsBetweenLinkedBodies)
{
constraint.GetRigidBodyA().AddConstraintRef(constraint);
constraint.GetRigidBodyB().AddConstraintRef(constraint);
}
}
public static void DebugDrawConstraint(TypedConstraint constraint, IDebugDraw debugDraw)
{
bool drawFrames = (debugDraw.GetDebugMode() & DebugDrawModes.DBG_DrawConstraints) != 0;
bool drawLimits = (debugDraw.GetDebugMode() & DebugDrawModes.DBG_DrawConstraintLimits) != 0;
float dbgDrawSize = constraint.GetDbgDrawSize();
if (dbgDrawSize <= 0f)
return;
IConstraintTypeDrawer constraintTypeDrawer;
if (!ConstraintDrawers.TryGetValue(constraint.GetConstraintType(), out constraintTypeDrawer))
return;
constraintTypeDrawer.DrawFrames = drawFrames;
constraintTypeDrawer.DrawLimits = drawLimits;
constraintTypeDrawer.DrawSize = dbgDrawSize;
constraintTypeDrawer.Draw(constraint, debugDraw);
}
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 static void PrintInfo1(StreamWriter writer,TypedConstraint constraint,ConstraintInfo1 info)
{
if (writer != null)
{
writer.WriteLine("getInfo1 [{0}] [{1}] [{2}] [{3}]", constraint.m_userConstraintId, constraint.GetObjectType(), (string)constraint.GetRigidBodyA().GetUserPointer(), (string)constraint.GetRigidBodyB().GetUserPointer());
MathUtil.PrintMatrix(writer, "rBA cmot", constraint.GetRigidBodyA().GetCenterOfMassTransform());
MathUtil.PrintMatrix(writer, "rBB cmot", constraint.GetRigidBodyB().GetCenterOfMassTransform());
MathUtil.PrintMatrix(writer, "rBA inv tensor", constraint.GetRigidBodyA().GetInvInertiaTensorWorld());
MathUtil.PrintMatrix(writer, "rBB inv tensor", constraint.GetRigidBodyB().GetInvInertiaTensorWorld());
writer.WriteLine(String.Format("NumRows [{0}] Nub[{1}]", info.m_numConstraintRows, info.nub));
}
}
public void RemoveConstraintRef(TypedConstraint c)
{
m_constraintRefs.Remove(c);
m_checkCollideWith = m_constraintRefs.Count > 0;
}
public void AddConstraintRef(TypedConstraint c)
{
if (!m_constraintRefs.Contains(c))
{
m_constraintRefs.Add(c);
}
m_checkCollideWith = true;
}