// packs color floats in the range [0,1] into an integer
static uint PackColor(ref idVec3 color)
{
uint dx = ColorFloatToByte(color.x);
uint dy = ColorFloatToByte(color.y);
uint dz = ColorFloatToByte(color.z);
uint dw = ColorFloatToByte(color.w);
return((dx << 0) | (dy << 8) | (dz << 16) | (dw << 24));
}
Node PointNode(ref idVec3 p, Model model)
{
Node node = model.node;
while (node.planeType != -1)
{
node = (p[node.planeType] > node.planeDist ? node.children[0] : node.children[1]);
Debug.Assert(node != null);
}
return(node);
}
int TransformedPointContents(ref idVec3 p, CmHandle model, ref idVec3 origin, ref idMat3 modelAxis)
{
// subtract origin offset
idVec3 p_l = p - origin;
if (modelAxis.IsRotated())
{
p_l *= modelAxis;
}
return(PointContents(p_l, model));
}
// returns true if any of the trm vertices is inside the brush
bool TestTrmVertsInBrush(TraceWork tw, Brush b)
{
if (b.checkcount == this.checkCount)
{
return(false);
}
b.checkcount = this.checkCount;
if ((b.contents & tw.contents) == 0)
{
return(false);
}
// if the brush bounds don't intersect the trace bounds
if (!b.bounds.IntersectsBounds(tw.bounds))
{
return(false);
}
int numVerts = (tw.pointTrace ? 1 : tw.numVerts);
for (int j = 0; j < numVerts; j++)
{
idVec3 p = tw.vertices[j].p;
// see if the point is inside the brush
int bestPlane = 0;
float bestd = float.NegativeInfinity;
for (int i = 0; i < b.numPlanes; i++)
{
float d = b.planes[i].Distance(p);
if (d >= 0.0f)
{
break;
}
if (d > bestd)
{
bestd = d;
bestPlane = i;
}
}
if (i >= b.numPlanes)
{
tw.trace.fraction = 0.0f;
tw.trace.c.type = ContactType.CONTACT_TRMVERTEX;
tw.trace.c.normal = b.planes[bestPlane].Normal();
tw.trace.c.dist = b.planes[bestPlane].Dist();
tw.trace.c.contents = b.contents;
tw.trace.c.material = b.material;
tw.trace.c.point = p;
tw.trace.c.modelFeature = 0;
tw.trace.c.trmFeature = j;
return(true);
}
}
return(false);
}
int Contents(ref idVec3 start, idTraceModel trm, ref idMat3 trmAxis, int contentMask, CmHandle model, ref idVec3 modelOrigin, ref idMat3 modelAxis)
{
if (model < 0 || model > this.maxModels || model > MAX_SUBMODELS)
{
common.Printf("CollisionModelManager::Contents: invalid model handle\n");
return(0);
}
if (this.models == null || this.models[model] == null)
{
common.Printf("CollisionModelManager::Contents: invalid model\n");
return(0);
}
Trace results;
return(ContentsTrm(out results, start, trm, trmAxis, contentMask, model, modelOrigin, modelAxis));
}
public int Contacts(ContactInfo contacts, int maxContacts, ref idVec3 start, ref idVec6 dir, float depth, idTraceModel trm, ref idMat3 trmAxis, int contentMask, CmHandle model, ref idVec3 origin, ref idMat3 modelAxis)
{
// same as Translation but instead of storing the first collision we store all collisions as contacts
this.getContacts = true;
this.contacts = contacts;
this.maxContacts = maxContacts;
this.numContacts = 0;
idVec3 end = start + dir.SubVec3(0) * depth;
Trace results;
Translation(out results, start, end, trm, trmAxis, contentMask, model, origin, modelAxis);
if (dir.SubVec3(1).LengthSqr() != 0.0f)
{
// FIXME: rotational contacts
}
this.getContacts = false;
this.maxContacts = 0;
return this.numContacts;
}
public int Contacts(ContactInfo contacts, int maxContacts, ref idVec3 start, ref idVec6 dir, float depth, idTraceModel trm, ref idMat3 trmAxis, int contentMask, CmHandle model, ref idVec3 origin, ref idMat3 modelAxis)
{
// same as Translation but instead of storing the first collision we store all collisions as contacts
this.getContacts = true;
this.contacts = contacts;
this.maxContacts = maxContacts;
this.numContacts = 0;
idVec3 end = start + dir.SubVec3(0) * depth;
Trace results;
Translation(out results, start, end, trm, trmAxis, contentMask, model, origin, modelAxis);
if (dir.SubVec3(1).LengthSqr() != 0.0f)
{
// FIXME: rotational contacts
}
this.getContacts = false;
this.maxContacts = 0;
return(this.numContacts);
}
int PointContents(idVec3 p, CmHandle model)
{
int i;
Node node = PointNode(p, this.models[(int)model]);
for (BrushRef bref = node.brushes; bref != null; bref = bref.next)
{
Brush b = bref.b;
// test if the point is within the brush bounds
for (i = 0; i < 3; i++)
{
if (p[i] < b.bounds[0][i])
{
break;
}
if (p[i] > b.bounds[1][i])
{
break;
}
}
if (i < 3)
{
continue;
}
// test if the point is inside the brush
idPlane plane = b.planes;
for (i = 0; i < b.numPlanes; i++, plane++)
{
float d = plane.Distance(p);
if (d >= 0)
{
break;
}
}
if (i >= b.numPlanes)
{
return(b.contents);
}
}
return(0);
}
int TransformedPointContents(ref idVec3 p, CmHandle model, ref idVec3 origin, ref idMat3 modelAxis)
{
// subtract origin offset
idVec3 p_l = p - origin;
if (modelAxis.IsRotated())
p_l *= modelAxis;
return PointContents(p_l, model);
}
int PointContents(idVec3 p, CmHandle model)
{
int i;
Node node = PointNode(p, this.models[(int)model]);
for (BrushRef bref = node.brushes; bref != null; bref = bref.next)
{
Brush b = bref.b;
// test if the point is within the brush bounds
for (i = 0; i < 3; i++)
{
if (p[i] < b.bounds[0][i])
break;
if (p[i] > b.bounds[1][i])
break;
}
if (i < 3)
continue;
// test if the point is inside the brush
idPlane plane = b.planes;
for (i = 0; i < b.numPlanes; i++, plane++)
{
float d = plane.Distance(p);
if (d >= 0)
break;
}
if (i >= b.numPlanes)
return b.contents;
}
return 0;
}
Node PointNode(ref idVec3 p, Model model)
{
Node node = model.node;
while (node.planeType != -1)
{
node = (p[node.planeType] > node.planeDist ? node.children[0] : node.children[1]);
Debug.Assert(node != null);
}
return node;
}
/*
* ================
* idCollisionModelManagerLocal::TraceThroughAxialBSPTree_r
* ================
*/
//#define NO_SPATIAL_SUBDIVISION
void idCollisionModelManagerLocal::TraceThroughAxialBSPTree_r(cm_traceWork_t *tw, cm_node_t *node, float p1f, float p2f, idVec3&p1, idVec3&p2)
{
float t1, t2, offset;
float frac, frac2;
float idist;
idVec3 mid;
int side;
float midf;
if (!node)
{
return;
}
if (tw->quickExit)
{
return; // stop immediately
}
if (tw->trace.fraction <= p1f)
{
return; // already hit something nearer
}
// if we need to test this node for collisions
if (node->polygons || (tw->positionTest && node->brushes))
{
// trace through node with collision data
idCollisionModelManagerLocal::TraceTrmThroughNode(tw, node);
}
// if already stuck in solid
if (tw->positionTest && tw->trace.fraction == 0.0f)
{
return;
}
// if this is a leaf node
if (node->planeType == -1)
{
return;
}
#ifdef NO_SPATIAL_SUBDIVISION
idCollisionModelManagerLocal::TraceThroughAxialBSPTree_r(tw, node->children[0], p1f, p2f, p1, p2);
idCollisionModelManagerLocal::TraceThroughAxialBSPTree_r(tw, node->children[1], p1f, p2f, p1, p2);
return;
#endif
// distance from plane for trace start and end
t1 = p1[node->planeType] - node->planeDist;
t2 = p2[node->planeType] - node->planeDist;
// adjust the plane distance appropriately for mins/maxs
offset = tw->extents[node->planeType];
// see which sides we need to consider
if (t1 >= offset && t2 >= offset)
{
idCollisionModelManagerLocal::TraceThroughAxialBSPTree_r(tw, node->children[0], p1f, p2f, p1, p2);
return;
}
if (t1 < -offset && t2 < -offset)
{
idCollisionModelManagerLocal::TraceThroughAxialBSPTree_r(tw, node->children[1], p1f, p2f, p1, p2);
return;
}
if (t1 < t2)
{
idist = 1.0f / (t1 - t2);
side = 1;
frac2 = (t1 + offset) * idist;
frac = (t1 - offset) * idist;
}
else if (t1 > t2)
{
idist = 1.0f / (t1 - t2);
side = 0;
frac2 = (t1 - offset) * idist;
frac = (t1 + offset) * idist;
}
else
{
side = 0;
frac = 1.0f;
frac2 = 0.0f;
}
// move up to the node
if (frac < 0.0f)
{
frac = 0.0f;
}
else if (frac > 1.0f)
{
frac = 1.0f;
}
midf = p1f + (p2f - p1f) * frac;
mid[0] = p1[0] + frac * (p2[0] - p1[0]);
mid[1] = p1[1] + frac * (p2[1] - p1[1]);
mid[2] = p1[2] + frac * (p2[2] - p1[2]);
idCollisionModelManagerLocal::TraceThroughAxialBSPTree_r(tw, node->children[side], p1f, midf, p1, mid);
// go past the node
if (frac2 < 0.0f)
{
frac2 = 0.0f;
}
else if (frac2 > 1.0f)
{
frac2 = 1.0f;
}
midf = p1f + (p2f - p1f) * frac2;
mid[0] = p1[0] + frac2 * (p2[0] - p1[0]);
mid[1] = p1[1] + frac2 * (p2[1] - p1[1]);
mid[2] = p1[2] + frac2 * (p2[2] - p1[2]);
idCollisionModelManagerLocal::TraceThroughAxialBSPTree_r(tw, node->children[side ^ 1], midf, p2f, mid, p2);
}
/*
===============================================================================
Collision detection for rotational motion
===============================================================================
*/
// epsilon for round-off errors in epsilon calculations
#define CM_PL_RANGE_EPSILON 1e-4f
// if the collision point is this close to the rotation axis it is not considered a collision
#define ROTATION_AXIS_EPSILON (CM_CLIP_EPSILON*0.25f)
/*
================
CM_RotatePoint
rotates a point about an arbitrary axis using the tangent of half the rotation angle
================
*/
void CM_RotatePoint( idVec3 &point, const idVec3 &origin, const idVec3 &axis, const float tanHalfAngle ) {
static void UnpackColor(uint color, ref idVec3 unpackedColor)
{
unpackedColor.Set(((color >> 0) & 255) * (1.0f / 255.0f),
((color >> 8) & 255) * (1.0f / 255.0f),
((color >> 16) & 255) * (1.0f / 255.0f));
}
/*
================
idCollisionModelManagerLocal::TraceThroughAxialBSPTree_r
================
*/
//#define NO_SPATIAL_SUBDIVISION
void idCollisionModelManagerLocal::TraceThroughAxialBSPTree_r( cm_traceWork_t *tw, cm_node_t *node, float p1f, float p2f, idVec3 &p1, idVec3 &p2) {
float t1, t2, offset;
float frac, frac2;
float idist;
idVec3 mid;
int side;
float midf;
if ( !node ) {
return;
}
if ( tw->quickExit ) {
return; // stop immediately
}
if ( tw->trace.fraction <= p1f ) {
return; // already hit something nearer
}
// if we need to test this node for collisions
if ( node->polygons || (tw->positionTest && node->brushes) ) {
// trace through node with collision data
idCollisionModelManagerLocal::TraceTrmThroughNode( tw, node );
}
// if already stuck in solid
if ( tw->positionTest && tw->trace.fraction == 0.0f ) {
return;
}
// if this is a leaf node
if ( node->planeType == -1 ) {
return;
}
#ifdef NO_SPATIAL_SUBDIVISION
idCollisionModelManagerLocal::TraceThroughAxialBSPTree_r( tw, node->children[0], p1f, p2f, p1, p2 );
idCollisionModelManagerLocal::TraceThroughAxialBSPTree_r( tw, node->children[1], p1f, p2f, p1, p2 );
return;
#endif
// distance from plane for trace start and end
t1 = p1[node->planeType] - node->planeDist;
t2 = p2[node->planeType] - node->planeDist;
// adjust the plane distance appropriately for mins/maxs
offset = tw->extents[node->planeType];
// see which sides we need to consider
if ( t1 >= offset && t2 >= offset ) {
idCollisionModelManagerLocal::TraceThroughAxialBSPTree_r( tw, node->children[0], p1f, p2f, p1, p2 );
return;
}
if ( t1 < -offset && t2 < -offset ) {
idCollisionModelManagerLocal::TraceThroughAxialBSPTree_r( tw, node->children[1], p1f, p2f, p1, p2 );
return;
}
if ( t1 < t2 ) {
idist = 1.0f / (t1-t2);
side = 1;
frac2 = (t1 + offset) * idist;
frac = (t1 - offset) * idist;
} else if (t1 > t2) {
idist = 1.0f / (t1-t2);
side = 0;
frac2 = (t1 - offset) * idist;
frac = (t1 + offset) * idist;
} else {
side = 0;
frac = 1.0f;
frac2 = 0.0f;
}
// move up to the node
if ( frac < 0.0f ) {
frac = 0.0f;
}
else if ( frac > 1.0f ) {
frac = 1.0f;
}
midf = p1f + (p2f - p1f)*frac;
mid[0] = p1[0] + frac*(p2[0] - p1[0]);
mid[1] = p1[1] + frac*(p2[1] - p1[1]);
mid[2] = p1[2] + frac*(p2[2] - p1[2]);
idCollisionModelManagerLocal::TraceThroughAxialBSPTree_r( tw, node->children[side], p1f, midf, p1, mid );
// go past the node
if ( frac2 < 0.0f ) {
frac2 = 0.0f;
}
else if ( frac2 > 1.0f ) {
frac2 = 1.0f;
}
midf = p1f + (p2f - p1f)*frac2;
mid[0] = p1[0] + frac2*(p2[0] - p1[0]);
mid[1] = p1[1] + frac2*(p2[1] - p1[1]);
mid[2] = p1[2] + frac2*(p2[2] - p1[2]);
idCollisionModelManagerLocal::TraceThroughAxialBSPTree_r( tw, node->children[side^1], midf, p2f, mid, p2 );
}
int ContentsTrm(Trace results, ref idVec3 start, idTraceModel trm, ref idMat3 trmAxis, int contentMask, CmHandle model, ref idVec3 modelOrigin, ref idMat3 modelAxis)
{
// fast point case
if (!trm || (trm.bounds[1][0] - trm.bounds[0][0] <= 0.0f &&
trm.bounds[1][1] - trm.bounds[0][1] <= 0.0f &&
trm.bounds[1][2] - trm.bounds[0][2] <= 0.0f))
{
results.c.contents = TransformedPointContents(start, model, modelOrigin, modelAxis);
results.fraction = (results.c.contents == 0);
results.endpos = start;
results.endAxis = trmAxis;
return results.c.contents;
}
this.checkCount++;
TraceWork tw = new TraceWork();
tw.trace.fraction = 1.0f;
tw.trace.c.contents = 0;
tw.trace.c.type = ContactType.CONTACT_NONE;
tw.contents = contentMask;
tw.isConvex = true;
tw.rotation = false;
tw.positionTest = true;
tw.pointTrace = false;
tw.quickExit = false;
tw.numContacts = 0;
tw.model = this.models[(int)model];
tw.start = start - modelOrigin;
tw.end = tw.start;
bool model_rotated = modelAxis.IsRotated();
if (model_rotated)
invModelAxis = modelAxis.Transpose();
// setup trm structure
SetupTrm(ref tw, trm);
bool trm_rotated = trmAxis.IsRotated();
// calculate vertex positions
if (trm_rotated)
for (int i = 0; i < tw.numVerts; i++)
// rotate trm around the start position
tw.vertices[i].p *= trmAxis;
for (int i = 0; i < tw.numVerts; i++)
// set trm at start position
tw.vertices[i].p += tw.start;
if (model_rotated)
for (int i = 0; i < tw.numVerts; i++)
// rotate trm around model instead of rotating the model
tw.vertices[i].p *= invModelAxis;
// add offset to start point
if (trm_rotated)
{
idVec3 dir = trm->offset * trmAxis;
tw.start += dir;
tw.end += dir;
}
else
{
tw.start += trm->offset;
tw.end += trm->offset;
}
if (model_rotated)
{
// rotate trace instead of model
tw.start *= invModelAxis;
tw.end *= invModelAxis;
}
// setup trm vertices
tw.size.Clear();
for (int i = 0; i < tw.numVerts; i++)
// get axial trm size after rotations
tw.size.AddPoint(tw.vertices[i].p - tw.start);
// setup trm edges
for (int i = 1; i <= tw.numEdges; i++)
{
// edge start, end and pluecker coordinate
tw.edges[i].start = tw.vertices[tw.edges[i].vertexNum[0]].p;
tw.edges[i].end = tw.vertices[tw.edges[i].vertexNum[1]].p;
tw.edges[i].pl.FromLine(tw.edges[i].start, tw.edges[i].end);
}
// setup trm polygons
if (trm_rotated & model_rotated)
{
idMat3 tmpAxis = trmAxis * invModelAxis;
for (int i = 0; i < tw.numPolys; i++)
tw.polys[i].plane *= tmpAxis;
}
else if (trm_rotated)
{
for (int i = 0; i < tw.numPolys; i++)
tw.polys[i].plane *= trmAxis;
}
else if (model_rotated)
{
for (int i = 0; i < tw.numPolys; i++)
tw.polys[i].plane *= invModelAxis;
}
for (int i = 0; i < tw.numPolys; i++)
tw.polys[i].plane.FitThroughPoint(tw.edges[abs(tw.polys[i].edges[0])].start);
// bounds for full trace, a little bit larger for epsilons
for (int i = 0; i < 3; i++)
{
if (tw.start[i] < tw.end[i])
{
tw.bounds[0][i] = tw.start[i] + tw.size[0][i] - CM_BOX_EPSILON;
tw.bounds[1][i] = tw.end[i] + tw.size[1][i] + CM_BOX_EPSILON;
}
else
{
tw.bounds[0][i] = tw.end[i] + tw.size[0][i] - CM_BOX_EPSILON;
tw.bounds[1][i] = tw.start[i] + tw.size[1][i] + CM_BOX_EPSILON;
}
if (idMath.Fabs(tw.size[0][i]) > idMath.Fabs(tw.size[1][i]))
tw.extents[i] = idMath.Fabs(tw.size[0][i]) + CM_BOX_EPSILON;
else
tw.extents[i] = idMath.Fabs(tw.size[1][i]) + CM_BOX_EPSILON;
}
// trace through the model
TraceThroughModel(ref tw);
results = tw.trace;
results.fraction = (results.c.contents == 0);
results.endpos = start;
results.endAxis = trmAxis;
return results.c.contents;
}
int Contents(ref idVec3 start, idTraceModel trm, ref idMat3 trmAxis, int contentMask, CmHandle model, ref idVec3 modelOrigin, ref idMat3 modelAxis)
{
if (model < 0 || model > this.maxModels || model > MAX_SUBMODELS)
{
common.Printf("CollisionModelManager::Contents: invalid model handle\n");
return 0;
}
if (this.models == null || this.models[model] == null)
{
common.Printf("CollisionModelManager::Contents: invalid model\n");
return 0;
}
Trace results;
return ContentsTrm(out results, start, trm, trmAxis, contentMask, model, modelOrigin, modelAxis);
}
int ContentsTrm(Trace results, ref idVec3 start, idTraceModel trm, ref idMat3 trmAxis, int contentMask, CmHandle model, ref idVec3 modelOrigin, ref idMat3 modelAxis)
{
// fast point case
if (!trm || (trm.bounds[1][0] - trm.bounds[0][0] <= 0.0f &&
trm.bounds[1][1] - trm.bounds[0][1] <= 0.0f &&
trm.bounds[1][2] - trm.bounds[0][2] <= 0.0f))
{
results.c.contents = TransformedPointContents(start, model, modelOrigin, modelAxis);
results.fraction = (results.c.contents == 0);
results.endpos = start;
results.endAxis = trmAxis;
return(results.c.contents);
}
this.checkCount++;
TraceWork tw = new TraceWork();
tw.trace.fraction = 1.0f;
tw.trace.c.contents = 0;
tw.trace.c.type = ContactType.CONTACT_NONE;
tw.contents = contentMask;
tw.isConvex = true;
tw.rotation = false;
tw.positionTest = true;
tw.pointTrace = false;
tw.quickExit = false;
tw.numContacts = 0;
tw.model = this.models[(int)model];
tw.start = start - modelOrigin;
tw.end = tw.start;
bool model_rotated = modelAxis.IsRotated();
if (model_rotated)
{
invModelAxis = modelAxis.Transpose();
}
// setup trm structure
SetupTrm(ref tw, trm);
bool trm_rotated = trmAxis.IsRotated();
// calculate vertex positions
if (trm_rotated)
{
for (int i = 0; i < tw.numVerts; i++)
{
// rotate trm around the start position
tw.vertices[i].p *= trmAxis;
}
}
for (int i = 0; i < tw.numVerts; i++)
{
// set trm at start position
tw.vertices[i].p += tw.start;
}
if (model_rotated)
{
for (int i = 0; i < tw.numVerts; i++)
{
// rotate trm around model instead of rotating the model
tw.vertices[i].p *= invModelAxis;
}
}
// add offset to start point
if (trm_rotated)
{
idVec3 dir = trm->offset * trmAxis;
tw.start += dir;
tw.end += dir;
}
else
{
tw.start += trm->offset;
tw.end += trm->offset;
}
if (model_rotated)
{
// rotate trace instead of model
tw.start *= invModelAxis;
tw.end *= invModelAxis;
}
// setup trm vertices
tw.size.Clear();
for (int i = 0; i < tw.numVerts; i++)
{
// get axial trm size after rotations
tw.size.AddPoint(tw.vertices[i].p - tw.start);
}
// setup trm edges
for (int i = 1; i <= tw.numEdges; i++)
{
// edge start, end and pluecker coordinate
tw.edges[i].start = tw.vertices[tw.edges[i].vertexNum[0]].p;
tw.edges[i].end = tw.vertices[tw.edges[i].vertexNum[1]].p;
tw.edges[i].pl.FromLine(tw.edges[i].start, tw.edges[i].end);
}
// setup trm polygons
if (trm_rotated & model_rotated)
{
idMat3 tmpAxis = trmAxis * invModelAxis;
for (int i = 0; i < tw.numPolys; i++)
{
tw.polys[i].plane *= tmpAxis;
}
}
else if (trm_rotated)
{
for (int i = 0; i < tw.numPolys; i++)
{
tw.polys[i].plane *= trmAxis;
}
}
else if (model_rotated)
{
for (int i = 0; i < tw.numPolys; i++)
{
tw.polys[i].plane *= invModelAxis;
}
}
for (int i = 0; i < tw.numPolys; i++)
{
tw.polys[i].plane.FitThroughPoint(tw.edges[abs(tw.polys[i].edges[0])].start);
}
// bounds for full trace, a little bit larger for epsilons
for (int i = 0; i < 3; i++)
{
if (tw.start[i] < tw.end[i])
{
tw.bounds[0][i] = tw.start[i] + tw.size[0][i] - CM_BOX_EPSILON;
tw.bounds[1][i] = tw.end[i] + tw.size[1][i] + CM_BOX_EPSILON;
}
else
{
tw.bounds[0][i] = tw.end[i] + tw.size[0][i] - CM_BOX_EPSILON;
tw.bounds[1][i] = tw.start[i] + tw.size[1][i] + CM_BOX_EPSILON;
}
if (idMath.Fabs(tw.size[0][i]) > idMath.Fabs(tw.size[1][i]))
{
tw.extents[i] = idMath.Fabs(tw.size[0][i]) + CM_BOX_EPSILON;
}
else
{
tw.extents[i] = idMath.Fabs(tw.size[1][i]) + CM_BOX_EPSILON;
}
}
// trace through the model
TraceThroughModel(ref tw);
results = tw.trace;
results.fraction = (results.c.contents == 0);
results.endpos = start;
results.endAxis = trmAxis;
return(results.c.contents);
}
/*
===============================================================================
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);
}
