public void AddChildShape(ref Matrix localTransform, CollisionShape shape)
{
m_updateRevision++;
//m_childTransforms.push_back(localTransform);
//m_childShapes.push_back(shape);
CompoundShapeChild child = new CompoundShapeChild();
child.m_transform = localTransform;
child.m_childShape = shape;
child.m_childShapeType = shape.ShapeType;
child.m_childMargin = shape.Margin;
//extend the local aabbMin/aabbMax
Vector3 localAabbMin = new Vector3();
Vector3 localAabbMax = new Vector3();
shape.GetAabb(ref localTransform, ref localAabbMin, ref localAabbMax);
MathUtil.VectorMin(ref localAabbMin, ref m_localAabbMin);
MathUtil.VectorMax(ref localAabbMax, ref m_localAabbMax);
if (m_dynamicAabbTree != null)
{
DbvtAabbMm bounds = DbvtAabbMm.FromMM(ref localAabbMin, ref localAabbMax);
int index = m_children.Count;
child.m_treeNode = m_dynamicAabbTree.Insert(ref bounds, (Object)index);
}
m_children.Add(child);
}
public void ProcessChildShape(CollisionShape childShape,int index)
{
CompoundShape compoundShape = (CompoundShape)(m_compoundColObj.GetCollisionShape());
//backup
Matrix orgTrans = m_compoundColObj.GetWorldTransform();
Matrix orgInterpolationTrans = m_compoundColObj.GetInterpolationWorldTransform();
Matrix childTrans = compoundShape.GetChildTransform(index);
Matrix newChildWorldTrans = MathUtil.BulletMatrixMultiply(ref orgTrans,ref childTrans);
//perform an AABB check first
Vector3 aabbMin0 = Vector3.Zero;
Vector3 aabbMax0 = Vector3.Zero;
Vector3 aabbMin1 = Vector3.Zero;
Vector3 aabbMax1 = Vector3.Zero;
childShape.GetAabb(ref newChildWorldTrans,ref aabbMin0,ref aabbMax0);
m_otherObj.GetCollisionShape().GetAabb(m_otherObj.GetWorldTransform(),ref aabbMin1,ref aabbMax1);
if (AabbUtil2.TestAabbAgainstAabb2(ref aabbMin0,ref aabbMax0,ref aabbMin1,ref aabbMax1))
{
m_compoundColObj.SetWorldTransform(ref newChildWorldTrans);
m_compoundColObj.SetInterpolationWorldTransform(ref newChildWorldTrans);
//the contactpoint is still projected back using the original inverted worldtrans
CollisionShape tmpShape = m_compoundColObj.GetCollisionShape();
m_compoundColObj.InternalSetTemporaryCollisionShape( childShape );
if (m_childCollisionAlgorithms[index] == null)
{
m_childCollisionAlgorithms[index] = m_dispatcher.FindAlgorithm(m_compoundColObj,m_otherObj,m_sharedManifold);
}
///detect swapping case
if (m_resultOut.GetBody0Internal() == m_compoundColObj)
{
m_resultOut.SetShapeIdentifiersA(-1, index);
}
else
{
m_resultOut.SetShapeIdentifiersB(-1, index);
}
m_childCollisionAlgorithms[index].ProcessCollision(m_compoundColObj,m_otherObj,m_dispatchInfo, m_resultOut);
if (m_dispatchInfo.getDebugDraw() != null && (((m_dispatchInfo.getDebugDraw().GetDebugMode() & DebugDrawModes.DBG_DrawAabb)) != 0))
{
Vector3 worldAabbMin = Vector3.Zero, worldAabbMax = Vector3.Zero;
m_dispatchInfo.getDebugDraw().DrawAabb(aabbMin0, aabbMax0, new Vector3(1, 1, 1));
m_dispatchInfo.getDebugDraw().DrawAabb(aabbMin1, aabbMax1, new Vector3(1, 1, 1));
}
//revert back transform
m_compoundColObj.InternalSetTemporaryCollisionShape( tmpShape);
m_compoundColObj.SetWorldTransform( ref orgTrans );
m_compoundColObj.SetInterpolationWorldTransform(ref orgInterpolationTrans);
}
}
public RigidBodyConstructionInfo(float mass, IMotionState motionState, CollisionShape collisionShape, Vector3 localInertia)
{
m_mass = mass;
m_motionState = motionState;
m_collisionShape = collisionShape;
m_localInertia = localInertia;
m_linearDamping = 0f;
m_angularDamping = 0f;
m_friction = 0.5f;
m_restitution = 0f;
m_linearSleepingThreshold = 0.8f;
m_angularSleepingThreshold = 1f;
m_additionalDamping = false;
m_additionalDampingFactor = 0.005f;
m_additionalLinearDampingThresholdSqr = 0.01f;
m_additionalAngularDampingThresholdSqr = 0.01f;
m_additionalAngularDampingFactor = 0.01f;
m_startWorldTransform = Matrix.Identity;
}
/// Remove all children shapes that contain the specified shape
public virtual void RemoveChildShape(CollisionShape shape)
{
m_updateRevision++;
// Find the children containing the shape specified, and remove those children.
//note: there might be multiple children using the same shape!
for (int i = m_children.Count - 1; i >= 0; i--)
{
if (m_children[i].m_childShape == shape)
{
RemoveChildShapeByIndex(i);
}
}
RecalculateLocalAabb();
}
public RigidBodyConstructionInfo(float mass, IMotionState motionState, CollisionShape collisionShape)
: this(mass, motionState, collisionShape, new Vector3(0, 0, 0))
{
}
public virtual void SetCollisionShape(CollisionShape collisionShape)
{
m_collisionShape = collisionShape;
m_rootCollisionShape = collisionShape;
}
///Avoid using this internal API call
///internalSetTemporaryCollisionShape is used to temporary replace the actual collision shape by a child collision shape.
public void InternalSetTemporaryCollisionShape(CollisionShape collisionShape)
{
m_collisionShape = collisionShape;
}
/// objectQuerySingle performs a collision detection query and calls the resultCallback. It is used internally by rayTest.
public static void ObjectQuerySingle(ConvexShape castShape,ref Matrix convexFromTrans,ref Matrix convexToTrans,
CollisionObject collisionObject,CollisionShape collisionShape,
ref Matrix colObjWorldTransform,
ConvexResultCallback resultCallback, float allowedPenetration)
{
if (collisionShape.IsConvex())
{
//BT_PROFILE("convexSweepConvex");
CastResult castResult = new CastResult();
castResult.m_allowedPenetration = allowedPenetration;
castResult.m_fraction = resultCallback.m_closestHitFraction;//float(1.);//??
ConvexShape convexShape = (ConvexShape) collisionShape;
VoronoiSimplexSolver simplexSolver = new VoronoiSimplexSolver();
GjkEpaPenetrationDepthSolver gjkEpaPenetrationSolver = new GjkEpaPenetrationDepthSolver();
ContinuousConvexCollision convexCaster1 = new ContinuousConvexCollision(castShape,convexShape,simplexSolver, gjkEpaPenetrationSolver);
//btGjkConvexCast convexCaster2(castShape,convexShape,&simplexSolver);
//btSubsimplexConvexCast convexCaster3(castShape,convexShape,&simplexSolver);
IConvexCast castPtr = convexCaster1;
if (castPtr.CalcTimeOfImpact(ref convexFromTrans,ref convexToTrans,ref colObjWorldTransform,ref colObjWorldTransform,castResult))
{
//add hit
if (castResult.m_normal.LengthSquared() > 0.0001f)
{
if (castResult.m_fraction < resultCallback.m_closestHitFraction)
{
castResult.m_normal.Normalize();
LocalConvexResult localConvexResult = new LocalConvexResult
(
collisionObject,
null,
ref castResult.m_normal,
ref castResult.m_hitPoint,
castResult.m_fraction
);
bool normalInWorldSpace = true;
resultCallback.AddSingleResult(localConvexResult, normalInWorldSpace);
}
}
}
}
else
{
if (collisionShape.IsConcave())
{
if (collisionShape.ShapeType==BroadphaseNativeTypes.TRIANGLE_MESH_SHAPE_PROXYTYPE)
{
//BT_PROFILE("convexSweepbtBvhTriangleMesh");
BvhTriangleMeshShape triangleMesh = (BvhTriangleMeshShape)collisionShape;
Matrix worldTocollisionObject = Matrix.Invert(colObjWorldTransform);
Vector3 convexFromLocal = Vector3.Transform(convexFromTrans.Translation,worldTocollisionObject);
Vector3 convexToLocal = Vector3.Transform(convexToTrans.Translation,worldTocollisionObject);
// rotation of box in local mesh space = MeshRotation^-1 * ConvexToRotation
Matrix rotationXform = MathUtil.BasisMatrix(ref worldTocollisionObject) * MathUtil.BasisMatrix(ref convexToTrans);
BridgeTriangleConvexcastCallback tccb = new BridgeTriangleConvexcastCallback(castShape, ref convexFromTrans,ref convexToTrans,resultCallback,collisionObject,triangleMesh, ref colObjWorldTransform);
tccb.m_hitFraction = resultCallback.m_closestHitFraction;
Vector3 boxMinLocal = new Vector3();
Vector3 boxMaxLocal = new Vector3();
castShape.GetAabb(ref rotationXform, ref boxMinLocal, ref boxMaxLocal);
triangleMesh.PerformConvexCast(tccb,ref convexFromLocal,ref convexToLocal,ref boxMinLocal, ref boxMaxLocal);
}
else
{
//BT_PROFILE("convexSweepConcave");
ConcaveShape concaveShape = (ConcaveShape)collisionShape;
Matrix worldTocollisionObject = Matrix.Invert(colObjWorldTransform);
Vector3 convexFromLocal = Vector3.Transform(convexFromTrans.Translation,worldTocollisionObject);
Vector3 convexToLocal = Vector3.Transform(convexToTrans.Translation,worldTocollisionObject);
// rotation of box in local mesh space = MeshRotation^-1 * ConvexToRotation
Matrix rotationXform = MathUtil.BasisMatrix(ref worldTocollisionObject) * MathUtil.BasisMatrix(ref convexToTrans);
BridgeTriangleConvexcastCallback2 tccb = new BridgeTriangleConvexcastCallback2(castShape, ref convexFromTrans, ref convexToTrans, resultCallback, collisionObject, concaveShape, ref colObjWorldTransform);
tccb.m_hitFraction = resultCallback.m_closestHitFraction;
Vector3 boxMinLocal = new Vector3();
Vector3 boxMaxLocal = new Vector3();
castShape.GetAabb(ref rotationXform, ref boxMinLocal, ref boxMaxLocal);
Vector3 rayAabbMinLocal = convexFromLocal;
MathUtil.VectorMin(ref convexToLocal,ref rayAabbMinLocal);
//rayAabbMinLocal.setMin(convexToLocal);
Vector3 rayAabbMaxLocal = convexFromLocal;
//rayAabbMaxLocal.setMax(convexToLocal);
MathUtil.VectorMax(ref convexToLocal,ref rayAabbMaxLocal);
rayAabbMinLocal += boxMinLocal;
rayAabbMaxLocal += boxMaxLocal;
concaveShape.ProcessAllTriangles(tccb,ref rayAabbMinLocal,ref rayAabbMaxLocal);
}
}
else
{
///@todo : use AABB tree or other BVH acceleration structure!
if (collisionShape.IsCompound())
{
//BT_PROFILE("convexSweepCompound");
CompoundShape compoundShape = (CompoundShape)collisionShape;
for (int i=0;i<compoundShape.GetNumChildShapes();i++)
{
Matrix childTrans = compoundShape.GetChildTransform(i);
CollisionShape childCollisionShape = compoundShape.GetChildShape(i);
Matrix childWorldTrans = MathUtil.BulletMatrixMultiply(colObjWorldTransform,childTrans);
// replace collision shape so that callback can determine the triangle
CollisionShape saveCollisionShape = collisionObject.GetCollisionShape();
collisionObject.InternalSetTemporaryCollisionShape((CollisionShape)childCollisionShape);
LocalInfoAdder my_cb = new LocalInfoAdder(i, resultCallback);
my_cb.m_closestHitFraction = resultCallback.m_closestHitFraction;
ObjectQuerySingle(castShape, ref convexFromTrans,ref convexToTrans,
collisionObject,
childCollisionShape,
ref childWorldTrans,
my_cb, allowedPenetration);
// restore
collisionObject.InternalSetTemporaryCollisionShape(saveCollisionShape);
}
}
}
}
}
public virtual void RayTestSingle(ref Matrix rayFromTrans,ref Matrix rayToTrans,
CollisionObject collisionObject,
CollisionShape collisionShape,
ref Matrix colObjWorldTransform,
RayResultCallback resultCallback)
{
SphereShape pointShape = new SphereShape(0.0f);
pointShape.Margin = 0f;
ConvexShape castShape = pointShape;
if (collisionShape.IsConvex())
{
// BT_PROFILE("rayTestConvex");
CastResult castResult = new CastResult();
castResult.m_fraction = resultCallback.m_closestHitFraction;
ConvexShape convexShape = (ConvexShape)collisionShape;
VoronoiSimplexSolver simplexSolver = new VoronoiSimplexSolver();
//#define USE_SUBSIMPLEX_CONVEX_CAST 1
//#ifdef USE_SUBSIMPLEX_CONVEX_CAST
// FIXME - MAN - convexcat here seems to make big difference to forklift.
SubSimplexConvexCast convexCaster = new SubSimplexConvexCast(castShape, convexShape, simplexSolver);
//GjkConvexCast convexCaster = new GjkConvexCast(castShape, convexShape, simplexSolver);
//#else
//btGjkConvexCast convexCaster(castShape,convexShape,&simplexSolver);
//btContinuousConvexCollision convexCaster(castShape,convexShape,&simplexSolver,0);
//#endif //#USE_SUBSIMPLEX_CONVEX_CAST
if (convexCaster.CalcTimeOfImpact(ref rayFromTrans,ref rayToTrans,ref colObjWorldTransform,ref colObjWorldTransform,castResult))
{
//add hit
if (castResult.m_normal.LengthSquared() > 0.0001f)
{
if (castResult.m_fraction < resultCallback.m_closestHitFraction)
{
//if (resultCallback.m_closestHitFraction != 1f)
//{
// int ibreak = 0;
// convexCaster.calcTimeOfImpact(ref rayFromTrans, ref rayToTrans, ref colObjWorldTransform, ref colObjWorldTransform, castResult);
//}
//#ifdef USE_SUBSIMPLEX_CONVEX_CAST
//rotate normal into worldspace
castResult.m_normal = Vector3.TransformNormal(castResult.m_normal,rayFromTrans);
//#endif //USE_SUBSIMPLEX_CONVEX_CAST
castResult.m_normal.Normalize();
LocalRayResult localRayResult = new LocalRayResult(
collisionObject,
null,
ref castResult.m_normal,
castResult.m_fraction
);
bool normalInWorldSpace = true;
resultCallback.AddSingleResult(localRayResult, normalInWorldSpace);
}
}
}
castResult.Cleanup();
}
else
{
if (collisionShape.IsConcave())
{
// BT_PROFILE("rayTestConcave");
if (collisionShape.ShapeType==BroadphaseNativeTypes.TRIANGLE_MESH_SHAPE_PROXYTYPE && collisionShape is BvhTriangleMeshShape)
{
///optimized version for btBvhTriangleMeshShape
BvhTriangleMeshShape triangleMesh = (BvhTriangleMeshShape)collisionShape;
Matrix worldTocollisionObject = Matrix.Invert(colObjWorldTransform);
Vector3 rayFromLocal = Vector3.Transform(rayFromTrans.Translation,worldTocollisionObject);
Vector3 rayToLocal = Vector3.Transform(rayToTrans.Translation,worldTocollisionObject);
Matrix transform = Matrix.Identity;
BridgeTriangleRaycastCallback rcb = new BridgeTriangleRaycastCallback(ref rayFromLocal,ref rayToLocal, resultCallback,collisionObject,triangleMesh,ref transform);
rcb.m_hitFraction = resultCallback.m_closestHitFraction;
triangleMesh.PerformRaycast(rcb,ref rayFromLocal,ref rayToLocal);
rcb.Cleanup();
}
else
{
//generic (slower) case
ConcaveShape concaveShape = (ConcaveShape)collisionShape;
Matrix worldTocollisionObject = Matrix.Invert(colObjWorldTransform);
Vector3 rayFromLocal = Vector3.Transform(rayFromTrans.Translation,worldTocollisionObject);
Vector3 rayToLocal = Vector3.Transform(rayToTrans.Translation,worldTocollisionObject);
//ConvexCast::CastResult
Matrix transform = Matrix.Identity;
BridgeTriangleConcaveRaycastCallback rcb = new BridgeTriangleConcaveRaycastCallback(ref rayFromLocal, ref rayToLocal, resultCallback, collisionObject, concaveShape,ref transform);
rcb.m_hitFraction = resultCallback.m_closestHitFraction;
Vector3 rayAabbMinLocal = rayFromLocal;
MathUtil.VectorMin(ref rayToLocal,ref rayAabbMinLocal);
Vector3 rayAabbMaxLocal = rayFromLocal;
MathUtil.VectorMax(ref rayToLocal,ref rayAabbMaxLocal);
concaveShape.ProcessAllTriangles(rcb,ref rayAabbMinLocal,ref rayAabbMaxLocal);
rcb.Cleanup();
}
}
else
{
// BT_PROFILE("rayTestCompound");
///@todo: use AABB tree or other BVH acceleration structure, see btDbvt
if (collisionShape.IsCompound())
{
CompoundShape compoundShape = (CompoundShape)(collisionShape);
int i=0;
for (i=0;i<compoundShape.GetNumChildShapes();i++)
{
Matrix childTrans = compoundShape.GetChildTransform(i);
CollisionShape childCollisionShape = compoundShape.GetChildShape(i);
Matrix childWorldTrans = MathUtil.BulletMatrixMultiply(colObjWorldTransform,childTrans) ;
// replace collision shape so that callback can determine the triangle
CollisionShape saveCollisionShape = collisionObject.GetCollisionShape();
collisionObject.InternalSetTemporaryCollisionShape((CollisionShape)childCollisionShape);
LocalInfoAdder2 my_cb = new LocalInfoAdder2(i, resultCallback);
my_cb.m_closestHitFraction = resultCallback.m_closestHitFraction;
RayTestSingle(ref rayFromTrans,ref rayToTrans,
collisionObject,
childCollisionShape,
ref childWorldTrans,
my_cb);
// restore
collisionObject.InternalSetTemporaryCollisionShape(saveCollisionShape);
my_cb.cleanup();
}
}
}
}
}
public virtual void DebugDrawObject(ref Matrix worldTransform, CollisionShape shape, ref Vector3 color)
{
}
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;
}
}
}
}
///btRigidBody constructor for backwards compatibility.
///To specify friction (etc) during rigid body construction, please use the other constructor (using btRigidBodyConstructionInfo)
public RigidBody(float mass, IMotionState motionState, CollisionShape collisionShape, Vector3 localInertia)
{
RigidBodyConstructionInfo cinfo = new RigidBodyConstructionInfo(mass, motionState, collisionShape, localInertia);
SetupRigidBody(cinfo);
}