// rayTestSingle performs a raycast call and calls the resultCallback. It is used internally by rayTest.
// In a future implementation, we consider moving the ray test as a virtual method in CollisionShape.
// This allows more customization.
public static void RayTestSingle(Matrix rayFromTrans, Matrix rayToTrans,
CollisionObject collisionObject,
CollisionShape collisionShape,
Matrix colObjWorldTransform,
RayResultCallback resultCallback)
{
SphereShape pointShape=new SphereShape(0.0f);
if (collisionShape.IsConvex)
{
CastResult castResult = new CastResult();
castResult.Fraction = 1f;//??
ConvexShape convexShape = collisionShape as ConvexShape;
VoronoiSimplexSolver simplexSolver = new VoronoiSimplexSolver();
SubsimplexConvexCast convexCaster = new SubsimplexConvexCast(pointShape, convexShape, simplexSolver);
//GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver);
//ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0);
if (convexCaster.CalcTimeOfImpact(rayFromTrans, rayToTrans, colObjWorldTransform, colObjWorldTransform, castResult))
{
//add hit
if (castResult.Normal.LengthSquared() > 0.0001f)
{
castResult.Normal.Normalize();
if (castResult.Fraction < resultCallback.ClosestHitFraction)
{
CollisionWorld.LocalRayResult localRayResult = new LocalRayResult
(
collisionObject,
new LocalShapeInfo(),
castResult.Normal,
castResult.Fraction
);
resultCallback.AddSingleResult(localRayResult);
}
}
}
else
{
if (collisionShape.IsConcave)
{
TriangleMeshShape triangleMesh = collisionShape as TriangleMeshShape;
Matrix worldTocollisionObject = MathHelper.InvertMatrix(colObjWorldTransform);
Vector3 rayFromLocal = Vector3.TransformNormal(rayFromTrans.Translation, worldTocollisionObject);
Vector3 rayToLocal = Vector3.TransformNormal(rayToTrans.Translation, worldTocollisionObject);
BridgeTriangleRaycastCallback rcb = new BridgeTriangleRaycastCallback(rayFromLocal, rayToLocal, resultCallback, collisionObject, triangleMesh);
rcb.HitFraction = resultCallback.ClosestHitFraction;
Vector3 rayAabbMinLocal = rayFromLocal;
MathHelper.SetMin(ref rayAabbMinLocal, rayToLocal);
Vector3 rayAabbMaxLocal = rayFromLocal;
MathHelper.SetMax(ref rayAabbMaxLocal, rayToLocal);
triangleMesh.ProcessAllTriangles(rcb, rayAabbMinLocal, rayAabbMaxLocal);
}
else
{
//todo: use AABB tree or other BVH acceleration structure!
if (collisionShape.IsCompound)
{
CompoundShape compoundShape = collisionShape as CompoundShape;
for (int i = 0; i < compoundShape.ChildShapeCount; i++)
{
Matrix childTrans = compoundShape.GetChildTransform(i);
CollisionShape childCollisionShape = compoundShape.GetChildShape(i);
Matrix childWorldTrans = colObjWorldTransform * childTrans;
RayTestSingle(rayFromTrans, rayToTrans,
collisionObject,
childCollisionShape,
childWorldTrans,
resultCallback);
}
}
}
}
}
}
/// <summary>
/// Creates new rigid body and adds it to the world.
/// </summary>
/// <param name="mass">Body mass, if 0 body is static.</param>
/// <param name="startTransform">Starting body transform.</param>
/// <param name="shape">Body shape.</param>
/// <returns>Created rigid body.</returns>
public RigidBody CreateRigidBody(float mass, Matrix startTransform, CollisionShape shape)
{
//rigidbody is dynamic if and only if mass is non zero, otherwise static
bool isDynamic = (mass != 0.0f);
Vector3 localInertia = new Vector3();
if (isDynamic)
shape.CalculateLocalInertia(mass, out localInertia);
//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
DefaultMotionState myMotionState = new DefaultMotionState(startTransform, Matrix.Identity);
RigidBody body = new RigidBody(mass, myMotionState, shape, localInertia, 0.0f, 0.0f, 0.5f, 0.0f);
_world.AddRigidBody(body);
return body;
}
public void AddChildShape(Matrix localTransform, CollisionShape shape)
{
_childTransforms.Add(localTransform);
_childShapes.Add(shape);
//extend the local aabbMin/aabbMax
Vector3 localAabbMin, localAabbMax;
shape.GetAabb(localTransform, out localAabbMin, out localAabbMax);
if (_localAabbMin.X > localAabbMin.X)
{
_localAabbMin.X = localAabbMin.X;
}
if (_localAabbMax.X < localAabbMax.X)
{
_localAabbMax.X = localAabbMax.X;
}
if (_localAabbMin.Y > localAabbMin.Y)
{
_localAabbMin.Y = localAabbMin.Y;
}
if (_localAabbMax.Y < localAabbMax.Y)
{
_localAabbMax.Y = localAabbMax.Y;
}
if (_localAabbMin.Z > localAabbMin.Z)
{
_localAabbMin.Z = localAabbMin.Z;
}
if (_localAabbMax.Z < localAabbMax.Z)
{
_localAabbMax.Z = localAabbMax.Z;
}
}
// rayTestSingle performs a raycast call and calls the resultCallback. It is used internally by rayTest.
// In a future implementation, we consider moving the ray test as a virtual method in CollisionShape.
// This allows more customization.
public static void RayTestSingle(Matrix rayFromTrans, Matrix rayToTrans,
CollisionObject collisionObject,
CollisionShape collisionShape,
Matrix colObjWorldTransform,
RayResultCallback resultCallback)
{
SphereShape pointShape = new SphereShape(0.0f);
if (collisionShape.IsConvex)
{
CastResult castResult = new CastResult();
castResult.Fraction = 1f; //??
ConvexShape convexShape = collisionShape as ConvexShape;
VoronoiSimplexSolver simplexSolver = new VoronoiSimplexSolver();
SubsimplexConvexCast convexCaster = new SubsimplexConvexCast(pointShape, convexShape, simplexSolver);
//GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver);
//ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0);
if (convexCaster.CalcTimeOfImpact(rayFromTrans, rayToTrans, colObjWorldTransform, colObjWorldTransform, castResult))
{
//add hit
if (castResult.Normal.LengthSquared() > 0.0001f)
{
castResult.Normal.Normalize();
if (castResult.Fraction < resultCallback.ClosestHitFraction)
{
CollisionWorld.LocalRayResult localRayResult = new LocalRayResult
(
collisionObject,
new LocalShapeInfo(),
castResult.Normal,
castResult.Fraction
);
resultCallback.AddSingleResult(localRayResult);
}
}
}
else
{
if (collisionShape.IsConcave)
{
TriangleMeshShape triangleMesh = collisionShape as TriangleMeshShape;
Matrix worldTocollisionObject = MathHelper.InvertMatrix(colObjWorldTransform);
Vector3 rayFromLocal = Vector3.TransformNormal(rayFromTrans.Translation, worldTocollisionObject);
Vector3 rayToLocal = Vector3.TransformNormal(rayToTrans.Translation, worldTocollisionObject);
BridgeTriangleRaycastCallback rcb = new BridgeTriangleRaycastCallback(rayFromLocal, rayToLocal, resultCallback, collisionObject, triangleMesh);
rcb.HitFraction = resultCallback.ClosestHitFraction;
Vector3 rayAabbMinLocal = rayFromLocal;
MathHelper.SetMin(ref rayAabbMinLocal, rayToLocal);
Vector3 rayAabbMaxLocal = rayFromLocal;
MathHelper.SetMax(ref rayAabbMaxLocal, rayToLocal);
triangleMesh.ProcessAllTriangles(rcb, rayAabbMinLocal, rayAabbMaxLocal);
}
else
{
//todo: use AABB tree or other BVH acceleration structure!
if (collisionShape.IsCompound)
{
CompoundShape compoundShape = collisionShape as CompoundShape;
for (int i = 0; i < compoundShape.ChildShapeCount; i++)
{
Matrix childTrans = compoundShape.GetChildTransform(i);
CollisionShape childCollisionShape = compoundShape.GetChildShape(i);
Matrix childWorldTrans = colObjWorldTransform * childTrans;
RayTestSingle(rayFromTrans, rayToTrans,
collisionObject,
childCollisionShape,
childWorldTrans,
resultCallback);
}
}
}
}
}
}