//----------------------------------------------------------------------------- // Purpose: // // // Output : //----------------------------------------------------------------------------- void CNPC_RollerDozer::RunTask( const Task_t *pTask ) { switch( pTask->iTask ) { case TASK_ROLLERDOZER_CLEAR_DEBRIS: if( gpGlobals->curtime > m_flWaitFinished ) { m_hDebris = NULL; m_flTimeDebrisSearch = gpGlobals->curtime; TaskComplete(); } else if( m_hDebris != NULL ) { float yaw = UTIL_VecToYaw( m_hDebris->GetLocalOrigin() - GetLocalOrigin() ); Vector vecRight, vecForward; AngleVectors( QAngle( 0, yaw, 0 ), &vecForward, &vecRight, NULL ); //Stop pushing if I'm going to push this object sideways or back towards the center of the cleanup area. Vector vecCleanupDir = m_hDebris->GetLocalOrigin() - m_vecCleanupPoint; VectorNormalize( vecCleanupDir ); if( DotProduct( vecForward, vecCleanupDir ) < -0.5 ) { // HACKHACK !!!HACKHACK - right now forcing an unstick. Do this better (sjb) // Clear the debris, suspend the search for debris, trick base class into unsticking me. m_hDebris = NULL; m_flTimeDebrisSearch = gpGlobals->curtime + 4; m_iFail = 10; TaskFail("Pushing Wrong Way"); } m_RollerController.m_vecAngular = WorldToLocalRotation( SetupMatrixAngles(GetLocalAngles()), vecRight, ROLLERDOZER_FORWARD_SPEED * 2 ); } else { TaskFail("No debris!!"); } break; default: BaseClass::RunTask( pTask ); break; } }
IMotionEvent::simresult_e CGravControllerPoint::Simulate( IPhysicsMotionController *pController, IPhysicsObject *pObject, float deltaTime, Vector &linear, AngularImpulse &angular ) { Vector vel; AngularImpulse angVel; float fracRemainingSimTime = 1.0; if ( m_timeToArrive > 0 ) { fracRemainingSimTime *= deltaTime / m_timeToArrive; if ( fracRemainingSimTime > 1 ) { fracRemainingSimTime = 1; } } m_timeToArrive -= deltaTime; if ( m_timeToArrive < 0 ) { m_timeToArrive = 0; } float invDeltaTime = (1.0f / deltaTime); Vector world; pObject->LocalToWorld( &world, m_localPosition ); m_worldPosition = world; pObject->GetVelocity( &vel, &angVel ); //pObject->GetVelocityAtPoint( world, &vel ); float damping = 1.0; world += vel * deltaTime * damping; Vector delta = (m_targetPosition - world) * fracRemainingSimTime * invDeltaTime; Vector alignDir; linear = vec3_origin; angular = vec3_origin; if ( m_align ) { QAngle angles; Vector origin; Vector axis; AngularImpulse torque; pObject->GetShadowPosition( &origin, &angles ); // align local normal to target normal VMatrix tmp = SetupMatrixOrgAngles( origin, angles ); Vector worldNormal = tmp.VMul3x3( m_localAlignNormal ); axis = CrossProduct( worldNormal, m_targetAlignNormal ); float trig = VectorNormalize(axis); float alignRotation = RAD2DEG(asin(trig)); axis *= alignRotation; if ( alignRotation < 10 ) { float dot = DotProduct( worldNormal, m_targetAlignNormal ); // probably 180 degrees off if ( dot < 0 ) { if ( worldNormal.x < 0.5 ) { axis.Init(10,0,0); } else { axis.Init(0,0,10); } alignRotation = 10; } } // Solve for the rotation around the target normal (at the local align pos) that will // move the grabbed spot to the destination. Vector worldRotCenter = tmp.VMul4x3( m_localAlignPosition ); Vector rotSrc = world - worldRotCenter; Vector rotDest = m_targetPosition - worldRotCenter; // Get a basis in the plane perpendicular to m_targetAlignNormal Vector srcN = rotSrc; VectorNormalize( srcN ); Vector tangent = CrossProduct( srcN, m_targetAlignNormal ); float len = VectorNormalize( tangent ); // needs at least ~5 degrees, or forget rotation (0.08 ~= sin(5)) if ( len > 0.08 ) { Vector binormal = CrossProduct( m_targetAlignNormal, tangent ); // Now project the src & dest positions into that plane Vector planeSrc( DotProduct( rotSrc, tangent ), DotProduct( rotSrc, binormal ), 0 ); Vector planeDest( DotProduct( rotDest, tangent ), DotProduct( rotDest, binormal ), 0 ); float rotRadius = VectorNormalize( planeSrc ); float destRadius = VectorNormalize( planeDest ); if ( rotRadius > 0.1 ) { if ( destRadius < rotRadius ) { destRadius = rotRadius; } //float ratio = rotRadius / destRadius; float angleSrc = atan2( planeSrc.y, planeSrc.x ); float angleDest = atan2( planeDest.y, planeDest.x ); float angleDiff = angleDest - angleSrc; angleDiff = RAD2DEG(angleDiff); axis += m_targetAlignNormal * angleDiff; //world = m_targetPosition;// + rotDest * (1-ratio); // NDebugOverlay::Line( worldRotCenter, worldRotCenter-m_targetAlignNormal*50, 255, 0, 0, false, 0.1 ); // NDebugOverlay::Line( worldRotCenter, worldRotCenter+tangent*50, 0, 255, 0, false, 0.1 ); // NDebugOverlay::Line( worldRotCenter, worldRotCenter+binormal*50, 0, 0, 255, false, 0.1 ); } } torque = WorldToLocalRotation( tmp, axis, 1 ); torque *= fracRemainingSimTime * invDeltaTime; torque -= angVel * 1.0; // damping for ( int i = 0; i < 3; i++ ) { if ( torque[i] > 0 ) { if ( torque[i] > m_maxAngularAcceleration[i] ) torque[i] = m_maxAngularAcceleration[i]; } else { if ( torque[i] < -m_maxAngularAcceleration[i] ) torque[i] = -m_maxAngularAcceleration[i]; } } torque *= invDeltaTime; angular += torque; // Calculate an acceleration that pulls the object toward the constraint // When you're out of alignment, don't pull very hard float factor = fabsf(alignRotation); if ( factor < 5 ) { factor = clamp( factor, 0, 5 ) * (1/5); alignDir = m_targetAlignPosition - worldRotCenter; // Limit movement to the part along m_targetAlignNormal if worldRotCenter is on the backside of // of the target plane (one inch epsilon)! float planeForward = DotProduct( alignDir, m_targetAlignNormal ); if ( planeForward > 1 ) { alignDir = m_targetAlignNormal * planeForward; } Vector accel = alignDir * invDeltaTime * fracRemainingSimTime * (1-factor) * 0.20 * invDeltaTime; float mag = accel.Length(); if ( mag > m_maxAcceleration ) { accel *= (m_maxAcceleration/mag); } linear += accel; } linear -= vel*damping*invDeltaTime; // UNDONE: Factor in the change in worldRotCenter due to applied torque! } else { // clamp future velocity to max speed Vector nextVel = delta + vel; float nextSpeed = nextVel.Length(); if ( nextSpeed > m_maxVel ) { nextVel *= (m_maxVel / nextSpeed); delta = nextVel - vel; } delta *= invDeltaTime; float linearAccel = delta.Length(); if ( linearAccel > m_maxAcceleration ) { delta *= m_maxAcceleration / linearAccel; } Vector accel; AngularImpulse angAccel; pObject->CalculateForceOffset( delta, world, &accel, &angAccel ); linear += accel; angular += angAccel; } return SIM_GLOBAL_ACCELERATION; }
//--------------------------------------------------------- //--------------------------------------------------------- void CNPC_Roller::RunTask( const Task_t *pTask ) { switch( pTask->iTask ) { case TASK_ROLLER_UNSTICK: { float yaw = UTIL_VecToYaw( m_vecUnstickDirection ); Vector vecRight; AngleVectors( QAngle( 0, yaw, 0 ), NULL, &vecRight, NULL ); m_RollerController.m_vecAngular = WorldToLocalRotation( SetupMatrixAngles(GetLocalAngles()), vecRight, m_flForwardSpeed ); } if( gpGlobals->curtime > m_flWaitFinished ) { TaskComplete(); } break; case TASK_ROLLER_WAIT_FOR_PHYSICS: { Vector vecVelocity; VPhysicsGetObject()->GetVelocity( &vecVelocity, NULL ); if( VPhysicsGetObject()->IsAsleep() ) { TaskComplete(); } } break; case TASK_RUN_PATH: case TASK_WALK_PATH: // Start turning early if( (GetLocalOrigin() - GetNavigator()->GetCurWaypointPos() ).Length() <= 64 ) { if( GetNavigator()->CurWaypointIsGoal() ) { // Hit the brakes a bit. float yaw = UTIL_VecToYaw( GetNavigator()->GetCurWaypointPos() - GetLocalOrigin() ); Vector vecRight; AngleVectors( QAngle( 0, yaw, 0 ), NULL, &vecRight, NULL ); m_RollerController.m_vecAngular += WorldToLocalRotation( SetupMatrixAngles(GetLocalAngles()), vecRight, -m_flForwardSpeed * 5 ); TaskComplete(); return; } GetNavigator()->AdvancePath(); } { float yaw = UTIL_VecToYaw( GetNavigator()->GetCurWaypointPos() - GetLocalOrigin() ); Vector vecRight; Vector vecToPath; // points at the path AngleVectors( QAngle( 0, yaw, 0 ), &vecToPath, &vecRight, NULL ); // figure out if the roller is turning. If so, cut the throttle a little. float flDot; Vector vecVelocity; VPhysicsGetObject()->GetVelocity( &vecVelocity, NULL ); VectorNormalize( vecVelocity ); vecVelocity.z = 0; flDot = DotProduct( vecVelocity, vecToPath ); m_RollerController.m_vecAngular = vec3_origin; if( flDot > 0.25 && flDot < 0.7 ) { // Feed a little torque backwards into the axis perpendicular to the velocity. // This will help get rid of momentum that would otherwise make us overshoot our goal. Vector vecCompensate; vecCompensate.x = vecVelocity.y; vecCompensate.y = -vecVelocity.x; vecCompensate.z = 0; m_RollerController.m_vecAngular = WorldToLocalRotation( SetupMatrixAngles(GetLocalAngles()), vecCompensate, m_flForwardSpeed * -0.75 ); } m_RollerController.m_vecAngular += WorldToLocalRotation( SetupMatrixAngles(GetLocalAngles()), vecRight, m_flForwardSpeed ); } break; case TASK_ROLLER_ISSUE_CODE: if( gpGlobals->curtime >= m_flWaitFinished ) { if( m_iCodeProgress == ROLLER_CODE_DIGITS ) { TaskComplete(); } else { m_flWaitFinished = gpGlobals->curtime + ROLLER_TONE_TIME; CPASAttenuationFilter filter( this ); EmitSound( filter, entindex(), CHAN_BODY, pCodeSounds[ m_iAccessCode[ m_iCodeProgress ] ], 1.0, ATTN_NORM ); m_iCodeProgress++; } } break; default: BaseClass::RunTask( pTask ); break; } }