void MeshExtractorImpl::visitShape(SgShape* shape)
{
SgMesh* mesh = shape->mesh();
if(mesh && mesh->vertices() && !mesh->vertices()->empty() && !mesh->triangleVertices().empty()){
meshFound = true;
currentMesh = mesh;
callback();
currentMesh = 0;
}
}
SgMesh* MeshGenerator::generateSphere(double radius)
{
if(radius < 0.0 || divisionNumber_ < 4){
return 0;
}
SgMesh* mesh = new SgMesh();
const int vdn = divisionNumber_ / 2; // latitudinal division number
const int hdn = divisionNumber_; // longitudinal division number
SgVertexArray& vertices = *mesh->setVertices(new SgVertexArray());
vertices.reserve((vdn - 1) * hdn + 2);
for(int i=1; i < vdn; i++){ // latitudinal direction
const double tv = i * PI / vdn;
for(int j=0; j < hdn; j++){ // longitudinal direction
const double th = j * 2.0 * PI / hdn;
vertices.push_back(Vector3f(radius * sin(tv) * cos(th), radius * cos(tv), radius * sin(tv) * sin(th)));
}
}
const int topIndex = vertices.size();
vertices.push_back(Vector3f(0.0f, radius, 0.0f));
const int bottomIndex = vertices.size();
vertices.push_back(Vector3f(0.0f, -radius, 0.0f));
mesh->reserveNumTriangles(vdn * hdn * 2);
// top faces
for(int i=0; i < hdn; ++i){
mesh->addTriangle(topIndex, (i+1) % hdn, i);
}
// side faces
for(int i=0; i < vdn - 2; ++i){
const int upper = i * hdn;
const int lower = (i + 1) * hdn;
for(int j=0; j < hdn; ++j) {
// upward convex triangle
mesh->addTriangle(j + upper, ((j + 1) % hdn) + lower, j + lower);
// downward convex triangle
mesh->addTriangle(j + upper, ((j + 1) % hdn) + upper, ((j + 1) % hdn) + lower);
}
}
// bottom faces
const int offset = (vdn - 2) * hdn;
for(int i=0; i < hdn; ++i){
mesh->addTriangle(bottomIndex, (i % hdn) + offset, ((i+1) % hdn) + offset);
}
mesh->setPrimitive(SgMesh::Sphere(radius));
mesh->updateBoundingBox();
//! \todo set normals directly without using the following function
generateNormals(mesh, PI);
return mesh;
}
SgMesh* MeshGenerator::generateCylinder(double radius, double height, bool bottom, bool top, bool side)
{
if(height < 0.0 || radius < 0.0){
return 0;
}
SgMesh* mesh = new SgMesh();
SgVertexArray& vertices = *mesh->setVertices(new SgVertexArray());
vertices.resize(divisionNumber_ * 2);
const double y = height / 2.0;
for(int i=0 ; i < divisionNumber_ ; i++ ){
const double angle = i * 2.0 * PI / divisionNumber_;
Vector3f& vtop = vertices[i];
Vector3f& vbottom = vertices[i + divisionNumber_];
vtop[0] = vbottom[0] = radius * cos(angle);
vtop[2] = vbottom[2] = radius * sin(angle);
vtop[1] = y;
vbottom[1] = -y;
}
const int topCenterIndex = vertices.size();
vertices.push_back(Vector3f(0.0f, y, 0.0f));
const int bottomCenterIndex = vertices.size();
vertices.push_back(Vector3f(0.0f, -y, 0.0f));
mesh->reserveNumTriangles(divisionNumber_ * 4);
for(int i=0; i < divisionNumber_; ++i){
// top face
if(top){
mesh->addTriangle(topCenterIndex, (i+1) % divisionNumber_, i);
}
// side face (upward convex triangle)
if(side){
mesh->addTriangle(i, ((i+1) % divisionNumber_) + divisionNumber_, i + divisionNumber_);
// side face (downward convex triangle)
mesh->addTriangle(i, (i+1) % divisionNumber_, ((i + 1) % divisionNumber_) + divisionNumber_);
}
// bottom face
if(bottom){
mesh->addTriangle(bottomCenterIndex, i + divisionNumber_, ((i+1) % divisionNumber_) + divisionNumber_);
}
}
mesh->setPrimitive(SgMesh::Cylinder(radius, height));
mesh->updateBoundingBox();
generateNormals(mesh, PI / 2.0);
return mesh;
}
void ColladaBodyLoaderImpl::createColdetModel(SgGroup* parentGroup, SgPosTransform* transParent, ColdetModel* model)
{
if (!parentGroup || parentGroup->empty()) {
return;
}
// Get the coordinates and transforms to create a ColdetModel.
if (SgPosTransform* transform = dynamic_cast<SgPosTransform*>(parentGroup)) {
for (SgGroup::iterator iter = transform->begin(); iter != transform->end(); iter++) {
if (SgShape* child = dynamic_cast<SgShape*>((*iter).get())) {
SgMesh* mesh = child->mesh();
const int vertexIndex = model->getNumVertices();
const SgVertexArray* vertices = mesh->vertices();
const Affine3& t = static_cast<Affine3>(transform->T());
for (unsigned int i = 0; i < vertices->size(); i++) {
const Vector3 v = t * vertices->at(i).cast<Position::Scalar>();
model->addVertex(v.x(), v.y(), v.z());
}
for (int i = 0; i < mesh->numTriangles(); i++) {
int v1 = vertexIndex + mesh->triangle(i)[0];
int v2 = vertexIndex + mesh->triangle(i)[1];
int v3 = vertexIndex + mesh->triangle(i)[2];
model->addTriangle(v1, v2, v3);
}
} else
if (SgGroup* child = dynamic_cast<SgGroup*>((*iter).get())) {
// It will follow the SceneGraph recursive.
createColdetModel(child, transform, model);
}
}
} else
if (SgGroup* group = dynamic_cast<SgGroup*>(parentGroup)){
// It will follow the SceneGraph recursive.
for (SgGroup::iterator iter = group->begin(); iter != group->end(); iter++) {
SgGroup* child = static_cast<SgGroup*>((*iter).get());
createColdetModel(child, transParent, model);
}
}
}
void AISTCollisionDetectorImpl::addMesh(ColdetModelEx* model)
{
SgMesh* mesh = meshExtractor->currentMesh();
const Affine3& T = meshExtractor->currentTransform();
const int vertexIndexTop = model->getNumVertices();
const SgVertexArray& vertices = *mesh->vertices();
const int numVertices = vertices.size();
for(int i=0; i < numVertices; ++i){
const Vector3 v = T * vertices[i].cast<Affine3::Scalar>();
model->addVertex(v.x(), v.y(), v.z());
}
const int numTriangles = mesh->numTriangles();
for(int i=0; i < numTriangles; ++i){
SgMesh::TriangleRef tri = mesh->triangle(i);
const int v0 = vertexIndexTop + tri[0];
const int v1 = vertexIndexTop + tri[1];
const int v2 = vertexIndexTop + tri[2];
model->addTriangle(v0, v1, v2);
}
}
static void integrateMesh(MeshExtractor* extractor, SgMesh* mesh)
{
SgMesh* srcMesh = extractor->currentMesh();
const Affine3f T = extractor->currentTransform().cast<Affine3f::Scalar>();
if(srcMesh->hasVertices()){
SgVertexArray& vertices = *mesh->getOrCreateVertices();
const int numVertices = vertices.size();
SgVertexArray& srcVertices = *srcMesh->vertices();
const int numSrcVertices = srcVertices.size();
vertices.reserve(numVertices + numSrcVertices);
for(int i=0; i < numSrcVertices; ++i){
vertices.push_back(T * srcVertices[i]);
}
SgIndexArray& indices = mesh->triangleVertices();
const int numIndices = indices.size();
SgIndexArray& srcIndices = srcMesh->triangleVertices();
const int numSrcIndices = srcIndices.size();
indices.reserve(numIndices + numSrcIndices);
for(int i=0; i < numSrcIndices; ++i){
indices.push_back(srcIndices[i] + numVertices);
}
if(srcMesh->hasNormals()){
SgNormalArray& normals = *mesh->getOrCreateNormals();
const int numNormals = normals.size();
SgNormalArray& srcNormals = *srcMesh->normals();
const int numSrcNormals = srcNormals.size();
normals.reserve(numNormals + numSrcNormals);
const Affine3f U = extractor->currentTransformWithoutScaling().cast<Affine3f::Scalar>();
for(int i=0; i < numSrcNormals; ++i){
normals.push_back(U * srcNormals[i]);
}
SgIndexArray& indices = mesh->normalIndices();
const int numIndices = indices.size();
SgIndexArray& srcIndices = srcMesh->normalIndices();
const int numSrcIndices = srcIndices.size();
indices.reserve(numIndices + numSrcIndices);
for(int i=0; i < numSrcIndices; ++i){
indices.push_back(srcIndices[i] + numNormals);
}
}
}
}
SgMesh* MeshGenerator::generateDisc(double radius, double innerRadius)
{
if(innerRadius <= 0.0 || radius <= innerRadius){
return 0;
}
SgMesh* mesh = new SgMesh();
SgVertexArray& vertices = *mesh->getOrCreateVertices();
vertices.reserve(divisionNumber_ * 2);
for(int i=0; i < divisionNumber_; ++i){
const double angle = i * 2.0 * PI / divisionNumber_;
const double x = cos(angle);
const double y = sin(angle);
vertices.push_back(Vector3f(innerRadius * x, innerRadius * y, 0.0f));
vertices.push_back(Vector3f(radius * x, radius * y, 0.0f));
}
mesh->reserveNumTriangles(divisionNumber_ * 2);
mesh->getOrCreateNormals()->push_back(Vector3f::UnitZ());
SgIndexArray& normalIndices = mesh->normalIndices();
normalIndices.reserve(divisionNumber_ * 2 * 3);
for(int i=0; i < divisionNumber_; ++i){
const int j = (i + 1) % divisionNumber_;
const int current = i * 2;
const int next = j * 2;
mesh->addTriangle(current, current + 1, next + 1);
mesh->addTriangle(current, next + 1, next);
for(int j=0; j < 6; ++j){
normalIndices.push_back(0);
}
}
mesh->updateBoundingBox();
return mesh;
}
SgMesh* MeshGenerator::generateCone(double radius, double height, bool bottom, bool side)
{
if(radius < 0.0 || height < 0.0){
return 0;
}
SgMesh* mesh = new SgMesh();
SgVertexArray& vertices = *mesh->setVertices(new SgVertexArray());
vertices.reserve(divisionNumber_ + 2);
for(int i=0; i < divisionNumber_; ++i){
const double angle = i * 2.0 * PI / divisionNumber_;
vertices.push_back(Vector3f(radius * cos(angle), -height / 2.0, radius * sin(angle)));
}
const int topIndex = vertices.size();
vertices.push_back(Vector3f(0.0f, height / 2.0, 0.0f));
const int bottomCenterIndex = vertices.size();
vertices.push_back(Vector3f(0.0f, -height / 2.0, 0.0f));
mesh->reserveNumTriangles(divisionNumber_ * 2);
for(int i=0; i < divisionNumber_; ++i){
// side faces
if(side){
mesh->addTriangle(topIndex, (i + 1) % divisionNumber_, i);
}
// bottom faces
if(bottom){
mesh->addTriangle(bottomCenterIndex, i, (i + 1) % divisionNumber_);
}
}
mesh->setPrimitive(SgMesh::Cone(radius, height));
mesh->updateBoundingBox();
generateNormals(mesh, PI / 2.0);
return mesh;
}
SgMesh* MeshGenerator::generateTorus(double radius, double crossSectionRadius)
{
int divisionNumber2 = divisionNumber_ / 4;
SgMesh* mesh = new SgMesh();
SgVertexArray& vertices = *mesh->getOrCreateVertices();
vertices.reserve(divisionNumber_ * divisionNumber2);
for(int i=0; i < divisionNumber_; ++i){
double phi = i * 2.0 * PI / divisionNumber_;
for(int j=0; j < divisionNumber2; ++j){
double theta = j * 2.0 * PI / divisionNumber2;
Vector3f v;
double r = crossSectionRadius * cos(theta) + radius;
v.x() = cos(phi) * r;
v.y() = crossSectionRadius * sin(theta);
v.z() = sin(phi) * r;
vertices.push_back(v);
}
}
mesh->reserveNumTriangles(2 * divisionNumber_ * divisionNumber2);
for(int i=0; i < divisionNumber_; ++i){
int current = i * divisionNumber2;
int next = ((i + 1) % divisionNumber_) * divisionNumber2;
for(int j=0; j < divisionNumber2; ++j){
int j_next = (j + 1) % divisionNumber2;
mesh->addTriangle(current + j, next + j_next, next + j);
mesh->addTriangle(current + j, current + j_next, next + j_next);
}
}
mesh->updateBoundingBox();
generateNormals(mesh, PI);
return mesh;
}
void ODECollisionDetectorImpl::addMesh(GeometryEx* model)
{
SgMesh* mesh = meshExtractor->currentMesh();
const Affine3& T = meshExtractor->currentTransform();
bool meshAdded = false;
if(mesh->primitiveType() != SgMesh::MESH){
bool doAddPrimitive = false;
Vector3 scale;
optional<Vector3> translation;
if(!meshExtractor->isCurrentScaled()){
scale.setOnes();
doAddPrimitive = true;
} else {
Affine3 S = meshExtractor->currentTransformWithoutScaling().inverse() *
meshExtractor->currentTransform();
if(S.linear().isDiagonal()){
if(!S.translation().isZero()){
translation = S.translation();
}
scale = S.linear().diagonal();
if(mesh->primitiveType() == SgMesh::BOX){
doAddPrimitive = true;
} else if(mesh->primitiveType() == SgMesh::SPHERE){
// check if the sphere is uniformly scaled for all the axes
if(scale.x() == scale.y() && scale.x() == scale.z()){
doAddPrimitive = true;
}
} else if(mesh->primitiveType() == SgMesh::CYLINDER){
// check if the bottom circle face is uniformly scaled
if(scale.x() == scale.z()){
doAddPrimitive = true;
}
}
}
}
if(doAddPrimitive){
bool created = false;
dGeomID geomId;
switch(mesh->primitiveType()){
case SgMesh::BOX : {
const Vector3& s = mesh->primitive<SgMesh::Box>().size;
geomId = dCreateBox(model->spaceID, s.x() * scale.x(), s.y() * scale.y(), s.z() * scale.z());
created = true;
break; }
case SgMesh::SPHERE : {
SgMesh::Sphere sphere = mesh->primitive<SgMesh::Sphere>();
geomId = dCreateSphere(model->spaceID, sphere.radius * scale.x());
created = true;
break; }
case SgMesh::CYLINDER : {
SgMesh::Cylinder cylinder = mesh->primitive<SgMesh::Cylinder>();
geomId = dCreateCylinder(model->spaceID, cylinder.radius * scale.x(), cylinder.height * scale.y());
created = true;
break; }
default :
break;
}
if(created){
model->primitiveGeomID.push_back(geomId);
if(translation){
offsetMap.insert(OffsetMap::value_type(geomId,
meshExtractor->currentTransformWithoutScaling() *
Translation3(*translation)));
} else {
offsetMap.insert(OffsetMap::value_type(geomId, meshExtractor->currentTransformWithoutScaling()));
}
meshAdded = true;
}
}
}
if(!meshAdded){
const int vertexIndexTop = model->vertices.size();
const SgVertexArray& vertices_ = *mesh->vertices();
const int numVertices = vertices_.size();
for(int i=0; i < numVertices; ++i){
const Vector3 v = T * vertices_[i].cast<Position::Scalar>();
model->vertices.push_back(Vertex(v.x(), v.y(), v.z()));
}
const int numTriangles = mesh->numTriangles();
for(int i=0; i < numTriangles; ++i){
SgMesh::TriangleRef src = mesh->triangle(i);
Triangle tri;
tri.indices[0] = vertexIndexTop + src[0];
tri.indices[1] = vertexIndexTop + src[1];
tri.indices[2] = vertexIndexTop + src[2];
model->triangles.push_back(tri);
}
}
}
SgMesh* MeshGenerator::generateBox(Vector3 size)
{
if(size.x() < 0.0 || size.y() < 0.0 || size.z() < 0.0){
return 0;
}
const float x = size.x() * 0.5;
const float y = size.y() * 0.5;
const float z = size.z() * 0.5;
SgMesh* mesh = new SgMesh;
SgVertexArray& vertices = *mesh->setVertices(new SgVertexArray());
vertices.reserve(8);
vertices.push_back(Vector3f( x, y, z));
vertices.push_back(Vector3f(-x, y, z));
vertices.push_back(Vector3f(-x,-y, z));
vertices.push_back(Vector3f( x,-y, z));
vertices.push_back(Vector3f( x, y,-z));
vertices.push_back(Vector3f(-x, y,-z));
vertices.push_back(Vector3f(-x,-y,-z));
vertices.push_back(Vector3f( x,-y,-z));
mesh->reserveNumTriangles(12);
mesh->addTriangle(0,1,2);
mesh->addTriangle(2,3,0);
mesh->addTriangle(0,5,1);
mesh->addTriangle(0,4,5);
mesh->addTriangle(1,5,6);
mesh->addTriangle(1,6,2);
mesh->addTriangle(2,6,7);
mesh->addTriangle(2,7,3);
mesh->addTriangle(3,7,4);
mesh->addTriangle(3,4,0);
mesh->addTriangle(4,6,5);
mesh->addTriangle(4,7,6);
mesh->setPrimitive(SgMesh::Box(size));
mesh->updateBoundingBox();
generateNormals(mesh, 0.0);
return mesh;
}
SgMesh* MeshGenerator::generateExtrusion(const Extrusion& extrusion)
{
const int numSpines = extrusion.spine.size();
const int numCrosses = extrusion.crossSection.size();
bool isClosed = false;
if(extrusion.spine[0][0] == extrusion.spine[numSpines - 1][0] &&
extrusion.spine[0][1] == extrusion.spine[numSpines - 1][1] &&
extrusion.spine[0][2] == extrusion.spine[numSpines - 1][2] ){
isClosed = true;
}
bool isCrossSectionClosed = (extrusion.crossSection[0] == extrusion.crossSection[numCrosses - 1]);
SgMesh* mesh = new SgMesh;
SgVertexArray& vertices = *mesh->setVertices(new SgVertexArray());
vertices.reserve(numSpines * numCrosses);
Vector3 preZaxis(Vector3::Zero());
int definedZaxis = -1;
vector<Vector3> Yaxisarray;
vector<Vector3> Zaxisarray;
if(numSpines > 2){
for(int i=0; i < numSpines; ++i){
Vector3 Yaxis, Zaxis;
if(i == 0){
if(isClosed){
const Vector3& spine1 = extrusion.spine[numSpines - 2];
const Vector3& spine2 = extrusion.spine[0];
const Vector3& spine3 = extrusion.spine[1];
Yaxis = spine3 - spine1;
Zaxis = (spine3 - spine2).cross(spine1 - spine2);
} else {
const Vector3& spine1 = extrusion.spine[0];
const Vector3& spine2 = extrusion.spine[1];
const Vector3& spine3 = extrusion.spine[2];
Yaxis = spine2 - spine1;
Zaxis = (spine3 - spine2).cross(spine1 - spine2);
}
} else if(i == numSpines - 1){
if(isClosed){
const Vector3& spine1 = extrusion.spine[numSpines - 2];
const Vector3& spine2 = extrusion.spine[0];
const Vector3& spine3 = extrusion.spine[1];
Yaxis = spine3 - spine1;
Zaxis = (spine3 - spine2).cross(spine1 - spine2);
} else {
const Vector3& spine1 = extrusion.spine[numSpines - 3];
const Vector3& spine2 = extrusion.spine[numSpines - 2];
const Vector3& spine3 = extrusion.spine[numSpines - 1];
Yaxis = spine3 - spine2;
Zaxis = (spine3 - spine2).cross(spine1 - spine2);
}
} else {
const Vector3& spine1 = extrusion.spine[i - 1];
const Vector3& spine2 = extrusion.spine[i];
const Vector3& spine3 = extrusion.spine[i + 1];
Yaxis = spine3 - spine1;
Zaxis = (spine3-spine2).cross(spine1-spine2);
}
if(!Zaxis.norm()){
if(definedZaxis != -1)
Zaxis = preZaxis;
} else {
if(definedZaxis == -1){
definedZaxis = i;
}
preZaxis = Zaxis;
}
Yaxisarray.push_back(Yaxis);
Zaxisarray.push_back(Zaxis);
}
} else {
const Vector3 Yaxis(extrusion.spine[1] - extrusion.spine[0]);
Yaxisarray.push_back(Yaxis);
Yaxisarray.push_back(Yaxis);
}
const int numScales = extrusion.scale.size();
const int numOrientations = extrusion.orientation.size();
Vector3 scale(1.0, 0.0, 1.0);
AngleAxis orientation(0.0, Vector3::UnitZ());
for(int i=0; i < numSpines; ++i){
Matrix3 Scp;
Vector3 y = Yaxisarray[i].normalized();
if(definedZaxis == -1){
AngleAxis R(acos(y[1]), Vector3(y[2], 0.0, -y[0]));
Scp = R.toRotationMatrix();
} else {
if(i < definedZaxis){
Zaxisarray[i] = Zaxisarray[definedZaxis];
}
if(i && (Zaxisarray[i].dot(Zaxisarray[i - 1]) < 0.0)){
Zaxisarray[i] *= -1.0;
}
Vector3 z = Zaxisarray[i].normalized();
Vector3 x = y.cross(z);
Scp << x, y, z;
}
const Vector3& spine = extrusion.spine[i];
if(numScales == 1){
scale << extrusion.scale[0][0], 0.0, extrusion.scale[0][1];
} else if(numScales > 1){
scale << extrusion.scale[i][0], 0.0, extrusion.scale[i][1];
}
if(numOrientations == 1){
orientation = extrusion.orientation[0];
} else if(numOrientations > 1){
orientation = extrusion.orientation[i];
}
for(int j=0; j < numCrosses; ++j){
const Vector3 crossSection(extrusion.crossSection[j][0], 0.0, extrusion.crossSection[j][1]);
const Vector3 v1(crossSection[0] * scale[0], 0.0, crossSection[2] * scale[2]);
const Vector3 v = Scp * orientation.toRotationMatrix() * v1 + spine;
vertices.push_back(v.cast<float>());
}
}
for(int i=0; i < numSpines - 1 ; ++i){
const int upper = i * numCrosses;
const int lower = (i + 1) * numCrosses;
for(int j=0; j < numCrosses - 1; ++j) {
mesh->addTriangle(j + upper, j + lower, (j + 1) + lower);
mesh->addTriangle(j + upper, (j + 1) + lower, j + 1 + upper);
}
}
int j = 0;
if(isCrossSectionClosed){
j = 1;
}
Triangulator<SgVertexArray> triangulator;
vector<int> polygon;
if(extrusion.beginCap && !isClosed){
triangulator.setVertices(vertices);
polygon.clear();
for(int i=0; i < numCrosses - j; ++i){
polygon.push_back(i);
}
triangulator.apply(polygon);
const vector<int>& triangles = triangulator.triangles();
for(size_t i=0; i < triangles.size(); i += 3){
mesh->addTriangle(polygon[triangles[i]], polygon[triangles[i+1]], polygon[triangles[i+2]]);
}
}
if(extrusion.endCap && !isClosed){
triangulator.setVertices(vertices);
polygon.clear();
for(int i=0; i < numCrosses - j; ++i){
polygon.push_back(numCrosses * (numSpines - 1) + i);
}
triangulator.apply(polygon);
const vector<int>& triangles = triangulator.triangles();
for(size_t i=0; i < triangles.size(); i +=3){
mesh->addTriangle(polygon[triangles[i]], polygon[triangles[i+2]], polygon[triangles[i+1]]);
}
}
mesh->updateBoundingBox();
generateNormals(mesh, extrusion.creaseAngle);
return mesh;
}
void BulletCollisionDetectorImpl::addMesh(GeometryEx* model)
{
SgMesh* mesh = meshExtractor->currentMesh();
const Affine3& T = meshExtractor->currentTransform();
bool meshAdded = false;
if(mesh->primitiveType() != SgMesh::MESH){
bool doAddPrimitive = false;
Vector3 scale;
optional<Vector3> translation;
if(!meshExtractor->isCurrentScaled()){
scale.setOnes();
doAddPrimitive = true;
} else {
Affine3 S = meshExtractor->currentTransformWithoutScaling().inverse() *
meshExtractor->currentTransform();
if(S.linear().isDiagonal()){
if(!S.translation().isZero()){
translation = S.translation();
}
scale = S.linear().diagonal();
if(mesh->primitiveType() == SgMesh::BOX){
doAddPrimitive = true;
} else if(mesh->primitiveType() == SgMesh::SPHERE){
// check if the sphere is uniformly scaled for all the axes
if(scale.x() == scale.y() && scale.x() == scale.z()){
doAddPrimitive = true;
}
} else if(mesh->primitiveType() == SgMesh::CYLINDER){
// check if the bottom circle face is uniformly scaled
if(scale.x() == scale.z()){
doAddPrimitive = true;
}
} else if(mesh->primitiveType() == SgMesh::CONE){
if(scale.x() == scale.z()){
doAddPrimitive = true;
}
}
}
}
if(doAddPrimitive){
bool created = false;
btCollisionShape* primitiveShape;
switch(mesh->primitiveType()){
case SgMesh::BOX : {
const Vector3& s = mesh->primitive<SgMesh::Box>().size;
primitiveShape = new btBoxShape(btVector3(s.x() * scale.x()/2.0, s.y() * scale.y()/2.0, s.z() * scale.z()/2.0));
created = true;
break; }
case SgMesh::SPHERE : {
SgMesh::Sphere sphere = mesh->primitive<SgMesh::Sphere>();
primitiveShape = new btSphereShape(sphere.radius * scale.x());
created = true;
break; }
case SgMesh::CYLINDER : {
SgMesh::Cylinder cylinder = mesh->primitive<SgMesh::Cylinder>();
primitiveShape = new btCylinderShape(btVector3(cylinder.radius * scale.x(), cylinder.height * scale.y()/2.0, cylinder.radius * scale.x()));
created = true;
break; }
case SgMesh::CONE : {
SgMesh::Cone cone = mesh->primitive<SgMesh::Cone>();
primitiveShape = new btConeShape(cone.radius * scale.x(), cone.height * scale.y());
created = true;
break; }
default :
break;
}
if(created){
primitiveShape->setMargin(DEFAULT_COLLISION_MARGIN);
btCompoundShape* compoundShape = dynamic_cast<btCompoundShape*>(model->collisionShape);
if(!compoundShape){
model->collisionShape = new btCompoundShape();
model->collisionShape->setLocalScaling(btVector3(1.f,1.f,1.f));
compoundShape = dynamic_cast<btCompoundShape*>(model->collisionShape);
}
Affine3 T_ = meshExtractor->currentTransformWithoutScaling();
if(translation){
T_ *= Translation3(*translation);
}
btVector3 p(T_(0,3), T_(1,3), T_(2,3));
btMatrix3x3 R(T_(0,0), T_(0,1), T_(0,2),
T_(1,0), T_(1,1), T_(1,2),
T_(2,0), T_(2,1), T_(2,2));
btTransform btT(R, p);
compoundShape->addChildShape(btT, primitiveShape);
meshAdded = true;
}
}
}
if(!meshAdded){
if(!useHACD || model->isStatic){
const int vertexIndexTop = model->vertices.size() / 3;
const SgVertexArray& vertices_ = *mesh->vertices();
const int numVertices = vertices_.size();
for(int i=0; i < numVertices; ++i){
const Vector3 v = T * vertices_[i].cast<Position::Scalar>();
model->vertices.push_back((btScalar)v.x());
model->vertices.push_back((btScalar)v.y());
model->vertices.push_back((btScalar)v.z());
}
const int numTriangles = mesh->numTriangles();
for(int i=0; i < numTriangles; ++i){
SgMesh::TriangleRef tri = mesh->triangle(i);
model->triangles.push_back(vertexIndexTop + tri[0]);
model->triangles.push_back(vertexIndexTop + tri[1]);
model->triangles.push_back(vertexIndexTop + tri[2]);
}
}else{
btConvexHullShape* convexHullShape = dynamic_cast<btConvexHullShape*>(model->collisionShape);
if(convexHullShape){
btCompoundShape* compoundShape = new btCompoundShape();
compoundShape->setLocalScaling(btVector3(1.f,1.f,1.f));
btTransform T;
T.setIdentity();
compoundShape->addChildShape(T, convexHullShape);
model->collisionShape = compoundShape;
}
std::vector< HACD::Vec3<HACD::Real> > points;
std::vector< HACD::Vec3<long> > triangles;
const SgVertexArray& vertices_ = *mesh->vertices();
const int numVertices = vertices_.size();
for(int i=0; i < numVertices; ++i){
const Vector3 v = T * vertices_[i].cast<Position::Scalar>();
HACD::Vec3<HACD::Real> vertex(v.x(), v.y(), v.z());
points.push_back(vertex);
}
const int numTriangles = mesh->numTriangles();
for(int i=0; i < numTriangles; ++i){
SgMesh::TriangleRef tri = mesh->triangle(i);
HACD::Vec3<long> triangle(tri[0], tri[1], tri[2]);
triangles.push_back(triangle);
}
HACD::HACD hacd;
hacd.SetPoints(&points[0]);
hacd.SetNPoints(points.size());
hacd.SetTriangles(&triangles[0]);
hacd.SetNTriangles(triangles.size());
hacd.SetCompacityWeight(0.1);
hacd.SetVolumeWeight(0.0);
size_t nClusters = 1;
double concavity = 100;
bool invert = false;
bool addExtraDistPoints = false;
bool addNeighboursDistPoints = false;
bool addFacesPoints = false;
hacd.SetNClusters(nClusters); // minimum number of clusters
hacd.SetNVerticesPerCH(100); // max of 100 vertices per convex-hull
hacd.SetConcavity(concavity); // maximum concavity
hacd.SetAddExtraDistPoints(addExtraDistPoints);
hacd.SetAddNeighboursDistPoints(addNeighboursDistPoints);
hacd.SetAddFacesPoints(addFacesPoints);
hacd.Compute();
btTransform T;
T.setIdentity();
nClusters = hacd.GetNClusters();
if(nClusters>1){
btCompoundShape* compoundShape = dynamic_cast<btCompoundShape*>(model->collisionShape);
if(!compoundShape){
model->collisionShape = new btCompoundShape();
model->collisionShape->setLocalScaling(btVector3(1.f,1.f,1.f));
}
}
for(size_t i=0; i<nClusters; i++){
size_t nPoints = hacd.GetNPointsCH(i);
size_t nTriangles = hacd.GetNTrianglesCH(i);
HACD::Vec3<HACD::Real> * pointsCH = new HACD::Vec3<HACD::Real>[nPoints];
HACD::Vec3<long> * trianglesCH = new HACD::Vec3<long>[nTriangles];
hacd.GetCH(i, pointsCH, trianglesCH);
btAlignedObjectArray<btVector3> newVertices_;
for(size_t j=0; j<nTriangles; j++){
long index0 = trianglesCH[j].X();
long index1 = trianglesCH[j].Y();
long index2 = trianglesCH[j].Z();
btVector3 vertex0(pointsCH[index0].X(), pointsCH[index0].Y(), pointsCH[index0].Z());
btVector3 vertex1(pointsCH[index1].X(), pointsCH[index1].Y(), pointsCH[index1].Z());
btVector3 vertex2(pointsCH[index2].X(), pointsCH[index2].Y(), pointsCH[index2].Z());
newVertices_.push_back(vertex0);
newVertices_.push_back(vertex1);
newVertices_.push_back(vertex2);
}
delete [] pointsCH;
delete [] trianglesCH;
/*
float collisionMargin = 0.01f;
btAlignedObjectArray<btVector3> planeEquations;
btGeometryUtil::getPlaneEquationsFromVertices(newVertices_, planeEquations);
btAlignedObjectArray<btVector3> shiftedPlaneEquations;
for (int j=0; j<planeEquations.size(); j++){
btVector3 plane = planeEquations[j];
plane[3] += collisionMargin;
shiftedPlaneEquations.push_back(plane);
}
btAlignedObjectArray<btVector3> shiftedVertices;
btGeometryUtil::getVerticesFromPlaneEquations(shiftedPlaneEquations,shiftedVertices);
btConvexHullShape* convexHullShape_ = new btConvexHullShape(&(shiftedVertices[0].getX()),shiftedVertices.size());
*/
btConvexHullShape* convexHullShape_ = new btConvexHullShape(&(newVertices_[0].getX()), newVertices_.size());
convexHullShape_->setMargin(DEFAULT_COLLISION_MARGIN);
btCompoundShape* compoundShape = dynamic_cast<btCompoundShape*>(model->collisionShape);
if(compoundShape)
compoundShape->addChildShape(T, convexHullShape_);
else
model->collisionShape = convexHullShape_;
}
}
}
}
void ODELink::addMesh(MeshExtractor* extractor, ODEBody* odeBody)
{
SgMesh* mesh = extractor->currentMesh();
const Affine3& T = extractor->currentTransform();
bool meshAdded = false;
if(mesh->primitiveType() != SgMesh::MESH){
bool doAddPrimitive = false;
Vector3 scale;
optional<Vector3> translation;
if(!extractor->isCurrentScaled()){
scale.setOnes();
doAddPrimitive = true;
} else {
Affine3 S = extractor->currentTransformWithoutScaling().inverse() *
extractor->currentTransform();
if(S.linear().isDiagonal()){
if(!S.translation().isZero()){
translation = S.translation();
}
scale = S.linear().diagonal();
if(mesh->primitiveType() == SgMesh::BOX){
doAddPrimitive = true;
} else if(mesh->primitiveType() == SgMesh::SPHERE){
// check if the sphere is uniformly scaled for all the axes
if(scale.x() == scale.y() && scale.x() == scale.z()){
doAddPrimitive = true;
}
} else if(mesh->primitiveType() == SgMesh::CYLINDER){
// check if the bottom circle face is uniformly scaled
if(scale.x() == scale.z()){
doAddPrimitive = true;
}
}
}
}
if(doAddPrimitive){
bool created = false;
dGeomID geomId;
switch(mesh->primitiveType()){
case SgMesh::BOX : {
const Vector3& s = mesh->primitive<SgMesh::Box>().size;
geomId = dCreateBox(odeBody->spaceID, s.x() * scale.x(), s.y() * scale.y(), s.z() * scale.z());
created = true;
break; }
case SgMesh::SPHERE : {
SgMesh::Sphere sphere = mesh->primitive<SgMesh::Sphere>();
geomId = dCreateSphere(odeBody->spaceID, sphere.radius * scale.x());
created = true;
break; }
case SgMesh::CYLINDER : {
SgMesh::Cylinder cylinder = mesh->primitive<SgMesh::Cylinder>();
geomId = dCreateCylinder(odeBody->spaceID, cylinder.radius * scale.x(), cylinder.height * scale.y());
created = true;
break; }
default :
break;
}
if(created){
geomID.push_back(geomId);
dGeomSetBody(geomId, bodyID);
Affine3 T_ = extractor->currentTransformWithoutScaling();
if(translation){
T_ *= Translation3(*translation);
}
Vector3 p = T_.translation()-link->c();
dMatrix3 R = { T_(0,0), T_(0,1), T_(0,2), 0.0,
T_(1,0), T_(1,1), T_(1,2), 0.0,
T_(2,0), T_(2,1), T_(2,2), 0.0 };
if(bodyID){
dGeomSetOffsetPosition(geomId, p.x(), p.y(), p.z());
dGeomSetOffsetRotation(geomId, R);
}else{
offsetMap.insert(OffsetMap::value_type(geomId,T_));
}
meshAdded = true;
}
}
}
if(!meshAdded){
const int vertexIndexTop = vertices.size();
const SgVertexArray& vertices_ = *mesh->vertices();
const int numVertices = vertices_.size();
for(int i=0; i < numVertices; ++i){
const Vector3 v = T * vertices_[i].cast<Position::Scalar>() - link->c();
vertices.push_back(Vertex(v.x(), v.y(), v.z()));
}
const int numTriangles = mesh->numTriangles();
for(int i=0; i < numTriangles; ++i){
SgMesh::TriangleRef src = mesh->triangle(i);
Triangle tri;
tri.indices[0] = vertexIndexTop + src[0];
tri.indices[1] = vertexIndexTop + src[1];
tri.indices[2] = vertexIndexTop + src[2];
triangles.push_back(tri);
}
}
}
