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