mirror of
https://github.com/TorqueGameEngines/Torque3D.git
synced 2026-02-23 00:23:46 +00:00
CollisionState membervar clarification
This commit is contained in:
Azaezel
parent
e23487b4b1
commit
46bbb88d71
6 changed files with 130 additions and 130 deletions
|
|
@ -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;
|
||||
}
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue