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);
}
}
static void setShape(Link* link, SgGroup* shape, bool isVisual)
{
SgNodePtr node;
if(shape->empty()){
node = new SgNode;
} else {
SgInvariantGroup* invariant = new SgInvariantGroup;
if(link->Rs().isApprox(Matrix3::Identity())){
shape->copyChildrenTo(invariant);
} else {
SgPosTransform* transformRs = new SgPosTransform;
transformRs->setRotation(link->Rs());
shape->copyChildrenTo(transformRs);
invariant->addChild(transformRs);
}
node = invariant;
}
if(node){
if(isVisual){
link->setVisualShape(node);
} else {
link->setCollisionShape(node);
}
}
}
SgMesh* MeshGenerator::generateArrow(double length, double width, double coneLengthRatio, double coneWidthRatio)
{
double r = width / 2.0;
double h = length * coneLengthRatio;
SgShapePtr cone = new SgShape;
//setDivisionNumber(20);
cone->setMesh(generateCone(r * coneWidthRatio, h));
SgPosTransform* conePos = new SgPosTransform;
conePos->setTranslation(Vector3(0.0, length / 2.0 + h / 2.0, 0.0));
conePos->addChild(cone);
SgShapePtr cylinder = new SgShape;
//setDivisionNumber(12);
cylinder->setMesh(generateCylinder(r, length, true, false));
//cylinder->setMesh(generateCylinder(r, length - h, true, false));
//SgPosTransform* cylinderPos = new SgPosTransform;
//cylinderPos->setTranslation(Vector3(0.0, -h, 0.0));
//cylinderPos->addChild(cylinder);
MeshExtractor meshExtractor;
SgGroupPtr group = new SgGroup;
group->addChild(conePos);
//group->addChild(cylinderPos);
group->addChild(cylinder);
return meshExtractor.integrate(group);
}
Link* VRMLBodyLoaderImpl::readJointNode(VRMLProtoInstance* jointNode, const Matrix3& parentRs)
{
putVerboseMessage(string("Joint node") + jointNode->defName);
Link* link = createLink(jointNode, parentRs);
LinkInfo iLink;
iLink.link = link;
SgInvariantGroupPtr shapeTop = new SgInvariantGroup();
if(link->Rs().isApprox(Matrix3::Identity())){
iLink.shape = shapeTop.get();
} else {
SgPosTransform* transformRs = new SgPosTransform;
transformRs->setRotation(link->Rs());
shapeTop->addChild(transformRs);
iLink.shape = transformRs;
}
iLink.m = 0.0;
iLink.c = Vector3::Zero();
iLink.I = Matrix3::Zero();
MFNode& childNodes = get<MFNode>(jointNode->fields["children"]);
Affine3 T(Affine3::Identity());
ProtoIdSet acceptableProtoIds;
acceptableProtoIds.set(PROTO_JOINT);
acceptableProtoIds.set(PROTO_SEGMENT);
acceptableProtoIds.set(PROTO_DEVICE);
readJointSubNodes(iLink, childNodes, acceptableProtoIds, T);
Matrix3& I = iLink.I;
for(size_t i=0; i < iLink.segments.size(); ++i){
const SegmentInfo& segment = iLink.segments[i];
const Vector3 o = segment.c - iLink.c;
const double& x = o.x();
const double& y = o.y();
const double& z = o.z();
const double& m = segment.m;
I(0,0) += m * (y * y + z * z);
I(0,1) += -m * (x * y);
I(0,2) += -m * (x * z);
I(1,0) += -m * (y * x);
I(1,1) += m * (z * z + x * x);
I(1,2) += -m * (y * z);
I(2,0) += -m * (z * x);
I(2,1) += -m * (z * y);
I(2,2) += m * (x * x + y * y);
}
link->setMass(iLink.m);
link->setCenterOfMass(link->Rs() * iLink.c);
link->setInertia(link->Rs() * iLink.I * link->Rs().transpose());
if(iLink.shape->empty()){
link->setShape(new SgNode()); // set empty node
} else {
link->setShape(shapeTop);
}
return link;
}
TranslationDragger::TranslationDragger(bool setDefaultAxes)
{
draggableAxes_ = TX | TY | TZ;
axisCylinderNormalizedRadius = 0.04;
defaultAxesScale = new SgScaleTransform;
customAxes = new SgGroup;
for(int i=0; i < 3; ++i){
SgMaterial* material = new SgMaterial;
Vector3f color(0.2f, 0.2f, 0.2f);
color[i] = 1.0f;
material->setDiffuseColor(Vector3f::Zero());
material->setEmissiveColor(color);
material->setAmbientIntensity(0.0f);
material->setTransparency(0.6f);
axisMaterials[i] = material;
}
if(setDefaultAxes){
MeshGenerator meshGenerator;
SgMeshPtr mesh = meshGenerator.generateArrow(1.8, 0.08, 0.1, 2.5);
for(int i=0; i < 3; ++i){
SgShape* shape = new SgShape;
shape->setMesh(mesh);
shape->setMaterial(axisMaterials[i]);
SgPosTransform* arrow = new SgPosTransform;
arrow->addChild(shape);
if(i == 0){
arrow->setRotation(AngleAxis(-PI / 2.0, Vector3::UnitZ()));
} else if(i == 2){
arrow->setRotation(AngleAxis( PI / 2.0, Vector3::UnitX()));
}
SgInvariantGroup* invariant = new SgInvariantGroup;
invariant->setName(axisNames[i]);
invariant->addChild(arrow);
SgSwitch* axis = new SgSwitch;
axis->addChild(invariant);
defaultAxesScale->addChild(axis);
}
addChild(defaultAxesScale);
}
isContainerMode_ = false;
}
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);
}
}
}
SgCamera* VisionRenderer::initializeCamera()
{
SgCamera* sceneCamera = 0;
SceneBody* sceneBody = sceneBodies[bodyIndex];
if(camera){
SceneDevice* sceneDevice = sceneBody->getSceneDevice(device);
if(sceneDevice){
sceneCamera = sceneDevice->findNodeOfType<SgCamera>();
pixelWidth = camera->resolutionX();
pixelHeight = camera->resolutionY();
double frameRate = std::max(0.1, std::min(camera->frameRate(), simImpl->maxFrameRate));
cycleTime = 1.0 / frameRate;
if(simImpl->isVisionDataRecordingEnabled){
camera->setImageStateClonable(true);
}
}
} else if(rangeSensor){
const double thresh = (170.0 * PI / 180.0); // 170[deg]
bool isWithinPossibleRanges =
(rangeSensor->yawRange() < thresh) && (rangeSensor->pitchRange() < thresh);
if(isWithinPossibleRanges){
SceneLink* sceneLink = sceneBody->sceneLink(rangeSensor->link()->index());
if(sceneLink){
SgPerspectiveCamera* persCamera = new SgPerspectiveCamera;
sceneCamera = persCamera;
persCamera->setNearDistance(rangeSensor->minDistance());
persCamera->setFarDistance(rangeSensor->maxDistance());
SgPosTransform* cameraPos = new SgPosTransform();
cameraPos->setTransform(rangeSensor->T_local());
cameraPos->addChild(persCamera);
sceneLink->addChild(cameraPos);
if(rangeSensor->yawRange() > rangeSensor->pitchRange()){
pixelWidth = rangeSensor->yawResolution() * simImpl->rangeSensorPrecisionRatio;
if(rangeSensor->pitchRange() == 0.0){
pixelHeight = 1;
double r = tan(rangeSensor->yawRange() / 2.0) * 2.0 / pixelWidth;
persCamera->setFieldOfView(atan2(r / 2.0, 1.0) * 2.0);
} else {
pixelHeight = rangeSensor->pitchResolution() * simImpl->rangeSensorPrecisionRatio;
persCamera->setFieldOfView(rangeSensor->pitchRange());
}
} else {
pixelHeight = rangeSensor->pitchResolution() * simImpl->rangeSensorPrecisionRatio;
if(rangeSensor->yawRange() == 0.0){
pixelWidth = 1;
double r = tan(rangeSensor->pitchRange() / 2.0) * 2.0 / pixelHeight;
persCamera->setFieldOfView(atan2(r / 2.0, 1.0) * 2.0);
} else {
pixelWidth = rangeSensor->yawResolution() * simImpl->rangeSensorPrecisionRatio;
persCamera->setFieldOfView(rangeSensor->yawRange());
}
}
double frameRate = std::max(0.1, std::min(rangeSensor->frameRate(), simImpl->maxFrameRate));
cycleTime = 1.0 / frameRate;
if(simImpl->isVisionDataRecordingEnabled){
rangeSensor->setRangeDataStateClonable(true);
}
depthError = simImpl->depthError;
}
}
}
return sceneCamera;
}