void NAMESPACE::Trackball::updateRotation(uint x, uint y)
{
float speed = 3.0f; // *m_Elapsed;
float dx = (float(x) - float(m_PrevX)) * speed / float(m_Width);
float dy = (float(y) - float(m_PrevY)) * speed / float(m_Height);
m_PrevX = x;
m_PrevY = y;
quatf dr = deltaRotation(dx, dy);
m_Rotation = dr * m_Rotation;
if (m_Walkthrough) {
// remove 'roll'
int d = -1;
if (m_Up == X_neg || m_Up == Y_neg || m_Up == Z_neg) {
d = 1;
}
quatf rinv = m_Rotation.inverse();
v3f up = dir2v3f(m_Up);
v3f realleft = rinv * V3F(1, 0, 0);
v3f frwd = rinv * V3F(0, 0, 1);
v3f flat = normalize_safe(realleft - dot(realleft, up)*up);
float cs = dot(realleft, flat);
if (cs > -1.0f && cs < 1.0f) {
float sign = dot(up, realleft) > 0 ? -1.0f : 1.0f;
float target_agl = sign * acos(cs);
float agl = target_agl;
m_Rotation = quatf(V3F(0, 0, 1), agl) * m_Rotation;
}
}
}
dgJacobian dgDynamicBody::IntegrateForceAndToque(const dgVector& force, const dgVector& torque, const dgVector& timestep)
{
dgJacobian velocStep;
if (m_gyroTorqueOn) {
dgVector dtHalf(timestep * dgVector::m_half);
dgMatrix matrix(m_gyroRotation, dgVector::m_wOne);
dgVector localOmega(matrix.UnrotateVector(m_omega));
dgVector localTorque(matrix.UnrotateVector(torque - m_gyroTorque));
// derivative at half time step. (similar to midpoint Euler so that it does not loses too much energy)
dgVector dw(localOmega * dtHalf);
dgMatrix jacobianMatrix(
dgVector(m_mass[0], (m_mass[2] - m_mass[1]) * dw[2], (m_mass[2] - m_mass[1]) * dw[1], dgFloat32(0.0f)),
dgVector((m_mass[0] - m_mass[2]) * dw[2], m_mass[1], (m_mass[0] - m_mass[2]) * dw[0], dgFloat32(1.0f)),
dgVector((m_mass[1] - m_mass[0]) * dw[1], (m_mass[1] - m_mass[0]) * dw[0], m_mass[2], dgFloat32(1.0f)),
dgVector::m_wOne);
// and solving for alpha we get the angular acceleration at t + dt
// calculate gradient at a full time step
//dgVector gradientStep(localTorque * timestep);
dgVector gradientStep(jacobianMatrix.SolveByGaussianElimination(localTorque * timestep));
dgVector omega(matrix.RotateVector(localOmega + gradientStep));
dgAssert(omega.m_w == dgFloat32(0.0f));
// integrate rotation here
dgFloat32 omegaMag2 = omega.DotProduct(omega).GetScalar() + dgFloat32(1.0e-12f);
dgFloat32 invOmegaMag = dgRsqrt(omegaMag2);
dgVector omegaAxis(omega.Scale(invOmegaMag));
dgFloat32 omegaAngle = invOmegaMag * omegaMag2 * timestep.GetScalar();
dgQuaternion deltaRotation(omegaAxis, omegaAngle);
m_gyroRotation = m_gyroRotation * deltaRotation;
dgAssert((m_gyroRotation.DotProduct(m_gyroRotation) - dgFloat32(1.0f)) < dgFloat32(1.0e-5f));
matrix = dgMatrix(m_gyroRotation, dgVector::m_wOne);
localOmega = matrix.UnrotateVector(omega);
//dgVector angularMomentum(inertia * localOmega);
//body->m_gyroTorque = matrix.RotateVector(localOmega.CrossProduct(angularMomentum));
//body->m_gyroAlpha = body->m_invWorldInertiaMatrix.RotateVector(body->m_gyroTorque);
dgVector localGyroTorque(localOmega.CrossProduct(m_mass * localOmega));
m_gyroTorque = matrix.RotateVector(localGyroTorque);
m_gyroAlpha = matrix.RotateVector(localGyroTorque * m_invMass);
velocStep.m_angular = matrix.RotateVector(gradientStep);
} else {
velocStep.m_angular = m_invWorldInertiaMatrix.RotateVector(torque) * timestep;
//velocStep.m_angular = velocStep.m_angular * dgVector::m_half;
}
velocStep.m_linear = force.Scale(m_invMass.m_w) * timestep;
return velocStep;
}
dQuaternion dQuaternion::IntegrateOmega (const dVector& omega, dFloat timestep) const
{
// this is correct
dQuaternion rotation (*this);
dFloat omegaMag2 = omega % omega;
const dFloat errAngle = 0.0125f * 3.141592f / 180.0f;
const dFloat errAngle2 = errAngle * errAngle;
if (omegaMag2 > errAngle2) {
dFloat invOmegaMag = 1.0f / dSqrt (omegaMag2);
dVector omegaAxis (omega.Scale (invOmegaMag));
dFloat omegaAngle = invOmegaMag * omegaMag2 * timestep;
dQuaternion deltaRotation (omegaAxis, omegaAngle);
rotation = rotation * deltaRotation;
rotation.Scale(1.0f / dSqrt (rotation.DotProduct (rotation)));
}
return rotation;
}
void CPlayerInput::PreUpdate()
{
CMovementRequest request;
// get rotation into a manageable form
float mouseSensitivity;
if (m_pPlayer->InZeroG())
mouseSensitivity = 0.00333f*MAX(0.01f, g_pGameCVars->cl_sensitivityZeroG);
else
mouseSensitivity = 0.00333f*MAX(0.01f, g_pGameCVars->cl_sensitivity);
mouseSensitivity *= gf_PI / 180.0f;//doesnt make much sense, but after all helps to keep reasonable values for the sensitivity cvars
//these 2 could be moved to CPlayerRotation
mouseSensitivity *= m_pPlayer->m_params.viewSensitivity;
mouseSensitivity *= m_pPlayer->GetMassFactor();
COffHand * pOffHand=static_cast<COffHand*>(m_pPlayer->GetWeaponByClass(CItem::sOffHandClass));
if(pOffHand && (pOffHand->GetOffHandState()&eOHS_HOLDING_NPC))
mouseSensitivity *= pOffHand->GetObjectMassScale();
// When carrying object/enemy, adapt mouse sensitiviy to feel the weight
// Designers requested we ignore single-handed objects (1 == m_iCarryingObject)
if(2 == m_iCarryingObject)
{
mouseSensitivity /= 2.0f;
}
if(m_fCrouchPressedTime>0.0f)
{
float fNow = gEnv->pTimer->GetAsyncTime().GetMilliSeconds();
if((fNow - m_fCrouchPressedTime) > 300.0f)
{
if(m_actions & ACTION_CROUCH)
{
m_actions &= ~ACTION_CROUCH;
m_actions |= ACTION_PRONE;
}
m_fCrouchPressedTime = -1.0f;
}
}
Ang3 deltaRotation(m_deltaRotation * mouseSensitivity);
if (m_pStats->isFrozen.Value() && m_pPlayer->IsPlayer() && m_pPlayer->GetHealth()>0)
{
float sMin = g_pGameCVars->cl_frozenSensMin;
float sMax = g_pGameCVars->cl_frozenSensMax;
float mult = sMin + (sMax-sMin)*(1.f-m_pPlayer->GetFrozenAmount(true));
deltaRotation *= mult;
m_pPlayer->UpdateUnfreezeInput(m_deltaRotation, m_deltaMovement-m_deltaMovementPrev, mult);
}
// apply rotation from xinput controller
if(!m_bDisabledXIRot)
{
// Controller framerate compensation needs frame time!
// The constant is to counter for small frame time values.
// adjust some too small values, should be handled differently later on
Ang3 xiDeltaRot=m_xi_deltaRotation*gEnv->pTimer->GetFrameTime() * mouseSensitivity * 50.0f;
SmoothControllerInput(xiDeltaRot);
ControlCameraMode();
// Applying aspect modifiers
if (g_pGameCVars->ctrl_aspectCorrection > 0)
{
int vx, vy, vw, vh;
gEnv->pRenderer->GetViewport(&vx, &vy, &vw, &vh);
float med=((float)vw+vh)/2.0f;
float crW=((float)vw)/med;
float crH=((float)vh)/med;
xiDeltaRot.x*=g_pGameCVars->ctrl_aspectCorrection == 2 ? crW : crH;
xiDeltaRot.z*=g_pGameCVars->ctrl_aspectCorrection == 2 ? crH : crW;
}
if(g_pGameCVars->cl_invertController)
xiDeltaRot.x*=-1;
deltaRotation+=xiDeltaRot;
IVehicle *pVehicle = m_pPlayer->GetLinkedVehicle();
if (pVehicle)
{
if (m_pPlayer->m_pVehicleClient)
{
m_pPlayer->m_pVehicleClient->PreUpdate(pVehicle, m_pPlayer->GetEntityId(), gEnv->pTimer->GetFrameTime());
}
//FIXME:not really good
m_actions = 0;
m_deltaMovement.Set(0,0,0);
m_deltaRotation.Set(0,0,0);
}
}
if(m_bUseXIInput)
{
m_deltaMovement.x = m_xi_deltaMovement.x;
m_deltaMovement.y = m_xi_deltaMovement.y;
m_deltaMovement.z = 0;
if (m_xi_deltaMovement.len2()>0.0f)
m_actions |= ACTION_MOVE;
else
m_actions &= ~ACTION_MOVE;
}
bool animControlled(m_pPlayer->m_stats.animationControlled);
// If there was a recent serialization, ignore the delta rotation, since it's accumulated over several frames.
if ((m_lastSerializeFrameID + 2) > gEnv->pRenderer->GetFrameID())
deltaRotation.Set(0,0,0);
//if(m_pPlayer->m_stats.isOnLadder)
//deltaRotation.z = 0.0f;
const SCVars* pGameCVars = g_pGameCVars;
if(pGameCVars->cl_cam_orbit != 0 && m_pPlayer->IsClient() && m_pPlayer->IsThirdPerson())
{
static bool IsInit = false;
if (!IsInit)
{
m_pPlayer->m_camViewMtxFinal = Matrix33(gEnv->pRenderer->GetCamera().GetViewMatrix());
IsInit = true;
}
float frameTime=gEnv->pTimer->GetFrameTime();
float frameTimeNormalised=(frameTime>1 ? 1 : frameTime<0.0001f ? 0.0001f : frameTime)*30; // 1/30th => 1 1/60th =>0.5 etc
float frameTimeClamped=(frameTime>1 ? 1 : frameTime<0.0001f ? 0.0001f : frameTime);
m_pCameraInputHelper->UpdateCameraInput(deltaRotation, frameTimeClamped,frameTimeNormalised); // also modifies deltaRotation.
}
if (!animControlled)
request.AddDeltaRotation( deltaRotation );
// add some movement...
if (!m_pStats->isFrozen.Value() && !animControlled)
request.AddDeltaMovement( FilterMovement(m_deltaMovement) );
m_deltaMovementPrev = m_deltaMovement;
// handle actions
if (m_actions & ACTION_JUMP)
{
if (m_pPlayer->GetStance() != STANCE_PRONE)
request.SetJump();
else
m_actions &= ~ACTION_JUMP;
//m_actions &= ~ACTION_PRONE;
/*if (m_pPlayer->GetStance() != STANCE_PRONE)
{
if(m_pPlayer->GetStance() == STANCE_STAND || m_pPlayer->TrySetStance(STANCE_STAND))
request.SetJump();
}
else if(!m_pPlayer->TrySetStance(STANCE_STAND))
m_actions &= ~ACTION_JUMP;
else
m_actions &= ~ACTION_PRONE;*/
}
if (m_pPlayer->m_stats.isOnLadder)
{
m_actions &= ~ACTION_PRONE;
m_actions &= ~ACTION_CROUCH;
}
request.SetStance(FigureOutStance());
float pseudoSpeed = 0.0f;
if (m_deltaMovement.len2() > 0.0f)
{
pseudoSpeed = m_pPlayer->CalculatePseudoSpeed(m_pPlayer->m_stats.bSprinting);
}
/* design changed: sprinting with controller is removed from full stick up to Left Bumper
if(m_bUseXIInput && m_xi_deltaMovement.len2() > 0.999f)
{
m_actions |= ACTION_SPRINT;
}
else if(m_bUseXIInput)
{
m_actions &= ~ACTION_SPRINT;
}*/
request.SetPseudoSpeed(pseudoSpeed);
if (m_deltaMovement.GetLength() > 0.1f)
{
float moveAngle = (float)RAD2DEG(fabs_tpl(cry_atan2f(-m_deltaMovement.x, fabsf(m_deltaMovement.y)<0.01f?0.01f:m_deltaMovement.y)));
request.SetAllowStrafing(moveAngle > 20.0f);
}
else
{
request.SetAllowStrafing(true);
}
// send the movement request to the appropriate spot!
m_pPlayer->m_pMovementController->RequestMovement( request );
m_pPlayer->m_actions = m_actions;
// reset things for next frame that need to be
m_lastMouseRawInput = m_deltaRotation;
m_deltaRotation = Ang3(0,0,0);
//static float color[] = {1,1,1,1};
//gEnv->pRenderer->Draw2dLabel(100,50,1.5,color,false,"deltaMovement:%f,%f", m_deltaMovement.x,m_deltaMovement.y);
// PLAYERPREDICTION
m_pPlayer->GetGameObject()->ChangedNetworkState(INPUT_ASPECT);
// ~PLAYERPREDICTION
}
Quaternion3f InterpMotion::absoluteRotation(FCL_REAL dt) const
{
Quaternion3f delta_t = deltaRotation(dt);
return delta_t * tf1.getQuatRotation();
}
