bool sphere::Intersect(ray Ray, fp32 *Distance) const { const v3fp32 LocalRayOrigin = Ray.Origin - Pos; const fp32 A = Ray.Direction.X * Ray.Direction.X + Ray.Direction.Y * Ray.Direction.Y + Ray.Direction.Z * Ray.Direction.Z; const fp32 B = 2 * (LocalRayOrigin.X * Ray.Direction.X + LocalRayOrigin.Y * Ray.Direction.Y + LocalRayOrigin.Z * Ray.Direction.Z); const fp32 C = LocalRayOrigin.X * LocalRayOrigin.X + LocalRayOrigin.Y * LocalRayOrigin.Y + LocalRayOrigin.Z * LocalRayOrigin.Z - Radius * Radius; quadratic_result QuadraticResult = SolveQuadratic(A, B, C); if(!QuadraticResult.SolutionExists) { return false; } if(QuadraticResult.Root1 > Epsilon) { if(QuadraticResult.Root2 > Epsilon) { *Distance = MinFP32(QuadraticResult.Root1, QuadraticResult.Root2); } else { *Distance = QuadraticResult.Root1; } return true; } else if(QuadraticResult.Root2 > Epsilon) { *Distance = QuadraticResult.Root2; return true; } else { return false; } }
/************* * DESCRIPTION: Root solver based on the Sturm sequences for a polynomial * INPUT: order * coeffs * roots * OUTPUT: number of real roots *************/ int PolySolve(int order, double *coeffs, double *roots) { POLYNOMIAL sseq[MAX_ORDER+1]; double min_value, max_value; int i, nroots, np, atmin, atmax; if(fabs(coeffs[0]) < COEFF_LIMIT) return SolveQuadratic(&coeffs[1], roots); // Put the coefficients into the top of the stack for(i = 0; i <= order; i++) sseq[0].coef[order-i] = coeffs[i]; // Build the Sturm sequence np = BuildSturm(order, &sseq[0]); // Get the total number of visible roots if((nroots = VisibleRoots(np, sseq, &atmin, &atmax)) == 0) return 0; // Bracket the roots min_value = 0.0; max_value = 1.0e7; atmin = NumChanges(np, sseq, min_value); atmax = NumChanges(np, sseq, max_value); nroots = atmin - atmax; if(nroots == 0) return 0; // perform the bisection return(SBisect(np, sseq, min_value, max_value, atmin, atmax, roots)); }
void LocalizationGL::drawRotatedUncertaintyEllipse(Point2D loc, double p00, double p01, double p10, double p11) { // kill warnings p10 = p10; double d1=0; double d2=0; SolveQuadratic(&d1,&d2,1.0,-1*(p00+p11),(p00*p11)-(p01*p01)); // cout << d1 << "," << d2 << endl << flush; double bigD = max(d1,d2); double smallD = min(d1,d2); double rot = 0.0; if (fabs(smallD - p00) > fabs(smallD - p11)) { rot = atan2(p01,smallD-p00); } else { rot = atan2(smallD-p11,p01); } Point2D sd; sd.x=bigD; sd.y=smallD; // cout << sd << endl << flush; // drawUncertaintyEllipse(loc,sd); drawRotatedUncertaintyEllipse(loc,sd,-rot); }
/************* * DESCRIPTION: Solve the cubic equation: * * x[0] * x^3 + x[1] * x^2 + x[2] * x + x[3] = 0. * * The result of this function is an integer that tells how many real * roots exist. Determination of how many are distinct is up to the * process that calls this routine. The roots that exist are stored * in (y[0], y[1], y[2]). * INPUT: x,y * OUTPUT: number of real roots *************/ int SolveCubic(double *x, double *y) { double Q, R, Q3, R2, sQ, d, an, theta; double A2, a0, a1, a2, a3; if(fabs(x[0]) < COEFF_LIMIT) { return SolveQuadratic(&x[1], y); } else { a0 = 1./x[0]; if(a0 != 1.) { a1 = x[1] * a0; a2 = x[2] * a0; a3 = x[3] * a0; } else { a1 = x[1]; a2 = x[2]; a3 = x[3]; } } A2 = a1 * a1; Q = (A2 - 3. * a2) * 0.11111111; R = (a1 * (A2 - 4.5 * a2) + 13.5 * a3) * 0.0370370370; Q3 = Q * Q * Q; R2 = R * R; d = Q3 - R2; an = a1 * 0.33333333; if (d >= 0.) { // Three real roots d = R / sqrt(Q3); theta = acos(d) * 0.33333333; sQ = -2.0 * sqrt(Q); y[0] = sQ * cos(theta) - an; y[1] = sQ * cos(theta + TWO_PI_3) - an; y[2] = sQ * cos(theta + TWO_PI_43) - an; return 3; } sQ = pow(sqrt(R2 - Q3) + fabs(R), 0.33333333); if(R < 0.) y[0] = (sQ + Q / sQ) - an; else y[0] = -(sQ + Q / sQ) - an; return 1; }
void CAI_BlendedMotor::InsertSlowdown( float distToObstruction, float idealAccel, bool bAlwaysSlowdown ) { int i; AI_Movementscript_t script; if (distToObstruction <= 0.0) return; for (i = 0; i < m_scriptMove.Count() - 1; i++) { if (m_scriptMove[i].flDist > 0 && distToObstruction - m_scriptMove[i].flDist < 0) { float a = distToObstruction / m_scriptMove[i].flDist; Assert( a >= 0 && a <= 1); script.vecLocation = (1 - a) * m_scriptMove[i].vecLocation + a * m_scriptMove[i+1].vecLocation; //NDebugOverlay::Line( m_scriptMove[i].vecLocation + Vector( 0, 0, 5 ), script.vecLocation + Vector( 0, 0, 5 ), 0,255,0, true, 0.1 ); //NDebugOverlay::Line( script.vecLocation + Vector( 0, 0, 5 ), m_scriptMove[i+1].vecLocation + Vector( 0, 0, 5 ), 0,0,255, true, 0.1 ); float r1, r2; // clamp the next velocity to the possible accel in the given distance if (!bAlwaysSlowdown && SolveQuadratic( -0.5 * idealAccel, m_scriptMove[0].flMaxVelocity, -distToObstruction, r1, r2 )) { script.flMaxVelocity = max( 10, m_scriptMove[0].flMaxVelocity - idealAccel * r1 ); } else { script.flMaxVelocity = 10.0; } script.flMaxVelocity = 1.0; // as much as reasonable script.pWaypoint = NULL; script.flDist = m_scriptMove[i].flDist - distToObstruction; m_scriptMove[i].flDist = distToObstruction; m_scriptMove.InsertAfter( i, script ); break; } else { distToObstruction -= m_scriptMove[i].flDist; } } }
void CAI_BlendedMotor::BuildVelocityScript( const AILocalMoveGoal_t &move ) { int i; float a; float idealVelocity = GetIdealSpeed(); if (idealVelocity == 0) { idealVelocity = 50; } float idealAccel = GetIdealAccel(); if (idealAccel == 0) { idealAccel = 100; } AI_Movementscript_t script; // set current location as start of script script.vecLocation = GetAbsOrigin(); script.flMaxVelocity = GetCurSpeed(); m_scriptMove.AddToTail( script ); //------------------------- extern ConVar *npc_height_adjust; if (npc_height_adjust->GetBool() && move.bHasTraced && move.directTrace.flTotalDist != move.thinkTrace.flTotalDist) { float flDist = (move.directTrace.vEndPosition - m_scriptMove[0].vecLocation).Length2D(); float flHeight = move.directTrace.vEndPosition.z - m_scriptMove[0].vecLocation.z; float flDelta; if (flDist > 0) { flDelta = flHeight / flDist; } else { flDelta = 0; } m_flPredictiveSpeedAdjust = 1.1 - fabs( flDelta ); m_flPredictiveSpeedAdjust = clamp( m_flPredictiveSpeedAdjust, (flHeight > 0.0) ? 0.5 : 0.8, 1.0 ); /* if ((GetOuter()->m_debugOverlays & OVERLAY_NPC_SELECTED_BIT)) { Msg("m_flPredictiveSpeedAdjust %.3f %.1f %.1f\n", m_flPredictiveSpeedAdjust, flHeight, flDist ); NDebugOverlay::Box( move.directTrace.vEndPosition, Vector( -2, -2, -2 ), Vector( 2, 2, 2 ), 0,255,255, 0, 0.12 ); } */ } if (npc_height_adjust->GetBool()) { float flDist = (move.thinkTrace.vEndPosition - m_vecPrevOrigin2).Length2D(); float flHeight = move.thinkTrace.vEndPosition.z - m_vecPrevOrigin2.z; float flDelta; if (flDist > 0) { flDelta = flHeight / flDist; } else { flDelta = 0; } float newSpeedAdjust = 1.1 - fabs( flDelta ); newSpeedAdjust = clamp( newSpeedAdjust, (flHeight > 0.0) ? 0.5 : 0.8, 1.0 ); // debounce speed adjust if (newSpeedAdjust < m_flReactiveSpeedAdjust) { m_flReactiveSpeedAdjust = m_flReactiveSpeedAdjust * 0.2 + newSpeedAdjust * 0.8; } else { m_flReactiveSpeedAdjust = m_flReactiveSpeedAdjust * 0.5 + newSpeedAdjust * 0.5; } // filter through origins m_vecPrevOrigin2 = m_vecPrevOrigin1; m_vecPrevOrigin1 = GetAbsOrigin(); /* if ((GetOuter()->m_debugOverlays & OVERLAY_NPC_SELECTED_BIT)) { NDebugOverlay::Box( m_vecPrevOrigin2, Vector( -2, -2, -2 ), Vector( 2, 2, 2 ), 255,0,255, 0, 0.12 ); NDebugOverlay::Box( move.thinkTrace.vEndPosition, Vector( -2, -2, -2 ), Vector( 2, 2, 2 ), 255,0,255, 0, 0.12 ); Msg("m_flReactiveSpeedAdjust %.3f %.1f %.1f\n", m_flReactiveSpeedAdjust, flHeight, flDist ); } */ } idealVelocity = idealVelocity * min( m_flReactiveSpeedAdjust, m_flPredictiveSpeedAdjust ); //------------------------- bool bAddedExpected = false; // add all waypoint locations and velocities AI_Waypoint_t *pCurWaypoint = GetNavigator()->GetPath()->GetCurWaypoint(); // there has to be at least one waypoint Assert( pCurWaypoint ); while (pCurWaypoint && (pCurWaypoint->NavType() == NAV_GROUND || pCurWaypoint->NavType() == NAV_FLY) /*&& flTotalDist / idealVelocity < 3.0*/) // limit lookahead to 3 seconds { script.Init(); AI_Waypoint_t *pNext = pCurWaypoint->GetNext(); if (ai_path_adjust_speed_on_immediate_turns->GetBool() && !bAddedExpected) { // hack in next expected immediate location for move script.vecLocation = GetAbsOrigin() + move.dir * move.curExpectedDist; bAddedExpected = true; pNext = pCurWaypoint; } else { script.vecLocation = pCurWaypoint->vecLocation; script.pWaypoint = pCurWaypoint; } //DevMsg("waypoint %.1f %.1f %.1f\n", script.vecLocation.x, script.vecLocation.y, script.vecLocation.z ); if (pNext) { switch( pNext->NavType()) { case NAV_GROUND: case NAV_FLY: { Vector d1 = pNext->vecLocation - script.vecLocation; Vector d2 = script.vecLocation - m_scriptMove[m_scriptMove.Count()-1].vecLocation; // remove very short, non terminal ground links // FIXME: is this safe? Maybe just check for co-located ground points? if (d1.Length2D() < 1.0) { /* if (m_scriptMove.Count() > 1) { int i = m_scriptMove.Count() - 1; m_scriptMove[i].vecLocation = pCurWaypoint->vecLocation; m_scriptMove[i].pWaypoint = pCurWaypoint; } */ pCurWaypoint = pNext; continue; } d1.z = 0; VectorNormalize( d1 ); d2.z = 0; VectorNormalize( d2 ); // figure velocity float dot = (DotProduct( d1, d2 ) + 0.2); if (dot > 0) { dot = clamp( dot, 0.0f, 1.0f ); script.flMaxVelocity = idealVelocity * dot; } else { script.flMaxVelocity = 0; } } break; case NAV_JUMP: // FIXME: information about what the jump should look like isn't stored in the waypoints // this'll need to call // GetMoveProbe()->MoveLimit( NAV_JUMP, GetLocalOrigin(), GetPath()->CurWaypointPos(), MASK_NPCSOLID, GetNavTargetEntity(), &moveTrace ); // to get how far/fast the jump will be, but this is also stateless, so it'd call it per frame. // So far it's not clear that the moveprobe doesn't also call this..... { float minJumpHeight = 0; float maxHorzVel = max( GetCurSpeed(), 100 ); float gravity = sv_gravity->GetFloat() * GetOuter()->GetGravity(); Vector vecApex; Vector rawJumpVel = GetMoveProbe()->CalcJumpLaunchVelocity(script.vecLocation, pNext->vecLocation, gravity, &minJumpHeight, maxHorzVel, &vecApex ); script.flMaxVelocity = rawJumpVel.Length2D(); // Msg("%.1f\n", script.flMaxVelocity ); } break; case NAV_CLIMB: { /* CAI_Node *pClimbNode = GetNavigator()->GetNetwork()->GetNode(pNext->iNodeID); check: pClimbNode->m_eNodeInfo bits_NODE_CLIMB_BOTTOM, bits_NODE_CLIMB_ON, bits_NODE_CLIMB_OFF_FORWARD, bits_NODE_CLIMB_OFF_LEFT, bits_NODE_CLIMB_OFF_RIGHT */ script.flMaxVelocity = 0; } break; /* case NAV_FLY: // FIXME: can there be a NAV_GROUND -> NAV_FLY transition? script.flMaxVelocity = 0; break; */ default: break; } } else { script.flMaxVelocity = GetNavigator()->GetArrivalSpeed(); // Assert( script.flMaxVelocity == 0 ); } m_scriptMove.AddToTail( script ); pCurWaypoint = pNext; } //------------------------- // update distances float flTotalDist = 0; for (i = 0; i < m_scriptMove.Count() - 1; i++ ) { flTotalDist += m_scriptMove[i].flDist = (m_scriptMove[i+1].vecLocation - m_scriptMove[i].vecLocation).Length2D(); } //------------------------- if ( !m_bDeceleratingToGoal && m_scriptMove.Count() && flTotalDist > 0 ) { float flNeededAccel = DeltaV( m_scriptMove[0].flMaxVelocity, m_scriptMove[m_scriptMove.Count() - 1].flMaxVelocity, flTotalDist ); m_bDeceleratingToGoal = (flNeededAccel < -idealAccel); //Assert( flNeededAccel != idealAccel); } //------------------------- // insert slowdown points due to blocking if (ai_path_insert_pause_at_obstruction->GetBool() && move.directTrace.pObstruction) { float distToObstruction = (move.directTrace.vEndPosition - m_scriptMove[0].vecLocation).Length2D(); // HACK move obstruction out "stepsize" to account for it being based on stand position and not a trace distToObstruction = distToObstruction + 16; InsertSlowdown( distToObstruction, idealAccel, false ); } if (ai_path_insert_pause_at_est_end->GetBool() && GetNavigator()->GetArrivalDistance() > 0.0) { InsertSlowdown( flTotalDist - GetNavigator()->GetArrivalDistance(), idealAccel, true ); } // calc initial velocity based on immediate direction changes if ( ai_path_adjust_speed_on_immediate_turns->GetBool() && m_scriptMove.Count() > 1) { /* if ((GetOuter()->m_debugOverlays & OVERLAY_NPC_SELECTED_BIT)) { Vector tmp = m_scriptMove[1].vecLocation - m_scriptMove[0].vecLocation; VectorNormalize( tmp ); NDebugOverlay::Line( m_scriptMove[0].vecLocation + Vector( 0, 0, 10 ), m_scriptMove[0].vecLocation + tmp * 32 + Vector( 0, 0, 10 ), 255,255,255, true, 0.1 ); NDebugOverlay::Line( m_scriptMove[0].vecLocation + Vector( 0, 0, 10 ), m_scriptMove[1].vecLocation + Vector( 0, 0, 10 ), 255,0,0, true, 0.1 ); tmp = GetCurVel(); VectorNormalize( tmp ); NDebugOverlay::Line( m_scriptMove[0].vecLocation + Vector( 0, 0, 10 ), m_scriptMove[0].vecLocation + tmp * 32 + Vector( 0, 0, 10 ), 0,0,255, true, 0.1 ); } */ Vector d1 = m_scriptMove[1].vecLocation - m_scriptMove[0].vecLocation; d1.z = 0; VectorNormalize( d1 ); Vector d2 = GetCurVel(); d2.z = 0; VectorNormalize( d2 ); float dot = (DotProduct( d1, d2 ) + MIN_STEER_DOT); dot = clamp( dot, 0.0f, 1.0f ); m_scriptMove[0].flMaxVelocity = m_scriptMove[0].flMaxVelocity * dot; } // clamp forward velocities for (i = 0; i < m_scriptMove.Count() - 1; i++ ) { // find needed acceleration float dv = m_scriptMove[i+1].flMaxVelocity - m_scriptMove[i].flMaxVelocity; if (dv > 0.0) { // find time, distance to accel to next max vel float t1 = dv / idealAccel; float d1 = m_scriptMove[i].flMaxVelocity * t1 + 0.5 * (idealAccel) * t1 * t1; // is there enough distance if (d1 > m_scriptMove[i].flDist) { float r1, r2; // clamp the next velocity to the possible accel in the given distance if (SolveQuadratic( 0.5 * idealAccel, m_scriptMove[i].flMaxVelocity, -m_scriptMove[i].flDist, r1, r2 )) { m_scriptMove[i+1].flMaxVelocity = m_scriptMove[i].flMaxVelocity + idealAccel * r1; } } } } // clamp decel velocities for (i = m_scriptMove.Count() - 1; i > 0; i-- ) { // find needed deceleration float dv = m_scriptMove[i].flMaxVelocity - m_scriptMove[i-1].flMaxVelocity; if (dv < 0.0) { // find time, distance to decal to next max vel float t1 = -dv / idealAccel; float d1 = m_scriptMove[i].flMaxVelocity * t1 + 0.5 * (idealAccel) * t1 * t1; // is there enough distance if (d1 > m_scriptMove[i-1].flDist) { float r1, r2; // clamp the next velocity to the possible decal in the given distance if (SolveQuadratic( 0.5 * idealAccel, m_scriptMove[i].flMaxVelocity, -m_scriptMove[i-1].flDist, r1, r2 )) { m_scriptMove[i-1].flMaxVelocity = m_scriptMove[i].flMaxVelocity + idealAccel * r1; } } } } /* for (i = 0; i < m_scriptMove.Count(); i++) { NDebugOverlay::Text( m_scriptMove[i].vecLocation, (const char *)CFmtStr( "%.2f ", m_scriptMove[i].flMaxVelocity ), false, 0.1 ); // DevMsg("%.2f ", m_scriptMove[i].flMaxVelocity ); } // DevMsg("\n"); */ // insert intermediate ideal velocities for (i = 0; i < m_scriptMove.Count() - 1;) { // accel to ideal float t1 = (idealVelocity - m_scriptMove[i].flMaxVelocity) / idealAccel; float d1 = m_scriptMove[i].flMaxVelocity * t1 + 0.5 * (idealAccel) * t1 * t1; // decel from ideal float t2 = (idealVelocity - m_scriptMove[i+1].flMaxVelocity) / idealAccel; float d2 = m_scriptMove[i+1].flMaxVelocity * t2 + 0.5 * (idealAccel) * t2 * t2; m_scriptMove[i].flDist = (m_scriptMove[i+1].vecLocation - m_scriptMove[i].vecLocation).Length2D(); // is it possible to accel and decal to idealVelocity between next two nodes if (d1 + d2 < m_scriptMove[i].flDist) { Vector start = m_scriptMove[i].vecLocation; Vector end = m_scriptMove[i+1].vecLocation; float dist = m_scriptMove[i].flDist; // insert the two points needed to end accel and start decel if (d1 > 1.0 && t1 > 0.1) { a = d1 / dist; script.Init(); script.vecLocation = end * a + start * (1 - a); script.flMaxVelocity = idealVelocity; m_scriptMove.InsertAfter( i, script ); i++; } if (dist - d2 > 1.0 && t2 > 0.1) { // DevMsg("%.2f : ", a ); a = (dist - d2) / dist; script.Init(); script.vecLocation = end * a + start * (1 - a); script.flMaxVelocity = idealVelocity; m_scriptMove.InsertAfter( i, script ); i++; } i++; } else { // check to see if the amount of change needed to reach target is less than the ideal acceleration float flNeededAccel = fabs( DeltaV( m_scriptMove[i].flMaxVelocity, m_scriptMove[i+1].flMaxVelocity, m_scriptMove[i].flDist ) ); if (flNeededAccel < idealAccel) { // if so, they it's possible to get a bit towards the ideal velocity float v1 = m_scriptMove[i].flMaxVelocity; float v2 = m_scriptMove[i+1].flMaxVelocity; float dist = m_scriptMove[i].flDist; // based on solving: // v1+A*t1-v2-A*t2=0 // v1*t1+0.5*A*t1*t1+v2*t2+0.5*A*t2*t2-D=0 float tmp = idealAccel*dist+0.5*v1*v1+0.5*v2*v2; Assert( tmp >= 0 ); t1 = (-v1+sqrt( tmp )) / idealAccel; t2 = (v1+idealAccel*t1-v2)/idealAccel; // if this assert hits, write down the v1, v2, dist, and idealAccel numbers and send them to me (Ken). // go ahead the comment it out, it's safe, but I'd like to know a test case where it's happening //Assert( t1 > 0 && t2 > 0 ); // check to make sure it's really worth it if (t1 > 0.0 && t2 > 0.0) { d1 = v1 * t1 + 0.5 * idealAccel * t1 * t1; /* d2 = v2 * t2 + 0.5 * idealAccel * t2 * t2; Assert( fabs( d1 + d2 - dist ) < 0.001 ); */ float a = d1 / m_scriptMove[i].flDist; script.Init(); script.vecLocation = m_scriptMove[i+1].vecLocation * a + m_scriptMove[i].vecLocation * (1 - a); script.flMaxVelocity = m_scriptMove[i].flMaxVelocity + idealAccel * t1; if (script.flMaxVelocity < idealVelocity) { // DevMsg("insert %.2f %.2f %.2f\n", m_scriptMove[i].flMaxVelocity, script.flMaxVelocity, m_scriptMove[i+1].flMaxVelocity ); m_scriptMove.InsertAfter( i, script ); i += 1; } } } i += 1; } } // clamp min velocities for (i = 0; i < m_scriptMove.Count(); i++) { m_scriptMove[i].flMaxVelocity = max( m_scriptMove[i].flMaxVelocity, MIN_VELOCITY ); } // rebuild fields m_scriptMove[0].flElapsedTime = 0; for (i = 0; i < m_scriptMove.Count() - 1; ) { m_scriptMove[i].flDist = (m_scriptMove[i+1].vecLocation - m_scriptMove[i].vecLocation).Length2D(); if (m_scriptMove[i].flMaxVelocity == 0 && m_scriptMove[i+1].flMaxVelocity == 0) { // force a minimum velocity //CE_assert //Assert( 0 ); m_scriptMove[i+1].flMaxVelocity = 1.0; } float t = m_scriptMove[i].flDist / (0.5 * (m_scriptMove[i].flMaxVelocity + m_scriptMove[i+1].flMaxVelocity)); m_scriptMove[i].flTime = t; /* if (m_scriptMove[i].flDist < 0.01) { // Assert( m_scriptMove[i+1].pWaypoint == NULL ); m_scriptMove.Remove( i + 1 ); continue; } */ m_scriptMove[i+1].flElapsedTime = m_scriptMove[i].flElapsedTime + m_scriptMove[i].flTime; i++; } /* for (i = 0; i < m_scriptMove.Count(); i++) { DevMsg("(%.2f : %.2f : %.2f)", m_scriptMove[i].flMaxVelocity, m_scriptMove[i].flDist, m_scriptMove[i].flTime ); // DevMsg("(%.2f:%.2f)", m_scriptMove[i].flTime, m_scriptMove[i].flElapsedTime ); } DevMsg("\n"); */ }