/**
* set up Smr IK tentacke according to Horde tentacle morphology
*/
SMRKinematicChain * setUpKinematikChain(H3DNode _firstNode)
{
float tx,ty,tz,rx,ry,rz,foo;
// currentJoint and previous joint referencing for relevant parenting when creating tentacles joints
SMRKinematicJoint *currentJoint, *prevJoint;
currentJoint = NULL;
prevJoint = NULL;
//prevJoint = NULL;
// instanciate an empty kinematic chain
SMRKinematicChain * tentacle = new SMRKinematicChain(RELATIVEMODE,TRANSLATIONFIRST,"tentacle");
// get Hordes tentacle first joint
_firstNode = h3dGetNodeChild(_firstNode,0);
// again
//_firstNode = h3dGetNodeChild(_firstNode,0);
// will help computing Smr joints gain
int i = 0;
// while Horde tentacles joints are found
while (_firstNode >0)
{
//instanciate a new SMRKinematicJoint
currentJoint = (new SMRKinematicJoint());
//get joint translation parameters in parents local frame (bone length)
h3dGetNodeTransform(_firstNode,&tx,&ty,&tz,&rx,&ry,&rz,&foo,&foo,&foo);
//add DOF along X axis
currentJoint->addDOF(SMRDOF::XAXIS,-M_PI/16.0f,M_PI/16.0f,degToRad(rx));
//add DOF along Y axis
currentJoint->addDOF(SMRDOF::YAXIS,-M_PI/3.0f,M_PI/3.0f,degToRad(ry));
//add DOF along Z axis
currentJoint->addDOF(SMRDOF::ZAXIS,-M_PI/3.0f,M_PI/3.0f,degToRad(rz));
//set up joint gain (the more distal, the more gain)
currentJoint->getDOF(0)->setGain(0.015*i);
currentJoint->getDOF(1)->setGain(0.015*i);
currentJoint->getDOF(2)->setGain(0.015*i);
//give a name to the joint (same as Horde)
currentJoint->setName(h3dGetNodeParamStr(_firstNode, H3DNodeParams::NameStr));
//set up joint translation parameters in local frame (bone length)
currentJoint->setPosition(tx,ty,tz);
// take care of parenting issues
if (prevJoint)
{
currentJoint->setParentName(prevJoint->getName());
}
// and add the brand new joint into the inverse kinematics chain !
tentacle->insertJoint(currentJoint);
// keep a reference of the newly created joint for the next turn (parenting issue)
prevJoint = currentJoint;
// get the next joint according to Horde structure
_firstNode = h3dGetNodeChild(_firstNode,0);
// increase the gain increment
i++;
}
tentacle->setStartJointIndex(tentacle->getJoint(0)->getName());
// force the root joint not to move (tentacle basis should be fix)
tentacle->getJoint(0)->getDOF(0)->setUpperBound(0);
tentacle->getJoint(0)->getDOF(0)->setLowerBound(0);
tentacle->getJoint(0)->getDOF(1)->setUpperBound(0);
tentacle->getJoint(0)->getDOF(1)->setLowerBound(0);
tentacle->getJoint(0)->getDOF(2)->setUpperBound(0);
tentacle->getJoint(0)->getDOF(2)->setLowerBound(0);
// and add the fly constraint (relative to tentacles' tip) to the tentacle
_ikConstraint = new SMRIKConstraint(SMRVector3(0,0,0),SMRVector3(0,0,0),currentJoint->getName());
// tentacle is instanciated and set up !
return tentacle;
}
Joint* Joint::getChild(int i)
{
H3DNode c = h3dGetNodeChild(m_horde_id, i);
return (c==0) ? 0 : getInstance(c);
}
btCollisionShape* createCollisionShape()
{
if (mesh->getModelNode() == 0) mesh->registerStart();
m_btTriangleMesh = new btTriangleMesh();
H3DNode meshNode = h3dGetNodeChild(mesh->getModelNode(), 0); //TODO handle multi-mesh stuff
if (meshNode == 0) throw Exception("Couldn't get mesh node.\n");
H3DRes geoRes = h3dGetNodeParamI(mesh->getModelNode(), H3DModel::GeoResI);
unsigned int numVertices = h3dGetNodeParamI(meshNode, H3DMesh::VertREndI) - h3dGetNodeParamI(meshNode, H3DMesh::VertRStartI) + 1;//get mesh vertices count
unsigned int numTriangleIndices = h3dGetNodeParamI(meshNode, H3DMesh::BatchCountI);//get mesh triangle indices count
unsigned int vertRStart = h3dGetNodeParamI(meshNode, H3DMesh::VertRStartI);//get the first vertex of the mesh
int off = 3;
std::cout << numVertices << " vertices.\n";
std::cout << numTriangleIndices << " indices.\n";
int vertexOffset = vertRStart * 3;
int indexOffset = h3dGetNodeParamI(meshNode, H3DMesh::BatchStartI);
float* vertexBase = (float*) h3dMapResStream(geoRes, H3DGeoRes::GeometryElem, 0, H3DGeoRes::GeoVertPosStream, true, false);//gets pointer to the vertex data
h3dUnmapResStream(geoRes);//closes vertex stream
if(vertexBase) vertexBase += vertexOffset;
unsigned int* TriangleBase32 = nullptr;
unsigned short* TriangleBase16 = nullptr;
if (h3dGetResParamI(geoRes, H3DGeoRes::GeometryElem, 0, H3DGeoRes::GeoIndices16I))
{
unsigned short* tb = (unsigned short*)h3dMapResStream(geoRes, H3DGeoRes::GeometryElem, 0, H3DGeoRes::GeoIndexStream, true, false);
TriangleBase16 = new unsigned short[ numTriangleIndices ];
memcpy(TriangleBase16, tb+indexOffset, sizeof(unsigned short)*numTriangleIndices);
h3dUnmapResStream(geoRes);
delete TriangleBase32;
}
else
{
unsigned int* tb = (unsigned int*)h3dMapResStream(geoRes, H3DGeoRes::GeometryElem, 0, H3DGeoRes::GeoIndexStream, true, false);
TriangleBase32 = new unsigned int[numTriangleIndices];
memcpy(TriangleBase32, tb+indexOffset, sizeof(unsigned int)*numTriangleIndices);
h3dUnmapResStream(geoRes);
delete TriangleBase16;
}
unsigned int indices[numTriangleIndices];
bool index16 = false;
if (TriangleBase16)
index16 = true;
for (unsigned int i = 0; i < numTriangleIndices - 2; i += 3)
{
unsigned int index1 = index16 ? (TriangleBase16[i] - vertRStart) * 3 : (TriangleBase32[i] - vertRStart) * 3;
unsigned int index2 = index16 ? (TriangleBase16[i+1] - vertRStart) * 3 : (TriangleBase32[i+1] - vertRStart) * 3;
unsigned int index3 = index16 ? (TriangleBase16[i+2] - vertRStart) * 3 : (TriangleBase32[i+2] - vertRStart) * 3;
m_btTriangleMesh->addTriangle(
btVector3(vertexBase[index1], vertexBase[index1+1], vertexBase[index1+2] ),
btVector3(vertexBase[index2], vertexBase[index2+1], vertexBase[index2+2] ),
btVector3(vertexBase[index3], vertexBase[index3+1], vertexBase[index3+2]),
false
);
}
return new btBvhTriangleMeshShape(m_btTriangleMesh, true, true);
}