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);
}
/// objectQuerySingle performs a collision detection query and calls the resultCallback. It is used internally by rayTest.
public static void ObjectQuerySingle(ConvexShape castShape, ref IndexedMatrix convexFromTrans, ref IndexedMatrix convexToTrans,
CollisionObject collisionObject, CollisionShape collisionShape,
ref IndexedMatrix colObjWorldTransform,
ConvexResultCallback resultCallback, float allowedPenetration)
{
if (collisionShape.IsConvex())
{
BulletGlobals.StartProfile("convexSweepConvex");
CastResult castResult = BulletGlobals.CastResultPool.Get();
castResult.m_allowedPenetration = allowedPenetration;
castResult.m_fraction = resultCallback.m_closestHitFraction;//float(1.);//??
ConvexShape convexShape = collisionShape as ConvexShape;
VoronoiSimplexSolver simplexSolver = BulletGlobals.VoronoiSimplexSolverPool.Get();
GjkEpaPenetrationDepthSolver gjkEpaPenetrationSolver = BulletGlobals.GjkEpaPenetrationDepthSolverPool.Get();
ContinuousConvexCollision convexCaster1 = BulletGlobals.ContinuousConvexCollisionPool.Get();
convexCaster1.Initialize(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, // updated to allow different ctor on struct
ref castResult.m_normal,
ref castResult.m_hitPoint,
castResult.m_fraction
);
bool normalInWorldSpace = true;
resultCallback.AddSingleResult(ref localConvexResult, normalInWorldSpace);
}
}
}
BulletGlobals.ContinuousConvexCollisionPool.Free(convexCaster1);
BulletGlobals.GjkEpaPenetrationDepthSolverPool.Free(gjkEpaPenetrationSolver);
BulletGlobals.VoronoiSimplexSolverPool.Free(simplexSolver);
castResult.Cleanup();
BulletGlobals.StopProfile();
}
else
{
if (collisionShape.IsConcave())
{
if (collisionShape.GetShapeType() == BroadphaseNativeTypes.TRIANGLE_MESH_SHAPE_PROXYTYPE)
{
BulletGlobals.StartProfile("convexSweepbtBvhTriangleMesh");
BvhTriangleMeshShape triangleMesh = (BvhTriangleMeshShape)collisionShape;
IndexedMatrix worldTocollisionObject = colObjWorldTransform.Inverse();
IndexedVector3 convexFromLocal = worldTocollisionObject * convexFromTrans._origin;
IndexedVector3 convexToLocal = worldTocollisionObject * convexToTrans._origin;
// rotation of box in local mesh space = MeshRotation^-1 * ConvexToRotation
IndexedMatrix rotationXform = new IndexedMatrix(worldTocollisionObject._basis * convexToTrans._basis,new IndexedVector3(0));
using (BridgeTriangleConvexcastCallback tccb = BulletGlobals.BridgeTriangleConvexcastCallbackPool.Get())
{
tccb.Initialize(castShape, ref convexFromTrans, ref convexToTrans, resultCallback, collisionObject, triangleMesh, ref colObjWorldTransform);
tccb.m_hitFraction = resultCallback.m_closestHitFraction;
tccb.m_allowedPenetration = allowedPenetration;
IndexedVector3 boxMinLocal;
IndexedVector3 boxMaxLocal;
castShape.GetAabb(ref rotationXform, out boxMinLocal, out boxMaxLocal);
triangleMesh.PerformConvexCast(tccb, ref convexFromLocal, ref convexToLocal, ref boxMinLocal, ref boxMaxLocal);
}
BulletGlobals.StopProfile();
}
else
{
if (collisionShape.GetShapeType() == BroadphaseNativeTypes.STATIC_PLANE_PROXYTYPE)
{
CastResult castResult = BulletGlobals.CastResultPool.Get();
castResult.m_allowedPenetration = allowedPenetration;
castResult.m_fraction = resultCallback.m_closestHitFraction;
StaticPlaneShape planeShape = collisionShape as StaticPlaneShape;
ContinuousConvexCollision convexCaster1 = new ContinuousConvexCollision(castShape, planeShape);
if (convexCaster1.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, // updated to allow different ctor on struct
ref castResult.m_normal,
ref castResult.m_hitPoint,
castResult.m_fraction
);
bool normalInWorldSpace = true;
resultCallback.AddSingleResult(ref localConvexResult, normalInWorldSpace);
}
}
}
castResult.Cleanup();
}
else
{
BulletGlobals.StartProfile("convexSweepConcave");
ConcaveShape concaveShape = (ConcaveShape)collisionShape;
IndexedMatrix worldTocollisionObject = colObjWorldTransform.Inverse();
IndexedVector3 convexFromLocal = worldTocollisionObject * convexFromTrans._origin;
IndexedVector3 convexToLocal = worldTocollisionObject * convexToTrans._origin;
// rotation of box in local mesh space = MeshRotation^-1 * ConvexToRotation
IndexedMatrix rotationXform = new IndexedMatrix(worldTocollisionObject._basis * convexToTrans._basis, new IndexedVector3(0));
using (BridgeTriangleConvexcastCallback tccb = BulletGlobals.BridgeTriangleConvexcastCallbackPool.Get())
{
tccb.Initialize(castShape, ref convexFromTrans, ref convexToTrans, resultCallback, collisionObject, concaveShape, ref colObjWorldTransform);
tccb.m_hitFraction = resultCallback.m_closestHitFraction;
tccb.m_allowedPenetration = allowedPenetration;
IndexedVector3 boxMinLocal;
IndexedVector3 boxMaxLocal;
castShape.GetAabb(ref rotationXform, out boxMinLocal, out boxMaxLocal);
IndexedVector3 rayAabbMinLocal = convexFromLocal;
MathUtil.VectorMin(ref convexToLocal, ref rayAabbMinLocal);
//rayAabbMinLocal.setMin(convexToLocal);
IndexedVector3 rayAabbMaxLocal = convexFromLocal;
//rayAabbMaxLocal.setMax(convexToLocal);
MathUtil.VectorMax(ref convexToLocal, ref rayAabbMaxLocal);
rayAabbMinLocal += boxMinLocal;
rayAabbMaxLocal += boxMaxLocal;
concaveShape.ProcessAllTriangles(tccb, ref rayAabbMinLocal, ref rayAabbMaxLocal);
BulletGlobals.StopProfile();
}
}
}
}
else
{
///@todo : use AABB tree or other BVH acceleration structure!
if (collisionShape.IsCompound())
{
BulletGlobals.StartProfile("convexSweepCompound");
CompoundShape compoundShape = (CompoundShape)collisionShape;
for (int i = 0; i < compoundShape.GetNumChildShapes(); i++)
{
IndexedMatrix childTrans = compoundShape.GetChildTransform(i);
CollisionShape childCollisionShape = compoundShape.GetChildShape(i);
IndexedMatrix childWorldTrans = colObjWorldTransform * childTrans;
// replace collision shape so that callback can determine the triangle
CollisionShape saveCollisionShape = collisionObject.GetCollisionShape();
collisionObject.InternalSetTemporaryCollisionShape(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);
}
BulletGlobals.StopProfile();
}
}
}
}
//! Calc the transformation relative 1 to 0. Inverts matrics by transposing
public void CalcFromHomogenic(ref IndexedMatrix trans0, ref IndexedMatrix trans1)
{
IndexedMatrix temp_trans = trans0.Inverse();
temp_trans = temp_trans * trans1;
m_T1to0 = temp_trans._origin;
m_R1to0 = temp_trans._basis;
CalcAbsoluteMatrix();
}
protected void GImpactVsShapeFindPairs(
ref IndexedMatrix trans0,
ref IndexedMatrix trans1,
GImpactShapeInterface shape0,
CollisionShape shape1,
ObjectArray<int> collided_primitives)
{
AABB boxshape = new AABB();
if (shape0.HasBoxSet())
{
IndexedMatrix trans1to0 = trans0.Inverse();
//trans1to0 *= trans1;
trans1to0 = trans1to0 * trans1;
//trans1to0 = MathUtil.BulletMatrixMultiply(trans1,trans1to0);
shape1.GetAabb(ref trans1to0, out boxshape.m_min, out boxshape.m_max);
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGimpactAlgo)
{
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "GImpactAglo::GImpactVsShapeFindPairs trans1to0", trans1to0);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "box min", boxshape.m_min);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "box max", boxshape.m_max);
}
shape0.GetBoxSet().BoxQuery(ref boxshape, collided_primitives);
}
else
{
shape1.GetAabb(ref trans1, out boxshape.m_min, out boxshape.m_max);
AABB boxshape0 = new AABB();
int i = shape0.GetNumChildShapes();
while (i-- != 0)
{
shape0.GetChildAabb(i, ref trans0, out boxshape0.m_min, out boxshape0.m_max);
if (boxshape.HasCollision(ref boxshape0))
{
collided_primitives.Add(i);
}
}
}
}
public void ComputeClosestPoints(ref IndexedMatrix transA, ref IndexedMatrix transB, PointCollector pointCollector)
{
if (m_convexB1 != null)
{
m_simplexSolver.Reset();
GjkPairDetector gjk = new GjkPairDetector(m_convexA, m_convexB1, m_convexA.GetShapeType(), m_convexB1.GetShapeType(), m_convexA.GetMargin(), m_convexB1.GetMargin(), m_simplexSolver, m_penetrationDepthSolver);
ClosestPointInput input = ClosestPointInput.Default();
input.m_transformA = transA;
input.m_transformB = transB;
gjk.GetClosestPoints(ref input, pointCollector, null);
}
else
{
//convex versus plane
ConvexShape convexShape = m_convexA;
StaticPlaneShape planeShape = m_planeShape;
bool hasCollision = false;
IndexedVector3 planeNormal = planeShape.GetPlaneNormal();
float planeConstant = planeShape.GetPlaneConstant();
IndexedMatrix convexWorldTransform = transA;
IndexedMatrix convexInPlaneTrans = transB.Inverse() * convexWorldTransform;
IndexedMatrix planeInConvex = convexWorldTransform.Inverse() * transB;
IndexedVector3 vtx = convexShape.LocalGetSupportingVertex(planeInConvex._basis * -planeNormal);
IndexedVector3 vtxInPlane = convexInPlaneTrans * vtx;
float distance = IndexedVector3.Dot(planeNormal, vtxInPlane) - planeConstant;
IndexedVector3 vtxInPlaneProjected = vtxInPlane - distance * planeNormal;
IndexedVector3 vtxInPlaneWorld = transB * vtxInPlaneProjected;
IndexedVector3 normalOnSurfaceB = transB._basis * planeNormal;
pointCollector.AddContactPoint(
ref normalOnSurfaceB,
ref vtxInPlaneWorld,
distance);
}
}
