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 }