void AeroCraftGUI:: camera (){
glMatrixMode( GL_PROJECTION );
glLoadIdentity();
float fov = VIEW_ZOOM_DEFAULT/zoom;
glFrustum( -ASPECT_RATIO, ASPECT_RATIO, -1, 1, 1*fov, VIEW_DEPTH*fov );
Mat3d camMat;
Vec3f camPos;
convert(world->myCraft->pos, camPos );
float camDist = 10.0;
if(first_person){
// third person camera attached to aero-craft
camMat.setT( world->myCraft->rotMat );
//glTranslatef ( -camPos.x, -camPos.y, -camPos.z );
}else{
// third person camera attached to aero-craft
qCamera.toMatrix( camMat );
camMat.T();
}
float glMat[16];
Draw3D::toGLMatCam( { 0.0f, 0.0f, 0.0f}, camMat, glMat );
glMultMatrixf( glMat );
glTranslatef ( -camPos.x+camMat.cx*camDist, -camPos.y+camMat.cy*camDist, -camPos.z+camMat.cz*camDist );
glMatrixMode (GL_MODELVIEW);
glLoadIdentity();
}
void drawRigidBody( const RigidBody& rb, int npoints, Vec3d* points ){
glColor3f(0.0f,0.0f,0.0f);
glPushMatrix();
float glMat[16];
Mat3d rotT;
rotT.setT(rb.rotMat);
Draw3D::toGLMat( rb.pos, rotT, glMat );
glMultMatrixf( glMat );
if( points ) for(int i=0; i<npoints; i++) Draw3D::drawPointCross(points[i],0.3);
glPopMatrix();
glColor3f(1.0f,0.0f,1.0f); Draw3D::drawVecInPos( rb.L, {0.0,0.0,0.0} );
glColor3f(0.0f,0.0f,1.0f); Draw3D::drawVecInPos( rb.omega, {0.0,0.0,0.0} );
if( points ){
for(int i=0; i<npoints; i++){
Vec3d p,v;
//rb.rotMat.dot_to_T(points[i], p);
//v.set_cross( p, rb.omega ); // omega is in global coordinates
rb.velOfPoint( points[i], v, p );
p.add(rb.pos);
Draw3D::drawVecInPos ( v, p );
Draw3D::drawPointCross( p, 0.2 );
}
};
};
void TetMesh::computeGradient(int element, const double * U, int numFields, double * grad) const
{
// grad is 9 x numFields
// grad is constant inside a tet
Vec3d vtx[4];
for(int i=0; i<4; i++)
vtx[i] = *getVertex(element,i);
// form M =
// [b - a]
// [c - a]
// [d - a]
Mat3d M(vtx[1] - vtx[0], vtx[2] - vtx[0], vtx[3] - vtx[0]);
Mat3d MInv = inv(M);
//printf("M=\n");
//M.print();
for(int field=0; field<numFields; field++)
{
// form rhs =
// [U1 - U0]
// [U2 - U0]
// [U3 - U0]
const double * u[4];
for(int i=0; i<4; i++)
u[i] = &U[3 * numVertices * field + 3 * getVertexIndex(element, i)];
Vec3d rows[3];
for(int i=0; i<3; i++)
rows[i] = Vec3d(u[i+1]) - Vec3d(u[0]);
Mat3d rhs(rows);
//printf("rhs=\n");
//rhs.print();
Mat3d gradM = trans(MInv * rhs);
gradM.convertToArray(&grad[9 * field]);
/*
// test gradient
if (field == 0)
{
printf("----\n");
printf("0: pos: %.15f %.15f %.15f | uExact: %.15f %.15f %.15f\n", vtx[0][0], vtx[0][1], vtx[0][2], u[0][0], u[0][1], u[0][2]);
for(int vertex=0; vertex<3; vertex++)
{
Vec3d u1 = Vec3d(u[vertex+1]);
printf("%d: ", vertex+1);
printf("pos: %.15f %.15f %.15f | uExact: %.15f %.15f %.15f | ", vtx[vertex+1][0], vtx[vertex+1][1], vtx[vertex+1][2], u1[0], u1[1], u1[2]);
Vec3d u1approx = Vec3d(u[0]) + gradM * (vtx[vertex+1] - vtx[0]);
printf("uApprox: %.15f %.15f %.15f\n", u1approx[0], u1approx[1], u1approx[2]);
}
printf("----\n");
}
*/
}
}
bool Spatial::rotateToHeading(Vec2d pos)
{
Vec2d toTarget = Vec2d(pos-mPos);
toTarget.normalize();
double angle = acos(mHeading.dot(pos));
if (angle < 0.000001)
return true;
if (angle > mMaxTurnRate)
angle = mMaxTurnRate;
Mat3d rotationMat;
rotationMat.rotate(angle*mHeading.sign(toTarget));
rotationMat.transformVec2d(mHeading);
rotationMat.transformVec2d(mVelocity);
mSide = mHeading.perp();
return false;
}
void MolecularEditorApp::draw(){
glClearColor( 0.5f, 0.5f, 0.5f, 0.0f );
glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT );
//glDisable( GL_DEPTH_TEST );
//converged = true;
//delay = 100; world.optimizer->dt_max = 0.00001; world.optimizer->dt_max = 0.00001; perFrame=1;
//delay = 1000; perFrame=1;
//perFrame=1; // world.optimizer->dt_max = 0.01;
//world.optimizer->dt_max = 0.01;
//world.nonCovalent = false;
if( !converged ){
long tick1 = getCPUticks();
int iter;
for(iter=0; iter<perFrame; iter++){
world.rigidOptStep( );
printf(" opt step %i fmax %g \n", world.optimizer->stepsDone, world.fmax );
if( world.fmax < fmaxConverg ){
converged = true;
printf(" converged after %i step \n", world.optimizer->stepsDone );
if(fout_xyz){ fclose(fout_xyz); fout_xyz = NULL; }
fout_xyz = fopen("relaxed.xyz", "w");
char str[256];
sprintf(str,"# fmax = %g", world.fmax );
world.exportAtomsXYZ( fout_xyz, str );
fclose(fout_xyz); fout_xyz = NULL;
break;
}
if( (getCPUticks()-tick1)>4e+8 ) break;
}
double ticks = (getCPUticks() - tick1)/((double)perFrame);
printf("======= %i %5.2f Mticks/iter %5.2f tick/atom %5.2f ticks/interaction \n", iter, ticks*1.0e-6, ticks/world.nAtomTot, ticks/world.nInteractions );
world.saveInstances( "instances_lastStep.ini" );
if(fout_xyz){
char str[256];
sprintf(str,"# fmax = %g", world.fmax );
world.exportAtomsXYZ( fout_xyz, str );
}
}
//exit(0);
//world.nonBondingFroces_buf(); return;
glMatrixMode(GL_MODELVIEW);
//glMatrixMode(GL_PROJECTION);
glEnable(GL_LIGHTING);
for (int i=0; i<world.nmols; i++){
if( world.instances[i]->viewlist > 0 ){
Mat3d rotmat;
float glMat[16];
//rot[i].toMatrix_unitary2( rotmat );
//rot[i].toMatrix_unitary( rotmat );
//printf( "%i (%3.3f,%3.3f,%3.3f) (%3.3f,%3.3f,%3.3f,%3.3f)\n", i, world.pos[i].x,world.pos[i].y,world.pos[i].z, world.rot[i].x,world.rot[i].y,world.rot[i].z,world.rot[i].w );
world.rot[i].toMatrix( rotmat );
glColor3f(0.0f,0.0f,0.0f); Draw3D::drawPointCross(world.pos[i],1.0);
Draw3D::toGLMat( world.pos[i], rotmat, glMat ); // somehow not working
//Draw3D::toGLMat( {0.0,0.0,0.0}, rotmat, glMat );
glPushMatrix();
glMultMatrixf( glMat );
//glTranslatef( world.pos[i].x,world.pos[i].y,world.pos[i].z );
//glMultTransposeMatrixf( glMat );
//glLoadMatrixf( glMat );
glCallList ( world.instances[i]->viewlist );
glPopMatrix();
}
};
//printf(" nLinkers %i &linkers %i \n" , world.nLinkers, world.linkers );
glDisable(GL_LIGHTING);
glColor3f(0.0f,1.0f,0.0f);
if( world.linkers ){
for (int il=0; il<world.nLinkers; il++){
Mat3d T;
Vec3d gpi,gpj;
MolecularLink& li = world.linkers[il];
int i = li.i;
world.rot[i].toMatrix( T);
T.dot_to( li.posi, gpi );
gpi.add( world.pos[i] );
int j = li.j;
world.rot[j].toMatrix(T);
T.dot_to( li.posj, gpj );
gpj.add( world.pos[j] );
Draw3D::drawLine( gpi, gpj );
Draw3D::drawPointCross(gpi,0.5);
Draw3D::drawPointCross(gpj,0.5);
//printf( "%i (%i,%i) (%3.3f,%3.3f,%3.3f) (%3.3f,%3.3f,%3.3f)\n" , il, i,j, li.posi.x, li.posi.y, li.posi.z, li.posj.x, li.posj.y, li.posj.z );
//printf( "%i (%3.3f,%3.3f,%3.3f) (%3.3f,%3.3f,%3.3f)\n" , il, gpi.x, gpi.y, gpi.z, gpj.x,gpj.y,gpj.z);
}
}
if( world.bonds ){
for (int il=0; il<world.nBonds; il++){
Mat3d T;
Vec3d lpi,lpj,gpi,gpj;
MolecularBond& bi = world.bonds[il];
int i = bi.imol;
world.rot[i].toMatrix( T);
lpi = world.instances[i]->xyzs[bi.iatom];
T.dot_to( lpi, gpi );
gpi.add( world.pos[i] );
int j = bi.jmol;
world.rot[j].toMatrix(T);
lpj = world.instances[j]->xyzs[bi.jatom];
T.dot_to( lpj, gpj );
gpj.add( world.pos[j] );
Draw3D::drawLine( gpi, gpj );
//printf( "%i (%i,%i) (%3.3f,%3.3f,%3.3f) (%3.3f,%3.3f,%3.3f)\n" , il, i,j, li.posi.x, li.posi.y, li.posi.z, li.posj.x, li.posj.y, li.posj.z );
//printf( "%i (%3.3f,%3.3f,%3.3f) (%3.3f,%3.3f,%3.3f)\n" , il, gpi.x, gpi.y, gpi.z, gpj.x,gpj.y,gpj.z);
}
}
//exit(0);
};
void PolarDecompositionGradient::Compute(const double * M, const double * Q, const double * S, const double * MDot, double * omega, double * QDot, double * SDot, const double * MDotDot, double * omegaDot, double * QDotDot)
{
// compute omega = G^{-1} (2 * skew(Q^T * MDot)), where G = (tr(S)I - S) * Q^T
// (see Barbic and Zhao, SIGGRAPH 2011)
// first, construct G, and invert it
// tempMatrix = tr(S)I - S
double tempMatrix[9];
for(int i=0; i<9; i++)
tempMatrix[i] = -S[i];
double trace = S[0] + S[4] + S[8];
tempMatrix[0] += trace;
tempMatrix[4] += trace;
tempMatrix[8] += trace;
double G[9]; // G = (tr(S)I - S) * Q^T
MATRIX_MULTIPLY3X3ABT(tempMatrix, Q, G);
Mat3d GM(G);
Mat3d GInvM = inv(GM);
double GInv[9];
GInvM.convertToArray(GInv);
// omega = GInv * (2 * skew(R^T * Mdot))
MATRIX_MULTIPLY3X3ATB(Q, MDot, tempMatrix);
double rhs[3];
SKEW_PART(tempMatrix, rhs);
VECTOR_SCALE3(rhs, 2.0);
MATRIX_VECTOR_MULTIPLY3X3(GInv, rhs, omega);
// compute QDot = tilde(omega) * Q
double omegaTilde[9];
SKEW_MATRIX(omega, omegaTilde);
//double QDot[9];
MATRIX_MULTIPLY3X3(omegaTilde, Q, QDot);
// compute SDot = Q^T * (MDot - QDot * S)
// tempMatrix = MDot - QDot * S
MATRIX_MULTIPLY3X3(QDot, S, tempMatrix);
for(int i=0; i<9; i++)
tempMatrix[i] = MDot[i] - tempMatrix[i];
// SDot = Q^T * tempMatrix
MATRIX_MULTIPLY3X3ATB(Q, tempMatrix, SDot);
if ((MDotDot != NULL) && (omegaDot != NULL))
{
// compute omegaDot = GInv * ( 2 skew(Q^T (ADotDot - omegaTilde * ADot)) - (tr(SDot) I - SDot) * Q^T * omega )
// (see Barbic and Zhao, SIGGRAPH 2011)
// tempMatrix = MDotDot - omegaTilde * MDot
MATRIX_MULTIPLY3X3(omegaTilde, MDot, tempMatrix);
for(int i=0; i<9; i++)
tempMatrix[i] = MDotDot[i] - tempMatrix[i];
double tempMatrix2[9];
// tempVector = 2 * skew(Q^T * tempMatrix)
MATRIX_MULTIPLY3X3ATB(Q, tempMatrix, tempMatrix2);
double tempVector[3];
SKEW_PART(tempMatrix2, tempVector);
VECTOR_SCALE3(tempVector, 2.0);
// tempMatrix = tr(SDot)I - SDot
for(int i=0; i<9; i++)
tempMatrix[i] = -SDot[i];
double trace = SDot[0] + SDot[4] + SDot[8];
tempMatrix[0] += trace;
tempMatrix[4] += trace;
tempMatrix[8] += trace;
// tempVector2 = (tempMatrix * Q^T) * omega
double tempVector2[3];
MATRIX_MULTIPLY3X3ABT(tempMatrix, Q, tempMatrix2);
MATRIX_VECTOR_MULTIPLY3X3(tempMatrix2, omega, tempVector2);
// tempVector -= tempVector2
VECTOR_SUBTRACTEQUAL3(tempVector, tempVector2);
// tempVector2 = GInv * tempVector
MATRIX_VECTOR_MULTIPLY3X3(GInv, tempVector, omegaDot);
if (QDotDot != NULL)
{
double tempMatrix[9];
SKEW_MATRIX(omegaDot, tempMatrix);
MATRIX_MULTIPLY3X3(omegaTilde, omegaTilde, tempMatrix2);
for(int i=0;i<9;i++)
tempMatrix[i] += tempMatrix2[i];
MATRIX_MULTIPLY3X3(tempMatrix, Q, QDotDot);
}
}
}
TestAppBlockBuilder::TestAppBlockBuilder( int& id, int WIDTH_, int HEIGHT_ ) : AppSDL2OGL_3D( id, WIDTH_, HEIGHT_ ) {
float l0_pos [] = {0.1f,0.2f,1.0f,0}; glLightfv ( GL_LIGHT0, GL_POSITION, l0_pos );
float l0_amb [] = {0.0f,0.0f,0.0f,0}; glLightfv ( GL_LIGHT0, GL_AMBIENT, l0_amb );
float l0_diff [] = {0.4f,0.5f,0.4f,0}; glLightfv ( GL_LIGHT0, GL_DIFFUSE, l0_diff );
float l0_spec [] = {0.0f,0.0f,0.0f,0}; glLightfv ( GL_LIGHT0, GL_SPECULAR, l0_spec );
glEnable ( GL_LIGHT0 );
float l1_pos [] = {+1.0f,1.0f,1.0f,0}; glLightfv ( GL_LIGHT1, GL_POSITION, l1_pos );
float l1_amb [] = {0.0f,0.0f,0.0f,0}; glLightfv ( GL_LIGHT1, GL_AMBIENT, l1_amb );
float l1_diff [] = {0.2f,0.0f,0.0f,0}; glLightfv ( GL_LIGHT1, GL_DIFFUSE, l1_diff );
float l1_spec [] = {0.0f,0.0f,0.0f,0}; glLightfv ( GL_LIGHT1, GL_SPECULAR, l1_spec );
glEnable ( GL_LIGHT1 );
float l2_pos [] = {-1.0f,0.0f,1.0f,0}; glLightfv ( GL_LIGHT2, GL_POSITION, l2_pos );
float l2_amb [] = {0.0f,0.0f,0.0f,0}; glLightfv ( GL_LIGHT2, GL_AMBIENT, l2_amb );
float l2_diff [] = {0.0f,0.0f,0.2f,0}; glLightfv ( GL_LIGHT2, GL_DIFFUSE, l2_diff );
float l2_spec [] = {0.0f,0.0f,0.0f,0}; glLightfv ( GL_LIGHT2, GL_SPECULAR, l2_spec );
glEnable ( GL_LIGHT2 );
glEnable ( GL_LIGHTING );
camPos.set( 0,0,-10 );
Mat3d rotMat; rotMat.set( {1.0d,0.0d,0.0d}, {0.0d,1.0d,0.0d}, {0.0d,0.0d,1.0d} );
BlockWorld::setupBlockWorld( {-127.0d,-127.0d,-127.0d}, {1.0d,1.0d,1.0d}, rotMat );
cursorShape = glGenLists(1);
glNewList( cursorShape , GL_COMPILE );
glPushMatrix();
glScalef( 0.5f, 0.5f, 0.5f );
glColor3f(0.01f,0.01f,0.01f); Draw3D::drawLines ( Solids::Cube_nedges, Solids::Cube_edges, Solids::Cube_verts );
//Draw3D::drawAxis( 0.5f );
glPopMatrix();
glEndList();
/*
shapes[nShapes] = glGenLists(1);
glNewList( shapes[nShapes] , GL_COMPILE );
glDisable ( GL_LIGHTING ); Draw3D::drawAxis( 0.5f );
glEndList();
nShapes++;
*/
shapes[nShapes] = glGenLists(1);
glNewList( shapes[nShapes] , GL_COMPILE );
//glPushMatrix();
//glScalef( 0.5f, 0.5f, 0.5f );
glEnable ( GL_LIGHTING );
glColor3f( 0.9f, 0.9f, 0.9f );
//Draw3D::drawConeFan ( 16, 0.5f, {0.0f,0.0f,-0.5f}, {0.0f,0.0f,0.5f} );
Draw3D::drawCone ( 4, M_PI/4.0f, 9*M_PI/4.0f, (float)sqrt(0.5f), 0.01f, {0.0f,0.0f,0.5f}, {0.0f,0.0f,0.0f}, false );
glBegin( GL_QUADS );
glNormal3f( 0.0f, 0.0f, 1.0f );
glVertex3f( 0.5f, 0.5f, 0.5f);
glVertex3f( 0.5f, -0.5f, 0.5f);
glVertex3f( -0.5f, -0.5f, 0.5f);
glVertex3f( -0.5f, 0.5f, 0.5f);
glEnd();
//Draw3D::drawCylinderStrip ( 16, 0.5f, 0.01f, {0.0f,0.0f,-0.5f}, {0.0f,0.0f,0.5f} );
//glPopMatrix();
glEndList();
nShapes++;
shapes[nShapes] = glGenLists(1);
glNewList( shapes[nShapes] , GL_COMPILE );
//glPushMatrix();
//glScalef( 0.5f, 0.5f, 0.5f );
glEnable ( GL_LIGHTING );
glColor3f( 0.9f, 0.9f, 0.9f );
//Draw3D::drawConeFan ( 16, 0.5f, {0.0f,0.0f,-0.5f}, {0.0f,0.0f,0.5f} );
Draw3D::drawCone ( 16, 0.0f, M_PI/2.0f, 0.5f, 0.1f, {0.0f,0.0f,-0.5f}, {0.0f,0.0f,0.5f}, false );
//Draw3D::drawCylinderStrip ( 16, 0.5f, 0.01f, {0.0f,0.0f,-0.5f}, {0.0f,0.0f,0.5f} );
//glPopMatrix();
glEndList();
nShapes++;
shapes[nShapes] = glGenLists(1);
glNewList( shapes[nShapes] , GL_COMPILE );
//glPushMatrix();
//glScalef( 0.5f, 0.5f, 0.5f );
glEnable ( GL_LIGHTING );
glColor3f( 0.9f, 0.9f, 0.9f );
//Draw3D::drawConeFan ( 16, 0.5f, {0.0f,0.0f,-0.5f}, {0.0f,0.0f,0.5f} );
Draw3D::drawCone ( 5, 0.0f, 2*M_PI, 0.5f, 0.1f, {0.0f,0.0f,-0.5f}, {0.0f,0.0f,0.5f}, false );
//Draw3D::drawCylinderStrip ( 16, 0.5f, 0.01f, {0.0f,0.0f,-0.5f}, {0.0f,0.0f,0.5f} );
//glPopMatrix();
glEndList();
nShapes++;
shapes[nShapes] = glGenLists(1);
glNewList( shapes[nShapes] , GL_COMPILE );
glPushMatrix();
//glScalef( 0.5f, 0.5f, 0.5f );
glTranslatef( -0.5f, 0.5f, 0.0f );
glEnable ( GL_LIGHTING );
glColor3f( 0.9f, 0.9f, 0.9f );
//Draw3D::drawConeFan ( 16, 0.5f, {0.0f,0.0f,-0.5f}, {0.0f,0.0f,0.5f} );
Draw3D::drawCone ( 3, 0.0f, M_PI/2, 1.0f, 1.0f, {0.0f,0.0f,-0.5f}, {0.0f,0.0f,0.5f}, false );
//Draw3D::drawCylinderStrip ( 16, 0.5f, 0.01f, {0.0f,0.0f,-0.5f}, {0.0f,0.0f,0.5f} );
glPopMatrix();
glEndList();
nShapes++;
shapes[nShapes] = glGenLists(1);
glNewList( shapes[nShapes] , GL_COMPILE );
//glPushMatrix();
//glScalef( 0.5f, 0.5f, 0.5f );
glEnable ( GL_LIGHTING );
glColor3f( 0.9f, 0.9f, 0.9f );
Draw3D::drawCylinderStrip ( 16, 0.5f, 0.5f, {0.0f,0.0f,-0.5f}, {0.0f,0.0f,0.5f} );
//glPopMatrix();
glEndList();
nShapes++;
shapes[nShapes] = glGenLists(1);
glNewList( shapes[nShapes] , GL_COMPILE );
glPushMatrix();
glScalef( 0.5f, 0.5f, 0.5f );
glEnable ( GL_LIGHTING );
glColor3f( 0.9f, 0.9f, 0.9f );
Draw3D::drawPolygons( Solids::Cube_nfaces, Solids::Cube_ngons, Solids::Cube_faces, Solids::Cube_verts );
glPopMatrix();
glEndList();
nShapes++;
shapes[nShapes] = glGenLists(1);
glNewList( shapes[nShapes] , GL_COMPILE );
glPushMatrix();
glScalef( 0.5f, 0.5f, 0.5f );
glEnable ( GL_LIGHTING );
glColor3f( 0.9f, 0.9f, 0.9f );
Draw3D::drawPolygons( Solids::Tetrahedron_nfaces, Solids::Tetrahedron_ngons, Solids::Tetrahedron_faces, Solids::Tetrahedron_verts );
glPopMatrix();
glEndList();
nShapes++;
shapes[nShapes] = glGenLists(1);
glNewList( shapes[nShapes] , GL_COMPILE );
glPushMatrix();
glScalef( 0.5f, 0.5f, 0.5f );
glEnable ( GL_LIGHTING );
glColor3f( 0.9f, 0.9f, 0.9f );
Draw3D::drawPolygons( Solids::Octahedron_nfaces, Solids::Octahedron_ngons, Solids::Octahedron_faces, Solids::Octahedron_verts );
glPopMatrix();
glEndList();
nShapes++;
shapes[nShapes] = glGenLists(1);
glNewList( shapes[nShapes] , GL_COMPILE );
glPushMatrix();
glScalef( 0.5f, 0.5f, 0.5f );
glEnable ( GL_LIGHTING );
glColor3f( 0.9f, 0.9f, 0.9f );
Draw3D::drawPolygons( Solids::RhombicDodecahedron_nfaces, Solids::RhombicDodecahedron_ngons, Solids::RhombicDodecahedron_faces, Solids::RhombicDodecahedron_verts );
glPopMatrix();
glEndList();
nShapes++;
shapes[nShapes] = glGenLists(1);
glNewList( shapes[nShapes] , GL_COMPILE );
glPushMatrix();
glScalef( 0.5f, 0.5f, 0.5f );
glEnable ( GL_LIGHTING );
glColor3f( 0.9f, 0.9f, 0.9f );
Draw3D::drawPolygons( Solids::Icosahedron_nfaces, Solids::Icosahedron_ngons, Solids::Icosahedron_faces, Solids::Icosahedron_verts );
glPopMatrix();
glEndList();
nShapes++;
/*
blocks[nBlocks].shape = shapes[iShape];
blocks[nBlocks].orientation = 0;
blocks[nBlocks].ix = 2;
blocks[nBlocks].iy = 3;
blocks[nBlocks].iz = 4;
nBlocks++;
*/
};
int SVD(Mat3d & F, Mat3d & U, Vec3d & Sigma, Mat3d & V, double singularValue_eps, int modifiedSVD)
{
// The code handles the following special situations:
//---------------------------------------------------------
// 1. det(V) == -1
// - multiply the first column of V by -1
//---------------------------------------------------------
// 2. An entry of Sigma is near zero
//---------------------------------------------------------
// (if modifiedSVD == 1) :
// 3. negative determinant (Tet is inverted in solid mechanics).
// - check if det(U) == -1
// - If yes, then negate the minimal element of Sigma
// and the corresponding column of U
//---------------------------------------------------------
// form F^T F and do eigendecomposition
Mat3d normalEq = trans(F) * F;
Vec3d eigenValues;
Vec3d eigenVectors[3];
eigen_sym(normalEq, eigenValues, eigenVectors);
V.set(eigenVectors[0][0], eigenVectors[1][0], eigenVectors[2][0],
eigenVectors[0][1], eigenVectors[1][1], eigenVectors[2][1],
eigenVectors[0][2], eigenVectors[1][2], eigenVectors[2][2]);
/*
printf("--- original V ---\n");
V.print();
printf("--- eigenValues ---\n");
printf("%G %G %G\n", eigenValues[0], eigenValues[1], eigenValues[2]);
*/
// Handle situation:
// 1. det(V) == -1
// - multiply the first column of V by -1
if (det(V) < 0.0)
{
// convert V into a rotation (multiply column 1 by -1)
V[0][0] *= -1.0;
V[1][0] *= -1.0;
V[2][0] *= -1.0;
}
Sigma[0] = (eigenValues[0] > 0.0) ? sqrt(eigenValues[0]) : 0.0;
Sigma[1] = (eigenValues[1] > 0.0) ? sqrt(eigenValues[1]) : 0.0;
Sigma[2] = (eigenValues[2] > 0.0) ? sqrt(eigenValues[2]) : 0.0;
//printf("--- Sigma ---\n");
//printf("%G %G %G\n", Sigma[0][0], Sigma[1][1], Sigma[2][2]);
// compute inverse of singular values
// also check if singular values are close to zero
Vec3d SigmaInverse;
SigmaInverse[0] = (Sigma[0] > singularValue_eps) ? (1.0 / Sigma[0]) : 0.0;
SigmaInverse[1] = (Sigma[1] > singularValue_eps) ? (1.0 / Sigma[1]) : 0.0;
SigmaInverse[2] = (Sigma[2] > singularValue_eps) ? (1.0 / Sigma[2]) : 0.0;
// compute U using the formula:
// U = F * V * diag(SigmaInverse)
U = F * V;
U.multiplyDiagRight(SigmaInverse);
// In theory, U is now orthonormal, U^T U = U U^T = I .. it may be a rotation or a reflection, depending on F.
// But in practice, if singular values are small or zero, it may not be orthonormal, so we need to fix it.
// Handle situation:
// 2. An entry of Sigma is near zero
// ---------------------------------------------------------
/*
printf("--- SigmaInverse ---\n");
SigmaInverse.print();
printf(" --- U ---\n");
U.print();
*/
if ((Sigma[0] < singularValue_eps) && (Sigma[1] < singularValue_eps) && (Sigma[2] < singularValue_eps))
{
// extreme case, all singular values are small, material has collapsed almost to a point
// see [Irving 04], p. 4
U.set(1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, 1.0);
}
else
{
// handle the case where two singular values are small, but the third one is not
// handle it by computing two (arbitrary) vectors orthogonal to the eigenvector for the large singular value
int done = 0;
for(int dim=0; dim<3; dim++)
{
int dimA = dim;
int dimB = (dim + 1) % 3;
int dimC = (dim + 2) % 3;
if ((Sigma[dimB] < singularValue_eps) && (Sigma[dimC] < singularValue_eps))
{
// only the column dimA can be trusted, columns dimB and dimC correspond to tiny singular values
Vec3d tmpVec1(U[0][dimA], U[1][dimA], U[2][dimA]); // column dimA
Vec3d tmpVec2;
tmpVec2 = tmpVec1.findOrthonormalVector();
Vec3d tmpVec3 = norm(cross(tmpVec1, tmpVec2));
U[0][dimB] = tmpVec2[0];
U[1][dimB] = tmpVec2[1];
U[2][dimB] = tmpVec2[2];
U[0][dimC] = tmpVec3[0];
U[1][dimC] = tmpVec3[1];
U[2][dimC] = tmpVec3[2];
if (det(U) < 0.0)
{
U[0][dimB] *= -1.0;
U[1][dimB] *= -1.0;
U[2][dimB] *= -1.0;
}
done = 1;
break; // out of for
}
}
// handle the case where one singular value is small, but the other two are not
// handle it by computing the cross product of the two eigenvectors for the two large singular values
if (!done)
{
for(int dim=0; dim<3; dim++)
{
int dimA = dim;
int dimB = (dim + 1) % 3;
int dimC = (dim + 2) % 3;
if (Sigma[dimA] < singularValue_eps)
{
// columns dimB and dimC are both good, but column dimA corresponds to a tiny singular value
Vec3d tmpVec1(U[0][dimB], U[1][dimB], U[2][dimB]); // column dimB
Vec3d tmpVec2(U[0][dimC], U[1][dimC], U[2][dimC]); // column dimC
Vec3d tmpVec3 = norm(cross(tmpVec1, tmpVec2));
U[0][dimA] = tmpVec3[0];
U[1][dimA] = tmpVec3[1];
U[2][dimA] = tmpVec3[2];
if (det(U) < 0.0)
{
U[0][dimA] *= -1.0;
U[1][dimA] *= -1.0;
U[2][dimA] *= -1.0;
}
done = 1;
break; // out of for
}
}
}
if ((!done) && (modifiedSVD == 1))
{
// Handle situation:
// 3. negative determinant (Tet is inverted in solid mechanics)
// - check if det(U) == -1
// - If yes, then negate the minimal element of Sigma
// and the corresponding column of U
double detU = det(U);
if (detU < 0.0)
{
// negative determinant
// find the smallest singular value (they are all non-negative)
int smallestSingularValueIndex = 0;
for(int dim=1; dim<3; dim++)
if (Sigma[dim] < Sigma[smallestSingularValueIndex])
smallestSingularValueIndex = dim;
// negate the smallest singular value
Sigma[smallestSingularValueIndex] *= -1.0;
U[0][smallestSingularValueIndex] *= -1.0;
U[1][smallestSingularValueIndex] *= -1.0;
U[2][smallestSingularValueIndex] *= -1.0;
}
}
}
/*
printf("U = \n");
U.print();
printf("Sigma = \n");
Sigma.print();
printf("V = \n");
V.print();
*/
return 0;
}
int ReducedStVKCubatureForceModel::ModifiedSVD(Mat3d & F, Mat3d & U, Vec3d & Fhat, Mat3d & V) const
{
// The code handles the following necessary special situations (see the code below) :
//---------------------------------------------------------
// 1. det(V) == -1
// - simply multiply a column of V by -1
//---------------------------------------------------------
// 2. An entry of Fhat is near zero
//---------------------------------------------------------
// 3. Tet is inverted.
// - check if det(U) == -1
// - If yes, then negate the minimal element of Fhat
// and the corresponding column of U
//---------------------------------------------------------
double modifiedSVD_singularValue_eps = 1e-8;
// form F^T F and do eigendecomposition
Mat3d normalEq = trans(F) * F;
Vec3d eigenValues;
Vec3d eigenVectors[3];
// note that normalEq is changed after calling eigen_sym
eigen_sym(normalEq, eigenValues, eigenVectors);
V.set(eigenVectors[0][0], eigenVectors[1][0], eigenVectors[2][0],
eigenVectors[0][1], eigenVectors[1][1], eigenVectors[2][1],
eigenVectors[0][2], eigenVectors[1][2], eigenVectors[2][2]);
/*
printf("--- original V ---\n");
V.print();
printf("--- eigenValues ---\n");
printf("%G %G %G\n", eigenValues[0], eigenValues[1], eigenValues[2]);
*/
// Handle situation:
// 1. det(V) == -1
// - simply multiply a column of V by -1
if (det(V) < 0.0)
{
// convert V into a rotation (multiply column 1 by -1)
V[0][0] *= -1.0;
V[1][0] *= -1.0;
V[2][0] *= -1.0;
}
Fhat[0] = (eigenValues[0] > 0.0) ? sqrt(eigenValues[0]) : 0.0;
Fhat[1] = (eigenValues[1] > 0.0) ? sqrt(eigenValues[1]) : 0.0;
Fhat[2] = (eigenValues[2] > 0.0) ? sqrt(eigenValues[2]) : 0.0;
//printf("--- Fhat ---\n");
//printf("%G %G %G\n", Fhat[0][0], Fhat[1][1], Fhat[2][2]);
// compute inverse of singular values
// also check if singular values are close to zero
Vec3d FhatInverse;
FhatInverse[0] = (Fhat[0] > modifiedSVD_singularValue_eps) ? (1.0 / Fhat[0]) : 0.0;
FhatInverse[1] = (Fhat[1] > modifiedSVD_singularValue_eps) ? (1.0 / Fhat[1]) : 0.0;
FhatInverse[2] = (Fhat[2] > modifiedSVD_singularValue_eps) ? (1.0 / Fhat[2]) : 0.0;
// compute U using the formula:
// U = F * V * diag(FhatInverse)
U = F * V;
U.multiplyDiagRight(FhatInverse);
// In theory, U is now orthonormal, U^T U = U U^T = I .. it may be a rotation or a reflection, depending on F.
// But in practice, if singular values are small or zero, it may not be orthonormal, so we need to fix it.
// Handle situation:
// 2. An entry of Fhat is near zero
// ---------------------------------------------------------
/*
printf("--- FhatInverse ---\n");
FhatInverse.print();
printf(" --- U ---\n");
U.print();
*/
if ((Fhat[0] < modifiedSVD_singularValue_eps) && (Fhat[1] < modifiedSVD_singularValue_eps) && (Fhat[2] < modifiedSVD_singularValue_eps))
{
// extreme case, material has collapsed almost to a point
// see [Irving 04], p. 4
U.set(1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, 1.0);
}
else
{
int done = 0;
for(int dim=0; dim<3; dim++)
{
int dimA = dim;
int dimB = (dim + 1) % 3;
int dimC = (dim + 2) % 3;
if ((Fhat[dimB] < modifiedSVD_singularValue_eps) && (Fhat[dimC] < modifiedSVD_singularValue_eps))
{
// only the column dimA can be trusted, columns dimB and dimC correspond to tiny singular values
Vec3d tmpVec1(U[0][dimA], U[1][dimA], U[2][dimA]); // column dimA
Vec3d tmpVec2;
FindOrthonormalVector(tmpVec1, tmpVec2);
Vec3d tmpVec3 = norm(cross(tmpVec1, tmpVec2));
U[0][dimB] = tmpVec2[0];
U[1][dimB] = tmpVec2[1];
U[2][dimB] = tmpVec2[2];
U[0][dimC] = tmpVec3[0];
U[1][dimC] = tmpVec3[1];
U[2][dimC] = tmpVec3[2];
if (det(U) < 0.0)
{
U[0][dimB] *= -1.0;
U[1][dimB] *= -1.0;
U[2][dimB] *= -1.0;
}
done = 1;
break; // out of for
}
}
if (!done)
{
for(int dim=0; dim<3; dim++)
{
int dimA = dim;
int dimB = (dim + 1) % 3;
int dimC = (dim + 2) % 3;
if (Fhat[dimA] < modifiedSVD_singularValue_eps)
{
// columns dimB and dimC are both good, but column dimA corresponds to a tiny singular value
Vec3d tmpVec1(U[0][dimB], U[1][dimB], U[2][dimB]); // column dimB
Vec3d tmpVec2(U[0][dimC], U[1][dimC], U[2][dimC]); // column dimC
Vec3d tmpVec3 = norm(cross(tmpVec1, tmpVec2));
U[0][dimA] = tmpVec3[0];
U[1][dimA] = tmpVec3[1];
U[2][dimA] = tmpVec3[2];
if (det(U) < 0.0)
{
U[0][dimA] *= -1.0;
U[1][dimA] *= -1.0;
U[2][dimA] *= -1.0;
}
done = 1;
break; // out of for
}
}
}
if (!done)
{
// Handle situation:
// 3. Tet is inverted.
// - check if det(U) == -1
// - If yes, then negate the minimal element of Fhat
// and the corresponding column of U
double detU = det(U);
if (detU < 0.0)
{
// tet is inverted
// find smallest singular value (they are all non-negative)
int smallestSingularValueIndex = 0;
for(int dim=1; dim<3; dim++)
if (Fhat[dim] < Fhat[smallestSingularValueIndex])
smallestSingularValueIndex = dim;
// negate smallest singular value
Fhat[smallestSingularValueIndex] *= -1.0;
U[0][smallestSingularValueIndex] *= -1.0;
U[1][smallestSingularValueIndex] *= -1.0;
U[2][smallestSingularValueIndex] *= -1.0;
}
}
}
/*
printf("U = \n");
U.print();
printf("Fhat = \n");
Fhat.print();
printf("V = \n");
V.print();
*/
return 0;
}
Tanks_single::Tanks_single( int& id, int WIDTH_, int HEIGHT_ ) : AppSDL2OGL_3D( id, WIDTH_, HEIGHT_ ) {
world.init_world();
printf( "DEBUG_SHIT : %i \n", world.debug_shit );
// ---- terrain
//new Terrain25D();
world.terrain = prepareTerrain();
// ---- Objects
int sphereShape = glGenLists(1);
glNewList( sphereShape , GL_COMPILE );
//glPushMatrix();
//glDisable ( GL_LIGHTING );
//Draw3D::drawAxis ( 3.0f );
//glColor3f( 1.0f, 0.0f, 1.0f ); Draw3D::drawLines ( Solids::Icosahedron_nedges, Solids::Icosahedron_edges, Solids::Icosahedron_verts );
//glEnable( GL_LIGHTING );
//glColor3f( 0.8f, 0.8f, 0.8f ); Draw3D::drawPolygons( Solids::Icosahedron_nfaces, Solids::Icosahedron_ngons, Solids::Icosahedron_faces, Solids::Icosahedron_verts );
glEnable( GL_LIGHTING ); glColor3f( 0.8f, 0.8f, 0.8f ); Draw3D::drawSphere_oct( 6, 1.0, {0.0,0.0,0.0} );
//glPopMatrix();
glEndList();
int nobjects=10;
double xrange = 10.0;
for( int i=0; i<nobjects; i++){
Object3D * o = new Object3D();
//o->bounds.orientation.set({});
//o->bounds.span.set(o->bounds.orientation.a.normalize(),o->bounds.orientation.a.normalize(),o->bounds.orientation.a.normalize());
o->bounds.orientation.fromRand( {randf(0,1),randf(0,1),randf(0,1)} );
o->bounds.span.set( randf(0.2,2.0), randf(0.2,2.0), randf(0.2,2.0) );
//o->bounds.span.set( 1, 2.0, 0.5 );
Mat3d m; m.set_mmul_NT( o->bounds.orientation, o->bounds.orientation );
/*
printf( " === %i \n", i );
printf( " %f %f %f \n", m.ax, m.ay, m.az );
printf( " %f %f %f \n", m.bx, m.by, m.bz );
printf( " %f %f %f \n", m.cx, m.cy, m.cz );
*/
//o->bounds.pos.set( randf(-xrange,xrange),randf(-xrange,xrange),randf(-xrange,xrange) );
Vec2d p,dv; p.set( randf(-xrange,xrange),randf(-xrange,xrange) );
double v = world.terrain->eval( p, dv );
o->bounds.pos.set( p.x, v, p.y );
//o->bounds.pos.set( {0.0,0.0,0.0} );
o->shape= sphereShape;
o->id = i;
world.objects.push_back(o);
}
//camPos.set();
warrior1 = world.makeWarrior( {0.0d,0.0d,0.0d}, {0.0d,0.0d,1.0d}, {0.0d,1.0d,0.0d}, 0 );
warrior1->pos.set( 0.0, 0.0, -15.0 );
zoom = 5.0;
first_person = true;
perspective = true;
}
