const Matrix &
Truss2::getDamp(void)
{
if (L == 0.0) { // - problem in setDomain() no further warnings
theMatrix->Zero();
return *theMatrix;
}
theMatrix->Zero();
if (doRayleighDamping == 1)
*theMatrix = this->Element::getDamp();
double eta = theMaterial->getDampTangent();
// come back later and redo this if too slow
Matrix &damp = *theMatrix;
int numDOF2 = numDOF/2;
double temp;
double etaAoverL = eta*A/L;
for (int i = 0; i < dimension; i++) {
for (int j = 0; j < dimension; j++) {
temp = cosX[i]*cosX[j]*etaAoverL;
damp(i,j) += temp;
damp(i+numDOF2,j) += -temp;
damp(i,j+numDOF2) += -temp;
damp(i+numDOF2,j+numDOF2) += temp;
}
}
return damp;
}
const Matrix &
CoupledZeroLength::getDamp(void)
{
Matrix& damp = *theMatrix;
damp.Zero();
if (useRayleighDamping == 1)
damp = this->Element::getDamp();
double eta;
eta = theMaterial->getDampTangent();
int numNodeDof = numDOF/2;
int dirn1b = dirn1+numNodeDof;
int dirn2b = dirn2+numNodeDof;
damp(dirn1,dirn1) += eta;
damp(dirn1b,dirn1b) += eta;
damp(dirn1,dirn1b) -= eta;
damp(dirn1b,dirn1) -= eta;
damp(dirn2,dirn2) += eta;
damp(dirn2b,dirn2b) += eta;
damp(dirn2,dirn2b) -= eta;
damp(dirn2b,dirn2) -= eta;
return damp;
}
void XC::Twenty_Node_Brick::formDampingTerms( int tangFlag ) const
{
//static double xsj ; // determinant jacaobian matrix
int i, j;
//double volume = 0.;
//zero damp
damp.Zero( ) ;
if(rayFactors.getBetaK() != 0.0)
damp.addMatrix(1.0, this->getTangentStiff(), rayFactors.getBetaK());
if(rayFactors.getBetaK0() != 0.0)
damp.addMatrix(1.0, this->getInitialStiff(), rayFactors.getBetaK0());
if(rayFactors.getBetaKc() != 0.0)
damp.addMatrix(1.0, Kc, rayFactors.getBetaKc());
if(rayFactors.getAlphaM() != 0.0)
{
this->getMass();
for( i = 0; i < 60; i++)
for( j = 0; j < 60; j++)
damp(i,j) += mass(i,j) * rayFactors.getAlphaM();
}
return; /////////
}
void dgDynamicBody::AddDampingAcceleration(dgFloat32 timestep)
{
dgVector damp (GetDampCoeffcient (timestep));
dgVector omegaDamp(damp & dgVector::m_triplexMask);
dgVector omega(m_matrix.UnrotateVector(m_omega) * omegaDamp);
m_veloc = m_veloc.Scale(damp.m_w);
m_omega = m_matrix.RotateVector(omega);
}
void resources::particle::integrate(const float dt) {
vel += acc * dt;
pos += vel * dt;
rotation += rotation_speed * dt;
vel.damp(linear_damping * dt);
damp(rotation_speed, angular_damping * dt);
lifetime_ms += dt * 1000.f;
}
void Twenty_Node_Brick::formDampingTerms( int tangFlag )
{
static double xsj ; // determinant jacaobian matrix
int i, j, k, m, ik, jk;
double volume = 0.;
//zero damp
damp.Zero( ) ;
if (betaK != 0.0)
damp.addMatrix(1.0, this->getTangentStiff(), betaK);
if (betaK0 != 0.0)
damp.addMatrix(1.0, this->getInitialStiff(), betaK0);
if (betaKc != 0.0)
damp.addMatrix(1.0, *Kc, betaKc);
if (alphaM != 0.0) {
this->getMass();
for( i = 0; i < 60; i++)
for( j = 0; j < 60; j++)
damp(i,j) += mass(i,j) * alphaM;
}
return; /////////
}
SICALLBACK aaOcean_BeginEvaluate( ICENodeContext& in_ctxt )
{
// get ocean pointer from user-data
aaOcean *pOcean = (aaOcean *)(CValue::siPtrType)in_ctxt.GetUserData();
// get ICE node input port arrays
CDataArrayLong PointID( in_ctxt, ID_IN_PointID);
CDataArrayLong resolution( in_ctxt, ID_IN_RESOLUTION);
CDataArrayLong seed( in_ctxt, ID_IN_SEED);
CDataArrayFloat waveHeight( in_ctxt, ID_IN_WAVE_HEIGHT);
CDataArrayFloat waveSpeed( in_ctxt, ID_IN_WAVESPEED);
CDataArrayFloat chop( in_ctxt, ID_IN_CHOP);
CDataArrayFloat oceanScale( in_ctxt, ID_IN_OCEAN_SCALE );
CDataArrayFloat oceanDepth( in_ctxt, ID_IN_OCEAN_DEPTH );
CDataArrayFloat windDir( in_ctxt, ID_IN_WINDDIR );
CDataArrayFloat cutoff( in_ctxt, ID_IN_CUTOFF);
CDataArrayFloat velocity( in_ctxt, ID_IN_WINDVELOCITY);
CDataArrayLong windAlign( in_ctxt, ID_IN_WINDALIGN );
CDataArrayFloat damp( in_ctxt, ID_IN_DAMP);
CDataArrayBool enableFoam( in_ctxt, ID_IN_ENABLEFOAM);
CDataArrayFloat time( in_ctxt, ID_IN_TIME);
CDataArrayFloat loopTime( in_ctxt, ID_IN_REPEAT_TIME);
CDataArrayFloat surfaceTension( in_ctxt, ID_IN_SURFACE_TENSION);
CDataArrayFloat randWeight( in_ctxt, ID_IN_RAND_WEIGHT);
pOcean->input(resolution[0],
seed[0],
oceanScale[0],
oceanDepth[0],
surfaceTension[0],
velocity[0],
cutoff[0],
windDir[0],
windAlign[0],
damp[0],
waveSpeed[0],
waveHeight[0],
chop[0],
time[0],
loopTime[0],
enableFoam[0],
randWeight[0]);
return CStatus::OK;
}
//get residual with inertia terms
const XC::Vector &XC::Twenty_Node_Brick::getResistingForceIncInertia(void) const
{
static XC::Vector res(60);
// printf("getResistingForceIncInertia()\n");
int i, j;
static double a[60];
for(i=0; i<nenu; i++) {
const XC::Vector &accel = theNodes[i]->getTrialAccel();
if( 3 != accel.Size() ) {
std::cerr << "XC::Twenty_Node_Brick::getResistingForceIncInertia matrix and vector sizes are incompatable\n";
exit(-1);
}
a[i*3] = accel(0);
a[i*3+1] = accel(1);
a[i*3+2] = accel(2);
}
// Compute the current resisting force
this->getResistingForce();
// std::cerr<<"K "<<resid<<std::endl;
// Compute the mass matrix
this->getMass();
for(i = 0; i < 60; i++) {
for(j = 0; j < 60; j++){
resid(i) += mass(i,j)*a[j];
}
}
// printf("\n");
//std::cerr<<"K+M "<<P<<std::endl;
for(i=0; i<nenu; i++) {
const XC::Vector &vel = theNodes[i]->getTrialVel();
if( 3!= vel.Size() ) {
std::cerr << "XC::Twenty_Node_Brick::getResistingForceIncInertia matrix and vector sizes are incompatable\n";
exit(-1);
}
a[i*3] = vel(0);
a[i*3+1] = vel(1);
a[i*3+2] = vel(2);
}
this->getDamp();
for(i = 0; i < 60; i++) {
for(j = 0; j < 60; j++) {
resid(i) += damp(i,j)*a[j];
}
}
// std::cerr<<"Pd"<<Pd<<std::endl;
res = resid;
// std::cerr<<"res "<<res<<std::endl;
// exit(-1);
if(isDead())
res*=dead_srf;
return res;
}
void dNewtonBody::SetAngularDamping(dFloat x, dFloat y, dFloat z)
{
dVector damp(x, y, z);
NewtonBodySetAngularDamping(m_body, &damp.m_x);
}
const Vector& TwentyEightNodeBrickUP::getResistingForceIncInertia( )
{
static Vector res(68);
int i, j, ik;
static double a[68];
for (i=0; i<nenu; i++) {
const Vector &accel = nodePointers[i]->getTrialAccel();
if ((i<nenp && 4 != accel.Size()) || (i>=nenp && 3 != accel.Size())) {
opserr << "TwentyEightNodeBrickUP::getResistingForceIncInertia matrix and vector sizes are incompatable\n";
exit(-1);
}
if (i<nenp)
ik = i*4;
else
ik = nenp*4 + (i-nenp)*3;
a[ik] = accel(0);
a[ik+1] = accel(1);
a[ik+2] = accel(2);
if (i<nenp) a[ik+3] = accel(3);
}
// Compute the current resisting force
this->getResistingForce();
// opserr<<"K "<<resid<<endln;
// Compute the mass matrix
this->getMass();
for (i = 0; i < 68; i++) {
for (j = 0; j < 68; j++){
resid(i) += mass(i,j)*a[j];
}
}
// printf("\n");
//opserr<<"K+M "<<P<<endln;
for (i=0; i<nenu; i++) {
const Vector &vel = nodePointers[i]->getTrialVel();
if ((i<nenp && 4 != vel.Size()) || (i>=nenp && 3 != vel.Size())) {
opserr << "TwentyEightNodeBrickUP::getResistingForceIncInertia matrix and vector sizes are incompatable\n";
exit(-1);
}
if (i<nenp)
ik = i*4;
else
ik = nenp*4 + (i-nenp)*3;
a[ik] = vel(0);
a[ik+1] = vel(1);
a[ik+2] = vel(2);
if (i<nenp) a[ik+3] = vel(3);
}
this->getDamp();
for (i = 0; i < 68; i++) {
for (j = 0; j < 68; j++) {
resid(i) += damp(i,j)*a[j];
}
}
res = resid;
// opserr<<"res "<<res<<endln;
return res;
}
void TwentyEightNodeBrickUP::formDampingTerms( int tangFlag )
{
static double xsj ; // determinant jacaobian matrix
int i, j, k, m, ik, jk;
double volume = 0.;
//zero damp
damp.Zero( ) ;
if (betaK != 0.0)
damp.addMatrix(1.0, this->getTangentStiff(), betaK);
if (betaK0 != 0.0)
damp.addMatrix(1.0, this->getInitialStiff(), betaK0);
if (betaKc != 0.0)
damp.addMatrix(1.0, *Kc, betaKc);
if (alphaM != 0.0) {
this->getMass();
for( i = 0; i < nenu; i++ ) {
if( i < nenp)
ik = i*4;
else
ik = nenp*4 + (i - nenp) * 3;
for( j = 0; j < nenu; j++) {
if( j < nenp)
jk = j * 4;
else
jk = nenp * 4 + (j-nenp) * 3;
for( k = 0; k < 3; k++)
damp(ik + k, jk + k) += mass(ik + k, jk + k) * alphaM;
}
}
}
//compute basis vectors and local nodal coordinates
computeBasis( ) ;
//gauss loop to compute and save shape functions
for( i = 0; i < nintp; i++ ) {
// compute Jacobian and global shape functions
Jacobian3d(i, xsj, 1);
//volume element to also be saved
dvolp[i] = wp[i] * xsj ;
volume += dvolp[i];
} // end for i
//printf("volume = %f\n", volume);
volume = 0.;
for( i = 0; i < nintp; i++ ) {
// compute Jacobian and global shape functions
Jacobian3d(i, xsj, 2);
//volume element to also be saved
dvolq[i] = wp[i] * xsj ;
volume += dvolq[i];
} // end for i
//printf("volume = %f\n", volume);
// Compute coupling matrix
for( i = 0; i < nenu; i++ ) {
if( i < nenp)
ik = i * 4;
else
ik = nenp * 4 + (i-nenp)*3;
for( j = 0; j < nenp; j++) {
jk = j * 4 + 3;
for( m = 0; m < nintp; m++) {
for( k = 0; k < 3; k++) {
damp(ik+k,jk) += -dvolq[m]*shgq[k][i][m]*shgp[3][j][m];
}
}
for( k = 0; k < 3; k++ ) {
damp(jk, ik+k) = damp(ik+k, jk);
}
}
}
// Compute permeability matrix
for( i = 0; i < nenp; i++ ) {
ik = i*4 + 3;
for( j = 0; j < nenp; j++ ) {
jk = j * 4 + 3;
for( m = 0; m < nintp; m++ ) {
damp(ik,jk) += - dvolp[m]*(perm[0]*shgp[0][i][m]*shgp[0][j][m] +
perm[1]*shgp[1][i][m]*shgp[1][j][m]+
perm[2]*shgp[2][i][m]*shgp[2][j][m]);
}
}
}
}
const Vector& Twenty_Node_Brick::getResistingForceIncInertia( )
{
static Vector res(60);
// printf("getResistingForceIncInertia()\n");
int i, j, ik;
static double a[60];
for (i=0; i<nenu; i++) {
const Vector &accel = nodePointers[i]->getTrialAccel();
if ( 3 != accel.Size() ) {
opserr << "Twenty_Node_Brick::getResistingForceIncInertia matrix and vector sizes are incompatable\n";
exit(-1);
}
a[i*3] = accel(0);
a[i*3+1] = accel(1);
a[i*3+2] = accel(2);
}
// Compute the current resisting force
this->getResistingForce();
// opserr<<"K "<<resid<<endln;
// Compute the mass matrix
this->getMass();
for (i = 0; i < 60; i++) {
for (j = 0; j < 60; j++){
resid(i) += mass(i,j)*a[j];
}
}
// printf("\n");
//opserr<<"K+M "<<P<<endln;
for (i=0; i<nenu; i++) {
const Vector &vel = nodePointers[i]->getTrialVel();
if ( 3!= vel.Size() ) {
opserr << "Twenty_Node_Brick::getResistingForceIncInertia matrix and vector sizes are incompatable\n";
exit(-1);
}
a[i*3] = vel(0);
a[i*3+1] = vel(1);
a[i*3+2] = vel(2);
}
this->getDamp();
for (i = 0; i < 60; i++) {
for (j = 0; j < 60; j++) {
resid(i) += damp(i,j)*a[j];
}
}
// opserr<<"Pd"<<Pd<<endln;
res = resid;
// opserr<<"res "<<res<<endln;
// exit(-1);
return res;
}
void HCOD::activeSearch( VectorXd & u )
{
// if( isDebugOnce ) { sotDebugTrace::openFile(); isDebugOnce = false; }
// else { if(sotDEBUGFLOW.outputbuffer.good()) sotDebugTrace::closeFile(); }
//if(sotDEBUGFLOW.outputbuffer.good()) { sotDebugTrace::closeFile();sotDebugTrace::openFile(); }
sotDEBUGIN(15);
/*
* foreach stage: stage.initCOD(Ir_init)
* u = 0
* u0 = solve
* do
* tau,cst_ref = max( violation(stages) )
* u += (1-tau)u0 + tau*u1
* if( tau<1 )
* update(cst_ref); break;
*
* lambda,w = computeLambda
* cst_ref,lmin = min( lambda,w )
* if lmin<0
* downdate( cst_ref )
*
*/
assert(VectorXi::LinSpaced(3,0,2)[0] == 0
&& VectorXi::LinSpaced(3,0,2)[1] == 1
&& VectorXi::LinSpaced(3,0,2)[2] == 2
&& "new version of Eigen might have change the "
"order of arguments in LinSpaced, please correct");
/*struct timeval t0,t1,t2;double time1,time2;
gettimeofday(&t0,NULL);*/
initialize();
sotDEBUG(5) << "Y= " << (MATLAB)Y.matrixExplicit<< std::endl;
Y.computeExplicitly(); // TODO: this should be done automatically on Y size.
sotDEBUG(5) << "Y= " << (MATLAB)Y.matrixExplicit<< std::endl;
/*gettimeofday(&t1,NULL);
time1 = ((t1.tv_sec-t0.tv_sec)+(t1.tv_usec-t0.tv_usec)/1.0e6);*/
int iter = 0;
startTime=getCPUtime();
Index stageMinimal = 0;
do
{
iter ++; sotDEBUG(5) << " --- *** \t" << iter << "\t***.---" << std::endl;
//if( iter>1 ) { break; }
if( sotDEBUG_ENABLE(15) ) show( sotDEBUGFLOW );
assert( testRecomposition(&std::cerr) );
damp();
computeSolution();
assert( testSolution(&std::cerr) );
double tau = computeStepAndUpdate();
if( tau<1 )
{
sotDEBUG(5) << "Update done, make step <1." << std::endl;
makeStep(tau);
}
else
{
sotDEBUG(5) << "No update, make step ==1." << std::endl;
makeStep();
for( ;stageMinimal<=(Index)stages.size();++stageMinimal )
{
sotDEBUG(5) << "--- Started to examinate stage " << stageMinimal << std::endl;
computeLagrangeMultipliers(stageMinimal);
if( sotDEBUG_ENABLE(15) ) show( sotDEBUGFLOW );
//assert( testLagrangeMultipliers(stageMinimal,std::cerr) );
if( searchAndDowndate(stageMinimal) )
{
sotDEBUG(5) << "Lagrange<0, downdate done." << std::endl;
break;
}
for( Index i=0;i<stageMinimal;++i )
stages[i]->freezeSlacks(false);
if( stageMinimal<nbStages() )
stages[stageMinimal]->freezeSlacks(true);
}
}
lastTime=getCPUtime()-startTime;
lastNumberIterations=iter;
if( lastTime>maxTime ) throw 667;
if( iter>maxNumberIterations ) throw 666;
} while(stageMinimal<=nbStages());
sotDEBUG(5) << "Lagrange>=0, no downdate, active search completed." << std::endl;
/*gettimeofday(&t2,NULL);
time2 = ((t2.tv_sec-t1.tv_sec)+(t2.tv_usec-t1.tv_usec)/1.0e6);
std::ofstream fup("/tmp/haset.dat",std::ios::app);
fup << time1<<"\t"<<time2<<"\t"<<iter<<"\t";*/
u=solution;
sotDEBUG(5) << "uf =" << (MATLAB)u << std::endl;
sotDEBUGOUT(15);
}
void dgCollisionDeformableSolidMesh::ApplyExternalForces (dgFloat32 timestep)
{
dgAssert (m_myBody);
dgBody* const body = GetBody();
dgAssert (body->GetMass().m_w > dgFloat32 (0.0f));
dgAssert (body->GetMass().m_w < dgFloat32 (1.0e5f));
const dgMatrix& matrix = body->GetCollision()->GetGlobalMatrix();
dgFloat32 invMass = body->GetInvMass().m_w;
dgVector velocyStep (body->GetForce().Scale4(invMass * timestep));
//velocyStep = dgVector(0.0f);
dgVector* const veloc = m_particles.m_veloc;
dgFloat32* const unitMass = m_particles.m_unitMass;
/*
invMass = 0;
dgVector w (0.0f, 0.0f, 1.0f, 0.0f);
for (dgInt32 i = 0; i < m_particles.m_count; i ++) {
unitMass[i] = 1.0f;
veloc[i] = w * m_posit[i];
invMass += unitMass[i];
}
invMass = 1.0f / invMass;
*/
dgVector com (dgFloat32 (0.0f));
dgVector comVeloc (dgFloat32 (0.0f));
for (dgInt32 i = 0; i < m_particles.m_count; i ++) {
dgVector mass (unitMass[i]);
const dgVector& p = m_posit[i];
veloc[i] += velocyStep;
com += p.CompProduct4(mass);
comVeloc += veloc[i].CompProduct4(mass);
}
com = com.Scale4(invMass);
comVeloc = comVeloc.Scale4(invMass);
const dgMatrix& indentity = dgGetIdentityMatrix();
dgMatrix inertiaMatrix (dgGetZeroMatrix());
inertiaMatrix.m_posit = dgVector::m_wOne;
dgVector comAngularMomentum (dgFloat32 (0.0f));
for (dgInt32 i = 0; i < m_particles.m_count; i ++) {
dgVector mass (unitMass[i]);
dgVector r (m_posit[i] - com);
dgVector mr (r.CompProduct4(mass));
dgVector relVeloc (veloc[i] - comVeloc);
comAngularMomentum += mr * relVeloc;
dgMatrix inertia (mr, r);
dgVector diagInertia (mr.DotProduct4(r));
inertiaMatrix.m_front += (indentity.m_front.CompProduct4(diagInertia) - inertia.m_front);
inertiaMatrix.m_up += (indentity.m_up.CompProduct4(diagInertia) - inertia.m_up);
inertiaMatrix.m_right += (indentity.m_right.CompProduct4(diagInertia) - inertia.m_right);
dgAssert (inertiaMatrix.TestSymetric3x3());
}
dgVector damp (0.3f);
dgMatrix invInertia (inertiaMatrix.Symetric3by3Inverse());
dgVector omega (invInertia.RotateVector(comAngularMomentum));
for (dgInt32 i = 0; i < m_particles.m_count; i ++) {
dgVector r (m_posit[i] - com);
dgVector deltaVeloc (comVeloc + omega * r - veloc[i]);
veloc[i] += deltaVeloc.CompProduct4(damp);
}
//Sleep (50);
dgMatrix tmp;
dgMatrix transform;
dgVector scale;
inertiaMatrix.PolarDecomposition (transform, scale, tmp, matrix);
body->GetCollision()->SetGlobalMatrix(transform);
body->SetMatrixOriginAndRotation(body->GetCollision()->GetLocalMatrix().Inverse() * transform);
body->SetVelocity(comVeloc);
body->SetOmega(omega);
}
char* Player::update(char *input)
{
//p is a char pointer to a mSendBuff which is the data that will be sent to the other players, it maps the Input data
char *p=mSendBuff;
//Get Xinput State
XInputGetState(0,&gXInputState);
if(eUserType==USERTYPE_SERVER)
{
if(eCarState==CARSTATE_COLLIDED)
{
if(eCollisionType==COLLISIONTYPE_MUD)
{
decAcceleration(0.2);
p[4]='1';
eCarState=CARSTATE_MOVINGREVERSE;
}
if(eCollisionType==COLLISIONTYPE_OIL)
{
decAcceleration(0.2);
if(mPosition.x<(SCREEN_WIDTH/2))
yaw(-5,-0.3);
else
yaw(5,0.3);
eCarState=CARSTATE_MOVINGREVERSE;
p[4]='2';
}
if(eCollisionType==COLLISIONTYPE_PEDESTRIAN)
{
deaccelerate(15);
eCarState=CARSTATE_MOVINGREVERSE;
p[4]='3';
}
if(eCollisionType==COLLISIONTYPE_ROADBLOCK || eCollisionType==COLLISIONTYPE_NPCCAR)
{
mCurrentVelocity=0;
mCurrentBVelocity=0;
deaccelerate(50);
eCarState=CARSTATE_STOP;
p[4]='4';
}
eCollisionType=COLLISIONTYPE_NONE;
}
else
{
p[4]='0';
}
if(hge->Input_GetKeyState(HGEK_UP) || gXInputState.Gamepad.wButtons&XINPUT_GAMEPAD_A && eCollisionType!=COLLISIONTYPE_ROADBLOCK)
{
accelerate(1);
eCarState=CARSTATE_MOVING;
p[0]='6';
}
else
{
p[0]='0';
damp(1);
}
if(hge->Input_GetKeyState(HGEK_DOWN)||gXInputState.Gamepad.wButtons&XINPUT_GAMEPAD_X)
{
deaccelerate(1);
eCarState=CARSTATE_MOVING;
p[1]='7';
}
else
{
p[1]='0';
Bdamp(1);
}
if(hge->Input_GetKeyState(HGEK_LEFT) ||gXInputState.Gamepad.wButtons&XINPUT_GAMEPAD_DPAD_LEFT&& (eCarState==CARSTATE_MOVING || eCarState==CARSTATE_DAMPING))
{
yaw(-5,-0.3);
p[2]='8';
}
else
{
p[2]='0';
RLdamp();
}
if(hge->Input_GetKeyState(HGEK_RIGHT)||gXInputState.Gamepad.wButtons&XINPUT_GAMEPAD_DPAD_RIGHT && (eCarState==CARSTATE_MOVING || eCarState==CARSTATE_DAMPING))
{
yaw(5,0.3);
p[3]='9';
}
else
{
RRdamp();
p[3]='0';
}
/*char c[256];
sprintf(c,"send : %s \n",mSendBuff);
OutputDebugString(c);*/
sprintf(SpeedString,"Speed : %d ",(int)mCurrentVelocity);
//OutputDebugString(SpeedString);
Integrator();
p[5]='\0';
}
else if(eUserType==USERTYPE_CLIENT)
{
if(input[4]=='1')
{
//MUD COLLISION
decAcceleration(0.2);
eCarState=CARSTATE_MOVINGREVERSE;
}
if(input[4]=='2')
{
//OIL COLLISION
decAcceleration(0.2);
if(mPosition.x<(SCREEN_WIDTH/2))
yaw(-5,-0.3);
else
yaw(5,0.3);
eCarState=CARSTATE_MOVINGREVERSE;
}
if(input[4]=='3')
{
//PEDESTRIAN COLLISION
deaccelerate(15);
eCarState=CARSTATE_MOVINGREVERSE;
}
if(input[4]=='4')
{
//NPC CAR COLLISION
mCurrentVelocity=0;
mCurrentBVelocity=0;
deaccelerate(50);
eCarState=CARSTATE_STOP;
}
if(input[0]=='6' || hge->Input_GetKeyState(HGEK_W))
{
accelerate(1);
eCarState=CARSTATE_MOVING;
}
else
{
damp(1);
}
if(input[1]=='7')
{
deaccelerate(1);
eCarState=CARSTATE_MOVING;
}
else
{
Bdamp(1);
}
if(input[2]=='8'&&(eCarState==CARSTATE_MOVING || eCarState==CARSTATE_DAMPING))
{
yaw(-5,-0.3);
}
else
{
RLdamp();
}
if(input[3]=='9'&& (eCarState==CARSTATE_MOVING|| eCarState==CARSTATE_DAMPING))
{
yaw(5,0.3);
}
else
{
RRdamp();
}
Integrator();
}
return p;
}
void PrecessingTops (DemoEntityManager* const scene)
{
scene->CreateSkyBox();
// customize the scene after loading
// set a user friction variable in the body for variable friction demos
// later this will be done using LUA script
dMatrix offsetMatrix (dGetIdentityMatrix());
CreateLevelMesh (scene, "flatPlane.ngd", 1);
dVector location (0.0f, 0.0f, 0.0f, 0.0f);
dVector size (3.0f, 2.0f, 0.0f, 0.0f);
// create an array of cones
const int count = 10;
// all shapes use the x axis as the axis of symmetry, to make an upright cone we apply a 90 degree rotation local matrix
dMatrix shapeOffsetMatrix (dRollMatrix(-3.141592f/2.0f));
AddPrimitiveArray(scene, 50.0f, location, size, count, count, 5.0f, _CONE_PRIMITIVE, 0, shapeOffsetMatrix);
// till the cont 30 degrees, and apply a local high angular velocity
dMatrix matrix (dRollMatrix (-25.0f * 3.141592f / 180.0f));
dVector omega (0.0f, 50.0f, 0.0f);
omega = matrix.RotateVector (omega);
dVector damp (0.0f, 0.0f, 0.0f, 0.0f);
int topscount = 0;
NewtonBody* array[count * count];
NewtonWorld* const world = scene->GetNewton();
for (NewtonBody* body = NewtonWorldGetFirstBody(world); body; body = NewtonWorldGetNextBody(world, body)) {
NewtonCollision* const collision = NewtonBodyGetCollision(body);
dVector com;
if (NewtonCollisionGetType (collision) == SERIALIZE_ID_CONE) {
array[topscount] = body;
topscount ++;
}
}
for (int i = 0; i < topscount ; i ++) {
dMatrix bodyMatrix;
NewtonBody* const body = array[i];
NewtonBodyGetMatrix(body, &bodyMatrix[0][0]);
matrix.m_posit = bodyMatrix.m_posit;
matrix.m_posit.m_y += 1.0f;
NewtonBodySetMatrix(body, &matrix[0][0]);
dFloat Ixx;
dFloat Iyy;
dFloat Izz;
dFloat mass;
NewtonBodyGetMassMatrix(body, &mass, &Ixx, &Iyy, &Izz);
NewtonBodySetMassMatrix(body, mass, Ixx, Iyy * 8.0f, Izz);
NewtonBodySetOmega (body, &omega[0]);
NewtonBodySetAutoSleep (body, 0);
NewtonBodySetLinearDamping(body, 0.0f);
NewtonBodySetAngularDamping (body, &damp[0]);
}
// place camera into position
dMatrix camMatrix (dGetIdentityMatrix());
dQuaternion rot (camMatrix);
dVector origin (-40.0f, 5.0f, 0.0f, 0.0f);
scene->SetCameraMatrix(rot, origin);
}