// FUNCTION ====== physics
void physics(){
double dt = PHYS_dt;
long long nt1 = getCPUticks();
for(int iSubStep=0; iSubStep<PHYS_STEPS_PER_FRAME; iSubStep++ ){
double Cd = -0.5;
double g = -9.81;
double k = 20;
rbody.clean_temp ( );
rbody.force.set ( 0,g*rbody.mass,0 );
/*
for ( int i=0; i<len; i++ ){
rbody.apply_anchor( k, poss[i], poss[i] );
}
rbody.vel.mul(0.8);
rbody.L .mul(0.8);
*/
Vec3d p0,p1; p1.set(0.0,0.0,0.0); p1.set(1.0,2.0,3.0);
rbody.apply_force ( rbody.vel*Cd , p0 );
rbody.apply_anchor( k, p1, p0 );
rbody2.clean_temp ( );
rbody2.force.set ( 0,g*rbody2.mass,0 );
rbody2.apply_force ( rbody2.vel*Cd, p0 );
constrain1->apply();
/*
rbody.apply_force ( rbody.vel*Cd , {0,0,0} );
rbody.apply_anchor( k, {1,2,3}, {0,0,0} );
rbody2.clean_temp ( );
rbody2.force.set ( 0,g*rbody2.mass,0 );
rbody2.apply_force ( rbody2.vel*Cd, {0,0,0} );
constrain1->apply();
*/
//printVec(rbody.force ); printf("force\n");
//printVec(rbody.torq ); printf("torq\n");
rbody .move(dt);
rbody2.move(dt);
}
long long nt2 = getCPUticks();
double perStep = double(nt2-nt1)/PHYS_STEPS_PER_FRAME;
tickSum+= perStep; stepSum++;
printf( " PERFORMANCE: %f ticks/step ( in %i steps ) average: %f \n", perStep, PHYS_STEPS_PER_FRAME, tickSum/stepSum );
}
void TestAppRigidBody::draw(){
//printf( " ==== frame %i \n", frameCount );
glClearColor( 0.5f, 0.5f, 0.5f, 1.0f );
glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT );
int perFrame = 1;
double dt = 0.01;
// ---- soft body test
//truss.dt = dt;
//truss.damp = 0.0;
//for(int itr=0; itr<perFrame; itr++){ truss.step(); }
//drawTruss( &truss, 1.0, 1.0, false );
// ---- RidigBody basics - drag
Vec3d gdp,gv,gf;
rb.clean_temp();
rb.velOfPoint ( {2.0,5.0,6.0}, gv, gdp );
gf.set_mul(gv, -0.01 );
rb.apply_force( gf, gdp );
rb.move(dt);
rb.pos.set(0.0);
rb.vel.set(0.0);
glPushMatrix();
glTranslatef(-5.0,0.0,0.0);
glScalef(0.6,0.6,0.6);
drawRigidBody( rb, npoints, (Vec3d*)points );
glColor3f(1.0f,1.0f,1.0f);
Draw3D::drawPointCross( gdp, 0.2 );
Draw3D::drawVecInPos ( gf*1000.0, gdp );
glPopMatrix();
// ---- RigidBody springs
for(int itr=0; itr<perFrame; itr++){
for(SpringConstrain * sp : springs){
sp->apply();
}
for( Object3D * o : objects ){
RigidBody * rb = o->controler;
if(rb){
rb->vel.mul( 1-dt*0.2 );
rb->L. mul( 1-dt*0.2 );
rb->apply_force({0,-9.81,0},{0.0,0.0,0.0});
rb->move_RigidBody(dt);
rb->clean_temp();
}
}
}
for( Object3D * o : objects ){
if(o->controler){
o->bounds.pos = o->controler->pos;
//o->bounds.orientation = o->controler->rotMat;
o->bounds.orientation.setT(o->controler->rotMat);
};
}
glEnable( GL_LIGHTING );
glEnable(GL_DEPTH_TEST);
for( Object3D * o : objects ){
if (o->shape){
float glMat[16];
glPushMatrix();
Draw3D::toGLMat( o->bounds.pos, o->bounds.orientation, o->bounds.span, glMat );
glMultMatrixf( glMat );
glCallList( o->shape );
glPopMatrix();
}
}
glDisable( GL_LIGHTING );
for(SpringConstrain * sp : springs ){
Vec3d gp1,gp2,f;
sp->getPoints( gp1, gp2 );
f = sp->getForce( gp1, gp2 );
glColor3f(1.0f,1.0f,1.0f); Draw3D::drawLine( gp1, gp2 );
//glColor3f(1.0f,0.0f,0.0f); Draw3D::drawVecInPos( f, gp1 );
}
Draw3D::drawAxis(1.0);
};