static inline float computeDenominator(float invMass, float invInertia, const vec3* offset, const vec3* norm)
{
// If you apply an impulse of 1.0f in the direction of 'norm'
// at position specified by 'offset' then the point will change
// velocity by the amount calculated here
vec3 cross;
veccross(&cross, offset, norm);
vecscale(&cross, &cross, invInertia);
veccross(&cross, &cross, offset);
return vecdot(norm, &cross) + invMass;
}
static inline void addImpulseAtOffset(vec3* vel, vec3* angVel, float invMass, float invInertia, const vec3* offset, const vec3* impulse)
{
vec3 tmp;
vecaddscale(vel, vel, impulse, invMass);
veccross(&tmp, offset, impulse);
vecscale(&tmp, &tmp, invInertia);
vecadd(angVel, angVel, &tmp);
}
static vec3 getPointVel(Chassis* c, const vec3* offset)
{
// offset is in world space
vec3 v;
veccross(&v, &c->angVel, offset);
vecadd(&v, &v, &c->vel);
return v;
}
static void doCorrection3(Chassis* c, const vec3* worldOffset, const vec3* axis, float requiredVelocityChange, float linearRatio)
{
const float u = 1.f - linearRatio;
// Add half to the linear
vec3 tmp;
vecaddscale(&c->vel, &c->vel, axis, requiredVelocityChange*linearRatio);
// Add the other half to the angular
vec3 cross;
veccross(&cross, worldOffset, axis);
veccross(&cross, &cross, worldOffset);
float angularResponse = vecdot(axis, &cross);
float angVelChange = requiredVelocityChange / angularResponse;
vec3 impulse;
vecscale(&impulse, axis, angVelChange*u);
veccross(&tmp, worldOffset, &impulse);
vecadd(&c->angVel, &c->angVel, &tmp);
}
/*--
Cat lbxgl;Entity;Query
Form
int LBXGL_Entity_QueryLine(LBXGL_Entity *ents, \
LBXGL_Entity **array, int max, \
double *start, double *end);
Description
Query entities which intersect with the line defined by start and end.
array is used to hold the query results, and max is the maximal \
number of entities that can be placed in array (at least max+1 \
entries should exist within array to handle the trailing NULL).
This returns the number of entities placed in array.
--*/
int LBXGL_Entity_QueryLine(LBXGL_Entity *ents,
LBXGL_Entity **array, int max,
double *start, double *end)
{
LBXGL_Entity *cur;
int i, j;
double *corg, *cmins, *cmaxs;
double crad, dx, dy, dz, dl, de;
double dir[3], px[3], py[3];
double rorg[3];
for(i=0; i<3; i++)dir[i]=end[i]-start[i];
vecnorm(dir);
perp_vector(dir, px);
veccross(dir, px, py);
for(i=0; i<3; i++)rorg[i]=end[i]-start[i];
de=vecdot(rorg, dir);
cur=ents;
i=0;
while(cur && (i<max))
{
corg=LBXGL_Entity_GetProperty(cur, "origin");
if(!corg)
{
cur=cur->next;
continue;
}
for(j=0; j<3; j++)rorg[j]=corg[j]-start[j];
dx=vecdot(rorg, px);
dy=vecdot(rorg, py);
dz=vecdot(rorg, dir);
dl=sqrt((dx*dx)+(dy*dy));
crad=LBXGL_Entity_GetRadius(cur);
if((dl<=crad) && (dz>=0) && (dz<=de))
{
array[i++]=cur;
array[i]=NULL;
}
cur=cur->next;
}
return(i);
}
int main (int argc, char **argv)
{
#if 0
const int N = 4;
float y[N] = {-0.653828, -0.653828, 0.753333, 0.753333};
float k[N];
float l[2] = {0.f, 0.f};
float kdl[2] = {0.f, 0.f};
float n[2] = {0.f, 0.f};
for (int i=0; i<N; i++)
{
if (y[i] > 0.f)
{
l[1] += y[i];
n[1] += 1.f;
}
else
{
l[0] -= y[i];
n[0] += 1.f;
}
}
kdl[1] = l[1] / (n[1]*l[0] + n[0]*l[1]);
kdl[0] = l[0] / (n[1]*l[0] + n[0]*l[1]);
for (int i=0; i<2; i++)
{
printf("[%d] l = %f kdl = %f\n", i, l[i], kdl[i]);
}
for (int i=0; i<N; i++)
{
k[i] = y[i] > 0.f ? kdl[1] : kdl[0];
}
float force = 0.f;
float torque = 0.f;
for (int i=0; i<N; i++)
{
force += k[i];
torque += y[i] * k[i];
}
printf("force = %f\n", force);
printf("torque = %f\n", torque);
printf("kdl[0]/kdl[1] = %f\n", kdl[0]/kdl[1]);
return 0;
#endif
#if 0
vec3 r = {1.f, -0.3f, 0.f};
vec3 fground = {0.f, 1.f, 0.f};
vec3 fcent = {-1.f, 0.f, 0.f};
vec3 tground;
vec3 tcent;
veccross(&tground, &r, &fground);
veccross(&tcent, &r, &fcent);
printf("tground = %f\n", tground.z);
printf("tcent = %f\n", tcent.z);
return 0;
float axleFriction = 200.1f;
float mass = 10.f;
float invMass = 1.f/mass;
float v = 10.0f;
float dt = 0.01;
for (int r=0; r<100; r++)
{
float force = -axleFriction*sgn(v);
v = v + force*invMass * dt;
printf("v = %f\n", v);
}
return 0;
#endif
#if 0
float mass = 10.f;
float wheelmass = 1.0f;
float radius = 0.1f;
float wheelInertia = 2.f/5.f*radius*radius*wheelmass;
float vel = 0.f;
float wheelVel= 0.f;
float dt = 0.01f;
float torque = 1000.f;
float angSpeed = -dt*torque*radius/wheelInertia;
float momentum = angSpeed*wheelInertia;
printf("momentum put in = %f \n", momentum);
for (int repeat = 0; repeat<10; repeat++)
{
{
float contactSpeed = radius * angSpeed + wheelVel;
float error = contactSpeed;
float denom = 1.f/wheelmass + radius*radius/wheelInertia;
float impulse = error / denom;
// Add impulse to the wheel
wheelVel = wheelVel - impulse/wheelmass;
angSpeed = angSpeed - radius*impulse/wheelInertia;
}
// Axis error
{
float error = wheelVel - vel;
float denom = 1.f/wheelmass + 1.f/mass;
float impulse = error/denom;
wheelVel = wheelVel - impulse/wheelmass;
vel = vel + impulse/mass;
}
}
printf("momentum c = %f\n", vel*mass);
printf("momentum w = %f\n", vel*wheelmass);
printf("momentum aw = %f\n", angSpeed*wheelInertia);
printf("total momentum = %f\n", vel*(mass+wheelmass) + angSpeed*wheelInertia);
printf("chassis = %f, wheel = %f\n", vel, wheelVel);
return 0;
#endif
#if 0
float mass = 10.f;
float inertia = mass * 0.4f;
vec3 wheelOffset = {0.f, 1.5f, -0.2f};
float wheelmass = 1.0f;
float radius = 0.1f;
float wheelInertia = 2.f/5.f*radius*radius*wheelmass;
vec3 vel = {0.f, 0.f, 0.f};
vec3 w = {0.f, 0.f, 0.f};
float wheelVel= 0.f;
float dt = 0.01f;
float torque = 1000.f;
float angSpeed = -dt*torque*radius/wheelInertia;
for (int repeat = 0; repeat<100; repeat++)
{
{
float contactSpeed = radius * angSpeed + wheelVel;
float error = contactSpeed;
float denom = 1.f/wheelmass + radius*radius/wheelInertia;
float impulse = error / denom;
// Add impulse to the wheel
wheelVel = wheelVel - impulse/wheelmass;
angSpeed = angSpeed - radius*impulse/wheelInertia;
}
// Axis error
{
// float axleVel = vel;
vec3 cross;
veccross(&cross, &w, &wheelOffset);
float axleVel = vel.y + cross.y;
vec3 pulldir = {0.f, 1.f, 0.f};
float error = wheelVel - axleVel;
if (error < 0.000001f) break;
float denom = 1.f/wheelmass + computeDenominator(1.f/mass, 1.f/inertia, &wheelOffset, &pulldir);
float impulse = error/denom;
wheelVel = wheelVel - impulse/wheelmass;
//vel = vel + impulse/mass;
{
vecscale(&pulldir, &pulldir, impulse);
addImpulseAtOffset(&vel, &w, 1.f/mass, 1.f/inertia, &wheelOffset, &pulldir);
}
}
}
printf("wheelVel = %f, vel = %f, w = %f\n", wheelVel, vel.y, w.x);
printf("%f\n", w.x/vel.y);
// Simple force application!
{
wheelVel = 0.f;
vec3 impulse = {0.f, dt * torque / radius, 0.f};
veczero(&vel);
veczero(&w);
vec3 offset = wheelOffset;
//offset.z -= radius;
printf("wheelVel = %f, vel = %f, w = %f\n", wheelVel, vel.y, w.x);
addImpulseAtOffset(&vel, &w, 1.f/mass, 1.f/inertia, &offset, &impulse);
}
printf("wheelVel = %f, vel = %f, w = %f\n", wheelVel, vel.y, w.x);
printf("%f\n", w.x/vel.y);
return 0;
#endif
#if 0
float x = 100.f;
float friction = 20.f;
float dt = 0.01f;
float r = dt*friction;
int n=0;
while (n<1000)
{
//x = x - r*x/(fabsf(x)+r);
printf("%f\n", x);
n++;
}
return 0;
#endif
timerUpdate(&g_time);
vehicleInit();
// GLUT Window Initialization:
glutInit (&argc, argv);
glutInitWindowSize (s_width, s_height);
glutInitDisplayMode ( GLUT_RGB | GLUT_DOUBLE | GLUT_DEPTH);
glutCreateWindow ("CS248 GLUT example");
// Initialize OpenGL graphics state
initGraphics();
// Register callbacks:
glutDisplayFunc (display);
glutReshapeFunc (reshape);
glutKeyboardFunc (keyboard);
glutMouseFunc (mouseButton);
glutMotionFunc (mouseMotion);
glutIdleFunc (animateScene);
//BuildPopupMenu ();
//glutAttachMenu (GLUT_RIGHT_BUTTON);
// Turn the flow of control over to GLUT
glutMainLoop ();
return 0;
}
static void vehicleSubTick(Chassis* c, float dt)
{
if (g_step==0) return;
if (g_step&1) g_step = 0;
vec3* chassisPos = &c->pose.v[3].v3;
vec3* x = &c->pose.v[0].v3;
vec3* y = &c->pose.v[1].v3;
vec3* z = &c->pose.v[2].v3;
// This bit is done by the physics engine
if(1)
{
vecaddscale(chassisPos, chassisPos, &c->vel, dt);
mtx rot;
matrixRotateByVelocity(&rot, &c->pose, &c->angVel, dt);
matrixCopy33(&c->pose, &rot);
}
// Damp
vecscale(&c->vel, &c->vel, expf(-dt*1.f));
vecscale(&c->angVel, &c->angVel, expf(-dt*1.f));
if (fabsf(c->angVel.x)<0.01f) c->angVel.x = 0.f;
if (fabsf(c->angVel.y)<0.01f) c->angVel.y = 0.f;
if (fabsf(c->angVel.z)<0.01f) c->angVel.z = 0.f;
ClampedImpulse frictionImpulse[numWheels][2];
c->steer = g_steer;
//g_steer *= expf(-dt*3.f);
//g_speed *= expf(-dt*3.f);
static float latf = 10.f;
static float angSpeed = 0.f;
if (g_handBrake>0.f)
{
g_handBrake *= expf(-4.f*dt);
g_speed *= expf(-4.f*dt);
if (g_handBrake < 0.1f)
{
g_handBrake = 0.f;
}
}
// Prepare
for (int i=0; i<numWheels; i++)
{
Suspension* s = c->suspension[i];
Wheel* w = s->wheel;
// Calculate the world position and offset of the suspension point
vec3mtx33mulvec3(&s->worldOffset, &c->pose, &s->offset);
vec3mtx43mulvec3(&s->worldDefaultPos, &c->pose, &s->offset);
w->pos = s->worldDefaultPos;
vec3 pointVel = getPointVel(c, &s->worldOffset);
vecadd(&w->vel, &w->vel, &pointVel);
float maxFriction0 = 2.0f * dt * c->mass * gravity * (1.f/(float)numWheels);
clampedImpulseInit(&frictionImpulse[i][0], maxFriction0);
float latfriction = 10.f;
float newAngSpeed = vecdot(z, &c->angVel);
float changeAngSpeed = (newAngSpeed - angSpeed)/dt;
angSpeed = newAngSpeed;
printf("changeAngSpeed = %f\n", changeAngSpeed);
float speed = fabsf(vecdot(y, &c->vel));
const float base = 0.5f;
if (g_speed>=0 && i>=2)
{
latfriction = 1.f*expf(-5.f*g_handBrake) + base;
// latfriction = 1.f*expf(-2.f*fabsf(speed*changeAngSpeed)) + base;
// if (angSpeed*g_steer < -0.1f)
// {
// latfriction = base;
// }
//if (g_steer == 0.f)
//{
// latf += (10.f - latf) * (1.f - exp(-0.1f*dt));
//}
//else
//{
// latf += (0.1f - latf) * (1.f - exp(-10.f*dt));
//}
//latfriction = latf;
}
else
{
latfriction = 10.f;
}
float maxFriction1 = latfriction * dt * c->mass * gravity * (1.f/(float)numWheels);
clampedImpulseInit(&frictionImpulse[i][1], maxFriction1);
vecset(&s->hitNorm, 0.f, 0.f, 1.f);
float steer = w->maxSteer*c->steer * (1.f + 0.3f*s->offset.x*c->steer);
vecscale(&w->wheelAxis, x, cosf(steer));
vecsubscale(&w->wheelAxis, &w->wheelAxis, y, sinf(steer));
w->frictionDir[0];
veccross(&w->frictionDir[0], z, &w->wheelAxis);
w->frictionDir[1] = w->wheelAxis;
w->angSpeed = -40.f*g_speed;
}
//=============
// VERBOSE
//=============
#define verbose false
#define dump if (verbose) printf
dump("==========================================\n");
dump("START ITERATION\n");
dump("==========================================\n");
float solverERP = numIterations>1 ? 0.1f : 1.f;
float changeSolverERP = numIterations>1 ? (1.f - solverERP)/(0.01f+ (float)(numIterations-1)) : 0.f;
for (int repeat=0; repeat<numIterations; repeat++)
{
dump(" == Start Iter == \n");
for (int i=0; i<numWheels; i++)
{
Suspension* s = c->suspension[i];
Wheel* w = s->wheel;
const bool axisError = true;
const bool friction = true;
// Friction
if (friction)
{
vec3 lateralVel;
vecaddscale(&lateralVel, &w->vel, &s->hitNorm, -vecdot(&s->hitNorm, &w->vel));
vecaddscale(&lateralVel, &lateralVel, &w->frictionDir[0], +w->angSpeed * w->radius);
{
int dir = 0;
float v = vecdot(&lateralVel, &w->frictionDir[dir]);
float denom = 1.f/w->mass + w->radius*w->radius*w->invInertia;
float impulse = clampedImpulseApply(&frictionImpulse[i][dir], - solverERP * v / denom);
vec3 impulseV;
vecscale(&impulseV, &w->frictionDir[dir], impulse);
vecaddscale(&w->vel, &w->vel, &impulseV, 1.f/w->mass);
w->angSpeed = w->angSpeed + (impulse * w->radius * w->invInertia);
}
if (1)
{
int dir=1;
float v = vecdot(&lateralVel, &w->frictionDir[dir]);
float denom = 1.f/w->mass;
float impulse = clampedImpulseApply(&frictionImpulse[i][dir], - solverERP * v / denom);
vec3 impulseV;
vecscale(&impulseV, &w->frictionDir[dir], impulse);
vecaddscale(&w->vel, &w->vel, &impulseV, 1.f/w->mass);
}
//dump("gound collision errorV = %f, vel of wheel after = %f\n", penetration, vecdot(&w->vel, &s->hitNorm));
}
if (axisError) // Axis Error
{
vec3 offset;
vecsub(&offset, &w->pos, chassisPos);
vec3 pointvel = getPointVel(c, &offset);
vec3 error;
vecsub(&error, &pointvel, &w->vel);
vecaddscale(&error, &error, z, -vecdot(&error, z));
vec3 norm;
if (vecsizesq(&error)>0.001f)
{
dump("axis error %f\n", vecsize(&error));
vecnormalise(&norm, &error);
float denom = computeDenominator(1.f/c->mass, 1.f/c->inertia, &offset, &norm) + 1.f/w->mass;
vecscale(&error, &error, -solverERP/denom);
addImpulseAtOffset(&c->vel, &c->angVel, 1.f/c->mass, 1.f/c->inertia, &offset, &error);
vecaddscale(&w->vel, &w->vel, &error, -solverERP/w->mass);
}
//dump("axis error vel of wheel after = %f, inline = %f\n", vecdot(&w->vel, &s->hitNorm), vecdot(&w->vel, &s->axis));
}
}
solverERP += changeSolverERP;
}
for (int i=0; i<numWheels; i++)
{
Suspension* s = c->suspension[i];
Wheel* w = s->wheel;
vec3 pointVel = getPointVel(c, s);
// Convert suspension wheel speed back to car space
vecsub(&w->vel, &w->vel, &pointVel);
}
}
