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