public bool CleanupVertices(int svcount,
IList<IndexedVector3> svertices,
int stride,
ref int vcount, // output number of vertices
IList<IndexedVector3> vertices, // location to store the results.
float normalepsilon,
ref IndexedVector3 scale)
{
if (svcount == 0)
{
return false;
}
m_vertexIndexMapping.Clear();
vcount = 0;
IndexedVector3 recip = new IndexedVector3();
scale = new IndexedVector3(1);
IndexedVector3 bmin = MathUtil.MAX_VECTOR;
IndexedVector3 bmax = MathUtil.MIN_VECTOR;
//char *vtx = (char *) svertices;
// if ( 1 )
{
for (int i = 0; i < svcount; i++)
{
IndexedVector3 p = svertices[i];
MathUtil.VectorMin(ref p, ref bmin);
MathUtil.VectorMax(ref p, ref bmax);
svertices[i] = p;
}
}
IndexedVector3 diff = bmax - bmin;
IndexedVector3 center = diff * 0.5f;
center += bmin;
if (diff.X < EPSILON || diff.Y < EPSILON || diff.Z < EPSILON || svcount < 3)
{
float len = float.MaxValue;
if (diff.X > EPSILON && diff.X < len) len = diff.X;
if (diff.Y > EPSILON && diff.Y < len) len = diff.Y;
if (diff.Z > EPSILON && diff.Z < len) len = diff.Z;
if (len == float.MaxValue)
{
diff = new IndexedVector3(0.01f);
}
else
{
if (diff.X < EPSILON) diff.X = len * 0.05f; // 1/5th the shortest non-zero edge.
if (diff.Y < EPSILON) diff.Y = len * 0.05f;
if (diff.Z < EPSILON) diff.Z = len * 0.05f;
}
float x1 = center.X - diff.X;
float x2 = center.X + diff.X;
float y1 = center.Y - diff.Y;
float y2 = center.Y + diff.Y;
float z1 = center.Z - diff.Z;
float z2 = center.Z + diff.Z;
AddPoint(ref vcount, vertices, x1, y1, z1);
AddPoint(ref vcount, vertices, x2, y1, z1);
AddPoint(ref vcount, vertices, x2, y2, z1);
AddPoint(ref vcount, vertices, x1, y2, z1);
AddPoint(ref vcount, vertices, x1, y1, z2);
AddPoint(ref vcount, vertices, x2, y1, z2);
AddPoint(ref vcount, vertices, x2, y2, z2);
AddPoint(ref vcount, vertices, x1, y2, z2);
return true; // return cube
}
else
{
if (scale.LengthSquared() > 0)
{
scale = diff;
//scale.Value.X = dx;
//scale.Value.Y = dy;
//scale.Value.Z = dz;
recip.X = 1 / diff.X;
recip.Y = 1 / diff.Y;
recip.Z = 1 / diff.Z;
//recip[0] = 1 / dx;
//recip[1] = 1 / dy;
//recip[2] = 1 / dz;
center = center * recip;
//center.X*=recip[0];
//center.Y*=recip[1];
//center.Z*=recip[2];
}
}
//vtx = (char *) svertices;
for (int i = 0; i < svcount; i++)
{
IndexedVector3 p = svertices[i];
if (scale.LengthSquared() > 0)
{
//p.Normalize();
p.X *= recip.X;
p.Y *= recip.Y;
p.Z *= recip.Z;
}
// if ( 1 )
{
int j = 0;
for (j = 0; j < vcount; j++)
{
/// XXX might be broken
IndexedVector3 v = vertices[j];
IndexedVector3 temp = v - p;
IndexedVector3 absTemp = temp.Absolute();
if (absTemp.X < normalepsilon && absTemp.Y < normalepsilon && absTemp.Z < normalepsilon)
{
// ok, it is close enough to the old one
// now let us see if it is further from the center of the point cloud than the one we already recorded.
// in which case we keep this one instead.
float dist1 = (p - center).LengthSquared();
float dist2 = (v - center).LengthSquared();
if (dist1 > dist2)
{
vertices[j] = p;
}
break;
}
}
if (j == vcount)
{
vertices[vcount] = p;
vcount++;
}
m_vertexIndexMapping.Add(j);
}
}
// ok..now make sure we didn't prune so many vertices it is now invalid.
// if ( 1 )
{
float[] bmin2 = new float[] { float.MaxValue, float.MaxValue, float.MaxValue };
float[] bmax2 = new float[] { float.MinValue, float.MinValue, float.MinValue };
for (int i = 0; i < vcount; i++)
{
IndexedVector3 p = vertices[i];
if (p.X < bmin2[0]) bmin2[0] = p.X;
if (p.X > bmax2[0]) bmax2[0] = p.X;
if (p.Y < bmin2[1]) bmin2[1] = p.Y;
if (p.Y > bmax2[1]) bmax2[1] = p.Y;
if (p.Z < bmin2[2]) bmin2[2] = p.Z;
if (p.Z > bmax2[2]) bmax2[2] = p.Z;
}
float dx2 = bmax2[0] - bmin2[0];
float dy2 = bmax2[1] - bmin2[1];
float dz2 = bmax2[2] - bmin2[2];
if (dx2 < EPSILON || dy2 < EPSILON || dz2 < EPSILON || vcount < 3)
{
float cx = dx2 * 0.5f + bmin2[0];
float cy = dy2 * 0.5f + bmin2[1];
float cz = dz2 * 0.5f + bmin2[2];
float len = float.MaxValue;
if (dx2 >= EPSILON && dx2 < len) len = dx2;
if (dy2 >= EPSILON && dy2 < len) len = dy2;
if (dz2 >= EPSILON && dz2 < len) len = dz2;
if (len == float.MaxValue)
{
dx2 = dy2 = dz2 = 0.01f; // one centimeter
}
else
{
if (dx2 < EPSILON) dx2 = len * 0.05f; // 1/5th the shortest non-zero edge.
if (dy2 < EPSILON) dy2 = len * 0.05f;
if (dz2 < EPSILON) dz2 = len * 0.05f;
}
float x1 = cx - dx2;
float x2 = cx + dx2;
float y1 = cy - dy2;
float y2 = cy + dy2;
float z1 = cz - dz2;
float z2 = cz + dz2;
vcount = 0; // add box
AddPoint(ref vcount, vertices, x1, y1, z1);
AddPoint(ref vcount, vertices, x2, y1, z1);
AddPoint(ref vcount, vertices, x2, y2, z1);
AddPoint(ref vcount, vertices, x1, y2, z1);
AddPoint(ref vcount, vertices, x1, y1, z2);
AddPoint(ref vcount, vertices, x2, y1, z2);
AddPoint(ref vcount, vertices, x2, y2, z2);
AddPoint(ref vcount, vertices, x1, y2, z2);
return true;
}
}
return true;
}
public void GetClosestPointsNonVirtual(ref ClosestPointInput input, IDiscreteCollisionDetectorInterfaceResult output, IDebugDraw debugDraw)
{
m_cachedSeparatingDistance = 0f;
float distance = 0f;
IndexedVector3 normalInB = IndexedVector3.Zero;
IndexedVector3 pointOnA = IndexedVector3.Zero, pointOnB = IndexedVector3.Zero;
IndexedMatrix localTransA = input.m_transformA;
IndexedMatrix localTransB = input.m_transformB;
IndexedVector3 positionOffset = (localTransA._origin + localTransB._origin) * .5f;
IndexedVector3.Subtract(out localTransA._origin, ref localTransA._origin,ref positionOffset);
IndexedVector3.Subtract(out localTransB._origin, ref localTransB._origin, ref positionOffset);
//localTransB._origin -= positionOffset;
bool check2d = m_minkowskiA.IsConvex2d() && m_minkowskiB.IsConvex2d();
float marginA = m_marginA;
float marginB = m_marginB;
#if TEST_NON_VIRTUAL
float marginAv = m_minkowskiA.getMarginNonVirtual();
float marginBv = m_minkowskiB.getMarginNonVirtual();
Debug.Assert(marginA == marginAv);
Debug.Assert(marginB == marginBv);
#endif //TEST_NON_VIRTUAL
gNumGjkChecks++;
#if DEBUG_SPU_COLLISION_DETECTION
spu_printf("inside gjk\n");
#endif
//for CCD we don't use margins
if (m_ignoreMargin)
{
marginA = 0f;
marginB = 0f;
#if DEBUG_SPU_COLLISION_DETECTION
spu_printf("ignoring margin\n");
#endif
}
m_curIter = 0;
int gGjkMaxIter = 1000;//this is to catch invalid input, perhaps check for #NaN?
m_cachedSeparatingAxis = new IndexedVector3(0, 1, 0);
bool isValid = false;
bool checkSimplex = false;
bool checkPenetration = true;
m_degenerateSimplex = 0;
m_lastUsedMethod = -1;
#if DEBUG
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGJKDetector)
{
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "gjk::getClosestPointsNonVirtual transA", localTransA);
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "gjk::getClosestPointsNonVirtual transB", localTransB);
}
#endif
{
float squaredDistance = MathUtil.BT_LARGE_FLOAT;
float delta = 0f;
float margin = marginA + marginB;
m_simplexSolver.Reset();
int count = 0;
for (; ; )
//while (true)
{
count++;
IndexedVector3 seperatingAxisInA = (-m_cachedSeparatingAxis) * input.m_transformA._basis;
IndexedVector3 seperatingAxisInB = m_cachedSeparatingAxis * input.m_transformB._basis;
IndexedVector3 pInA = m_minkowskiA.LocalGetSupportVertexWithoutMarginNonVirtual(ref seperatingAxisInA);
IndexedVector3 qInB = m_minkowskiB.LocalGetSupportVertexWithoutMarginNonVirtual(ref seperatingAxisInB);
IndexedVector3 pWorld = localTransA * pInA;
IndexedVector3 qWorld = localTransB * qInB;
if (check2d)
{
pWorld.Z = 0.0f;
qWorld.Z = 0.0f;
}
IndexedVector3 w = new IndexedVector3(pWorld.X - qWorld.X,pWorld.Y - qWorld.Y,pWorld.Z - qWorld.Z);
delta = IndexedVector3.Dot(ref m_cachedSeparatingAxis, ref w);
#if DEBUG
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGJKDetector)
{
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "m_cachedSeparatingAxis", m_cachedSeparatingAxis);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "w", w);
BulletGlobals.g_streamWriter.WriteLine(String.Format("simplex num vertices [{0}]", m_simplexSolver.NumVertices()));
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "sepAxisA", seperatingAxisInA);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "sepAxisB", seperatingAxisInB);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "pInA", pInA);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "qInB", qInB);
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "localTransA", localTransA);
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "localTransB", localTransB);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "pWorld", pWorld);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "qWorld", qWorld);
}
#endif
// potential exit, they don't overlap
if ((delta > 0f) && (delta * delta > squaredDistance * input.m_maximumDistanceSquared))
{
m_degenerateSimplex = 10;
checkSimplex = true;
//checkPenetration = false;
break;
}
//exit 0: the new point is already in the simplex, or we didn't come any closer
if (m_simplexSolver.InSimplex(ref w))
{
m_degenerateSimplex = 1;
checkSimplex = true;
break;
}
// are we getting any closer ?
float f0 = squaredDistance - delta;
float f1 = squaredDistance * REL_ERROR2;
if (f0 <= f1)
{
if (f0 <= 0f)
{
m_degenerateSimplex = 2;
}
else
{
m_degenerateSimplex = 11;
}
checkSimplex = true;
break;
}
//add current vertex to simplex
m_simplexSolver.AddVertex(ref w, ref pWorld, ref qWorld);
//calculate the closest point to the origin (update vector v)
IndexedVector3 newCachedSeparatingAxis;
if (!m_simplexSolver.Closest(out newCachedSeparatingAxis))
{
m_degenerateSimplex = 3;
checkSimplex = true;
break;
}
if (newCachedSeparatingAxis.LengthSquared() < REL_ERROR2)
{
m_cachedSeparatingAxis = newCachedSeparatingAxis;
m_degenerateSimplex = 6;
checkSimplex = true;
break;
}
float previousSquaredDistance = squaredDistance;
squaredDistance = newCachedSeparatingAxis.LengthSquared();
#if DEBUG
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGJKDetector)
{
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "sepAxisA", seperatingAxisInA);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "sepAxisB", seperatingAxisInB);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "pInA", pInA);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "qInB", qInB);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "pWorld", pWorld);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "qWorld", qWorld);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "newSeperatingAxis", newCachedSeparatingAxis);
BulletGlobals.g_streamWriter.WriteLine(String.Format("f0[{0:0.00000000}] f1[{1:0.00000000}] checkSimplex[{2}] degen[{3}]", f0, f1, checkSimplex, m_degenerateSimplex));
}
#endif
#if false
///warning: this termination condition leads to some problems in 2d test case see Bullet/Demos/Box2dDemo
if (squaredDistance>previousSquaredDistance)
{
m_degenerateSimplex = 7;
squaredDistance = previousSquaredDistance;
checkSimplex = false;
break;
}
#endif //
//redundant m_simplexSolver->compute_points(pointOnA, pointOnB);
//are we getting any closer ?
if (previousSquaredDistance - squaredDistance <= MathUtil.SIMD_EPSILON * previousSquaredDistance)
{
//m_simplexSolver.BackupClosest(ref m_cachedSeparatingAxis);
checkSimplex = true;
m_degenerateSimplex = 12;
break;
}
m_cachedSeparatingAxis = newCachedSeparatingAxis;
//degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject
if (m_curIter++ > gGjkMaxIter)
{
//#if defined(DEBUG) || defined (_DEBUG) || defined (DEBUG_SPU_COLLISION_DETECTION)
// printf("btGjkPairDetector maxIter exceeded:%i\n",m_curIter);
// printf("sepAxis=(%f,%f,%f), squaredDistance = %f, shapeTypeA=%i,shapeTypeB=%i\n",
// m_cachedSeparatingAxis.getX(),
// m_cachedSeparatingAxis.getY(),
// m_cachedSeparatingAxis.getZ(),
// squaredDistance,
// m_minkowskiA->getShapeType(),
// m_minkowskiB->getShapeType());
//#endif
break;
}
bool check = (!m_simplexSolver.FullSimplex());
//bool check = (!m_simplexSolver->fullSimplex() && squaredDistance > SIMD_EPSILON * m_simplexSolver->maxVertex());
if (!check)
{
//do we need this backup_closest here ?
//m_simplexSolver.BackupClosest(ref m_cachedSeparatingAxis);
m_degenerateSimplex = 13;
break;
}
}
if (checkSimplex)
{
m_simplexSolver.ComputePoints(out pointOnA, out pointOnB);
normalInB = m_cachedSeparatingAxis;
float lenSqr = m_cachedSeparatingAxis.LengthSquared();
//valid normal
if (lenSqr < 0.0001f)
{
m_degenerateSimplex = 5;
}
if (lenSqr > MathUtil.SIMD_EPSILON * MathUtil.SIMD_EPSILON)
{
//float rlen = 1 / normalInB.Length();
float rlen = 1.0f / (float)Math.Sqrt((float)lenSqr);
//normalInB.Normalize();
normalInB *= rlen;
float s = (float)Math.Sqrt((float)squaredDistance);
Debug.Assert(s > 0f);
pointOnA -= m_cachedSeparatingAxis * (marginA / s);
pointOnB += m_cachedSeparatingAxis * (marginB / s);
distance = ((1f / rlen) - margin);
isValid = true;
m_lastUsedMethod = 1;
}
else
{
m_lastUsedMethod = 2;
}
}
bool catchDegeneratePenetrationCase =
(m_catchDegeneracies && m_penetrationDepthSolver != null && m_degenerateSimplex > 0 && ((distance + margin) < 0.01));
//if (checkPenetration && !isValid)
if (checkPenetration && (!isValid || catchDegeneratePenetrationCase))
{
//penetration case
//if there is no way to handle penetrations, bail ref
if (m_penetrationDepthSolver != null)
{
// Penetration depth case.
IndexedVector3 tmpPointOnA = IndexedVector3.Zero, tmpPointOnB = IndexedVector3.Zero;
gNumDeepPenetrationChecks++;
m_cachedSeparatingAxis = IndexedVector3.Zero;
bool isValid2 = m_penetrationDepthSolver.CalcPenDepth(
m_simplexSolver,
m_minkowskiA, m_minkowskiB,
ref localTransA, ref localTransB,
ref m_cachedSeparatingAxis, ref tmpPointOnA, ref tmpPointOnB,
debugDraw
);
#if DEBUG
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGJKDetector)
{
BulletGlobals.g_streamWriter.WriteLine("calcPenDepthResult");
BulletGlobals.g_streamWriter.WriteLine("lastMethodUsed : " + m_lastUsedMethod);
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "localTransA", localTransA);
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "localTransB", localTransB);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "sepAxis", m_cachedSeparatingAxis);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "tmpA", tmpPointOnA);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "tmpB", tmpPointOnB);
}
#endif
if (isValid2)
{
IndexedVector3 tmpNormalInB = tmpPointOnB - tmpPointOnA;
float lenSqr = tmpNormalInB.LengthSquared();
if (lenSqr <= (MathUtil.SIMD_EPSILON * MathUtil.SIMD_EPSILON))
{
tmpNormalInB = m_cachedSeparatingAxis;
lenSqr = m_cachedSeparatingAxis.LengthSquared();
}
if (lenSqr > (MathUtil.SIMD_EPSILON * MathUtil.SIMD_EPSILON))
{
tmpNormalInB /= (float)Math.Sqrt(lenSqr);
float distance2 = -(tmpPointOnA - tmpPointOnB).Length();
//only replace valid penetrations when the result is deeper (check)
if (!isValid || (distance2 < distance))
{
distance = distance2;
pointOnA = tmpPointOnA;
pointOnB = tmpPointOnB;
normalInB = tmpNormalInB;
isValid = true;
//FIXME! check2d THIS
m_lastUsedMethod = 3;
}
else
{
m_lastUsedMethod = 8;
}
}
else
{
//isValid = false;
m_lastUsedMethod = 9;
}
}
else
{
///this is another degenerate case, where the initial GJK calculation reports a degenerate case
///EPA reports no penetration, and the second GJK (using the supporting vector without margin)
///reports a valid positive distance. Use the results of the second GJK instead of failing.
///thanks to Jacob.Langford for the reproduction case
///http://code.google.com/p/bullet/issues/detail?id=250
if (m_cachedSeparatingAxis.LengthSquared() > 0f)
{
float distance2 = (tmpPointOnA - tmpPointOnB).Length() - margin;
//only replace valid distances when the distance is less
if (!isValid || (distance2 < distance))
{
distance = distance2;
pointOnA = tmpPointOnA;
pointOnB = tmpPointOnB;
pointOnA -= m_cachedSeparatingAxis * marginA;
pointOnB += m_cachedSeparatingAxis * marginB;
normalInB = m_cachedSeparatingAxis;
normalInB.Normalize();
isValid = true;
m_lastUsedMethod = 6;
}
else
{
m_lastUsedMethod = 5;
}
}
}
}
}
}
#if DEBUG
if(BulletGlobals.g_streamWriter != null && BulletGlobals.debugGJKDetector)
{
BulletGlobals.g_streamWriter.WriteLine("valid [{0}] distance[{1:0000.00000000}][{2:0000.00000000}] maxDistSq[{3:0000.00000000}]", isValid, distance, distance * distance, input.m_maximumDistanceSquared);
}
#endif
if (isValid && ((distance < 0) || (distance * distance < input.m_maximumDistanceSquared)))
{
m_cachedSeparatingAxis = normalInB;
m_cachedSeparatingDistance = distance;
IndexedVector3 temp = pointOnB + positionOffset;
output.AddContactPoint(
ref normalInB,
ref temp,
distance);
}
}
public bool GetSphereDistance(CollisionObject boxObj, ref IndexedVector3 pointOnBox, ref IndexedVector3 normal, ref float penetrationDepth, IndexedVector3 sphereCenter, float fRadius, float maxContactDistance)
{
BoxShape boxShape = boxObj.GetCollisionShape() as BoxShape;
IndexedVector3 boxHalfExtent = boxShape.GetHalfExtentsWithoutMargin();
float boxMargin = boxShape.GetMargin();
penetrationDepth = 1.0f;
// convert the sphere position to the box's local space
IndexedMatrix m44T = boxObj.GetWorldTransform();
IndexedVector3 sphereRelPos = m44T.InvXform(sphereCenter);
// Determine the closest point to the sphere center in the box
IndexedVector3 closestPoint = sphereRelPos;
closestPoint.X = (Math.Min(boxHalfExtent.X, closestPoint.X));
closestPoint.X = (Math.Max(-boxHalfExtent.X, closestPoint.X));
closestPoint.Y = (Math.Min(boxHalfExtent.Y, closestPoint.Y));
closestPoint.Y = (Math.Max(-boxHalfExtent.Y, closestPoint.Y));
closestPoint.Z = (Math.Min(boxHalfExtent.Z, closestPoint.Z));
closestPoint.Z = (Math.Max(-boxHalfExtent.Z, closestPoint.Z));
float intersectionDist = fRadius + boxMargin;
float contactDist = intersectionDist + maxContactDistance;
normal = sphereRelPos - closestPoint;
//if there is no penetration, we are done
float dist2 = normal.LengthSquared();
if (dist2 > contactDist * contactDist)
{
return false;
}
float distance;
//special case if the sphere center is inside the box
if (dist2 == 0.0f)
{
distance = -GetSpherePenetration(ref boxHalfExtent, ref sphereRelPos, ref closestPoint, ref normal);
}
else //compute the penetration details
{
distance = normal.Length();
normal /= distance;
}
pointOnBox = closestPoint + normal * boxMargin;
// v3PointOnSphere = sphereRelPos - (normal * fRadius);
penetrationDepth = distance - intersectionDist;
// transform back in world space
IndexedVector3 tmp = m44T * pointOnBox;
pointOnBox = tmp;
// tmp = m44T(v3PointOnSphere);
// v3PointOnSphere = tmp;
tmp = m44T._basis * normal;
normal = tmp;
return true;
}
public static void CalculateDiffAxisAngleQuaternion(ref IndexedQuaternion orn0, ref IndexedQuaternion orn1a, out IndexedVector3 axis, out float angle)
{
IndexedQuaternion orn1 = MathUtil.QuatFurthest(ref orn0, ref orn1a);
IndexedQuaternion dorn = orn1 * MathUtil.QuaternionInverse(ref orn0);
///floating point inaccuracy can lead to w component > 1..., which breaks
dorn.Normalize();
angle = MathUtil.QuatAngle(ref dorn);
axis = new IndexedVector3(dorn.X, dorn.Y, dorn.Z);
//check for axis length
float len = axis.LengthSquared();
if (len < MathUtil.SIMD_EPSILON * MathUtil.SIMD_EPSILON)
{
axis = new IndexedVector3(1f, 0, 0);
}
else
{
axis.Normalize();
}
}
public static void CalculateDiffAxisAngle(ref IndexedMatrix transform0, ref IndexedMatrix transform1, out IndexedVector3 axis, out float angle)
{
//IndexedMatrix dmat = GetRotateMatrix(ref transform1) * IndexedMatrix.Invert(GetRotateMatrix(ref transform0));
IndexedBasisMatrix dmat = transform1._basis * transform0._basis.Inverse();
IndexedQuaternion dorn = IndexedQuaternion.Identity;
GetRotation(ref dmat, out dorn);
///floating point inaccuracy can lead to w component > 1..., which breaks
dorn.Normalize();
angle = MathUtil.QuatAngle(ref dorn);
axis = new IndexedVector3(dorn.X, dorn.Y, dorn.Z);
//axis[3] = float(0.);
//check for axis length
float len = axis.LengthSquared();
if (len < MathUtil.SIMD_EPSILON * MathUtil.SIMD_EPSILON)
{
axis = new IndexedVector3(1,0,0);
}
else
{
axis.Normalize();
}
}
public eStatus Evaluate(GJK gjk,ref IndexedVector3 guess)
{
sSimplex simplex=gjk.m_simplex;
if((simplex.rank>1)&&gjk.EncloseOrigin())
{
/* Clean up */
while(m_hull.Count > 0)
{
sFace f = m_hull[0];
Remove(m_hull,f);
Append(m_stock,f);
}
m_status = eStatus.Valid;
m_nextsv = 0;
/* Orient simplex */
if(GJK.Det( simplex.c[0].w-simplex.c[3].w,
simplex.c[1].w-simplex.c[3].w,
simplex.c[2].w-simplex.c[3].w)<0)
{
SwapSv(simplex.c,0,1);
SwapFloat(simplex.p,0,1);
}
/* Build initial hull */
tetra[0] = NewFace(simplex.c[0], simplex.c[1], simplex.c[2], true);
tetra[1] = NewFace(simplex.c[1], simplex.c[0], simplex.c[3], true);
tetra[2] = NewFace(simplex.c[2], simplex.c[1], simplex.c[3], true);
tetra[3] = NewFace(simplex.c[0], simplex.c[2], simplex.c[3], true);
if(m_hull.Count==4)
{
sFace best=FindBest();
sFace outer = best;
uint pass=0;
uint iterations=0;
Bind(tetra[0],0,tetra[1],0);
Bind(tetra[0],1,tetra[2],0);
Bind(tetra[0],2,tetra[3],0);
Bind(tetra[1],1,tetra[3],2);
Bind(tetra[1],2,tetra[2],1);
Bind(tetra[2],2,tetra[3],1);
m_status=eStatus.Valid;
for (; iterations < GjkEpaSolver2.EPA_MAX_ITERATIONS; ++iterations)
{
if (m_nextsv < GjkEpaSolver2.EPA_MAX_VERTICES)
{
sHorizon horizon = new sHorizon() ;
sSV w = m_sv_store[m_nextsv++];
bool valid = true;
best.pass = (uint)(++pass);
gjk.GetSupport(ref best.n,ref w);
float wdist=IndexedVector3.Dot(ref best.n,ref w.w)-best.d;
if (wdist > GjkEpaSolver2.EPA_ACCURACY)
{
for(int j=0;(j<3)&&valid;++j)
{
valid&=Expand( pass,w,
best.f[j],best.e[j],
ref horizon);
}
if(valid&&(horizon.nf>=3))
{
Bind(horizon.cf,1,horizon.ff,2);
Remove(m_hull,best);
Append(m_stock,best);
best=FindBest();
if (best.p >= outer.p)
{
outer = best;
}
}
else
{
m_status=eStatus.InvalidHull;
break;
}
}
else
{
m_status=eStatus.AccuraryReached;
break;
}
}
else
{
m_status=eStatus.OutOfVertices;
break;
}
}
IndexedVector3 projection=outer.n*outer.d;
m_normal = outer.n;
m_depth = outer.d;
m_result.rank = 3;
m_result.c[0] = outer.c[0];
m_result.c[1] = outer.c[1];
m_result.c[2] = outer.c[2];
m_result.p[0] = IndexedVector3.Cross( outer.c[1].w-projection,
outer.c[2].w-projection).Length();
m_result.p[1] = IndexedVector3.Cross(outer.c[2].w - projection,
outer.c[0].w-projection).Length();
m_result.p[2] = IndexedVector3.Cross(outer.c[0].w - projection,
outer.c[1].w-projection).Length();
float sum=m_result.p[0]+m_result.p[1]+m_result.p[2];
m_result.p[0] /= sum;
m_result.p[1] /= sum;
m_result.p[2] /= sum;
return(m_status);
}
}
/* Fallback */
m_status = eStatus.FallBack;
m_normal = -guess;
float nl=m_normal.LengthSquared();
if(nl>0)
{
m_normal.Normalize();
}
else
{
m_normal = new IndexedVector3(1,0,0);
}
m_depth = 0;
m_result.rank=1;
m_result.c[0]=simplex.c[0];
m_result.p[0]=1;
return(m_status);
}
public void ApplyDamping(float timeStep)
{
//On new damping: see discussion/issue report here: http://code.google.com/p/bullet/issues/detail?id=74
//todo: do some performance comparisons (but other parts of the engine are probably bottleneck anyway
//#define USE_OLD_DAMPING_METHOD 1
#if USE_OLD_DAMPING_METHOD
m_linearVelocity *= GEN_clamped((float(1.) - timeStep * m_linearDamping), (float)float(0.0), (float)float(1.0));
m_angularVelocity *= GEN_clamped((float(1.) - timeStep * m_angularDamping), (float)float(0.0), (float)float(1.0));
#else
m_linearVelocity *= (float)Math.Pow((1f-m_linearDamping), timeStep);
m_angularVelocity *= (float)Math.Pow((1f - m_angularDamping), timeStep);
MathUtil.SanityCheckVector(ref m_linearVelocity);
MathUtil.SanityCheckVector(ref m_angularVelocity);
#endif
if (m_additionalDamping)
{
//Additional damping can help avoiding lowpass jitter motion, help stability for ragdolls etc.
//Such damping is undesirable, so once the overall simulation quality of the rigid body dynamics system has improved, this should become obsolete
if ((m_angularVelocity.LengthSquared() < m_additionalAngularDampingThresholdSqr) &&
(m_linearVelocity.LengthSquared() < m_additionalLinearDampingThresholdSqr))
{
m_angularVelocity *= m_additionalDampingFactor;
m_linearVelocity *= m_additionalDampingFactor;
}
MathUtil.SanityCheckVector(ref m_linearVelocity);
MathUtil.SanityCheckVector(ref m_angularVelocity);
float speed = m_linearVelocity.Length();
if (speed < m_linearDamping)
{
float dampVel = 0.005f;
if (speed > dampVel)
{
IndexedVector3 dir = m_linearVelocity;
dir.Normalize();
m_linearVelocity -= dir * dampVel;
}
else
{
m_linearVelocity = IndexedVector3.Zero;
}
}
float angSpeed = m_angularVelocity.Length();
if (angSpeed < m_angularDamping)
{
float angDampVel = 0.005f;
if (angSpeed > angDampVel)
{
IndexedVector3 dir = m_angularVelocity;
dir.Normalize();
m_angularVelocity -= dir * angDampVel;
} else
{
m_angularVelocity = IndexedVector3.Zero;
}
}
}
MathUtil.SanityCheckVector(ref m_linearVelocity);
MathUtil.SanityCheckVector(ref m_angularVelocity);
}
public static float ProjectOrigin(ref IndexedVector3 a,ref IndexedVector3 b,ref IndexedVector4 w,ref uint m)
{
IndexedVector3 d=b-a;
float l=d.LengthSquared();
if (l > GjkEpaSolver2.GJK_SIMPLEX2_EPS)
{
float t = (l>0f?(-IndexedVector3.Dot(ref a,ref d)/l):0f);
if(t>=1)
{
w.X=0f;
w.Y=1f;
m=2;
return b.LengthSquared();
}
else if(t<=0)
{
w.X=1f;
w.Y=0f;
m=1;
return a.LengthSquared();
}
else
{
w.X=1-(w.Y=t);
m=3;
return (a+d*t).LengthSquared();
}
}
return(-1);
}
public GJKStatus Evaluate(GjkEpaSolver2MinkowskiDiff shapearg, ref IndexedVector3 guess)
{
uint iterations=0;
float sqdist=0f;
float alpha=0f;
uint clastw=0;
/* Initialize solver */
m_free[0] = m_store[0];
m_free[1] = m_store[1];
m_free[2] = m_store[2];
m_free[3] = m_store[3];
m_nfree = 4;
m_current = 0;
m_status = GJKStatus.Valid;
m_shape = shapearg;
m_distance = 0f;
/* Initialize simplex */
m_simplices[0].rank = 0;
m_ray = guess;
float sqrl= m_ray.LengthSquared();
IndexedVector3 temp = sqrl>0?-m_ray:new IndexedVector3(1,0,0);
AppendVertice(m_simplices[0],ref temp);
m_simplices[0].p[0] = 1;
m_ray = m_simplices[0].c[0].w;
sqdist = sqrl;
lastw[0]=lastw[1]=lastw[2]=lastw[3] = m_ray;
/* Loop */
do
{
uint next = 1-m_current;
sSimplex cs = m_simplices[m_current];
sSimplex ns = m_simplices[next];
/* Check zero */
float rl=m_ray.Length();
if (rl < GjkEpaSolver2.GJK_MIN_DISTANCE)
{/* Touching or inside */
m_status=GJKStatus.Inside;
break;
}
/* Append new vertice in -'v' direction */
IndexedVector3 temp2 = -m_ray;
AppendVertice(cs,ref temp2);
IndexedVector3 w = cs.c[cs.rank-1].w;
bool found = false;
for(int i=0;i<4;++i)
{
if ((w - lastw[i]).LengthSquared() < GjkEpaSolver2.GJK_DUPLICATED_EPS)
{
found=true;
break;
}
}
if(found)
{/* Return old simplex */
RemoveVertice(m_simplices[m_current]);
break;
}
else
{/* Update lastw */
lastw[clastw=(clastw+1)&3]=w;
}
/* Check for termination */
float omega=IndexedVector3.Dot(ref m_ray,ref w)/rl;
alpha=Math.Max(omega,alpha);
if (((rl - alpha) - (GjkEpaSolver2.GJK_ACCURARY * rl)) <= 0)
{/* Return old simplex */
RemoveVertice(m_simplices[m_current]);
break;
}
/* Reduce simplex */
IndexedVector4 weights = new IndexedVector4();
uint mask=0;
switch(cs.rank)
{
case 2 :
{
sqdist=GJK.ProjectOrigin(ref cs.c[0].w,ref cs.c[1].w,ref weights,ref mask);
break;
}
case 3:
{
sqdist = GJK.ProjectOrigin(ref cs.c[0].w, ref cs.c[1].w, ref cs.c[2].w, ref weights, ref mask);
break;
}
case 4:
{
sqdist = GJK.ProjectOrigin(ref cs.c[0].w, ref cs.c[1].w, ref cs.c[2].w, ref cs.c[3].w, ref weights, ref mask);
break;
}
}
if(sqdist>=0)
{/* Valid */
ns.rank = 0;
m_ray = IndexedVector3.Zero;
m_current = next;
for(uint i=0,ni=cs.rank;i<ni;++i)
{
if((mask&(1<<(int)i)) != 0)
{
ns.c[ns.rank] = cs.c[i];
float weight = weights[(int)i];
ns.p[ns.rank++] = weight;
m_ray += cs.c[i].w * weight;
}
else
{
m_free[m_nfree++] = cs.c[i];
}
}
if(mask==15)
{
m_status=GJKStatus.Inside;
}
}
else
{/* Return old simplex */
RemoveVertice(m_simplices[m_current]);
break;
}
m_status = ((++iterations) < GjkEpaSolver2.GJK_MAX_ITERATIONS) ? m_status : GJKStatus.Failed;
} while(m_status==GJKStatus.Valid);
m_simplex = m_simplices[m_current];
switch(m_status)
{
case GJKStatus.Valid:
{
m_distance=m_ray.Length();
break;
}
case GJKStatus.Inside:
{
m_distance=0;
break;
}
}
#if DEBUG
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGJK)
{
BulletGlobals.g_streamWriter.WriteLine(String.Format("gjk eval dist[{0}]", m_distance));
}
#endif
return(m_status);
}
public void CalcAngleInfo2(ref IndexedMatrix transA, ref IndexedMatrix transB, ref IndexedBasisMatrix invInertiaWorldA, ref IndexedBasisMatrix invInertiaWorldB)
{
m_swingCorrection = 0;
m_twistLimitSign = 0;
m_solveTwistLimit = false;
m_solveSwingLimit = false;
// compute rotation of A wrt B (in constraint space)
if (m_bMotorEnabled && (!m_useSolveConstraintObsolete))
{ // it is assumed that setMotorTarget() was alredy called
// and motor target m_qTarget is within constraint limits
// TODO : split rotation to pure swing and pure twist
// compute desired transforms in world
IndexedMatrix trPose = IndexedMatrix.CreateFromQuaternion(m_qTarget);
IndexedMatrix trA = transA * m_rbAFrame;
IndexedMatrix trB = transB * m_rbBFrame;
IndexedMatrix trDeltaAB = trB * trPose * trA.Inverse();
IndexedQuaternion qDeltaAB = trDeltaAB.GetRotation();
IndexedVector3 swingAxis = new IndexedVector3(qDeltaAB.X, qDeltaAB.Y, qDeltaAB.Z);
float swingAxisLen2 = swingAxis.LengthSquared();
if (MathUtil.FuzzyZero(swingAxisLen2))
{
return;
}
m_swingAxis = swingAxis;
m_swingAxis.Normalize();
m_swingCorrection = MathUtil.QuatAngle(ref qDeltaAB);
if (!MathUtil.FuzzyZero(m_swingCorrection))
{
m_solveSwingLimit = true;
}
return;
}
{
// compute rotation of A wrt B (in constraint space)
// Not sure if these need order swapping as well?
IndexedQuaternion qA = transA.GetRotation() * m_rbAFrame.GetRotation();
IndexedQuaternion qB = transB.GetRotation() * m_rbBFrame.GetRotation();
IndexedQuaternion qAB = MathUtil.QuaternionInverse(qB) * qA;
// split rotation into cone and twist
// (all this is done from B's perspective. Maybe I should be averaging axes...)
IndexedVector3 vConeNoTwist = MathUtil.QuatRotate(ref qAB, ref vTwist);
vConeNoTwist.Normalize();
IndexedQuaternion qABCone = MathUtil.ShortestArcQuat(ref vTwist, ref vConeNoTwist);
qABCone.Normalize();
IndexedQuaternion qABTwist = MathUtil.QuaternionInverse(qABCone) * qAB;
qABTwist.Normalize();
if (m_swingSpan1 >= m_fixThresh && m_swingSpan2 >= m_fixThresh)
{
float swingAngle = 0f, swingLimit = 0f;
IndexedVector3 swingAxis = IndexedVector3.Zero;
ComputeConeLimitInfo(ref qABCone, ref swingAngle, ref swingAxis, ref swingLimit);
if (swingAngle > swingLimit * m_limitSoftness)
{
m_solveSwingLimit = true;
// compute limit ratio: 0->1, where
// 0 == beginning of soft limit
// 1 == hard/real limit
m_swingLimitRatio = 1f;
if (swingAngle < swingLimit && m_limitSoftness < 1f - MathUtil.SIMD_EPSILON)
{
m_swingLimitRatio = (swingAngle - swingLimit * m_limitSoftness) /
(swingLimit - (swingLimit * m_limitSoftness));
}
// swing correction tries to get back to soft limit
m_swingCorrection = swingAngle - (swingLimit * m_limitSoftness);
// adjustment of swing axis (based on ellipse normal)
AdjustSwingAxisToUseEllipseNormal(ref swingAxis);
// Calculate necessary axis & factors
m_swingAxis = MathUtil.QuatRotate(qB, -swingAxis);
m_twistAxisA = IndexedVector3.Zero;
m_kSwing = 1f /
(ComputeAngularImpulseDenominator(ref m_swingAxis, ref invInertiaWorldA) +
ComputeAngularImpulseDenominator(ref m_swingAxis, ref invInertiaWorldB));
}
}
else
{
// you haven't set any limits;
// or you're trying to set at least one of the swing limits too small. (if so, do you really want a conetwist constraint?)
// anyway, we have either hinge or fixed joint
IndexedVector3 ivA = transA._basis * m_rbAFrame._basis.GetColumn(0);
IndexedVector3 jvA = transA._basis * m_rbAFrame._basis.GetColumn(1);
IndexedVector3 kvA = transA._basis * m_rbAFrame._basis.GetColumn(2);
IndexedVector3 ivB = transB._basis * m_rbBFrame._basis.GetColumn(0);
IndexedVector3 target = IndexedVector3.Zero;
float x = ivB.Dot(ref ivA);
float y = ivB.Dot(ref jvA);
float z = ivB.Dot(ref kvA);
if ((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh))
{ // fixed. We'll need to add one more row to constraint
if ((!MathUtil.FuzzyZero(y)) || (!(MathUtil.FuzzyZero(z))))
{
m_solveSwingLimit = true;
m_swingAxis = -ivB.Cross(ref ivA);
}
}
else
{
if (m_swingSpan1 < m_fixThresh)
{ // hinge around Y axis
if (!(MathUtil.FuzzyZero(y)))
{
m_solveSwingLimit = true;
if (m_swingSpan2 >= m_fixThresh)
{
y = 0;
float span2 = (float)Math.Atan2(z, x);
if (span2 > m_swingSpan2)
{
x = (float)Math.Cos(m_swingSpan2);
z = (float)Math.Sin(m_swingSpan2);
}
else if (span2 < -m_swingSpan2)
{
x = (float)Math.Cos(m_swingSpan2);
z = -(float)Math.Sin(m_swingSpan2);
}
}
}
}
else
{ // hinge around Z axis
if (!MathUtil.FuzzyZero(z))
{
m_solveSwingLimit = true;
if (m_swingSpan1 >= m_fixThresh)
{
z = 0f;
float span1 = (float)Math.Atan2(y, x);
if (span1 > m_swingSpan1)
{
x = (float)Math.Cos(m_swingSpan1);
y = (float)Math.Sin(m_swingSpan1);
}
else if (span1 < -m_swingSpan1)
{
x = (float)Math.Cos(m_swingSpan1);
y = -(float)Math.Sin(m_swingSpan1);
}
}
}
}
target.X = x * ivA.X + y * jvA.X + z * kvA.X;
target.Y = x * ivA.Y + y * jvA.Y + z * kvA.Y;
target.Z = x * ivA.Z + y * jvA.Z + z * kvA.Z;
target.Normalize();
m_swingAxis = -(ivB.Cross(ref target));
m_swingCorrection = m_swingAxis.Length();
m_swingAxis.Normalize();
}
}
if (m_twistSpan >= 0f)
{
IndexedVector3 twistAxis;
ComputeTwistLimitInfo(ref qABTwist, out m_twistAngle, out twistAxis);
if (m_twistAngle > m_twistSpan * m_limitSoftness)
{
m_solveTwistLimit = true;
m_twistLimitRatio = 1f;
if (m_twistAngle < m_twistSpan && m_limitSoftness < 1f - MathUtil.SIMD_EPSILON)
{
m_twistLimitRatio = (m_twistAngle - m_twistSpan * m_limitSoftness) /
(m_twistSpan - m_twistSpan * m_limitSoftness);
}
// twist correction tries to get back to soft limit
m_twistCorrection = m_twistAngle - (m_twistSpan * m_limitSoftness);
m_twistAxis = MathUtil.QuatRotate(qB, -twistAxis);
m_kTwist = 1f /
(ComputeAngularImpulseDenominator(ref m_twistAxis, ref invInertiaWorldA) +
ComputeAngularImpulseDenominator(ref m_twistAxis, ref invInertiaWorldB));
}
if (m_solveSwingLimit)
{
m_twistAxisA = MathUtil.QuatRotate(qA, -twistAxis);
}
}
else
{
m_twistAngle = 0f;
}
}
}
///bilateral constraint between two dynamic objects
///positive distance = separation, negative distance = penetration
public static void ResolveSingleBilateral(RigidBody body1, ref IndexedVector3 pos1,
RigidBody body2, ref IndexedVector3 pos2,
float distance, ref IndexedVector3 normal, ref float impulse, float timeStep)
{
float normalLenSqr = normal.LengthSquared();
Debug.Assert(Math.Abs(normalLenSqr) < 1.1f);
if (normalLenSqr > 1.1f)
{
impulse = 0f;
return;
}
IndexedVector3 rel_pos1 = pos1 - body1.GetCenterOfMassPosition();
IndexedVector3 rel_pos2 = pos2 - body2.GetCenterOfMassPosition();
//this jacobian entry could be re-used for all iterations
IndexedVector3 vel1 = body1.GetVelocityInLocalPoint(ref rel_pos1);
IndexedVector3 vel2 = body2.GetVelocityInLocalPoint(ref rel_pos2);
IndexedVector3 vel = vel1 - vel2;
IndexedBasisMatrix m1 = body1.GetCenterOfMassTransform()._basis.Transpose();
IndexedBasisMatrix m2 = body2.GetCenterOfMassTransform()._basis.Transpose();
JacobianEntry jac = new JacobianEntry(m1, m2, rel_pos1, rel_pos2, normal,
body1.GetInvInertiaDiagLocal(), body1.GetInvMass(),
body2.GetInvInertiaDiagLocal(), body2.GetInvMass());
float jacDiagAB = jac.GetDiagonal();
float jacDiagABInv = 1f / jacDiagAB;
float rel_vel = jac.GetRelativeVelocity(
body1.GetLinearVelocity(),
body1.GetCenterOfMassTransform()._basis.Transpose() * body1.GetAngularVelocity(),
body2.GetLinearVelocity(),
body2.GetCenterOfMassTransform()._basis.Transpose() * body2.GetAngularVelocity());
float a = jacDiagABInv;
rel_vel = normal.Dot(ref vel);
//todo: move this into proper structure
float contactDamping = 0.2f;
if (ONLY_USE_LINEAR_MASS)
{
float massTerm = 1f / (body1.GetInvMass() + body2.GetInvMass());
impulse = -contactDamping * rel_vel * massTerm;
}
else
{
float velocityImpulse = -contactDamping * rel_vel * jacDiagABInv;
impulse = velocityImpulse;
}
}
public static void ZeroCheckVector(ref IndexedVector3 v)
{
#if DEBUG
if (FuzzyZero(v.LengthSquared()))
{
//Debug.Assert(false);
}
#endif
}
