123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816817818819820821822823824825826827828829830831832833834835836837838839840841842843844845846847848849850851852853854855856857858859860861862863864865866867868869870871872873874875876877878879880881882883884885886887888889890891892893894895896897898899900901902903904905906907908909910911912913914915916917918919920921922923924925926927928929930931932933934935936937938939940941942943944945946947948949950951952953954955956957958959960961962963964965966967968969970971972973974975976977978979980981982983984985986987988989990991992993994995996997998999100010011002100310041005100610071008100910101011101210131014101510161017101810191020102110221023102410251026102710281029103010311032103310341035103610371038103910401041104210431044104510461047104810491050105110521053105410551056105710581059106010611062106310641065106610671068106910701071107210731074107510761077107810791080108110821083108410851086108710881089109010911092109310941095109610971098109911001101110211031104110511061107110811091110111111121113111411151116111711181119112011211122112311241125112611271128112911301131113211331134113511361137113811391140114111421143114411451146114711481149115011511152115311541155115611571158115911601161116211631164116511661167116811691170117111721173117411751176117711781179118011811182118311841185118611871188118911901191119211931194119511961197119811991200120112021203120412051206120712081209121012111212121312141215121612171218121912201221122212231224122512261227122812291230123112321233123412351236123712381239124012411242124312441245124612471248124912501251125212531254125512561257125812591260126112621263126412651266126712681269127012711272127312741275127612771278127912801281128212831284128512861287128812891290129112921293129412951296129712981299130013011302130313041305130613071308130913101311131213131314131513161317131813191320132113221323132413251326132713281329133013311332133313341335133613371338133913401341134213431344134513461347134813491350135113521353135413551356135713581359136013611362136313641365136613671368136913701371137213731374137513761377137813791380138113821383138413851386138713881389139013911392139313941395139613971398139914001401140214031404140514061407140814091410141114121413141414151416141714181419142014211422142314241425142614271428142914301431143214331434143514361437143814391440144114421443144414451446144714481449145014511452145314541455145614571458145914601461146214631464146514661467146814691470147114721473147414751476147714781479148014811482148314841485148614871488148914901491149214931494149514961497149814991500150115021503150415051506150715081509151015111512151315141515151615171518151915201521152215231524152515261527152815291530153115321533153415351536153715381539154015411542154315441545154615471548154915501551155215531554155515561557155815591560156115621563156415651566156715681569157015711572157315741575157615771578157915801581158215831584158515861587158815891590159115921593159415951596159715981599160016011602160316041605160616071608160916101611161216131614161516161617161816191620162116221623162416251626162716281629163016311632163316341635163616371638163916401641164216431644164516461647164816491650165116521653165416551656165716581659166016611662166316641665166616671668166916701671167216731674167516761677167816791680168116821683168416851686168716881689169016911692169316941695 |
- /*
- ===========================================================================
- 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
- }
|