/* =============================================================================== 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); }