public BridgeTriangleConcaveRaycastCallback(ref Vector3 from, ref Vector3 to,
RayResultCallback resultCallback, CollisionObject collisionObject, ConcaveShape triangleMesh, ref Matrix colObjWorldTransform) :
//@BP Mod
base(ref from, ref to, resultCallback.m_flags)
{
m_resultCallback = resultCallback;
m_collisionObject = collisionObject;
m_triangleMesh = triangleMesh;
m_colObjWorldTransform = colObjWorldTransform;
}
public SingleRayCallback(ref Vector3 rayFromWorld,ref Vector3 rayToWorld,CollisionWorld world,RayResultCallback resultCallback)
{
m_rayFromWorld = rayFromWorld;
m_rayToWorld = rayToWorld;
m_world = world;
m_resultCallback = resultCallback;
m_rayFromTrans = Matrix.Identity;
m_rayFromTrans.Translation = m_rayFromWorld;
m_rayToTrans = Matrix.Identity;
m_rayToTrans.Translation = m_rayToWorld;
Vector3 rayDir = (rayToWorld-rayFromWorld);
rayDir.Normalize();
///what about division by zero? -. just set rayDirection[i] to INF/1e30
m_rayDirectionInverse.X = MathUtil.FuzzyZero(rayDir.X) ? float.MaxValue : 1f / rayDir.X;
m_rayDirectionInverse.Y = MathUtil.FuzzyZero(rayDir.Y) ? float.MaxValue : 1f / rayDir.Y;
m_rayDirectionInverse.Z = MathUtil.FuzzyZero(rayDir.Z) ? float.MaxValue : 1f / rayDir.Z;
m_signs[0] = m_rayDirectionInverse.X < 0.0f;
m_signs[1] = m_rayDirectionInverse.Y < 0.0f;
m_signs[2] = m_rayDirectionInverse.Z < 0.0f;
m_lambda_max = Vector3.Dot(rayDir,(m_rayToWorld-m_rayFromWorld));
}
public void RayTest(ref Vector3 rayFromWorld, ref Vector3 rayToWorld, RayResultCallback resultCallback)
{
}
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();
}
}
}
}
}
/// rayTest performs a raycast on all objects in the btCollisionWorld, and calls the resultCallback
/// This allows for several queries: first hit, all hits, any hit, dependent on the value returned by the callback.
public virtual void RayTest(ref Vector3 rayFromWorld, ref Vector3 rayToWorld, RayResultCallback resultCallback)
{
//BT_PROFILE("rayTest");
/// use the broadphase to accelerate the search for objects, based on their aabb
/// and for each object with ray-aabb overlap, perform an exact ray test
SingleRayCallback rayCB = new SingleRayCallback(ref rayFromWorld,ref rayToWorld,this,resultCallback);
#if !USE_BRUTEFORCE_RAYBROADPHASE
m_broadphasePairCache.RayTest(ref rayFromWorld,ref rayToWorld,rayCB);
rayCB.Cleanup();
#else
for (int i=0;i<GetNumCollisionObjects();i++)
{
rayCB.Process(m_collisionObjects[i].GetBroadphaseHandle());
}
#endif //USE_BRUTEFORCE_RAYBROADPHASE
}