Vector4 P_ProjectVector(Vector4 orig, Vector4 onto) {
float orig_length = Length3(orig);
if(orig_length==0) return orig;
float onto_length = Length3(onto);
float cos_theta = DotProduct3(orig,onto)/(orig_length*onto_length);
return cos_theta*onto_length*onto;
}
void RPhysics::ProcessCollision(float distLeft, Vector4 collPlane,Vector4 currDir) {
currDir = Normalize3(currDir);
collPlane = Normalize3(collPlane);
Vector4 newDir = currDir-2*collPlane*(DotProduct3(collPlane,currDir))*Length3(currDir);
Vector4 retDir;
m_player->Move(distLeft,newDir,retDir);
m_velocity = .3*Length3(m_velocity)*Normalize3(retDir);
//m_velocity = 1.1*Length3(m_velocity)*Normalize3(retDir); //for bumper-car style racing
collision = true;
m_boostLeft = 0;
}
float RPhysics::GetAccelConst() {
int speedIndex = (int)(Length3(m_velocity)/5);
//cerr<<"speed index:"<<speedIndex<<endl;
//cerr<<"speed:"<<Length3(m_velocity)<<endl;
switch(speedIndex) {
case 0:
return 9;
case 1:
return 12;
case 2:
return 16;
case 3:
return 22;
case 4:
return 25;
case 5:
return 28;
case 6:
return 30;
case 7:
return 33;
default:
return 35;
}
}
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;
}
void TTraction::ResistanceCalc(int d, double r, TTractionPowerSource *ps)
{ //(this) jest przês³em zasilanym, o rezystancji (r), policzyæ rezystancjê zastêpcz¹ s¹siednich
if (d >= 0)
{ // pod¹¿anie we wskazanym kierunku
TTraction *t = hvNext[d], *p;
if (ps)
psPower[d ^ 1] = ps; // pod³¹czenie podanego
else
ps = psPower[d ^ 1]; // zasilacz od przeciwnej strony ni¿ idzie analiza
d = iNext[d]; // kierunek
// double r; //sumaryczna rezystancja
if (DebugModeFlag) // tylko podczas testów
Material = 4; // pokazanie, ¿e to przês³o ma pod³¹czone zasilanie
while (t ? !t->psPower[d] : false) // jeœli jest jakiœ kolejny i nie ma ustalonego zasilacza
{ // ustawienie zasilacza i policzenie rezystancji zastêpczej
if (DebugModeFlag) // tylko podczas testów
if (t->Material != 4) // przês³a zasilaj¹cego nie modyfikowaæ
{
if (t->Material < 4)
t->Material = 4; // tymczasowo, aby zmieni³a kolor
t->Material |= d ? 2 : 1; // kolor zale¿ny od strony, z której jest zasilanie
}
t->psPower[d] = ps; // skopiowanie wskaŸnika zasilacza od danej strony
t->fResistance[d] = r; // wpisanie rezystancji w kierunku tego zasilacza
r += t->fResistivity * Length3(t->vParametric); // doliczenie oporu kolejnego odcinka
p = t; // zapamiêtanie dotychczasowego
t = p->hvNext[d ^ 1]; // pod¹¿anie w tê sam¹ stronê
d = p->iNext[d ^ 1];
// w przypadku zapêtlenia sieci mo¿e siê zawiesiæ?
}
}
else
{ // pod¹¿anie w obu kierunkach, mo¿na by rekurencj¹, ale szkoda zasobów
r = 0.5 * fResistivity *
Length3(vParametric); // powiedzmy, ¿e w zasilanym przêœle jest po³owa
if (fResistance[0] == 0.0)
ResistanceCalc(0, r); // do ty³u (w stronê Point1)
if (fResistance[1] == 0.0)
ResistanceCalc(1, r); // do przodu (w stronê Point2)
}
};
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;
}
void RPhysics::DoPhysics(int deltaTime) {
Vector4 ahead = m_player->GetAhead();
if(Length3(m_velocity)==0) {
m_velocity = MIN_VEL*ahead;
}
Vector4 inputForce = CalculateInputForce();
Vector4 resistance = CalculateResistanceForce();
//cerr<<"input force";
//PrintVector(inputForce);
//cerr<<"resistance force:";
//PrintVector(resistance);
Vector4 turnDir = GetTurnDir();
if(m_boostLeft>0) {
inputForce+=BOOST_AMT*Normalize3(turnDir);
m_boostLeft-=deltaTime;
}
Vector4 totalForce = inputForce+resistance;
for(int i=0;i<m_impulseForces.size();i++) {
totalForce+=m_impulseForces[i];
}
m_impulseForces.clear();
float currSpeed = Length3(m_velocity);
Vector4 newVelocity = m_velocity+deltaTime/1000.0*totalForce;
//cerr<<"new velocity:";
//PrintVector(newVelocity);
float newSpeed = Length3(newVelocity);
Vector4 deltaPos = newVelocity*deltaTime/1000.0;
float distance = Length3(deltaPos);
if(distance>0) {
Vector4 oldUp = m_player->GetUp();
Vector4 oldAhead = m_player->GetAhead();
Vector4 oldDir = newVelocity;
if(currSpeed==0) oldDir = oldAhead;
collision = false;
Vector4 newDir;
if(newSpeed>MIN_VEL)
m_player->Move(distance,deltaPos,newDir);
else
newDir = deltaPos;
if(!collision) {
m_velocity = Normalize3(newDir)*newSpeed;
}
Matrix4 transform = GetOrientTransform(oldDir,oldUp,newDir,m_player->GetUp());
oldAhead(3) = 1;
//cerr<<"orient transform:";
//PrintMatrix(transform);
Vector4 newAhead = transform*oldAhead;
Vector4 desiredAhead = transform*turnDir;
//cerr<<"new ahead:";
//PrintVector(newAhead);
//cerr<<deltaTime<<endl;
Vector4 toSetAhead = Normalize3(8*33.3/deltaTime*newAhead+desiredAhead);
// if(currSpeed==0&&(m_brakeState==PRESSED||m_pedalState==RELEASED)) toSetAhead*=-1;
m_player->SetAhead(toSetAhead);
//}
}
}
bool RPhysics::VelIsPositive() {
if(Length3(m_velocity)>MIN_VEL) return true;
else return false;
}
void RCamera::Animate(int deltaTime) {
const float UP_AMT = 1;
const float BACK_AMT = 2.2;
const float LOOK_AHEAD = 6;
glLoadIdentity();
RPlayer *player = m_engine->GetPlayer(m_playerId);
Vector4 desiredUp = Normalize3(player->GetUp());
Vector4 playAhead = player->GetAhead();
Vector4 dirNormal = CrossProduct(playAhead,desiredUp);
playAhead = Normalize3(CrossProduct(desiredUp,dirNormal));
Vector4 desiredPos = player->GetPosition()+desiredUp*UP_AMT-playAhead*BACK_AMT;
Vector4 desiredAhead = Normalize3(player->GetPosition()+playAhead*LOOK_AHEAD-desiredPos);
float distance = Length3(desiredPos - m_pos);
m_pos = desiredPos;
m_ahead = desiredAhead;
m_up = CrossProduct(CrossProduct(m_ahead,desiredUp),m_ahead);
Vector4 aa = m_pos+m_ahead;
if(!freeCameraEnabled)
gluLookAt(m_pos[0],m_pos[1],m_pos[2],aa[0],aa[1],aa[2],m_up[0],m_up[1],m_up[2]);
//m_ahead = playAhead;
// NOTE:
// m_ahead and m_up must be normal to each other
// AND correspond exactly to depth and height.
// View Frustum culling finds the the vertices
// of the view frustum by moving in these directions,
// so if they are not exactly straight ahead and straight up
// there will be a problem.
//m_ahead = Normalize3(player->GetAhead());
//m_ahead = Normalize3(desiredAhead);
//m_up = desiredUp;
//m_up = Normalize3(player->GetUp());
//Vector4 pointAhead = desiredPos+desiredAhead;
//Vector4 pointAhead = m_pos+m_ahead;
//gluLookAt(m_pos[0],m_pos[1],m_pos[2],pointAhead[0],pointAhead[1],pointAhead[2],m_up[0],m_up[1],m_up[2]);
//glLoadIdentity();
timeSinceLastMove +=deltaTime;
//if(timeSinceLastMove>100) {
timeSinceLastMove = 0;
viewPos[0]+=-speed*sin(viewAngle*PI/180);;
viewPos[2]+=speed*cos(viewAngle*PI/180);
if (freeCameraEnabled) {
if(onTrack) m_engine->GetWorld()->getTrack()->Viewpoint(trackDistance,12);
else {
glRotatef(viewAngle,0,1,0);
glTranslatef(viewPos[0],viewPos[1],viewPos[2]);
}
}
/*
Vector4 pos = player->GetPosition()+.2*desiredUp;
Vector4 a = pos+playAhead;
m_ahead = Normalize3(playAhead);
Vector4 upNormalizer = CrossProduct(m_ahead,desiredUp);
m_up = Normalize3(CrossProduct(upNormalizer,m_ahead));
if (!freeCameraEnabled)
gluLookAt(pos[0],pos[1],pos[2],a[0],a[1],a[2],m_up[0],m_up[1],m_up[2]);
m_pos = pos;
*/
// cout << "Position: "; PrintVector(m_pos); cout << endl;
// cout << "Ahead: "; PrintVector(m_ahead); cout << endl;
// cout << "Up: "; PrintVector(m_up); cout << endl;
}