12345678910111213141516171819202122232425262728293031323334353637383940414243444546474849505152535455565758596061626364656667686970717273747576777879808182838485868788899091929394959697989910010110210310410510610710810911011111211311411511611711811912012112212312412512612712812913013113213313413513613713813914014114214314414514614714814915015115215315415515615715815916016116216316416516616716816917017117217317417517617717817918018118218318418518618718818919019119219319419519619719819920020120220320420520620720820921021121221321421521621721821922022122222322422522622722822923023123223323423523623723823924024124224324424524624724824925025125225325425525625725825926026126226326426526626726826927027127227327427527627727827928028128228328428528628728828929029129229329429529629729829930030130230330430530630730830931031131231331431531631731831932032132232332432532632732832933033133233333433533633733833934034134234334434534634734834935035135235335435535635735835936036136236336436536636736836937037137237337437537637737837938038138238338438538638738838939039139239339439539639739839940040140240340440540640740840941041141241341441541641741841942042142242342442542642742842943043143243343443543643743843944044144244344444544644744844945045145245345445545645745845946046146246346446546646746846947047147247347447547647747847948048148248348448548648748848949049149249349449549649749849950050150250350450550650750850951051151251351451551651751851952052152252352452552652752852953053153253353453553653753853954054154254354454554654754854955055155255355455555655755855956056156256356456556656756856957057157257357457557657757857958058158258358458558658758858959059159259359459559659759859960060160260360460560660760860961061161261361461561661761861962062162262362462562662762862963063163263363463563663763863964064164264364464564664764864965065165265365465565665765865966066166266366466566666766866967067167267367467567667767867968068168268368468568668768868969069169269369469569669769869970070170270370470570670770870971071171271371471571671771871972072172272372472572672772872973073173273373473573673773873974074174274374474574674774874975075175275375475575675775875976076176276376476576676776876977077177277377477577677777877978078178278378478578678778878979079179279379479579679779879980080180280380480580680780880981081181281381481581681781881982082182282382482582682782882983083183283383483583683783883984084184284384484584684784884985085185285385485585685785885986086186286386486586686786886987087187287387487587687787887988088188288388488588688788888989089189289389489589689789889990090190290390490590690790890991091191291391491591691791891992092192292392492592692792892993093193293393493593693793893994094194294394494594694794894995095195295395495595695795895996096196296396496596696796896997097197297397497597697797897998098198298398498598698798898999099199299399499599699799899910001001100210031004100510061007100810091010101110121013101410151016101710181019102010211022102310241025102610271028102910301031103210331034103510361037103810391040104110421043104410451046104710481049105010511052105310541055105610571058105910601061106210631064106510661067106810691070107110721073107410751076107710781079108010811082108310841085108610871088108910901091109210931094109510961097109810991100110111021103110411051106110711081109111011111112111311141115111611171118111911201121 |
- /*
- ===========================================================================
- Doom 3 GPL Source Code
- Copyright (C) 1999-2011 id Software LLC, a ZeniMax Media company.
- This file is part of the Doom 3 GPL Source Code (?Doom 3 Source Code?).
- Doom 3 Source Code is free software: you can redistribute it and/or modify
- it under the terms of the GNU General Public License as published by
- the Free Software Foundation, either version 3 of the License, or
- (at your option) any later version.
- Doom 3 Source Code is distributed in the hope that it will be useful,
- but WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- GNU General Public License for more details.
- You should have received a copy of the GNU General Public License
- along with Doom 3 Source Code. If not, see <http://www.gnu.org/licenses/>.
- In addition, the Doom 3 Source Code is also subject to certain additional terms. You should have received a copy of these additional terms immediately following the terms and conditions of the GNU General Public License which accompanied the Doom 3 Source Code. If not, please request a copy in writing from id Software at the address below.
- If you have questions concerning this license or the applicable additional terms, you may contact in writing id Software LLC, c/o ZeniMax Media Inc., Suite 120, Rockville, Maryland 20850 USA.
- ===========================================================================
- */
- /*
- ===============================================================================
- Trace model vs. polygonal model collision detection.
- ===============================================================================
- */
- #include "../idlib/precompiled.h"
- #pragma hdrstop
- #include "CollisionModel_local.h"
- /*
- ===============================================================================
- 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;
- }
- /*
- ================
- CM_AddContact
- ================
- */
- ID_INLINE void CM_AddContact( cm_traceWork_t *tw ) {
- if ( tw->numContacts >= tw->maxContacts ) {
- return;
- }
- // copy contact information from trace_t
- tw->contacts[tw->numContacts] = tw->trace.c;
- tw->numContacts++;
- // set fraction back to 1 to find all other contacts
- tw->trace.fraction = 1.0f;
- }
- /*
- ================
- CM_SetVertexSidedness
- stores for the given model vertex at which side of one of the trm edges it passes
- ================
- */
- ID_INLINE void CM_SetVertexSidedness( cm_vertex_t *v, const idPluecker &vpl, const idPluecker &epl, const int bitNum ) {
- if ( !(v->sideSet & (1<<bitNum)) ) {
- float fl;
- fl = vpl.PermutedInnerProduct( epl );
- v->side = (v->side & ~(1<<bitNum)) | (FLOATSIGNBITSET(fl) << bitNum);
- v->sideSet |= (1 << bitNum);
- }
- }
- /*
- ================
- CM_SetEdgeSidedness
- stores for the given model edge at which side one of the trm vertices
- ================
- */
- ID_INLINE void CM_SetEdgeSidedness( cm_edge_t *edge, const idPluecker &vpl, const idPluecker &epl, const int bitNum ) {
- if ( !(edge->sideSet & (1<<bitNum)) ) {
- float fl;
- fl = vpl.PermutedInnerProduct( epl );
- edge->side = (edge->side & ~(1<<bitNum)) | (FLOATSIGNBITSET(fl) << bitNum);
- edge->sideSet |= (1 << bitNum);
- }
- }
- /*
- ================
- idCollisionModelManagerLocal::TranslateTrmEdgeThroughPolygon
- ================
- */
- void idCollisionModelManagerLocal::TranslateTrmEdgeThroughPolygon( cm_traceWork_t *tw, cm_polygon_t *poly, cm_trmEdge_t *trmEdge ) {
- int i, edgeNum;
- float f1, f2, dist, d1, d2;
- idVec3 start, end, normal;
- cm_edge_t *edge;
- cm_vertex_t *v1, *v2;
- idPluecker *pl, epsPl;
- // check edges for a collision
- for ( i = 0; i < poly->numEdges; i++) {
- edgeNum = poly->edges[i];
- edge = tw->model->edges + abs(edgeNum);
- // if this edge is already checked
- if ( edge->checkcount == idCollisionModelManagerLocal::checkCount ) {
- continue;
- }
- // can never collide with internal edges
- if ( edge->internal ) {
- continue;
- }
- pl = &tw->polygonEdgePlueckerCache[i];
- // get the sides at which the trm edge vertices pass the polygon edge
- CM_SetEdgeSidedness( edge, *pl, tw->vertices[trmEdge->vertexNum[0]].pl, trmEdge->vertexNum[0] );
- CM_SetEdgeSidedness( edge, *pl, tw->vertices[trmEdge->vertexNum[1]].pl, trmEdge->vertexNum[1] );
- // if the trm edge start and end vertex do not pass the polygon edge at different sides
- if ( !(((edge->side >> trmEdge->vertexNum[0]) ^ (edge->side >> trmEdge->vertexNum[1])) & 1) ) {
- continue;
- }
- // get the sides at which the polygon edge vertices pass the trm edge
- v1 = tw->model->vertices + edge->vertexNum[INTSIGNBITSET(edgeNum)];
- CM_SetVertexSidedness( v1, tw->polygonVertexPlueckerCache[i], trmEdge->pl, trmEdge->bitNum );
- v2 = tw->model->vertices + edge->vertexNum[INTSIGNBITNOTSET(edgeNum)];
- CM_SetVertexSidedness( v2, tw->polygonVertexPlueckerCache[i+1], trmEdge->pl, trmEdge->bitNum );
- // if the polygon edge start and end vertex do not pass the trm edge at different sides
- if ( !((v1->side ^ v2->side) & (1<<trmEdge->bitNum)) ) {
- continue;
- }
- // if there is no possible collision between the trm edge and the polygon edge
- if ( !idCollisionModelManagerLocal::TranslateEdgeThroughEdge( trmEdge->cross, trmEdge->pl, *pl, &f1 ) ) {
- continue;
- }
- // if moving away from edge
- if ( f1 < 0.0f ) {
- continue;
- }
- // pluecker coordinate for epsilon expanded edge
- epsPl.FromLine( tw->model->vertices[edge->vertexNum[0]].p + edge->normal * CM_CLIP_EPSILON,
- tw->model->vertices[edge->vertexNum[1]].p + edge->normal * CM_CLIP_EPSILON );
- // calculate collision fraction with epsilon expanded edge
- if ( !idCollisionModelManagerLocal::TranslateEdgeThroughEdge( trmEdge->cross, trmEdge->pl, epsPl, &f2 ) ) {
- continue;
- }
- // if no collision with epsilon edge or moving away from edge
- if ( f2 > 1.0f || f1 < f2 ) {
- continue;
- }
- if ( f2 < 0.0f ) {
- f2 = 0.0f;
- }
- if ( f2 < tw->trace.fraction ) {
- tw->trace.fraction = f2;
- // create plane with normal vector orthogonal to both the polygon edge and the trm edge
- start = tw->model->vertices[edge->vertexNum[0]].p;
- end = tw->model->vertices[edge->vertexNum[1]].p;
- tw->trace.c.normal = ( end - start ).Cross( trmEdge->end - trmEdge->start );
- // FIXME: do this normalize when we know the first collision
- tw->trace.c.normal.Normalize();
- tw->trace.c.dist = tw->trace.c.normal * start;
- // make sure the collision plane faces the trace model
- if ( tw->trace.c.normal * trmEdge->start - tw->trace.c.dist < 0.0f ) {
- tw->trace.c.normal = -tw->trace.c.normal;
- tw->trace.c.dist = -tw->trace.c.dist;
- }
- tw->trace.c.contents = poly->contents;
- tw->trace.c.material = poly->material;
- tw->trace.c.type = CONTACT_EDGE;
- tw->trace.c.modelFeature = edgeNum;
- tw->trace.c.trmFeature = trmEdge - tw->edges;
- // calculate collision point
- normal[0] = trmEdge->cross[2];
- normal[1] = -trmEdge->cross[1];
- normal[2] = trmEdge->cross[0];
- dist = normal * trmEdge->start;
- d1 = normal * start - dist;
- d2 = normal * end - dist;
- f1 = d1 / ( d1 - d2 );
- //assert( f1 >= 0.0f && f1 <= 1.0f );
- tw->trace.c.point = start + f1 * ( end - start );
- // if retrieving contacts
- if ( tw->getContacts ) {
- CM_AddContact( tw );
- }
- }
- }
- }
- /*
- ================
- CM_TranslationPlaneFraction
- ================
- */
- #if 0
- float CM_TranslationPlaneFraction( idPlane &plane, idVec3 &start, idVec3 &end ) {
- float d1, d2;
- d2 = plane.Distance( end );
- // if the end point is closer to the plane than an epsilon we still take it for a collision
- if ( d2 >= CM_CLIP_EPSILON ) {
- return 1.0f;
- }
- d1 = plane.Distance( start );
- // if completely behind the polygon
- if ( d1 <= 0.0f ) {
- return 1.0f;
- }
- // leaves polygon
- if ( d1 <= d2 ) {
- return 1.0f;
- }
- return (d1-CM_CLIP_EPSILON) / (d1-d2);
- }
- #else
- float CM_TranslationPlaneFraction( idPlane &plane, idVec3 &start, idVec3 &end ) {
- float d1, d2, d2eps;
- d2 = plane.Distance( end );
- // if the end point is closer to the plane than an epsilon we still take it for a collision
- // if ( d2 >= CM_CLIP_EPSILON ) {
- d2eps = d2 - CM_CLIP_EPSILON;
- if ( FLOATSIGNBITNOTSET(d2eps) ) {
- return 1.0f;
- }
- d1 = plane.Distance( start );
- // if completely behind the polygon
- if ( FLOATSIGNBITSET(d1) ) {
- return 1.0f;
- }
- // if going towards the front of the plane and
- // the start and end point are not at equal distance from the plane
- // if ( d1 > d2 )
- d2 = d1 - d2;
- if ( d2 <= 0.0f ) {
- return 1.0f;
- }
- return (d1-CM_CLIP_EPSILON) / d2;
- }
- #endif
- /*
- ================
- idCollisionModelManagerLocal::TranslateTrmVertexThroughPolygon
- ================
- */
- void idCollisionModelManagerLocal::TranslateTrmVertexThroughPolygon( cm_traceWork_t *tw, cm_polygon_t *poly, cm_trmVertex_t *v, int bitNum ) {
- int i, edgeNum;
- float f;
- cm_edge_t *edge;
- f = CM_TranslationPlaneFraction( poly->plane, v->p, v->endp );
- if ( f < tw->trace.fraction ) {
- for ( i = 0; i < poly->numEdges; i++ ) {
- edgeNum = poly->edges[i];
- edge = tw->model->edges + abs(edgeNum);
- CM_SetEdgeSidedness( edge, tw->polygonEdgePlueckerCache[i], v->pl, bitNum );
- if ( INTSIGNBITSET(edgeNum) ^ ((edge->side >> bitNum) & 1) ) {
- return;
- }
- }
- if ( f < 0.0f ) {
- f = 0.0f;
- }
- tw->trace.fraction = f;
- // collision plane is the polygon plane
- tw->trace.c.normal = poly->plane.Normal();
- tw->trace.c.dist = poly->plane.Dist();
- tw->trace.c.contents = poly->contents;
- tw->trace.c.material = poly->material;
- tw->trace.c.type = CONTACT_TRMVERTEX;
- tw->trace.c.modelFeature = *reinterpret_cast<int *>(&poly);
- tw->trace.c.trmFeature = v - tw->vertices;
- tw->trace.c.point = v->p + tw->trace.fraction * ( v->endp - v->p );
- // if retrieving contacts
- if ( tw->getContacts ) {
- CM_AddContact( tw );
- // no need to store the trm vertex more than once as a contact
- v->used = false;
- }
- }
- }
- /*
- ================
- idCollisionModelManagerLocal::TranslatePointThroughPolygon
- ================
- */
- void idCollisionModelManagerLocal::TranslatePointThroughPolygon( cm_traceWork_t *tw, cm_polygon_t *poly, cm_trmVertex_t *v ) {
- int i, edgeNum;
- float f;
- cm_edge_t *edge;
- idPluecker pl;
- f = CM_TranslationPlaneFraction( poly->plane, v->p, v->endp );
- if ( f < tw->trace.fraction ) {
- for ( i = 0; i < poly->numEdges; i++ ) {
- edgeNum = poly->edges[i];
- edge = tw->model->edges + abs(edgeNum);
- // if we didn't yet calculate the sidedness for this edge
- if ( edge->checkcount != idCollisionModelManagerLocal::checkCount ) {
- float fl;
- edge->checkcount = idCollisionModelManagerLocal::checkCount;
- pl.FromLine(tw->model->vertices[edge->vertexNum[0]].p, tw->model->vertices[edge->vertexNum[1]].p);
- fl = v->pl.PermutedInnerProduct( pl );
- edge->side = FLOATSIGNBITSET(fl);
- }
- // if the point passes the edge at the wrong side
- //if ( (edgeNum > 0) == edge->side ) {
- if ( INTSIGNBITSET(edgeNum) ^ edge->side ) {
- return;
- }
- }
- if ( f < 0.0f ) {
- f = 0.0f;
- }
- tw->trace.fraction = f;
- // collision plane is the polygon plane
- tw->trace.c.normal = poly->plane.Normal();
- tw->trace.c.dist = poly->plane.Dist();
- tw->trace.c.contents = poly->contents;
- tw->trace.c.material = poly->material;
- tw->trace.c.type = CONTACT_TRMVERTEX;
- tw->trace.c.modelFeature = *reinterpret_cast<int *>(&poly);
- tw->trace.c.trmFeature = v - tw->vertices;
- tw->trace.c.point = v->p + tw->trace.fraction * ( v->endp - v->p );
- // if retrieving contacts
- if ( tw->getContacts ) {
- CM_AddContact( tw );
- // no need to store the trm vertex more than once as a contact
- v->used = false;
- }
- }
- }
- /*
- ================
- idCollisionModelManagerLocal::TranslateVertexThroughTrmPolygon
- ================
- */
- void idCollisionModelManagerLocal::TranslateVertexThroughTrmPolygon( cm_traceWork_t *tw, cm_trmPolygon_t *trmpoly, cm_polygon_t *poly, cm_vertex_t *v, idVec3 &endp, idPluecker &pl ) {
- int i, edgeNum;
- float f;
- cm_trmEdge_t *edge;
- f = CM_TranslationPlaneFraction( trmpoly->plane, v->p, endp );
- if ( f < tw->trace.fraction ) {
- for ( i = 0; i < trmpoly->numEdges; i++ ) {
- edgeNum = trmpoly->edges[i];
- edge = tw->edges + abs(edgeNum);
- CM_SetVertexSidedness( v, pl, edge->pl, edge->bitNum );
- if ( INTSIGNBITSET(edgeNum) ^ ((v->side >> edge->bitNum) & 1) ) {
- return;
- }
- }
- if ( f < 0.0f ) {
- f = 0.0f;
- }
- tw->trace.fraction = f;
- // collision plane is the inverse trm polygon plane
- tw->trace.c.normal = -trmpoly->plane.Normal();
- tw->trace.c.dist = -trmpoly->plane.Dist();
- tw->trace.c.contents = poly->contents;
- tw->trace.c.material = poly->material;
- tw->trace.c.type = CONTACT_MODELVERTEX;
- tw->trace.c.modelFeature = v - tw->model->vertices;
- tw->trace.c.trmFeature = trmpoly - tw->polys;
- tw->trace.c.point = v->p + tw->trace.fraction * ( endp - v->p );
- // if retrieving contacts
- if ( tw->getContacts ) {
- CM_AddContact( tw );
- }
- }
- }
- /*
- ================
- idCollisionModelManagerLocal::TranslateTrmThroughPolygon
- returns true if the polygon blocks the complete translation
- ================
- */
- bool idCollisionModelManagerLocal::TranslateTrmThroughPolygon( cm_traceWork_t *tw, cm_polygon_t *p ) {
- int i, j, k, edgeNum;
- float fraction, d;
- idVec3 endp;
- idPluecker *pl;
- cm_trmVertex_t *bv;
- cm_trmEdge_t *be;
- cm_trmPolygon_t *bp;
- cm_vertex_t *v;
- cm_edge_t *e;
- // if already checked this polygon
- if ( p->checkcount == idCollisionModelManagerLocal::checkCount ) {
- return false;
- }
- p->checkcount = idCollisionModelManagerLocal::checkCount;
- // if this polygon does not have the right contents behind it
- if ( !(p->contents & tw->contents) ) {
- return false;
- }
- // if the the trace bounds do not intersect the polygon bounds
- if ( !tw->bounds.IntersectsBounds( p->bounds ) ) {
- return false;
- }
- // only collide with the polygon if approaching at the front
- if ( ( p->plane.Normal() * tw->dir ) > 0.0f ) {
- return false;
- }
- // if the polygon is too far from the first heart plane
- d = p->bounds.PlaneDistance( tw->heartPlane1 );
- if ( idMath::Fabs(d) > tw->maxDistFromHeartPlane1 ) {
- return false;
- }
- // if the polygon is too far from the second heart plane
- d = p->bounds.PlaneDistance( tw->heartPlane2 );
- if ( idMath::Fabs(d) > tw->maxDistFromHeartPlane2 ) {
- return false;
- }
- fraction = tw->trace.fraction;
- // fast point trace
- if ( tw->pointTrace ) {
- idCollisionModelManagerLocal::TranslatePointThroughPolygon( tw, p, &tw->vertices[0] );
- }
- else {
- // trace bounds should cross polygon plane
- switch ( tw->bounds.PlaneSide( p->plane ) ) {
- case PLANESIDE_CROSS:
- break;
- case PLANESIDE_FRONT:
- if ( tw->model->isConvex ) {
- tw->quickExit = true;
- return true;
- }
- default:
- return false;
- }
- // calculate pluecker coordinates for the polygon edges and polygon vertices
- for ( i = 0; i < p->numEdges; i++ ) {
- edgeNum = p->edges[i];
- e = tw->model->edges + abs(edgeNum);
- // reset sidedness cache if this is the first time we encounter this edge during this trace
- if ( e->checkcount != idCollisionModelManagerLocal::checkCount ) {
- e->sideSet = 0;
- }
- // pluecker coordinate for edge
- tw->polygonEdgePlueckerCache[i].FromLine( tw->model->vertices[e->vertexNum[0]].p,
- tw->model->vertices[e->vertexNum[1]].p );
- v = &tw->model->vertices[e->vertexNum[INTSIGNBITSET(edgeNum)]];
- // reset sidedness cache if this is the first time we encounter this vertex during this trace
- if ( v->checkcount != idCollisionModelManagerLocal::checkCount ) {
- v->sideSet = 0;
- }
- // pluecker coordinate for vertex movement vector
- tw->polygonVertexPlueckerCache[i].FromRay( v->p, -tw->dir );
- }
- // copy first to last so we can easily cycle through for the edges
- tw->polygonVertexPlueckerCache[p->numEdges] = tw->polygonVertexPlueckerCache[0];
- // trace trm vertices through polygon
- for ( i = 0; i < tw->numVerts; i++ ) {
- bv = tw->vertices + i;
- if ( bv->used ) {
- idCollisionModelManagerLocal::TranslateTrmVertexThroughPolygon( tw, p, bv, i );
- }
- }
- // trace trm edges through polygon
- for ( i = 1; i <= tw->numEdges; i++ ) {
- be = tw->edges + i;
- if ( be->used ) {
- idCollisionModelManagerLocal::TranslateTrmEdgeThroughPolygon( tw, p, be);
- }
- }
- // trace all polygon vertices through the trm
- for ( i = 0; i < p->numEdges; i++ ) {
- edgeNum = p->edges[i];
- e = tw->model->edges + abs(edgeNum);
- if ( e->checkcount == idCollisionModelManagerLocal::checkCount ) {
- continue;
- }
- // set edge check count
- e->checkcount = idCollisionModelManagerLocal::checkCount;
- // can never collide with internal edges
- if ( e->internal ) {
- continue;
- }
- // got to check both vertices because we skip internal edges
- for ( k = 0; k < 2; k++ ) {
- v = tw->model->vertices + e->vertexNum[k ^ INTSIGNBITSET(edgeNum)];
- // if this vertex is already checked
- if ( v->checkcount == idCollisionModelManagerLocal::checkCount ) {
- continue;
- }
- // set vertex check count
- v->checkcount = idCollisionModelManagerLocal::checkCount;
- // if the vertex is outside the trace bounds
- if ( !tw->bounds.ContainsPoint( v->p ) ) {
- continue;
- }
- // vertex end point after movement
- endp = v->p - tw->dir;
- // pluecker coordinate for vertex movement vector
- pl = &tw->polygonVertexPlueckerCache[i+k];
- for ( j = 0; j < tw->numPolys; j++ ) {
- bp = tw->polys + j;
- if ( bp->used ) {
- idCollisionModelManagerLocal::TranslateVertexThroughTrmPolygon( tw, bp, p, v, endp, *pl );
- }
- }
- }
- }
- }
- // if there was a collision with this polygon and we are not retrieving contacts
- if ( tw->trace.fraction < fraction && !tw->getContacts ) {
- fraction = tw->trace.fraction;
- endp = tw->start + fraction * tw->dir;
- // decrease bounds
- for ( i = 0; i < 3; i++ ) {
- if ( tw->start[i] < endp[i] ) {
- tw->bounds[0][i] = tw->start[i] + tw->size[0][i] - CM_BOX_EPSILON;
- tw->bounds[1][i] = endp[i] + tw->size[1][i] + CM_BOX_EPSILON;
- }
- else {
- tw->bounds[0][i] = endp[i] + tw->size[0][i] - CM_BOX_EPSILON;
- tw->bounds[1][i] = tw->start[i] + tw->size[1][i] + CM_BOX_EPSILON;
- }
- }
- }
- return ( tw->trace.fraction == 0.0f );
- }
- /*
- ================
- idCollisionModelManagerLocal::SetupTrm
- ================
- */
- void idCollisionModelManagerLocal::SetupTrm( cm_traceWork_t *tw, const idTraceModel *trm ) {
- int i, j;
- // vertices
- tw->numVerts = trm->numVerts;
- for ( i = 0; i < trm->numVerts; i++ ) {
- tw->vertices[i].p = trm->verts[i];
- tw->vertices[i].used = false;
- }
- // edges
- tw->numEdges = trm->numEdges;
- for ( i = 1; i <= trm->numEdges; i++ ) {
- tw->edges[i].vertexNum[0] = trm->edges[i].v[0];
- tw->edges[i].vertexNum[1] = trm->edges[i].v[1];
- tw->edges[i].used = false;
- }
- // polygons
- tw->numPolys = trm->numPolys;
- for ( i = 0; i < trm->numPolys; i++ ) {
- tw->polys[i].numEdges = trm->polys[i].numEdges;
- for ( j = 0; j < trm->polys[i].numEdges; j++ ) {
- tw->polys[i].edges[j] = trm->polys[i].edges[j];
- }
- tw->polys[i].plane.SetNormal( trm->polys[i].normal );
- tw->polys[i].used = false;
- }
- // is the trace model convex or not
- tw->isConvex = trm->isConvex;
- }
- /*
- ================
- idCollisionModelManagerLocal::SetupTranslationHeartPlanes
- ================
- */
- void idCollisionModelManagerLocal::SetupTranslationHeartPlanes( cm_traceWork_t *tw ) {
- idVec3 dir, normal1, normal2;
- // calculate trace heart planes
- dir = tw->dir;
- dir.Normalize();
- dir.NormalVectors( normal1, normal2 );
- tw->heartPlane1.SetNormal( normal1 );
- tw->heartPlane1.FitThroughPoint( tw->start );
- tw->heartPlane2.SetNormal( normal2 );
- tw->heartPlane2.FitThroughPoint( tw->start );
- }
- /*
- ================
- idCollisionModelManagerLocal::Translation
- ================
- */
- #ifdef _DEBUG
- static int entered = 0;
- #endif
- void idCollisionModelManagerLocal::Translation( trace_t *results, const idVec3 &start, const idVec3 &end,
- const idTraceModel *trm, const idMat3 &trmAxis, int contentMask,
- cmHandle_t model, const idVec3 &modelOrigin, const idMat3 &modelAxis ) {
- int i, j;
- float dist;
- bool model_rotated, trm_rotated;
- idVec3 dir1, dir2, dir;
- idMat3 invModelAxis, tmpAxis;
- cm_trmPolygon_t *poly;
- cm_trmEdge_t *edge;
- cm_trmVertex_t *vert;
- ALIGN16( static cm_traceWork_t tw );
- assert( ((byte *)&start) < ((byte *)results) || ((byte *)&start) >= (((byte *)results) + sizeof( trace_t )) );
- assert( ((byte *)&end) < ((byte *)results) || ((byte *)&end) >= (((byte *)results) + sizeof( trace_t )) );
- assert( ((byte *)&trmAxis) < ((byte *)results) || ((byte *)&trmAxis) >= (((byte *)results) + sizeof( trace_t )) );
- memset( results, 0, sizeof( *results ) );
- if ( model < 0 || model > MAX_SUBMODELS || model > idCollisionModelManagerLocal::maxModels ) {
- common->Printf("idCollisionModelManagerLocal::Translation: invalid model handle\n");
- return;
- }
- if ( !idCollisionModelManagerLocal::models[model] ) {
- common->Printf("idCollisionModelManagerLocal::Translation: invalid model\n");
- return;
- }
- // if case special position test
- if ( start[0] == end[0] && start[1] == end[1] && start[2] == end[2] ) {
- idCollisionModelManagerLocal::ContentsTrm( results, start, trm, trmAxis, contentMask, model, modelOrigin, modelAxis );
- return;
- }
- #ifdef _DEBUG
- bool startsolid = false;
- // test whether or not stuck to begin with
- if ( cm_debugCollision.GetBool() ) {
- if ( !entered && !idCollisionModelManagerLocal::getContacts ) {
- entered = 1;
- // if already messed up to begin with
- if ( idCollisionModelManagerLocal::Contents( start, trm, trmAxis, -1, model, modelOrigin, modelAxis ) & contentMask ) {
- startsolid = true;
- }
- entered = 0;
- }
- }
- #endif
- idCollisionModelManagerLocal::checkCount++;
- tw.trace.fraction = 1.0f;
- tw.trace.c.contents = 0;
- tw.trace.c.type = CONTACT_NONE;
- tw.contents = contentMask;
- tw.isConvex = true;
- tw.rotation = false;
- tw.positionTest = false;
- tw.quickExit = false;
- tw.getContacts = idCollisionModelManagerLocal::getContacts;
- tw.contacts = idCollisionModelManagerLocal::contacts;
- tw.maxContacts = idCollisionModelManagerLocal::maxContacts;
- tw.numContacts = 0;
- tw.model = idCollisionModelManagerLocal::models[model];
- tw.start = start - modelOrigin;
- tw.end = end - modelOrigin;
- tw.dir = end - start;
- model_rotated = modelAxis.IsRotated();
- if ( model_rotated ) {
- invModelAxis = modelAxis.Transpose();
- }
- // if optimized point trace
- 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 ) ) {
- if ( model_rotated ) {
- // rotate trace instead of model
- tw.start *= invModelAxis;
- tw.end *= invModelAxis;
- tw.dir *= invModelAxis;
- }
- // trace bounds
- for ( i = 0; i < 3; i++ ) {
- if ( tw.start[i] < tw.end[i] ) {
- tw.bounds[0][i] = tw.start[i] - CM_BOX_EPSILON;
- tw.bounds[1][i] = tw.end[i] + CM_BOX_EPSILON;
- }
- else {
- tw.bounds[0][i] = tw.end[i] - CM_BOX_EPSILON;
- tw.bounds[1][i] = tw.start[i] + CM_BOX_EPSILON;
- }
- }
- tw.extents[0] = tw.extents[1] = tw.extents[2] = CM_BOX_EPSILON;
- tw.size.Zero();
- // setup trace heart planes
- idCollisionModelManagerLocal::SetupTranslationHeartPlanes( &tw );
- tw.maxDistFromHeartPlane1 = CM_BOX_EPSILON;
- tw.maxDistFromHeartPlane2 = CM_BOX_EPSILON;
- // collision with single point
- tw.numVerts = 1;
- tw.vertices[0].p = tw.start;
- tw.vertices[0].endp = tw.vertices[0].p + tw.dir;
- tw.vertices[0].pl.FromRay( tw.vertices[0].p, tw.dir );
- tw.numEdges = tw.numPolys = 0;
- tw.pointTrace = true;
- // trace through the model
- idCollisionModelManagerLocal::TraceThroughModel( &tw );
- // store results
- *results = tw.trace;
- results->endpos = start + results->fraction * (end - start);
- results->endAxis = mat3_identity;
- if ( results->fraction < 1.0f ) {
- // rotate trace plane normal if there was a collision with a rotated model
- if ( model_rotated ) {
- results->c.normal *= modelAxis;
- results->c.point *= modelAxis;
- }
- results->c.point += modelOrigin;
- results->c.dist += modelOrigin * results->c.normal;
- }
- idCollisionModelManagerLocal::numContacts = tw.numContacts;
- return;
- }
- // the trace fraction is too inaccurate to describe translations over huge distances
- if ( tw.dir.LengthSqr() > Square( CM_MAX_TRACE_DIST ) ) {
- results->fraction = 0.0f;
- results->endpos = start;
- results->endAxis = trmAxis;
- results->c.normal = vec3_origin;
- results->c.material = NULL;
- results->c.point = start;
- if ( session->rw ) {
- session->rw->DebugArrow( colorRed, start, end, 1 );
- }
- common->Printf( "idCollisionModelManagerLocal::Translation: huge translation\n" );
- return;
- }
- tw.pointTrace = false;
- tw.size.Clear();
- // setup trm structure
- idCollisionModelManagerLocal::SetupTrm( &tw, trm );
- trm_rotated = trmAxis.IsRotated();
- // calculate vertex positions
- if ( trm_rotated ) {
- for ( i = 0; i < tw.numVerts; i++ ) {
- // rotate trm around the start position
- tw.vertices[i].p *= trmAxis;
- }
- }
- for ( i = 0; i < tw.numVerts; i++ ) {
- // set trm at start position
- tw.vertices[i].p += tw.start;
- }
- if ( model_rotated ) {
- for ( 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 ) {
- 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;
- tw.dir *= invModelAxis;
- }
- // rotate trm polygon planes
- if ( trm_rotated & model_rotated ) {
- tmpAxis = trmAxis * invModelAxis;
- for ( poly = tw.polys, i = 0; i < tw.numPolys; i++, poly++ ) {
- poly->plane *= tmpAxis;
- }
- } else if ( trm_rotated ) {
- for ( poly = tw.polys, i = 0; i < tw.numPolys; i++, poly++ ) {
- poly->plane *= trmAxis;
- }
- } else if ( model_rotated ) {
- for ( poly = tw.polys, i = 0; i < tw.numPolys; i++, poly++ ) {
- poly->plane *= invModelAxis;
- }
- }
- // setup trm polygons
- for ( poly = tw.polys, i = 0; i < tw.numPolys; i++, poly++ ) {
- // if the trm poly plane is facing in the movement direction
- dist = poly->plane.Normal() * tw.dir;
- if ( dist > 0.0f || ( !trm->isConvex && dist == 0.0f ) ) {
- // this trm poly and it's edges and vertices need to be used for collision
- poly->used = true;
- for ( j = 0; j < poly->numEdges; j++ ) {
- edge = &tw.edges[abs( poly->edges[j] )];
- edge->used = true;
- tw.vertices[edge->vertexNum[0]].used = true;
- tw.vertices[edge->vertexNum[1]].used = true;
- }
- }
- }
- // setup trm vertices
- for ( vert = tw.vertices, i = 0; i < tw.numVerts; i++, vert++ ) {
- if ( !vert->used ) {
- continue;
- }
- // get axial trm size after rotations
- tw.size.AddPoint( vert->p - tw.start );
- // calculate the end position of each vertex for a full trace
- vert->endp = vert->p + tw.dir;
- // pluecker coordinate for vertex movement line
- vert->pl.FromRay( vert->p, tw.dir );
- }
- // setup trm edges
- for ( edge = tw.edges + 1, i = 1; i <= tw.numEdges; i++, edge++ ) {
- if ( !edge->used ) {
- continue;
- }
- // edge start, end and pluecker coordinate
- edge->start = tw.vertices[edge->vertexNum[0]].p;
- edge->end = tw.vertices[edge->vertexNum[1]].p;
- edge->pl.FromLine( edge->start, edge->end );
- // calculate normal of plane through movement plane created by the edge
- dir = edge->start - edge->end;
- edge->cross[0] = dir[0] * tw.dir[1] - dir[1] * tw.dir[0];
- edge->cross[1] = dir[0] * tw.dir[2] - dir[2] * tw.dir[0];
- edge->cross[2] = dir[1] * tw.dir[2] - dir[2] * tw.dir[1];
- // bit for vertex sidedness bit cache
- edge->bitNum = i;
- }
- // set trm plane distances
- for ( poly = tw.polys, i = 0; i < tw.numPolys; i++, poly++ ) {
- if ( poly->used ) {
- poly->plane.FitThroughPoint( tw.edges[abs(poly->edges[0])].start );
- }
- }
- // bounds for full trace, a little bit larger for epsilons
- for ( 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;
- }
- }
- // setup trace heart planes
- idCollisionModelManagerLocal::SetupTranslationHeartPlanes( &tw );
- tw.maxDistFromHeartPlane1 = 0;
- tw.maxDistFromHeartPlane2 = 0;
- // calculate maximum trm vertex distance from both heart planes
- for ( vert = tw.vertices, i = 0; i < tw.numVerts; i++, vert++ ) {
- if ( !vert->used ) {
- continue;
- }
- dist = idMath::Fabs( tw.heartPlane1.Distance( vert->p ) );
- if ( dist > tw.maxDistFromHeartPlane1 ) {
- tw.maxDistFromHeartPlane1 = dist;
- }
- dist = idMath::Fabs( tw.heartPlane2.Distance( vert->p ) );
- if ( dist > tw.maxDistFromHeartPlane2 ) {
- tw.maxDistFromHeartPlane2 = dist;
- }
- }
- // for epsilons
- tw.maxDistFromHeartPlane1 += CM_BOX_EPSILON;
- tw.maxDistFromHeartPlane2 += CM_BOX_EPSILON;
- // trace through the model
- idCollisionModelManagerLocal::TraceThroughModel( &tw );
- // if we're getting contacts
- if ( tw.getContacts ) {
- // move all contacts to world space
- if ( model_rotated ) {
- for ( i = 0; i < tw.numContacts; i++ ) {
- tw.contacts[i].normal *= modelAxis;
- tw.contacts[i].point *= modelAxis;
- }
- }
- if ( modelOrigin != vec3_origin ) {
- for ( i = 0; i < tw.numContacts; i++ ) {
- tw.contacts[i].point += modelOrigin;
- tw.contacts[i].dist += modelOrigin * tw.contacts[i].normal;
- }
- }
- idCollisionModelManagerLocal::numContacts = tw.numContacts;
- } else {
- // store results
- *results = tw.trace;
- results->endpos = start + results->fraction * ( end - start );
- results->endAxis = trmAxis;
- if ( results->fraction < 1.0f ) {
- // if the fraction is tiny the actual movement could end up zero
- if ( results->fraction > 0.0f && results->endpos.Compare( start ) ) {
- results->fraction = 0.0f;
- }
- // rotate trace plane normal if there was a collision with a rotated model
- if ( model_rotated ) {
- results->c.normal *= modelAxis;
- results->c.point *= modelAxis;
- }
- results->c.point += modelOrigin;
- results->c.dist += modelOrigin * results->c.normal;
- }
- }
- #ifdef _DEBUG
- // test for missed collisions
- if ( cm_debugCollision.GetBool() ) {
- if ( !entered && !idCollisionModelManagerLocal::getContacts ) {
- entered = 1;
- // if the trm is stuck in the model
- if ( idCollisionModelManagerLocal::Contents( results->endpos, trm, trmAxis, -1, model, modelOrigin, modelAxis ) & contentMask ) {
- trace_t tr;
- // test where the trm is stuck in the model
- idCollisionModelManagerLocal::Contents( results->endpos, trm, trmAxis, -1, model, modelOrigin, modelAxis );
- // re-run collision detection to find out where it failed
- idCollisionModelManagerLocal::Translation( &tr, start, end, trm, trmAxis, contentMask, model, modelOrigin, modelAxis );
- }
- entered = 0;
- }
- }
- #endif
- }
|