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