public static bool Distance(ConvexShape shape0, btTransform wtrs0, ConvexShape shape1, btTransform wtrs1, btVector3 guess, ref sResults results) { tShape shape = new MinkowskiDiff(); Initialize(shape0, wtrs0, shape1, wtrs1, ref results, ref shape, false); using(GJK gjk = GJK.CreateFromPool()) { GJK.eStatus gjk_status = gjk.Evaluate(shape, guess); if (gjk_status == GJK.eStatus.Valid) { btVector3 w0 = new btVector3(0, 0, 0); btVector3 w1 = new btVector3(0, 0, 0); for (U i = 0; i < gjk.m_simplex.rank; ++i) { float p = gjk.m_simplex.p[i]; btVector3 temp,temp2,temp3; #region w0 += shape.Support(gjk.m_simplex.c[i].d, 0) * p; shape.Support(ref gjk.m_simplex.c[i].d, 0, out temp); btVector3.Multiply(ref temp, p, out temp2); w0.Add(ref temp2); #endregion #region w1 += shape.Support(-gjk.m_simplex.c[i].d, 1) * p; btVector3.Minus(ref gjk.m_simplex.c[i].d, out temp3); shape.Support(ref temp3, 1, out temp); btVector3.Multiply(ref temp, p, out temp2); w1.Add(ref temp2); #endregion } results.witnesses0 = wtrs0 * w0; results.witnesses1 = wtrs0 * w1; results.normal = w0 - w1; results.distance = results.normal.Length; results.normal /= results.distance > GJK_MIN_DISTANCE ? results.distance : 1; return (true); } else { results.status = gjk_status == GJK.eStatus.Inside ? sResults.eStatus.Penetrating : sResults.eStatus.GJK_Failed; return (false); } } }
public eStatus Evaluate(tShape shapearg, btVector3 guess) { U iterations = 0; float sqdist = 0; float alpha = 0; //btVector3 *lastw=stackalloc btVector3[4]; //btVector3[] lastw = new btVector3[4]; StackPtr<btVector3> lastw = StackPtr<btVector3>.Allocate(4); try { U 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 = eStatus.Valid; m_shape = shapearg; m_distance = 0; /* Initialize simplex */ m_simplices[0].rank = 0; m_ray = guess; float sqrl = m_ray.Length2; appendvertice(m_simplices[0], sqrl > 0 ? -m_ray : new btVector3(1, 0, 0)); 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 { U next = 1 - m_current; sSimplex cs = m_simplices[m_current]; sSimplex ns = m_simplices[next]; /* Check zero */ float rl = m_ray.Length; if (rl < GJK_MIN_DISTANCE) {/* Touching or inside */ m_status = eStatus.Inside; break; } /* Append new vertice in -'v' direction */ appendvertice(cs, -m_ray); btVector3 w = cs.c[cs.rank - 1].w; bool found = false; for (U i = 0; i < 4; ++i) { if ((w - lastw[i]).Length2 < 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 = btVector3.dot(m_ray, w) / rl; alpha = (float)Math.Max(omega, alpha); if (((rl - alpha) - (GJK_ACCURARY * rl)) <= 0) {/* Return old simplex */ removevertice(m_simplices[m_current]); break; } /* Reduce simplex */ //float* weights = stackalloc float[4]; StackPtr<float> weights = StackPtr<float>.Allocate(4); try { U mask = 0; switch (cs.rank) { case 2: sqdist = projectorigin(cs.c[0].w, cs.c[1].w, weights, ref mask); break; case 3: sqdist = projectorigin(cs.c[0].w, cs.c[1].w, cs.c[2].w, weights, ref mask); break; case 4: sqdist = projectorigin(cs.c[0].w, cs.c[1].w, cs.c[2].w, cs.c[3].w, weights, ref mask); break; } if (sqdist >= 0) {/* Valid */ ns.rank = 0; m_ray = new btVector3(0, 0, 0); m_current = next; for (U i = 0, ni = cs.rank; i < ni; ++i) { if ((mask & (1 << (int)i)) != 0) { ns.c[ns.rank] = cs.c[i]; ns.p[ns.rank++] = weights[i]; #region m_ray += cs.c[i].w * weights[i]; { btVector3 temp; btVector3.Multiply(ref cs.c[i].w, weights[i], out temp); m_ray.Add(ref temp); } #endregion } else { m_free[m_nfree++] = cs.c[i]; } } if (mask == 15) m_status = eStatus.Inside; } else {/* Return old simplex */ removevertice(m_simplices[m_current]); break; } m_status = ((++iterations) < GJK_MAX_ITERATIONS) ? m_status : eStatus.Failed; } finally { weights.Dispose(); } } while (m_status == eStatus.Valid); m_simplex = m_simplices[m_current]; switch (m_status) { case eStatus.Valid: m_distance = m_ray.Length; break; case eStatus.Inside: m_distance = 0; break; default: { break; } } return (m_status); } finally { lastw.Dispose(); } }
public static bool Penetration(ConvexShape shape0, btTransform wtrs0, ConvexShape shape1, btTransform wtrs1, btVector3 guess, ref sResults results, bool usemargins) { tShape shape=new MinkowskiDiff(); Initialize(shape0,wtrs0,shape1,wtrs1,ref results,ref shape,usemargins); using (GJK gjk = GJK.CreateFromPool()) { GJK.eStatus gjk_status = gjk.Evaluate(shape, -guess); switch (gjk_status) { case GJK.eStatus.Inside: { using(EPA epa = EPA.CreateFromPool()) { EPA.eStatus epa_status = epa.Evaluate(gjk, -guess); if (epa_status != EPA.eStatus.Failed) { btVector3 w0 = new btVector3(0, 0, 0); for (U i = 0; i < epa.m_result.rank; ++i) { #region w0 += shape.Support(epa.m_result.c[i].d, 0) * epa.m_result.p[i]; { btVector3 temp1, temp2; shape.Support(ref epa.m_result.c[i].d, 0, out temp1); btVector3.Multiply(ref temp1, epa.m_result.p[i], out temp2); w0.Add(ref temp2); } #endregion } results.status = sResults.eStatus.Penetrating; results.witnesses0 = wtrs0 * w0; results.witnesses1 = wtrs0 * (w0 - epa.m_normal * epa.m_depth); results.normal = -epa.m_normal; results.distance = -epa.m_depth; return (true); } else results.status = sResults.eStatus.EPA_Failed; } } break; case GJK.eStatus.Failed: results.status = sResults.eStatus.GJK_Failed; break; default: { break; } } return (false); } }
///calculateTemporalAabb calculates the enclosing aabb for the moving object over interval [0..timeStep) ///result is conservative public void calculateTemporalAabb(btTransform curTrans, btVector3 linvel, btVector3 angvel, float timeStep, out btVector3 temporalAabbMin, out btVector3 temporalAabbMax) { //start with static aabb getAabb(curTrans, out temporalAabbMin, out temporalAabbMax); float temporalAabbMaxx = temporalAabbMax.X; float temporalAabbMaxy = temporalAabbMax.Y; float temporalAabbMaxz = temporalAabbMax.Z; float temporalAabbMinx = temporalAabbMin.X; float temporalAabbMiny = temporalAabbMin.Y; float temporalAabbMinz = temporalAabbMin.Z; // add linear motion btVector3 linMotion;// = linvel * timeStep; btVector3.Multiply(ref linvel, timeStep, out linMotion); ///@todo: simd would have a vector max/min operation, instead of per-element access if (linMotion.X > 0f) temporalAabbMaxx += linMotion.X; else temporalAabbMinx += linMotion.X; if (linMotion.Y > 0f) temporalAabbMaxy += linMotion.Y; else temporalAabbMiny += linMotion.Y; if (linMotion.Z > 0f) temporalAabbMaxz += linMotion.Z; else temporalAabbMinz += linMotion.Z; //add conservative angular motion float angularMotion = angvel.Length * getAngularMotionDisc() * timeStep; btVector3 angularMotion3d = new btVector3(angularMotion, angularMotion, angularMotion); temporalAabbMin = new btVector3(temporalAabbMinx, temporalAabbMiny, temporalAabbMinz); temporalAabbMax = new btVector3(temporalAabbMaxx, temporalAabbMaxy, temporalAabbMaxz); //temporalAabbMin -= angularMotion3d; //temporalAabbMax += angularMotion3d; temporalAabbMin.Subtract(ref angularMotion3d); temporalAabbMax.Add(ref angularMotion3d); }
public void getAabbNonVirtual(btTransform t, out btVector3 aabbMin, out btVector3 aabbMax) { switch (ShapeType) { case BroadphaseNativeTypes.SPHERE_SHAPE_PROXYTYPE: { SphereShape sphereShape = (SphereShape)this; float radius = sphereShape.ImplicitShapeDimensions.X;// * convexShape->getLocalScaling().getX(); float margin = radius + sphereShape.MarginNonVirtual; btVector3 center = t.Origin; btVector3 extent = new btVector3(margin, margin, margin); //aabbMin = center - extent; btVector3.Subtract(ref center, ref extent, out aabbMin); //aabbMax = center + extent; btVector3.Add(ref center, ref extent, out aabbMax); } break; case BroadphaseNativeTypes.CYLINDER_SHAPE_PROXYTYPE: /* fall through */ case BroadphaseNativeTypes.BOX_SHAPE_PROXYTYPE: { BoxShape convexShape = (BoxShape)this; float margin = convexShape.MarginNonVirtual; btVector3 halfExtents = convexShape.ImplicitShapeDimensions; #region halfExtents += new btVector3(margin, margin, margin); { btVector3 temp = new btVector3(margin, margin, margin); halfExtents.Add(ref temp); } #endregion btMatrix3x3 abs_b;// = t.Basis.absolute(); t.Basis.absolute(out abs_b); btVector3 center = t.Origin; btVector3 extent = new btVector3(abs_b.el0.dot(halfExtents), abs_b.el1.dot(halfExtents), abs_b.el2.dot(halfExtents)); //aabbMin = center - extent; btVector3.Subtract(ref center, ref extent, out aabbMin); //aabbMax = center + extent; btVector3.Add(ref center, ref extent, out aabbMax); break; } case BroadphaseNativeTypes.TRIANGLE_SHAPE_PROXYTYPE: throw new NotImplementedException(); #if false { btTriangleShape* triangleShape = (btTriangleShape*)this; float margin = triangleShape->getMarginNonVirtual(); for (int i=0;i<3;i++) { btVector3 vec(btScalar(0.),btScalar(0.),btScalar(0.)); vec[i] = btScalar(1.); btVector3 sv = localGetSupportVertexWithoutMarginNonVirtual(vec*t.getBasis()); btVector3 tmp = t(sv); aabbMax[i] = tmp[i]+margin; vec[i] = btScalar(-1.); tmp = t(localGetSupportVertexWithoutMarginNonVirtual(vec*t.getBasis())); aabbMin[i] = tmp[i]-margin; } } break; #endif case BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE: { CapsuleShape capsuleShape = (CapsuleShape)this; btVector3 halfExtents = new btVector3(capsuleShape.Radius, capsuleShape.Radius, capsuleShape.Radius); int m_upAxis = capsuleShape.UpAxis; halfExtents[m_upAxis] = capsuleShape.Radius + capsuleShape.HalfHeight; #region halfExtents += new btVector3(capsuleShape.MarginNonVirtual, capsuleShape.MarginNonVirtual, capsuleShape.MarginNonVirtual); { btVector3 temp = new btVector3(capsuleShape.MarginNonVirtual, capsuleShape.MarginNonVirtual, capsuleShape.MarginNonVirtual); halfExtents.Add(ref temp); } #endregion btMatrix3x3 abs_b;// = t.Basis.absolute(); t.Basis.absolute(out abs_b); btVector3 center = t.Origin; btVector3 extent = new btVector3(abs_b.el0.dot(halfExtents), abs_b.el1.dot(halfExtents), abs_b.el2.dot(halfExtents)); //aabbMin = center - extent; btVector3.Subtract(ref center, ref extent, out aabbMin); //aabbMax = center + extent; btVector3.Add(ref center, ref extent, out aabbMax); } break; case BroadphaseNativeTypes.CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE: case BroadphaseNativeTypes.CONVEX_HULL_SHAPE_PROXYTYPE: throw new NotImplementedException(); #if false { btPolyhedralConvexAabbCachingShape* convexHullShape = (btPolyhedralConvexAabbCachingShape*)this; btScalar margin = convexHullShape->getMarginNonVirtual(); convexHullShape->getNonvirtualAabb (t, aabbMin, aabbMax, margin); } break; #endif default: this.getAabb(t, out aabbMin, out aabbMax); Debug.Assert(false); break; } // should never reach here Debug.Assert(false); }
public virtual void drawAabb(ref btVector3 from,ref btVector3 to,ref btVector3 color) { btVector3 halfExtents = (to - from) * 0.5f; btVector3 center;// = (to + from) * 0.5f; { btVector3 temp; btVector3.Add(ref to, ref from, out temp); btVector3.Multiply(ref temp, 0.5f, out center); } int i, j; btVector3 edgecoord = new btVector3(1f, 1f, 1f), pa, pb; for (i = 0; i < 4; i++) { for (j = 0; j < 3; j++) { pa = new btVector3(edgecoord.X * halfExtents.X, edgecoord.Y * halfExtents.Y, edgecoord.Z * halfExtents.Z); //pa += center; pa.Add(ref center); int othercoord = j % 3; edgecoord[othercoord] *= -1f; pb = new btVector3(edgecoord.X * halfExtents.X, edgecoord.Y * halfExtents.Y, edgecoord.Z * halfExtents.Z); //pb += center; pb.Add(ref center); drawLine(ref pa,ref pb,ref color); } edgecoord = new btVector3(-1f, -1f, -1f); if (i < 3) edgecoord[i] *= -1f; } }