public void Draw(IDebugDraw drawer) { int i; for (i = 0; i < NUMRAYS_IN_BAR; i++) { drawer.DrawLine(ref source[i], ref hit_com[i], Color.Lime); } const float normalScale = 10.0f; // easier to see if this is big for (i = 0; i < NUMRAYS_IN_BAR; i++) { Vector3 to = hit_surface[i] + normalScale * normal[i]; drawer.DrawLine(ref hit_surface[i], ref to, Color.White); } Quaternion qFrom = Quaternion.RotationAxis(new Vector3(1.0f, 0.0f, 0.0f), 0.0f); Quaternion qTo = Quaternion.RotationAxis(new Vector3(1.0f, 0.0f, 0.0f), 0.7f); for (i = 0; i < NUMRAYS_IN_BAR; i++) { Matrix from = Matrix.RotationQuaternion(qFrom) * Matrix.Translation(source[i]); Matrix to = Matrix.RotationQuaternion(qTo) * Matrix.Translation(dest[i]); Vector3 linVel, angVel; TransformUtil.CalculateVelocity(from, to, 1.0f, out linVel, out angVel); Matrix T; TransformUtil.IntegrateTransform(from, linVel, angVel, hit_fraction[i], out T); Vector3 box1 = boxShapeHalfExtents; Vector3 box2 = -boxShapeHalfExtents; drawer.DrawBox(ref box1, ref box2, ref T, Color.Aqua); } }
/// <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 unsafe static void DrawBox(this IDebugDraw obj, ref OpenTK.Vector3 bbMin, ref OpenTK.Vector3 bbMax, ref OpenTK.Matrix4 trans, ref OpenTK.Vector3 color) { fixed(OpenTK.Vector3 *bbMinValue = &bbMin) { fixed(OpenTK.Vector3 *bbMaxValue = &bbMax) { fixed(OpenTK.Matrix4 *transValue = &trans) { fixed(OpenTK.Vector3 *colorValue = &color) { obj.DrawBox(ref *(BulletSharp.Math.Vector3 *)bbMinValue, ref *(BulletSharp.Math.Vector3 *)bbMaxValue, ref *(BulletSharp.Math.Matrix *)transValue, ref *(BulletSharp.Math.Vector3 *)colorValue); } } } } }
public void Draw(IDebugDraw drawer) { foreach (var ray in _rays) { drawer.DrawLine(ref ray.Source, ref ray.HitCenterOfMass, ref _green); Vector3 to = ray.HitPoint + NormalScale * ray.Normal; drawer.DrawLine(ref ray.HitPoint, ref to, ref _white); Matrix fromTransform = _fromRotation * Matrix.Translation(ray.Source); Matrix toTransform = _toRotation * Matrix.Translation(ray.Destination); Vector3 linVel, angVel; TransformUtil.CalculateVelocity(ref fromTransform, ref toTransform, 1.0f, out linVel, out angVel); Matrix transform; TransformUtil.IntegrateTransform(ref fromTransform, ref linVel, ref angVel, ray.HitFraction, out transform); drawer.DrawBox(ref _boxBoundMin, ref _boxBoundMax, ref transform, ref _cyan); } }
public void Draw(IDebugDraw drawer) { for (int i = 0; i < NumRays; i++) { drawer.DrawLine(ref _source[i], ref _hitCenterOfMass[i], Color.Lime); Vector3 to = _hitPoint[i] + NormalScale * _normal[i]; drawer.DrawLine(ref _hitPoint[i], ref to, Color.White); Matrix fromTransform = _fromRotation * Matrix.Translation(_source[i]); Matrix toTransform = _toRotation * Matrix.Translation(_destination[i]); Vector3 linVel, angVel; TransformUtil.CalculateVelocity(fromTransform, toTransform, 1.0f, out linVel, out angVel); Matrix transform; TransformUtil.IntegrateTransform(fromTransform, linVel, angVel, _hitFraction[i], out transform); drawer.DrawBox(ref _boxBoundMin, ref _boxBoundMax, ref transform, Color.Aqua); } }
public void Draw(IDebugDraw drawer) { int i; for (i = 0; i < NUMRAYS_IN_BAR; i++) { drawer.DrawLine(ref source[i], ref hit_com[i], ref green); } const float normalScale = 10.0f; // easier to see if this is big for (i = 0; i < NUMRAYS_IN_BAR; i++) { Vector3 to = hit_surface[i] + normalScale * normal[i]; drawer.DrawLine(ref hit_surface[i], ref to, ref white); } Quaternion qFrom = Quaternion.RotationAxis(new Vector3(1.0f, 0.0f, 0.0f), 0.0f); Quaternion qTo = Quaternion.RotationAxis(new Vector3(1.0f, 0.0f, 0.0f), 0.7f); for (i = 0; i < NUMRAYS_IN_BAR; i++) { Matrix from = Matrix.RotationQuaternion(qFrom) * Matrix.Translation(source[i]); Matrix to = Matrix.RotationQuaternion(qTo) * Matrix.Translation(dest[i]); Vector3 linVel, angVel; TransformUtil.CalculateVelocity(ref from, ref to, 1.0f, out linVel, out angVel); Matrix T; TransformUtil.IntegrateTransform(ref from, ref linVel, ref angVel, hit_fraction[i], out T); Vector3 box1 = boxShapeHalfExtents; Vector3 box2 = -boxShapeHalfExtents; drawer.DrawBox(ref box1, ref box2, ref T, ref cyan); } }
public void Draw(IDebugDraw drawer) { for (int i = 0; i < NumRays; i++) { drawer.DrawLine(ref _source[i], ref _hitCenterOfMass[i], ref _green); Vector3 to = _hitPoint[i] + NormalScale * _normal[i]; drawer.DrawLine(ref _hitPoint[i], ref to, ref _white); Matrix fromTransform = _fromRotation * Matrix.Translation(_source[i]); Matrix toTransform = _toRotation * Matrix.Translation(_destination[i]); Vector3 linVel, angVel; TransformUtil.CalculateVelocity(ref fromTransform, ref toTransform, 1.0f, out linVel, out angVel); Matrix transform; TransformUtil.IntegrateTransform(ref fromTransform, ref linVel, ref angVel, _hitFraction[i], out transform); drawer.DrawBox(ref _boxBoundMin, ref _boxBoundMax, ref transform, ref _cyan); } }
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; } switch (constraint.GetConstraintType()) { case TypedConstraintType.POINT2POINT_CONSTRAINT_TYPE: { Point2PointConstraint p2pC = constraint as Point2PointConstraint; IndexedMatrix tr = IndexedMatrix.Identity; IndexedVector3 pivot = p2pC.GetPivotInA(); pivot = p2pC.GetRigidBodyA().GetCenterOfMassTransform()* pivot; tr._origin = pivot; debugDraw.DrawTransform(ref tr, dbgDrawSize); // that ideally should draw the same frame pivot = p2pC.GetPivotInB(); pivot = p2pC.GetRigidBodyB().GetCenterOfMassTransform() * pivot; tr._origin = pivot; if (drawFrames) debugDraw.DrawTransform(ref tr, dbgDrawSize); } break; case TypedConstraintType.HINGE_CONSTRAINT_TYPE: { HingeConstraint pHinge = constraint as HingeConstraint; IndexedMatrix tr = pHinge.GetRigidBodyA().GetCenterOfMassTransform() * pHinge.GetAFrame(); if (drawFrames) { debugDraw.DrawTransform(ref tr, dbgDrawSize); } tr = pHinge.GetRigidBodyB().GetCenterOfMassTransform() * pHinge.GetBFrame(); if (drawFrames) { debugDraw.DrawTransform(ref tr, dbgDrawSize); } float minAng = pHinge.GetLowerLimit(); float maxAng = pHinge.GetUpperLimit(); if (minAng == maxAng) { break; } bool drawSect = true; if (minAng > maxAng) { minAng = 0f; maxAng = MathUtil.SIMD_2_PI; drawSect = false; } if (drawLimits) { IndexedVector3 center = tr._origin; IndexedVector3 normal = tr._basis.GetColumn(2); IndexedVector3 axis = tr._basis.GetColumn(0); IndexedVector3 zero = IndexedVector3.Zero; debugDraw.DrawArc(ref center, ref normal, ref axis, dbgDrawSize, dbgDrawSize, minAng, maxAng, ref zero, drawSect); } } break; case TypedConstraintType.CONETWIST_CONSTRAINT_TYPE: { ConeTwistConstraint pCT = constraint as ConeTwistConstraint; IndexedMatrix tr = pCT.GetRigidBodyA().GetCenterOfMassTransform() * pCT.GetAFrame(); if (drawFrames) debugDraw.DrawTransform(ref tr, dbgDrawSize); tr = pCT.GetRigidBodyB().GetCenterOfMassTransform() * pCT.GetBFrame(); if (drawFrames) debugDraw.DrawTransform(ref tr, dbgDrawSize); IndexedVector3 zero = IndexedVector3.Zero; if (drawLimits) { //const float length = float(5); float length = dbgDrawSize; const int nSegments = 8 * 4; float fAngleInRadians = MathUtil.SIMD_2_PI * (float)(nSegments - 1) / (float)nSegments; IndexedVector3 pPrev = pCT.GetPointForAngle(fAngleInRadians, length); pPrev = tr * pPrev; for (int i = 0; i < nSegments; i++) { fAngleInRadians = MathUtil.SIMD_2_PI * (float)i / (float)nSegments; IndexedVector3 pCur = pCT.GetPointForAngle(fAngleInRadians, length); pCur = tr * pCur; debugDraw.DrawLine(ref pPrev, ref pCur, ref zero); if (i % (nSegments / 8) == 0) { IndexedVector3 origin = tr._origin; 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 = pCT.GetRigidBodyB().GetCenterOfMassTransform() * pCT.GetBFrame(); } else { tr = pCT.GetRigidBodyA().GetCenterOfMassTransform() * pCT.GetAFrame(); } IndexedVector3 pivot = tr._origin; IndexedVector3 normal = tr._basis.GetColumn(0); IndexedVector3 axis = tr._basis.GetColumn(1); debugDraw.DrawArc(ref pivot, ref normal, ref axis, dbgDrawSize, dbgDrawSize, -twa - tws, -twa + tws, ref zero, true); } } break; case TypedConstraintType.D6_CONSTRAINT_TYPE: case TypedConstraintType.D6_SPRING_CONSTRAINT_TYPE: { Generic6DofConstraint p6DOF = constraint as Generic6DofConstraint; IndexedMatrix tr = p6DOF.GetCalculatedTransformA(); if (drawFrames) { debugDraw.DrawTransform(ref tr, dbgDrawSize); } tr = p6DOF.GetCalculatedTransformB(); if (drawFrames) { debugDraw.DrawTransform(ref tr, dbgDrawSize); } IndexedVector3 zero = IndexedVector3.Zero; if (drawLimits) { tr = p6DOF.GetCalculatedTransformA(); IndexedVector3 center = p6DOF.GetCalculatedTransformB()._origin; // up is axis 1 not 2 ? IndexedVector3 up = tr._basis.GetColumn(1); IndexedVector3 axis = tr._basis.GetColumn(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, dbgDrawSize * .9f, minTh, maxTh, minPs, maxPs, ref zero); axis = tr._basis.GetColumn(1); float ay = p6DOF.GetAngle(1); float az = p6DOF.GetAngle(2); float cy = (float)Math.Cos(ay); float sy = (float)Math.Sin(ay); float cz = (float)Math.Cos(az); float sz = (float)Math.Sin(az); IndexedVector3 ref1 = new IndexedVector3( cy * cz * axis.X + cy * sz * axis.Y - sy * axis.Z, -sz * axis.X + cz * axis.Y, cz * sy * axis.X + sz * sy * axis.Y + cy * axis.Z); tr = p6DOF.GetCalculatedTransformB(); IndexedVector3 normal = -tr._basis.GetColumn(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, dbgDrawSize, dbgDrawSize, -MathUtil.SIMD_PI, MathUtil.SIMD_PI, ref zero, false); } else if (minFi < maxFi) { debugDraw.DrawArc(ref center, ref normal, ref ref1, dbgDrawSize, dbgDrawSize, minFi, maxFi, ref zero, false); } tr = p6DOF.GetCalculatedTransformA(); IndexedVector3 bbMin = p6DOF.GetTranslationalLimitMotor().m_lowerLimit; IndexedVector3 bbMax = p6DOF.GetTranslationalLimitMotor().m_upperLimit; debugDraw.DrawBox(ref bbMin, ref bbMax, ref tr, ref zero); } } break; case TypedConstraintType.SLIDER_CONSTRAINT_TYPE: { SliderConstraint pSlider = constraint as SliderConstraint; IndexedMatrix tr = pSlider.GetCalculatedTransformA(); if (drawFrames) debugDraw.DrawTransform(ref tr, dbgDrawSize); tr = pSlider.GetCalculatedTransformB(); if (drawFrames) debugDraw.DrawTransform(ref tr, dbgDrawSize); IndexedVector3 zero = IndexedVector3.Zero; if (drawLimits) { IndexedMatrix tr2 = pSlider.GetCalculatedTransformA(); IndexedVector3 li_min = tr2 * new IndexedVector3(pSlider.GetLowerLinLimit(), 0f, 0f); IndexedVector3 li_max = tr2 * new IndexedVector3(pSlider.GetUpperLinLimit(), 0f, 0f); debugDraw.DrawLine(ref li_min, ref li_max, ref zero); IndexedVector3 normal = tr._basis.GetColumn(0); IndexedVector3 axis = tr._basis.GetColumn(1); float a_min = pSlider.GetLowerAngLimit(); float a_max = pSlider.GetUpperAngLimit(); IndexedVector3 center = pSlider.GetCalculatedTransformB()._origin; debugDraw.DrawArc(ref center, ref normal, ref axis, dbgDrawSize, dbgDrawSize, a_min, a_max, ref zero, true); } } break; default: break; } return; }
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; } switch (constraint.GetConstraintType()) { case TypedConstraintType.POINT2POINT_CONSTRAINT_TYPE: { Point2PointConstraint p2pC = constraint as Point2PointConstraint; IndexedMatrix tr = IndexedMatrix.Identity; IndexedVector3 pivot = p2pC.GetPivotInA(); pivot = p2pC.GetRigidBodyA().GetCenterOfMassTransform() * pivot; tr._origin = pivot; debugDraw.DrawTransform(ref tr, dbgDrawSize); // that ideally should draw the same frame pivot = p2pC.GetPivotInB(); pivot = p2pC.GetRigidBodyB().GetCenterOfMassTransform() * pivot; tr._origin = pivot; if (drawFrames) { debugDraw.DrawTransform(ref tr, dbgDrawSize); } } break; case TypedConstraintType.HINGE_CONSTRAINT_TYPE: { HingeConstraint pHinge = constraint as HingeConstraint; IndexedMatrix tr = pHinge.GetRigidBodyA().GetCenterOfMassTransform() * pHinge.GetAFrame(); if (drawFrames) { debugDraw.DrawTransform(ref tr, dbgDrawSize); } tr = pHinge.GetRigidBodyB().GetCenterOfMassTransform() * pHinge.GetBFrame(); if (drawFrames) { debugDraw.DrawTransform(ref tr, dbgDrawSize); } float minAng = pHinge.GetLowerLimit(); float maxAng = pHinge.GetUpperLimit(); if (minAng == maxAng) { break; } bool drawSect = true; if (minAng > maxAng) { minAng = 0f; maxAng = MathUtil.SIMD_2_PI; drawSect = false; } if (drawLimits) { IndexedVector3 center = tr._origin; IndexedVector3 normal = tr._basis.GetColumn(2); IndexedVector3 axis = tr._basis.GetColumn(0); IndexedVector3 zero = IndexedVector3.Zero; debugDraw.DrawArc(ref center, ref normal, ref axis, dbgDrawSize, dbgDrawSize, minAng, maxAng, ref zero, drawSect); } } break; case TypedConstraintType.CONETWIST_CONSTRAINT_TYPE: { ConeTwistConstraint pCT = constraint as ConeTwistConstraint; IndexedMatrix tr = pCT.GetRigidBodyA().GetCenterOfMassTransform() * pCT.GetAFrame(); if (drawFrames) { debugDraw.DrawTransform(ref tr, dbgDrawSize); } tr = pCT.GetRigidBodyB().GetCenterOfMassTransform() * pCT.GetBFrame(); if (drawFrames) { debugDraw.DrawTransform(ref tr, dbgDrawSize); } IndexedVector3 zero = IndexedVector3.Zero; if (drawLimits) { //const float length = float(5); float length = dbgDrawSize; const int nSegments = 8 * 4; float fAngleInRadians = MathUtil.SIMD_2_PI * (float)(nSegments - 1) / (float)nSegments; IndexedVector3 pPrev = pCT.GetPointForAngle(fAngleInRadians, length); pPrev = tr * pPrev; for (int i = 0; i < nSegments; i++) { fAngleInRadians = MathUtil.SIMD_2_PI * (float)i / (float)nSegments; IndexedVector3 pCur = pCT.GetPointForAngle(fAngleInRadians, length); pCur = tr * pCur; debugDraw.DrawLine(ref pPrev, ref pCur, ref zero); if (i % (nSegments / 8) == 0) { IndexedVector3 origin = tr._origin; 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 = pCT.GetRigidBodyB().GetCenterOfMassTransform() * pCT.GetBFrame(); } else { tr = pCT.GetRigidBodyA().GetCenterOfMassTransform() * pCT.GetAFrame(); } IndexedVector3 pivot = tr._origin; IndexedVector3 normal = tr._basis.GetColumn(0); IndexedVector3 axis = tr._basis.GetColumn(1); debugDraw.DrawArc(ref pivot, ref normal, ref axis, dbgDrawSize, dbgDrawSize, -twa - tws, -twa + tws, ref zero, true); } } break; case TypedConstraintType.D6_CONSTRAINT_TYPE: case TypedConstraintType.D6_SPRING_CONSTRAINT_TYPE: { Generic6DofConstraint p6DOF = constraint as Generic6DofConstraint; IndexedMatrix tr = p6DOF.GetCalculatedTransformA(); if (drawFrames) { debugDraw.DrawTransform(ref tr, dbgDrawSize); } tr = p6DOF.GetCalculatedTransformB(); if (drawFrames) { debugDraw.DrawTransform(ref tr, dbgDrawSize); } IndexedVector3 zero = IndexedVector3.Zero; if (drawLimits) { tr = p6DOF.GetCalculatedTransformA(); IndexedVector3 center = p6DOF.GetCalculatedTransformB()._origin; // up is axis 1 not 2 ? IndexedVector3 up = tr._basis.GetColumn(1); IndexedVector3 axis = tr._basis.GetColumn(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, dbgDrawSize * .9f, minTh, maxTh, minPs, maxPs, ref zero); axis = tr._basis.GetColumn(1); float ay = p6DOF.GetAngle(1); float az = p6DOF.GetAngle(2); float cy = (float)Math.Cos(ay); float sy = (float)Math.Sin(ay); float cz = (float)Math.Cos(az); float sz = (float)Math.Sin(az); IndexedVector3 ref1 = new IndexedVector3( cy * cz * axis.X + cy * sz * axis.Y - sy * axis.Z, -sz * axis.X + cz * axis.Y, cz * sy * axis.X + sz * sy * axis.Y + cy * axis.Z); tr = p6DOF.GetCalculatedTransformB(); IndexedVector3 normal = -tr._basis.GetColumn(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, dbgDrawSize, dbgDrawSize, -MathUtil.SIMD_PI, MathUtil.SIMD_PI, ref zero, false); } else if (minFi < maxFi) { debugDraw.DrawArc(ref center, ref normal, ref ref1, dbgDrawSize, dbgDrawSize, minFi, maxFi, ref zero, false); } tr = p6DOF.GetCalculatedTransformA(); IndexedVector3 bbMin = p6DOF.GetTranslationalLimitMotor().m_lowerLimit; IndexedVector3 bbMax = p6DOF.GetTranslationalLimitMotor().m_upperLimit; debugDraw.DrawBox(ref bbMin, ref bbMax, ref tr, ref zero); } } break; case TypedConstraintType.SLIDER_CONSTRAINT_TYPE: { SliderConstraint pSlider = constraint as SliderConstraint; IndexedMatrix tr = pSlider.GetCalculatedTransformA(); if (drawFrames) { debugDraw.DrawTransform(ref tr, dbgDrawSize); } tr = pSlider.GetCalculatedTransformB(); if (drawFrames) { debugDraw.DrawTransform(ref tr, dbgDrawSize); } IndexedVector3 zero = IndexedVector3.Zero; if (drawLimits) { IndexedMatrix tr2 = pSlider.GetCalculatedTransformA(); IndexedVector3 li_min = tr2 * new IndexedVector3(pSlider.GetLowerLinLimit(), 0f, 0f); IndexedVector3 li_max = tr2 * new IndexedVector3(pSlider.GetUpperLinLimit(), 0f, 0f); debugDraw.DrawLine(ref li_min, ref li_max, ref zero); IndexedVector3 normal = tr._basis.GetColumn(0); IndexedVector3 axis = tr._basis.GetColumn(1); float a_min = pSlider.GetLowerAngLimit(); float a_max = pSlider.GetUpperAngLimit(); IndexedVector3 center = pSlider.GetCalculatedTransformB()._origin; debugDraw.DrawArc(ref center, ref normal, ref axis, dbgDrawSize, dbgDrawSize, a_min, a_max, ref zero, true); } } break; default: break; } return; }