mirror of
https://github.com/blendogames/quadrilateralcowboy.git
synced 2024-11-21 11:41:20 +00:00
1120 lines
35 KiB
C++
1120 lines
35 KiB
C++
/*
|
|
===========================================================================
|
|
|
|
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
|
|
}
|