mirror of
https://github.com/dhewm/dhewm3.git
synced 2024-11-30 16:11:11 +00:00
1695 lines
49 KiB
C++
1695 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 "../idlib/precompiled.h"
|
||
|
|
#pragma hdrstop
|
||
|
|
|
||
|
|
#include "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
|
||
|
|
================
|
||
|
|
*/
|
||
|
|
#ifdef _DEBUG
|
||
|
|
static int entered = 0;
|
||
|
|
#endif
|
||
|
|
|
||
|
|
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;
|
||
|
|
}
|
||
|
|
|
||
|
|
#ifdef _DEBUG
|
||
|
|
bool startsolid = false;
|
||
|
|
// test whether or not stuck to begin with
|
||
|
|
if ( cm_debugCollision.GetBool() ) {
|
||
|
|
if ( !entered ) {
|
||
|
|
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
|
||
|
|
|
||
|
|
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 missed collisions
|
||
|
|
if ( cm_debugCollision.GetBool() ) {
|
||
|
|
if ( !entered ) {
|
||
|
|
entered = 1;
|
||
|
|
// 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 );
|
||
|
|
}
|
||
|
|
entered = 0;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
#endif
|
||
|
|
}
|