/*
=============
idWinding2D::PlaneDistance
=============
*/
float idWinding2D::PlaneDistance( const idVec3 &plane ) const {
int i;
float d, min, max;
min = idMath::INFINITY;
max = -min;
for ( i = 0; i < numPoints; i++ ) {
d = plane.x * p[i].x + plane.y * p[i].y + plane.z;
if ( d < min ) {
min = d;
if ( FLOATSIGNBITSET( min ) & FLOATSIGNBITNOTSET( max ) ) {
return 0.0f;
}
}
if ( d > max ) {
max = d;
if ( FLOATSIGNBITSET( min ) & FLOATSIGNBITNOTSET( max ) ) {
return 0.0f;
}
}
}
if ( FLOATSIGNBITNOTSET( min ) ) {
return min;
}
if ( FLOATSIGNBITSET( max ) ) {
return max;
}
return 0.0f;
}
/*
============
GetFirstBlockingObstacle
============
*/
bool GetFirstBlockingObstacle( const obstacle_t *obstacles, int numObstacles, int skipObstacle, const idVec2 &startPos, const idVec2 &delta, float &blockingScale, int &blockingObstacle, int &blockingEdgeNum ) {
int i, edgeNums[2];
float dist, scale1, scale2;
idVec2 bounds[2];
// get bounds for the current movement delta
bounds[0] = startPos - idVec2( CM_BOX_EPSILON, CM_BOX_EPSILON );
bounds[1] = startPos + idVec2( CM_BOX_EPSILON, CM_BOX_EPSILON );
bounds[FLOATSIGNBITNOTSET(delta.x)].x += delta.x;
bounds[FLOATSIGNBITNOTSET(delta.y)].y += delta.y;
// test for obstacles blocking the path
blockingScale = idMath::INFINITY;
dist = delta.Length();
for ( i = 0; i < numObstacles; i++ ) {
if ( i == skipObstacle ) {
continue;
}
if ( bounds[0].x > obstacles[i].bounds[1].x || bounds[0].y > obstacles[i].bounds[1].y ||
bounds[1].x < obstacles[i].bounds[0].x || bounds[1].y < obstacles[i].bounds[0].y ) {
continue;
}
if ( obstacles[i].winding.RayIntersection( startPos, delta, scale1, scale2, edgeNums ) ) {
if ( scale1 < blockingScale && scale1 * dist > -0.01f && scale2 * dist > 0.01f ) {
blockingScale = scale1;
blockingObstacle = i;
blockingEdgeNum = edgeNums[0];
}
}
}
return ( blockingScale < 1.0f );
}
/*
============
idBox::GetProjectionSilhouetteVerts
============
*/
int idBox::GetProjectionSilhouetteVerts( const idVec3 &projectionOrigin, idVec3 silVerts[6] ) const {
float f;
int i, planeBits, *index;
idVec3 points[8], dir1, dir2;
ToPoints( points );
dir1 = points[0] - projectionOrigin;
dir2 = points[6] - projectionOrigin;
f = dir1 * axis[0];
planeBits = FLOATSIGNBITNOTSET( f );
f = dir2 * axis[0];
planeBits |= FLOATSIGNBITSET( f ) << 1;
f = dir1 * axis[1];
planeBits |= FLOATSIGNBITNOTSET( f ) << 2;
f = dir2 * axis[1];
planeBits |= FLOATSIGNBITSET( f ) << 3;
f = dir1 * axis[2];
planeBits |= FLOATSIGNBITNOTSET( f ) << 4;
f = dir2 * axis[2];
planeBits |= FLOATSIGNBITSET( f ) << 5;
index = boxPlaneBitsSilVerts[planeBits];
for ( i = 0; i < index[0]; i++ ) {
silVerts[i] = points[index[i+1]];
}
return index[0];
}
/*
============
idWinding2D::LineIntersection
============
*/
bool idWinding2D::LineIntersection( const idVec2 &start, const idVec2 &end ) const {
int i, numEdges;
int sides[MAX_POINTS_ON_WINDING_2D+1], counts[3];
float d1, d2, epsilon = 0.1f;
idVec3 plane, edges[2];
counts[SIDE_FRONT] = counts[SIDE_BACK] = counts[SIDE_ON] = 0;
plane = Plane2DFromPoints( start, end );
for ( i = 0; i < numPoints; i++ ) {
d1 = plane.x * p[i].x + plane.y * p[i].y + plane.z;
if ( d1 > epsilon ) {
sides[i] = SIDE_FRONT;
}
else if ( d1 < -epsilon ) {
sides[i] = SIDE_BACK;
}
else {
sides[i] = SIDE_ON;
}
counts[sides[i]]++;
}
sides[i] = sides[0];
if ( !counts[SIDE_FRONT] ) {
return false;
}
if ( !counts[SIDE_BACK] ) {
return false;
}
numEdges = 0;
for ( i = 0; i < numPoints; i++ ) {
if ( sides[i] != sides[i+1] && sides[i+1] != SIDE_ON ) {
edges[numEdges++] = Plane2DFromPoints( p[i], p[(i+1)%numPoints] );
if ( numEdges >= 2 ) {
break;
}
}
}
if ( numEdges < 2 ) {
return false;
}
d1 = edges[0].x * start.x + edges[0].y * start.y + edges[0].z;
d2 = edges[0].x * end.x + edges[0].y * end.y + edges[0].z;
if ( FLOATSIGNBITNOTSET( d1 ) & FLOATSIGNBITNOTSET( d2 ) ) {
return false;
}
d1 = edges[1].x * start.x + edges[1].y * start.y + edges[1].z;
d2 = edges[1].x * end.x + edges[1].y * end.y + edges[1].z;
if ( FLOATSIGNBITNOTSET( d1 ) & FLOATSIGNBITNOTSET( d2 ) ) {
return false;
}
return true;
}
/*
============
OptimizePath
============
*/
int OptimizePath( const pathNode_t *root, const pathNode_t *leafNode, const obstacle_t *obstacles, int numObstacles, idVec2 optimizedPath[MAX_OBSTACLE_PATH] ) {
int i, numPathPoints, edgeNums[2];
const pathNode_t *curNode, *nextNode;
idVec2 curPos, curDelta, bounds[2];
float scale1, scale2, curLength;
optimizedPath[0] = root->pos;
numPathPoints = 1;
for ( nextNode = curNode = root; curNode != leafNode; curNode = nextNode ) {
for ( nextNode = leafNode; nextNode->parent != curNode; nextNode = nextNode->parent ) {
// can only take shortcuts when going from one object to another
if ( nextNode->obstacle == curNode->obstacle ) {
continue;
}
curPos = curNode->pos;
curDelta = nextNode->pos - curPos;
curLength = curDelta.Length();
// get bounds for the current movement delta
bounds[0] = curPos - idVec2( CM_BOX_EPSILON, CM_BOX_EPSILON );
bounds[1] = curPos + idVec2( CM_BOX_EPSILON, CM_BOX_EPSILON );
bounds[FLOATSIGNBITNOTSET(curDelta.x)].x += curDelta.x;
bounds[FLOATSIGNBITNOTSET(curDelta.y)].y += curDelta.y;
// test if the shortcut intersects with any obstacles
for ( i = 0; i < numObstacles; i++ ) {
if ( bounds[0].x > obstacles[i].bounds[1].x || bounds[0].y > obstacles[i].bounds[1].y ||
bounds[1].x < obstacles[i].bounds[0].x || bounds[1].y < obstacles[i].bounds[0].y ) {
continue;
}
if ( obstacles[i].winding.RayIntersection( curPos, curDelta, scale1, scale2, edgeNums ) ) {
if ( scale1 >= 0.0f && scale1 <= 1.0f && ( i != nextNode->obstacle || scale1 * curLength < curLength - 0.5f ) ) {
break;
}
if ( scale2 >= 0.0f && scale2 <= 1.0f && ( i != nextNode->obstacle || scale2 * curLength < curLength - 0.5f ) ) {
break;
}
}
}
if ( i >= numObstacles ) {
break;
}
}
// store the next position along the optimized path
optimizedPath[numPathPoints++] = nextNode->pos;
}
return numPathPoints;
}
