void Rotation::setValue(const Vector3d & rotateFrom, const Vector3d & rotateTo)
{
Vector3d u(rotateFrom); u.Normalize();
Vector3d v(rotateTo); v.Normalize();
// The vector from x to is the roatation axis because it's the normal of the plane defined by (0,u,v)
const double dot = u * v;
Vector3d w = u % v;
const double wlen = w.Length();
if (wlen == 0.0) { // Parallel vectors
// Check if they are pointing in the same direction.
if (dot > 0.0) {
this->setValue(0.0, 0.0, 0.0, 1.0);
}
else {
// We can use any axis perpendicular to u (and v)
Vector3d t = u % Vector3d(1.0, 0.0, 0.0);
if(t.Length() < FLT_EPSILON)
t = u % Vector3d(0.0, 1.0, 0.0);
this->setValue(t.x, t.y, t.z, 0.0);
}
}
else { // Vectors are not parallel
// Note: A quaternion is not well-defined by specifying a point and its transformed point.
// Every quaternion with a rotation axis having the same angle to the vectors of both points is okay.
double angle = (double)acos(dot);
this->setValue(w, angle);
}
}
// This is a "perceptually tuned predictive filter", which means that it is optimized
// for improvements in the VR experience, rather than pure error. In particular,
// jitter is more perceptible at lower speeds whereas latency is more perceptible
// after a high-speed motion. Therefore, the prediction interval is dynamically
// adjusted based on speed. Significant more research is needed to further improve
// this family of filters.
static Pose<double> calcPredictedPose(const PoseState<double>& poseState, double predictionDt)
{
Pose<double> pose = poseState.ThePose;
const double linearCoef = 1.0;
Vector3d angularVelocity = poseState.AngularVelocity;
double angularSpeed = angularVelocity.Length();
// This could be tuned so that linear and angular are combined with different coefficients
double speed = angularSpeed + linearCoef * poseState.LinearVelocity.Length();
const double slope = 0.2; // The rate at which the dynamic prediction interval varies
double candidateDt = slope * speed; // TODO: Replace with smoothstep function
double dynamicDt = predictionDt;
// Choose the candidate if it is shorter, to improve stability
if (candidateDt < predictionDt)
{
dynamicDt = candidateDt;
}
if (angularSpeed > 0.001)
{
pose.Rotation = pose.Rotation * Quatd(angularVelocity, angularSpeed * dynamicDt);
}
pose.Translation += poseState.LinearVelocity * dynamicDt;
return pose;
}
void cEntity::Tick(float a_Dt, cChunk & a_Chunk)
{
if (m_InvulnerableTicks > 0)
{
m_InvulnerableTicks--;
}
if (m_AttachedTo != NULL)
{
Vector3d DeltaPos = m_Pos - m_AttachedTo->GetPosition();
if (DeltaPos.Length() > 0.5)
{
SetPosition(m_AttachedTo->GetPosition());
if (IsPlayer())
{
cPlayer * Player = (cPlayer *)this;
Player->UpdateMovementStats(DeltaPos);
}
}
}
else
{
if (!a_Chunk.IsValid())
{
return;
}
// Position changed -> super::Tick() called
GET_AND_VERIFY_CURRENT_CHUNK(NextChunk, POSX_TOINT, POSZ_TOINT)
TickBurning(*NextChunk);
if (GetPosY() < VOID_BOUNDARY)
{
TickInVoid(*NextChunk);
}
else
{
m_TicksSinceLastVoidDamage = 0;
}
if (IsMob() || IsPlayer() || IsPickup() || IsExpOrb())
{
DetectCacti();
}
if (IsMob() || IsPlayer())
{
// Set swimming state
SetSwimState(*NextChunk);
// Handle drowning
HandleAir();
}
// None of the above functions change position, we remain in the chunk of NextChunk
HandlePhysics(a_Dt, *NextChunk);
}
}
void cPlayer::UpdateMovementStats(const Vector3d & a_DeltaPos)
{
StatValue Value = (StatValue)floor(a_DeltaPos.Length() * 100 + 0.5);
if (m_AttachedTo == NULL)
{
if (IsClimbing())
{
if (a_DeltaPos.y > 0.0) // Going up
{
m_Stats.AddValue(statDistClimbed, (StatValue)floor(a_DeltaPos.y * 100 + 0.5));
}
}
else if (IsSubmerged())
{
m_Stats.AddValue(statDistDove, Value);
AddFoodExhaustion(0.00015 * (double)Value);
}
else if (IsSwimming())
{
m_Stats.AddValue(statDistSwum, Value);
AddFoodExhaustion(0.00015 * (double)Value);
}
else if (IsOnGround())
{
m_Stats.AddValue(statDistWalked, Value);
AddFoodExhaustion((m_IsSprinting ? 0.001 : 0.0001) * (double)Value);
}
else
{
if (Value >= 25) // Ignore small/slow movement
{
m_Stats.AddValue(statDistFlown, Value);
}
}
}
else
{
switch (m_AttachedTo->GetEntityType())
{
case cEntity::etMinecart: m_Stats.AddValue(statDistMinecart, Value); break;
case cEntity::etBoat: m_Stats.AddValue(statDistBoat, Value); break;
case cEntity::etMonster:
{
cMonster * Monster = (cMonster *)m_AttachedTo;
switch (Monster->GetMobType())
{
case cMonster::mtPig: m_Stats.AddValue(statDistPig, Value); break;
case cMonster::mtHorse: m_Stats.AddValue(statDistHorse, Value); break;
default: break;
}
break;
}
default: break;
}
}
}
virtual bool Item(cPlayer * a_Player) override
{
// Don't check players who are in creative gamemode
if (a_Player->IsGameModeCreative())
{
return false;
}
Vector3d Direction = m_EndermanPos - a_Player->GetPosition();
// Don't check players who are more then SightDistance (64) blocks away
if (Direction.Length() > m_SightDistance)
{
return false;
}
// Don't check if the player has a pumpkin on his head
if (a_Player->GetEquippedHelmet().m_ItemType == E_BLOCK_PUMPKIN)
{
return false;
}
Vector3d LookVector = a_Player->GetLookVector();
double dot = Direction.Dot(LookVector);
// 0.09 rad ~ 5 degrees
// If the player's crosshair is within 5 degrees of the enderman, it counts as looking
if (dot <= cos(0.09))
{
return false;
}
cTracer LineOfSight(a_Player->GetWorld());
if (LineOfSight.Trace(m_EndermanPos, Direction, static_cast<int>(Direction.Length())))
{
// No direct line of sight
return false;
}
m_Player = a_Player;
return true;
}
void ModelRenderInstance::SetPosition(const Vector3d& vPos, double dViewAngle)
{
m_dViewAngle = dViewAngle;
if (m_bFirstPosInit)
{
m_vPos = vPos;
m_bFirstPosInit = false;
return;
}
// check if new distance is "too far" and we should "correct"
if (!m_bCorrectPos)
{
Vector3d vDist = m_vPos - vPos;
double dMaxDist = 0.5;
bool bTooFar = vDist.Length() > dMaxDist;
if (bTooFar)
{
// start correcting
m_bCorrectPos = true;
m_timerCorrect.Start();
m_vCorrectStart = m_vPos;
m_vCorrectTarget = vPos;
}
else
{
// just use new coordinate
m_vPos = vPos;
}
}
else
{
// already correcting
bool bFinishedCorrecting = m_timerCorrect.Elapsed() >= c_dCorrectTimeRange;
if (!bFinishedCorrecting)
{
// still too far
m_vCorrectTarget = vPos;
}
else
{
// correction finished
m_timerCorrect.Stop();
m_bCorrectPos = false;
m_vPos = vPos;
}
}
}
void PerspectiveCamera::SetPositionLookAt(const Vector3d& vPos, const Vector3d& vLookAt)
{
Vector3d vDir = vLookAt - vPos;
if (vDir.Length() < 1e-6)
vDir = Vector3d(0.0, 0.0, 1.0);
double dAngleDirection = Rad2Deg(atan2(vDir.X(), -vDir.Z()));
double dAngleUp = Rad2Deg(atan2(vDir.Y(), -vDir.Z()));
SetPosition(vPos, dAngleDirection, dAngleUp);
}
// This is a simple predictive filter based only on extrapolating the smoothed, current angular velocity.
// Note that both QP (the predicted future orientation) and Q (the current orientation) are both maintained.
Quatf SensorFusion::GetPredictedOrientation()
{
Lock::Locker lockScope(Handler.GetHandlerLock());
Quatf qP = QUncorrected;
if (EnablePrediction) {
#if 1
Vector3f angVelF = FAngV.SavitzkyGolaySmooth8();
float angVelFL = angVelF.Length();
if (angVelFL > 0.001f)
{
Vector3f rotAxisP = angVelF / angVelFL;
float halfRotAngleP = angVelFL * PredictionDT * 0.5f;
float sinaHRAP = sin(halfRotAngleP);
Quatf deltaQP(rotAxisP.x*sinaHRAP, rotAxisP.y*sinaHRAP,
rotAxisP.z*sinaHRAP, cos(halfRotAngleP));
qP = QUncorrected * deltaQP;
}
#else
Quatd qpd = Quatd(Q.x,Q.y,Q.z,Q.w);
int predictionStages = (int)(PredictionDT / DeltaT);
Vector3f aa = FAngV.SavitzkyGolayDerivative12();
Vector3d aad = Vector3d(aa.x,aa.y,aa.z);
Vector3f angVelF = FAngV.SavitzkyGolaySmooth8();
Vector3d avkd = Vector3d(angVelF.x,angVelF.y,angVelF.z);
for (int i = 0; i < predictionStages; i++)
{
double angVelLengthd = avkd.Length();
Vector3d rotAxisd = avkd / angVelLengthd;
double halfRotAngled = angVelLengthd * DeltaT * 0.5;
double sinHRAd = sin(halfRotAngled);
Quatd deltaQd = Quatd(rotAxisd.x*sinHRAd, rotAxisd.y*sinHRAd, rotAxisd.z*sinHRAd,
cos(halfRotAngled));
qpd = qpd * deltaQd;
// Update vel
avkd += aad;
}
qP = Quatf((float)qpd.x,(float)qpd.y,(float)qpd.z,(float)qpd.w);
#endif
}
return qP;
}
void SelectionList::Update(const Vector3d& vCurrentPos, const ObjectMap& objMap, double dMaxDistance)
{
// remember last selected id
ObjectId lastObjId = GetSelected();
m_vecSelectionList.clear();
ObjectMap::const_iterator iter = objMap.GetMap().begin();
ObjectMap::const_iterator stop = objMap.GetMap().end();
size_t uiLastSelected = std::numeric_limits<size_t>::max();
for (; iter != stop; --iter)
{
ATLASSERT(iter->second != NULL);
const ObjectId& objId = iter->first;
// check length
const Object& obj = *(iter->second);
Vector3d vDist = vCurrentPos - obj.Pos();
if (vDist.Length() > dMaxDistance)
continue;
if (objId == lastObjId)
uiLastSelected = m_vecSelectionList.size();
m_vecSelectionList.push_back(objId);
}
// TODO sort by nearest
if (uiLastSelected < m_vecSelectionList.size())
m_iterSelected = m_vecSelectionList.begin() + uiLastSelected;
else
m_iterSelected = m_vecSelectionList.end();
}
