source/SoPhysicsTriMesh.cpp

Go to the documentation of this file.

/*
 * Copyright (c) 2007, 2008, 2009, University of Karlsruhe (for members see AUTHORS file)
 *
 * ISC License
 *
 * Permission to use, copy, modify, and/or distribute this software for any
 * purpose with or without fee is hereby granted, provided that the above
 * copyright notice and this permission notice appear in all copies.
 *
 * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
 * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
 * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
 * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
 * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
 * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
 * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
 */

#include "../include/IPSA/SoPhysicsTriMesh.h"
#include "../include/IPSA/SoCollisionShapeTriMesh.h"
#include <Inventor/nodes/SoRotation.h>

SO_KIT_SOURCE(SoPhysicsTriMesh);

void SoPhysicsTriMesh::initClass()
{
                SO_KIT_INIT_CLASS(SoPhysicsTriMesh, SoPhysics, "Physics");
}


SoPhysicsTriMesh::SoPhysicsTriMesh(const SbString& nodeName /*=""*/)
: SoPhysics(nodeName),
matrixbuf0(false),
dblbfilled(false)
{
                SO_KIT_CONSTRUCTOR(SoPhysicsTriMesh);
                isBuiltIn = TRUE;

                SO_KIT_INIT_INSTANCE();

                //updateDependentObjectsTrigger.appendConnection(&);
}


SoBaseKit* SoPhysicsTriMesh::copy(SbBool copyConnections) const
{
                // Use the standard version of the copy method to create a
                // copy of this instance, including its field data
                SoPhysicsTriMesh *newSoPhysicsTriMesh = (SoPhysicsTriMesh *) SoPhysics::copy(copyConnections);

                for(unsigned int i = 0; i < sizeof(matrix_dblbuff); i++)
                {
                                newSoPhysicsTriMesh->matrix_dblbuff[i] = matrix_dblbuff[i];
                }
                newSoPhysicsTriMesh->matrixbuf0 = matrixbuf0;
                newSoPhysicsTriMesh->dblbfilled = dblbfilled;

                return newSoPhysicsTriMesh;
}


void SoPhysicsTriMesh::updateOIPosition()
{
                SoPhysics::updateOIPosition();
                // This should return the same values as the call in
                // SoPhysics::updateOIPosition() in the line above
                const dReal* TRANSLATION = dBodyGetPosition (body);
                const dReal* ROTATION = dBodyGetQuaternion (body);

                // tell the tri-tri collider the current transform of the trimesh --
                // this is fairly important for good results:
                dReal* p_matrix = matrix_dblbuff + ((matrixbuf0==false) ? 0 : 16);

                p_matrix[ 0 ] = ROTATION[ 0 ];  p_matrix[ 1 ] = ROTATION[ 1 ];  p_matrix[ 2 ] = ROTATION[ 2 ];  p_matrix[ 3 ] = 0;
                p_matrix[ 4 ] = ROTATION[ 4 ];  p_matrix[ 5 ] = ROTATION[ 5 ];  p_matrix[ 6 ] = ROTATION[ 6 ];  p_matrix[ 7 ] = 0;
                p_matrix[ 8 ] = ROTATION[ 8 ];  p_matrix[ 9 ] = ROTATION[ 9 ];  p_matrix[10 ] = ROTATION[10 ];  p_matrix[11 ] = 0;
                p_matrix[12 ] = TRANSLATION[ 0 ];               p_matrix[13 ] = TRANSLATION[ 1 ];               p_matrix[14 ] = TRANSLATION[ 2 ];               p_matrix[15 ] = 1;

                //Flip to other matrix.
                matrixbuf0  = !matrixbuf0;
                assert( geom != NULL );         // need to know the geom for the Tri-Tri-Collider!

                if (dblbfilled)         // after filled for the first time
                                dGeomTriMeshSetLastTransform( geom, *(dMatrix4*)( matrix_dblbuff + ((matrixbuf0==false) ? 0 : 16) ) );

                dblbfilled = true;
}


dMass SoPhysicsTriMesh::getInitialMass()
{
                assert( geom != NULL );

                // calculate mass and inertia from geom:
                dMass triMeshMass;
                dMassSetTrimeshTotal( &triMeshMass, this->mass.getValue(), geom );
                printf("getInitialMass(Tri): COG before shift is [%f,%f,%f], mass %2.2f\n",triMeshMass.c[0],triMeshMass.c[1],triMeshMass.c[2],triMeshMass.mass);

                // now shift mass and geom to coincide with center of mass:
                dGeomSetPosition(geom, triMeshMass.c[0], triMeshMass.c[1], triMeshMass.c[2]);
                dMassTranslate(&triMeshMass, -triMeshMass.c[0], -triMeshMass.c[1], -triMeshMass.c[2]);

                assert( dMassCheck(&triMeshMass) ); // if mass<0 there is an error in the trimesh vertex description (normal orientation, count direction..)

                return triMeshMass;
}

---