void SimAeroDamage(tCar *car, sgVec3 poc, tdble F)
{
tAero* aero = &car->aero;
tdble dmg = F*0.0001;
aero->rot_front[0] += dmg*(urandom()-.5);
aero->rot_front[1] += dmg*(urandom()-.5);
aero->rot_front[2] += dmg*(urandom()-.5);
if (sgLengthVec3(car->aero.rot_front) > 1.0) {
sgNormaliseVec3 (car->aero.rot_front);
}
aero->rot_lateral[0] += dmg*(urandom()-.5);
aero->rot_lateral[1] += dmg*(urandom()-.5);
aero->rot_lateral[2] += dmg*(urandom()-.5);
if (sgLengthVec3(car->aero.rot_lateral) > 1.0) {
sgNormaliseVec3 (car->aero.rot_lateral);
}
aero->rot_vertical[0] += dmg*(urandom()-.5);
aero->rot_vertical[1] += dmg*(urandom()-.5);
aero->rot_vertical[2] += dmg*(urandom()-.5);
if (sgLengthVec3(car->aero.rot_vertical) > 1.0) {
sgNormaliseVec3 (car->aero.rot_vertical);
}
//printf ("aero damage:%f (->%f %f %f)\n", dmg, sgLengthVec3(car->aero.rot_front),
// sgLengthVec3(car->aero.rot_lateral), sgLengthVec3(car->aero.rot_vertical));
}
void SimCarCollideAddDeformation(tCar* car, sgVec3 pos, sgVec3 force)
{
// just compute values, gr does deformation later.
// must average position and add up force.
tCollisionState* collision_state = &car->carElt->priv.collision_state;
collision_state->collision_count++;
//tdble k = (tdble) cnt;
if (sgLengthVec3(collision_state->force) < sgLengthVec3(force)) {
for (int i=0; i<3; i++) {
collision_state->pos[i] = pos[i];// + k*collision_state->pos[i])/(k+1.0);
collision_state->force[i] = (float)(0.0001*force[i]);// + k*collision_state->force[i])/(k+1.0);
//printf ("F:%f\n", collision_state->force[i]);
}
}
}
void grPropagateDamage (ssgEntity* l, sgVec3 poc, sgVec3 force, int cnt)
{
//showEntityType (l);
if (l->isAKindOf (ssgTypeBranch())) {
ssgBranch* br = (ssgBranch*) l;
for (int i = 0 ; i < br -> getNumKids () ; i++ ) {
ssgEntity* ln = br->getKid (i);
grPropagateDamage(ln, poc, force, cnt+1);
}
}
if (l->isAKindOf (ssgTypeVtxTable())) {
sgVec3* v;
int Nv;
ssgVtxTable* vt = (ssgVtxTable*) l;
Nv = vt->getNumVertices();
vt->getVertexList ((void**) &v);
tdble sigma = sgLengthVec3 (force);
tdble invSigma = 5.0;
for (int i=0; i<Nv; i++) {
tdble r = sgDistanceSquaredVec3 (poc, v[i]);
tdble f = exp(-r*invSigma)*5.0;
v[i][0] += force[0]*f;
v[i][1] += force[1]*f;
// use sigma as a random number generator (!)
v[i][2] += (force[2]+0.02*sin(2.0*r + 10.0*sigma))*f;
//printf ("(%f %f %f)\n", v[i][0], v[i][1], v[i][2]);
}
}
}
/**
* Create a CRRCControlSurfaceAnimation object
*
* Initialize the animation from an
* <animation type="ControlSurface"> tag
*/
CRRCControlSurfaceAnimation::CRRCControlSurfaceAnimation(SimpleXMLTransfer *xml)
: CRRCAnimation(new ssgTransform()), fallback_data(0.0f),
eventAdapter(this, &CRRCControlSurfaceAnimation::axisValueCallback, Event::Input),
aileron(0.0f), elevator(0.0f), rudder(0.0f), throttle(0.0f),
spoiler(0.0f), flap(0.0f), retract(0.0f), pitch(0.0f)
{
bool failed = false;
// evaluate <object> tag
SimpleXMLTransfer *map = xml->getChild("object", true);
symbolic_name = map->getString("name", "no_name_set");
max_angle = (float)(map->getDouble("max_angle", 0.0) * SG_RADIANS_TO_DEGREES);
abs_max_angle = (float)fabs((double)max_angle);
// find hinges and evaluate all <control> tags
int num_controls = 0;
int num_hinges = 0;
for (int i = 0; i < xml->getChildCount(); i++)
{
SimpleXMLTransfer *child = xml->getChildAt(i);
if (child->getName() == "hinge")
{
// found a <hinge> child
sgVec3 pos;
pos[SG_X] = (float)(-1 * child->getDouble("y", 0.0));
pos[SG_Y] = (float)(-1 * child->getDouble("x", 0.0));
pos[SG_Z] = (float)(-1 * child->getDouble("z", 0.0));
if (num_hinges == 0)
{
sgCopyVec3(hinge_1, pos);
}
else if (num_hinges == 1)
{
sgCopyVec3(hinge_2, pos);
}
num_hinges++;
}
else if (child->getName() == "control")
{
// found a <control> child
// The "*2" factor for each gain value scales the control input
// values from -0.5...+0.5 to -1.0...+1.0. This saves one
// float multiplication per mapping in the runtime update() routine.
std::string mapping = child->getString("mapping", "NOTHING");
float gain = (float)child->getDouble("gain", 1.0);
if (mapping == "ELEVATOR")
{
datasource.push_back(&elevator);
source_gain.push_back(gain * 2);
num_controls++;
}
else if (mapping == "AILERON")
{
datasource.push_back(&aileron);
source_gain.push_back(gain * 2);
num_controls++;
}
else if (mapping == "THROTTLE")
{
datasource.push_back(&throttle);
source_gain.push_back(gain * 2);
num_controls++;
}
else if (mapping == "RUDDER")
{
datasource.push_back(&rudder);
source_gain.push_back(gain * 2);
num_controls++;
}
else if (mapping == "FLAP")
{
datasource.push_back(&flap);
source_gain.push_back(gain * 2);
num_controls++;
}
else if (mapping == "SPOILER")
{
datasource.push_back(&spoiler);
source_gain.push_back(gain * 2);
num_controls++;
}
else if (mapping == "RETRACT")
{
datasource.push_back(&retract);
source_gain.push_back(gain * 2);
num_controls++;
}
else if (mapping == "PITCH")
{
datasource.push_back(&pitch);
source_gain.push_back(gain * 2);
num_controls++;
}
else
{
std::cerr << "ControlSurfaceAnimation: ignoring <control> tag without mapping." << std::endl;
}
}
}
if (num_controls < 1)
{
std::cerr << "ControlSurfaceAnimation: found animation without proper <control> tag. Animation disabled." << std::endl;
failed = true;
}
if (num_hinges < 2)
{
std::cerr << "ControlSurfaceAnimation: Must specify exactly two hinges!" << std::endl;
failed = true;
}
else
{
if (num_hinges > 2)
{
std::cerr << "ControlSurfaceAnimation: Must specify exactly two hinges!" << std::endl;
std::cerr << "ControlSurfaceAnimation: Ignoring excessive hinge tag(s)." << std::endl;
}
sgSubVec3(axis, hinge_2, hinge_1);
if (sgLengthVec3(axis) < 0.001)
{
std::cerr << "ControlSurfaceAnimation: Insufficient spacing between hinges!" << std::endl;
failed = true;
}
}
if (failed)
{
std::cerr << "ControlSurfaceAnimation: Animation setup failed." << std::endl;
// set to some non-critical defaults
datasource.resize(1);
datasource[0] = &fallback_data;
source_gain.resize(1);
source_gain[0] = 1.0;
sgSetVec3(hinge_1, 0.0f, 0.0f, 0.0f);
sgSetVec3(hinge_2, 1.0f, 0.0f, 0.0f);
sgSubVec3(axis, hinge_2, hinge_1);
}
sgMakeIdentMat4(move_to_origin);
move_to_origin[3][0] = -hinge_1[0];
move_to_origin[3][1] = -hinge_1[1];
move_to_origin[3][2] = -hinge_1[2];
sgMakeTransMat4(move_back, hinge_1);
realInit();
}
void
SimCarCollideXYScene(tCar *car)
{
tTrackSeg *seg = car->trkPos.seg;
tTrkLocPos trkpos;
int i;
tDynPt *corner;
//t3Dd normal;
tdble initDotProd;
tdble dotProd;
tTrackBarrier *curBarrier;
tdble dmg;
if (car->carElt->_state & RM_CAR_STATE_NO_SIMU) {
return;
}
tdble energy_restitution = 0.999f;
corner = &(car->corner[0]);
for (i = 0; i < 4; i++, corner++) {
seg = car->trkPos.seg;
RtTrackGlobal2Local(seg, corner->pos.ax, corner->pos.ay, &trkpos, TR_LPOS_TRACK);
seg = trkpos.seg;
tdble toSide;
if (trkpos.toRight < 0.0) {
// collision with right border.
curBarrier = seg->barrier[TR_SIDE_RGT];
toSide = trkpos.toRight;
} else if (trkpos.toLeft < 0.0) {
// collision with left border.
curBarrier = seg->barrier[TR_SIDE_LFT];
toSide = trkpos.toLeft;
} else {
continue;
}
const tdble& nx = curBarrier->normal.x;
const tdble& ny = curBarrier->normal.y;
t3Dd normal = {nx, ny, 0.0f};
car->DynGCg.pos.x -= nx * toSide;
car->DynGCg.pos.y -= ny * toSide;
car->DynGC.pos.x = car->DynGCg.pos.x;
car->DynGC.pos.y = car->DynGCg.pos.y;
// Corner position relative to center of gravity.
//tdble cx = corner->pos.ax - car->DynGCg.pos.x;
//tdble cy = corner->pos.ay - car->DynGCg.pos.y;
car->blocked = 1;
car->collision |= SEM_COLLISION;
// Impact speed perpendicular to barrier (of corner).
initDotProd = nx * corner->vel.x + ny * corner->vel.y;
//printf("%f = (%f %f)'(%f %f)\n", initDotProd, nx, ny, corner->vel.x, corner->vel.y);
// Compute dmgDotProd (base value for later damage) with a heuristic.
tdble absvel = (float)MAX(1.0, sqrt(car->DynGCg.vel.x*car->DynGCg.vel.x + car->DynGCg.vel.y*car->DynGCg.vel.y));
tdble GCgnormvel = car->DynGCg.vel.x*nx + car->DynGCg.vel.y*ny;
tdble cosa = GCgnormvel/absvel;
tdble dmgDotProd = GCgnormvel*cosa;
// veolcity projected to normal
tdble vPx = nx * corner->vel.x;
tdble vPy = ny * corner->vel.y;
//tdble vP = sqrt(vPx*vPx + vPy*vPy);
// veolcity projected to tangent plane
tdble vQx = corner->vel.x - vPx;
tdble vQy = corner->vel.y - vPy;
tdble vQ = sqrt(vQx*vQx + vQy*vQy);
// Fix this to be applied only perpendicular to the normal
dotProd = initDotProd * curBarrier->surface->kFriction;
// calculate projection of velocity to perpendicular
{
// this is only used for propagating response to other layers
sgVec3 normal_l;
tdble d2 = dotProd;
t2sg3(normal, normal_l);
sgRotateVecQuat (normal_l, car->posQuat);
car->DynGC.acc.x -= normal_l[SG_X] * d2;
car->DynGC.acc.y -= normal_l[SG_Y] * d2;
car->carElt->_accel_x -= normal_l[SG_X] * d2;
car->carElt->_accel_y -= normal_l[SG_Y] * d2;
}
// Dammage.
dotProd = initDotProd;
dmg = 0.0f;
if (curBarrier->surface->kRebound > 1.0) {
printf("warning: rebound constant %f > 1\n", curBarrier->surface->kRebound);
} else {
dotProd *= curBarrier->surface->kRebound;
}
// If the car moves toward the barrier, rebound.
tdble normal_impulse_x = - nx * dotProd;
tdble normal_impulse_y = - ny * dotProd;
tdble dP3 = initDotProd * curBarrier->surface->kFriction / vQ;// could divide by vQ, but it's better (I think) to have it proportional to speed.
tdble friction_impulse_x = vQx * dP3;
tdble friction_impulse_y = vQy * dP3;
if (dotProd < 0.0f) {
//printf ("CollideXY\n");
tdble E_prev = SimCarDynamicEnergy(car);
// propagate damages
if ((car->carElt->_state & RM_CAR_STATE_FINISH) == 0) {
dmgDotProd = (float)(dmgDotProd*dmgDotProd*0.5
+ friction_impulse_x*friction_impulse_x
+ friction_impulse_y*friction_impulse_y);
dmg = curBarrier->surface->kDammage * dmgDotProd * simDammageFactor[car->carElt->_skillLevel];
car->dammage += (int)dmg;
}
car->collision |= SEM_COLLISION_XYSCENE;
car->normal.x = nx * dmg;
car->normal.y = ny * dmg;
car->collpos.x = corner->pos.ax;
car->collpos.y = corner->pos.ay;
//printf ("ColXY: (%f %f) + (%f %f)\n",
//normal_impulse_x, normal_impulse_y,
//friction_impulse_x, friction_impulse_y);
// Calculate change in rotational momentum.
// ----------------------------------------
// Put the impulse in a 3d vector
sgVec3 impulse = {normal_impulse_x + friction_impulse_x,
normal_impulse_y + friction_impulse_y,
0.0};
// rotate it to the target frame
sgRotateVecQuat (impulse, car->posQuat);
car->DynGC.vel.x += impulse[SG_X];
car->DynGC.vel.y += impulse[SG_Y];
car->DynGC.vel.z += impulse[SG_Z];
// Put the point of impact in a 3d vector
sgVec3 v = {car->statGC.x + corner->pos.x,
car->statGC.y + corner->pos.y,
-car->statGC.z};
// Calculate moments
tdble Mx = + impulse[SG_Z] * v[SG_Y] - impulse[SG_Y] * v[SG_Z];
tdble My = - impulse[SG_Z] * v[SG_X] + impulse[SG_X] * v[SG_Z];
tdble Mz = - impulse[SG_X] * v[SG_Y] + impulse[SG_Y] * v[SG_X];
// Add moments to rotational inertia
tdble rot_mom_scale = 0.25f*car->mass;// * SimDeltaTime;
car->rot_mom[SG_X] -= rot_mom_scale * Mx;// * car->Iinv.x;
car->rot_mom[SG_Y] -= rot_mom_scale * My;// * car->Iinv.y;
car->rot_mom[SG_Z] -= rot_mom_scale * Mz; //* car->Iinv.z;
//printf ("M_w:%f J:%f M_c:%g\n", car->rot_acc[SG_Z], car->rot_mom[SG_Z], rot_mom_scale * Mz);
for (int i=0; i<3; i++) {
if (fabs(car->rot_mom[i]) > 2000.0) {
//printf ("rot_mom: %f\n", (car->rot_mom[i]));
car->rot_mom[i] = (float)(2000*SIGN(car->rot_mom[i]));
}
}
// transform velocity to global frame
if (1) {
t3Dd original;
t3Dd updated;
original.x = car->DynGC.vel.x;
original.y = car->DynGC.vel.y;
original.z = car->DynGC.vel.z;
QuatRotate(original, car->posQuat, updated);
car->DynGCg.vel.x = updated.x;
car->DynGCg.vel.y = updated.y;
car->DynGCg.vel.z = updated.z;
}
SimCarLimitDynamicEnergy(car, energy_restitution*E_prev);
}
#if 0
static tdble DEFORMATION_THRESHOLD = 0.01f;
if (car->options->aero_damage
|| sgLengthVec3(force) > DEFORMATION_THRESHOLD) {
sgVec3 poc;
poc[0] = corner->pos.x;
poc[1] = corner->pos.y;
poc[2] = (urandom()-0.5)*2.0;
sgRotateVecQuat (force, car->posQuat);
sgNormaliseVec3(force);
for (int i=0; i<3; i++) {
force[i]*=dmg;
}
// just compute values, gr does deformation later.
// must average position and add up force.
SimCarCollideAddDeformation(car, poc, force);
// add aero damage if applicable
if (car->options->aero_damage) {
SimAeroDamage (car, poc, sgLengthVec3(force));
}
}
#endif
}
}
void TransIcelandicExpress::simulate() {
sgVec3 fwd, right, up, frict, v;
sgVec3 playerForce;
float cf_accel = 1.0, // m/s^2
maxVel = 0.5, // m/2
cf_friction_land = 8.0,
cf_friction_ice = 5.0;
float pv,
ff; // friction fudge... more friction for slow objects
float timestep = 0.01f;
static float timeleft = 0.0f;
timeleft += deltaT;
sgSetVec3( up, 0.0, 1.0, 0.0 );
sgSetVec3( playerForce, 0.0, 0.0, 0.0 );
while (timeleft > timestep) {
sgCopyVec3( fwd, cameraPos );
sgNegateVec3( fwd );
fwd[1] = 0.0;
sgNormalizeVec3( fwd );
sgVectorProductVec3( right, fwd, up );
// todo: if on the ground
sgScaleVec3( fwd, cf_moveForward );
sgAddVec3( playerForce, fwd );
sgScaleVec3( right, cf_moveSideways );
sgAddVec3( playerForce, right );
sgScaleVec3( playerForce, cf_accel * timestep );
sgAddVec3( player->vel, playerForce );
pv = sgLengthVec3( player->vel ) ;
ff = (1.0 - ((pv / maxVel)* 0.8));
ff = ff*ff;
sgCopyVec3( frict, player->vel );
sgNegateVec3( frict );
sgScaleVec3( frict, ff * cf_friction_ice * timestep );
sgAddVec3( player->vel, frict );
dbgVel.push_back( pv );
if (dbgVel.size() > 100 ) {
dbgVel.pop_front();
}
if ( pv > maxVel ) {
//printf("maxvel!\n" );
sgNormalizeVec3( player->vel );
sgScaleVec3( player->vel, maxVel );
}
sgCopyVec3( v, player->vel );
sgScaleVec3( v, timestep );
sgAddVec3( player->pos, v );
// advance
timeleft -= timestep;
}
player->pos[1] = getHeight( player->pos );
}
