/*
===============================================================================
Collision detection for translational motion
===============================================================================
*/
/*
================
idCollisionModelManagerLocal::TranslateEdgeThroughEdge
calculates fraction of the translation completed at which the edges collide
================
*/
ID_INLINE int idCollisionModelManagerLocal::TranslateEdgeThroughEdge( idVec3 &cross, idPluecker &l1, idPluecker &l2, float *fraction ) {
float d, t;
/*
a = start of line
b = end of line
dir = movement direction
l1 = pluecker coordinate for line
l2 = pluecker coordinate for edge we might collide with
a+dir = start of line after movement
b+dir = end of line after movement
t = scale factor
solve pluecker inner product for t of line (a+t*dir : b+t*dir) and line l2
v[0] = (a[0]+t*dir[0]) * (b[1]+t*dir[1]) - (b[0]+t*dir[0]) * (a[1]+t*dir[1]);
v[1] = (a[0]+t*dir[0]) * (b[2]+t*dir[2]) - (b[0]+t*dir[0]) * (a[2]+t*dir[2]);
v[2] = (a[0]+t*dir[0]) - (b[0]+t*dir[0]);
v[3] = (a[1]+t*dir[1]) * (b[2]+t*dir[2]) - (b[1]+t*dir[1]) * (a[2]+t*dir[2]);
v[4] = (a[2]+t*dir[2]) - (b[2]+t*dir[2]);
v[5] = (b[1]+t*dir[1]) - (a[1]+t*dir[1]);
l2[0] * v[4] + l2[1] * v[5] + l2[2] * v[3] + l2[4] * v[0] + l2[5] * v[1] + l2[3] * v[2] = 0;
solve t
v[0] = (a[0]+t*dir[0]) * (b[1]+t*dir[1]) - (b[0]+t*dir[0]) * (a[1]+t*dir[1]);
v[0] = (a[0]*b[1]) + a[0]*t*dir[1] + b[1]*t*dir[0] + (t*t*dir[0]*dir[1]) -
((b[0]*a[1]) + b[0]*t*dir[1] + a[1]*t*dir[0] + (t*t*dir[0]*dir[1]));
v[0] = a[0]*b[1] + a[0]*t*dir[1] + b[1]*t*dir[0] - b[0]*a[1] - b[0]*t*dir[1] - a[1]*t*dir[0];
v[1] = (a[0]+t*dir[0]) * (b[2]+t*dir[2]) - (b[0]+t*dir[0]) * (a[2]+t*dir[2]);
v[1] = (a[0]*b[2]) + a[0]*t*dir[2] + b[2]*t*dir[0] + (t*t*dir[0]*dir[2]) -
((b[0]*a[2]) + b[0]*t*dir[2] + a[2]*t*dir[0] + (t*t*dir[0]*dir[2]));
v[1] = a[0]*b[2] + a[0]*t*dir[2] + b[2]*t*dir[0] - b[0]*a[2] - b[0]*t*dir[2] - a[2]*t*dir[0];
v[2] = (a[0]+t*dir[0]) - (b[0]+t*dir[0]);
v[2] = a[0] - b[0];
v[3] = (a[1]+t*dir[1]) * (b[2]+t*dir[2]) - (b[1]+t*dir[1]) * (a[2]+t*dir[2]);
v[3] = (a[1]*b[2]) + a[1]*t*dir[2] + b[2]*t*dir[1] + (t*t*dir[1]*dir[2]) -
((b[1]*a[2]) + b[1]*t*dir[2] + a[2]*t*dir[1] + (t*t*dir[1]*dir[2]));
v[3] = a[1]*b[2] + a[1]*t*dir[2] + b[2]*t*dir[1] - b[1]*a[2] - b[1]*t*dir[2] - a[2]*t*dir[1];
v[4] = (a[2]+t*dir[2]) - (b[2]+t*dir[2]);
v[4] = a[2] - b[2];
v[5] = (b[1]+t*dir[1]) - (a[1]+t*dir[1]);
v[5] = b[1] - a[1];
v[0] = a[0]*b[1] + a[0]*t*dir[1] + b[1]*t*dir[0] - b[0]*a[1] - b[0]*t*dir[1] - a[1]*t*dir[0];
v[1] = a[0]*b[2] + a[0]*t*dir[2] + b[2]*t*dir[0] - b[0]*a[2] - b[0]*t*dir[2] - a[2]*t*dir[0];
v[2] = a[0] - b[0];
v[3] = a[1]*b[2] + a[1]*t*dir[2] + b[2]*t*dir[1] - b[1]*a[2] - b[1]*t*dir[2] - a[2]*t*dir[1];
v[4] = a[2] - b[2];
v[5] = b[1] - a[1];
v[0] = (a[0]*dir[1] + b[1]*dir[0] - b[0]*dir[1] - a[1]*dir[0]) * t + a[0]*b[1] - b[0]*a[1];
v[1] = (a[0]*dir[2] + b[2]*dir[0] - b[0]*dir[2] - a[2]*dir[0]) * t + a[0]*b[2] - b[0]*a[2];
v[2] = a[0] - b[0];
v[3] = (a[1]*dir[2] + b[2]*dir[1] - b[1]*dir[2] - a[2]*dir[1]) * t + a[1]*b[2] - b[1]*a[2];
v[4] = a[2] - b[2];
v[5] = b[1] - a[1];
l2[4] * (a[0]*dir[1] + b[1]*dir[0] - b[0]*dir[1] - a[1]*dir[0]) * t + l2[4] * (a[0]*b[1] - b[0]*a[1])
+ l2[5] * (a[0]*dir[2] + b[2]*dir[0] - b[0]*dir[2] - a[2]*dir[0]) * t + l2[5] * (a[0]*b[2] - b[0]*a[2])
+ l2[3] * (a[0] - b[0])
+ l2[2] * (a[1]*dir[2] + b[2]*dir[1] - b[1]*dir[2] - a[2]*dir[1]) * t + l2[2] * (a[1]*b[2] - b[1]*a[2])
+ l2[0] * (a[2] - b[2])
+ l2[1] * (b[1] - a[1]) = 0
t = (- l2[4] * (a[0]*b[1] - b[0]*a[1]) -
l2[5] * (a[0]*b[2] - b[0]*a[2]) -
l2[3] * (a[0] - b[0]) -
l2[2] * (a[1]*b[2] - b[1]*a[2]) -
l2[0] * (a[2] - b[2]) -
l2[1] * (b[1] - a[1])) /
(l2[4] * (a[0]*dir[1] + b[1]*dir[0] - b[0]*dir[1] - a[1]*dir[0]) +
l2[5] * (a[0]*dir[2] + b[2]*dir[0] - b[0]*dir[2] - a[2]*dir[0]) +
l2[2] * (a[1]*dir[2] + b[2]*dir[1] - b[1]*dir[2] - a[2]*dir[1]));
d = l2[4] * (a[0]*dir[1] + b[1]*dir[0] - b[0]*dir[1] - a[1]*dir[0]) +
l2[5] * (a[0]*dir[2] + b[2]*dir[0] - b[0]*dir[2] - a[2]*dir[0]) +
l2[2] * (a[1]*dir[2] + b[2]*dir[1] - b[1]*dir[2] - a[2]*dir[1]);
t = - ( l2[4] * (a[0]*b[1] - b[0]*a[1]) +
l2[5] * (a[0]*b[2] - b[0]*a[2]) +
l2[3] * (a[0] - b[0]) +
l2[2] * (a[1]*b[2] - b[1]*a[2]) +
l2[0] * (a[2] - b[2]) +
l2[1] * (b[1] - a[1]));
t /= d;
MrE pats Pluecker on the head.. good monkey
edgeDir = a - b;
d = l2[4] * (edgeDir[0]*dir[1] - edgeDir[1]*dir[0]) +
l2[5] * (edgeDir[0]*dir[2] - edgeDir[2]*dir[0]) +
l2[2] * (edgeDir[1]*dir[2] - edgeDir[2]*dir[1]);
*/
d = l2[4] * cross[0] + l2[5] * cross[1] + l2[2] * cross[2];
if ( d == 0.0f ) {
*fraction = 1.0f;
// no collision ever
return false;
}
t = -l1.PermutedInnerProduct( l2 );
// if the lines cross each other to begin with
if ( t == 0.0f ) {
*fraction = 0.0f;
return true;
}
// fraction of movement at the time the lines cross each other
*fraction = t / d;
return true;
}
/*
* ===============================================================================
*
* Collision detection for translational motion
*
* ===============================================================================
*/
/*
* ================
* idCollisionModelManagerLocal::TranslateEdgeThroughEdge
*
* calculates fraction of the translation completed at which the edges collide
* ================
*/
ID_INLINE int idCollisionModelManagerLocal::TranslateEdgeThroughEdge(idVec3&cross, idPluecker&l1, idPluecker&l2, float *fraction)
{
float d, t;
/*
*
* a = start of line
* b = end of line
* dir = movement direction
* l1 = pluecker coordinate for line
* l2 = pluecker coordinate for edge we might collide with
* a+dir = start of line after movement
* b+dir = end of line after movement
* t = scale factor
* solve pluecker inner product for t of line (a+t*dir : b+t*dir) and line l2
*
* v[0] = (a[0]+t*dir[0]) * (b[1]+t*dir[1]) - (b[0]+t*dir[0]) * (a[1]+t*dir[1]);
* v[1] = (a[0]+t*dir[0]) * (b[2]+t*dir[2]) - (b[0]+t*dir[0]) * (a[2]+t*dir[2]);
* v[2] = (a[0]+t*dir[0]) - (b[0]+t*dir[0]);
* v[3] = (a[1]+t*dir[1]) * (b[2]+t*dir[2]) - (b[1]+t*dir[1]) * (a[2]+t*dir[2]);
* v[4] = (a[2]+t*dir[2]) - (b[2]+t*dir[2]);
* v[5] = (b[1]+t*dir[1]) - (a[1]+t*dir[1]);
*
* l2[0] * v[4] + l2[1] * v[5] + l2[2] * v[3] + l2[4] * v[0] + l2[5] * v[1] + l2[3] * v[2] = 0;
*
* solve t
*
* v[0] = (a[0]+t*dir[0]) * (b[1]+t*dir[1]) - (b[0]+t*dir[0]) * (a[1]+t*dir[1]);
* v[0] = (a[0]*b[1]) + a[0]*t*dir[1] + b[1]*t*dir[0] + (t*t*dir[0]*dir[1]) -
* ((b[0]*a[1]) + b[0]*t*dir[1] + a[1]*t*dir[0] + (t*t*dir[0]*dir[1]));
* v[0] = a[0]*b[1] + a[0]*t*dir[1] + b[1]*t*dir[0] - b[0]*a[1] - b[0]*t*dir[1] - a[1]*t*dir[0];
*
* v[1] = (a[0]+t*dir[0]) * (b[2]+t*dir[2]) - (b[0]+t*dir[0]) * (a[2]+t*dir[2]);
* v[1] = (a[0]*b[2]) + a[0]*t*dir[2] + b[2]*t*dir[0] + (t*t*dir[0]*dir[2]) -
* ((b[0]*a[2]) + b[0]*t*dir[2] + a[2]*t*dir[0] + (t*t*dir[0]*dir[2]));
* v[1] = a[0]*b[2] + a[0]*t*dir[2] + b[2]*t*dir[0] - b[0]*a[2] - b[0]*t*dir[2] - a[2]*t*dir[0];
*
* v[2] = (a[0]+t*dir[0]) - (b[0]+t*dir[0]);
* v[2] = a[0] - b[0];
*
* v[3] = (a[1]+t*dir[1]) * (b[2]+t*dir[2]) - (b[1]+t*dir[1]) * (a[2]+t*dir[2]);
* v[3] = (a[1]*b[2]) + a[1]*t*dir[2] + b[2]*t*dir[1] + (t*t*dir[1]*dir[2]) -
* ((b[1]*a[2]) + b[1]*t*dir[2] + a[2]*t*dir[1] + (t*t*dir[1]*dir[2]));
* v[3] = a[1]*b[2] + a[1]*t*dir[2] + b[2]*t*dir[1] - b[1]*a[2] - b[1]*t*dir[2] - a[2]*t*dir[1];
*
* v[4] = (a[2]+t*dir[2]) - (b[2]+t*dir[2]);
* v[4] = a[2] - b[2];
*
* v[5] = (b[1]+t*dir[1]) - (a[1]+t*dir[1]);
* v[5] = b[1] - a[1];
*
*
* v[0] = a[0]*b[1] + a[0]*t*dir[1] + b[1]*t*dir[0] - b[0]*a[1] - b[0]*t*dir[1] - a[1]*t*dir[0];
* v[1] = a[0]*b[2] + a[0]*t*dir[2] + b[2]*t*dir[0] - b[0]*a[2] - b[0]*t*dir[2] - a[2]*t*dir[0];
* v[2] = a[0] - b[0];
* v[3] = a[1]*b[2] + a[1]*t*dir[2] + b[2]*t*dir[1] - b[1]*a[2] - b[1]*t*dir[2] - a[2]*t*dir[1];
* v[4] = a[2] - b[2];
* v[5] = b[1] - a[1];
*
* v[0] = (a[0]*dir[1] + b[1]*dir[0] - b[0]*dir[1] - a[1]*dir[0]) * t + a[0]*b[1] - b[0]*a[1];
* v[1] = (a[0]*dir[2] + b[2]*dir[0] - b[0]*dir[2] - a[2]*dir[0]) * t + a[0]*b[2] - b[0]*a[2];
* v[2] = a[0] - b[0];
* v[3] = (a[1]*dir[2] + b[2]*dir[1] - b[1]*dir[2] - a[2]*dir[1]) * t + a[1]*b[2] - b[1]*a[2];
* v[4] = a[2] - b[2];
* v[5] = b[1] - a[1];
*
* l2[4] * (a[0]*dir[1] + b[1]*dir[0] - b[0]*dir[1] - a[1]*dir[0]) * t + l2[4] * (a[0]*b[1] - b[0]*a[1])
+ l2[5] * (a[0]*dir[2] + b[2]*dir[0] - b[0]*dir[2] - a[2]*dir[0]) * t + l2[5] * (a[0]*b[2] - b[0]*a[2])
+ l2[3] * (a[0] - b[0])
+ l2[2] * (a[1]*dir[2] + b[2]*dir[1] - b[1]*dir[2] - a[2]*dir[1]) * t + l2[2] * (a[1]*b[2] - b[1]*a[2])
+ l2[0] * (a[2] - b[2])
+ l2[1] * (b[1] - a[1]) = 0
+
+ t = (- l2[4] * (a[0]*b[1] - b[0]*a[1]) -
+ l2[5] * (a[0]*b[2] - b[0]*a[2]) -
+ l2[3] * (a[0] - b[0]) -
+ l2[2] * (a[1]*b[2] - b[1]*a[2]) -
+ l2[0] * (a[2] - b[2]) -
+ l2[1] * (b[1] - a[1])) /
+ (l2[4] * (a[0]*dir[1] + b[1]*dir[0] - b[0]*dir[1] - a[1]*dir[0]) +
+ l2[5] * (a[0]*dir[2] + b[2]*dir[0] - b[0]*dir[2] - a[2]*dir[0]) +
+ l2[2] * (a[1]*dir[2] + b[2]*dir[1] - b[1]*dir[2] - a[2]*dir[1]));
+
+ d = l2[4] * (a[0]*dir[1] + b[1]*dir[0] - b[0]*dir[1] - a[1]*dir[0]) +
+ l2[5] * (a[0]*dir[2] + b[2]*dir[0] - b[0]*dir[2] - a[2]*dir[0]) +
+ l2[2] * (a[1]*dir[2] + b[2]*dir[1] - b[1]*dir[2] - a[2]*dir[1]);
+
+ t = - ( l2[4] * (a[0]*b[1] - b[0]*a[1]) +
+ l2[5] * (a[0]*b[2] - b[0]*a[2]) +
+ l2[3] * (a[0] - b[0]) +
+ l2[2] * (a[1]*b[2] - b[1]*a[2]) +
+ l2[0] * (a[2] - b[2]) +
+ l2[1] * (b[1] - a[1]));
+ t /= d;
+
+ MrE pats Pluecker on the head.. good monkey
+
+ edgeDir = a - b;
+ d = l2[4] * (edgeDir[0]*dir[1] - edgeDir[1]*dir[0]) +
+ l2[5] * (edgeDir[0]*dir[2] - edgeDir[2]*dir[0]) +
+ l2[2] * (edgeDir[1]*dir[2] - edgeDir[2]*dir[1]);
*/
d = l2[4] * cross[0] + l2[5] * cross[1] + l2[2] * cross[2];
if (d == 0.0f)
{
*fraction = 1.0f;
// no collision ever
return(false);
}
t = -l1.PermutedInnerProduct(l2);
// if the lines cross each other to begin with
if (t == 0.0f)
{
*fraction = 0.0f;
return(true);
}
// fraction of movement at the time the lines cross each other
*fraction = t / d;
return(true);
}
