string MeshShapeItemImpl::toURDF()
{
ostringstream ss;
ss << "<link name=\"" << self->name() << "\">" << endl;
Affine3 relative;
JointItem* parentjoint = dynamic_cast<JointItem*>(self->parentItem());
if (parentjoint) {
Affine3 parent, child;
parent.translation() = parentjoint->translation;
parent.linear() = parentjoint->rotation;
child.translation() = self->translation;
child.linear() = self->rotation;
relative = parent.inverse() * child;
} else {
relative.translation() = self->translation;
relative.linear() = self->rotation;
}
for (int i=0; i < 2; i++) {
if (i == 0) {
ss << " <visual>" << endl;
} else {
ss << " <collision>" << endl;
}
if (i == 0) {
ss << " </visual>" << endl;
} else {
ss << " </collision>" << endl;
}
}
return ss.str();
}
double EuclidDistHeuristic::computeDistance(
const Affine3& a,
const Affine3& b) const
{
auto sqrd = [](double d) { return d * d; };
Vector3 diff = b.translation() - a.translation();
double dp2 =
m_x_coeff * sqrd(diff.x()) +
m_y_coeff * sqrd(diff.y()) +
m_z_coeff * sqrd(diff.z());
Quaternion qb(b.rotation());
Quaternion qa(a.rotation());
double dot = qa.dot(qb);
if (dot < 0.0) {
qb = Quaternion(-qb.w(), -qb.x(), -qb.y(), -qb.z());
dot = qa.dot(qb);
}
double dr2 = angles::normalize_angle(2.0 * std::acos(dot));
dr2 *= (m_rot_coeff * dr2);
SMPL_DEBUG_NAMED(LOG, "Compute Distance: sqrt(%f + %f)", dp2, dr2);
return std::sqrt(dp2 + dr2);
}
Affine3 SgCamera::positionLookingFor(const Vector3& eye, const Vector3& direction, const Vector3& up)
{
Vector3 d = direction.normalized();
Vector3 c = d.cross(up).normalized();
Vector3 u = c.cross(d);
Affine3 C;
C.linear() << c, u, -d;
C.translation() = eye;
return C;
}
bool RotationDragger::onButtonPressEvent(const SceneWidgetEvent& event)
{
int axis;
int indexOfTopNode;
if(detectAxisFromNodePath(event.nodePath(), this, axis, indexOfTopNode)){
Affine3 T = calcTotalTransform(event.nodePath(), this);
dragProjector.setInitialPosition(T);
dragProjector.setRotationAxis(T.linear().col(axis));
if(dragProjector.startRotation(event)){
sigRotationStarted_();
return true;
}
}
return false;
}
static inline void writeAffine3(YAMLWriter& writer, const Affine3& value)
{
writer.startFlowStyleListing();
writer.putScalar(value.translation().x());
writer.putScalar(value.translation().y());
writer.putScalar(value.translation().z());
Vector3 rpy(rpyFromRot(value.linear()));
writer.putScalar(rpy[0]);
writer.putScalar(rpy[1]);
writer.putScalar(rpy[2]);
writer.endListing();
}
void VRMLBodyLoaderImpl::readJointSubNodes(LinkInfo& iLink, MFNode& childNodes, const ProtoIdSet& acceptableProtoIds, const Affine3& T)
{
for(size_t i = 0; i < childNodes.size(); ++i){
bool doTraverse = false;
VRMLNode* childNode = childNodes[i].get();
if(!childNode->isCategoryOf(PROTO_INSTANCE_NODE)){
doTraverse = true;
} else {
VRMLProtoInstance* protoInstance = static_cast<VRMLProtoInstance*>(childNode);
int id = PROTO_UNDEFINED;
const string& protoName = protoInstance->proto->protoName;
ProtoInfoMap::iterator p = protoInfoMap.find(protoName);
if(p == protoInfoMap.end()){
doTraverse = true;
childNode = protoInstance->actualNode.get();
} else {
id = p->second.id;
if(!acceptableProtoIds.test(id)){
throw invalid_argument(str(format(_("%1% node is not in a correct place.")) % protoName));
}
if(isVerbose){
messageIndent += 2;
}
switch(id){
case PROTO_JOINT:
if(!T.matrix().isApprox(Affine3::MatrixType::Identity())){
throw invalid_argument(
str(format(_("Joint node \"%1%\" is not in a correct place.")) % protoInstance->defName));
}
iLink.link->appendChild(readJointNode(protoInstance, iLink.link->Rs()));
break;
case PROTO_SEGMENT:
readSegmentNode(iLink, protoInstance, T);
linkOriginalMap[iLink.link] = childNodes[i];
break;
case PROTO_SURFACE:
readSurfaceNode(iLink, protoInstance, T);
break;
case PROTO_DEVICE:
readDeviceNode(iLink, protoInstance, T);
break;
default:
doTraverse = true;
break;
}
if(isVerbose){
messageIndent -= 2;
}
}
}
if(doTraverse && childNode->isCategoryOf(GROUPING_NODE)){
VRMLGroup* group = static_cast<VRMLGroup*>(childNode);
if(VRMLTransform* transform = dynamic_cast<VRMLTransform*>(group)){
readJointSubNodes(iLink, group->getChildren(), acceptableProtoIds, T * transform->toAffine3d());
} else {
readJointSubNodes(iLink, group->getChildren(), acceptableProtoIds, T);
}
}
}
}
static void readAffine3(const Listing& node, Affine3& out_value)
{
if(node.size() == 6){
Affine3::TranslationPart t = out_value.translation();
t[0] = node[0].toDouble();
t[1] = node[1].toDouble();
t[2] = node[2].toDouble();
const double r = node[3].toDouble();
const double p = node[4].toDouble();
const double y = node[5].toDouble();
out_value.linear() = rotFromRpy(r, p, y);
}
}
void VRMLBodyLoaderImpl::readSurfaceNode(LinkInfo& iLink, VRMLProtoInstance* segmentShapeNode, const Affine3& T)
{
const string& typeName = segmentShapeNode->proto->protoName;
if(isVerbose) putMessage(string("Surface node ") + segmentShapeNode->defName);
iLink.isSurfaceNodeUsed = true;
// check if another Surface node does not appear in the subtree
MFNode& visualNodes = get<MFNode>(segmentShapeNode->fields["visual"]);
ProtoIdSet acceptableProtoIds;
readJointSubNodes(iLink, visualNodes, acceptableProtoIds, T);
MFNode& collisionNodes = get<MFNode>(segmentShapeNode->fields["collision"]);
readJointSubNodes(iLink, collisionNodes, acceptableProtoIds, T);
SgGroup* group;
SgPosTransform* transform = 0;
if(T.isApprox(Affine3::Identity())){
group = iLink.collisionShape;
} else {
transform = new SgPosTransform(T);
group = transform;
}
for(size_t i=0; i < collisionNodes.size(); ++i){
SgNodePtr node = sgConverter.convert(collisionNodes[i]);
if(node){
group->addChild(node);
}
}
if(transform && !transform->empty()){
iLink.collisionShape->addChild(transform);
}
}
bool PhongShadowProgram::renderLight(int index, const SgLight* light, const Affine3& T, const Affine3& viewMatrix, bool shadowCasting)
{
LightInfo& info = lightInfos[index];
if(const SgDirectionalLight* dirLight = dynamic_cast<const SgDirectionalLight*>(light)){
Vector3 d = viewMatrix.linear() * T.linear() * -dirLight->direction();
Vector4f pos(d.x(), d.y(), d.z(), 0.0f);
glUniform4fv(info.positionLocation, 1, pos.data());
} else if(const SgPointLight* pointLight = dynamic_cast<const SgPointLight*>(light)){
Vector3 p(viewMatrix * T.translation());
Vector4f pos(p.x(), p.y(), p.z(), 1.0f);
glUniform4fv(info.positionLocation, 1, pos.data());
glUniform1f(info.constantAttenuationLocation, pointLight->constantAttenuation());
glUniform1f(info.linearAttenuationLocation, pointLight->linearAttenuation());
glUniform1f(info.quadraticAttenuationLocation, pointLight->quadraticAttenuation());
if(const SgSpotLight* spotLight = dynamic_cast<const SgSpotLight*>(pointLight)){
Vector3 d = viewMatrix.linear() * T.linear() * spotLight->direction();
Vector3f direction(d.cast<float>());
glUniform3fv(info.directionLocation, 1, direction.data());
glUniform1f(info.cutoffAngleLocation, spotLight->cutOffAngle());
glUniform1f(info.beamWidthLocation, spotLight->beamWidth());
glUniform1f(info.cutoffExponentLocation, spotLight->cutOffExponent());
}
} else {
return false;
}
Vector3f intensity(light->intensity() * light->color());
glUniform3fv(info.intensityLocation, 1, intensity.data());
Vector3f ambientIntensity(light->ambientIntensity() * light->color());
glUniform3fv(info.ambientIntensityLocation, 1, ambientIntensity.data());
if(shadowCasting){
if(currentShadowIndex < numShadows_){
ShadowInfo& shadow = shadowInfos[currentShadowIndex];
glUniform1i(shadow.shadowMapLocation, currentShadowIndex + 1);
glUniform1i(shadow.lightIndexLocation, index);
++currentShadowIndex;
}
}
return true;
}
void VRMLBodyLoaderImpl::readSegmentNode(LinkInfo& iLink, VRMLProtoInstance* segmentNode, const Affine3& T)
{
if(isVerbose) putMessage(string("Segment node ") + segmentNode->defName);
/*
Mass = Sigma mass
C = (Sigma mass * T * c) / Mass
I = Sigma(R * I * Rt + G)
R = Rotation matrix part of T
G = y*y+z*z, -x*y, -x*z, -y*x, z*z+x*x, -y*z, -z*x, -z*y, x*x+y*y
(x, y, z ) = T * c - C
*/
VRMLProtoFieldMap& sf = segmentNode->fields;
SegmentInfo iSegment;
readVRMLfield(sf["mass"], iSegment.m);
Vector3 c;
readVRMLfield(sf["centerOfMass"], c);
iSegment.c = T.linear() * c + T.translation();
iLink.c = (iSegment.c * iSegment.m + iLink.c * iLink.m) / (iLink.m + iSegment.m);
iLink.m += iSegment.m;
Matrix3 I;
readVRMLfield(sf["momentsOfInertia"], I);
iLink.I.noalias() += T.linear() * I * T.linear().transpose();
MFNode& childNodes = get<MFNode>(segmentNode->fields["children"]);
ProtoIdSet acceptableProtoIds;
acceptableProtoIds.set(PROTO_SURFACE);
acceptableProtoIds.set(PROTO_DEVICE);
readJointSubNodes(iLink, childNodes, acceptableProtoIds, T);
SgNodePtr node = sgConverter.convert(segmentNode);
if(node){
if(T.isApprox(Affine3::Identity())){
iLink.visualShape->addChild(node);
} else {
SgPosTransform* transform = new SgPosTransform(T);
transform->addChild(node);
iLink.visualShape->addChild(transform);
}
}
}
void SfmData::applyTransform(const Affine3<FltType> &M) {
auto M_inv = M.inv();
for (auto& cam : cameras) {
auto tmp = cam.pose()*M_inv;
cam.R = tmp.rotation();
cam.t = tmp.translation();
}
for (auto &p : cloud) {
p = M*p;
}
}
int EuclidDistHeuristic::GetGoalHeuristic(int state_id)
{
if (state_id == planningSpace()->getGoalStateID()) {
return 0;
}
if (m_pose_ext) {
Affine3 p;
if (!m_pose_ext->projectToPose(state_id, p)) {
return 0;
}
auto& goal_pose = planningSpace()->goal().pose;
const double dist = computeDistance(p, goal_pose);
const int h = FIXED_POINT_RATIO * dist;
double Y, P, R;
angles::get_euler_zyx(p.rotation(), Y, P, R);
SMPL_DEBUG_NAMED(LOG, "h(%0.3f, %0.3f, %0.3f, %0.3f, %0.3f, %0.3f) = %d", p.translation()[0], p.translation()[1], p.translation()[2], Y, P, R, h);
return h;
} else if (m_point_ext) {
Vector3 p;
if (!m_point_ext->projectToPoint(state_id, p)) {
return 0;
}
auto& goal_pose = planningSpace()->goal().pose;
Vector3 gp(goal_pose.translation());
double dist = computeDistance(p, gp);
const int h = FIXED_POINT_RATIO * dist;
SMPL_DEBUG_NAMED(LOG, "h(%d) = %d", state_id, h);
return h;
} else {
return 0;
}
}
void PhongShadowProgram::setTransformMatrices(const Affine3& viewMatrix, const Affine3& modelMatrix, const Matrix4& PV)
{
const Affine3f VM = (viewMatrix * modelMatrix).cast<float>();
const Matrix3f N = VM.linear();
const Matrix4f PVM = (PV * modelMatrix.matrix()).cast<float>();
if(useUniformBlockToPassTransformationMatrices){
transformBlockBuffer.write(modelViewMatrixIndex, VM);
transformBlockBuffer.write(normalMatrixIndex, N);
transformBlockBuffer.write(MVPIndex, PVM);
transformBlockBuffer.flush();
} else {
glUniformMatrix4fv(modelViewMatrixLocation, 1, GL_FALSE, VM.data());
glUniformMatrix3fv(normalMatrixLocation, 1, GL_FALSE, N.data());
glUniformMatrix4fv(MVPLocation, 1, GL_FALSE, PVM.data());
for(int i=0; i < numShadows_; ++i){
ShadowInfo& shadow = shadowInfos[i];
const Matrix4f BPVM = (shadow.BPV * modelMatrix.matrix()).cast<float>();
glUniformMatrix4fv(shadow.shadowMatrixLocation, 1, GL_FALSE, BPVM.data());
}
}
}
void cnoid::savePCD(SgPointSet* pointSet, const std::string& filename, const Affine3& viewpoint)
{
if(!pointSet->hasVertices()){
throw empty_data_error() << error_info_message(_("Empty pointset"));
}
ofstream ofs;
ofs.open(filename.c_str());
ofs <<
"# .PCD v.7 - Point Cloud Data file format\n"
"VERSION .7\n"
"FIELDS x y z\n"
"SIZE 4 4 4\n"
"TYPE F F F\n"
"COUNT 1 1 1\n";
const SgVertexArray& points = *pointSet->vertices();
const int numPoints = points.size();
ofs << "WIDTH " << numPoints << "\n";
ofs << "HEIGHT 1\n";
ofs << "VIEWPOINT ";
Affine3::ConstTranslationPart t = viewpoint.translation();
ofs << t.x() << " " << t.y() << " " << t.z() << " ";
const Quaternion q(viewpoint.rotation());
ofs << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << "\n";
ofs << "POINTS " << numPoints << "\n";
ofs << "DATA ascii\n";
for(int i=0; i < numPoints; ++i){
const Vector3f& p = points[i];
ofs << p.x() << " " << p.y() << " " << p.z() << "\n";
}
ofs.close();
}
bool TranslationDragger::onButtonPressEvent(const SceneWidgetEvent& event)
{
int axis;
int indexOfTopNode;
const SgNodePath& path = event.nodePath();
if(detectAxisFromNodePath(path, this, axis, indexOfTopNode)){
SgNodePath::const_iterator axisIter = path.begin() + indexOfTopNode + 1;
const Affine3 T_global = calcTotalTransform(path.begin(), axisIter);
dragProjector.setInitialPosition(T_global);
const Affine3 T_axis = calcTotalTransform(axisIter, path.end());
const Vector3 p_local = (T_global * T_axis).inverse() * event.point();
if(p_local.norm() < 2.0 * axisCylinderNormalizedRadius){
dragProjector.setTranslationAlongViewPlane();
} else {
dragProjector.setTranslationAxis(T_global.linear().col(axis));
}
if(dragProjector.startTranslation(event)){
sigTranslationStarted_();
return true;
}
}
return false;
}
void VRMLBodyLoaderImpl::readDeviceNode(LinkInfo& iLink, VRMLProtoInstance* deviceNode, const Affine3& T)
{
const string& typeName = deviceNode->proto->protoName;
if(isVerbose) putMessage(typeName + " node " + deviceNode->defName);
DeviceFactoryMap::iterator p = deviceFactories.find(typeName);
if(p == deviceFactories.end()){
os() << str(format("Sensor type %1% is not supported.\n") % typeName) << endl;
} else {
DeviceFactory& factory = p->second;
DevicePtr device = factory(deviceNode);
if(device){
device->setLink(iLink.link);
const Matrix3 RsT = iLink.link->Rs();
device->setLocalTranslation(RsT * (T * device->localTranslation()));
device->setLocalRotation(RsT * (T.linear() * device->localRotation()));
body->addDevice(device);
}
}
}
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);
}
}
}
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_;
}
}
}
}
bool PoseProviderToBodyMotionConverter::convert(BodyPtr body, PoseProvider* provider, BodyMotion& motion)
{
const double frameRate = motion.frameRate();
const int beginningFrame = static_cast<int>(frameRate * std::max(provider->beginningTime(), lowerTime));
const int endingFrame = static_cast<int>(frameRate * std::min(provider->endingTime(), upperTime));
const int numJoints = body->numJoints();
const int numLinksToPut = (allLinkPositionOutputMode ? body->numLinks() : 1);
motion.setDimension(endingFrame + 1, numJoints, numLinksToPut, true);
MultiValueSeq& qseq = *motion.jointPosSeq();
MultiAffine3Seq& pseq = *motion.linkPosSeq();
Vector3Seq& relZmpSeq = *motion.relativeZmpSeq();
bool isZmpValid = false;
Link* rootLink = body->rootLink();
Link* baseLink = rootLink;
shared_ptr<LinkTraverse> fkTraverse;
if(allLinkPositionOutputMode){
fkTraverse.reset(new LinkTraverse(baseLink, true, true));
} else {
fkTraverse.reset(new LinkPath(baseLink, rootLink));
}
// store the original state
vector<double> orgq(numJoints);
for(int i=0; i < numJoints; ++i){
orgq[i] = body->joint(i)->q;
}
Affine3 orgp;
orgp.translation() = rootLink->p;
orgp.linear() = rootLink->R;
std::vector< boost::optional<double> > jointPositions(numJoints);
for(int frame = beginningFrame; frame <= endingFrame; ++frame){
provider->seek(frame / frameRate);
const int baseLinkIndex = provider->baseLinkIndex();
if(baseLinkIndex >= 0){
if(baseLinkIndex != baseLink->index){
baseLink = body->link(baseLinkIndex);
if(allLinkPositionOutputMode){
fkTraverse->find(baseLink, true, true);
} else {
static_pointer_cast<LinkPath>(fkTraverse)->find(baseLink, rootLink);
}
}
provider->getBaseLinkPosition(baseLink->p, baseLink->R);
}
MultiValueSeq::View qs = qseq.frame(frame);
provider->getJointPositions(jointPositions);
for(int i=0; i < numJoints; ++i){
const optional<double>& q = jointPositions[i];
qs[i] = q ? *q : 0.0;
body->joint(i)->q = qs[i];
}
if(allLinkPositionOutputMode || baseLink != rootLink){
fkTraverse->calcForwardKinematics();
}
for(int i=0; i < numLinksToPut; ++i){
Affine3& p = pseq.at(frame, i);
Link* link = body->link(i);
p.translation() = link->p;
p.linear() = link->R;
}
optional<Vector3> zmp = provider->zmp();
if(zmp){
relZmpSeq[frame].noalias() = rootLink->R.transpose() * (*zmp - rootLink->p);
isZmpValid = true;
}
}
if(!isZmpValid){
//bodyMotionItem->clearRelativeZmpSeq();
}
// restore the original state
for(int i=0; i < numJoints; ++i){
body->joint(i)->q = orgq[i];
}
rootLink->p = orgp.translation();
rootLink->R = orgp.linear();
body->calcForwardKinematics();
return true;
}
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);
}
}
}
