//-----------------------------------------------------------------------------
// Purpose: Main raycast airboat simulation.
//-----------------------------------------------------------------------------
void IVP_Controller_Raycast_Airboat::do_simulation_controller( IVP_Event_Sim *pEventSim, IVP_U_Vector<IVP_Core> * )
{
IVP_Raycast_Airboat_Pontoon_Temp pontoonsTemp[IVP_RAYCAST_AIRBOAT_MAX_WHEELS];
IVP_Ray_Solver_Template raySolverTemplates[IVP_RAYCAST_AIRBOAT_MAX_WHEELS];
IVP_Raycast_Airboat_Impact impacts[IVP_RAYCAST_AIRBOAT_MAX_WHEELS];
IVP_Core *pAirboatCore = m_pAirboatBody->get_core();
const IVP_U_Matrix *matWorldFromCore = pAirboatCore->get_m_world_f_core_PSI();
matWorldFromCore->vimult3(&(pAirboatCore->speed), &m_LocalVelocity);
// Raycasts.
PreRaycasts( raySolverTemplates, matWorldFromCore, pontoonsTemp );
do_raycasts_gameside( n_wheels, raySolverTemplates, impacts );
if ( !PostRaycasts( raySolverTemplates, matWorldFromCore, pontoonsTemp, impacts ) )
return;
// Airborne state;
UpdateAirborneState(impacts, pEventSim);
// Pontoons.
DoSimulationPontoons(pontoonsTemp, impacts, pEventSim);
// Drag (Water)
DoSimulationDrag(pontoonsTemp, impacts, pEventSim);
// Turbine (fan).
DoSimulationTurbine(pEventSim);
// Steering.
DoSimulationSteering(pEventSim);
// Keep upright.
DoSimulationKeepUprightPitch(impacts, pEventSim);
DoSimulationKeepUprightRoll(impacts, pEventSim);
}
void CPhysicsObject::CalculateVelocityOffset( const Vector &forceVector, const Vector &worldPosition, Vector ¢erVelocity, AngularImpulse ¢erAngularVelocity )
{
IVP_U_Point pos;
IVP_U_Float_Point force;
ConvertForceImpulseToIVP( forceVector, force );
ConvertPositionToIVP( worldPosition, pos );
IVP_Core *core = m_pObject->get_core();
const IVP_U_Matrix *m_world_f_core = core->get_m_world_f_core_PSI();
IVP_U_Float_Point point_d_ws;
point_d_ws.subtract(&pos, m_world_f_core->get_position());
IVP_U_Float_Point cross_point_dir;
cross_point_dir.calc_cross_product( &point_d_ws, &force);
m_world_f_core->inline_vimult3( &cross_point_dir, &cross_point_dir);
cross_point_dir.set_pairwise_mult( &cross_point_dir, core->get_inv_rot_inertia());
ConvertAngularImpulseToHL( cross_point_dir, centerAngularVelocity );
force.set_multiple( &force, core->get_inv_mass() );
ConvertForceImpulseToHL( force, centerVelocity );
}
//-----------------------------------------------------------------------------
// Purpose: Main raycast airboat simulation.
//-----------------------------------------------------------------------------
void IVP_Controller_Raycast_Airboat::do_simulation_controller( IVP_Event_Sim *pEventSim, IVP_U_Vector<IVP_Core> * )
{
IVP_Raycast_Airboat_Pontoon_Temp pontoonsTemp[IVP_RAYCAST_AIRBOAT_MAX_WHEELS];
IVP_Ray_Solver_Template raySolverTemplates[IVP_RAYCAST_AIRBOAT_MAX_WHEELS];
IVP_Raycast_Airboat_Impact impacts[IVP_RAYCAST_AIRBOAT_MAX_WHEELS];
IVP_Core *pAirboatCore = m_pAirboatBody->get_core();
const IVP_U_Matrix *matWorldFromCore = pAirboatCore->get_m_world_f_core_PSI();
// Raycasts.
PreRaycasts( raySolverTemplates, matWorldFromCore, pontoonsTemp );
do_raycasts_gameside( n_wheels, raySolverTemplates, impacts );
if ( !PostRaycasts( raySolverTemplates, matWorldFromCore, pontoonsTemp, impacts, pAirboatCore ) )
return;
// Pontoons.
DoSimulationPontoons( pontoonsTemp, impacts, pEventSim, pAirboatCore );
// Drag (Water)
DoSimulationDrag( pontoonsTemp, pEventSim, pAirboatCore );
// Turbine (fan).
DoSimulationTurbine( pontoonsTemp, pEventSim, pAirboatCore );
// Steering.
DoSimulationSteering( pontoonsTemp, pAirboatCore, pEventSim );
}
void CPlayerController::do_simulation_controller( IVP_Event_Sim *es,IVP_U_Vector<IVP_Core> *)
{
if ( !m_enable )
return;
IVP_Real_Object *pivp = m_pObject->GetObject();
IVP_Environment *pIVPEnv = pivp->get_environment();
CPhysicsEnvironment *pVEnv = (CPhysicsEnvironment *)pIVPEnv->client_data;
float psiScale = pVEnv->GetInvPSIScale();
if ( !psiScale )
return;
IVP_Core *pCore = pivp->get_core();
// current situation
const IVP_U_Matrix *m_world_f_core = pCore->get_m_world_f_core_PSI();
const IVP_U_Point *cur_pos_ws = m_world_f_core->get_position();
// ---------------------------------------------------------
// Translation
// ---------------------------------------------------------
IVP_U_Float_Point delta_position; delta_position.subtract( &m_targetPosition, cur_pos_ws);
if (!pivp->flags.shift_core_f_object_is_zero)
{
IVP_U_Float_Point shift_cs_os_ws;
m_world_f_core->vmult3( pivp->get_shift_core_f_object(), &shift_cs_os_ws);
delta_position.subtract( &shift_cs_os_ws );
}
IVP_DOUBLE qdist = delta_position.quad_length();
// UNDONE: This is totally bogus! Measure error using last known estimate
// not current position!
if ( qdist > m_maxDeltaPosition * m_maxDeltaPosition )
{
if ( TryTeleportObject() )
return;
}
// float to allow stepping
if ( m_onground )
{
const IVP_U_Point *pgrav = es->environment->get_gravity();
IVP_U_Float_Point gravSpeed;
gravSpeed.set_multiple( pgrav, es->delta_time );
pCore->speed.subtract( &gravSpeed );
}
ComputeController( pCore->speed, delta_position, m_maxSpeed, psiScale / es->delta_time, m_dampFactor );
}
//-----------------------------------------------------------------------------
// Purpose:
//-----------------------------------------------------------------------------
void IVP_Controller_Raycast_Airboat::DoSimulationTurbine(IVP_Event_Sim *pEventSim)
{
IVP_Core *pAirboatCore = m_pAirboatBody->get_core();
IVP_FLOAT thrust = m_flThrust;
if (m_bAirborneIdle || (m_bAirborne && (thrust < 0.0f)))
thrust *= 0.5f;
IVP_U_Float_Point vecImpulse;
pAirboatCore->get_m_world_f_core_PSI()->get_col(IVP_COORDINATE_INDEX(index_z), &vecImpulse);
IVP_FLOAT flForwardY = vecImpulse.k[1];
if ((flForwardY < -0.5f) && (thrust > 0.0f))
thrust *= 1.0f + flForwardY;
else if ((flForwardY > 0.5f) && (thrust < 0.0f))
thrust *= 1.0f - flForwardY;
vecImpulse.mult(pAirboatCore->get_mass() * thrust * pEventSim->delta_time);
pAirboatCore->center_push_core_multiple_ws(&vecImpulse);
}
void IVP_Controller_Raycast_Airboat::DoSimulationKeepUprightRoll(IVP_Raycast_Airboat_Impact *pImpacts,
IVP_Event_Sim *pEventSim)
{
IVP_Core *pAirboatCore = m_pAirboatBody->get_core();
IVP_U_Float_Point dir, temp, cross;
temp.set(0.0f, -1.0f, 0.0f);
pAirboatCore->get_m_world_f_core_PSI()->vimult3(&temp, &dir);
dir.k[1] = -0.9848077f; // -cos(10deg)
dir.normize();
temp.set(0.0f, -0.9848077f, 0.17364818f); // 0.0, -cos(10deg), sin(10deg)
cross.calc_cross_product(&temp, &dir);
IVP_DOUBLE roll = IVP_Inline_Math::atan2d(cross.real_length_plus_normize(), temp.dot_product(&dir));
IVP_BOOL bImpact = IVP_FALSE;
int iPoint;
for (iPoint = 0; iPoint < n_wheels; ++iPoint, ++pImpacts)
{
if (pImpacts->bImpact)
{
bImpact = IVP_TRUE;
break;
}
}
if (bImpact || (fabs(roll) < (10.0 * IVP_PI / 180.0)))
{
m_flKeepUprightRoll = roll;
return;
}
IVP_FLOAT mass = pAirboatCore->get_mass();
temp.set_multiple(&cross, (pEventSim->i_delta_time * 0.3 * (roll - m_flKeepUprightRoll) + roll * 0.2) * mass);
m_flKeepUprightRoll = roll;
mass *= (2.0 * IVP_PI / 180.0);
IVP_FLOAT length = temp.real_length_plus_normize();
if (length > mass)
length = mass;
temp.mult(length);
pAirboatCore->rot_push_core_cs(&temp);
}
void CPhysicsObject::GetVelocityAtPoint( const Vector &worldPosition, Vector &velocity )
{
IVP_Core *core = m_pObject->get_core();
IVP_U_Point pos;
ConvertPositionToIVP( worldPosition, pos );
IVP_U_Float_Point rotSpeed;
rotSpeed.add( &core->rot_speed, &core->rot_speed_change );
IVP_U_Float_Point av_ws;
core->get_m_world_f_core_PSI()->vmult3( &rotSpeed, &av_ws);
IVP_U_Float_Point pos_rel;
pos_rel.subtract( &pos, core->get_position_PSI());
IVP_U_Float_Point cross;
cross.inline_calc_cross_product(&av_ws,&pos_rel);
IVP_U_Float_Point speed;
speed.add(&core->speed, &cross);
speed.add(&core->speed_change);
ConvertPositionToHL( speed, velocity );
}
void CPhysicsObject::CalculateForceOffset( const Vector &forceVector, const Vector &worldPosition, Vector ¢erForce, AngularImpulse ¢erTorque )
{
IVP_U_Point pos;
IVP_U_Float_Point force;
ConvertPositionToIVP( forceVector, force );
ConvertPositionToIVP( worldPosition, pos );
IVP_Core *core = m_pObject->get_core();
const IVP_U_Matrix *m_world_f_core = core->get_m_world_f_core_PSI();
IVP_U_Float_Point point_d_ws;
point_d_ws.subtract(&pos, m_world_f_core->get_position());
IVP_U_Float_Point cross_point_dir;
cross_point_dir.calc_cross_product( &point_d_ws, &force);
m_world_f_core->inline_vimult3( &cross_point_dir, &cross_point_dir);
ConvertAngularImpulseToHL( cross_point_dir, centerTorque );
ConvertForceImpulseToHL( force, centerForce );
}
void IVP_Controller_Raycast_Airboat::DoSimulationKeepUprightPitch(IVP_Raycast_Airboat_Impact *pImpacts,
IVP_Event_Sim *pEventSim)
{
if (m_bAirborneIdle)
return;
IVP_Core *pAirboatCore = m_pAirboatBody->get_core();
IVP_U_Float_Point dir, temp, cross;
temp.set(0.0f, -1.0f, 0.0f);
pAirboatCore->get_m_world_f_core_PSI()->vimult3(&temp, &dir);
dir.k[0] = 0.0f;
dir.normize();
temp.set(0.0f, -0.9848077f, 0.17364818f); // 0.0, -cos(10deg), sin(10deg)
cross.calc_cross_product(&temp, &dir);
IVP_DOUBLE pitch = IVP_Inline_Math::atan2d(cross.real_length_plus_normize(), temp.dot_product(&dir));
int iPoint;
for (iPoint = 0; iPoint < n_wheels; ++iPoint, ++pImpacts)
{
if (pImpacts->bImpact)
{
m_flKeepUprightPitch = pitch;
return;
}
}
IVP_FLOAT mass = pAirboatCore->get_mass();
temp.set_multiple(&cross, (pEventSim->i_delta_time * 0.04 * (pitch - m_flKeepUprightPitch) + pitch * 0.1) * mass);
m_flKeepUprightPitch = pitch;
mass *= (1.5 * IVP_PI / 180.0);
IVP_FLOAT length = temp.real_length_plus_normize();
if (length > mass)
length = mass;
temp.mult(length);
pAirboatCore->rot_push_core_cs(&temp);
}
void CPhysicsMotionController::do_simulation_controller(IVP_Event_Sim *event,IVP_U_Vector<IVP_Core> *core_list)
{
if ( m_handler )
{
for ( int i = 0; i < core_list->len(); i++ )
{
IVP_U_Float_Point ivpSpeed, ivpRot;
IVP_Core *pCore = core_list->element_at(i);
Vector speed;
AngularImpulse rot;
IVP_Real_Object *pivp = pCore->objects.element_at(0);
IPhysicsObject *pPhys = static_cast<IPhysicsObject *>(pivp->client_data);
IMotionEvent::simresult_e ret = m_handler->Simulate( this, pPhys, event->delta_time, speed, rot );
switch( ret )
{
case IMotionEvent::SIM_NOTHING:
break;
case IMotionEvent::SIM_LOCAL_ACCELERATION:
{
ConvertForceImpulseToIVP( speed, ivpSpeed );
ConvertAngularImpulseToIVP( rot, ivpRot );
const IVP_U_Matrix *m_world_f_core = pCore->get_m_world_f_core_PSI();
// transform to world space
m_world_f_core->inline_vmult3( &ivpSpeed, &ivpSpeed );
// UNDONE: Put these values into speed change / rot_speed_change instead?
pCore->speed.add_multiple( &ivpSpeed, event->delta_time );
pCore->rot_speed.add_multiple( &ivpRot, event->delta_time );
}
break;
case IMotionEvent::SIM_LOCAL_FORCE:
{
ConvertForceImpulseToIVP( speed, ivpSpeed );
ConvertAngularImpulseToIVP( rot, ivpRot );
const IVP_U_Matrix *m_world_f_core = pCore->get_m_world_f_core_PSI();
// transform to world space
m_world_f_core->inline_vmult3( &ivpSpeed, &ivpSpeed );
pCore->center_push_core_multiple_ws( &ivpSpeed, event->delta_time );
pCore->rot_push_core_multiple_cs( &ivpRot, event->delta_time );
}
break;
case IMotionEvent::SIM_GLOBAL_ACCELERATION:
{
ConvertAngularImpulseToIVP( rot, ivpRot );
ConvertForceImpulseToIVP( speed, ivpSpeed );
pCore->speed.add_multiple( &ivpSpeed, event->delta_time );
pCore->rot_speed.add_multiple( &ivpRot, event->delta_time );
}
break;
case IMotionEvent::SIM_GLOBAL_FORCE:
{
ConvertAngularImpulseToIVP( rot, ivpRot );
ConvertForceImpulseToIVP( speed, ivpSpeed );
pCore->center_push_core_multiple_ws( &ivpSpeed, event->delta_time );
pCore->rot_push_core_multiple_cs( &ivpRot, event->delta_time );
}
break;
}
}
}
}
