public void addChildShape( ref btTransform localTransform, btCollisionShape shape )
{
m_updateRevision++;
//m_childTransforms.Add(localTransform);
//m_childShapes.Add(shape);
btCompoundShapeChild child = new btCompoundShapeChild();
child.m_node = null;
child.m_transform = localTransform;
child.m_childShape = shape;
child.m_childShapeType = shape.getShapeType();
child.m_childMargin = shape.getMargin();
//extend the local aabbMin/aabbMax
btVector3 localAabbMin, localAabbMax;
shape.getAabb( ref localTransform, out localAabbMin, out localAabbMax );
for( int i = 0; i < 3; i++ )
{
if( m_localAabbMin[i] > localAabbMin[i] )
{
m_localAabbMin[i] = localAabbMin[i];
}
if( m_localAabbMax[i] < localAabbMax[i] )
{
m_localAabbMax[i] = localAabbMax[i];
}
}
if( m_dynamicAabbTree != null )
{
btDbvt.btDbvtVolume bounds = btDbvt.btDbvtVolume.FromMM( ref localAabbMin, ref localAabbMax );
int index = m_children.Count;
child.m_node = m_dynamicAabbTree.insert( ref bounds, index );
}
m_children.Add( child );
}
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 );
}
public static void objectQuerySingleInternal( btConvexShape castShape, ref btTransform convexFromTrans, ref btTransform convexToTrans
, btCollisionShape collisionShape//btCollisionObjectWrapper colObjWrap
, btCollisionObject collisionObject
, ref btTransform colObjTransform
, ConvexResultCallback resultCallback, double allowedPenetration )
{
//btCollisionShape collisionShape = colObjWrap.getCollisionShape();
//btTransform colObjWorldTransform = colObjWrap.m_worldTransform;
if( collisionShape.isConvex() )
{
//CProfileSample sample = new CProfileSample("convexSweepConvex");
btConvexCast.CastResult castResult = new btConvexCast.CastResult();
castResult.m_allowedPenetration = allowedPenetration;
castResult.m_fraction = resultCallback.m_closestHitFraction;//btScalar.BT_ONE;//??
btConvexShape convexShape = (btConvexShape)collisionShape;
btVoronoiSimplexSolver simplexSolver = BulletGlobals.VoronoiSimplexSolverPool.Get();
btGjkEpaPenetrationDepthSolver gjkEpaPenetrationSolver = BulletGlobals.GjkEpaPenetrationDepthSolverPool.Get();
// new btGjkEpaPenetrationDepthSolver();
btContinuousConvexCollision convexCaster1 = BulletGlobals.ContinuousConvexCollisionPool.Get();
convexCaster1.Initialize( castShape, convexShape, simplexSolver, gjkEpaPenetrationSolver );
//btGjkConvexCast convexCaster2(castShape,convexShape,&simplexSolver);
//btSubsimplexConvexCast convexCaster3(castShape,convexShape,&simplexSolver);
btConvexCast castPtr = convexCaster1;
if( castPtr.calcTimeOfImpact( ref convexFromTrans, ref convexToTrans, ref colObjTransform, ref colObjTransform, castResult ) )
{
//add hit
if( castResult.m_normal.length2() > (double)( 0.0001 ) )
{
if( castResult.m_fraction < resultCallback.m_closestHitFraction )
{
castResult.m_normal.normalize();
LocalConvexResult localConvexResult =
new LocalConvexResult
(
collisionObject,
null,
ref castResult.m_normal,
ref castResult.m_hitPoint,
castResult.m_fraction
);
bool normalInWorldSpace = true;
resultCallback.addSingleResult( ref localConvexResult, normalInWorldSpace );
}
}
}
BulletGlobals.GjkEpaPenetrationDepthSolverPool.Free( gjkEpaPenetrationSolver );
BulletGlobals.VoronoiSimplexSolverPool.Free( simplexSolver );
BulletGlobals.ContinuousConvexCollisionPool.Free( convexCaster1 );
}
else
{
if( collisionShape.isConcave() )
{
if( collisionShape.getShapeType() == BroadphaseNativeTypes.TRIANGLE_MESH_SHAPE_PROXYTYPE )
{
//CProfileSample sample = new CProfileSample("convexSweepbtBvhTriangleMesh");
btConcaveShape triangleMesh = (btConcaveShape)collisionShape;
btTransform worldTocollisionObject; colObjTransform.inverse( out worldTocollisionObject );
btVector3 convexFromLocal; worldTocollisionObject.Apply( ref convexFromTrans.m_origin, out convexFromLocal );
btVector3 convexToLocal; worldTocollisionObject.Apply( ref convexToTrans.m_origin, out convexToLocal );
// rotation of box in local mesh space = MeshRotation^-1 ConvexToRotation
btTransform rotationXform; worldTocollisionObject.m_basis.Apply( ref convexToTrans.m_basis, out rotationXform.m_basis );
rotationXform.m_origin = btVector3.Zero;
//ConvexCast::CastResult
BridgeTriangleConvexcastCallback tccb = BulletGlobals.BridgeTriangleConvexcastCallbackPool.Get();
tccb.Initialize( castShape, ref convexFromTrans, ref convexToTrans
, resultCallback, collisionObject
, triangleMesh, ref colObjTransform );
tccb.m_hitFraction = resultCallback.m_closestHitFraction;
tccb.m_allowedPenetration = allowedPenetration;
btVector3 boxMinLocal, boxMaxLocal;
castShape.getAabb( ref rotationXform, out boxMinLocal, out boxMaxLocal );
triangleMesh.performConvexcast( tccb, ref convexFromLocal, ref convexToLocal, ref boxMinLocal, ref boxMaxLocal );
BulletGlobals.BridgeTriangleConvexcastCallbackPool.Free( tccb );
}
else
{
if( collisionShape.getShapeType() == BroadphaseNativeTypes.STATIC_PLANE_PROXYTYPE )
{
btConvexCast.CastResult castResult = BulletGlobals.CastResultPool.Get();
castResult.m_allowedPenetration = allowedPenetration;
castResult.m_fraction = resultCallback.m_closestHitFraction;
btStaticPlaneShape planeShape = (btStaticPlaneShape)collisionShape;
btContinuousConvexCollision convexCaster1 = BulletGlobals.ContinuousConvexCollisionPool.Get();
convexCaster1.Initialize( castShape, planeShape );
btConvexCast castPtr = convexCaster1;
if( castPtr.calcTimeOfImpact(ref convexFromTrans, ref convexToTrans, ref colObjTransform, ref colObjTransform, castResult ) )
{
//add hit
if( castResult.m_normal.length2() > (double)( 0.0001 ) )
{
if( castResult.m_fraction < resultCallback.m_closestHitFraction )
{
castResult.m_normal.normalize();
LocalConvexResult localConvexResult = new LocalConvexResult
(
collisionObject,
null,
ref castResult.m_normal,
ref castResult.m_hitPoint,
castResult.m_fraction
);
bool normalInWorldSpace = true;
resultCallback.addSingleResult( ref localConvexResult, normalInWorldSpace );
}
}
}
}
else
{
//CProfileSample sample = new CProfileSample("convexSweepConcave");
btConcaveShape concaveShape = (btConcaveShape)collisionShape;
btTransform worldTocollisionObject; colObjTransform.inverse( out worldTocollisionObject );
btVector3 convexFromLocal; worldTocollisionObject.Apply( ref convexFromTrans.m_origin, out convexFromLocal );
btVector3 convexToLocal; worldTocollisionObject.Apply( ref convexToTrans.m_origin, out convexToLocal );
// rotation of box in local mesh space = MeshRotation^-1 ConvexToRotation
btTransform rotationXform; worldTocollisionObject.m_basis.Apply( ref convexToTrans.m_basis, out rotationXform.m_basis );
rotationXform.m_origin = btVector3.Zero;
BridgeTriangleConvexcastCallback tccb = BulletGlobals.BridgeTriangleConvexcastCallbackPool.Get();
tccb.Initialize( castShape, ref convexFromTrans, ref convexToTrans, resultCallback, collisionObject
, concaveShape, ref colObjTransform );
tccb.m_hitFraction = resultCallback.m_closestHitFraction;
tccb.m_allowedPenetration = allowedPenetration;
btVector3 boxMinLocal, boxMaxLocal;
castShape.getAabb( ref rotationXform, out boxMinLocal, out boxMaxLocal );
btVector3 rayAabbMinLocal = convexFromLocal;
rayAabbMinLocal.setMin( ref convexToLocal );
btVector3 rayAabbMaxLocal = convexFromLocal;
rayAabbMaxLocal.setMax( ref convexToLocal );
rayAabbMinLocal += boxMinLocal;
rayAabbMaxLocal += boxMaxLocal;
concaveShape.processAllTriangles( tccb, ref rayAabbMinLocal, ref rayAabbMaxLocal );
BulletGlobals.BridgeTriangleConvexcastCallbackPool.Free( tccb );
}
}
}
else
{
///@todo : use AABB tree or other BVH acceleration structure!
if( collisionShape.isCompound() )
{
CProfileSample sample = new CProfileSample( "convexSweepCompound" );
btCompoundShape compoundShape = (btCompoundShape)( collisionShape );
int i = 0;
for( i = 0; i < compoundShape.getNumChildShapes(); i++ )
{
//btTransform childTrans = compoundShape.getChildTransform( i );
btCollisionShape childCollisionShape = compoundShape.getChildShape( i );
btTransform childWorldTrans; colObjTransform.Apply( ref compoundShape.m_children.InternalArray[i].m_transform
, out childWorldTrans );
LocalInfoAdder my_cb = new LocalInfoAdder( i, resultCallback );
//btCollisionObjectWrapper tmpObj = BulletGlobals.CollisionObjectWrapperPool.Get();
//tmpObj.Initialize( colObjWrap, childCollisionShape, colObjWrap.m_collisionObject, ref childWorldTrans, -1, i );
objectQuerySingleInternal( castShape, ref convexFromTrans, ref convexToTrans
, childCollisionShape
, collisionObject
, ref childWorldTrans
, my_cb, allowedPenetration );
//BulletGlobals.CollisionObjectWrapperPool.Free( tmpObj );
}
}
}
}
}
public virtual void debugDrawObject( ref btTransform worldTransform, btCollisionShape shape, btVector3 color )
{
// Draw a small simplex at the center of the object
if( m_debugDrawer != null && ( ( m_debugDrawer.getDebugMode() & btIDebugDraw.DebugDrawModes.DBG_DrawFrames ) != 0 ) )
{
m_debugDrawer.drawTransform( ref worldTransform, 1 );
}
if( shape.getShapeType() == BroadphaseNativeTypes.COMPOUND_SHAPE_PROXYTYPE )
{
btCompoundShape compoundShape = (btCompoundShape)( shape );
for( int i = compoundShape.getNumChildShapes() - 1; i >= 0; i-- )
{
//btITransform childTrans = compoundShape.getChildTransform( i );
btCollisionShape colShape = compoundShape.getChildShape( i );
btTransform tmp; worldTransform.Apply( ref compoundShape.m_children.InternalArray[i].m_transform, out tmp );
debugDrawObject( ref tmp, colShape, color );
}
}
else
{
switch( shape.getShapeType() )
{
case BroadphaseNativeTypes.BOX_SHAPE_PROXYTYPE:
{
btBoxShape boxShape = (btBoxShape)( shape );
btVector3 halfExtents; boxShape.getHalfExtentsWithMargin( out halfExtents );
btVector3 tmp; halfExtents.Invert( out tmp );
m_debugDrawer.drawBox( ref tmp, ref halfExtents, ref worldTransform, ref color );
break;
}
case BroadphaseNativeTypes.SPHERE_SHAPE_PROXYTYPE:
{
btSphereShape sphereShape = (btSphereShape)( shape );
double radius = sphereShape.getMargin();//radius doesn't include the margin, so draw with margin
m_debugDrawer.drawSphere( radius, ref worldTransform, ref color );
break;
}
case BroadphaseNativeTypes.MULTI_SPHERE_SHAPE_PROXYTYPE:
{
btMultiSphereShape multiSphereShape = (btMultiSphereShape)( shape );
btTransform childTransform = btTransform.Identity;
for( int i = multiSphereShape.getSphereCount() - 1; i >= 0; i-- )
{
multiSphereShape.getSpherePosition( i, out childTransform.m_origin );
btTransform tmp;
worldTransform.Apply( ref childTransform, out tmp );
m_debugDrawer.drawSphere( multiSphereShape.getSphereRadius( i ), ref tmp, ref color );
}
break;
}
case BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE:
{
btCapsuleShape capsuleShape = (btCapsuleShape)( shape );
double radius = capsuleShape.getRadius();
double halfHeight = capsuleShape.getHalfHeight();
int upAxis = capsuleShape.getUpAxis();
m_debugDrawer.drawCapsule( radius, halfHeight, upAxis, ref worldTransform, ref color );
break;
}
case BroadphaseNativeTypes.CONE_SHAPE_PROXYTYPE:
{
btConeShape coneShape = (btConeShape)( shape );
double radius = coneShape.getRadius();//+coneShape.getMargin();
double height = coneShape.getHeight();//+coneShape.getMargin();
int upAxis = coneShape.getConeUpIndex();
m_debugDrawer.drawCone( radius, height, upAxis, ref worldTransform, ref color );
break;
}
case BroadphaseNativeTypes.CYLINDER_SHAPE_PROXYTYPE:
{
btCylinderShape cylinder = (btCylinderShape)( shape );
int upAxis = cylinder.getUpAxis();
double radius = cylinder.getRadius();
btVector3 tmp;
cylinder.getHalfExtentsWithMargin( out tmp );
double halfHeight = tmp[upAxis];
m_debugDrawer.drawCylinder( radius, halfHeight, upAxis, ref worldTransform, ref color );
break;
}
case BroadphaseNativeTypes.STATIC_PLANE_PROXYTYPE:
{
btStaticPlaneShape staticPlaneShape = (btStaticPlaneShape)( shape );
double planeConst = staticPlaneShape.getPlaneConstant();
btVector3 planeNormal; staticPlaneShape.getPlaneNormal( out planeNormal );
m_debugDrawer.drawPlane( ref planeNormal, planeConst, ref worldTransform, ref color );
break;
}
default:
{
/// for polyhedral shapes
if( shape.isPolyhedral() )
{
btPolyhedralConvexShape polyshape = (btPolyhedralConvexShape)shape;
int i;
if( polyshape.getConvexPolyhedron() != null )
{
btConvexPolyhedron poly = polyshape.getConvexPolyhedron();
for( i = 0; i < poly.m_faces.Count; i++ )
{
btVector3 centroid = btVector3.Zero;
int numVerts = poly.m_faces[i].m_indices.Count;
if( numVerts > 0 )
{
int lastV = poly.m_faces[i].m_indices[numVerts - 1];
for( int v = 0; v < poly.m_faces[i].m_indices.Count; v++ )
{
int curVert = poly.m_faces[i].m_indices[v];
centroid += poly.m_vertices[curVert];
btVector3 tmp1, tmp2;
worldTransform.Apply( ref poly.m_vertices.InternalArray[lastV], out tmp1 );
worldTransform.Apply( ref poly.m_vertices.InternalArray[curVert], out tmp2 );
m_debugDrawer.drawLine( ref tmp1, ref tmp2, ref color );
lastV = curVert;
}
}
centroid *= (double)( 1 ) / (double)( numVerts );
if( ( m_debugDrawer.getDebugMode() & btIDebugDraw.DebugDrawModes.DBG_DrawNormals ) != 0 )
{
btVector3 normalColor = new btVector3( 1, 1, 0, 1 );
btVector3 faceNormal = new btVector3( poly.m_faces[i].m_plane[0], poly.m_faces[i].m_plane[1], poly.m_faces[i].m_plane[2] );
btVector3 tmp, tmp2;
centroid.Add( ref faceNormal, out tmp );
worldTransform.Apply( ref tmp, out tmp2 );
worldTransform.Apply( ref centroid, out tmp );
m_debugDrawer.drawLine( ref tmp, ref tmp2, ref normalColor );
}
}
}
else
{
for( i = 0; i < polyshape.getNumEdges(); i++ )
{
btVector3 a, b;
polyshape.getEdge( i, out a, out b );
btVector3 wa; worldTransform.Apply( ref a, out wa );
btVector3 wb; worldTransform.Apply( ref b, out wb );
m_debugDrawer.drawLine( ref wa, ref wb, ref color );
}
}
}
if( shape.isConcave() )
{
btConcaveShape concaveMesh = (btConcaveShape)shape;
///@todo pass camera, for some culling? no . we are not a graphics lib
btVector3 aabbMax = btVector3.Max;
btVector3 aabbMin = btVector3.Min;
DebugDrawcallback drawCallback = new DebugDrawcallback( m_debugDrawer, ref worldTransform, ref color );
concaveMesh.processAllTriangles( drawCallback, ref aabbMin, ref aabbMax );
}
if( shape.getShapeType() == BroadphaseNativeTypes.CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE )
{
Debug.Assert( false, "This needs some work... don't know the mesher stride interface types; replace with interfaces..." );
#if asdfasdf
btConvexTriangleMeshShape convexMesh = (btConvexTriangleMeshShape)shape;
//todo: pass camera for some culling
btVector3 aabbMax = btVector3.Max;// ( (double)( BT_LARGE_FLOAT ), (double)( BT_LARGE_FLOAT ), (double)( BT_LARGE_FLOAT ));
btVector3 aabbMin = btVector3.Min;// ( (double)( -BT_LARGE_FLOAT ), (double)( -BT_LARGE_FLOAT ), (double)( -BT_LARGE_FLOAT ));
//DebugDrawcallback drawCallback;
DebugDrawcallback drawCallback = new DebugDrawcallback( m_debugDrawer, ref worldTransform, color );
convexMesh.getMeshInterface().InternalProcessAllTriangles( drawCallback, aabbMin, aabbMax );
#endif
}
break;
}
}
}
}