//----------------------------------------------------------------------------- // Purpose: calculate intensity ratio for a worldlight by distance // Author: Valve Software //----------------------------------------------------------------------------- static float Engine_WorldLightDistanceFalloff( const dworldlight_t *wl, const Vector& delta ) { float falloff; switch (wl->type) { case emit_surface: // Cull out stuff that's too far if(wl->radius != 0) { if(DotProduct( delta, delta ) > (wl->radius * wl->radius)) return 0.0f; } return InvRSquared(delta); break; case emit_skylight: return 1.f; break; case emit_quakelight: // X - r; falloff = wl->linear_attn - FastSqrt( DotProduct( delta, delta ) ); if(falloff < 0) return 0.f; return falloff; break; case emit_skyambient: return 1.f; break; case emit_point: case emit_spotlight: // directional & positional { float dist2, dist; dist2 = DotProduct(delta, delta); dist = FastSqrt(dist2); // Cull out stuff that's too far if(wl->radius != 0 && dist > wl->radius) return 0.f; return 1.f / (wl->constant_attn + wl->linear_attn * dist + wl->quadratic_attn * dist2); } break; } return 1.f; }
void BaseZone::GenerateSlopeMap_ (float32* slopeMap, uint32 lengthX, uint32 lengthY) { // Note- this generate slope map method differs from the world gen generate slope map in that it does not sample neighboring height maps. In addition no max_ slope value needs to be enforced. const int32 townSlopeSampDist = 4; int32 x; int32 y; for (y = 0; y < (int32)lengthY; ++y) { for (x = 0; x < (int32)lengthX; ++x) { float32 divisor = 0.0f; float32 sum = 0.0f; for (int32 yOff = -townSlopeSampDist; yOff <= townSlopeSampDist; ++yOff) { for (int32 xOff = -townSlopeSampDist; xOff <= townSlopeSampDist; ++xOff) { if ((Square (xOff) + Square (yOff)) > Square(townSlopeSampDist + 1)) // keeps sample locations in a circular pattern rather than a square one. continue; if (x + xOff < 0 || x + xOff > (int32)lengthX || y + yOff < 0 || y + yOff > (int32)lengthY) continue; if (xOff == 0 && yOff == 0) continue; divisor += 1.0f; sum += Abs(heightMap_[x + y * lengthX] - heightMap_[(x + lengthX + xOff) % lengthX + ((y + lengthY + yOff) % lengthY) * lengthX]) / FastSqrt (float32 (Square (xOff) + Square (yOff))); // PERFORMANCE: use lookup table } } slopeMap[x + y * lengthX] = sum / divisor; } } }
//----------------------------------------------------------------------------- // Purpose: //----------------------------------------------------------------------------- void CASWInput::CalculateCameraShift( C_ASW_Player *pPlayer, float flDeltaX, float flDeltaY, float &flShiftX, float &flShiftY ) { // Init. flShiftX = 0.0f; flShiftY = 0.0f; if ( !asw_cam_marine_shift_enable.GetBool() ) return; if ( m_bCameraFixed || Holdout_Resupply_Frame::HasResupplyFrameOpen() || g_asw_iPlayerListOpen > 0 || ( pPlayer && pPlayer->GetSpectatingMarine() ) ) { m_fShiftFraction = Approach( 0.0f, m_fShiftFraction, gpGlobals->frametime ); } else { m_fShiftFraction = Approach( 1.0f, m_fShiftFraction, gpGlobals->frametime ); } if ( ASWGameRules() ) { m_fShiftFraction = m_fShiftFraction * ( 1.0f - ASWGameRules()->GetMarineDeathCamInterp() ); } flShiftX = flDeltaX * asw_cam_marine_shift_maxx.GetFloat() * m_fShiftFraction; float camshifty = (flDeltaY < 0) ? asw_cam_marine_shift_maxy.GetFloat() : asw_cam_marine_shift_maxy_south.GetFloat(); flShiftY = flDeltaY * camshifty * m_fShiftFraction; return; // Calculate the shift, spherically, based on the cursor distance from the player. float flDistance = FastSqrt( flDeltaX * flDeltaX + flDeltaY * flDeltaY ); if ( flDistance > asw_cam_marine_sphere_min.GetFloat() ) { flDistance -= asw_cam_marine_sphere_min.GetFloat(); float flRatio = 1.0f; if ( m_flCurrentCameraDist < asw_cam_marine_dist.GetFloat() ) { flRatio = ( m_flCurrentCameraDist / asw_cam_marine_dist.GetFloat() ) * 0.8f; } float flTemp = flDistance / ( asw_cam_marine_sphere_max.GetFloat() * flRatio ); flTemp = clamp( flTemp, 0.0f, 1.0f ); float flAngle = atan2( (float)flDeltaY, (float)flDeltaX ); flShiftX = cos( flAngle ) * flTemp * ( asw_cam_marine_shift_maxx.GetFloat() * flRatio ); if ( flDeltaY < 0 ) { flShiftY = sin( flAngle ) * flTemp * ( asw_cam_marine_shift_maxy.GetFloat() * flRatio ); } else { flShiftY = sin( flAngle ) * flTemp * ( asw_cam_marine_shift_maxy_south.GetFloat() * flRatio ); } } }
void FP_EKF_IMUInit(float *accel, float *gyro) { //NED coordinate system unit vector float nedVector[3] = {0, 0 , -1.0f}; float accelVector[3] = {0, 0 , 0}; float norm; float crossVector[3]; float sinwi, cosw, sinhalfw, coshalfw; float q[4]; //unit accel norm = FastSqrtI(accel[0] * accel[0] + accel[1] * accel[1] + accel[2] * accel[2]); accelVector[0] = accel[0] * norm; accelVector[1] = accel[1] * norm; accelVector[2] = accel[2] * norm; //cross product between accel and reference crossVector[0] = accelVector[1] * nedVector[2] - accelVector[2] * nedVector[1]; crossVector[1] = accelVector[2] * nedVector[0] - accelVector[0] * nedVector[2]; crossVector[2] = accelVector[0] * nedVector[1] - accelVector[1] * nedVector[0]; sinwi = FastSqrtI(crossVector[0] * crossVector[0] + crossVector[1] * crossVector[1] + crossVector[2] * crossVector[2]); crossVector[0] *= sinwi; crossVector[1] *= sinwi; crossVector[2] *= sinwi; //the angle between accel and reference is the dot product of the two vectors cosw = accelVector[0] * nedVector[0] + accelVector[1] * nedVector[1] + accelVector[2] * nedVector[2]; coshalfw = FastSqrt(0.5f + 0.5f * cosw); sinhalfw = FastSqrt(0.5f - 0.5f * cosw); q[0] = coshalfw; q[1] = crossVector[0] * sinhalfw; q[2] = crossVector[1] * sinhalfw; q[3] = crossVector[2] * sinhalfw; X[0] = FT_Q16(q[0]); X[1] = FT_Q16(q[1]); X[2] = FT_Q16(q[2]); X[3] = FT_Q16(q[3]); }
bool IsPrime(UInt32 inNumber, const std::vector<UInt32> & inPrecedingPrimes) { UInt32 squaredNumber = FastSqrt(inNumber); for (size_t idx = 1; idx < inPrecedingPrimes.size(); ++idx) { const UInt32 & primeFactor = inPrecedingPrimes[idx]; if (primeFactor > squaredNumber) { return true; } if (inNumber % primeFactor == 0) { return false; } } return true; }
//============================================================================================== //----------------------------------------------------------------------------- // Purpose: //----------------------------------------------------------------------------- float ReadDamageTable( impactentry_t *pTable, int tableCount, float impulse, bool bDebug ) { if ( pTable ) { int i; for ( i = 0; i < tableCount; i++ ) { if ( impulse < pTable[i].impulse ) break; } if ( i > 0 ) { i--; if ( bDebug ) { Msg("Damage %.0f, energy %.0f\n", pTable[i].damage, FastSqrt(impulse) ); } return pTable[i].damage; } } return 0; }
//----------------------------------------------------------------------------- bool CAI_PlaneSolver::GenerateCircleObstacleSuggestions( const AILocalMoveGoal_t &moveGoal, float probeDist ) { bool result = false; Vector npcLoc = m_pNpc->WorldSpaceCenter(); Vector mins, maxs; m_pNpc->CollisionProp()->WorldSpaceSurroundingBounds( &mins, &maxs ); float radiusNpc = (mins.AsVector2D() - maxs.AsVector2D()).Length() * 0.5; for ( int i = 0; i < m_Obstacles.Count(); i++ ) { CBaseEntity *pObstacleEntity = NULL; float zDistTooFar; if ( m_Obstacles[i].hEntity && m_Obstacles[i].hEntity->CollisionProp() ) { pObstacleEntity = m_Obstacles[i].hEntity.Get(); if( pObstacleEntity == moveGoal.pMoveTarget && (pObstacleEntity->IsNPC() || pObstacleEntity->IsPlayer()) ) { // HEY! I'm trying to avoid the very thing I'm trying to get to. This will make we wobble like a drunk as I approach. Don't do it. continue; } pObstacleEntity->CollisionProp()->WorldSpaceSurroundingBounds( &mins, &maxs ); zDistTooFar = ( maxs.z - mins.z ) * 0.5 + GetNpc()->GetHullHeight() * 0.5; } else zDistTooFar = GetNpc()->GetHullHeight(); if ( fabs( m_Obstacles[i].center.z - npcLoc.z ) > zDistTooFar ) continue; Vector vecToNpc = npcLoc - m_Obstacles[i].center; vecToNpc.z = 0; float distToObstacleSq = sq(vecToNpc.x) + sq(vecToNpc.y); float radius = m_Obstacles[i].radius + radiusNpc; if ( distToObstacleSq > 0.001 && distToObstacleSq < sq( radius + probeDist ) ) { Vector vecToObstacle = vecToNpc * -1; float distToObstacle = VectorNormalize( vecToObstacle ); float weight; float arc; float radiusSq = sq(radius); float flDot = DotProduct( vecToObstacle, moveGoal.dir ); // Don't steer around to avoid obstacles we've already passed, unless we're right up against them. // That is, do this computation without the probeDist added in. if( flDot < 0.0f && distToObstacleSq > radiusSq ) { continue; } if ( radiusSq < distToObstacleSq ) { Vector vecTangent; float distToTangent = FastSqrt( distToObstacleSq - radiusSq ); float oneOverDistToObstacleSq = 1 / distToObstacleSq; vecTangent.x = ( -distToTangent * vecToNpc.x + radius * vecToNpc.y ) * oneOverDistToObstacleSq; vecTangent.y = ( -distToTangent * vecToNpc.y - radius * vecToNpc.x ) * oneOverDistToObstacleSq; vecTangent.z = 0; float cosHalfArc = vecToObstacle.Dot( vecTangent ); arc = RAD2DEG(acosf( cosHalfArc )) * 2.0; weight = 1.0 - (distToObstacle - radius) / probeDist; if ( weight > 0.75 ) arc += (arc * 0.5) * (weight - 0.75) / 0.25; Assert( weight >= 0.0 && weight <= 1.0 ); #if DEBUG_OBSTACLES // ------------------------- Msg( "Adding arc %f, w %f\n", arc, weight ); Vector pointTangent = npcLoc + ( vecTangent * distToTangent ); NDebugOverlay::Line( npcLoc - Vector( 0, 0, 64 ), npcLoc + Vector(0,0,64), 0,255,0, false, 0.1 ); NDebugOverlay::Line( center - Vector( 0, 0, 64 ), center + Vector(0,0,64), 0,255,0, false, 0.1 ); NDebugOverlay::Line( pointTangent - Vector( 0, 0, 64 ), pointTangent + Vector(0,0,64), 0,255,0, false, 0.1 ); NDebugOverlay::Line( npcLoc + Vector(0,0,64), center + Vector(0,0,64), 0,0,255, false, 0.1 ); NDebugOverlay::Line( center + Vector(0,0,64), pointTangent + Vector(0,0,64), 0,0,255, false, 0.1 ); NDebugOverlay::Line( pointTangent + Vector(0,0,64), npcLoc + Vector(0,0,64), 0,0,255, false, 0.1 ); #endif } else { arc = 210; weight = 1.0; } if ( m_Obstacles[i].hEntity != NULL ) { weight = AdjustRegulationWeight( m_Obstacles[i].hEntity, weight ); } AI_MoveSuggestion_t suggestion( m_Obstacles[i].type, weight, UTIL_VecToYaw(vecToObstacle), arc ); m_Solver.AddRegulation( suggestion ); result = true; } } m_Obstacles.RemoveAll(); return result; }
void TokenChannels_t::Tokenize( const char *string ) { SetDefaultChannels(); if ( !string || !*string ) return; if ( Q_strlen( string ) > 128 ) { iActiveChannels = 1; bHasAnyValues = true; bReady = true; bChannelAsValue[0] = true; } while ( *string == ' ' ) string++; const bool bNormalize = string && (*string == 'n' || *string == 'N' ); if ( bNormalize ) string++; while ( *string == ' ' ) string++; char localString[ MAX_OPERATION_C ]; Q_snprintf( localString, sizeof(localString), "%s\0", string ); char *MakeUpper = localString; while( MakeUpper && *MakeUpper ) { if ( *MakeUpper == 'e' ) *MakeUpper = 'E'; else if ( *MakeUpper == 'p' ) *MakeUpper = 'P'; else if ( *MakeUpper == 'i' ) *MakeUpper = 'I'; MakeUpper++; } const char *stringtest = localString; while (stringtest && *stringtest) { if ( ISSTRING_LITERAL( localString, stringtest ) || ISSTRING_VALUE( localString, stringtest ) ) bHasAnyValues = true; stringtest++; } char *cleanupReader = localString; while ( *cleanupReader ) { if ( !ISSTRING_VALUE( localString, cleanupReader ) && *cleanupReader != ' ' && !ISSTRING_OPERATOR( cleanupReader ) && GetChannelNumFromChar( cleanupReader ) < 0 && !ISSTRING_LITERAL( localString, cleanupReader ) && !ISCHAR_BRACKET( *cleanupReader ) && *cleanupReader != '$' ) { SnipCharFromString( cleanupReader ); //cleanupReader--; } cleanupReader++; } char cleanupString[MAX_OPERATION_C]; cleanupReader = cleanupString; Q_strcpy( cleanupString, localString ); Q_memset( localString, 0, sizeof( localString ) ); #define STEST_VALUE 0 #define STEST_OPERATOR 1 #define STEST_BRACKET_OPEN 2 #define STEST_BRACKET_CLOSE 3 #define STEST_BRACKET( x ) ( x == STEST_BRACKET_OPEN || x == STEST_BRACKET_CLOSE ) while( *cleanupReader ) { if ( *cleanupReader == ' ' ) { const char *searchfront = cleanupReader; const char *searchback = cleanupReader; int priorToSpace = -1; int afterSpace = -1; while ( searchfront >= cleanupString ) { if ( ISSTRING_VALUE( cleanupString, searchfront ) || ISSTRING_LITERAL( cleanupString, searchfront ) || *searchfront == '$' ) priorToSpace = STEST_VALUE; else if ( ISSTRING_OPERATOR( searchfront ) ) priorToSpace = STEST_OPERATOR; else if ( ISCHAR_BOPEN( *searchfront ) ) priorToSpace = STEST_BRACKET_OPEN; else if ( ISCHAR_BCLOSE( *searchfront ) ) priorToSpace = STEST_BRACKET_CLOSE; else if ( GetChannelNumFromChar( searchfront ) >= 0 ) break; if ( priorToSpace >= 0 ) break; searchfront--; } while ( *searchback ) { if ( ISSTRING_VALUE( cleanupString, searchback ) || ISSTRING_LITERAL( cleanupString, searchback ) || *searchback == '$' ) afterSpace = STEST_VALUE; else if ( ISSTRING_OPERATOR( searchback ) ) afterSpace = STEST_OPERATOR; else if ( ISCHAR_BOPEN( *searchback ) ) afterSpace = STEST_BRACKET_OPEN; else if ( ISCHAR_BCLOSE( *searchback ) ) afterSpace = STEST_BRACKET_CLOSE; else if ( GetChannelNumFromChar( searchback ) >= 0 ) break; if ( afterSpace >= 0 ) break; searchback++; } // 1 * ( 2 + 1 ) ( 3 ) + 1 // 1 ( 10 ) 0 if ( ( !STEST_BRACKET(priorToSpace) && !STEST_BRACKET(afterSpace) && priorToSpace != afterSpace && priorToSpace >= 0 && afterSpace >= 0) || priorToSpace == STEST_BRACKET_OPEN || afterSpace == STEST_BRACKET_CLOSE || (STEST_BRACKET(priorToSpace) && afterSpace == STEST_OPERATOR ) || //!STEST_BRACKET(afterSpace)) || (STEST_BRACKET(afterSpace) && priorToSpace == STEST_OPERATOR ) //!STEST_BRACKET(priorToSpace)) ) { SnipCharFromString( cleanupReader ); cleanupReader--; } } cleanupReader++; } Q_strcpy( localString, cleanupString ); char *GlobalExpression = localString; while( *GlobalExpression ) { if ( *GlobalExpression == '$' ) { *GlobalExpression = '\0'; GlobalExpression++; while ( *GlobalExpression == ' ' ) GlobalExpression++; char *SetEnd = GlobalExpression; while ( *SetEnd ) { if ( *SetEnd == '$' ) *SetEnd = '\0'; SetEnd++; } break; } GlobalExpression++; } #if 0 cleanupReader = localString; while( *cleanupReader ) { if ( ISCHAR_BRACKET( *cleanupReader ) ) SnipCharFromString( cleanupReader ); cleanupReader++; } #endif iActiveChannels = 0; if ( !bHasAnyValues && bAllowChars ) { int slotWrite = 0; const char *walk = localString; while (walk && *walk && slotWrite < 4) { if ( GetChannelNumFromChar( walk ) >= 0 ) { bChannelAsValue[ slotWrite ] = false; cChannel[ slotWrite ] = *walk; slotWrite++; iActiveChannels++; } walk++; } } else { SimpleTokenize _tokens; _tokens.Tokenize( localString ); //CCommand args; //args.Tokenize( localString ); for ( int i = 0; i < min( _tokens.Count(), 4 ); i++ ) //for ( int i = 0; i < min( args.ArgC(), 4 ); i++ ) { const char *token = _tokens.GetToken( i ); //const char *token = args[i]; if ( GetChannelNumFromChar( token ) >= 0 && bAllowChars ) { bChannelAsValue[ i ] = false; cChannel[ i ] = *token; } else { bChannelAsValue[ i ] = true; const bool bDoGlExp = (GlobalExpression && *GlobalExpression); char tmp[ MAX_OPERATION_C ]; char *target = tmp; if ( bDoGlExp ) { *target = '('; target++; } Q_strcpy( target, token ); if ( bDoGlExp ) { Q_strcat( tmp, ")", MAX_OPERATION_C ); Q_strcat( tmp, GlobalExpression, MAX_OPERATION_C ); } flValue[ i ] = PerformSolve( tmp ); } iActiveChannels++; } } bReady = !!iActiveChannels; Assert( iActiveChannels >= 0 && iActiveChannels <= 4 ); iChannelDepth = 0; for ( int i = 0; i < iActiveChannels; i++ ) if ( bChannelAsValue[ i ] ) bHasAnyValues = true; for ( int i = 0; i < iActiveChannels; i++ ) { iChannel[ i ] = 0; if ( bChannelAsValue[i] ) continue; iChannel[ i ] = GetChannelNumFromChar( &cChannel[i] ); int num = iChannel[ i ] + 1; iChannelDepth = max( num, iChannelDepth ); } // manual normalize if ( bNormalize ) { float flVec[4]; Q_memset( flVec, 0, sizeof(flVec) ); for ( int i = 0; i < iActiveChannels; i++ ) if ( bChannelAsValue[ i ] ) flVec[ i ] = flValue[ i ]; double vecSize = 0; for ( int i = 0; i < 4; i++ ) vecSize += Sqr( flVec[i] ); if ( vecSize == 0 ) vecSize = 1; vecSize = FastSqrt( vecSize ); vecSize = 1.0f / vecSize; for ( int i = 0; i < 4; i++ ) if ( bChannelAsValue[ i ] ) flValue[ i ] *= vecSize; } }
bool CAI_PlaneSolver::GenerateCircleObstacleSuggestions( float probeDist ) { bool result = false; Vector npcLoc = m_pNpc->WorldSpaceCenter(); Vector mins, maxs; m_pNpc->CollisionProp()->WorldSpaceSurroundingBounds( &mins, &maxs ); float radiusNpc = (mins.AsVector2D() - maxs.AsVector2D()).Length() * 0.5; for ( int i = 0; i < m_Obstacles.Count(); i++ ) { float zDistTooFar; if ( m_Obstacles[i].hEntity && m_Obstacles[i].hEntity->CollisionProp() ) { m_Obstacles[i].hEntity->CollisionProp()->WorldSpaceSurroundingBounds( &mins, &maxs ); zDistTooFar = ( maxs.z - mins.z ) * 0.5 + GetNpc()->GetHullHeight() * 0.5; } else zDistTooFar = GetNpc()->GetHullHeight(); if ( fabs( m_Obstacles[i].center.z - npcLoc.z ) > zDistTooFar ) continue; Vector vecToNpc = npcLoc - m_Obstacles[i].center; vecToNpc.z = 0; float distToObstacleSq = sq(vecToNpc.x) + sq(vecToNpc.y); float radius = m_Obstacles[i].radius + radiusNpc; if ( distToObstacleSq > 0.001 && distToObstacleSq < sq(radius + probeDist)) { Vector vecToObstacle = vecToNpc * -1; float distToObstacle = VectorNormalize( vecToObstacle ); float weight; float arc; float radiusSq = sq(radius); if ( radiusSq < distToObstacleSq ) { Vector vecTangent; float distToTangent = FastSqrt( distToObstacleSq - radiusSq ); float oneOverDistToObstacleSq = 1 / distToObstacleSq; vecTangent.x = ( -distToTangent * vecToNpc.x + radius * vecToNpc.y ) * oneOverDistToObstacleSq; vecTangent.y = ( -distToTangent * vecToNpc.y - radius * vecToNpc.x ) * oneOverDistToObstacleSq; vecTangent.z = 0; float cosHalfArc = vecToObstacle.Dot( vecTangent ); arc = RAD2DEG(acosf( cosHalfArc )) * 2.0; weight = 1.0 - (distToObstacle - radius) / probeDist; if ( weight > 0.75 ) arc += (arc * 0.5) * (weight - 0.75) / 0.25; Assert( weight >= 0.0 && weight <= 1.0 ); #if DEBUG_OBSTACLES // ------------------------- Msg( "Adding arc %f, w %f\n", arc, weight ); Vector pointTangent = npcLoc + ( vecTangent * distToTangent ); NDebugOverlay::Line( npcLoc - Vector( 0, 0, 64 ), npcLoc + Vector(0,0,64), 0,255,0, false, 0.1 ); NDebugOverlay::Line( center - Vector( 0, 0, 64 ), center + Vector(0,0,64), 0,255,0, false, 0.1 ); NDebugOverlay::Line( pointTangent - Vector( 0, 0, 64 ), pointTangent + Vector(0,0,64), 0,255,0, false, 0.1 ); NDebugOverlay::Line( npcLoc + Vector(0,0,64), center + Vector(0,0,64), 0,0,255, false, 0.1 ); NDebugOverlay::Line( center + Vector(0,0,64), pointTangent + Vector(0,0,64), 0,0,255, false, 0.1 ); NDebugOverlay::Line( pointTangent + Vector(0,0,64), npcLoc + Vector(0,0,64), 0,0,255, false, 0.1 ); #endif } else { arc = 210; weight = 1.0; } if ( m_Obstacles[i].hEntity != NULL ) { weight = AdjustRegulationWeight( m_Obstacles[i].hEntity, weight ); } AI_MoveSuggestion_t suggestion( m_Obstacles[i].type, weight, UTIL_VecToYaw(vecToObstacle), arc ); m_Solver.AddRegulation( suggestion ); result = true; } } m_Obstacles.RemoveAll(); return result; }