mirror of
https://github.com/dhewm/dhewm3.git
synced 2024-11-30 08:01:02 +00:00
736ec20d4d
Don't include the lazy precompiled.h everywhere, only what's required for the compilation unit. platform.h needs to be included instead to provide all essential defines and types. All includes use the relative path to the neo or the game specific root. Move all idlib related includes from idlib/Lib.h to precompiled.h. precompiled.h still exists for the MFC stuff in tools/. Add some missing header guards.
1670 lines
49 KiB
C++
1670 lines
49 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 "sys/platform.h"
|
|
|
|
#include "cm/CollisionModel_local.h"
|
|
|
|
/*
|
|
===============================================================================
|
|
|
|
Collision detection for rotational motion
|
|
|
|
===============================================================================
|
|
*/
|
|
|
|
// epsilon for round-off errors in epsilon calculations
|
|
#define CM_PL_RANGE_EPSILON 1e-4f
|
|
// if the collision point is this close to the rotation axis it is not considered a collision
|
|
#define ROTATION_AXIS_EPSILON (CM_CLIP_EPSILON*0.25f)
|
|
|
|
|
|
/*
|
|
================
|
|
CM_RotatePoint
|
|
|
|
rotates a point about an arbitrary axis using the tangent of half the rotation angle
|
|
================
|
|
*/
|
|
void CM_RotatePoint( idVec3 &point, const idVec3 &origin, const idVec3 &axis, const float tanHalfAngle ) {
|
|
double d, t, s, c;
|
|
idVec3 proj, v1, v2;
|
|
|
|
point -= origin;
|
|
proj = axis * ( point * axis );
|
|
v1 = point - proj;
|
|
v2 = axis.Cross( v1 );
|
|
|
|
// r = tan( a / 2 );
|
|
// sin(a) = 2*r/(1+r*r);
|
|
// cos(a) = (1-r*r)/(1+r*r);
|
|
t = tanHalfAngle * tanHalfAngle;
|
|
d = 1.0f / ( 1.0f + t );
|
|
s = 2.0f * tanHalfAngle * d;
|
|
c = ( 1.0f - t ) * d;
|
|
|
|
point = v1 * c - v2 * s + proj + origin;
|
|
}
|
|
|
|
/*
|
|
================
|
|
CM_RotateEdge
|
|
|
|
rotates an edge about an arbitrary axis using the tangent of half the rotation angle
|
|
================
|
|
*/
|
|
void CM_RotateEdge( idVec3 &start, idVec3 &end, const idVec3 &origin, const idVec3 &axis, const float tanHalfAngle ) {
|
|
double d, t, s, c;
|
|
idVec3 proj, v1, v2;
|
|
|
|
// r = tan( a / 2 );
|
|
// sin(a) = 2*r/(1+r*r);
|
|
// cos(a) = (1-r*r)/(1+r*r);
|
|
t = tanHalfAngle * tanHalfAngle;
|
|
d = 1.0f / ( 1.0f + t );
|
|
s = 2.0f * tanHalfAngle * d;
|
|
c = ( 1.0f - t ) * d;
|
|
|
|
start -= origin;
|
|
proj = axis * ( start * axis );
|
|
v1 = start - proj;
|
|
v2 = axis.Cross( v1 );
|
|
start = v1 * c - v2 * s + proj + origin;
|
|
|
|
end -= origin;
|
|
proj = axis * ( end * axis );
|
|
v1 = end - proj;
|
|
v2 = axis.Cross( v1 );
|
|
end = v1 * c - v2 * s + proj + origin;
|
|
}
|
|
|
|
/*
|
|
================
|
|
idCollisionModelManagerLocal::CollisionBetweenEdgeBounds
|
|
|
|
verifies if the collision of two edges occurs between the edge bounds
|
|
also calculates the collision point and collision plane normal if the collision occurs between the bounds
|
|
================
|
|
*/
|
|
int idCollisionModelManagerLocal::CollisionBetweenEdgeBounds( cm_traceWork_t *tw, const idVec3 &va, const idVec3 &vb,
|
|
const idVec3 &vc, const idVec3 &vd, float tanHalfAngle,
|
|
idVec3 &collisionPoint, idVec3 &collisionNormal ) {
|
|
float d1, d2, d;
|
|
idVec3 at, bt, dir, dir1, dir2;
|
|
idPluecker pl1, pl2;
|
|
|
|
at = va;
|
|
bt = vb;
|
|
if ( tanHalfAngle != 0.0f ) {
|
|
CM_RotateEdge( at, bt, tw->origin, tw->axis, tanHalfAngle );
|
|
}
|
|
|
|
dir1 = (at - tw->origin).Cross( tw->axis );
|
|
dir2 = (bt - tw->origin).Cross( tw->axis );
|
|
if ( dir1 * dir1 > dir2 * dir2 ) {
|
|
dir = dir1;
|
|
}
|
|
else {
|
|
dir = dir2;
|
|
}
|
|
if ( tw->angle < 0.0f ) {
|
|
dir = -dir;
|
|
}
|
|
|
|
pl1.FromLine( at, bt );
|
|
pl2.FromRay( vc, dir );
|
|
d1 = pl1.PermutedInnerProduct( pl2 );
|
|
pl2.FromRay( vd, dir );
|
|
d2 = pl1.PermutedInnerProduct( pl2 );
|
|
if ( ( d1 > 0.0f && d2 > 0.0f ) || ( d1 < 0.0f && d2 < 0.0f ) ) {
|
|
return false;
|
|
}
|
|
|
|
pl1.FromLine( vc, vd );
|
|
pl2.FromRay( at, dir );
|
|
d1 = pl1.PermutedInnerProduct( pl2 );
|
|
pl2.FromRay( bt, dir );
|
|
d2 = pl1.PermutedInnerProduct( pl2 );
|
|
if ( ( d1 > 0.0f && d2 > 0.0f ) || ( d1 < 0.0f && d2 < 0.0f ) ) {
|
|
return false;
|
|
}
|
|
|
|
// collision point on the edge at-bt
|
|
dir1 = (vd - vc).Cross( dir );
|
|
d = dir1 * vc;
|
|
d1 = dir1 * at - d;
|
|
d2 = dir1 * bt - d;
|
|
if ( d1 == d2 ) {
|
|
return false;
|
|
}
|
|
collisionPoint = at + ( d1 / (d1 - d2) ) * ( bt - at );
|
|
|
|
// normal is cross product of the rotated edge va-vb and the edge vc-vd
|
|
collisionNormal.Cross( bt-at, vd-vc );
|
|
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
================
|
|
idCollisionModelManagerLocal::RotateEdgeThroughEdge
|
|
|
|
calculates the tangent of half the rotation angle at which the edges collide
|
|
================
|
|
*/
|
|
int idCollisionModelManagerLocal::RotateEdgeThroughEdge( cm_traceWork_t *tw, const idPluecker &pl1,
|
|
const idVec3 &vc, const idVec3 &vd,
|
|
const float minTan, float &tanHalfAngle ) {
|
|
double v0, v1, v2, a, b, c, d, sqrtd, q, frac1, frac2;
|
|
idVec3 ct, dt;
|
|
idPluecker pl2;
|
|
|
|
/*
|
|
|
|
a = start of line being rotated
|
|
b = end of line being rotated
|
|
pl1 = pluecker coordinate for line (a - b)
|
|
pl2 = pluecker coordinate for edge we might collide with (c - d)
|
|
t = rotation angle around the z-axis
|
|
solve pluecker inner product for t of rotating line a-b and line l2
|
|
|
|
// start point of rotated line during rotation
|
|
an[0] = a[0] * cos(t) + a[1] * sin(t)
|
|
an[1] = a[0] * -sin(t) + a[1] * cos(t)
|
|
an[2] = a[2];
|
|
// end point of rotated line during rotation
|
|
bn[0] = b[0] * cos(t) + b[1] * sin(t)
|
|
bn[1] = b[0] * -sin(t) + b[1] * cos(t)
|
|
bn[2] = b[2];
|
|
|
|
pl1[0] = a[0] * b[1] - b[0] * a[1];
|
|
pl1[1] = a[0] * b[2] - b[0] * a[2];
|
|
pl1[2] = a[0] - b[0];
|
|
pl1[3] = a[1] * b[2] - b[1] * a[2];
|
|
pl1[4] = a[2] - b[2];
|
|
pl1[5] = b[1] - a[1];
|
|
|
|
v[0] = (a[0] * cos(t) + a[1] * sin(t)) * (b[0] * -sin(t) + b[1] * cos(t)) - (b[0] * cos(t) + b[1] * sin(t)) * (a[0] * -sin(t) + a[1] * cos(t));
|
|
v[1] = (a[0] * cos(t) + a[1] * sin(t)) * b[2] - (b[0] * cos(t) + b[1] * sin(t)) * a[2];
|
|
v[2] = (a[0] * cos(t) + a[1] * sin(t)) - (b[0] * cos(t) + b[1] * sin(t));
|
|
v[3] = (a[0] * -sin(t) + a[1] * cos(t)) * b[2] - (b[0] * -sin(t) + b[1] * cos(t)) * a[2];
|
|
v[4] = a[2] - b[2];
|
|
v[5] = (b[0] * -sin(t) + b[1] * cos(t)) - (a[0] * -sin(t) + a[1] * cos(t));
|
|
|
|
pl2[0] * v[4] + pl2[1] * v[5] + pl2[2] * v[3] + pl2[4] * v[0] + pl2[5] * v[1] + pl2[3] * v[2] = 0;
|
|
|
|
v[0] = (a[0] * cos(t) + a[1] * sin(t)) * (b[0] * -sin(t) + b[1] * cos(t)) - (b[0] * cos(t) + b[1] * sin(t)) * (a[0] * -sin(t) + a[1] * cos(t));
|
|
v[0] = (a[1] * b[1] - a[0] * b[0]) * cos(t) * sin(t) + (a[0] * b[1] + a[1] * b[0] * cos(t)^2) - (a[1] * b[0]) - ((b[1] * a[1] - b[0] * a[0]) * cos(t) * sin(t) + (b[0] * a[1] + b[1] * a[0]) * cos(t)^2 - (b[1] * a[0]))
|
|
v[0] = - (a[1] * b[0]) - ( - (b[1] * a[0]))
|
|
v[0] = (b[1] * a[0]) - (a[1] * b[0])
|
|
|
|
v[0] = (a[0]*b[1]) - (a[1]*b[0]);
|
|
v[1] = (a[0]*b[2] - b[0]*a[2]) * cos(t) + (a[1]*b[2] - b[1]*a[2]) * sin(t);
|
|
v[2] = (a[0]-b[0]) * cos(t) + (a[1]-b[1]) * sin(t);
|
|
v[3] = (b[0]*a[2] - a[0]*b[2]) * sin(t) + (a[1]*b[2] - b[1]*a[2]) * cos(t);
|
|
v[4] = a[2] - b[2];
|
|
v[5] = (a[0]-b[0]) * sin(t) + (b[1]-a[1]) * cos(t);
|
|
|
|
v[0] = (a[0]*b[1]) - (a[1]*b[0]);
|
|
v[1] = (a[0]*b[2] - b[0]*a[2]) * cos(t) + (a[1]*b[2] - b[1]*a[2]) * sin(t);
|
|
v[2] = (a[0]-b[0]) * cos(t) - (b[1]-a[1]) * sin(t);
|
|
v[3] = (a[0]*b[2] - b[0]*a[2]) * -sin(t) + (a[1]*b[2] - b[1]*a[2]) * cos(t);
|
|
v[4] = a[2] - b[2];
|
|
v[5] = (a[0]-b[0]) * sin(t) + (b[1]-a[1]) * cos(t);
|
|
|
|
v[0] = pl1[0];
|
|
v[1] = pl1[1] * cos(t) + pl1[3] * sin(t);
|
|
v[2] = pl1[2] * cos(t) - pl1[5] * sin(t);
|
|
v[3] = pl1[3] * cos(t) - pl1[1] * sin(t);
|
|
v[4] = pl1[4];
|
|
v[5] = pl1[5] * cos(t) + pl1[2] * sin(t);
|
|
|
|
pl2[0] * v[4] + pl2[1] * v[5] + pl2[2] * v[3] + pl2[4] * v[0] + pl2[5] * v[1] + pl2[3] * v[2] = 0;
|
|
|
|
0 = pl2[0] * pl1[4] +
|
|
pl2[1] * (pl1[5] * cos(t) + pl1[2] * sin(t)) +
|
|
pl2[2] * (pl1[3] * cos(t) - pl1[1] * sin(t)) +
|
|
pl2[4] * pl1[0] +
|
|
pl2[5] * (pl1[1] * cos(t) + pl1[3] * sin(t)) +
|
|
pl2[3] * (pl1[2] * cos(t) - pl1[5] * sin(t));
|
|
|
|
v2 * cos(t) + v1 * sin(t) + v0 = 0;
|
|
|
|
// rotation about the z-axis
|
|
v0 = pl2[0] * pl1[4] + pl2[4] * pl1[0];
|
|
v1 = pl2[1] * pl1[2] - pl2[2] * pl1[1] + pl2[5] * pl1[3] - pl2[3] * pl1[5];
|
|
v2 = pl2[1] * pl1[5] + pl2[2] * pl1[3] + pl2[5] * pl1[1] + pl2[3] * pl1[2];
|
|
|
|
// rotation about the x-axis
|
|
//v0 = pl2[3] * pl1[2] + pl2[2] * pl1[3];
|
|
//v1 = -pl2[5] * pl1[0] + pl2[4] * pl1[1] - pl2[1] * pl1[4] + pl2[0] * pl1[5];
|
|
//v2 = pl2[4] * pl1[0] + pl2[5] * pl1[1] + pl2[0] * pl1[4] + pl2[1] * pl1[5];
|
|
|
|
r = tan(t / 2);
|
|
sin(t) = 2*r/(1+r*r);
|
|
cos(t) = (1-r*r)/(1+r*r);
|
|
|
|
v1 * 2 * r / (1 + r*r) + v2 * (1 - r*r) / (1 + r*r) + v0 = 0
|
|
(v1 * 2 * r + v2 * (1 - r*r)) / (1 + r*r) = -v0
|
|
(v1 * 2 * r + v2 - v2 * r*r) / (1 + r*r) = -v0
|
|
v1 * 2 * r + v2 - v2 * r*r = -v0 * (1 + r*r)
|
|
v1 * 2 * r + v2 - v2 * r*r = -v0 + -v0 * r*r
|
|
(v0 - v2) * r * r + (2 * v1) * r + (v0 + v2) = 0;
|
|
|
|
MrE gives Pluecker a banana.. good monkey
|
|
|
|
*/
|
|
|
|
tanHalfAngle = tw->maxTan;
|
|
|
|
// transform rotation axis to z-axis
|
|
ct = (vc - tw->origin) * tw->matrix;
|
|
dt = (vd - tw->origin) * tw->matrix;
|
|
|
|
pl2.FromLine( ct, dt );
|
|
|
|
v0 = pl2[0] * pl1[4] + pl2[4] * pl1[0];
|
|
v1 = pl2[1] * pl1[2] - pl2[2] * pl1[1] + pl2[5] * pl1[3] - pl2[3] * pl1[5];
|
|
v2 = pl2[1] * pl1[5] + pl2[2] * pl1[3] + pl2[5] * pl1[1] + pl2[3] * pl1[2];
|
|
|
|
a = v0 - v2;
|
|
b = v1;
|
|
c = v0 + v2;
|
|
if ( a == 0.0f ) {
|
|
if ( b == 0.0f ) {
|
|
return false;
|
|
}
|
|
frac1 = -c / ( 2.0f * b );
|
|
frac2 = 1e10; // = tan( idMath::HALF_PI )
|
|
}
|
|
else {
|
|
d = b * b - c * a;
|
|
if ( d <= 0.0f ) {
|
|
return false;
|
|
}
|
|
sqrtd = sqrt( d );
|
|
if ( b > 0.0f ) {
|
|
q = - b + sqrtd;
|
|
}
|
|
else {
|
|
q = - b - sqrtd;
|
|
}
|
|
frac1 = q / a;
|
|
frac2 = c / q;
|
|
}
|
|
|
|
if ( tw->angle < 0.0f ) {
|
|
frac1 = -frac1;
|
|
frac2 = -frac2;
|
|
}
|
|
|
|
// get smallest tangent for which a collision occurs
|
|
if ( frac1 >= minTan && frac1 < tanHalfAngle ) {
|
|
tanHalfAngle = frac1;
|
|
}
|
|
if ( frac2 >= minTan && frac2 < tanHalfAngle ) {
|
|
tanHalfAngle = frac2;
|
|
}
|
|
|
|
if ( tw->angle < 0.0f ) {
|
|
tanHalfAngle = -tanHalfAngle;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
================
|
|
idCollisionModelManagerLocal::EdgeFurthestFromEdge
|
|
|
|
calculates the direction of motion at the initial position, where dir < 0 means the edges move towards each other
|
|
if the edges move away from each other the tangent of half the rotation angle at which
|
|
the edges are furthest apart is also calculated
|
|
================
|
|
*/
|
|
int idCollisionModelManagerLocal::EdgeFurthestFromEdge( cm_traceWork_t *tw, const idPluecker &pl1,
|
|
const idVec3 &vc, const idVec3 &vd,
|
|
float &tanHalfAngle, float &dir ) {
|
|
double v0, v1, v2, a, b, c, d, sqrtd, q, frac1, frac2;
|
|
idVec3 ct, dt;
|
|
idPluecker pl2;
|
|
|
|
/*
|
|
|
|
v2 * cos(t) + v1 * sin(t) + v0 = 0;
|
|
|
|
// rotation about the z-axis
|
|
v0 = pl2[0] * pl1[4] + pl2[4] * pl1[0];
|
|
v1 = pl2[1] * pl1[2] - pl2[2] * pl1[1] + pl2[5] * pl1[3] - pl2[3] * pl1[5];
|
|
v2 = pl2[1] * pl1[5] + pl2[2] * pl1[3] + pl2[5] * pl1[1] + pl2[3] * pl1[2];
|
|
|
|
derivative:
|
|
v1 * cos(t) - v2 * sin(t) = 0;
|
|
|
|
r = tan(t / 2);
|
|
sin(t) = 2*r/(1+r*r);
|
|
cos(t) = (1-r*r)/(1+r*r);
|
|
|
|
-v2 * 2 * r / (1 + r*r) + v1 * (1 - r*r)/(1+r*r);
|
|
-v2 * 2 * r + v1 * (1 - r*r) / (1 + r*r) = 0;
|
|
-v2 * 2 * r + v1 * (1 - r*r) = 0;
|
|
(-v1) * r * r + (-2 * v2) * r + (v1) = 0;
|
|
|
|
*/
|
|
|
|
tanHalfAngle = 0.0f;
|
|
|
|
// transform rotation axis to z-axis
|
|
ct = (vc - tw->origin) * tw->matrix;
|
|
dt = (vd - tw->origin) * tw->matrix;
|
|
|
|
pl2.FromLine( ct, dt );
|
|
|
|
v0 = pl2[0] * pl1[4] + pl2[4] * pl1[0];
|
|
v1 = pl2[1] * pl1[2] - pl2[2] * pl1[1] + pl2[5] * pl1[3] - pl2[3] * pl1[5];
|
|
v2 = pl2[1] * pl1[5] + pl2[2] * pl1[3] + pl2[5] * pl1[1] + pl2[3] * pl1[2];
|
|
|
|
// get the direction of motion at the initial position
|
|
c = v0 + v2;
|
|
if ( tw->angle > 0.0f ) {
|
|
if ( c > 0.0f ) {
|
|
dir = v1;
|
|
}
|
|
else {
|
|
dir = -v1;
|
|
}
|
|
}
|
|
else {
|
|
if ( c > 0.0f ) {
|
|
dir = -v1;
|
|
}
|
|
else {
|
|
dir = v1;
|
|
}
|
|
}
|
|
// negative direction means the edges move towards each other at the initial position
|
|
if ( dir <= 0.0f ) {
|
|
return true;
|
|
}
|
|
|
|
a = -v1;
|
|
b = -v2;
|
|
c = v1;
|
|
if ( a == 0.0f ) {
|
|
if ( b == 0.0f ) {
|
|
return false;
|
|
}
|
|
frac1 = -c / ( 2.0f * b );
|
|
frac2 = 1e10; // = tan( idMath::HALF_PI )
|
|
}
|
|
else {
|
|
d = b * b - c * a;
|
|
if ( d <= 0.0f ) {
|
|
return false;
|
|
}
|
|
sqrtd = sqrt( d );
|
|
if ( b > 0.0f ) {
|
|
q = - b + sqrtd;
|
|
}
|
|
else {
|
|
q = - b - sqrtd;
|
|
}
|
|
frac1 = q / a;
|
|
frac2 = c / q;
|
|
}
|
|
|
|
if ( tw->angle < 0.0f ) {
|
|
frac1 = -frac1;
|
|
frac2 = -frac2;
|
|
}
|
|
|
|
if ( frac1 < 0.0f && frac2 < 0.0f ) {
|
|
return false;
|
|
}
|
|
|
|
if ( frac1 > frac2 ) {
|
|
tanHalfAngle = frac1;
|
|
}
|
|
else {
|
|
tanHalfAngle = frac2;
|
|
}
|
|
|
|
if ( tw->angle < 0.0f ) {
|
|
tanHalfAngle = -tanHalfAngle;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
================
|
|
idCollisionModelManagerLocal::RotateTrmEdgeThroughPolygon
|
|
================
|
|
*/
|
|
void idCollisionModelManagerLocal::RotateTrmEdgeThroughPolygon( cm_traceWork_t *tw, cm_polygon_t *poly, cm_trmEdge_t *trmEdge ) {
|
|
int i, j, edgeNum;
|
|
float f1, f2, startTan, dir, tanHalfAngle;
|
|
cm_edge_t *edge;
|
|
cm_vertex_t *v1, *v2;
|
|
idVec3 collisionPoint, collisionNormal, origin, epsDir;
|
|
idPluecker epsPl;
|
|
idBounds bounds;
|
|
|
|
// if the trm is convex and the rotation axis intersects the trm
|
|
if ( tw->isConvex && tw->axisIntersectsTrm ) {
|
|
// if both points are behind the polygon the edge cannot collide within a 180 degrees rotation
|
|
if ( tw->vertices[trmEdge->vertexNum[0]].polygonSide & tw->vertices[trmEdge->vertexNum[1]].polygonSide ) {
|
|
return;
|
|
}
|
|
}
|
|
|
|
// if the trace model edge rotation bounds do not intersect the polygon bounds
|
|
if ( !trmEdge->rotationBounds.IntersectsBounds( poly->bounds ) ) {
|
|
return;
|
|
}
|
|
|
|
// edge rotation bounds should cross polygon plane
|
|
if ( trmEdge->rotationBounds.PlaneSide( poly->plane ) != SIDE_CROSS ) {
|
|
return;
|
|
}
|
|
|
|
// 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;
|
|
}
|
|
|
|
v1 = tw->model->vertices + edge->vertexNum[INTSIGNBITSET(edgeNum)];
|
|
v2 = tw->model->vertices + edge->vertexNum[INTSIGNBITNOTSET(edgeNum)];
|
|
|
|
// edge bounds
|
|
for ( j = 0; j < 3; j++ ) {
|
|
if ( v1->p[j] > v2->p[j] ) {
|
|
bounds[0][j] = v2->p[j];
|
|
bounds[1][j] = v1->p[j];
|
|
}
|
|
else {
|
|
bounds[0][j] = v1->p[j];
|
|
bounds[1][j] = v2->p[j];
|
|
}
|
|
}
|
|
|
|
// if the trace model edge rotation bounds do not intersect the polygon edge bounds
|
|
if ( !trmEdge->rotationBounds.IntersectsBounds( bounds ) ) {
|
|
continue;
|
|
}
|
|
|
|
f1 = trmEdge->pl.PermutedInnerProduct( tw->polygonEdgePlueckerCache[i] );
|
|
|
|
// pluecker coordinate for epsilon expanded edge
|
|
epsDir = edge->normal * (CM_CLIP_EPSILON+CM_PL_RANGE_EPSILON);
|
|
epsPl.FromLine( tw->model->vertices[edge->vertexNum[0]].p + epsDir,
|
|
tw->model->vertices[edge->vertexNum[1]].p + epsDir );
|
|
|
|
f2 = trmEdge->pl.PermutedInnerProduct( epsPl );
|
|
|
|
// if the rotating edge is inbetween the polygon edge and the epsilon expanded edge
|
|
if ( ( f1 < 0.0f && f2 > 0.0f ) || ( f1 > 0.0f && f2 < 0.0f ) ) {
|
|
|
|
if ( !EdgeFurthestFromEdge( tw, trmEdge->plzaxis, v1->p, v2->p, startTan, dir ) ) {
|
|
continue;
|
|
}
|
|
|
|
if ( dir <= 0.0f ) {
|
|
// moving towards the polygon edge so stop immediately
|
|
tanHalfAngle = 0.0f;
|
|
}
|
|
else if ( idMath::Fabs( startTan ) >= tw->maxTan ) {
|
|
// never going to get beyond the start tangent during the current rotation
|
|
continue;
|
|
}
|
|
else {
|
|
// collide with the epsilon expanded edge
|
|
if ( !RotateEdgeThroughEdge(tw, trmEdge->plzaxis, v1->p + epsDir, v2->p + epsDir, idMath::Fabs( startTan ), tanHalfAngle ) ) {
|
|
tanHalfAngle = startTan;
|
|
}
|
|
}
|
|
}
|
|
else {
|
|
// collide with the epsilon expanded edge
|
|
epsDir = edge->normal * CM_CLIP_EPSILON;
|
|
if ( !RotateEdgeThroughEdge(tw, trmEdge->plzaxis, v1->p + epsDir, v2->p + epsDir, 0.0f, tanHalfAngle ) ) {
|
|
continue;
|
|
}
|
|
}
|
|
|
|
if ( idMath::Fabs( tanHalfAngle ) >= tw->maxTan ) {
|
|
continue;
|
|
}
|
|
|
|
// check if the collision is between the edge bounds
|
|
if ( !CollisionBetweenEdgeBounds( tw, trmEdge->start, trmEdge->end, v1->p, v2->p,
|
|
tanHalfAngle, collisionPoint, collisionNormal ) ) {
|
|
continue;
|
|
}
|
|
|
|
// allow rotation if the rotation axis goes through the collisionPoint
|
|
origin = tw->origin + tw->axis * ( tw->axis * ( collisionPoint - tw->origin ) );
|
|
if ( ( collisionPoint - origin ).LengthSqr() < ROTATION_AXIS_EPSILON * ROTATION_AXIS_EPSILON ) {
|
|
continue;
|
|
}
|
|
|
|
// fill in trace structure
|
|
tw->maxTan = idMath::Fabs( tanHalfAngle );
|
|
tw->trace.c.normal = collisionNormal;
|
|
tw->trace.c.normal.Normalize();
|
|
tw->trace.c.dist = tw->trace.c.normal * v1->p;
|
|
// make sure the collision plane faces the trace model
|
|
if ( (tw->trace.c.normal * trmEdge->start) - tw->trace.c.dist < 0 ) {
|
|
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;
|
|
tw->trace.c.point = collisionPoint;
|
|
// if no collision can be closer
|
|
if ( tw->maxTan == 0.0f ) {
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
/*
|
|
================
|
|
idCollisionModelManagerLocal::RotatePointThroughPlane
|
|
|
|
calculates the tangent of half the rotation angle at which the point collides with the plane
|
|
================
|
|
*/
|
|
int idCollisionModelManagerLocal::RotatePointThroughPlane( const cm_traceWork_t *tw, const idVec3 &point, const idPlane &plane,
|
|
const float angle, const float minTan, float &tanHalfAngle ) {
|
|
double v0, v1, v2, a, b, c, d, sqrtd, q, frac1, frac2;
|
|
idVec3 p, normal;
|
|
|
|
/*
|
|
|
|
p[0] = point[0] * cos(t) + point[1] * sin(t)
|
|
p[1] = point[0] * -sin(t) + point[1] * cos(t)
|
|
p[2] = point[2];
|
|
|
|
normal[0] * (p[0] * cos(t) + p[1] * sin(t)) +
|
|
normal[1] * (p[0] * -sin(t) + p[1] * cos(t)) +
|
|
normal[2] * p[2] + dist = 0
|
|
|
|
normal[0] * p[0] * cos(t) + normal[0] * p[1] * sin(t) +
|
|
-normal[1] * p[0] * sin(t) + normal[1] * p[1] * cos(t) +
|
|
normal[2] * p[2] + dist = 0
|
|
|
|
v2 * cos(t) + v1 * sin(t) + v0
|
|
|
|
// rotation about the z-axis
|
|
v0 = normal[2] * p[2] + dist
|
|
v1 = normal[0] * p[1] - normal[1] * p[0]
|
|
v2 = normal[0] * p[0] + normal[1] * p[1]
|
|
|
|
r = tan(t / 2);
|
|
sin(t) = 2*r/(1+r*r);
|
|
cos(t) = (1-r*r)/(1+r*r);
|
|
|
|
v1 * 2 * r / (1 + r*r) + v2 * (1 - r*r) / (1 + r*r) + v0 = 0
|
|
(v1 * 2 * r + v2 * (1 - r*r)) / (1 + r*r) = -v0
|
|
(v1 * 2 * r + v2 - v2 * r*r) / (1 + r*r) = -v0
|
|
v1 * 2 * r + v2 - v2 * r*r = -v0 * (1 + r*r)
|
|
v1 * 2 * r + v2 - v2 * r*r = -v0 + -v0 * r*r
|
|
(v0 - v2) * r * r + (2 * v1) * r + (v0 + v2) = 0;
|
|
|
|
*/
|
|
|
|
tanHalfAngle = tw->maxTan;
|
|
|
|
// transform rotation axis to z-axis
|
|
p = (point - tw->origin) * tw->matrix;
|
|
d = plane[3] + plane.Normal() * tw->origin;
|
|
normal = plane.Normal() * tw->matrix;
|
|
|
|
v0 = normal[2] * p[2] + d;
|
|
v1 = normal[0] * p[1] - normal[1] * p[0];
|
|
v2 = normal[0] * p[0] + normal[1] * p[1];
|
|
|
|
a = v0 - v2;
|
|
b = v1;
|
|
c = v0 + v2;
|
|
if ( a == 0.0f ) {
|
|
if ( b == 0.0f ) {
|
|
return false;
|
|
}
|
|
frac1 = -c / ( 2.0f * b );
|
|
frac2 = 1e10; // = tan( idMath::HALF_PI )
|
|
}
|
|
else {
|
|
d = b * b - c * a;
|
|
if ( d <= 0.0f ) {
|
|
return false;
|
|
}
|
|
sqrtd = sqrt( d );
|
|
if ( b > 0.0f ) {
|
|
q = - b + sqrtd;
|
|
}
|
|
else {
|
|
q = - b - sqrtd;
|
|
}
|
|
frac1 = q / a;
|
|
frac2 = c / q;
|
|
}
|
|
|
|
if ( angle < 0.0f ) {
|
|
frac1 = -frac1;
|
|
frac2 = -frac2;
|
|
}
|
|
|
|
// get smallest tangent for which a collision occurs
|
|
if ( frac1 >= minTan && frac1 < tanHalfAngle ) {
|
|
tanHalfAngle = frac1;
|
|
}
|
|
if ( frac2 >= minTan && frac2 < tanHalfAngle ) {
|
|
tanHalfAngle = frac2;
|
|
}
|
|
|
|
if ( angle < 0.0f ) {
|
|
tanHalfAngle = -tanHalfAngle;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
================
|
|
idCollisionModelManagerLocal::PointFurthestFromPlane
|
|
|
|
calculates the direction of motion at the initial position, where dir < 0 means the point moves towards the plane
|
|
if the point moves away from the plane the tangent of half the rotation angle at which
|
|
the point is furthest away from the plane is also calculated
|
|
================
|
|
*/
|
|
int idCollisionModelManagerLocal::PointFurthestFromPlane( const cm_traceWork_t *tw, const idVec3 &point, const idPlane &plane,
|
|
const float angle, float &tanHalfAngle, float &dir ) {
|
|
|
|
double v1, v2, a, b, c, d, sqrtd, q, frac1, frac2;
|
|
idVec3 p, normal;
|
|
|
|
/*
|
|
|
|
v2 * cos(t) + v1 * sin(t) + v0 = 0;
|
|
|
|
// rotation about the z-axis
|
|
v0 = normal[2] * p[2] + dist
|
|
v1 = normal[0] * p[1] - normal[1] * p[0]
|
|
v2 = normal[0] * p[0] + normal[1] * p[1]
|
|
|
|
derivative:
|
|
v1 * cos(t) - v2 * sin(t) = 0;
|
|
|
|
r = tan(t / 2);
|
|
sin(t) = 2*r/(1+r*r);
|
|
cos(t) = (1-r*r)/(1+r*r);
|
|
|
|
-v2 * 2 * r / (1 + r*r) + v1 * (1 - r*r)/(1+r*r);
|
|
-v2 * 2 * r + v1 * (1 - r*r) / (1 + r*r) = 0;
|
|
-v2 * 2 * r + v1 * (1 - r*r) = 0;
|
|
(-v1) * r * r + (-2 * v2) * r + (v1) = 0;
|
|
|
|
*/
|
|
|
|
tanHalfAngle = 0.0f;
|
|
|
|
// transform rotation axis to z-axis
|
|
p = (point - tw->origin) * tw->matrix;
|
|
normal = plane.Normal() * tw->matrix;
|
|
|
|
v1 = normal[0] * p[1] - normal[1] * p[0];
|
|
v2 = normal[0] * p[0] + normal[1] * p[1];
|
|
|
|
// the point will always start at the front of the plane, therefore v0 + v2 > 0 is always true
|
|
if ( angle < 0.0f ) {
|
|
dir = -v1;
|
|
}
|
|
else {
|
|
dir = v1;
|
|
}
|
|
// negative direction means the point moves towards the plane at the initial position
|
|
if ( dir <= 0.0f ) {
|
|
return true;
|
|
}
|
|
|
|
a = -v1;
|
|
b = -v2;
|
|
c = v1;
|
|
if ( a == 0.0f ) {
|
|
if ( b == 0.0f ) {
|
|
return false;
|
|
}
|
|
frac1 = -c / ( 2.0f * b );
|
|
frac2 = 1e10; // = tan( idMath::HALF_PI )
|
|
}
|
|
else {
|
|
d = b * b - c * a;
|
|
if ( d <= 0.0f ) {
|
|
return false;
|
|
}
|
|
sqrtd = sqrt( d );
|
|
if ( b > 0.0f ) {
|
|
q = - b + sqrtd;
|
|
}
|
|
else {
|
|
q = - b - sqrtd;
|
|
}
|
|
frac1 = q / a;
|
|
frac2 = c / q;
|
|
}
|
|
|
|
if ( angle < 0.0f ) {
|
|
frac1 = -frac1;
|
|
frac2 = -frac2;
|
|
}
|
|
|
|
if ( frac1 < 0.0f && frac2 < 0.0f ) {
|
|
return false;
|
|
}
|
|
|
|
if ( frac1 > frac2 ) {
|
|
tanHalfAngle = frac1;
|
|
}
|
|
else {
|
|
tanHalfAngle = frac2;
|
|
}
|
|
|
|
if ( angle < 0.0f ) {
|
|
tanHalfAngle = -tanHalfAngle;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
================
|
|
idCollisionModelManagerLocal::RotatePointThroughEpsilonPlane
|
|
================
|
|
*/
|
|
int idCollisionModelManagerLocal::RotatePointThroughEpsilonPlane( const cm_traceWork_t *tw, const idVec3 &point, const idVec3 &endPoint,
|
|
const idPlane &plane, const float angle, const idVec3 &origin,
|
|
float &tanHalfAngle, idVec3 &collisionPoint, idVec3 &endDir ) {
|
|
float d, dir, startTan;
|
|
idVec3 vec, startDir;
|
|
idPlane epsPlane;
|
|
|
|
// epsilon expanded plane
|
|
epsPlane = plane;
|
|
epsPlane.SetDist( epsPlane.Dist() + CM_CLIP_EPSILON );
|
|
|
|
// if the rotation sphere at the rotation origin is too far away from the polygon plane
|
|
d = epsPlane.Distance( origin );
|
|
vec = point - origin;
|
|
if ( d * d > vec * vec ) {
|
|
return false;
|
|
}
|
|
|
|
// calculate direction of motion at vertex start position
|
|
startDir = ( point - origin ).Cross( tw->axis );
|
|
if ( angle < 0.0f ) {
|
|
startDir = -startDir;
|
|
}
|
|
// if moving away from plane at start position
|
|
if ( startDir * epsPlane.Normal() >= 0.0f ) {
|
|
// if end position is outside epsilon range
|
|
d = epsPlane.Distance( endPoint );
|
|
if ( d >= 0.0f ) {
|
|
return false; // no collision
|
|
}
|
|
// calculate direction of motion at vertex end position
|
|
endDir = ( endPoint - origin ).Cross( tw->axis );
|
|
if ( angle < 0.0f ) {
|
|
endDir = -endDir;
|
|
}
|
|
// if also moving away from plane at end position
|
|
if ( endDir * epsPlane.Normal() > 0.0f ) {
|
|
return false; // no collision
|
|
}
|
|
}
|
|
|
|
// if the start position is in the epsilon range
|
|
d = epsPlane.Distance( point );
|
|
if ( d <= CM_PL_RANGE_EPSILON ) {
|
|
|
|
// calculate tangent of half the rotation for which the vertex is furthest away from the plane
|
|
if ( !PointFurthestFromPlane( tw, point, plane, angle, startTan, dir ) ) {
|
|
return false;
|
|
}
|
|
|
|
if ( dir <= 0.0f ) {
|
|
// moving towards the polygon plane so stop immediately
|
|
tanHalfAngle = 0.0f;
|
|
}
|
|
else if ( idMath::Fabs( startTan ) >= tw->maxTan ) {
|
|
// never going to get beyond the start tangent during the current rotation
|
|
return false;
|
|
}
|
|
else {
|
|
// calculate collision with epsilon expanded plane
|
|
if ( !RotatePointThroughPlane( tw, point, epsPlane, angle, idMath::Fabs( startTan ), tanHalfAngle ) ) {
|
|
tanHalfAngle = startTan;
|
|
}
|
|
}
|
|
}
|
|
else {
|
|
// calculate collision with epsilon expanded plane
|
|
if ( !RotatePointThroughPlane( tw, point, epsPlane, angle, 0.0f, tanHalfAngle ) ) {
|
|
return false;
|
|
}
|
|
}
|
|
|
|
// calculate collision point
|
|
collisionPoint = point;
|
|
if ( tanHalfAngle != 0.0f ) {
|
|
CM_RotatePoint( collisionPoint, tw->origin, tw->axis, tanHalfAngle );
|
|
}
|
|
// calculate direction of motion at collision point
|
|
endDir = ( collisionPoint - origin ).Cross( tw->axis );
|
|
if ( angle < 0.0f ) {
|
|
endDir = -endDir;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
================
|
|
idCollisionModelManagerLocal::RotateTrmVertexThroughPolygon
|
|
================
|
|
*/
|
|
void idCollisionModelManagerLocal::RotateTrmVertexThroughPolygon( cm_traceWork_t *tw, cm_polygon_t *poly, cm_trmVertex_t *v, int vertexNum ) {
|
|
int i;
|
|
float tanHalfAngle;
|
|
idVec3 endDir, collisionPoint;
|
|
idPluecker pl;
|
|
|
|
// if the trm vertex is behind the polygon plane it cannot collide with the polygon within a 180 degrees rotation
|
|
if ( tw->isConvex && tw->axisIntersectsTrm && v->polygonSide ) {
|
|
return;
|
|
}
|
|
|
|
// if the trace model vertex rotation bounds do not intersect the polygon bounds
|
|
if ( !v->rotationBounds.IntersectsBounds( poly->bounds ) ) {
|
|
return;
|
|
}
|
|
|
|
// vertex rotation bounds should cross polygon plane
|
|
if ( v->rotationBounds.PlaneSide( poly->plane ) != SIDE_CROSS ) {
|
|
return;
|
|
}
|
|
|
|
// rotate the vertex through the epsilon plane
|
|
if ( !RotatePointThroughEpsilonPlane( tw, v->p, v->endp, poly->plane, tw->angle, v->rotationOrigin,
|
|
tanHalfAngle, collisionPoint, endDir ) ) {
|
|
return;
|
|
}
|
|
|
|
if ( idMath::Fabs( tanHalfAngle ) < tw->maxTan ) {
|
|
// verify if 'collisionPoint' moving along 'endDir' moves between polygon edges
|
|
pl.FromRay( collisionPoint, endDir );
|
|
for ( i = 0; i < poly->numEdges; i++ ) {
|
|
if ( poly->edges[i] < 0 ) {
|
|
if ( pl.PermutedInnerProduct( tw->polygonEdgePlueckerCache[i] ) > 0.0f ) {
|
|
return;
|
|
}
|
|
}
|
|
else {
|
|
if ( pl.PermutedInnerProduct( tw->polygonEdgePlueckerCache[i] ) < 0.0f ) {
|
|
return;
|
|
}
|
|
}
|
|
}
|
|
tw->maxTan = idMath::Fabs( tanHalfAngle );
|
|
// 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 = collisionPoint;
|
|
}
|
|
}
|
|
|
|
/*
|
|
================
|
|
idCollisionModelManagerLocal::RotateVertexThroughTrmPolygon
|
|
================
|
|
*/
|
|
void idCollisionModelManagerLocal::RotateVertexThroughTrmPolygon( cm_traceWork_t *tw, cm_trmPolygon_t *trmpoly, cm_polygon_t *poly, cm_vertex_t *v, idVec3 &rotationOrigin ) {
|
|
int i, edgeNum;
|
|
float tanHalfAngle;
|
|
idVec3 dir, endp, endDir, collisionPoint;
|
|
idPluecker pl;
|
|
cm_trmEdge_t *edge;
|
|
|
|
// if the polygon vertex is behind the trm plane it cannot collide with the trm polygon within a 180 degrees rotation
|
|
if ( tw->isConvex && tw->axisIntersectsTrm && trmpoly->plane.Distance( v->p ) < 0.0f ) {
|
|
return;
|
|
}
|
|
|
|
// if the model vertex is outside the trm polygon rotation bounds
|
|
if ( !trmpoly->rotationBounds.ContainsPoint( v->p ) ) {
|
|
return;
|
|
}
|
|
|
|
// if the rotation axis goes through the polygon vertex
|
|
dir = v->p - rotationOrigin;
|
|
if ( dir * dir < ROTATION_AXIS_EPSILON * ROTATION_AXIS_EPSILON ) {
|
|
return;
|
|
}
|
|
|
|
// calculate vertex end position
|
|
endp = v->p;
|
|
tw->modelVertexRotation.RotatePoint( endp );
|
|
|
|
// rotate the vertex through the epsilon plane
|
|
if ( !RotatePointThroughEpsilonPlane( tw, v->p, endp, trmpoly->plane, -tw->angle, rotationOrigin,
|
|
tanHalfAngle, collisionPoint, endDir ) ) {
|
|
return;
|
|
}
|
|
|
|
if ( idMath::Fabs( tanHalfAngle ) < tw->maxTan ) {
|
|
// verify if 'collisionPoint' moving along 'endDir' moves between polygon edges
|
|
pl.FromRay( collisionPoint, endDir );
|
|
for ( i = 0; i < trmpoly->numEdges; i++ ) {
|
|
edgeNum = trmpoly->edges[i];
|
|
edge = tw->edges + abs(edgeNum);
|
|
if ( edgeNum < 0 ) {
|
|
if ( pl.PermutedInnerProduct( edge->pl ) > 0.0f ) {
|
|
return;
|
|
}
|
|
}
|
|
else {
|
|
if ( pl.PermutedInnerProduct( edge->pl ) < 0.0f ) {
|
|
return;
|
|
}
|
|
}
|
|
}
|
|
tw->maxTan = idMath::Fabs( tanHalfAngle );
|
|
// collision plane is the flipped trm polygon plane
|
|
tw->trace.c.normal = -trmpoly->plane.Normal();
|
|
tw->trace.c.dist = tw->trace.c.normal * v->p;
|
|
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;
|
|
}
|
|
}
|
|
|
|
/*
|
|
================
|
|
idCollisionModelManagerLocal::RotateTrmThroughPolygon
|
|
|
|
returns true if the polygon blocks the complete rotation
|
|
================
|
|
*/
|
|
bool idCollisionModelManagerLocal::RotateTrmThroughPolygon( cm_traceWork_t *tw, cm_polygon_t *p ) {
|
|
int i, j, k, edgeNum;
|
|
float d;
|
|
cm_trmVertex_t *bv;
|
|
cm_trmEdge_t *be;
|
|
cm_trmPolygon_t *bp;
|
|
cm_vertex_t *v;
|
|
cm_edge_t *e;
|
|
idVec3 *rotationOrigin;
|
|
|
|
// 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;
|
|
}
|
|
|
|
// back face culling
|
|
if ( tw->isConvex ) {
|
|
// if the center of the convex trm is behind the polygon plane
|
|
if ( p->plane.Distance( tw->start ) < 0.0f ) {
|
|
// if the rotation axis intersects the trace model
|
|
if ( tw->axisIntersectsTrm ) {
|
|
return false;
|
|
}
|
|
else {
|
|
// if the direction of motion at the start and end position of the
|
|
// center of the trm both go towards or away from the polygon plane
|
|
// or if the intersections of the rotation axis with the expanded heart planes
|
|
// are both in front of the polygon plane
|
|
}
|
|
}
|
|
}
|
|
|
|
// 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;
|
|
}
|
|
|
|
// rotation 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;
|
|
}
|
|
|
|
for ( i = 0; i < tw->numVerts; i++ ) {
|
|
bv = tw->vertices + i;
|
|
// calculate polygon side this vertex is on
|
|
d = p->plane.Distance( bv->p );
|
|
bv->polygonSide = FLOATSIGNBITSET( d );
|
|
}
|
|
|
|
for ( i = 0; i < p->numEdges; i++ ) {
|
|
edgeNum = p->edges[i];
|
|
e = tw->model->edges + abs(edgeNum);
|
|
v = tw->model->vertices + e->vertexNum[INTSIGNBITSET(edgeNum)];
|
|
|
|
// pluecker coordinate for edge
|
|
tw->polygonEdgePlueckerCache[i].FromLine( tw->model->vertices[e->vertexNum[0]].p,
|
|
tw->model->vertices[e->vertexNum[1]].p );
|
|
|
|
// calculate rotation origin projected into rotation plane through the vertex
|
|
tw->polygonRotationOriginCache[i] = tw->origin + tw->axis * ( tw->axis * ( v->p - tw->origin ) );
|
|
}
|
|
// copy first to last so we can easily cycle through
|
|
tw->polygonRotationOriginCache[p->numEdges] = tw->polygonRotationOriginCache[0];
|
|
|
|
// fast point rotation
|
|
if ( tw->pointTrace ) {
|
|
RotateTrmVertexThroughPolygon( tw, p, &tw->vertices[0], 0 );
|
|
}
|
|
else {
|
|
// rotate trm vertices through polygon
|
|
for ( i = 0; i < tw->numVerts; i++ ) {
|
|
bv = tw->vertices + i;
|
|
if ( bv->used ) {
|
|
RotateTrmVertexThroughPolygon( tw, p, bv, i );
|
|
}
|
|
}
|
|
|
|
// rotate trm edges through polygon
|
|
for ( i = 1; i <= tw->numEdges; i++ ) {
|
|
be = tw->edges + i;
|
|
if ( be->used ) {
|
|
RotateTrmEdgeThroughPolygon( tw, p, be );
|
|
}
|
|
}
|
|
|
|
// rotate 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 trm rotation bounds
|
|
if ( !tw->bounds.ContainsPoint( v->p ) ) {
|
|
continue;
|
|
}
|
|
|
|
rotationOrigin = &tw->polygonRotationOriginCache[i+k];
|
|
|
|
for ( j = 0; j < tw->numPolys; j++ ) {
|
|
bp = tw->polys + j;
|
|
if ( bp->used ) {
|
|
RotateVertexThroughTrmPolygon( tw, bp, p, v, *rotationOrigin );
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
return ( tw->maxTan == 0.0f );
|
|
}
|
|
|
|
/*
|
|
================
|
|
idCollisionModelManagerLocal::BoundsForRotation
|
|
|
|
only for rotations < 180 degrees
|
|
================
|
|
*/
|
|
void idCollisionModelManagerLocal::BoundsForRotation( const idVec3 &origin, const idVec3 &axis, const idVec3 &start, const idVec3 &end, idBounds &bounds ) {
|
|
int i;
|
|
float radiusSqr;
|
|
idVec3 v1, v2;
|
|
|
|
radiusSqr = ( start - origin ).LengthSqr();
|
|
v1 = ( start - origin ).Cross( axis );
|
|
v2 = ( end - origin ).Cross( axis );
|
|
|
|
for ( i = 0; i < 3; i++ ) {
|
|
// if the derivative changes sign along this axis during the rotation from start to end
|
|
if ( ( v1[i] > 0.0f && v2[i] < 0.0f ) || ( v1[i] < 0.0f && v2[i] > 0.0f ) ) {
|
|
if ( ( 0.5f * (start[i] + end[i]) - origin[i] ) > 0.0f ) {
|
|
bounds[0][i] = Min( start[i], end[i] );
|
|
bounds[1][i] = origin[i] + idMath::Sqrt( radiusSqr * ( 1.0f - axis[i] * axis[i] ) );
|
|
}
|
|
else {
|
|
bounds[0][i] = origin[i] - idMath::Sqrt( radiusSqr * ( 1.0f - axis[i] * axis[i] ) );
|
|
bounds[1][i] = Max( start[i], end[i] );
|
|
}
|
|
}
|
|
else if ( start[i] > end[i] ) {
|
|
bounds[0][i] = end[i];
|
|
bounds[1][i] = start[i];
|
|
}
|
|
else {
|
|
bounds[0][i] = start[i];
|
|
bounds[1][i] = end[i];
|
|
}
|
|
// expand for epsilons
|
|
bounds[0][i] -= CM_BOX_EPSILON;
|
|
bounds[1][i] += CM_BOX_EPSILON;
|
|
}
|
|
}
|
|
|
|
/*
|
|
================
|
|
idCollisionModelManagerLocal::Rotation180
|
|
================
|
|
*/
|
|
void idCollisionModelManagerLocal::Rotation180( trace_t *results, const idVec3 &rorg, const idVec3 &axis,
|
|
const float startAngle, const float endAngle, const idVec3 &start,
|
|
const idTraceModel *trm, const idMat3 &trmAxis, int contentMask,
|
|
cmHandle_t model, const idVec3 &modelOrigin, const idMat3 &modelAxis ) {
|
|
int i, j, edgeNum;
|
|
float d, maxErr, initialTan;
|
|
bool model_rotated, trm_rotated;
|
|
idVec3 dir, dir1, dir2, tmp, vr, vup, org, at, bt;
|
|
idMat3 invModelAxis, endAxis, tmpAxis;
|
|
idRotation startRotation, endRotation;
|
|
idPluecker plaxis;
|
|
cm_trmPolygon_t *poly;
|
|
cm_trmEdge_t *edge;
|
|
cm_trmVertex_t *vert;
|
|
ALIGN16( static cm_traceWork_t tw );
|
|
|
|
if ( model < 0 || model > MAX_SUBMODELS || model > idCollisionModelManagerLocal::maxModels ) {
|
|
common->Printf("idCollisionModelManagerLocal::Rotation180: invalid model handle\n");
|
|
return;
|
|
}
|
|
if ( !idCollisionModelManagerLocal::models[model] ) {
|
|
common->Printf("idCollisionModelManagerLocal::Rotation180: invalid model\n");
|
|
return;
|
|
}
|
|
|
|
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 = true;
|
|
tw.positionTest = false;
|
|
tw.axisIntersectsTrm = false;
|
|
tw.quickExit = false;
|
|
tw.angle = endAngle - startAngle;
|
|
assert( tw.angle > -180.0f && tw.angle < 180.0f );
|
|
tw.maxTan = initialTan = idMath::Fabs( tan( ( idMath::PI / 360.0f ) * tw.angle ) );
|
|
tw.model = idCollisionModelManagerLocal::models[model];
|
|
tw.start = start - modelOrigin;
|
|
// rotation axis, axis is assumed to be normalized
|
|
tw.axis = axis;
|
|
assert( tw.axis[0] * tw.axis[0] + tw.axis[1] * tw.axis[1] + tw.axis[2] * tw.axis[2] > 0.99f );
|
|
// rotation origin projected into rotation plane through tw.start
|
|
tw.origin = rorg - modelOrigin;
|
|
d = (tw.axis * tw.origin) - ( tw.axis * tw.start );
|
|
tw.origin = tw.origin - d * tw.axis;
|
|
// radius of rotation
|
|
tw.radius = ( tw.start - tw.origin ).Length();
|
|
// maximum error of the circle approximation traced through the axial BSP tree
|
|
d = tw.radius * tw.radius - (CIRCLE_APPROXIMATION_LENGTH*CIRCLE_APPROXIMATION_LENGTH*0.25f);
|
|
if ( d > 0.0f ) {
|
|
maxErr = tw.radius - idMath::Sqrt( d );
|
|
} else {
|
|
maxErr = tw.radius;
|
|
}
|
|
|
|
model_rotated = modelAxis.IsRotated();
|
|
if ( model_rotated ) {
|
|
invModelAxis = modelAxis.Transpose();
|
|
tw.axis *= invModelAxis;
|
|
tw.origin *= invModelAxis;
|
|
}
|
|
|
|
startRotation.Set( tw.origin, tw.axis, startAngle );
|
|
endRotation.Set( tw.origin, tw.axis, endAngle );
|
|
|
|
// create matrix which rotates the rotation axis to the z-axis
|
|
tw.axis.NormalVectors( vr, vup );
|
|
tw.matrix[0][0] = vr[0];
|
|
tw.matrix[1][0] = vr[1];
|
|
tw.matrix[2][0] = vr[2];
|
|
tw.matrix[0][1] = -vup[0];
|
|
tw.matrix[1][1] = -vup[1];
|
|
tw.matrix[2][1] = -vup[2];
|
|
tw.matrix[0][2] = tw.axis[0];
|
|
tw.matrix[1][2] = tw.axis[1];
|
|
tw.matrix[2][2] = tw.axis[2];
|
|
|
|
// 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 = tw.start;
|
|
// if we start at a specific angle
|
|
if ( startAngle != 0.0f ) {
|
|
startRotation.RotatePoint( tw.start );
|
|
}
|
|
// calculate end position of rotation
|
|
endRotation.RotatePoint( tw.end );
|
|
|
|
// calculate rotation origin projected into rotation plane through the vertex
|
|
tw.numVerts = 1;
|
|
tw.vertices[0].p = tw.start;
|
|
tw.vertices[0].endp = tw.end;
|
|
tw.vertices[0].used = true;
|
|
tw.vertices[0].rotationOrigin = tw.origin + tw.axis * ( tw.axis * ( tw.vertices[0].p - tw.origin ) );
|
|
BoundsForRotation( tw.vertices[0].rotationOrigin, tw.axis, tw.start, tw.end, tw.vertices[0].rotationBounds );
|
|
// rotation bounds
|
|
tw.bounds = tw.vertices[0].rotationBounds;
|
|
tw.numEdges = tw.numPolys = 0;
|
|
|
|
// collision with single point
|
|
tw.pointTrace = true;
|
|
|
|
// extents is set to maximum error of the circle approximation traced through the axial BSP tree
|
|
tw.extents[0] = tw.extents[1] = tw.extents[2] = maxErr + CM_BOX_EPSILON;
|
|
|
|
// setup rotation heart plane
|
|
tw.heartPlane1.SetNormal( tw.axis );
|
|
tw.heartPlane1.FitThroughPoint( tw.start );
|
|
tw.maxDistFromHeartPlane1 = CM_BOX_EPSILON;
|
|
|
|
// trace through the model
|
|
idCollisionModelManagerLocal::TraceThroughModel( &tw );
|
|
|
|
// store results
|
|
*results = tw.trace;
|
|
results->endpos = start;
|
|
if ( tw.maxTan == initialTan ) {
|
|
results->fraction = 1.0f;
|
|
} else {
|
|
results->fraction = idMath::Fabs( atan( tw.maxTan ) * ( 2.0f * 180.0f / idMath::PI ) / tw.angle );
|
|
}
|
|
assert( results->fraction <= 1.0f );
|
|
endRotation.Set( rorg, axis, startAngle + (endAngle-startAngle) * results->fraction );
|
|
endRotation.RotatePoint( results->endpos );
|
|
results->endAxis.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;
|
|
}
|
|
return;
|
|
}
|
|
|
|
tw.pointTrace = false;
|
|
|
|
// 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++ ) {
|
|
tw.vertices[i].p *= invModelAxis;
|
|
}
|
|
}
|
|
for ( i = 0; i < tw.numVerts; i++ ) {
|
|
tw.vertices[i].endp = tw.vertices[i].p;
|
|
}
|
|
// if we start at a specific angle
|
|
if ( startAngle != 0.0f ) {
|
|
for ( i = 0; i < tw.numVerts; i++ ) {
|
|
startRotation.RotatePoint( tw.vertices[i].p );
|
|
}
|
|
}
|
|
for ( i = 0; i < tw.numVerts; i++ ) {
|
|
// end position of vertex
|
|
endRotation.RotatePoint( tw.vertices[i].endp );
|
|
}
|
|
|
|
// add offset to start point
|
|
if ( trm_rotated ) {
|
|
tw.start += trm->offset * trmAxis;
|
|
} else {
|
|
tw.start += trm->offset;
|
|
}
|
|
// if the model is rotated
|
|
if ( model_rotated ) {
|
|
// rotate trace instead of model
|
|
tw.start *= invModelAxis;
|
|
}
|
|
tw.end = tw.start;
|
|
// if we start at a specific angle
|
|
if ( startAngle != 0.0f ) {
|
|
startRotation.RotatePoint( tw.start );
|
|
}
|
|
// calculate end position of rotation
|
|
endRotation.RotatePoint( tw.end );
|
|
|
|
// setup trm vertices
|
|
for ( vert = tw.vertices, i = 0; i < tw.numVerts; i++, vert++ ) {
|
|
// calculate rotation origin projected into rotation plane through the vertex
|
|
vert->rotationOrigin = tw.origin + tw.axis * ( tw.axis * ( vert->p - tw.origin ) );
|
|
// calculate rotation bounds for this vertex
|
|
BoundsForRotation( vert->rotationOrigin, tw.axis, vert->p, vert->endp, vert->rotationBounds );
|
|
// if the rotation axis goes through the vertex then the vertex is not used
|
|
d = ( vert->p - vert->rotationOrigin ).LengthSqr();
|
|
if ( d > ROTATION_AXIS_EPSILON * ROTATION_AXIS_EPSILON ) {
|
|
vert->used = true;
|
|
}
|
|
}
|
|
|
|
// setup trm edges
|
|
for ( edge = tw.edges + 1, i = 1; i <= tw.numEdges; i++, edge++ ) {
|
|
// if the rotation axis goes through both the edge vertices then the edge is not used
|
|
if ( tw.vertices[edge->vertexNum[0]].used | tw.vertices[edge->vertexNum[1]].used ) {
|
|
edge->used = true;
|
|
}
|
|
// 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 );
|
|
// pluecker coordinate for edge being rotated about the z-axis
|
|
at = ( edge->start - tw.origin ) * tw.matrix;
|
|
bt = ( edge->end - tw.origin ) * tw.matrix;
|
|
edge->plzaxis.FromLine( at, bt );
|
|
// get edge rotation bounds from the rotation bounds of both vertices
|
|
edge->rotationBounds = tw.vertices[edge->vertexNum[0]].rotationBounds;
|
|
edge->rotationBounds.AddBounds( tw.vertices[edge->vertexNum[1]].rotationBounds );
|
|
// used to calculate if the rotation axis intersects the trm
|
|
edge->bitNum = 0;
|
|
}
|
|
|
|
tw.bounds.Clear();
|
|
|
|
// 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++ ) {
|
|
poly->used = true;
|
|
// set trm polygon plane distance
|
|
poly->plane.FitThroughPoint( tw.edges[abs(poly->edges[0])].start );
|
|
// get polygon bounds from edge bounds
|
|
poly->rotationBounds.Clear();
|
|
for ( j = 0; j < poly->numEdges; j++ ) {
|
|
// add edge rotation bounds to polygon rotation bounds
|
|
edge = &tw.edges[abs( poly->edges[j] )];
|
|
poly->rotationBounds.AddBounds( edge->rotationBounds );
|
|
}
|
|
// get trace bounds from polygon bounds
|
|
tw.bounds.AddBounds( poly->rotationBounds );
|
|
}
|
|
|
|
// extents including the maximum error of the circle approximation traced through the axial BSP tree
|
|
for ( i = 0; i < 3; i++ ) {
|
|
tw.size[0][i] = tw.bounds[0][i] - tw.start[i];
|
|
tw.size[1][i] = tw.bounds[1][i] - tw.start[i];
|
|
if ( idMath::Fabs( tw.size[0][i] ) > idMath::Fabs( tw.size[1][i] ) ) {
|
|
tw.extents[i] = idMath::Fabs( tw.size[0][i] ) + maxErr + CM_BOX_EPSILON;
|
|
} else {
|
|
tw.extents[i] = idMath::Fabs( tw.size[1][i] ) + maxErr + CM_BOX_EPSILON;
|
|
}
|
|
}
|
|
|
|
// for back-face culling
|
|
if ( tw.isConvex ) {
|
|
if ( tw.start == tw.origin ) {
|
|
tw.axisIntersectsTrm = true;
|
|
} else {
|
|
// determine if the rotation axis intersects the trm
|
|
plaxis.FromRay( tw.origin, tw.axis );
|
|
for ( poly = tw.polys, i = 0; i < tw.numPolys; i++, poly++ ) {
|
|
// back face cull polygons
|
|
if ( poly->plane.Normal() * tw.axis > 0.0f ) {
|
|
continue;
|
|
}
|
|
// test if the axis goes between the polygon edges
|
|
for ( j = 0; j < poly->numEdges; j++ ) {
|
|
edgeNum = poly->edges[j];
|
|
edge = tw.edges + abs(edgeNum);
|
|
if ( !(edge->bitNum & 2) ) {
|
|
d = plaxis.PermutedInnerProduct( edge->pl );
|
|
edge->bitNum = FLOATSIGNBITSET( d ) | 2;
|
|
}
|
|
if ( ( edge->bitNum ^ INTSIGNBITSET( edgeNum ) ) & 1 ) {
|
|
break;
|
|
}
|
|
}
|
|
if ( j >= poly->numEdges ) {
|
|
tw.axisIntersectsTrm = true;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
// setup rotation heart plane
|
|
tw.heartPlane1.SetNormal( tw.axis );
|
|
tw.heartPlane1.FitThroughPoint( tw.start );
|
|
tw.maxDistFromHeartPlane1 = 0.0f;
|
|
for ( i = 0; i < tw.numVerts; i++ ) {
|
|
d = idMath::Fabs( tw.heartPlane1.Distance( tw.vertices[i].p ) );
|
|
if ( d > tw.maxDistFromHeartPlane1 ) {
|
|
tw.maxDistFromHeartPlane1 = d;
|
|
}
|
|
}
|
|
tw.maxDistFromHeartPlane1 += CM_BOX_EPSILON;
|
|
|
|
// inverse rotation to rotate model vertices towards trace model
|
|
tw.modelVertexRotation.Set( tw.origin, tw.axis, -tw.angle );
|
|
|
|
// trace through the model
|
|
idCollisionModelManagerLocal::TraceThroughModel( &tw );
|
|
|
|
// store results
|
|
*results = tw.trace;
|
|
results->endpos = start;
|
|
if ( tw.maxTan == initialTan ) {
|
|
results->fraction = 1.0f;
|
|
} else {
|
|
results->fraction = idMath::Fabs( atan( tw.maxTan ) * ( 2.0f * 180.0f / idMath::PI ) / tw.angle );
|
|
}
|
|
assert( results->fraction <= 1.0f );
|
|
endRotation.Set( rorg, axis, startAngle + (endAngle-startAngle) * results->fraction );
|
|
endRotation.RotatePoint( results->endpos );
|
|
results->endAxis = trmAxis * endRotation.ToMat3();
|
|
|
|
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::Rotation
|
|
================
|
|
*/
|
|
void idCollisionModelManagerLocal::Rotation( trace_t *results, const idVec3 &start, const idRotation &rotation,
|
|
const idTraceModel *trm, const idMat3 &trmAxis, int contentMask,
|
|
cmHandle_t model, const idVec3 &modelOrigin, const idMat3 &modelAxis ) {
|
|
idVec3 tmp;
|
|
float maxa, stepa, a, lasta;
|
|
|
|
assert( ((byte *)&start) < ((byte *)results) || ((byte *)&start) > (((byte *)results) + sizeof( trace_t )) );
|
|
assert( ((byte *)&trmAxis) < ((byte *)results) || ((byte *)&trmAxis) > (((byte *)results) + sizeof( trace_t )) );
|
|
|
|
memset( results, 0, sizeof( *results ) );
|
|
|
|
// if special position test
|
|
if ( rotation.GetAngle() == 0.0f ) {
|
|
idCollisionModelManagerLocal::ContentsTrm( results, start, trm, trmAxis, contentMask, model, modelOrigin, modelAxis );
|
|
return;
|
|
}
|
|
|
|
if ( rotation.GetAngle() >= 180.0f || rotation.GetAngle() <= -180.0f) {
|
|
if ( rotation.GetAngle() >= 360.0f ) {
|
|
maxa = 360.0f;
|
|
stepa = 120.0f; // three steps strictly < 180 degrees
|
|
} else if ( rotation.GetAngle() <= -360.0f ) {
|
|
maxa = -360.0f;
|
|
stepa = -120.0f; // three steps strictly < 180 degrees
|
|
} else {
|
|
maxa = rotation.GetAngle();
|
|
stepa = rotation.GetAngle() * 0.5f; // two steps strictly < 180 degrees
|
|
}
|
|
for ( lasta = 0.0f, a = stepa; fabs( a ) < fabs( maxa ) + 1.0f; lasta = a, a += stepa ) {
|
|
// partial rotation
|
|
idCollisionModelManagerLocal::Rotation180( results, rotation.GetOrigin(), rotation.GetVec(), lasta, a, start, trm, trmAxis, contentMask, model, modelOrigin, modelAxis );
|
|
// if there is a collision
|
|
if ( results->fraction < 1.0f ) {
|
|
// fraction of total rotation
|
|
results->fraction = (lasta + stepa * results->fraction) / rotation.GetAngle();
|
|
return;
|
|
}
|
|
}
|
|
results->fraction = 1.0f;
|
|
return;
|
|
}
|
|
|
|
idCollisionModelManagerLocal::Rotation180( results, rotation.GetOrigin(), rotation.GetVec(), 0.0f, rotation.GetAngle(), start, trm, trmAxis, contentMask, model, modelOrigin, modelAxis );
|
|
|
|
#ifdef _DEBUG
|
|
// test for collisions
|
|
if ( cm_debugCollision.GetBool() ) {
|
|
// if the trm is stuck in the model
|
|
if ( idCollisionModelManagerLocal::Contents( results->endpos, trm, results->endAxis, -1, model, modelOrigin, modelAxis ) & contentMask ) {
|
|
trace_t tr;
|
|
|
|
// test where the trm is stuck in the model
|
|
idCollisionModelManagerLocal::Contents( results->endpos, trm, results->endAxis, -1, model, modelOrigin, modelAxis );
|
|
// re-run collision detection to find out where it failed
|
|
idCollisionModelManagerLocal::Rotation( &tr, start, rotation, trm, trmAxis, contentMask, model, modelOrigin, modelAxis );
|
|
}
|
|
}
|
|
#endif
|
|
}
|