Matrix4 RPhysics::GetOrientTransform(Vector4 dirStart, Vector4 upStart1, Vector4 dirEnd, Vector4 upEnd1) {
Vector4 temp = CrossProduct(dirStart,upStart1);
Vector4 upStart = CrossProduct(temp,dirStart);
temp = CrossProduct(dirEnd,upEnd1);
Vector4 upEnd = CrossProduct(temp,dirEnd);
dirStart = Normalize3(dirStart);
dirEnd = Normalize3(dirEnd);
upStart = Normalize3(upStart);
upEnd = Normalize3(upEnd);
Matrix4 transform;
transform.Identity();
Vector4 axis1 = CrossProduct(dirStart,dirEnd);
if(Length3(axis1)==0) axis1 = upEnd;
float angle1 = Angle3(dirStart,dirEnd);
Matrix4 rotMat;
rotMat.Rotation(angle1,axis1);
Vector4 newUp = rotMat*upStart;
Vector4 axis2 = CrossProduct(newUp,upEnd);
if(Length3(axis2)==0) axis2 = dirEnd;
float angle2 = Angle3(upEnd,newUp);
if(angle1*angle2*0!=0) return transform;
Matrix4 toRot;
toRot.Rotation(angle2,axis2);
transform= transform*toRot;
toRot.Rotation(angle1,axis1);
transform = transform*toRot;
if(!(transform[0][0]<=3||transform[0][0]>=3)) {
cerr<<endl;
cerr<<angle1<<endl;
cerr<<angle2<<endl;
PrintVector(dirStart);
PrintVector(upStart);
PrintVector(dirEnd);
PrintVector(upEnd);
cout<<flush;
exit(1);
}
return transform;
}
Vector4 RPhysics::CalculateInputForce() {
const float accel = 1.25*GetAccelConst();
Vector4 ret(0,0,0);
if(m_pedalState==PRESSED||m_boostLeft>0) {
Vector4 inputDir = GetTurnDir();
ret = inputDir*accel;
float angle = fabs(Angle3(m_player->GetAhead(),m_velocity));
if(angle*0==0)
ret*=(1+angle);
//ret*=2;
//cerr<<"A:"<<angle<<endl;
} else if(m_brakeState==PRESSED) {
Vector4 ahead = m_player->GetAhead();
ret = -ahead*BRAKE_CONSTANT;
}
return ret;
}
Vector4 RPhysics::CalculateResistanceForce() {
Vector4 drag = -DRAG_CONSTANT*m_velocity;
Vector4 ahead = m_player->GetAhead();
float currSpeed = Length3(m_velocity);
float frictionPercent=0;
if(currSpeed>0) {
frictionPercent = Angle3(m_velocity,ahead);
if(frictionPercent>PI/2) frictionPercent = PI-frictionPercent;
frictionPercent/=(PI/2);
//frictionPercent = frictionPercent*frictionPercent;
//cerr<<"f:"<<frictionPercent<<endl;
}
Vector4 friction = -FRICTION_CONST*frictionPercent*m_velocity/PI;
Vector4 ret = friction+drag;
if(ret[0]*0!=0) return Vector4(0,0,0);
return ret;
}
Angle3 Angle3::operator * (const float & ratio) const
{
return Angle3(x * ratio, y * ratio, z * ratio);
}
Angle3 Angle3::operator + (const Angle3 & other) const
{
return Angle3(x + other.x, y + other.y, z + other.z);
}
Angle3 Angle3::operator - (const Angle3 & other) const
{
return Angle3(x - other.x, y - other.y, z - other.z);
}
