Exemplo n.º 1
0
// 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);


};