public unsafe static void SetHitPointLocal(this LocalConvexResult obj, ref OpenTK.Vector3 value)
{
fixed(OpenTK.Vector3 *valuePtr = &value)
{
obj.HitPointLocal = *(BulletSharp.Math.Vector3 *)valuePtr;
}
}
public unsafe static void GetHitNormalLocal(this LocalConvexResult obj, out OpenTK.Vector3 value)
{
fixed(OpenTK.Vector3 *valuePtr = &value)
{
*(BulletSharp.Math.Vector3 *)valuePtr = obj.HitNormalLocal;
}
}
public override float AddSingleResult(LocalConvexResult convexResult, bool normalInWorldSpace)
{
if (convexResult.HitCollisionObject == _me)
{
return(1.0f);
}
if (!convexResult.HitCollisionObject.HasContactResponse)
{
return(1.0f);
}
Vector3 hitNormalWorld;
if (normalInWorldSpace)
{
hitNormalWorld = convexResult.HitNormalLocal;
}
else
{
// need to transform normal into worldspace
hitNormalWorld = Vector3.TransformCoordinate(convexResult.HitNormalLocal, convexResult.HitCollisionObject.WorldTransform.Basis);
}
float dotUp;
Vector3.Dot(ref _up, ref hitNormalWorld, out dotUp);
if (dotUp < _minSlopeDot)
{
return(1.0f);
}
return(base.AddSingleResult(convexResult, normalInWorldSpace));
}
public override float AddSingleResult(ref LocalConvexResult convexResult, bool normalInWorldSpace)
{
if (convexResult.m_hitCollisionObject == m_me)
{
return(1.0f);
}
//ignore result if there is no contact response
if (!convexResult.m_hitCollisionObject.HasContactResponse())
{
return(1.0f);
}
IndexedVector3 linVelA, linVelB;
linVelA = m_convexToWorld - m_convexFromWorld;
linVelB = IndexedVector3.Zero;//toB._origin-fromB._origin;
IndexedVector3 relativeVelocity = (linVelA - linVelB);
//don't report time of impact for motion away from the contact normal (or causes minor penetration)
if (IndexedVector3.Dot(convexResult.m_hitNormalLocal, relativeVelocity) >= -m_allowedPenetration)
{
return(1f);
}
return(base.AddSingleResult(ref convexResult, normalInWorldSpace));
}
public override float AddSingleResult(ref LocalConvexResult convexResult, bool normalInWorldSpace)
{
if (convexResult.m_hitCollisionObject == m_me)
{
return(1.0f);
}
IndexedVector3 hitNormalWorld;
if (normalInWorldSpace)
{
hitNormalWorld = convexResult.m_hitNormalLocal;
}
else
{
///need to transform normal into worldspace
hitNormalWorld = convexResult.m_hitCollisionObject.GetWorldTransform()._basis *convexResult.m_hitNormalLocal;
}
float dotUp = IndexedVector3.Dot(m_up, hitNormalWorld);
if (dotUp < m_minSlopeDot)
{
return(1.0f);
}
return(base.AddSingleResult(ref convexResult, normalInWorldSpace));
}
public override float AddSingleResult(LocalConvexResult r, bool b)
{
LocalShapeInfo shapeInfo = new LocalShapeInfo();
shapeInfo.m_shapePart = -1;
shapeInfo.m_triangleIndex = m_i;
if (r.m_localShapeInfo == null)
{
r.m_localShapeInfo = shapeInfo;
}
return m_userCallback.AddSingleResult(r, b);
}
public override double AddSingleResult(LocalConvexResult convexResult, bool normalInWorldSpace)
{
//caller already does the filter on the m_closestHitFraction
Debug.Assert(convexResult.HitFraction <= ClosestHitFraction);
ClosestHitFraction = convexResult.HitFraction;
HitCollisionObject = convexResult.HitCollisionObject;
if (normalInWorldSpace)
{
HitNormalWorld = convexResult.HitNormalLocal;
}
else
{
// need to transform normal into worldspace
HitNormalWorld = Vector3.TransformCoordinate(convexResult.HitNormalLocal, HitCollisionObject.WorldTransform.Basis);
}
HitPointWorld = convexResult.HitPointLocal;
return(convexResult.HitFraction);
}
public static void SetHitPointLocal(this LocalConvexResult obj, OpenTK.Vector3 value)
{
SetHitPointLocal(obj, ref value);
}
public static OpenTK.Vector3 GetHitPointLocal(this LocalConvexResult obj)
{
OpenTK.Vector3 value;
GetHitPointLocal(obj, out value);
return(value);
}
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 override double addSingleResult( ref LocalConvexResult r, bool b )
{
LocalShapeInfo shapeInfo = new LocalShapeInfo();
shapeInfo.m_shapePart = -1;
shapeInfo.m_triangleIndex = m_i;
if( r.m_localShapeInfo == null )
r.m_localShapeInfo = shapeInfo;
btScalar result = m_userCallback.addSingleResult( ref r, b );
m_closestHitFraction = m_userCallback.m_closestHitFraction;
return result;
}
internal override double reportHit( ref btVector3 hitNormalLocal, ref btVector3 hitPointLocal
, double hitFraction, int partId, int triangleIndex )
{
LocalShapeInfo shapeInfo = new LocalShapeInfo();
shapeInfo.m_shapePart = partId;
shapeInfo.m_triangleIndex = triangleIndex;
if( hitFraction <= m_resultCallback.m_closestHitFraction )
{
LocalConvexResult convexResult = new LocalConvexResult
( m_collisionObject,
shapeInfo,
ref hitNormalLocal,
ref hitPointLocal,
hitFraction );
bool normalInWorldSpace = true;
return m_resultCallback.addSingleResult( ref convexResult, normalInWorldSpace );
}
return hitFraction;
}
internal static global::System.Runtime.InteropServices.HandleRef getCPtr(LocalConvexResult obj)
{
return((obj == null) ? new global::System.Runtime.InteropServices.HandleRef(null, global::System.IntPtr.Zero) : obj.swigCPtr);
}
public override double addSingleResult( ref LocalConvexResult convexResult, bool normalInWorldSpace )
{
//caller already does the filter on the m_closestHitFraction
Debug.Assert( convexResult.m_hitFraction <= m_closestHitFraction );
m_closestHitFraction = convexResult.m_hitFraction;
m_hitCollisionObject = convexResult.m_hitCollisionObject;
if( normalInWorldSpace )
{
m_hitNormalWorld = convexResult.m_hitNormalLocal;
}
else
{
///need to transform normal into worldspace
m_hitCollisionObject.m_worldTransform.m_basis.Apply( ref convexResult.m_hitNormalLocal, out m_hitNormalWorld );
//m_hitNormalWorld =
}
m_hitPointWorld = convexResult.m_hitPointLocal;
return convexResult.m_hitFraction;
}
public abstract double addSingleResult( ref LocalConvexResult convexResult, bool normalInWorldSpace );
