id/doom3-bfg
0
0
Fork 0
mirror of https://github.com/id-Software/DOOM-3-BFG.git synced 2024-12-02 17:02:17 +00:00
Code Issues Projects Releases Packages Wiki Activity
doom3-bfg/neo/cm/CollisionModel_translate.cpp

1235 lines
35 KiB
C++
Raw Blame History

/*
===========================================================================
Doom 3 BFG Edition GPL Source Code
Copyright (C) 1993-2012 id Software LLC, a ZeniMax Media company.
This file is part of the Doom 3 BFG Edition GPL Source Code ("Doom 3 BFG Edition Source Code").
Doom 3 BFG Edition 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 BFG Edition 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 BFG Edition Source Code. If not, see <http://www.gnu.org/licenses/>.
In addition, the Doom 3 BFG Edition 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 BFG Edition 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.
===============================================================================
*/
#pragma hdrstop
#include "../idlib/precompiled.h"
#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( fabs( t ) < idMath::FLT_SMALLEST_NON_DENORMAL )
{
*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 )
{
const int mask = 1 << bitNum;
if( ( v->sideSet & mask ) == 0 )
{
const float fl = vpl.PermutedInnerProduct( epl );
v->side = ( v->side & ~mask ) | ( ( fl < 0.0f ) ? mask : 0 );
v->sideSet |= mask;
}
}
/*
================
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 )
{
const int mask = 1 << bitNum;
if( ( edge->sideSet & mask ) == 0 )
{
const float fl = vpl.PermutedInnerProduct( epl );
edge->side = ( edge->side & ~mask ) | ( ( fl < 0.0f ) ? mask : 0 );
edge->sideSet |= mask;
}
}
/*
================
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[INT32_SIGNBITSET( edgeNum )];
CM_SetVertexSidedness( v1, tw->polygonVertexPlueckerCache[i], trmEdge->pl, trmEdge->bitNum );
v2 = tw->model->vertices + edge->vertexNum[INT32_SIGNBITNOTSET( 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
================
*/
float CM_TranslationPlaneFraction( const idPlane& plane, const idVec3& start, const idVec3& end )
{
const float 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;
}
const float d1 = plane.Distance( start );
// if completely behind the polygon
if( d1 <= 0.0f )
{
return 1.0f;
}
// leaves polygon
if( d1 - d2 < idMath::FLT_SMALLEST_NON_DENORMAL )
{
return 1.0f;
}
return ( d1 - CM_CLIP_EPSILON ) / ( d1 - d2 );
}
/*
================
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( INT32_SIGNBITSET( 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 = ( fl < 0.0f );
}
// if the point passes the edge at the wrong side
//if ( (edgeNum > 0) == edge->side ) {
if( INT32_SIGNBITSET( 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( INT32_SIGNBITSET( 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[INT32_SIGNBITSET( 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 ^ INT32_SIGNBITSET( 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( common->RW() )
{
common->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
}
Reference in a new issue View git blame Copy permalink