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