// 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 void RayTest(Vector3 rayFromWorld, Vector3 rayToWorld, RayResultCallback resultCallback)
{
Matrix rayFromTrans, rayToTrans;
rayFromTrans = Matrix.Identity;
rayFromTrans.Translation = rayFromWorld;
rayToTrans = Matrix.Identity;
rayToTrans.Translation = rayToWorld;
// brute force go over all objects. Once there is a broadphase, use that, or
// add a raycast against aabb first.
foreach (CollisionObject collisionObject in _collisionObjects)
{
//RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
Vector3 collisionObjectAabbMin, collisionObjectAabbMax;
collisionObject.CollisionShape.GetAabb(collisionObject.WorldTransform, out collisionObjectAabbMin, out collisionObjectAabbMax);
float hitLambda = 1f; //could use resultCallback.m_closestHitFraction, but needs testing
Vector3 hitNormal = new Vector3();
//if (MathHelper.TestAabbAgainstAabb2(rayAabbMin, rayAabbMax, collisionObjectAabbMin, collisionObjectAabbMax))
if (MathHelper.RayAabb(rayFromWorld, rayToWorld, collisionObjectAabbMin, collisionObjectAabbMax, hitLambda, hitNormal))
{
RayTestSingle(rayFromTrans, rayToTrans,
collisionObject, collisionObject.CollisionShape,
collisionObject.WorldTransform, 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 void RayTest(Vector3 rayFromWorld, Vector3 rayToWorld, RayResultCallback resultCallback)
{
Matrix rayFromTrans, rayToTrans;
rayFromTrans = Matrix.Identity;
rayFromTrans.Translation = rayFromWorld;
rayToTrans = Matrix.Identity;
rayToTrans.Translation = rayToWorld;
// brute force go over all objects. Once there is a broadphase, use that, or
// add a raycast against aabb first.
foreach (CollisionObject collisionObject in _collisionObjects)
{
//RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
Vector3 collisionObjectAabbMin, collisionObjectAabbMax;
collisionObject.CollisionShape.GetAabb(collisionObject.WorldTransform, out collisionObjectAabbMin, out collisionObjectAabbMax);
float hitLambda = 1f; //could use resultCallback.m_closestHitFraction, but needs testing
Vector3 hitNormal = new Vector3();
//if (MathHelper.TestAabbAgainstAabb2(rayAabbMin, rayAabbMax, collisionObjectAabbMin, collisionObjectAabbMax))
if (MathHelper.RayAabb(rayFromWorld, rayToWorld, collisionObjectAabbMin, collisionObjectAabbMax, hitLambda, hitNormal))
{
RayTestSingle(rayFromTrans, rayToTrans,
collisionObject, collisionObject.CollisionShape,
collisionObject.WorldTransform, resultCallback);
}
}
}
// 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);
}
}
}
}
}
}
public unsafe static void RayTest(this CollisionWorld obj, ref OpenTK.Vector3 rayFromWorld, ref OpenTK.Vector3 rayToWorld, RayResultCallback resultCallback)
{
fixed(OpenTK.Vector3 *rayFromWorldPtr = &rayFromWorld)
{
fixed(OpenTK.Vector3 *rayToWorldPtr = &rayToWorld)
{
obj.RayTest(ref *(BulletSharp.Math.Vector3 *)rayFromWorldPtr, ref *(BulletSharp.Math.Vector3 *)rayToWorldPtr, resultCallback);
}
}
}
//!@}
//! virtual method for ray collision
public virtual void RayTest(ref IndexedVector3 rayFrom, ref IndexedVector3 rayTo, RayResultCallback resultCallback)
{
}
public LocalInfoAdder2 (int i, RayResultCallback user)
{
m_i = i;
m_userCallback = user;
}
internal void Initialize( ref btVector3 rayFromWorld, ref btVector3 rayToWorld, btCollisionWorld world, RayResultCallback resultCallback )
{
m_rayFromWorld = ( rayFromWorld );
m_rayToWorld = ( rayToWorld );
m_world = ( world );
m_resultCallback = ( resultCallback );
m_rayFromTrans.setIdentity();
m_rayFromTrans.setOrigin( ref m_rayFromWorld );
m_rayToTrans.setIdentity();
m_rayToTrans.setOrigin( ref m_rayToWorld );
btVector3 rayDir = ( rayToWorld - rayFromWorld );
rayDir.normalize();
///what about division by zero? -. just set rayDirection[i] to INF/BT_LARGE_FLOAT
m_rayDirectionInverse[0] = rayDir[0] == btScalar.BT_ZERO ? (double)( btScalar.BT_LARGE_FLOAT ) : (double)( 1.0 ) / rayDir[0];
m_rayDirectionInverse[1] = rayDir[1] == btScalar.BT_ZERO ? (double)( btScalar.BT_LARGE_FLOAT ) : (double)( 1.0 ) / rayDir[1];
m_rayDirectionInverse[2] = rayDir[2] == btScalar.BT_ZERO ? (double)( btScalar.BT_LARGE_FLOAT ) : (double)( 1.0 ) / rayDir[2];
m_signs[0] = m_rayDirectionInverse[0] < 0.0 ? 1U : 0;
m_signs[1] = m_rayDirectionInverse[1] < 0.0 ? 1U : 0;
m_signs[2] = m_rayDirectionInverse[2] < 0.0 ? 1U : 0;
m_lambda_max = rayDir.dot( m_rayToWorld - m_rayFromWorld );
}
public void RayTestRef(ref Vector3 rayFromWorld, ref Vector3 rayToWorld, RayResultCallback resultCallback)
{
btGhostObject_rayTest(_native, ref rayFromWorld, ref rayToWorld, resultCallback._native);
}
/// 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 btCollisionShape.
/// This allows more customization.
public static void rayTestSingle( ref btTransform rayFromTrans, ref btTransform rayToTrans,
btCollisionObject collisionObject,
btCollisionShape collisionShape,
ref btTransform colObjWorldTransform,
RayResultCallback resultCallback )
{
//btCollisionObjectWrapper colObWrap = BulletGlobals.CollisionObjectWrapperPool.Get();
//colObWrap.Initialize( null, collisionShape, collisionObject, ref colObjWorldTransform, -1, -1 );
rayTestSingleInternal( ref rayFromTrans, ref rayToTrans, collisionShape, collisionObject
, ref colObjWorldTransform, resultCallback );
//BulletGlobals.CollisionObjectWrapperPool.Free( colObWrap );
}
public static void rayTestSingleInternal( ref btTransform rayFromTrans, ref btTransform rayToTrans,
btCollisionShape collisionShape,
btCollisionObject collisionObject,
ref btTransform useTransform,
RayResultCallback resultCallback )
{
btSphereShape pointShape = BulletGlobals.SphereShapePool.Get();
pointShape.Initialize( btScalar.BT_ZERO );
pointShape.setMargin( 0f );
btConvexShape castShape = pointShape;
//btCollisionShape collisionShape = collisionObjectWrap.getCollisionShape();
btTransform colObjWorldTransform = useTransform;// collisionObjectWrap.m_worldTransform;
if( collisionShape.isConvex() )
{
// CProfileSample sample = new CProfileSample("rayTestConvex");
btConvexCast.CastResult castResult = new btConvexCast.CastResult();
castResult.m_fraction = resultCallback.m_closestHitFraction;
btConvexShape convexShape = (btConvexShape)collisionShape;
btVoronoiSimplexSolver simplexSolver = BulletGlobals.VoronoiSimplexSolverPool.Get();
//new btVoronoiSimplexSolver();
btSubsimplexConvexCast subSimplexConvexCaster = BulletGlobals.SubSimplexConvexCastPool.Get();
subSimplexConvexCaster.Initialize( castShape, convexShape, simplexSolver );
btGjkConvexCast gjkConvexCaster = BulletGlobals.GjkConvexCastPool.Get();
gjkConvexCaster.Initialize( castShape, convexShape, simplexSolver );
BulletGlobals.SubSimplexConvexCastPool.Free( subSimplexConvexCaster );
BulletGlobals.VoronoiSimplexSolverPool.Free( simplexSolver );
//btContinuousConvexCollision convexCaster(castShape,convexShape,&simplexSolver,0);
btConvexCast convexCasterPtr = null;
//use kF_UseSubSimplexConvexCastRaytest by default
if( ( resultCallback.m_flags & (uint)btTriangleRaycastCallback.EFlags.kF_UseGjkConvexCastRaytest ) != 0 )
convexCasterPtr = gjkConvexCaster;
else
convexCasterPtr = subSimplexConvexCaster;
btConvexCast convexCaster = convexCasterPtr;
if( convexCaster.calcTimeOfImpact( ref rayFromTrans, ref rayToTrans
, ref useTransform/*collisionObjectWrap.m_worldTransform*/
, ref useTransform/*collisionObjectWrap.m_worldTransform*/
, castResult ) )
{
//add hit
if( castResult.m_normal.length2() > (double)( 0.0001 ) )
{
if( castResult.m_fraction < resultCallback.m_closestHitFraction )
{
//todo: figure out what this is about. When is rayFromTest.getBasis() not identity?
#if USE_SUBSIMPLEX_CONVEX_CAST
//rotate normal into worldspace
castResult.m_normal = rayFromTrans.getBasis() castResult.m_normal;
#endif //USE_SUBSIMPLEX_CONVEX_CAST
castResult.m_normal.normalize();
LocalRayResult localRayResult = new LocalRayResult
(
collisionObject,
null,
castResult.m_normal,
castResult.m_fraction
);
bool normalInWorldSpace = true;
resultCallback.addSingleResult( localRayResult, normalInWorldSpace );
}
}
}
BulletGlobals.GjkConvexCastPool.Free( gjkConvexCaster );
}
else
{
if( collisionShape.isConcave() )
{
btTransform worldTocollisionObject; colObjWorldTransform.inverse( out worldTocollisionObject );
btVector3 tmp;
rayFromTrans.getOrigin( out tmp );
btVector3 rayFromLocal; worldTocollisionObject.Apply( ref tmp, out rayFromLocal );
rayToTrans.getOrigin( out tmp );
btVector3 rayToLocal; worldTocollisionObject.Apply( ref tmp, out rayToLocal );
// CProfileSample sample = new CProfileSample("rayTestConcave");
#if SUPPORT_TRIANGLE_MESH
if( collisionShape.getShapeType() == BroadphaseNativeTypes.TRIANGLE_MESH_SHAPE_PROXYTYPE )
{
///optimized version for btBvhTriangleMeshShape
btBvhTriangleMeshShape triangleMesh = (btBvhTriangleMeshShape)collisionShape;
BridgeTriangleRaycastCallback rcb( rayFromLocal, rayToLocal,&resultCallback, collisionObjectWrap.getCollisionObject(), triangleMesh, colObjWorldTransform);
rcb.m_hitFraction = resultCallback.m_closestHitFraction;
triangleMesh.performRaycast( &rcb, rayFromLocal, rayToLocal );
}
else
#endif
{
//generic (slower) case
btConcaveShape concaveShape = (btConcaveShape)collisionShape;
//ConvexCast::CastResult
BridgeTriangleRaycastCallback rcb = new BridgeTriangleRaycastCallback( ref rayFromLocal, ref rayToLocal, resultCallback
, collisionObject, concaveShape, ref colObjWorldTransform );
rcb.m_hitFraction = resultCallback.m_closestHitFraction;
btVector3 rayAabbMinLocal = rayFromLocal;
rayAabbMinLocal.setMin( ref rayToLocal );
btVector3 rayAabbMaxLocal = rayFromLocal;
rayAabbMaxLocal.setMax( ref rayToLocal );
concaveShape.processAllTriangles( rcb, ref rayAabbMinLocal, ref rayAabbMaxLocal );
}
}
else
{
// CProfileSample sample = new CProfileSample("rayTestCompound");
if( collisionShape.isCompound() )
{
btCompoundShape compoundShape = (btCompoundShape)( collisionShape );
btDbvt 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 )
{
btTransform tmp;
colObjWorldTransform.inverseTimes( ref rayFromTrans, out tmp );
btVector3 localRayFrom; tmp.getOrigin( out localRayFrom );
colObjWorldTransform.inverseTimes( ref rayToTrans, out tmp );
btVector3 localRayTo; tmp.getOrigin( out localRayTo );
btDbvt.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 );
}
}
}
}
}
BulletGlobals.SphereShapePool.Free( pointShape );
}
internal RayTester( btCollisionObject collisionObject,
btCompoundShape compoundShape,
ref btTransform colObjWorldTransform,
ref btTransform rayFromTrans,
ref btTransform rayToTrans,
RayResultCallback resultCallback )
{
m_collisionObject = ( collisionObject );
m_compoundShape = ( compoundShape );
m_colObjWorldTransform = ( colObjWorldTransform );
m_rayFromTrans = ( rayFromTrans );
m_rayToTrans = ( rayToTrans );
m_resultCallback = ( resultCallback );
}
internal LocalInfoAdder2( int i, RayResultCallback user )
{
m_userCallback = ( user ); m_i = ( i );
m_closestHitFraction = m_userCallback.m_closestHitFraction;
m_flags = m_userCallback.m_flags;
}
internal static global::System.Runtime.InteropServices.HandleRef getCPtr(RayResultCallback obj)
{
return((obj == null) ? new global::System.Runtime.InteropServices.HandleRef(null, global::System.IntPtr.Zero) : obj.swigCPtr);
}
public void RayTest(ref IndexedVector3 rayFromWorld, ref IndexedVector3 rayToWorld, RayResultCallback resultCallback)
{
int ibreak = 0;
}
/// 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.
void rayTest( btVector3 rayFromWorld, btVector3 rayToWorld, RayResultCallback resultCallback )
{
//CProfileSample sample = new CProfileSample("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
btSingleRayCallback 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 < this.getNumCollisionObjects(); i++ )
{
rayCB.process( m_collisionObjects[i].getBroadphaseHandle() );
}
#endif //USE_BRUTEFORCE_RAYBROADPHASE
BulletGlobals.SingleRayCallbackPool.Free( rayCB );
}
public void RayTest(Vector3 rayFrom, Vector3 rayTo, RayResultCallback resultCallback)
{
btGImpactShapeInterface_rayTest(_native, ref rayFrom, ref rayTo, resultCallback._native);
}
internal BridgeTriangleRaycastCallback( ref btVector3 from, ref btVector3 to
, RayResultCallback resultCallback
, btCollisionObject collisionObject, btConcaveShape triangleMesh
, ref btTransform colObjWorldTransform )
: base( ref from, ref to, (btTriangleRaycastCallback.EFlags)resultCallback.m_flags )
{
m_resultCallback = ( resultCallback );
m_collisionObject = ( collisionObject );
m_triangleMesh = ( triangleMesh );
m_colObjWorldTransform = ( colObjWorldTransform );
}
public void RayTest(Vector3 rayFromWorld, Vector3 rayToWorld, RayResultCallback resultCallback)
{
btGhostObject_rayTest(Native, ref rayFromWorld, ref rayToWorld, resultCallback.Native);
}
// 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);
}
}
}
}
}
}
