public static void RayTestSingle(ref IndexedMatrix rayFromTrans, ref IndexedMatrix rayToTrans,
CollisionObject collisionObject,
CollisionShape collisionShape,
ref IndexedMatrix colObjWorldTransform,
RayResultCallback resultCallback)
{
SphereShape pointShape = BulletGlobals.SphereShapePool.Get();
pointShape.Initialize(0.0f);
pointShape.SetMargin(0f);
ConvexShape castShape = pointShape;
if (collisionShape.IsConvex())
{
BulletGlobals.StartProfile("rayTestConvex");
CastResult castResult = BulletGlobals.CastResultPool.Get();
castResult.m_fraction = resultCallback.m_closestHitFraction;
ConvexShape convexShape = collisionShape as ConvexShape;
VoronoiSimplexSolver simplexSolver = BulletGlobals.VoronoiSimplexSolverPool.Get();
//#define USE_SUBSIMPLEX_CONVEX_CAST 1
//#ifdef USE_SUBSIMPLEX_CONVEX_CAST
// FIXME - MAN - convexcat here seems to make big difference to forklift.
SubSimplexConvexCast convexCaster = BulletGlobals.SubSimplexConvexCastPool.Get();
convexCaster.Initialize(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 = rayFromTrans._basis * castResult.m_normal;
//#endif //USE_SUBSIMPLEX_CONVEX_CAST
castResult.m_normal.Normalize();
LocalRayResult localRayResult = new LocalRayResult(
collisionObject,
//null, // updated to allow different ctor on struct
ref castResult.m_normal,
castResult.m_fraction
);
bool normalInWorldSpace = true;
resultCallback.AddSingleResult(ref localRayResult, normalInWorldSpace);
}
}
}
castResult.Cleanup();
BulletGlobals.SubSimplexConvexCastPool.Free(convexCaster);
BulletGlobals.VoronoiSimplexSolverPool.Free(simplexSolver);
BulletGlobals.StopProfile();
}
else
{
if (collisionShape.IsConcave())
{
BulletGlobals.StartProfile("rayTestConcave");
if (collisionShape.GetShapeType() == BroadphaseNativeTypes.TRIANGLE_MESH_SHAPE_PROXYTYPE && collisionShape is BvhTriangleMeshShape)
{
///optimized version for btBvhTriangleMeshShape
BvhTriangleMeshShape triangleMesh = (BvhTriangleMeshShape)collisionShape;
IndexedMatrix worldTocollisionObject = colObjWorldTransform.Inverse();
IndexedVector3 rayFromLocal = worldTocollisionObject * rayFromTrans._origin;
IndexedVector3 rayToLocal = worldTocollisionObject * rayToTrans._origin;
IndexedMatrix transform = IndexedMatrix.Identity;
using (BridgeTriangleRaycastCallback rcb = BulletGlobals.BridgeTriangleRaycastCallbackPool.Get())
{
rcb.Initialize(ref rayFromLocal, ref rayToLocal, resultCallback, collisionObject, triangleMesh, ref transform);
rcb.m_hitFraction = resultCallback.m_closestHitFraction;
triangleMesh.PerformRaycast(rcb, ref rayFromLocal, ref rayToLocal);
}
}
else if (collisionShape.GetShapeType() == BroadphaseNativeTypes.TERRAIN_SHAPE_PROXYTYPE && collisionShape is HeightfieldTerrainShape)
{
///optimized version for btBvhTriangleMeshShape
HeightfieldTerrainShape heightField = (HeightfieldTerrainShape)collisionShape;
IndexedMatrix worldTocollisionObject = colObjWorldTransform.Inverse();
IndexedVector3 rayFromLocal = worldTocollisionObject * rayFromTrans._origin;
IndexedVector3 rayToLocal = worldTocollisionObject * rayToTrans._origin;
IndexedMatrix transform = IndexedMatrix.Identity;
using (BridgeTriangleConcaveRaycastCallback rcb = BulletGlobals.BridgeTriangleConcaveRaycastCallbackPool.Get())
{
rcb.Initialize(ref rayFromLocal, ref rayToLocal, resultCallback, collisionObject, heightField, ref transform);
rcb.m_hitFraction = resultCallback.m_closestHitFraction;
heightField.PerformRaycast(rcb, ref rayFromLocal, ref rayToLocal);
}
}
else
{
//generic (slower) case
ConcaveShape concaveShape = (ConcaveShape)collisionShape;
IndexedMatrix worldTocollisionObject = colObjWorldTransform.Inverse();
IndexedVector3 rayFromLocal = worldTocollisionObject * rayFromTrans._origin;
IndexedVector3 rayToLocal = worldTocollisionObject * rayToTrans._origin;
//ConvexCast::CastResult
IndexedMatrix transform = IndexedMatrix.Identity;
using (BridgeTriangleConcaveRaycastCallback rcb = BulletGlobals.BridgeTriangleConcaveRaycastCallbackPool.Get())
{
rcb.Initialize(ref rayFromLocal, ref rayToLocal, resultCallback, collisionObject, concaveShape, ref transform);
rcb.m_hitFraction = resultCallback.m_closestHitFraction;
IndexedVector3 rayAabbMinLocal = rayFromLocal;
MathUtil.VectorMin(ref rayToLocal, ref rayAabbMinLocal);
IndexedVector3 rayAabbMaxLocal = rayFromLocal;
MathUtil.VectorMax(ref rayToLocal, ref rayAabbMaxLocal);
concaveShape.ProcessAllTriangles(rcb, ref rayAabbMinLocal, ref rayAabbMaxLocal);
}
}
BulletGlobals.StopProfile();
}
else
{
BulletGlobals.StartProfile("rayTestCompound");
///@todo: use AABB tree or other BVH acceleration structure, see btDbvt
if (collisionShape.IsCompound())
{
CompoundShape compoundShape = collisionShape as CompoundShape;
Dbvt dbvt = compoundShape.GetDynamicAabbTree();
RayTester rayCB = new RayTester(
collisionObject,
compoundShape,
ref colObjWorldTransform,
ref rayFromTrans,
ref rayToTrans,
resultCallback);
#if !DISABLE_DBVT_COMPOUNDSHAPE_RAYCAST_ACCELERATION
if (dbvt != null)
{
IndexedVector3 localRayFrom = colObjWorldTransform.InverseTimes(ref rayFromTrans)._origin;
IndexedVector3 localRayTo = colObjWorldTransform.InverseTimes(ref rayToTrans)._origin;
Dbvt.RayTest(dbvt.m_root, ref localRayFrom, ref localRayTo, rayCB);
}
else
#endif //DISABLE_DBVT_COMPOUNDSHAPE_RAYCAST_ACCELERATION
{
for (int i = 0, n = compoundShape.GetNumChildShapes(); i < n; ++i)
{
rayCB.Process(i);
}
}
rayCB.Cleanup();
BulletGlobals.StopProfile();
}
}
}
BulletGlobals.SphereShapePool.Free(pointShape);
}
public RayTester(CollisionObject collisionObject,
CompoundShape compoundShape,
ref IndexedMatrix colObjWorldTransform,
ref IndexedMatrix rayFromTrans,
ref IndexedMatrix rayToTrans,
RayResultCallback resultCallback)
{
m_collisionObject = collisionObject;
m_compoundShape = compoundShape;
m_colObjWorldTransform = colObjWorldTransform;
m_rayFromTrans = rayFromTrans;
m_rayToTrans = rayToTrans;
m_resultCallback = resultCallback;
}
/// 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 IndexedVector3 rayFromWorld, ref IndexedVector3 rayToWorld, RayResultCallback resultCallback)
{
BulletGlobals.StartProfile("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
using (SingleRayCallback rayCB = BulletGlobals.SingleRayCallbackPool.Get())
{
rayCB.Initialize(ref rayFromWorld, ref rayToWorld, this, resultCallback);
#if !USE_BRUTEFORCE_RAYBROADPHASE
m_broadphasePairCache.RayTest(ref rayFromWorld, ref rayToWorld, rayCB);
#else
for (int i=0;i<GetNumCollisionObjects();i++)
{
rayCB.Process(m_collisionObjects[i].GetBroadphaseHandle());
}
#endif //USE_BRUTEFORCE_RAYBROADPHASE
}
BulletGlobals.StopProfile();
}
public void Initialize(ref IndexedVector3 rayFromWorld, ref IndexedVector3 rayToWorld, CollisionWorld world, RayResultCallback resultCallback)
{
m_rayFromWorld = rayFromWorld;
m_rayToWorld = rayToWorld;
m_world = world;
m_resultCallback = resultCallback;
m_rayFromTrans = IndexedMatrix.CreateTranslation(m_rayFromWorld);
m_rayToTrans = IndexedMatrix.CreateTranslation(m_rayToWorld);
IndexedVector3 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 = rayDir.Dot(m_rayToWorld - m_rayFromWorld);
}
public LocalInfoAdder2(int i, RayResultCallback user)
{
m_i = i;
m_userCallback = user;
m_closestHitFraction = user.m_closestHitFraction;
}
public SingleRayCallback(ref IndexedVector3 rayFromWorld, ref IndexedVector3 rayToWorld, CollisionWorld world, RayResultCallback resultCallback)
{
Initialize(ref rayFromWorld,ref rayToWorld,world,resultCallback);
}
public void Initialize(ref IndexedVector3 from, ref IndexedVector3 to,
RayResultCallback resultCallback, CollisionObject collisionObject, ConcaveShape triangleMesh, ref IndexedMatrix colObjWorldTransform)
{
base.Initialize(ref from, ref to, resultCallback.m_flags);
m_resultCallback = resultCallback;
m_collisionObject = collisionObject;
m_triangleMesh = triangleMesh;
m_colObjWorldTransform = colObjWorldTransform;
}
public BridgeTriangleConcaveRaycastCallback() { } // for pool
public BridgeTriangleConcaveRaycastCallback(ref IndexedVector3 from, ref IndexedVector3 to,
RayResultCallback resultCallback, CollisionObject collisionObject, ConcaveShape triangleMesh, ref IndexedMatrix colObjWorldTransform) :
//@BP Mod
base(ref from, ref to, resultCallback.m_flags)
{
m_resultCallback = resultCallback;
m_collisionObject = collisionObject;
m_triangleMesh = triangleMesh;
m_colObjWorldTransform = colObjWorldTransform;
}
public void RayTest(ref IndexedVector3 rayFromWorld, ref IndexedVector3 rayToWorld, RayResultCallback resultCallback)
{
int ibreak = 0;
}
//!@}
//! virtual method for ray collision
public virtual void RayTest(ref IndexedVector3 rayFrom, ref IndexedVector3 rayTo, RayResultCallback resultCallback)
{
}