IC void q_scalem(Fmatrix &m, float v)
{
Fquaternion q;
q.set(m);
q_scale(q,v);
m.rotation(q);
}
IC void q_scale(Fquaternion &q, float v)
{
float angl;Fvector ax;
q.get_axis_angle(ax,angl);
q.rotation(ax,angl*v);
q.normalize();
}
void CCameraFirstEye::Update(Fvector& point, Fvector& noise_dangle)
{
Fmatrix mR, R;
Fmatrix rX, rY, rZ;
rX.rotateX (noise_dangle.x);
rY.rotateY (-noise_dangle.y);
rZ.rotateZ (noise_dangle.z);
R.mul_43 (rY,rX);
R.mulB_43 (rZ);
mR.identity ();
Fquaternion Q;
Q.rotationYawPitchRoll(roll,yaw,pitch);
mR.rotation (Q);
mR.transpose ();
mR.mulB_43 (R);
vDirection.set (mR.k);
vNormal.set (mR.j);
if (m_Flags.is(flRelativeLink)) {
parent->XFORM().transform_dir (vDirection);
parent->XFORM().transform_dir (vNormal);
}
vPosition.set (point);
}
IC void key_sub(CKey &rk, const CKey &k0, const CKey& k1)//sub right
{
Fquaternion q;
q.inverse(k1.Q);
rk.Q.mul(k0.Q,q);
//rk.Q.normalize();//?
rk.T.sub(k0.T,k1.T);
}
IC void q_add_scaled_basem( Fquaternion &q, const Fquaternion &base, const Fquaternion &q0, const Fquaternion &q1, float v1 )
{
//VERIFY(0.f =< v && 1.f >= v );
Fmatrix m0;m0.rotation(q0);
Fmatrix m,ml1;
q_scale_vs_basem( ml1, q1, base, v1 );
m.mul(m0,ml1);
q.set(m);
q.normalize();
}
bool is_similar( const Fmatrix &m0, const Fmatrix &m1, float param )
{
Fmatrix tmp1; tmp1.invert( m0 );
Fmatrix tmp2; tmp2.mul( tmp1, m1 );
Fvector ax;float ang;
Fquaternion q;
q.set( tmp2 );
q.get_axis_angle( ax, ang );
return _abs( ang )<M_PI/2.f;
}
IC bool cmp( const Fmatrix &f0, const Fmatrix &f1 )
{
Fmatrix if0; if0.invert( f0 );
Fmatrix cm;cm.mul_43( if0, f1 );
Fvector ax; float ang;
Fquaternion q;
q.set( cm );
q.get_axis_angle( ax, ang );
return ang < cmp_angle && cm.c.square_magnitude() < cmp_ldisp* cmp_ldisp ;
}
void CInventoryItem::make_Interpolation ()
{
net_updateData* p = NetSync();
p->m_dwILastUpdateTime = Level().timeServer();
if(!object().H_Parent() && object().getVisible() && object().m_pPhysicsShell && m_flags.test(FInInterpolation) )
{
u32 CurTime = Level().timeServer();
if (CurTime >= p->m_dwIEndTime)
{
m_flags.set(FInInterpolation, FALSE);
object().m_pPhysicsShell->NetInterpolationModeOFF();
CPHSynchronize* pSyncObj = NULL;
pSyncObj = object().PHGetSyncItem(0);
pSyncObj->set_State (p->PredictedState);
Fmatrix xformI;
pSyncObj->cv2obj_Xfrom (p->PredictedState.quaternion, p->PredictedState.position, xformI);
VERIFY2 (_valid(object().renderable.xform),*object().cName());
object().XFORM().set (xformI);
VERIFY2 (_valid(object().renderable.xform),*object().cName());
}
else
{
VERIFY (CurTime <= p->m_dwIEndTime);
float factor = float(CurTime - p->m_dwIStartTime)/(p->m_dwIEndTime - p->m_dwIStartTime);
if (factor > 1) factor = 1.0f;
else if (factor < 0) factor = 0;
Fvector IPos;
Fquaternion IRot;
float c = factor;
for (u32 k=0; k<3; k++)
{
IPos[k] = c*(c*(c*p->SCoeff[k][0]+p->SCoeff[k][1])+p->SCoeff[k][2])+p->SCoeff[k][3];
};
VERIFY2 (_valid(IPos),*object().cName());
VERIFY (factor>=0.f && factor<=1.f);
IRot.slerp(p->IStartRot, p->IEndRot, factor);
VERIFY2 (_valid(IRot),*object().cName());
object().XFORM().rotation(IRot);
VERIFY2 (_valid(object().renderable.xform),*object().cName());
object().Position().set(IPos);
VERIFY2 (_valid(object().renderable.xform),*object().cName());
};
}
else
{
m_flags.set(FInInterpolation,FALSE);
};
#ifdef DEBUG
Fvector iPos = object().Position();
if (!object().H_Parent() && object().getVisible())
{
if(m_net_updateData)
m_net_updateData->LastVisPos.push_back(iPos);
};
#endif
}