public virtual bool CalcPenDepth(ISimplexSolverInterface simplexSolver, ConvexShape convexA, ConvexShape convexB, ref Matrix transA, ref Matrix transB,
ref Vector3 v, ref Vector3 wWitnessOnA, ref Vector3 wWitnessOnB, IDebugDraw debugDraw)
{
//float radialmargin = 0f;
Vector3 guessVector = (transA.Translation - transB.Translation);
GjkEpaSolver2Results results = new GjkEpaSolver2Results();
if (GjkEpaSolver2.Penetration(convexA, ref transA,
convexB, ref transB,
ref guessVector, results))
{
// debugDraw->drawLine(results.witnesses[1],results.witnesses[1]+results.normal,btVector3(255,0,0));
//resultOut->addContactPoint(results.normal,results.witnesses[1],-results.depth);
wWitnessOnA = results.witnesses0;
wWitnessOnB = results.witnesses1;
v = results.normal;
return true;
}
else
{
if (GjkEpaSolver2.Distance(convexA, ref transA, convexB, ref transB, ref guessVector, results))
{
wWitnessOnA = results.witnesses0;
wWitnessOnB = results.witnesses1;
v = results.normal;
return false;
}
}
return false;
}
public static bool Distance(ConvexShape shape0,ref Matrix wtrs0,ConvexShape shape1,ref Matrix wtrs1,ref Vector3 guess,GjkEpaSolver2Results results)
{
GjkEpaSolver2MinkowskiDiff shape = new GjkEpaSolver2MinkowskiDiff();
Initialize(shape0,ref wtrs0,shape1,ref wtrs1,results,shape,false);
GJK gjk = new GJK();
GJKStatus gjk_status= gjk.Evaluate(shape,ref guess);
if(gjk_status == GJKStatus.Valid)
{
Vector3 w0 = Vector3.Zero;
Vector3 w1 = Vector3.Zero;
for(uint i=0;i<gjk.m_simplex.rank;++i)
{
float p=gjk.m_simplex.p[i];
w0+=shape.Support(ref gjk.m_simplex.c[i].d,0)*p;
Vector3 temp = -gjk.m_simplex.c[i].d;
w1+=shape.Support(ref temp,1)*p;
}
results.witnesses0 = Vector3.Transform(w0,wtrs0);
results.witnesses1 = Vector3.Transform(w1,wtrs0);
results.normal = w0-w1;
results.distance = results.normal.Length();
results.normal /= results.distance>GJK_MIN_DISTANCE?results.distance:1;
return(true);
}
else
{
//GjkEpaSolver2Status
results.status = (gjk_status==GJKStatus.Inside)?GjkEpaSolver2Status.Penetrating :GjkEpaSolver2Status.GJK_Failed ;
return(false);
}
}
public static void Initialize(ConvexShape shape0,ref Matrix wtrs0,
ConvexShape shape1,ref Matrix wtrs1,
GjkEpaSolver2Results results,
GjkEpaSolver2MinkowskiDiff shapeR,
bool withmargins)
{
/* Results */
results.witnesses0 = Vector3.Zero;
results.witnesses1 = Vector3.Zero;
results.status = GjkEpaSolver2Status.Separated;
/* Shape */
shapeR.m_shapes[0] = shape0;
shapeR.m_shapes[1] = shape1;
shapeR.m_toshape1 = MathUtil.TransposeTimesBasis(ref wtrs1,ref wtrs0);
shapeR.m_toshape0 = MathUtil.InverseTimes(ref wtrs0, ref wtrs1);
if (BulletGlobals.g_streamWriter != null && debugGJK)
{
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "gjksolver2::init::shape0", shapeR.m_toshape0);
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "gjksolver2::init::shape1", shapeR.m_toshape1);
}
shapeR.EnableMargin(withmargins);
}
public ContinuousConvexCollision(ConvexShape shapeA, ConvexShape shapeB, ISimplexSolverInterface simplexSolver, IConvexPenetrationDepthSolver penetrationDepthSolver)
{
m_convexA = shapeA;
m_convexB = shapeB;
m_simplexSolver = simplexSolver;
m_penetrationDepthSolver = penetrationDepthSolver;
}
public TriangleConvexcastCallback(ConvexShape convexShape, ref Matrix convexShapeFrom, ref Matrix convexShapeTo, ref Matrix triangleToWorld, float triangleCollisionMargin)
{
m_convexShape = convexShape;
m_convexShapeFrom = convexShapeFrom;
m_convexShapeTo = convexShapeTo;
m_triangleToWorld = triangleToWorld;
m_triangleCollisionMargin = triangleCollisionMargin;
}
public BridgeTriangleConvexcastCallback(ConvexShape castShape, ref Matrix from,ref Matrix to,
ConvexResultCallback resultCallback, CollisionObject collisionObject,TriangleMeshShape triangleMesh, ref Matrix triangleToWorld):
base(castShape, ref from,ref to, ref triangleToWorld, triangleMesh.Margin)
{
m_resultCallback = resultCallback;
m_collisionObject = collisionObject;
m_triangleMesh = triangleMesh;
}
public GjkPairDetector(ConvexShape objectA, ConvexShape objectB, BroadphaseNativeTypes shapeTypeA, BroadphaseNativeTypes shapeTypeB, float marginA, float marginB, ISimplexSolverInterface simplexSolver, IConvexPenetrationDepthSolver penetrationDepthSolver)
{
m_minkowskiA = objectA;
m_minkowskiB = objectB;
m_shapeTypeA = shapeTypeA;
m_shapeTypeB = shapeTypeB;
m_marginA = marginA;
m_marginB = marginB;
m_cachedSeparatingAxis = new Vector3(0, 1, 0);
m_simplexSolver = simplexSolver;
m_penetrationDepthSolver = penetrationDepthSolver;
m_ignoreMargin = false;
m_lastUsedMethod = -1;
m_catchDegeneracies = true;
}
public void ConvexSweepTest(ConvexShape castShape, ref Matrix convexFromWorld, ref Matrix convexToWorld, ConvexResultCallback resultCallback, float allowedCcdPenetration)
{
Matrix convexFromTrans = convexFromWorld;
Matrix convexToTrans = convexToWorld;
Vector3 castShapeAabbMin = Vector3.Zero, castShapeAabbMax = Vector3.Zero;
/* Compute AABB that encompasses angular movement */
Vector3 linVel = Vector3.Zero, angVel = Vector3.Zero;
TransformUtil.CalculateVelocity (ref convexFromTrans, ref convexToTrans, 1.0f, ref linVel, ref angVel);
Matrix R = MathUtil.BasisMatrix(ref convexFromTrans);
castShape.CalculateTemporalAabb (ref R, ref linVel, ref angVel, 1.0f, ref castShapeAabbMin, ref castShapeAabbMax);
/// go over all objects, and if the ray intersects their aabb + cast shape aabb,
// do a ray-shape query using convexCaster (CCD)
for (int i=0;i<m_overlappingObjects.Count;i++)
{
CollisionObject collisionObject = m_overlappingObjects[i];
//only perform raycast if filterMask matches
if(resultCallback.NeedsCollision(collisionObject.GetBroadphaseHandle()))
{
//RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
Vector3 collisionObjectAabbMin = Vector3.Zero, collisionObjectAabbMax = Vector3.Zero;
Matrix t = collisionObject.GetWorldTransform();
collisionObject.GetCollisionShape().GetAabb(ref t,ref collisionObjectAabbMin,ref collisionObjectAabbMax);
AabbUtil2.AabbExpand (ref collisionObjectAabbMin, ref collisionObjectAabbMax, ref castShapeAabbMin, ref castShapeAabbMax);
float hitLambda = 1f; //could use resultCallback.m_closestHitFraction, but needs testing
Vector3 hitNormal = Vector3.Zero;
if (AabbUtil2.RayAabb(convexFromWorld.Translation, convexToWorld.Translation, collisionObjectAabbMin, collisionObjectAabbMax, hitLambda, hitNormal))
{
Matrix wt = collisionObject.GetWorldTransform();
CollisionWorld.ObjectQuerySingle(castShape, ref convexFromTrans,ref convexToTrans,
collisionObject,
collisionObject.GetCollisionShape(),
ref wt,
resultCallback,
allowedCcdPenetration);
}
}
}
}
public SingleSweepCallback(ConvexShape castShape, ref Matrix convexFromTrans,ref Matrix convexToTrans,CollisionWorld world,ConvexResultCallback resultCallback,float allowedPenetration)
{
m_convexFromTrans = convexFromTrans;
m_convexToTrans = convexToTrans;
m_world = world;
m_resultCallback = resultCallback;
m_allowedCcdPenetration = allowedPenetration;
m_castShape = castShape;
Vector3 unnormalizedRayDir = (m_convexToTrans.Translation-m_convexFromTrans.Translation);
Vector3 rayDir = unnormalizedRayDir;
rayDir.Normalize();
///what about division by zero? -. just set rayDirection[i] to INF/1e30
m_rayDirectionInverse.X = MathUtil.CompareFloat(rayDir.X ,0.0f) ? float.MaxValue : 1f / rayDir.X;
m_rayDirectionInverse.Y = MathUtil.CompareFloat(rayDir.Y ,0.0f) ? float.MaxValue : 1f / rayDir.Y;
m_rayDirectionInverse.Z = MathUtil.CompareFloat(rayDir.Z ,0.0f) ? float.MaxValue : 1f / rayDir.Z;
m_signs[0] = m_rayDirectionInverse.X < 0.0;
m_signs[1] = m_rayDirectionInverse.Y < 0.0;
m_signs[2] = m_rayDirectionInverse.Z < 0.0;
m_lambda_max = Vector3.Dot(rayDir,unnormalizedRayDir);
}
public void SetMinkowskiA(ConvexShape minkA)
{
m_minkowskiA = minkA;
}
//
public bool SignedDistance(ConvexShape shape0,ref Matrix wtrs0,ConvexShape shape1,ref Matrix wtrs1,ref Vector3 guess,GjkEpaSolver2Results results)
{
if(!Distance(shape0,ref wtrs0,shape1,ref wtrs1,ref guess,results))
return(Penetration(shape0,ref wtrs0,shape1,ref wtrs1,ref guess,results,false));
else
return(true);
}
//
public float SignedDistance(ref Vector3 position, float margin, ConvexShape shape0, ref Matrix wtrs0, GjkEpaSolver2Results results)
{
GjkEpaSolver2MinkowskiDiff shape = new GjkEpaSolver2MinkowskiDiff();
SphereShape shape1 = new SphereShape(margin);
Matrix wtrs1 = Matrix.CreateFromQuaternion(Quaternion.Identity);
wtrs0.Translation = position;
Initialize(shape0,ref wtrs0,shape1,ref wtrs1,results,shape,false);
GJK gjk = new GJK();
Vector3 guess = new Vector3(1,1,1);
GJKStatus gjk_status=gjk.Evaluate(shape,ref guess);
if(gjk_status==GJKStatus.Valid)
{
Vector3 w0=Vector3.Zero;
Vector3 w1=Vector3.Zero;
for(int i=0;i<gjk.m_simplex.rank;++i)
{
float p=gjk.m_simplex.p[i];
w0+=shape.Support( ref gjk.m_simplex.c[i].d,0)*p;
Vector3 temp = -gjk.m_simplex.c[i].d;
w1+=shape.Support(ref temp,1)*p;
}
results.witnesses0 = Vector3.Transform(w0,wtrs0);
results.witnesses1 = Vector3.Transform(w1,wtrs0);
Vector3 delta= results.witnesses1-results.witnesses0;
float margin2 = shape0.GetMarginNonVirtual()+shape1.GetMarginNonVirtual();
float length = delta.Length();
results.normal = delta/length;
results.witnesses0 += results.normal*margin2;
return(length-margin2);
}
else
{
if(gjk_status==GJKStatus.Inside)
{
if(Penetration(shape0,ref wtrs0,shape1,ref wtrs1,ref gjk.m_ray,results))
{
Vector3 delta= results.witnesses0-results.witnesses1;
float length= delta.Length();
if (length >= MathUtil.SIMD_EPSILON)
results.normal = delta/length;
return(-length);
}
}
}
return(MathUtil.SIMD_INFINITY);
}
public KinematicCharacterController(PairCachingGhostObject ghostObject, ConvexShape convexShape, float stepHeight, int upAxis)
{
m_upAxis = upAxis;
m_addedMargin = 0.02f;
m_walkDirection = Vector3.Zero;
m_useGhostObjectSweepTest = true;
m_ghostObject = ghostObject;
m_stepHeight = stepHeight;
m_turnAngle = 0f;
m_convexShape = convexShape;
m_useWalkDirection = true; // use walk direction by default, legacy behavior
m_velocityTimeInterval = 0.0f;
}
public virtual void ConvexSweepTest (ConvexShape castShape, ref Matrix convexFromWorld, ref Matrix convexToWorld, ConvexResultCallback resultCallback, float allowedCcdPenetration)
{
//BT_PROFILE("convexSweepTest");
/// 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
/// unfortunately the implementation for rayTest and convexSweepTest duplicated, albeit practically identical
Matrix convexFromTrans = new Matrix();
Matrix convexToTrans = new Matrix();
convexFromTrans = convexFromWorld;
convexToTrans = convexToWorld;
Vector3 castShapeAabbMin = new Vector3();
Vector3 castShapeAabbMax = new Vector3();
/* Compute AABB that encompasses angular movement */
{
Vector3 linVel = new Vector3();
Vector3 angVel = new Vector3();
TransformUtil.CalculateVelocity (ref convexFromTrans, ref convexToTrans, 1.0f, ref linVel, ref angVel);
Vector3 zeroLinVel = new Vector3();
Matrix R = MathUtil.BasisMatrix(ref convexFromTrans);
castShape.CalculateTemporalAabb (ref R, ref zeroLinVel, ref angVel, 1.0f, ref castShapeAabbMin, ref castShapeAabbMax);
}
#if !USE_BRUTEFORCE_RAYBROADPHASE
SingleSweepCallback convexCB = new SingleSweepCallback(castShape,ref convexFromWorld,ref convexToWorld,this,resultCallback,allowedCcdPenetration);
Vector3 tempFrom = convexFromTrans.Translation;
Vector3 tempTo = convexToTrans.Translation;
m_broadphasePairCache.RayTest(ref tempFrom,ref tempTo,convexCB,ref castShapeAabbMin,ref castShapeAabbMax);
convexCB.Cleanup();
#else
/// go over all objects, and if the ray intersects their aabb + cast shape aabb,
// do a ray-shape query using convexCaster (CCD)
int i;
for (i=0;i<m_collisionObjects.Count;i++)
{
CollisionObject collisionObject= m_collisionObjects[i];
//only perform raycast if filterMask matches
if(resultCallback.NeedsCollision(collisionObject.GetBroadphaseHandle()))
{
//RigidcollisionObject* collisionObject = ctrl.GetRigidcollisionObject();
Vector3 collisionObjectAabbMin = new Vector3();
Vector3 collisionObjectAabbMax = new Vector3();
collisionObject.GetCollisionShape().GetAabb(collisionObject.GetWorldTransform(),ref collisionObjectAabbMin,ref collisionObjectAabbMax);
AabbUtil2.AabbExpand(ref collisionObjectAabbMin, ref collisionObjectAabbMax, ref castShapeAabbMin, ref castShapeAabbMax);
float hitLambda = 1f; //could use resultCallback.m_closestHitFraction, but needs testing
Vector3 hitNormal = new Vector3();
Vector3 fromOrigin = convexFromWorld.Translation;
Vector3 toOrigin = convexToWorld.Translation;
if (AabbUtil2.RayAabb(ref fromOrigin, ref toOrigin, ref collisionObjectAabbMin, ref collisionObjectAabbMax, ref hitLambda, ref hitNormal))
{
Matrix trans = collisionObject.GetWorldTransform();
ObjectQuerySingle(castShape, ref convexFromTrans,ref convexToTrans,
collisionObject,
collisionObject.GetCollisionShape(),
ref trans,
resultCallback,
allowedCcdPenetration);
}
}
}
#endif //USE_BRUTEFORCE_RAYBROADPHASE
}
// convexTest performs a swept convex cast 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 return by the callback.
public virtual void ConvexSweepTest(ConvexShape castShape, Matrix convexFromWorld, Matrix convexToWorld, ConvexResultCallback resultCallback, float allowedCcdPenetration)
{
ConvexSweepTest(castShape, ref convexFromWorld, ref convexToWorld, resultCallback, allowedCcdPenetration);
}
public static bool Penetration(ConvexShape shape0,ref Matrix wtrs0,ConvexShape shape1,ref Matrix wtrs1,ref Vector3 guess,GjkEpaSolver2Results results,bool usemargins)
{
GjkEpaSolver2MinkowskiDiff shape = new GjkEpaSolver2MinkowskiDiff();
Initialize(shape0,ref wtrs0,shape1,ref wtrs1,results, shape,usemargins);
GJK gjk = new GJK();
Vector3 minusGuess = -guess;
GJKStatus gjk_status=gjk.Evaluate(shape,ref minusGuess);
switch(gjk_status)
{
case GJKStatus.Inside:
{
EPA epa = new EPA();
eStatus epa_status=epa.Evaluate(gjk,ref minusGuess);
if(epa_status!=eStatus.Failed)
{
Vector3 w0 = Vector3.Zero;
for(uint i=0;i<epa.m_result.rank;++i)
{
// order of results here is 'different' , EPA.evaluate.
w0+=shape.Support(ref epa.m_result.c[i].d,0)*epa.m_result.p[i];
}
results.status = GjkEpaSolver2Status.Penetrating;
results.witnesses0 = Vector3.Transform(w0,wtrs0);
results.witnesses1 = Vector3.Transform((w0-epa.m_normal*epa.m_depth),wtrs0);
results.normal = -epa.m_normal;
results.distance = -epa.m_depth;
return(true);
} else results.status=GjkEpaSolver2Status.EPA_Failed;
}
break;
case GJKStatus.Failed:
results.status=GjkEpaSolver2Status.GJK_Failed;
break;
}
return(false);
}
/// objectQuerySingle performs a collision detection query and calls the resultCallback. It is used internally by rayTest.
public static void ObjectQuerySingle(ConvexShape castShape,ref Matrix convexFromTrans,ref Matrix convexToTrans,
CollisionObject collisionObject,CollisionShape collisionShape,
ref Matrix colObjWorldTransform,
ConvexResultCallback resultCallback, float allowedPenetration)
{
if (collisionShape.IsConvex())
{
//BT_PROFILE("convexSweepConvex");
CastResult castResult = new CastResult();
castResult.m_allowedPenetration = allowedPenetration;
castResult.m_fraction = resultCallback.m_closestHitFraction;//float(1.);//??
ConvexShape convexShape = (ConvexShape) collisionShape;
VoronoiSimplexSolver simplexSolver = new VoronoiSimplexSolver();
GjkEpaPenetrationDepthSolver gjkEpaPenetrationSolver = new GjkEpaPenetrationDepthSolver();
ContinuousConvexCollision convexCaster1 = new ContinuousConvexCollision(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,
ref castResult.m_normal,
ref castResult.m_hitPoint,
castResult.m_fraction
);
bool normalInWorldSpace = true;
resultCallback.AddSingleResult(localConvexResult, normalInWorldSpace);
}
}
}
}
else
{
if (collisionShape.IsConcave())
{
if (collisionShape.ShapeType==BroadphaseNativeTypes.TRIANGLE_MESH_SHAPE_PROXYTYPE)
{
//BT_PROFILE("convexSweepbtBvhTriangleMesh");
BvhTriangleMeshShape triangleMesh = (BvhTriangleMeshShape)collisionShape;
Matrix worldTocollisionObject = Matrix.Invert(colObjWorldTransform);
Vector3 convexFromLocal = Vector3.Transform(convexFromTrans.Translation,worldTocollisionObject);
Vector3 convexToLocal = Vector3.Transform(convexToTrans.Translation,worldTocollisionObject);
// rotation of box in local mesh space = MeshRotation^-1 * ConvexToRotation
Matrix rotationXform = MathUtil.BasisMatrix(ref worldTocollisionObject) * MathUtil.BasisMatrix(ref convexToTrans);
BridgeTriangleConvexcastCallback tccb = new BridgeTriangleConvexcastCallback(castShape, ref convexFromTrans,ref convexToTrans,resultCallback,collisionObject,triangleMesh, ref colObjWorldTransform);
tccb.m_hitFraction = resultCallback.m_closestHitFraction;
Vector3 boxMinLocal = new Vector3();
Vector3 boxMaxLocal = new Vector3();
castShape.GetAabb(ref rotationXform, ref boxMinLocal, ref boxMaxLocal);
triangleMesh.PerformConvexCast(tccb,ref convexFromLocal,ref convexToLocal,ref boxMinLocal, ref boxMaxLocal);
}
else
{
//BT_PROFILE("convexSweepConcave");
ConcaveShape concaveShape = (ConcaveShape)collisionShape;
Matrix worldTocollisionObject = Matrix.Invert(colObjWorldTransform);
Vector3 convexFromLocal = Vector3.Transform(convexFromTrans.Translation,worldTocollisionObject);
Vector3 convexToLocal = Vector3.Transform(convexToTrans.Translation,worldTocollisionObject);
// rotation of box in local mesh space = MeshRotation^-1 * ConvexToRotation
Matrix rotationXform = MathUtil.BasisMatrix(ref worldTocollisionObject) * MathUtil.BasisMatrix(ref convexToTrans);
BridgeTriangleConvexcastCallback2 tccb = new BridgeTriangleConvexcastCallback2(castShape, ref convexFromTrans, ref convexToTrans, resultCallback, collisionObject, concaveShape, ref colObjWorldTransform);
tccb.m_hitFraction = resultCallback.m_closestHitFraction;
Vector3 boxMinLocal = new Vector3();
Vector3 boxMaxLocal = new Vector3();
castShape.GetAabb(ref rotationXform, ref boxMinLocal, ref boxMaxLocal);
Vector3 rayAabbMinLocal = convexFromLocal;
MathUtil.VectorMin(ref convexToLocal,ref rayAabbMinLocal);
//rayAabbMinLocal.setMin(convexToLocal);
Vector3 rayAabbMaxLocal = convexFromLocal;
//rayAabbMaxLocal.setMax(convexToLocal);
MathUtil.VectorMax(ref convexToLocal,ref rayAabbMaxLocal);
rayAabbMinLocal += boxMinLocal;
rayAabbMaxLocal += boxMaxLocal;
concaveShape.ProcessAllTriangles(tccb,ref rayAabbMinLocal,ref rayAabbMaxLocal);
}
}
else
{
///@todo : use AABB tree or other BVH acceleration structure!
if (collisionShape.IsCompound())
{
//BT_PROFILE("convexSweepCompound");
CompoundShape compoundShape = (CompoundShape)collisionShape;
for (int i=0;i<compoundShape.GetNumChildShapes();i++)
{
Matrix childTrans = compoundShape.GetChildTransform(i);
CollisionShape childCollisionShape = compoundShape.GetChildShape(i);
Matrix childWorldTrans = MathUtil.BulletMatrixMultiply(colObjWorldTransform,childTrans);
// replace collision shape so that callback can determine the triangle
CollisionShape saveCollisionShape = collisionObject.GetCollisionShape();
collisionObject.InternalSetTemporaryCollisionShape((CollisionShape)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);
}
}
}
}
}
public UniformScalingShape(ConvexShape childConvexShape, float uniformScalingFactor)
{
m_childConvexShape = childConvexShape;
m_uniformScalingFactor = uniformScalingFactor;
m_shapeType = BroadphaseNativeTypes.UNIFORM_SCALING_SHAPE_PROXYTYPE;
}
public static bool Penetration(ConvexShape shape0, ref Matrix wtrs0, ConvexShape shape1, ref Matrix wtrs1, ref Vector3 guess, GjkEpaSolver2Results results)
{
return Penetration(shape0, ref wtrs0, shape1, ref wtrs1, ref guess, results, true);
}
public void SetMinkowskiB(ConvexShape minkB)
{
m_minkowskiB = minkB;
}
public bool CalcPenDepth(ISimplexSolverInterface simplexSolver, ConvexShape convexA, ConvexShape convexB, ref Matrix transA, ref Matrix transB,
ref Vector3 v, ref Vector3 pa, ref Vector3 pb, IDebugDraw debugDraw)
{
bool check2d = convexA.IsConvex2D() && convexB.IsConvex2D();
float minProj = float.MaxValue;
Vector3 minNorm = Vector3.Zero;
Vector3 minA = Vector3.Zero, minB = Vector3.Zero;
Vector3 seperatingAxisInA, seperatingAxisInB;
Vector3 pInA, qInB, pWorld, qWorld, w;
#if USE_BATCHED_SUPPORT
IList<Vector4> supportVerticesABatch = new ObjectArray<Vector4>(NUM_UNITSPHERE_POINTS + ConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2);
IList<Vector4> supportVerticesBBatch = new ObjectArray<Vector4>(NUM_UNITSPHERE_POINTS + ConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2);
IList<Vector3> seperatingAxisInABatch = new ObjectArray<Vector3>(NUM_UNITSPHERE_POINTS + ConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2);
IList<Vector3> seperatingAxisInBBatch = new ObjectArray<Vector3>(NUM_UNITSPHERE_POINTS + ConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2);
int numSampleDirections = NUM_UNITSPHERE_POINTS;
for (int i = 0; i < numSampleDirections; i++)
{
Vector3 norm = sPenetrationDirections[i];
seperatingAxisInABatch[i] = MathUtil.TransposeTransformNormal(-norm, transA);
seperatingAxisInBBatch[i] = MathUtil.TransposeTransformNormal(norm, transB);
}
{
int numPDA = convexA.GetNumPreferredPenetrationDirections();
if (numPDA > 0)
{
for (int i = 0; i < numPDA; i++)
{
Vector3 norm = Vector3.Up;
convexA.GetPreferredPenetrationDirection(i, ref norm);
norm = Vector3.TransformNormal(norm, transA);
sPenetrationDirections[numSampleDirections] = norm;
seperatingAxisInABatch[numSampleDirections] = Vector3.TransformNormal(-norm, transA);
seperatingAxisInBBatch[numSampleDirections] = Vector3.Transform(norm, transB);
numSampleDirections++;
}
}
}
{
int numPDB = convexB.GetNumPreferredPenetrationDirections();
if (numPDB > 0)
{
for (int i = 0; i < numPDB; i++)
{
Vector3 norm = Vector3.Up;
convexB.GetPreferredPenetrationDirection(i, ref norm);
norm = Vector3.TransformNormal(norm, transB);
sPenetrationDirections[numSampleDirections] = norm;
seperatingAxisInABatch[numSampleDirections] = Vector3.TransformNormal(-norm, transA);
seperatingAxisInBBatch[numSampleDirections] = Vector3.TransformNormal(norm, transB);
numSampleDirections++;
}
}
}
convexA.BatchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch, supportVerticesABatch, numSampleDirections);
convexB.BatchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch, supportVerticesBBatch, numSampleDirections);
for (int i = 0; i < numSampleDirections; i++)
{
Vector3 norm = sPenetrationDirections[i];
if (check2d)
{
// shouldn't this be Y ?
norm.Z = 0;
}
seperatingAxisInA = seperatingAxisInABatch[i];
seperatingAxisInB = seperatingAxisInBBatch[i];
pInA = new Vector3(supportVerticesABatch[i].X, supportVerticesABatch[i].Y, supportVerticesABatch[i].Z);
qInB = new Vector3(supportVerticesBBatch[i].X, supportVerticesBBatch[i].Y, supportVerticesBBatch[i].Z);
pWorld = Vector3.Transform(pInA, transA);
qWorld = Vector3.Transform(qInB, transB);
if (check2d)
{
// shouldn't this be Y ?
pWorld.Z = 0f;
qWorld.Z = 0f;
}
w = qWorld - pWorld;
float delta = Vector3.Dot(norm, w);
//find smallest delta
if (delta < minProj)
{
minProj = delta;
minNorm = norm;
minA = pWorld;
minB = qWorld;
}
}
#else
int numSampleDirections = NUM_UNITSPHERE_POINTS;
{
int numPDA = convexA.getNumPreferredPenetrationDirections();
if (numPDA > 0)
{
for (int i=0;i<numPDA;i++)
{
Vector3 norm = Vector3.Zero;
convexA.getPreferredPenetrationDirection(i,ref norm);
norm = Vector3.TransformNormal(norm,transA);
sPenetrationDirections[numSampleDirections] = norm;
numSampleDirections++;
}
}
}
{
int numPDB = convexB.getNumPreferredPenetrationDirections();
if (numPDB > 0)
{
for (int i=0;i<numPDB;i++)
{
Vector3 norm = Vector3.Zero;
convexB.getPreferredPenetrationDirection(i,ref norm);
norm = Vector3.TransformNormal(norm,transB);
sPenetrationDirections[numSampleDirections] = norm;
numSampleDirections++;
}
}
}
for (int i=0;i<numSampleDirections;i++)
{
Vector3 norm = sPenetrationDirections[i];
if (check2d)
{
norm.Z = 0f;
}
if (norm.LengthSquared() > 0.01f)
{
seperatingAxisInA = Vector3.TransformNormal(-norm, transA);
seperatingAxisInB = Vector3.TransformNormal(norm, transB);
pInA = convexA.localGetSupportVertexWithoutMarginNonVirtual(ref seperatingAxisInA);
qInB = convexB.localGetSupportVertexWithoutMarginNonVirtual(ref seperatingAxisInB);
pWorld = Vector3.Transform(pInA, transA);
qWorld = Vector3.Transform(qInB, transB);
if (check2d)
{
pWorld.Z = 0.0f;
qWorld.Z = 0.0f;
}
w = qWorld - pWorld;
float delta = Vector3.Dot(norm, w);
//find smallest delta
if (delta < minProj)
{
minProj = delta;
minNorm = norm;
minA = pWorld;
minB = qWorld;
}
}
}
#endif //USE_BATCHED_SUPPORT
//add the margins
minA += minNorm * convexA.GetMarginNonVirtual();
minB -= minNorm * convexB.GetMarginNonVirtual();
//no penetration
if (minProj < 0f)
{
return false;
}
float extraSeparation = 0.5f;///scale dependent
minProj += extraSeparation + (convexA.GetMarginNonVirtual() + convexB.GetMarginNonVirtual());
#if DEBUG_DRAW
if (debugDraw)
{
Vector3 color = new Vector3(0,1,0);
debugDraw.drawLine(minA,minB,color);
color = new Vector3(1,1,1);
Vector3 vec = minB-minA;
float prj2 = Vector3.Dot(minNorm,vec);
debugDraw.drawLine(minA,minA+(minNorm*minProj),color);
}
#endif //DEBUG_DRAW
GjkPairDetector gjkdet = new GjkPairDetector(convexA, convexB, simplexSolver, null);
float offsetDist = minProj;
Vector3 offset = minNorm * offsetDist;
ClosestPointInput input = new ClosestPointInput();
Vector3 newOrg = transA.Translation + offset;
Matrix displacedTrans = transA;
displacedTrans.Translation = newOrg;
input.m_transformA = displacedTrans;
input.m_transformB = transB;
input.m_maximumDistanceSquared = float.MaxValue;
MinkowskiIntermediateResult res = new MinkowskiIntermediateResult();
Vector3 temp = -minNorm;
gjkdet.SetCachedSeperatingAxis(-minNorm);
gjkdet.GetClosestPoints(input, res, debugDraw,false);
float correctedMinNorm = minProj - res.m_depth;
//the penetration depth is over-estimated, relax it
float penetration_relaxation = 1f;
minNorm *= penetration_relaxation;
if (res.m_hasResult)
{
pa = res.m_pointInWorld - minNorm * correctedMinNorm;
pb = res.m_pointInWorld;
v = minNorm;
#if DEBUG_DRAW
if (debugDraw != null)
{
Vector3 color = new Vector3(1,0,0);
debugDraw.drawLine(pa,pb,color);
}
#endif//DEBUG_DRAW
}
return res.m_hasResult;
}
public SubSimplexConvexCast (ConvexShape shapeA,ConvexShape shapeB,ISimplexSolverInterface simplexSolver)
{
m_convexA = shapeA;
m_convexB = shapeB;
m_simplexSolver = simplexSolver;
}
