diff options
Diffstat (limited to 'src/qcommon/cm_trace.c')
| -rw-r--r-- | src/qcommon/cm_trace.c | 1473 |
1 files changed, 1473 insertions, 0 deletions
diff --git a/src/qcommon/cm_trace.c b/src/qcommon/cm_trace.c new file mode 100644 index 00000000..bc52c289 --- /dev/null +++ b/src/qcommon/cm_trace.c @@ -0,0 +1,1473 @@ +/* +=========================================================================== +Copyright (C) 1999-2005 Id Software, Inc. + +This file is part of Quake III Arena source code. + +Quake III Arena 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 2 of the License, +or (at your option) any later version. + +Quake III Arena 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 Quake III Arena source code; if not, write to the Free Software +Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA +=========================================================================== +*/ +#include "cm_local.h" + +// always use bbox vs. bbox collision and never capsule vs. bbox or vice versa +//#define ALWAYS_BBOX_VS_BBOX +// always use capsule vs. capsule collision and never capsule vs. bbox or vice versa +//#define ALWAYS_CAPSULE_VS_CAPSULE + +//#define CAPSULE_DEBUG + +/* +=============================================================================== + +BASIC MATH + +=============================================================================== +*/ + +/* +================ +RotatePoint +================ +*/ +void RotatePoint(vec3_t point, /*const*/ vec3_t matrix[3]) { // bk: FIXME + vec3_t tvec; + + VectorCopy(point, tvec); + point[0] = DotProduct(matrix[0], tvec); + point[1] = DotProduct(matrix[1], tvec); + point[2] = DotProduct(matrix[2], tvec); +} + +/* +================ +TransposeMatrix +================ +*/ +void TransposeMatrix(/*const*/ vec3_t matrix[3], vec3_t transpose[3]) { // bk: FIXME + int i, j; + for (i = 0; i < 3; i++) { + for (j = 0; j < 3; j++) { + transpose[i][j] = matrix[j][i]; + } + } +} + +/* +================ +CreateRotationMatrix +================ +*/ +void CreateRotationMatrix(const vec3_t angles, vec3_t matrix[3]) { + AngleVectors(angles, matrix[0], matrix[1], matrix[2]); + VectorInverse(matrix[1]); +} + +/* +================ +CM_ProjectPointOntoVector +================ +*/ +void CM_ProjectPointOntoVector( vec3_t point, vec3_t vStart, vec3_t vDir, vec3_t vProj ) +{ + vec3_t pVec; + + VectorSubtract( point, vStart, pVec ); + // project onto the directional vector for this segment + VectorMA( vStart, DotProduct( pVec, vDir ), vDir, vProj ); +} + +/* +================ +CM_DistanceFromLineSquared +================ +*/ +float CM_DistanceFromLineSquared(vec3_t p, vec3_t lp1, vec3_t lp2, vec3_t dir) { + vec3_t proj, t; + int j; + + CM_ProjectPointOntoVector(p, lp1, dir, proj); + for (j = 0; j < 3; j++) + if ((proj[j] > lp1[j] && proj[j] > lp2[j]) || + (proj[j] < lp1[j] && proj[j] < lp2[j])) + break; + if (j < 3) { + if (fabs(proj[j] - lp1[j]) < fabs(proj[j] - lp2[j])) + VectorSubtract(p, lp1, t); + else + VectorSubtract(p, lp2, t); + return VectorLengthSquared(t); + } + VectorSubtract(p, proj, t); + return VectorLengthSquared(t); +} + +/* +================ +CM_VectorDistanceSquared +================ +*/ +float CM_VectorDistanceSquared(vec3_t p1, vec3_t p2) { + vec3_t dir; + + VectorSubtract(p2, p1, dir); + return VectorLengthSquared(dir); +} + +/* +================ +SquareRootFloat +================ +*/ +float SquareRootFloat(float number) { + union { + float f; + int i; + } t; + float x, y; + const float f = 1.5F; + + x = number * 0.5F; + t.f = number; + t.i = 0x5f3759df - ( t.i >> 1 ); + y = t.f; + y = y * ( f - ( x * y * y ) ); + y = y * ( f - ( x * y * y ) ); + return number * y; +} + + +/* +=============================================================================== + +POSITION TESTING + +=============================================================================== +*/ + +/* +================ +CM_TestBoxInBrush +================ +*/ +void CM_TestBoxInBrush( traceWork_t *tw, cbrush_t *brush ) { + int i; + cplane_t *plane; + float dist; + float d1; + cbrushside_t *side; + float t; + vec3_t startp; + + if (!brush->numsides) { + return; + } + + // special test for axial + if ( tw->bounds[0][0] > brush->bounds[1][0] + || tw->bounds[0][1] > brush->bounds[1][1] + || tw->bounds[0][2] > brush->bounds[1][2] + || tw->bounds[1][0] < brush->bounds[0][0] + || tw->bounds[1][1] < brush->bounds[0][1] + || tw->bounds[1][2] < brush->bounds[0][2] + ) { + return; + } + + if ( tw->sphere.use ) { + // the first six planes are the axial planes, so we only + // need to test the remainder + for ( i = 6 ; i < brush->numsides ; i++ ) { + side = brush->sides + i; + plane = side->plane; + + // adjust the plane distance apropriately for radius + dist = plane->dist + tw->sphere.radius; + // find the closest point on the capsule to the plane + t = DotProduct( plane->normal, tw->sphere.offset ); + if ( t > 0 ) + { + VectorSubtract( tw->start, tw->sphere.offset, startp ); + } + else + { + VectorAdd( tw->start, tw->sphere.offset, startp ); + } + d1 = DotProduct( startp, plane->normal ) - dist; + // if completely in front of face, no intersection + if ( d1 > 0 ) { + return; + } + } + } else { + // the first six planes are the axial planes, so we only + // need to test the remainder + for ( i = 6 ; i < brush->numsides ; i++ ) { + side = brush->sides + i; + plane = side->plane; + + // adjust the plane distance apropriately for mins/maxs + dist = plane->dist - DotProduct( tw->offsets[ plane->signbits ], plane->normal ); + + d1 = DotProduct( tw->start, plane->normal ) - dist; + + // if completely in front of face, no intersection + if ( d1 > 0 ) { + return; + } + } + } + + // inside this brush + tw->trace.startsolid = tw->trace.allsolid = qtrue; + tw->trace.fraction = 0; + tw->trace.contents = brush->contents; +} + + + +/* +================ +CM_TestInLeaf +================ +*/ +void CM_TestInLeaf( traceWork_t *tw, cLeaf_t *leaf ) { + int k; + int brushnum; + cbrush_t *b; + cPatch_t *patch; + + // test box position against all brushes in the leaf + for (k=0 ; k<leaf->numLeafBrushes ; k++) { + brushnum = cm.leafbrushes[leaf->firstLeafBrush+k]; + b = &cm.brushes[brushnum]; + if (b->checkcount == cm.checkcount) { + continue; // already checked this brush in another leaf + } + b->checkcount = cm.checkcount; + + if ( !(b->contents & tw->contents)) { + continue; + } + + CM_TestBoxInBrush( tw, b ); + if ( tw->trace.allsolid ) { + return; + } + } + + // test against all patches +#ifdef BSPC + if (1) { +#else + if ( !cm_noCurves->integer ) { +#endif //BSPC + for ( k = 0 ; k < leaf->numLeafSurfaces ; k++ ) { + patch = cm.surfaces[ cm.leafsurfaces[ leaf->firstLeafSurface + k ] ]; + if ( !patch ) { + continue; + } + if ( patch->checkcount == cm.checkcount ) { + continue; // already checked this brush in another leaf + } + patch->checkcount = cm.checkcount; + + if ( !(patch->contents & tw->contents)) { + continue; + } + + if ( CM_PositionTestInPatchCollide( tw, patch->pc ) ) { + tw->trace.startsolid = tw->trace.allsolid = qtrue; + tw->trace.fraction = 0; + tw->trace.contents = patch->contents; + return; + } + } + } +} + +/* +================== +CM_TestCapsuleInCapsule + +capsule inside capsule check +================== +*/ +void CM_TestCapsuleInCapsule( traceWork_t *tw, clipHandle_t model ) { + int i; + vec3_t mins, maxs; + vec3_t top, bottom; + vec3_t p1, p2, tmp; + vec3_t offset, symetricSize[2]; + float radius, halfwidth, halfheight, offs, r; + + CM_ModelBounds(model, mins, maxs); + + VectorAdd(tw->start, tw->sphere.offset, top); + VectorSubtract(tw->start, tw->sphere.offset, bottom); + for ( i = 0 ; i < 3 ; i++ ) { + offset[i] = ( mins[i] + maxs[i] ) * 0.5; + symetricSize[0][i] = mins[i] - offset[i]; + symetricSize[1][i] = maxs[i] - offset[i]; + } + halfwidth = symetricSize[ 1 ][ 0 ]; + halfheight = symetricSize[ 1 ][ 2 ]; + radius = ( halfwidth > halfheight ) ? halfheight : halfwidth; + offs = halfheight - radius; + + r = Square(tw->sphere.radius + radius); + // check if any of the spheres overlap + VectorCopy(offset, p1); + p1[2] += offs; + VectorSubtract(p1, top, tmp); + if ( VectorLengthSquared(tmp) < r ) { + tw->trace.startsolid = tw->trace.allsolid = qtrue; + tw->trace.fraction = 0; + } + VectorSubtract(p1, bottom, tmp); + if ( VectorLengthSquared(tmp) < r ) { + tw->trace.startsolid = tw->trace.allsolid = qtrue; + tw->trace.fraction = 0; + } + VectorCopy(offset, p2); + p2[2] -= offs; + VectorSubtract(p2, top, tmp); + if ( VectorLengthSquared(tmp) < r ) { + tw->trace.startsolid = tw->trace.allsolid = qtrue; + tw->trace.fraction = 0; + } + VectorSubtract(p2, bottom, tmp); + if ( VectorLengthSquared(tmp) < r ) { + tw->trace.startsolid = tw->trace.allsolid = qtrue; + tw->trace.fraction = 0; + } + // if between cylinder up and lower bounds + if ( (top[2] >= p1[2] && top[2] <= p2[2]) || + (bottom[2] >= p1[2] && bottom[2] <= p2[2]) ) { + // 2d coordinates + top[2] = p1[2] = 0; + // if the cylinders overlap + VectorSubtract(top, p1, tmp); + if ( VectorLengthSquared(tmp) < r ) { + tw->trace.startsolid = tw->trace.allsolid = qtrue; + tw->trace.fraction = 0; + } + } +} + +/* +================== +CM_TestBoundingBoxInCapsule + +bounding box inside capsule check +================== +*/ +void CM_TestBoundingBoxInCapsule( traceWork_t *tw, clipHandle_t model ) { + vec3_t mins, maxs, offset, size[2]; + clipHandle_t h; + cmodel_t *cmod; + int i; + + // mins maxs of the capsule + CM_ModelBounds(model, mins, maxs); + + // offset for capsule center + for ( i = 0 ; i < 3 ; i++ ) { + offset[i] = ( mins[i] + maxs[i] ) * 0.5; + size[0][i] = mins[i] - offset[i]; + size[1][i] = maxs[i] - offset[i]; + tw->start[i] -= offset[i]; + tw->end[i] -= offset[i]; + } + + // replace the bounding box with the capsule + tw->sphere.use = qtrue; + tw->sphere.radius = ( size[1][0] > size[1][2] ) ? size[1][2]: size[1][0]; + tw->sphere.halfheight = size[1][2]; + VectorSet( tw->sphere.offset, 0, 0, size[1][2] - tw->sphere.radius ); + + // replace the capsule with the bounding box + h = CM_TempBoxModel(tw->size[0], tw->size[1], qfalse); + // calculate collision + cmod = CM_ClipHandleToModel( h ); + CM_TestInLeaf( tw, &cmod->leaf ); +} + +/* +================== +CM_PositionTest +================== +*/ +#define MAX_POSITION_LEAFS 1024 +void CM_PositionTest( traceWork_t *tw ) { + int leafs[MAX_POSITION_LEAFS]; + int i; + leafList_t ll; + + // identify the leafs we are touching + VectorAdd( tw->start, tw->size[0], ll.bounds[0] ); + VectorAdd( tw->start, tw->size[1], ll.bounds[1] ); + + for (i=0 ; i<3 ; i++) { + ll.bounds[0][i] -= 1; + ll.bounds[1][i] += 1; + } + + ll.count = 0; + ll.maxcount = MAX_POSITION_LEAFS; + ll.list = leafs; + ll.storeLeafs = CM_StoreLeafs; + ll.lastLeaf = 0; + ll.overflowed = qfalse; + + cm.checkcount++; + + CM_BoxLeafnums_r( &ll, 0 ); + + + cm.checkcount++; + + // test the contents of the leafs + for (i=0 ; i < ll.count ; i++) { + CM_TestInLeaf( tw, &cm.leafs[leafs[i]] ); + if ( tw->trace.allsolid ) { + break; + } + } +} + +/* +=============================================================================== + +TRACING + +=============================================================================== +*/ + + +/* +================ +CM_TraceThroughPatch +================ +*/ + +void CM_TraceThroughPatch( traceWork_t *tw, cPatch_t *patch ) { + float oldFrac; + + c_patch_traces++; + + oldFrac = tw->trace.fraction; + + CM_TraceThroughPatchCollide( tw, patch->pc ); + + if ( tw->trace.fraction < oldFrac ) { + tw->trace.surfaceFlags = patch->surfaceFlags; + tw->trace.contents = patch->contents; + } +} + +/* +================ +CM_TraceThroughBrush +================ +*/ +void CM_TraceThroughBrush( traceWork_t *tw, cbrush_t *brush ) { + int i; + cplane_t *plane, *clipplane; + float dist; + float enterFrac, leaveFrac; + float d1, d2; + qboolean getout, startout; + float f; + cbrushside_t *side, *leadside; + float t; + vec3_t startp; + vec3_t endp; + + enterFrac = -1.0; + leaveFrac = 1.0; + clipplane = NULL; + + if ( !brush->numsides ) { + return; + } + + c_brush_traces++; + + getout = qfalse; + startout = qfalse; + + leadside = NULL; + + if ( tw->sphere.use ) { + // + // compare the trace against all planes of the brush + // find the latest time the trace crosses a plane towards the interior + // and the earliest time the trace crosses a plane towards the exterior + // + for (i = 0; i < brush->numsides; i++) { + side = brush->sides + i; + plane = side->plane; + + // adjust the plane distance apropriately for radius + dist = plane->dist + tw->sphere.radius; + + // find the closest point on the capsule to the plane + t = DotProduct( plane->normal, tw->sphere.offset ); + if ( t > 0 ) + { + VectorSubtract( tw->start, tw->sphere.offset, startp ); + VectorSubtract( tw->end, tw->sphere.offset, endp ); + } + else + { + VectorAdd( tw->start, tw->sphere.offset, startp ); + VectorAdd( tw->end, tw->sphere.offset, endp ); + } + + d1 = DotProduct( startp, plane->normal ) - dist; + d2 = DotProduct( endp, plane->normal ) - dist; + + if (d2 > 0) { + getout = qtrue; // endpoint is not in solid + } + if (d1 > 0) { + startout = qtrue; + } + + // if completely in front of face, no intersection with the entire brush + if (d1 > 0 && ( d2 >= SURFACE_CLIP_EPSILON || d2 >= d1 ) ) { + return; + } + + // if it doesn't cross the plane, the plane isn't relevent + if (d1 <= 0 && d2 <= 0 ) { + continue; + } + + // crosses face + if (d1 > d2) { // enter + f = (d1-SURFACE_CLIP_EPSILON) / (d1-d2); + if ( f < 0 ) { + f = 0; + } + if (f > enterFrac) { + enterFrac = f; + clipplane = plane; + leadside = side; + } + } else { // leave + f = (d1+SURFACE_CLIP_EPSILON) / (d1-d2); + if ( f > 1 ) { + f = 1; + } + if (f < leaveFrac) { + leaveFrac = f; + } + } + } + } else { + // + // compare the trace against all planes of the brush + // find the latest time the trace crosses a plane towards the interior + // and the earliest time the trace crosses a plane towards the exterior + // + for (i = 0; i < brush->numsides; i++) { + side = brush->sides + i; + plane = side->plane; + + // adjust the plane distance apropriately for mins/maxs + dist = plane->dist - DotProduct( tw->offsets[ plane->signbits ], plane->normal ); + + d1 = DotProduct( tw->start, plane->normal ) - dist; + d2 = DotProduct( tw->end, plane->normal ) - dist; + + if (d2 > 0) { + getout = qtrue; // endpoint is not in solid + } + if (d1 > 0) { + startout = qtrue; + } + + // if completely in front of face, no intersection with the entire brush + if (d1 > 0 && ( d2 >= SURFACE_CLIP_EPSILON || d2 >= d1 ) ) { + return; + } + + // if it doesn't cross the plane, the plane isn't relevent + if (d1 <= 0 && d2 <= 0 ) { + continue; + } + + // crosses face + if (d1 > d2) { // enter + f = (d1-SURFACE_CLIP_EPSILON) / (d1-d2); + if ( f < 0 ) { + f = 0; + } + if (f > enterFrac) { + enterFrac = f; + clipplane = plane; + leadside = side; + } + } else { // leave + f = (d1+SURFACE_CLIP_EPSILON) / (d1-d2); + if ( f > 1 ) { + f = 1; + } + if (f < leaveFrac) { + leaveFrac = f; + } + } + } + } + + // + // all planes have been checked, and the trace was not + // completely outside the brush + // + if (!startout) { // original point was inside brush + tw->trace.startsolid = qtrue; + if (!getout) { + tw->trace.allsolid = qtrue; + tw->trace.fraction = 0; + tw->trace.contents = brush->contents; + } + return; + } + + if (enterFrac < leaveFrac) { + if (enterFrac > -1 && enterFrac < tw->trace.fraction) { + if (enterFrac < 0) { + enterFrac = 0; + } + tw->trace.fraction = enterFrac; + tw->trace.plane = *clipplane; + tw->trace.surfaceFlags = leadside->surfaceFlags; + tw->trace.contents = brush->contents; + } + } +} + +/* +================ +CM_TraceThroughLeaf +================ +*/ +void CM_TraceThroughLeaf( traceWork_t *tw, cLeaf_t *leaf ) { + int k; + int brushnum; + cbrush_t *b; + cPatch_t *patch; + + // trace line against all brushes in the leaf + for ( k = 0 ; k < leaf->numLeafBrushes ; k++ ) { + brushnum = cm.leafbrushes[leaf->firstLeafBrush+k]; + + b = &cm.brushes[brushnum]; + if ( b->checkcount == cm.checkcount ) { + continue; // already checked this brush in another leaf + } + b->checkcount = cm.checkcount; + + if ( !(b->contents & tw->contents) ) { + continue; + } + + CM_TraceThroughBrush( tw, b ); + if ( !tw->trace.fraction ) { + return; + } + } + + // trace line against all patches in the leaf +#ifdef BSPC + if (1) { +#else + if ( !cm_noCurves->integer ) { +#endif + for ( k = 0 ; k < leaf->numLeafSurfaces ; k++ ) { + patch = cm.surfaces[ cm.leafsurfaces[ leaf->firstLeafSurface + k ] ]; + if ( !patch ) { + continue; + } + if ( patch->checkcount == cm.checkcount ) { + continue; // already checked this patch in another leaf + } + patch->checkcount = cm.checkcount; + + if ( !(patch->contents & tw->contents) ) { + continue; + } + + CM_TraceThroughPatch( tw, patch ); + if ( !tw->trace.fraction ) { + return; + } + } + } +} + +#define RADIUS_EPSILON 1.0f + +/* +================ +CM_TraceThroughSphere + +get the first intersection of the ray with the sphere +================ +*/ +void CM_TraceThroughSphere( traceWork_t *tw, vec3_t origin, float radius, vec3_t start, vec3_t end ) { + float l1, l2, length, scale, fraction; + float a, b, c, d, sqrtd; + vec3_t v1, dir, intersection; + + // if inside the sphere + VectorSubtract(start, origin, dir); + l1 = VectorLengthSquared(dir); + if (l1 < Square(radius)) { + tw->trace.fraction = 0; + tw->trace.startsolid = qtrue; + // test for allsolid + VectorSubtract(end, origin, dir); + l1 = VectorLengthSquared(dir); + if (l1 < Square(radius)) { + tw->trace.allsolid = qtrue; + } + return; + } + // + VectorSubtract(end, start, dir); + length = VectorNormalize(dir); + // + l1 = CM_DistanceFromLineSquared(origin, start, end, dir); + VectorSubtract(end, origin, v1); + l2 = VectorLengthSquared(v1); + // if no intersection with the sphere and the end point is at least an epsilon away + if (l1 >= Square(radius) && l2 > Square(radius+SURFACE_CLIP_EPSILON)) { + return; + } + // + // | origin - (start + t * dir) | = radius + // a = dir[0]^2 + dir[1]^2 + dir[2]^2; + // b = 2 * (dir[0] * (start[0] - origin[0]) + dir[1] * (start[1] - origin[1]) + dir[2] * (start[2] - origin[2])); + // c = (start[0] - origin[0])^2 + (start[1] - origin[1])^2 + (start[2] - origin[2])^2 - radius^2; + // + VectorSubtract(start, origin, v1); + // dir is normalized so a = 1 + a = 1.0f;//dir[0] * dir[0] + dir[1] * dir[1] + dir[2] * dir[2]; + b = 2.0f * (dir[0] * v1[0] + dir[1] * v1[1] + dir[2] * v1[2]); + c = v1[0] * v1[0] + v1[1] * v1[1] + v1[2] * v1[2] - (radius+RADIUS_EPSILON) * (radius+RADIUS_EPSILON); + + d = b * b - 4.0f * c;// * a; + if (d > 0) { + sqrtd = SquareRootFloat(d); + // = (- b + sqrtd) * 0.5f; // / (2.0f * a); + fraction = (- b - sqrtd) * 0.5f; // / (2.0f * a); + // + if (fraction < 0) { + fraction = 0; + } + else { + fraction /= length; + } + if ( fraction < tw->trace.fraction ) { + tw->trace.fraction = fraction; + VectorSubtract(end, start, dir); + VectorMA(start, fraction, dir, intersection); + VectorSubtract(intersection, origin, dir); + #ifdef CAPSULE_DEBUG + l2 = VectorLength(dir); + if (l2 < radius) { + int bah = 1; + } + #endif + scale = 1 / (radius+RADIUS_EPSILON); + VectorScale(dir, scale, dir); + VectorCopy(dir, tw->trace.plane.normal); + VectorAdd( tw->modelOrigin, intersection, intersection); + tw->trace.plane.dist = DotProduct(tw->trace.plane.normal, intersection); + tw->trace.contents = CONTENTS_BODY; + } + } + else if (d == 0) { + //t1 = (- b ) / 2; + // slide along the sphere + } + // no intersection at all +} + +/* +================ +CM_TraceThroughVerticalCylinder + +get the first intersection of the ray with the cylinder +the cylinder extends halfheight above and below the origin +================ +*/ +void CM_TraceThroughVerticalCylinder( traceWork_t *tw, vec3_t origin, float radius, float halfheight, vec3_t start, vec3_t end) { + float length, scale, fraction, l1, l2; + float a, b, c, d, sqrtd; + vec3_t v1, dir, start2d, end2d, org2d, intersection; + + // 2d coordinates + VectorSet(start2d, start[0], start[1], 0); + VectorSet(end2d, end[0], end[1], 0); + VectorSet(org2d, origin[0], origin[1], 0); + // if between lower and upper cylinder bounds + if (start[2] <= origin[2] + halfheight && + start[2] >= origin[2] - halfheight) { + // if inside the cylinder + VectorSubtract(start2d, org2d, dir); + l1 = VectorLengthSquared(dir); + if (l1 < Square(radius)) { + tw->trace.fraction = 0; + tw->trace.startsolid = qtrue; + VectorSubtract(end2d, org2d, dir); + l1 = VectorLengthSquared(dir); + if (l1 < Square(radius)) { + tw->trace.allsolid = qtrue; + } + return; + } + } + // + VectorSubtract(end2d, start2d, dir); + length = VectorNormalize(dir); + // + l1 = CM_DistanceFromLineSquared(org2d, start2d, end2d, dir); + VectorSubtract(end2d, org2d, v1); + l2 = VectorLengthSquared(v1); + // if no intersection with the cylinder and the end point is at least an epsilon away + if (l1 >= Square(radius) && l2 > Square(radius+SURFACE_CLIP_EPSILON)) { + return; + } + // + // + // (start[0] - origin[0] - t * dir[0]) ^ 2 + (start[1] - origin[1] - t * dir[1]) ^ 2 = radius ^ 2 + // (v1[0] + t * dir[0]) ^ 2 + (v1[1] + t * dir[1]) ^ 2 = radius ^ 2; + // v1[0] ^ 2 + 2 * v1[0] * t * dir[0] + (t * dir[0]) ^ 2 + + // v1[1] ^ 2 + 2 * v1[1] * t * dir[1] + (t * dir[1]) ^ 2 = radius ^ 2 + // t ^ 2 * (dir[0] ^ 2 + dir[1] ^ 2) + t * (2 * v1[0] * dir[0] + 2 * v1[1] * dir[1]) + + // v1[0] ^ 2 + v1[1] ^ 2 - radius ^ 2 = 0 + // + VectorSubtract(start, origin, v1); + // dir is normalized so we can use a = 1 + a = 1.0f;// * (dir[0] * dir[0] + dir[1] * dir[1]); + b = 2.0f * (v1[0] * dir[0] + v1[1] * dir[1]); + c = v1[0] * v1[0] + v1[1] * v1[1] - (radius+RADIUS_EPSILON) * (radius+RADIUS_EPSILON); + + d = b * b - 4.0f * c;// * a; + if (d > 0) { + sqrtd = SquareRootFloat(d); + // = (- b + sqrtd) * 0.5f;// / (2.0f * a); + fraction = (- b - sqrtd) * 0.5f;// / (2.0f * a); + // + if (fraction < 0) { + fraction = 0; + } + else { + fraction /= length; + } + if ( fraction < tw->trace.fraction ) { + VectorSubtract(end, start, dir); + VectorMA(start, fraction, dir, intersection); + // if the intersection is between the cylinder lower and upper bound + if (intersection[2] <= origin[2] + halfheight && + intersection[2] >= origin[2] - halfheight) { + // + tw->trace.fraction = fraction; + VectorSubtract(intersection, origin, dir); + dir[2] = 0; + #ifdef CAPSULE_DEBUG + l2 = VectorLength(dir); + if (l2 <= radius) { + int bah = 1; + } + #endif + scale = 1 / (radius+RADIUS_EPSILON); + VectorScale(dir, scale, dir); + VectorCopy(dir, tw->trace.plane.normal); + VectorAdd( tw->modelOrigin, intersection, intersection); + tw->trace.plane.dist = DotProduct(tw->trace.plane.normal, intersection); + tw->trace.contents = CONTENTS_BODY; + } + } + } + else if (d == 0) { + //t[0] = (- b ) / 2 * a; + // slide along the cylinder + } + // no intersection at all +} + +/* +================ +CM_TraceCapsuleThroughCapsule + +capsule vs. capsule collision (not rotated) +================ +*/ +void CM_TraceCapsuleThroughCapsule( traceWork_t *tw, clipHandle_t model ) { + int i; + vec3_t mins, maxs; + vec3_t top, bottom, starttop, startbottom, endtop, endbottom; + vec3_t offset, symetricSize[2]; + float radius, halfwidth, halfheight, offs, h; + + CM_ModelBounds(model, mins, maxs); + // test trace bounds vs. capsule bounds + if ( tw->bounds[0][0] > maxs[0] + RADIUS_EPSILON + || tw->bounds[0][1] > maxs[1] + RADIUS_EPSILON + || tw->bounds[0][2] > maxs[2] + RADIUS_EPSILON + || tw->bounds[1][0] < mins[0] - RADIUS_EPSILON + || tw->bounds[1][1] < mins[1] - RADIUS_EPSILON + || tw->bounds[1][2] < mins[2] - RADIUS_EPSILON + ) { + return; + } + // top origin and bottom origin of each sphere at start and end of trace + VectorAdd(tw->start, tw->sphere.offset, starttop); + VectorSubtract(tw->start, tw->sphere.offset, startbottom); + VectorAdd(tw->end, tw->sphere.offset, endtop); + VectorSubtract(tw->end, tw->sphere.offset, endbottom); + + // calculate top and bottom of the capsule spheres to collide with + for ( i = 0 ; i < 3 ; i++ ) { + offset[i] = ( mins[i] + maxs[i] ) * 0.5; + symetricSize[0][i] = mins[i] - offset[i]; + symetricSize[1][i] = maxs[i] - offset[i]; + } + halfwidth = symetricSize[ 1 ][ 0 ]; + halfheight = symetricSize[ 1 ][ 2 ]; + radius = ( halfwidth > halfheight ) ? halfheight : halfwidth; + offs = halfheight - radius; + VectorCopy(offset, top); + top[2] += offs; + VectorCopy(offset, bottom); + bottom[2] -= offs; + // expand radius of spheres + radius += tw->sphere.radius; + // if there is horizontal movement + if ( tw->start[0] != tw->end[0] || tw->start[1] != tw->end[1] ) { + // height of the expanded cylinder is the height of both cylinders minus the radius of both spheres + h = halfheight + tw->sphere.halfheight - radius; + // if the cylinder has a height + if ( h > 0 ) { + // test for collisions between the cylinders + CM_TraceThroughVerticalCylinder(tw, offset, radius, h, tw->start, tw->end); + } + } + // test for collision between the spheres + CM_TraceThroughSphere(tw, top, radius, startbottom, endbottom); + CM_TraceThroughSphere(tw, bottom, radius, starttop, endtop); +} + +/* +================ +CM_TraceBoundingBoxThroughCapsule + +bounding box vs. capsule collision +================ +*/ +void CM_TraceBoundingBoxThroughCapsule( traceWork_t *tw, clipHandle_t model ) { + vec3_t mins, maxs, offset, size[2]; + clipHandle_t h; + cmodel_t *cmod; + int i; + + // mins maxs of the capsule + CM_ModelBounds(model, mins, maxs); + + // offset for capsule center + for ( i = 0 ; i < 3 ; i++ ) { + offset[i] = ( mins[i] + maxs[i] ) * 0.5; + size[0][i] = mins[i] - offset[i]; + size[1][i] = maxs[i] - offset[i]; + tw->start[i] -= offset[i]; + tw->end[i] -= offset[i]; + } + + // replace the bounding box with the capsule + tw->sphere.use = qtrue; + tw->sphere.radius = ( size[1][0] > size[1][2] ) ? size[1][2]: size[1][0]; + tw->sphere.halfheight = size[1][2]; + VectorSet( tw->sphere.offset, 0, 0, size[1][2] - tw->sphere.radius ); + + // replace the capsule with the bounding box + h = CM_TempBoxModel(tw->size[0], tw->size[1], qfalse); + // calculate collision + cmod = CM_ClipHandleToModel( h ); + CM_TraceThroughLeaf( tw, &cmod->leaf ); +} + +//========================================================================================= + +/* +================== +CM_TraceThroughTree + +Traverse all the contacted leafs from the start to the end position. +If the trace is a point, they will be exactly in order, but for larger +trace volumes it is possible to hit something in a later leaf with +a smaller intercept fraction. +================== +*/ +void CM_TraceThroughTree( traceWork_t *tw, int num, float p1f, float p2f, vec3_t p1, vec3_t p2) { + cNode_t *node; + cplane_t *plane; + float t1, t2, offset; + float frac, frac2; + float idist; + vec3_t mid; + int side; + float midf; + + if (tw->trace.fraction <= p1f) { + return; // already hit something nearer + } + + // if < 0, we are in a leaf node + if (num < 0) { + CM_TraceThroughLeaf( tw, &cm.leafs[-1-num] ); + return; + } + + // + // find the point distances to the seperating plane + // and the offset for the size of the box + // + node = cm.nodes + num; + plane = node->plane; + + // adjust the plane distance apropriately for mins/maxs + if ( plane->type < 3 ) { + t1 = p1[plane->type] - plane->dist; + t2 = p2[plane->type] - plane->dist; + offset = tw->extents[plane->type]; + } else { + t1 = DotProduct (plane->normal, p1) - plane->dist; + t2 = DotProduct (plane->normal, p2) - plane->dist; + if ( tw->isPoint ) { + offset = 0; + } else { +#if 0 // bk010201 - DEAD + // an axial brush right behind a slanted bsp plane + // will poke through when expanded, so adjust + // by sqrt(3) + offset = fabs(tw->extents[0]*plane->normal[0]) + + fabs(tw->extents[1]*plane->normal[1]) + + fabs(tw->extents[2]*plane->normal[2]); + + offset *= 2; + offset = tw->maxOffset; +#endif + // this is silly + offset = 2048; + } + } + + // see which sides we need to consider + if ( t1 >= offset + 1 && t2 >= offset + 1 ) { + CM_TraceThroughTree( tw, node->children[0], p1f, p2f, p1, p2 ); + return; + } + if ( t1 < -offset - 1 && t2 < -offset - 1 ) { + CM_TraceThroughTree( tw, node->children[1], p1f, p2f, p1, p2 ); + return; + } + + // put the crosspoint SURFACE_CLIP_EPSILON pixels on the near side + if ( t1 < t2 ) { + idist = 1.0/(t1-t2); + side = 1; + frac2 = (t1 + offset + SURFACE_CLIP_EPSILON)*idist; + frac = (t1 - offset + SURFACE_CLIP_EPSILON)*idist; + } else if (t1 > t2) { + idist = 1.0/(t1-t2); + side = 0; + frac2 = (t1 - offset - SURFACE_CLIP_EPSILON)*idist; + frac = (t1 + offset + SURFACE_CLIP_EPSILON)*idist; + } else { + side = 0; + frac = 1; + frac2 = 0; + } + + // move up to the node + if ( frac < 0 ) { + frac = 0; + } + if ( frac > 1 ) { + frac = 1; + } + + midf = p1f + (p2f - p1f)*frac; + + mid[0] = p1[0] + frac*(p2[0] - p1[0]); + mid[1] = p1[1] + frac*(p2[1] - p1[1]); + mid[2] = p1[2] + frac*(p2[2] - p1[2]); + + CM_TraceThroughTree( tw, node->children[side], p1f, midf, p1, mid ); + + + // go past the node + if ( frac2 < 0 ) { + frac2 = 0; + } + if ( frac2 > 1 ) { + frac2 = 1; + } + + midf = p1f + (p2f - p1f)*frac2; + + mid[0] = p1[0] + frac2*(p2[0] - p1[0]); + mid[1] = p1[1] + frac2*(p2[1] - p1[1]); + mid[2] = p1[2] + frac2*(p2[2] - p1[2]); + + CM_TraceThroughTree( tw, node->children[side^1], midf, p2f, mid, p2 ); +} + + +//====================================================================== + + +/* +================== +CM_Trace +================== +*/ +void CM_Trace( trace_t *results, const vec3_t start, const vec3_t end, vec3_t mins, vec3_t maxs, + clipHandle_t model, const vec3_t origin, int brushmask, int capsule, sphere_t *sphere ) { + int i; + traceWork_t tw; + vec3_t offset; + cmodel_t *cmod; + + cmod = CM_ClipHandleToModel( model ); + + cm.checkcount++; // for multi-check avoidance + + c_traces++; // for statistics, may be zeroed + + // fill in a default trace + Com_Memset( &tw, 0, sizeof(tw) ); + tw.trace.fraction = 1; // assume it goes the entire distance until shown otherwise + VectorCopy(origin, tw.modelOrigin); + + if (!cm.numNodes) { + *results = tw.trace; + + return; // map not loaded, shouldn't happen + } + + // allow NULL to be passed in for 0,0,0 + if ( !mins ) { + mins = vec3_origin; + } + if ( !maxs ) { + maxs = vec3_origin; + } + + // set basic parms + tw.contents = brushmask; + + // adjust so that mins and maxs are always symetric, which + // avoids some complications with plane expanding of rotated + // bmodels + for ( i = 0 ; i < 3 ; i++ ) { + offset[i] = ( mins[i] + maxs[i] ) * 0.5; + tw.size[0][i] = mins[i] - offset[i]; + tw.size[1][i] = maxs[i] - offset[i]; + tw.start[i] = start[i] + offset[i]; + tw.end[i] = end[i] + offset[i]; + } + + // if a sphere is already specified + if ( sphere ) { + tw.sphere = *sphere; + } + else { + tw.sphere.use = capsule; + tw.sphere.radius = ( tw.size[1][0] > tw.size[1][2] ) ? tw.size[1][2]: tw.size[1][0]; + tw.sphere.halfheight = tw.size[1][2]; + VectorSet( tw.sphere.offset, 0, 0, tw.size[1][2] - tw.sphere.radius ); + } + + tw.maxOffset = tw.size[1][0] + tw.size[1][1] + tw.size[1][2]; + + // tw.offsets[signbits] = vector to apropriate corner from origin + tw.offsets[0][0] = tw.size[0][0]; + tw.offsets[0][1] = tw.size[0][1]; + tw.offsets[0][2] = tw.size[0][2]; + + tw.offsets[1][0] = tw.size[1][0]; + tw.offsets[1][1] = tw.size[0][1]; + tw.offsets[1][2] = tw.size[0][2]; + + tw.offsets[2][0] = tw.size[0][0]; + tw.offsets[2][1] = tw.size[1][1]; + tw.offsets[2][2] = tw.size[0][2]; + + tw.offsets[3][0] = tw.size[1][0]; + tw.offsets[3][1] = tw.size[1][1]; + tw.offsets[3][2] = tw.size[0][2]; + + tw.offsets[4][0] = tw.size[0][0]; + tw.offsets[4][1] = tw.size[0][1]; + tw.offsets[4][2] = tw.size[1][2]; + + tw.offsets[5][0] = tw.size[1][0]; + tw.offsets[5][1] = tw.size[0][1]; + tw.offsets[5][2] = tw.size[1][2]; + + tw.offsets[6][0] = tw.size[0][0]; + tw.offsets[6][1] = tw.size[1][1]; + tw.offsets[6][2] = tw.size[1][2]; + + tw.offsets[7][0] = tw.size[1][0]; + tw.offsets[7][1] = tw.size[1][1]; + tw.offsets[7][2] = tw.size[1][2]; + + // + // calculate bounds + // + if ( tw.sphere.use ) { + for ( i = 0 ; i < 3 ; i++ ) { + if ( tw.start[i] < tw.end[i] ) { + tw.bounds[0][i] = tw.start[i] - fabs(tw.sphere.offset[i]) - tw.sphere.radius; + tw.bounds[1][i] = tw.end[i] + fabs(tw.sphere.offset[i]) + tw.sphere.radius; + } else { + tw.bounds[0][i] = tw.end[i] - fabs(tw.sphere.offset[i]) - tw.sphere.radius; + tw.bounds[1][i] = tw.start[i] + fabs(tw.sphere.offset[i]) + tw.sphere.radius; + } + } + } + else { + for ( i = 0 ; i < 3 ; i++ ) { + if ( tw.start[i] < tw.end[i] ) { + tw.bounds[0][i] = tw.start[i] + tw.size[0][i]; + tw.bounds[1][i] = tw.end[i] + tw.size[1][i]; + } else { + tw.bounds[0][i] = tw.end[i] + tw.size[0][i]; + tw.bounds[1][i] = tw.start[i] + tw.size[1][i]; + } + } + } + + // + // check for position test special case + // + if (start[0] == end[0] && start[1] == end[1] && start[2] == end[2]) { + if ( model ) { +#ifdef ALWAYS_BBOX_VS_BBOX // bk010201 - FIXME - compile time flag? + if ( model == BOX_MODEL_HANDLE || model == CAPSULE_MODEL_HANDLE) { + tw.sphere.use = qfalse; + CM_TestInLeaf( &tw, &cmod->leaf ); + } + else +#elif defined(ALWAYS_CAPSULE_VS_CAPSULE) + if ( model == BOX_MODEL_HANDLE || model == CAPSULE_MODEL_HANDLE) { + CM_TestCapsuleInCapsule( &tw, model ); + } + else +#endif + if ( model == CAPSULE_MODEL_HANDLE ) { + if ( tw.sphere.use ) { + CM_TestCapsuleInCapsule( &tw, model ); + } + else { + CM_TestBoundingBoxInCapsule( &tw, model ); + } + } + else { + CM_TestInLeaf( &tw, &cmod->leaf ); + } + } else { + CM_PositionTest( &tw ); + } + } else { + // + // check for point special case + // + if ( tw.size[0][0] == 0 && tw.size[0][1] == 0 && tw.size[0][2] == 0 ) { + tw.isPoint = qtrue; + VectorClear( tw.extents ); + } else { + tw.isPoint = qfalse; + tw.extents[0] = tw.size[1][0]; + tw.extents[1] = tw.size[1][1]; + tw.extents[2] = tw.size[1][2]; + } + + // + // general sweeping through world + // + if ( model ) { +#ifdef ALWAYS_BBOX_VS_BBOX + if ( model == BOX_MODEL_HANDLE || model == CAPSULE_MODEL_HANDLE) { + tw.sphere.use = qfalse; + CM_TraceThroughLeaf( &tw, &cmod->leaf ); + } + else +#elif defined(ALWAYS_CAPSULE_VS_CAPSULE) + if ( model == BOX_MODEL_HANDLE || model == CAPSULE_MODEL_HANDLE) { + CM_TraceCapsuleThroughCapsule( &tw, model ); + } + else +#endif + if ( model == CAPSULE_MODEL_HANDLE ) { + if ( tw.sphere.use ) { + CM_TraceCapsuleThroughCapsule( &tw, model ); + } + else { + CM_TraceBoundingBoxThroughCapsule( &tw, model ); + } + } + else { + CM_TraceThroughLeaf( &tw, &cmod->leaf ); + } + } else { + CM_TraceThroughTree( &tw, 0, 0, 1, tw.start, tw.end ); + } + } + + // generate endpos from the original, unmodified start/end + if ( tw.trace.fraction == 1 ) { + VectorCopy (end, tw.trace.endpos); + } else { + for ( i=0 ; i<3 ; i++ ) { + tw.trace.endpos[i] = start[i] + tw.trace.fraction * (end[i] - start[i]); + } + } + + // If allsolid is set (was entirely inside something solid), the plane is not valid. + // If fraction == 1.0, we never hit anything, and thus the plane is not valid. + // Otherwise, the normal on the plane should have unit length + assert(tw.trace.allsolid || + tw.trace.fraction == 1.0 || + VectorLengthSquared(tw.trace.plane.normal) > 0.9999); + *results = tw.trace; +} + +/* +================== +CM_BoxTrace +================== +*/ +void CM_BoxTrace( trace_t *results, const vec3_t start, const vec3_t end, + vec3_t mins, vec3_t maxs, + clipHandle_t model, int brushmask, int capsule ) { + CM_Trace( results, start, end, mins, maxs, model, vec3_origin, brushmask, capsule, NULL ); +} + +/* +================== +CM_TransformedBoxTrace + +Handles offseting and rotation of the end points for moving and +rotating entities +================== +*/ +void CM_TransformedBoxTrace( trace_t *results, const vec3_t start, const vec3_t end, + vec3_t mins, vec3_t maxs, + clipHandle_t model, int brushmask, + const vec3_t origin, const vec3_t angles, int capsule ) { + trace_t trace; + vec3_t start_l, end_l; + qboolean rotated; + vec3_t offset; + vec3_t symetricSize[2]; + vec3_t matrix[3], transpose[3]; + int i; + float halfwidth; + float halfheight; + float t; + sphere_t sphere; + + if ( !mins ) { + mins = vec3_origin; + } + if ( !maxs ) { + maxs = vec3_origin; + } + + // adjust so that mins and maxs are always symetric, which + // avoids some complications with plane expanding of rotated + // bmodels + for ( i = 0 ; i < 3 ; i++ ) { + offset[i] = ( mins[i] + maxs[i] ) * 0.5; + symetricSize[0][i] = mins[i] - offset[i]; + symetricSize[1][i] = maxs[i] - offset[i]; + start_l[i] = start[i] + offset[i]; + end_l[i] = end[i] + offset[i]; + } + + // subtract origin offset + VectorSubtract( start_l, origin, start_l ); + VectorSubtract( end_l, origin, end_l ); + + // rotate start and end into the models frame of reference + if ( model != BOX_MODEL_HANDLE && + (angles[0] || angles[1] || angles[2]) ) { + rotated = qtrue; + } else { + rotated = qfalse; + } + + halfwidth = symetricSize[ 1 ][ 0 ]; + halfheight = symetricSize[ 1 ][ 2 ]; + + sphere.use = capsule; + sphere.radius = ( halfwidth > halfheight ) ? halfheight : halfwidth; + sphere.halfheight = halfheight; + t = halfheight - sphere.radius; + + if (rotated) { + // rotation on trace line (start-end) instead of rotating the bmodel + // NOTE: This is still incorrect for bounding boxes because the actual bounding + // box that is swept through the model is not rotated. We cannot rotate + // the bounding box or the bmodel because that would make all the brush + // bevels invalid. + // However this is correct for capsules since a capsule itself is rotated too. + CreateRotationMatrix(angles, matrix); + RotatePoint(start_l, matrix); + RotatePoint(end_l, matrix); + // rotated sphere offset for capsule + sphere.offset[0] = matrix[0][ 2 ] * t; + sphere.offset[1] = -matrix[1][ 2 ] * t; + sphere.offset[2] = matrix[2][ 2 ] * t; + } + else { + VectorSet( sphere.offset, 0, 0, t ); + } + + // sweep the box through the model + CM_Trace( &trace, start_l, end_l, symetricSize[0], symetricSize[1], model, origin, brushmask, capsule, &sphere ); + + // if the bmodel was rotated and there was a collision + if ( rotated && trace.fraction != 1.0 ) { + // rotation of bmodel collision plane + TransposeMatrix(matrix, transpose); + RotatePoint(trace.plane.normal, transpose); + } + + // re-calculate the end position of the trace because the trace.endpos + // calculated by CM_Trace could be rotated and have an offset + trace.endpos[0] = start[0] + trace.fraction * (end[0] - start[0]); + trace.endpos[1] = start[1] + trace.fraction * (end[1] - start[1]); + trace.endpos[2] = start[2] + trace.fraction * (end[2] - start[2]); + + *results = trace; +} |