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);
}
}
public void DebugDraw(IDebugDraw debugDrawer)
{
for (int v = 0; v < NumWheels; v++)
{
WheelInfo wheelInfo = GetWheelInfo(v);
Vector3 wheelColor;
if (wheelInfo.RaycastInfo.IsInContact)
{
wheelColor = blue;
}
else
{
wheelColor = magenta;
}
Matrix transform = wheelInfo.WorldTransform;
Vector3 wheelPosWS = transform.Origin;
Vector3 axle = new Vector3(
transform[0, RightAxis],
transform[1, RightAxis],
transform[2, RightAxis]);
Vector3 to1 = wheelPosWS + axle;
Vector3 to2 = GetWheelInfo(v).RaycastInfo.ContactPointWS;
//debug wheels (cylinders)
debugDrawer.DrawLine(ref wheelPosWS, ref to1, ref wheelColor);
debugDrawer.DrawLine(ref wheelPosWS, ref to2, ref wheelColor);
}
}
///btActionInterface interface
public virtual void DebugDraw(IDebugDraw debugDrawer)
{
for (int v = 0; v < GetNumWheels(); v++)
{
IndexedVector3 wheelColor = new IndexedVector3(0, 1, 1);
if (GetWheelInfo(v).m_raycastInfo.m_isInContact)
{
wheelColor = new IndexedVector3(0, 0, 1);
}
else
{
wheelColor = new IndexedVector3(1, 0, 1);
}
IndexedVector3 wheelPosWS = GetWheelInfo(v).m_worldTransform._origin;
IndexedMatrix temp = GetWheelInfo(v).m_worldTransform;
IndexedVector3 axle = new IndexedVector3(
GetWheelInfo(v).m_worldTransform._basis._el0[GetRightAxis()],
GetWheelInfo(v).m_worldTransform._basis._el1[GetRightAxis()],
GetWheelInfo(v).m_worldTransform._basis._el2[GetRightAxis()]);
//debug wheels (cylinders)
debugDrawer.DrawLine(wheelPosWS, wheelPosWS + axle, wheelColor);
debugDrawer.DrawLine(wheelPosWS, GetWheelInfo(v).m_raycastInfo.m_contactPointWS, wheelColor);
}
}
///btActionInterface interface
public virtual void DebugDraw(IDebugDraw debugDrawer)
{
for (int v=0;v<GetNumWheels();v++)
{
IndexedVector3 wheelColor = new IndexedVector3(0,1,1);
if (GetWheelInfo(v).m_raycastInfo.m_isInContact)
{
wheelColor = new IndexedVector3(0,0,1);
} else
{
wheelColor= new IndexedVector3(1,0,1);
}
IndexedVector3 wheelPosWS = GetWheelInfo(v).m_worldTransform._origin;
IndexedMatrix temp = GetWheelInfo(v).m_worldTransform;
IndexedVector3 axle = new IndexedVector3(
GetWheelInfo(v).m_worldTransform._basis._el0[GetRightAxis()],
GetWheelInfo(v).m_worldTransform._basis._el1[GetRightAxis()],
GetWheelInfo(v).m_worldTransform._basis._el2[GetRightAxis()]);
//debug wheels (cylinders)
debugDrawer.DrawLine(wheelPosWS,wheelPosWS+axle,wheelColor);
debugDrawer.DrawLine(wheelPosWS,GetWheelInfo(v).m_raycastInfo.m_contactPointWS,wheelColor);
}
}
public void Draw(IDebugDraw drawer)
{
foreach (var ray in _rays)
{
drawer.DrawLine(ref ray.Source, ref ray.HitPoint, Color.Green);
Vector3 to = ray.HitPoint + ray.Normal;
drawer.DrawLine(ref ray.HitPoint, ref to, Color.White);
}
}
public void ProcessTriangle(Vector3[] triangle, int partID, int triangleIndex)
{
Vector3 wv0, wv1, wv2;
wv0 = Vector3.TransformNormal(triangle[0], _worldTrans);
wv1 = Vector3.TransformNormal(triangle[1], _worldTrans);
wv2 = Vector3.TransformNormal(triangle[2], _worldTrans);
_debugDrawer.DrawLine(wv0, wv1, _color);
_debugDrawer.DrawLine(wv1, wv2, _color);
_debugDrawer.DrawLine(wv2, wv0, _color);
}
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 void Draw(IDebugDraw drawer)
{
int i;
for (i = 0; i < NUMRAYS_IN_BAR; i++)
{
drawer.DrawLine(ref source[i], ref hit[i], ref green);
}
for (i = 0; i < NUMRAYS_IN_BAR; i++)
{
Vector3 to = hit[i] + normal[i];
drawer.DrawLine(ref hit[i], ref to, ref white);
}
}
public void Draw(IDebugDraw drawer)
{
int i;
for (i = 0; i < NumRays; i++)
{
drawer.DrawLine(ref _source[i], ref _hitPoint[i], Color.Lime);
}
for (i = 0; i < NumRays; i++)
{
Vector3 to = _hitPoint[i] + _normal[i];
drawer.DrawLine(ref _hitPoint[i], ref to, Color.White);
}
}
public override void AddContactPoint(ref IndexedVector3 normalOnBInWorld, ref IndexedVector3 pointInWorld, float orgDepth)
{
IndexedVector3 endPt, startPt;
float newDepth;
IndexedVector3 newNormal = new IndexedVector3(0, 1, 0);
if (m_perturbA)
{
IndexedVector3 endPtOrg = pointInWorld + normalOnBInWorld * orgDepth;
endPt = (m_unPerturbedTransform * m_transformA.Inverse()) * (endPtOrg);
newDepth = IndexedVector3.Dot((endPt - pointInWorld), normalOnBInWorld);
startPt = endPt + normalOnBInWorld * newDepth;
}
else
{
endPt = pointInWorld + normalOnBInWorld * orgDepth;
startPt = (m_unPerturbedTransform * m_transformB.Inverse()) * (pointInWorld);
newDepth = IndexedVector3.Dot((endPt - startPt), normalOnBInWorld);
}
//#define DEBUG_CONTACTS 1
#if DEBUG_CONTACTS
m_debugDrawer.DrawLine(startPt, endPt, new IndexedVector3(1, 0, 0));
m_debugDrawer.DrawSphere(startPt, 0.5f, new IndexedVector3(0, 1, 0));
m_debugDrawer.DrawSphere(endPt, 0.5f, new IndexedVector3(0, 0, 1));
#endif //DEBUG_CONTACTS
m_originalManifoldResult.AddContactPoint(ref normalOnBInWorld, ref startPt, newDepth);
}
public static void DrawAabb(IDebugDraw debugDrawer, Vector3 from, Vector3 to, Vector3 color)
{
Vector3 halfExtents = (to - from) * 0.5f;
Vector3 center = (to + from) * 0.5f;
Vector3 edgecoord = new Vector3(1f, 1f, 1f), pa, pb;
for (int i = 0; i < 4; i++)
{
for (int j = 0; j < 3; j++)
{
pa = new Vector3(edgecoord.X * halfExtents.X, edgecoord.Y * halfExtents.Y,
edgecoord.Z * halfExtents.Z);
pa += center;
int othercoord = j % 3;
MathHelper.SetElement(ref edgecoord, othercoord, MathHelper.GetElement(edgecoord, othercoord) * -1f);
pb = new Vector3(edgecoord.X * halfExtents.X, edgecoord.Y * halfExtents.Y,
edgecoord.Z * halfExtents.Z);
pb += center;
debugDrawer.DrawLine(pa, pb, color);
}
edgecoord = new Vector3(-1f, -1f, -1f);
if (i < 3)
{
MathHelper.SetElement(ref edgecoord, i, MathHelper.GetElement(edgecoord, i) * -1f);
}
}
}
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)
{
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()
{
if (m_debugDraw != null)
{
int i;
for (i = 0; i < NUMRAYS_IN_BAR; i++)
{
m_debugDraw.DrawLine(source[i], hit[i], new IndexedVector3(0, 1, 0));
}
for (i = 0; i < NUMRAYS_IN_BAR; i++)
{
m_debugDraw.DrawLine(hit[i], hit[i] + (normal[i] * 5), new IndexedVector3(1, 1, 1));
}
IndexedVector3 pointSize = new IndexedVector3(0.5f, 0.5f, 0.5f);
for (i = 0; i < NUMRAYS_IN_BAR; i++)
{
m_debugDraw.DrawLine(hit[i], hit[i] + pointSize, new IndexedVector3(0, 1, 1));
}
}
}
public unsafe static void DrawLine(this IDebugDraw obj, ref OpenTK.Vector3 from, ref OpenTK.Vector3 to, ref OpenTK.Vector3 fromColor)
{
fixed(OpenTK.Vector3 *fromValue = &from)
{
fixed(OpenTK.Vector3 *toValue = &to)
{
fixed(OpenTK.Vector3 *fromColorValue = &fromColor)
{
obj.DrawLine(ref *(BulletSharp.Math.Vector3 *)fromValue, ref *(BulletSharp.Math.Vector3 *)toValue, ref *(BulletSharp.Math.Vector3 *)fromColorValue);
}
}
}
}
public virtual void ProcessTriangle(IndexedVector3[] triangle, int partId, int triangleIndex)
{
//(void)partId;
//(void)triangleIndex;
IndexedVector3 wv0, wv1, wv2;
wv0 = m_worldTrans * triangle[0];
wv1 = m_worldTrans * triangle[1];
wv2 = m_worldTrans * triangle[2];
if ((int)(m_debugDrawer.GetDebugMode() & DebugDrawModes.DBG_DrawNormals) != 0)
{
IndexedVector3 center = (wv0 + wv1 + wv2) * (1f / 3f);
IndexedVector3 normal = (wv1 - wv0).Cross(wv2 - wv0);
normal.Normalize();
IndexedVector3 normalColor = new IndexedVector3(1, 1, 0);
//m_debugDrawer.DrawLine(center, center + normal, normalColor);
}
m_debugDrawer.DrawLine(ref wv0, ref wv1, ref m_color);
m_debugDrawer.DrawLine(ref wv1, ref wv2, ref m_color);
m_debugDrawer.DrawLine(ref wv2, ref wv0, ref m_color);
}
///btActionInterface interface
public virtual void DebugDraw(IDebugDraw debugDrawer)
{
for (int v=0;v<GetNumWheels();v++)
{
Vector3 wheelColor = new Vector3(0,1,1);
if (GetWheelInfo(v).m_raycastInfo.m_isInContact)
{
wheelColor = new Vector3(0,0,1);
} else
{
wheelColor= new Vector3(1,0,1);
}
Vector3 wheelPosWS = GetWheelInfo(v).m_worldTransform.Translation;
Matrix temp = GetWheelInfo(v).m_worldTransform;
Vector3 axle = MathUtil.MatrixColumn(ref temp, GetRightAxis());
//debug wheels (cylinders)
debugDrawer.DrawLine(wheelPosWS,wheelPosWS+axle,wheelColor);
debugDrawer.DrawLine(wheelPosWS,GetWheelInfo(v).m_raycastInfo.m_contactPointWS,wheelColor);
}
}
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 void Draw(IDebugDraw drawer)
{
int i;
for (i = 0; i < NUMRAYS_IN_BAR; i++)
{
drawer.DrawLine(ref source[i], ref hit[i], Color.Lime);
}
for (i = 0; i < NUMRAYS_IN_BAR; i++)
{
Vector3 to = hit[i] + normal[i];
drawer.DrawLine(ref hit[i], ref to, Color.White);
}
}
public void DebugDraw(IDebugDraw debugDrawer)
{
for (int v = 0; v < NumWheels; v++)
{
WheelInfo wheelInfo = GetWheelInfo(v);
Color wheelColor;
if (wheelInfo.RaycastInfo.IsInContact)
{
wheelColor = Color.Blue;
}
else
{
wheelColor = Color.Magenta;
}
Matrix transform = wheelInfo.WorldTransform;
Vector3 wheelPosWS = transform.Origin;
Vector3 axle = new Vector3(
transform[0, RightAxis],
transform[1, RightAxis],
transform[2, RightAxis]);
Vector3 to1 = wheelPosWS + axle;
Vector3 to2 = GetWheelInfo(v).RaycastInfo.ContactPointWS;
//debug wheels (cylinders)
debugDrawer.DrawLine(ref wheelPosWS, ref to1, wheelColor);
debugDrawer.DrawLine(ref wheelPosWS, ref to2, wheelColor);
}
}
protected static void DebugDrawSphere(float radius, ref Matrix transform, ref Vector3 color, IDebugDraw debugDraw)
{
Vector3 start = transform.Translation;
Vector3 xoffs = Vector3.TransformNormal(new Vector3(radius, 0, 0), transform);
Vector3 yoffs = Vector3.TransformNormal(new Vector3(0, radius, 0), transform);
Vector3 zoffs = Vector3.TransformNormal(new Vector3(0, 0, radius), transform);
// XY
debugDraw.DrawLine(start - xoffs, start + yoffs, color);
debugDraw.DrawLine(start + yoffs, start + xoffs, color);
debugDraw.DrawLine(start + xoffs, start - yoffs, color);
debugDraw.DrawLine(start - yoffs, start - xoffs, color);
// XZ
debugDraw.DrawLine(start - xoffs, start + zoffs, color);
debugDraw.DrawLine(start + zoffs, start + xoffs, color);
debugDraw.DrawLine(start + xoffs, start - zoffs, color);
debugDraw.DrawLine(start - zoffs, start - xoffs, color);
// YZ
debugDraw.DrawLine(start - yoffs, start + zoffs, color);
debugDraw.DrawLine(start + zoffs, start + yoffs, color);
debugDraw.DrawLine(start + yoffs, start - zoffs, color);
debugDraw.DrawLine(start - zoffs, start - yoffs, color);
}
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 static void DebugDrawObject(ref Matrix worldTransform, CollisionShape shape, ref Vector3 color,
IDebugDraw debugDraw)
{
// Draw a small simplex at the center of the object
{
Vector3 start = worldTransform.Translation;
float scale = 10f;
debugDraw.DrawLine(start, start + (Vector3.TransformNormal(Vector3.Right, worldTransform) * scale), Vector3.Right);
debugDraw.DrawLine(start, start + (Vector3.TransformNormal(Vector3.Up, worldTransform) * scale), Vector3.Up);
debugDraw.DrawLine(start, start + (Vector3.TransformNormal(Vector3.Backward, worldTransform) * scale),
Vector3.Backward);
}
//return;
if (shape.ShapeType == BroadphaseNativeTypes.COMPOUND_SHAPE_PROXYTYPE)
{
var compoundShape = (CompoundShape)shape;
for (int i = compoundShape.GetNumChildShapes() - 1; i >= 0; i--)
{
Matrix childTrans = compoundShape.GetChildTransform(i);
CollisionShape colShape = compoundShape.GetChildShape(i);
Matrix temp = MathUtil.BulletMatrixMultiply(worldTransform, childTrans);
DebugDrawObject(ref temp, colShape, ref color, debugDraw);
}
}
else
{
switch (shape.ShapeType)
{
case BroadphaseNativeTypes.SPHERE_SHAPE_PROXYTYPE:
{
var sphereShape = (SphereShape)shape;
float radius = sphereShape.Margin; //radius doesn't include the margin, so draw with margin
DebugDrawSphere(radius, ref worldTransform, ref color, debugDraw);
break;
}
case BroadphaseNativeTypes.MULTI_SPHERE_SHAPE_PROXYTYPE:
{
var multiSphereShape = (MultiSphereShape)shape;
for (int i = multiSphereShape.GetSphereCount() - 1; i >= 0; i--)
{
Matrix childTransform = worldTransform;
childTransform.Translation += multiSphereShape.GetSpherePosition(i);
DebugDrawSphere(multiSphereShape.GetSphereRadius(i), ref childTransform, ref color, debugDraw);
}
break;
}
case BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE:
{
var capsuleShape = (CapsuleShape)shape;
float radius = capsuleShape.getRadius();
float halfHeight = capsuleShape.getHalfHeight();
int upAxis = capsuleShape.GetUpAxis();
Vector3 capStart = Vector3.Zero;
;
MathUtil.VectorComponent(ref capStart, upAxis, -halfHeight);
Vector3 capEnd = Vector3.Zero;
MathUtil.VectorComponent(ref capEnd, upAxis, halfHeight);
// Draw the ends
{
Matrix childTransform = worldTransform;
childTransform.Translation = Vector3.Transform(capStart, worldTransform);
DebugDrawSphere(radius, ref childTransform, ref color, debugDraw);
}
{
Matrix childTransform = worldTransform;
childTransform.Translation = Vector3.Transform(capEnd, worldTransform);
DebugDrawSphere(radius, ref childTransform, ref color, debugDraw);
}
// Draw some additional lines
Vector3 start = worldTransform.Translation;
MathUtil.VectorComponent(ref capStart, (upAxis + 1) % 3, radius);
MathUtil.VectorComponent(ref capEnd, (upAxis + 1) % 3, radius);
debugDraw.DrawLine(start + Vector3.TransformNormal(capStart, worldTransform),
start + Vector3.TransformNormal(capEnd, worldTransform), color);
MathUtil.VectorComponent(ref capStart, (upAxis + 1) % 3, -radius);
MathUtil.VectorComponent(ref capEnd, (upAxis + 1) % 3, -radius);
debugDraw.DrawLine(start + Vector3.TransformNormal(capStart, worldTransform),
start + Vector3.TransformNormal(capEnd, worldTransform), color);
MathUtil.VectorComponent(ref capStart, (upAxis + 1) % 3, radius);
MathUtil.VectorComponent(ref capEnd, (upAxis + 1) % 3, radius);
MathUtil.VectorComponent(ref capStart, (upAxis + 2) % 3, radius);
MathUtil.VectorComponent(ref capEnd, (upAxis + 2) % 3, radius);
debugDraw.DrawLine(start + Vector3.TransformNormal(capStart, worldTransform),
start + Vector3.TransformNormal(capEnd, worldTransform), color);
MathUtil.VectorComponent(ref capStart, (upAxis + 2) % 3, -radius);
MathUtil.VectorComponent(ref capEnd, (upAxis + 2) % 3, -radius);
debugDraw.DrawLine(start + Vector3.TransformNormal(capStart, worldTransform),
start + Vector3.TransformNormal(capEnd, worldTransform), color);
break;
}
case BroadphaseNativeTypes.CONE_SHAPE_PROXYTYPE:
{
var coneShape = (ConeShape)shape;
float radius = coneShape.GetRadius(); //+coneShape->getMargin();
float height = coneShape.GetHeight(); //+coneShape->getMargin();
Vector3 start = worldTransform.Translation;
int upAxis = coneShape.GetConeUpIndex();
Vector3 offsetHeight = Vector3.Zero;
MathUtil.VectorComponent(ref offsetHeight, upAxis, height * 0.5f);
Vector3 offsetRadius = Vector3.Zero;
MathUtil.VectorComponent(ref offsetRadius, (upAxis + 1) % 3, radius);
Vector3 offset2Radius = Vector3.Zero;
MathUtil.VectorComponent(ref offsetRadius, (upAxis + 2) % 3, radius);
debugDraw.DrawLine(start + Vector3.TransformNormal(offsetHeight, worldTransform),
start + Vector3.TransformNormal(-offsetHeight + offsetRadius, worldTransform), color);
debugDraw.DrawLine(start + Vector3.TransformNormal(offsetHeight, worldTransform),
start + Vector3.TransformNormal(-offsetHeight - offsetRadius, worldTransform), color);
debugDraw.DrawLine(start + Vector3.TransformNormal(offsetHeight, worldTransform),
start + Vector3.TransformNormal(-offsetHeight + offset2Radius, worldTransform), color);
debugDraw.DrawLine(start + Vector3.TransformNormal(offsetHeight, worldTransform),
start + Vector3.TransformNormal(-offsetHeight - offset2Radius, worldTransform), color);
break;
}
case BroadphaseNativeTypes.CYLINDER_SHAPE_PROXYTYPE:
{
var cylinder = (CylinderShape)shape;
int upAxis = cylinder.GetUpAxis();
float radius = cylinder.GetRadius();
float halfHeight = MathUtil.VectorComponent(cylinder.GetHalfExtentsWithMargin(), upAxis);
Vector3 start = worldTransform.Translation;
Vector3 offsetHeight = Vector3.Zero;
MathUtil.VectorComponent(ref offsetHeight, upAxis, halfHeight);
Vector3 offsetRadius = Vector3.Zero;
MathUtil.VectorComponent(ref offsetRadius, (upAxis + 1) % 3, radius);
debugDraw.DrawLine(start + Vector3.TransformNormal(offsetHeight + offsetRadius, worldTransform),
start + Vector3.TransformNormal(-offsetHeight + offsetRadius, worldTransform), color);
debugDraw.DrawLine(start + Vector3.TransformNormal(offsetHeight - offsetRadius, worldTransform),
start + Vector3.TransformNormal(-offsetHeight - offsetRadius, worldTransform), color);
break;
}
case BroadphaseNativeTypes.STATIC_PLANE_PROXYTYPE:
{
var staticPlaneShape = (StaticPlaneShape)shape;
float planeConst = staticPlaneShape.GetPlaneConstant();
Vector3 planeNormal = staticPlaneShape.GetPlaneNormal();
Vector3 planeOrigin = planeNormal * planeConst;
Vector3 vec0 = Vector3.Zero, vec1 = Vector3.Zero;
TransformUtil.PlaneSpace1(ref planeNormal, ref vec0, ref vec1);
float vecLen = 100f;
Vector3 pt0 = planeOrigin + vec0 * vecLen;
Vector3 pt1 = planeOrigin - vec0 * vecLen;
Vector3 pt2 = planeOrigin + vec1 * vecLen;
Vector3 pt3 = planeOrigin - vec1 * vecLen;
debugDraw.DrawLine(Vector3.Transform(pt0, worldTransform), Vector3.Transform(pt1, worldTransform), color);
debugDraw.DrawLine(Vector3.Transform(pt2, worldTransform), Vector3.Transform(pt3, worldTransform), color);
break;
}
//case (BroadphaseNativeTypes.BOX_SHAPE_PROXYTYPE):
// {
// BoxShape boxShape = (BoxShape)shape;
// Vector3 minPos = Vector3.Zero;
// Vector3 maxPos = Vector3.Zero;
// Matrix transform = Matrix.Identity;
// boxShape.getAabb(ref transform, ref minPos,ref maxPos);
// debugDraw.drawBox(ref minPos, ref maxPos, ref worldTransform, ref color);
// break;
// }
default:
{
if (shape.IsConcave())
{
var concaveMesh = (ConcaveShape)shape;
///@todo pass camera, for some culling? no -> we are not a graphics lib
Vector3 aabbMax = MathUtil.MAX_VECTOR;
Vector3 aabbMin = MathUtil.MIN_VECTOR;
var drawCallback = new DebugDrawcallback(debugDraw, ref worldTransform, ref color);
concaveMesh.ProcessAllTriangles(drawCallback, ref aabbMin, ref aabbMax);
drawCallback.Cleanup();
}
else if (shape.ShapeType == BroadphaseNativeTypes.CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE)
{
var convexMesh = (ConvexTriangleMeshShape)shape;
//todo: pass camera for some culling
Vector3 aabbMax = MathUtil.MAX_VECTOR;
Vector3 aabbMin = MathUtil.MIN_VECTOR;
//DebugDrawcallback drawCallback;
var drawCallback = new DebugDrawcallback(debugDraw, ref worldTransform, ref color);
convexMesh.GetMeshInterface().InternalProcessAllTriangles(drawCallback, ref aabbMin, ref aabbMax);
drawCallback.Cleanup();
}
else /// for polyhedral shapes
if (shape.IsPolyhedral())
{
var polyshape = (PolyhedralConvexShape)shape;
for (int i = 0; i < polyshape.GetNumEdges(); i++)
{
Vector3 a = Vector3.Zero, b = Vector3.Zero;
polyshape.GetEdge(i, ref a, ref b);
Vector3 wa = Vector3.Transform(a, worldTransform);
Vector3 wb = Vector3.Transform(b, worldTransform);
debugDraw.DrawLine(ref wa, ref wb, ref color);
}
}
break;
}
}
}
}
public static void DrawAabb(IDebugDraw debugDrawer, Vector3 from, Vector3 to, Vector3 color)
{
Vector3 halfExtents = (to - from) * 0.5f;
Vector3 center = (to + from) * 0.5f;
Vector3 edgecoord = new Vector3(1f, 1f, 1f), pa, pb;
for (int i = 0; i < 4; i++)
{
for (int j = 0; j < 3; j++)
{
pa = new Vector3(edgecoord.X * halfExtents.X, edgecoord.Y * halfExtents.Y,
edgecoord.Z * halfExtents.Z);
pa += center;
int othercoord = j % 3;
MathHelper.SetElement(ref edgecoord, othercoord, MathHelper.GetElement(edgecoord, othercoord) * -1f);
pb = new Vector3(edgecoord.X * halfExtents.X, edgecoord.Y * halfExtents.Y,
edgecoord.Z * halfExtents.Z);
pb += center;
debugDrawer.DrawLine(pa, pb, color);
}
edgecoord = new Vector3(-1f, -1f, -1f);
if (i < 3)
MathHelper.SetElement(ref edgecoord, i, MathHelper.GetElement(edgecoord, i) * -1f);
}
}
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 void Draw(IDebugDraw drawer)
{
int i;
for (i = 0; i < NumRays; i++)
{
drawer.DrawLine(ref _source[i], ref _hitPoint[i], ref green);
}
for (i = 0; i < NumRays; i++)
{
Vector3 to = _hitPoint[i] + _normal[i];
drawer.DrawLine(ref _hitPoint[i], ref to, ref white);
}
}
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;
}
