|
@@ -46,7 +46,7 @@ S32 num_irregularities = 0;
|
|
|
|
|
|
GjkCollisionState::GjkCollisionState()
|
|
|
{
|
|
|
- a = b = 0;
|
|
|
+ mA = mB = 0;
|
|
|
}
|
|
|
|
|
|
GjkCollisionState::~GjkCollisionState()
|
|
@@ -58,9 +58,9 @@ GjkCollisionState::~GjkCollisionState()
|
|
|
|
|
|
void GjkCollisionState::swap()
|
|
|
{
|
|
|
- Convex* t = a; a = b; b = t;
|
|
|
+ Convex* t = mA; mA = mB; mB = t;
|
|
|
CollisionStateList* l = mLista; mLista = mListb; mListb = l;
|
|
|
- v.neg();
|
|
|
+ mDistvec.neg();
|
|
|
}
|
|
|
|
|
|
|
|
@@ -70,44 +70,44 @@ void GjkCollisionState::compute_det()
|
|
|
{
|
|
|
// Dot new point with current set
|
|
|
for (S32 i = 0, bit = 1; i < 4; ++i, bit <<=1)
|
|
|
- if (bits & bit)
|
|
|
- dp[i][last] = dp[last][i] = mDot(y[i], y[last]);
|
|
|
- dp[last][last] = mDot(y[last], y[last]);
|
|
|
+ if (mBits & bit)
|
|
|
+ mDP[i][mLast] = mDP[mLast][i] = mDot(mY[i], mY[mLast]);
|
|
|
+ mDP[mLast][mLast] = mDot(mY[mLast], mY[mLast]);
|
|
|
|
|
|
// Calulate the determinent
|
|
|
- det[last_bit][last] = 1;
|
|
|
+ mDet[mLast_bit][mLast] = 1;
|
|
|
for (S32 j = 0, sj = 1; j < 4; ++j, sj <<= 1) {
|
|
|
- if (bits & sj) {
|
|
|
- S32 s2 = sj | last_bit;
|
|
|
- det[s2][j] = dp[last][last] - dp[last][j];
|
|
|
- det[s2][last] = dp[j][j] - dp[j][last];
|
|
|
+ if (mBits & sj) {
|
|
|
+ S32 s2 = sj | mLast_bit;
|
|
|
+ mDet[s2][j] = mDP[mLast][mLast] - mDP[mLast][j];
|
|
|
+ mDet[s2][mLast] = mDP[j][j] - mDP[j][mLast];
|
|
|
for (S32 k = 0, sk = 1; k < j; ++k, sk <<= 1) {
|
|
|
- if (bits & sk) {
|
|
|
+ if (mBits & sk) {
|
|
|
S32 s3 = sk | s2;
|
|
|
- det[s3][k] = det[s2][j] * (dp[j][j] - dp[j][k]) +
|
|
|
- det[s2][last] * (dp[last][j] - dp[last][k]);
|
|
|
- det[s3][j] = det[sk | last_bit][k] * (dp[k][k] - dp[k][j]) +
|
|
|
- det[sk | last_bit][last] * (dp[last][k] - dp[last][j]);
|
|
|
- det[s3][last] = det[sk | sj][k] * (dp[k][k] - dp[k][last]) +
|
|
|
- det[sk | sj][j] * (dp[j][k] - dp[j][last]);
|
|
|
+ mDet[s3][k] = mDet[s2][j] * (mDP[j][j] - mDP[j][k]) +
|
|
|
+ mDet[s2][mLast] * (mDP[mLast][j] - mDP[mLast][k]);
|
|
|
+ mDet[s3][j] = mDet[sk | mLast_bit][k] * (mDP[k][k] - mDP[k][j]) +
|
|
|
+ mDet[sk | mLast_bit][mLast] * (mDP[mLast][k] - mDP[mLast][j]);
|
|
|
+ mDet[s3][mLast] = mDet[sk | sj][k] * (mDP[k][k] - mDP[k][mLast]) +
|
|
|
+ mDet[sk | sj][j] * (mDP[j][k] - mDP[j][mLast]);
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
|
|
|
- if (all_bits == 15) {
|
|
|
- det[15][0] = det[14][1] * (dp[1][1] - dp[1][0]) +
|
|
|
- det[14][2] * (dp[2][1] - dp[2][0]) +
|
|
|
- det[14][3] * (dp[3][1] - dp[3][0]);
|
|
|
- det[15][1] = det[13][0] * (dp[0][0] - dp[0][1]) +
|
|
|
- det[13][2] * (dp[2][0] - dp[2][1]) +
|
|
|
- det[13][3] * (dp[3][0] - dp[3][1]);
|
|
|
- det[15][2] = det[11][0] * (dp[0][0] - dp[0][2]) +
|
|
|
- det[11][1] * (dp[1][0] - dp[1][2]) +
|
|
|
- det[11][3] * (dp[3][0] - dp[3][2]);
|
|
|
- det[15][3] = det[7][0] * (dp[0][0] - dp[0][3]) +
|
|
|
- det[7][1] * (dp[1][0] - dp[1][3]) +
|
|
|
- det[7][2] * (dp[2][0] - dp[2][3]);
|
|
|
+ if (mAll_bits == 15) {
|
|
|
+ mDet[15][0] = mDet[14][1] * (mDP[1][1] - mDP[1][0]) +
|
|
|
+ mDet[14][2] * (mDP[2][1] - mDP[2][0]) +
|
|
|
+ mDet[14][3] * (mDP[3][1] - mDP[3][0]);
|
|
|
+ mDet[15][1] = mDet[13][0] * (mDP[0][0] - mDP[0][1]) +
|
|
|
+ mDet[13][2] * (mDP[2][0] - mDP[2][1]) +
|
|
|
+ mDet[13][3] * (mDP[3][0] - mDP[3][1]);
|
|
|
+ mDet[15][2] = mDet[11][0] * (mDP[0][0] - mDP[0][2]) +
|
|
|
+ mDet[11][1] * (mDP[1][0] - mDP[1][2]) +
|
|
|
+ mDet[11][3] * (mDP[3][0] - mDP[3][2]);
|
|
|
+ mDet[15][3] = mDet[7][0] * (mDP[0][0] - mDP[0][3]) +
|
|
|
+ mDet[7][1] * (mDP[1][0] - mDP[1][3]) +
|
|
|
+ mDet[7][2] * (mDP[2][0] - mDP[2][3]);
|
|
|
}
|
|
|
}
|
|
|
|
|
@@ -120,8 +120,8 @@ inline void GjkCollisionState::compute_vector(S32 bits, VectorF& v)
|
|
|
v.set(0, 0, 0);
|
|
|
for (S32 i = 0, bit = 1; i < 4; ++i, bit <<= 1) {
|
|
|
if (bits & bit) {
|
|
|
- sum += det[bits][i];
|
|
|
- v += y[i] * det[bits][i];
|
|
|
+ sum += mDet[bits][i];
|
|
|
+ v += mY[i] * mDet[bits][i];
|
|
|
}
|
|
|
}
|
|
|
v *= 1 / sum;
|
|
@@ -133,13 +133,13 @@ inline void GjkCollisionState::compute_vector(S32 bits, VectorF& v)
|
|
|
inline bool GjkCollisionState::valid(S32 s)
|
|
|
{
|
|
|
for (S32 i = 0, bit = 1; i < 4; ++i, bit <<= 1) {
|
|
|
- if (all_bits & bit) {
|
|
|
+ if (mAll_bits & bit) {
|
|
|
if (s & bit) {
|
|
|
- if (det[s][i] <= 0)
|
|
|
+ if (mDet[s][i] <= 0)
|
|
|
return false;
|
|
|
}
|
|
|
else
|
|
|
- if (det[s | bit][i] > 0)
|
|
|
+ if (mDet[s | bit][i] > 0)
|
|
|
return false;
|
|
|
}
|
|
|
}
|
|
@@ -152,19 +152,19 @@ inline bool GjkCollisionState::valid(S32 s)
|
|
|
inline bool GjkCollisionState::closest(VectorF& v)
|
|
|
{
|
|
|
compute_det();
|
|
|
- for (S32 s = bits; s; --s) {
|
|
|
- if ((s & bits) == s) {
|
|
|
- if (valid(s | last_bit)) {
|
|
|
- bits = s | last_bit;
|
|
|
- if (bits != 15)
|
|
|
- compute_vector(bits, v);
|
|
|
+ for (S32 s = mBits; s; --s) {
|
|
|
+ if ((s & mBits) == s) {
|
|
|
+ if (valid(s | mLast_bit)) {
|
|
|
+ mBits = s | mLast_bit;
|
|
|
+ if (mBits != 15)
|
|
|
+ compute_vector(mBits, v);
|
|
|
return true;
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
- if (valid(last_bit)) {
|
|
|
- bits = last_bit;
|
|
|
- v = y[last];
|
|
|
+ if (valid(mLast_bit)) {
|
|
|
+ mBits = mLast_bit;
|
|
|
+ v = mY[mLast];
|
|
|
return true;
|
|
|
}
|
|
|
return false;
|
|
@@ -176,7 +176,7 @@ inline bool GjkCollisionState::closest(VectorF& v)
|
|
|
inline bool GjkCollisionState::degenerate(const VectorF& w)
|
|
|
{
|
|
|
for (S32 i = 0, bit = 1; i < 4; ++i, bit <<= 1)
|
|
|
- if ((all_bits & bit) && y[i] == w)
|
|
|
+ if ((mAll_bits & bit) && mY[i] == w)
|
|
|
return true;
|
|
|
return false;
|
|
|
}
|
|
@@ -186,11 +186,11 @@ inline bool GjkCollisionState::degenerate(const VectorF& w)
|
|
|
|
|
|
inline void GjkCollisionState::nextBit()
|
|
|
{
|
|
|
- last = 0;
|
|
|
- last_bit = 1;
|
|
|
- while (bits & last_bit) {
|
|
|
- ++last;
|
|
|
- last_bit <<= 1;
|
|
|
+ mLast = 0;
|
|
|
+ mLast_bit = 1;
|
|
|
+ while (mBits & mLast_bit) {
|
|
|
+ ++mLast;
|
|
|
+ mLast_bit <<= 1;
|
|
|
}
|
|
|
}
|
|
|
|
|
@@ -203,11 +203,11 @@ inline void GjkCollisionState::nextBit()
|
|
|
void GjkCollisionState::set(Convex* aa, Convex* bb,
|
|
|
const MatrixF& a2w, const MatrixF& b2w)
|
|
|
{
|
|
|
- a = aa;
|
|
|
- b = bb;
|
|
|
+ mA = aa;
|
|
|
+ mB = bb;
|
|
|
|
|
|
- bits = 0;
|
|
|
- all_bits = 0;
|
|
|
+ mBits = 0;
|
|
|
+ mAll_bits = 0;
|
|
|
reset(a2w,b2w);
|
|
|
|
|
|
// link
|
|
@@ -223,10 +223,10 @@ void GjkCollisionState::set(Convex* aa, Convex* bb,
|
|
|
void GjkCollisionState::reset(const MatrixF& a2w, const MatrixF& b2w)
|
|
|
{
|
|
|
VectorF zero(0,0,0),sa,sb;
|
|
|
- a2w.mulP(a->support(zero),&sa);
|
|
|
- b2w.mulP(b->support(zero),&sb);
|
|
|
- v = sa - sb;
|
|
|
- dist = v.len();
|
|
|
+ a2w.mulP(mA->support(zero),&sa);
|
|
|
+ b2w.mulP(mB->support(zero),&sb);
|
|
|
+ mDistvec = sa - sb;
|
|
|
+ mDist = mDistvec.len();
|
|
|
}
|
|
|
|
|
|
|
|
@@ -237,18 +237,18 @@ void GjkCollisionState::getCollisionInfo(const MatrixF& mat, Collision* info)
|
|
|
AssertFatal(false, "GjkCollisionState::getCollisionInfo() - There remain scaling problems here.");
|
|
|
// This assumes that the shapes do not intersect
|
|
|
Point3F pa,pb;
|
|
|
- if (bits) {
|
|
|
+ if (mBits) {
|
|
|
getClosestPoints(pa,pb);
|
|
|
mat.mulP(pa,&info->point);
|
|
|
- b->getTransform().mulP(pb,&pa);
|
|
|
+ mB->getTransform().mulP(pb,&pa);
|
|
|
info->normal = info->point - pa;
|
|
|
}
|
|
|
else {
|
|
|
- mat.mulP(p[last],&info->point);
|
|
|
- info->normal = v;
|
|
|
+ mat.mulP(mP[mLast],&info->point);
|
|
|
+ info->normal = mDistvec;
|
|
|
}
|
|
|
info->normal.normalize();
|
|
|
- info->object = b->getObject();
|
|
|
+ info->object = mB->getObject();
|
|
|
}
|
|
|
|
|
|
void GjkCollisionState::getClosestPoints(Point3F& p1, Point3F& p2)
|
|
@@ -257,10 +257,10 @@ void GjkCollisionState::getClosestPoints(Point3F& p1, Point3F& p2)
|
|
|
p1.set(0, 0, 0);
|
|
|
p2.set(0, 0, 0);
|
|
|
for (S32 i = 0, bit = 1; i < 4; ++i, bit <<= 1) {
|
|
|
- if (bits & bit) {
|
|
|
- sum += det[bits][i];
|
|
|
- p1 += p[i] * det[bits][i];
|
|
|
- p2 += q[i] * det[bits][i];
|
|
|
+ if (mBits & bit) {
|
|
|
+ sum += mDet[mBits][i];
|
|
|
+ p1 += mP[i] * mDet[mBits][i];
|
|
|
+ p2 += mQ[i] * mDet[mBits][i];
|
|
|
}
|
|
|
}
|
|
|
F32 s = 1 / sum;
|
|
@@ -282,40 +282,40 @@ bool GjkCollisionState::intersect(const MatrixF& a2w, const MatrixF& b2w)
|
|
|
w2b.inverse();
|
|
|
reset(a2w,b2w);
|
|
|
|
|
|
- bits = 0;
|
|
|
- all_bits = 0;
|
|
|
+ mBits = 0;
|
|
|
+ mAll_bits = 0;
|
|
|
|
|
|
do {
|
|
|
nextBit();
|
|
|
|
|
|
VectorF va,sa;
|
|
|
- w2a.mulV(-v,&va);
|
|
|
- p[last] = a->support(va);
|
|
|
- a2w.mulP(p[last],&sa);
|
|
|
+ w2a.mulV(-mDistvec,&va);
|
|
|
+ mP[mLast] = mA->support(va);
|
|
|
+ a2w.mulP(mP[mLast],&sa);
|
|
|
|
|
|
VectorF vb,sb;
|
|
|
- w2b.mulV(v,&vb);
|
|
|
- q[last] = b->support(vb);
|
|
|
- b2w.mulP(q[last],&sb);
|
|
|
+ w2b.mulV(mDistvec,&vb);
|
|
|
+ mQ[mLast] = mB->support(vb);
|
|
|
+ b2w.mulP(mQ[mLast],&sb);
|
|
|
|
|
|
VectorF w = sa - sb;
|
|
|
- if (mDot(v,w) > 0)
|
|
|
+ if (mDot(mDistvec,w) > 0)
|
|
|
return false;
|
|
|
if (degenerate(w)) {
|
|
|
++num_irregularities;
|
|
|
return false;
|
|
|
}
|
|
|
|
|
|
- y[last] = w;
|
|
|
- all_bits = bits | last_bit;
|
|
|
+ mY[mLast] = w;
|
|
|
+ mAll_bits = mBits | mLast_bit;
|
|
|
|
|
|
++num_iterations;
|
|
|
- if (!closest(v) || num_iterations > sIteration) {
|
|
|
+ if (!closest(mDistvec) || num_iterations > sIteration) {
|
|
|
++num_irregularities;
|
|
|
return false;
|
|
|
}
|
|
|
}
|
|
|
- while (bits < 15 && v.lenSquared() > sEpsilon2);
|
|
|
+ while (mBits < 15 && mDistvec.lenSquared() > sEpsilon2);
|
|
|
return true;
|
|
|
}
|
|
|
|
|
@@ -337,51 +337,51 @@ F32 GjkCollisionState::distance(const MatrixF& a2w, const MatrixF& b2w,
|
|
|
}
|
|
|
|
|
|
reset(a2w,b2w);
|
|
|
- bits = 0;
|
|
|
- all_bits = 0;
|
|
|
+ mBits = 0;
|
|
|
+ mAll_bits = 0;
|
|
|
F32 mu = 0;
|
|
|
|
|
|
do {
|
|
|
nextBit();
|
|
|
|
|
|
VectorF va,sa;
|
|
|
- w2a.mulV(-v,&va);
|
|
|
- p[last] = a->support(va);
|
|
|
- a2w.mulP(p[last],&sa);
|
|
|
+ w2a.mulV(-mDistvec,&va);
|
|
|
+ mP[mLast] = mA->support(va);
|
|
|
+ a2w.mulP(mP[mLast],&sa);
|
|
|
|
|
|
VectorF vb,sb;
|
|
|
- w2b.mulV(v,&vb);
|
|
|
- q[last] = b->support(vb);
|
|
|
- b2w.mulP(q[last],&sb);
|
|
|
+ w2b.mulV(mDistvec,&vb);
|
|
|
+ mQ[mLast] = mB->support(vb);
|
|
|
+ b2w.mulP(mQ[mLast],&sb);
|
|
|
|
|
|
VectorF w = sa - sb;
|
|
|
- F32 nm = mDot(v, w) / dist;
|
|
|
+ F32 nm = mDot(mDistvec, w) / mDist;
|
|
|
if (nm > mu)
|
|
|
mu = nm;
|
|
|
if (mu > dontCareDist)
|
|
|
return mu;
|
|
|
- if (mFabs(dist - mu) <= dist * rel_error)
|
|
|
- return dist;
|
|
|
+ if (mFabs(mDist - mu) <= mDist * rel_error)
|
|
|
+ return mDist;
|
|
|
|
|
|
++num_iterations;
|
|
|
if (degenerate(w) || num_iterations > sIteration) {
|
|
|
++num_irregularities;
|
|
|
- return dist;
|
|
|
+ return mDist;
|
|
|
}
|
|
|
|
|
|
- y[last] = w;
|
|
|
- all_bits = bits | last_bit;
|
|
|
+ mY[mLast] = w;
|
|
|
+ mAll_bits = mBits | mLast_bit;
|
|
|
|
|
|
- if (!closest(v)) {
|
|
|
+ if (!closest(mDistvec)) {
|
|
|
++num_irregularities;
|
|
|
- return dist;
|
|
|
+ return mDist;
|
|
|
}
|
|
|
|
|
|
- dist = v.len();
|
|
|
+ mDist = mDistvec.len();
|
|
|
}
|
|
|
- while (bits < 15 && dist > sTolerance) ;
|
|
|
+ while (mBits < 15 && mDist > sTolerance) ;
|
|
|
|
|
|
- if (bits == 15 && mu <= 0)
|
|
|
- dist = 0;
|
|
|
- return dist;
|
|
|
+ if (mBits == 15 && mu <= 0)
|
|
|
+ mDist = 0;
|
|
|
+ return mDist;
|
|
|
}
|