glm::mat4 VisibleObject::getParentWorldTransform()
{
if (parent == NULL) {
return getLocalTransformation();
}
else {
return parent->getParentWorldTransform() * parent->getLocalTransformation();
}
}
RobotNodePtr RobotNodeFixed::_clone(const RobotPtr newRobot, const VisualizationNodePtr visualizationModel, const CollisionModelPtr collisionModel, CollisionCheckerPtr colChecker, float scaling)
{
ReadLockPtr lock = getRobot()->getReadLock();
RobotNodePtr result;
Physics p = physics.scale(scaling);
if (optionalDHParameter.isSet)
{
result.reset(new RobotNodeFixed(newRobot,name,optionalDHParameter.aMM()*scaling,optionalDHParameter.dMM()*scaling, optionalDHParameter.alphaRadian(), optionalDHParameter.thetaRadian(),visualizationModel,collisionModel,p,colChecker,nodeType));
} else
{
Eigen::Matrix4f lt = getLocalTransformation();
lt.block(0,3,3,1) *= scaling;
result.reset(new RobotNodeFixed(newRobot,name,lt,visualizationModel,collisionModel,p,colChecker,nodeType));
}
return result;
}
void RobotNode::updateVisualizationPose( const Eigen::Matrix4f &globalPose, bool updateChildren )
{
// check if we are a root node
SceneObjectPtr parent = getParent();
RobotPtr rob = getRobot();
if (!parent || parent == rob)
{
if (rob && rob->getRootNode() == static_pointer_cast<RobotNode>(shared_from_this()))
{
Eigen::Matrix4f gpPre = getLocalTransformation().inverse() * globalPose;
rob->setGlobalPose(gpPre, false);
} else
VR_WARNING << "INTERNAL ERROR: getParent==robot but getRoot!=this ?! " << endl;
}
this->globalPose = globalPose;
// update collision and visualization model and children
SceneObject::updatePose(updateChildren);
}
void RobotNodePrismatic::updateTransformationMatrices(const Eigen::Matrix4f& parentPose)
{
Eigen::Affine3f tmpT(Eigen::Translation3f((this->getJointValue() + jointValueOffset)*jointTranslationDirection));
globalPose = parentPose * getLocalTransformation() * tmpT.matrix();
if (visuScaling)
{
float scValue = 0;
if (jointValueOffset == 0)
{
scValue = jointValue;
}
else
{
scValue = (jointValue) / jointValueOffset;
}
Eigen::Vector3f scVec;
for (int i = 0; i < 3; i++)
{
scVec(i) = 1.0f + visuScaleFactor(i) * scValue;
}
if (visualizationModel)
{
visualizationModel->scale(scVec);
}
if (collisionModel)
{
collisionModel->scale(scVec);
}
for (int i = 0; i < 3; i++)
{
physics.localCoM(i) = unscaledLocalCoM(i) * scVec(i);
}
}
}
RobotNodePtr RobotNodePrismatic::_clone(const RobotPtr newRobot, const VisualizationNodePtr visualizationModel, const CollisionModelPtr collisionModel, CollisionCheckerPtr colChecker, float scaling)
{
boost::shared_ptr<RobotNodePrismatic> result;
ReadLockPtr lock = getRobot()->getReadLock();
Physics p = physics.scale(scaling);
if (optionalDHParameter.isSet)
{
result.reset(new RobotNodePrismatic(newRobot, name, jointLimitLo, jointLimitHi, optionalDHParameter.aMM()*scaling, optionalDHParameter.dMM()*scaling, optionalDHParameter.alphaRadian(), optionalDHParameter.thetaRadian(), visualizationModel, collisionModel, jointValueOffset, p, colChecker, nodeType));
}
else
{
Eigen::Matrix4f lt = getLocalTransformation();
lt.block(0, 3, 3, 1) *= scaling;
result.reset(new RobotNodePrismatic(newRobot, name, jointLimitLo, jointLimitHi, lt, jointTranslationDirection, visualizationModel, collisionModel, jointValueOffset, p, colChecker, nodeType));
}
if (result && visuScaling)
{
result->setVisuScaleFactor(visuScaleFactor);
}
return result;
}
void RobotNodeRevolute::updateTransformationMatrices(const Eigen::Matrix4f& parentPose)
{
Eigen::Affine3f tmpT(Eigen::AngleAxisf(jointValue + jointValueOffset, jointRotationAxis));
globalPose = parentPose * getLocalTransformation() * tmpT.matrix();
}
void RobotNode::updateTransformationMatrices(const Eigen::Matrix4f &parentPose)
{
this->globalPose = parentPose * getLocalTransformation();
//globalPosePostJoint = this->globalPose*getPostJointTransformation();
}
glm::mat4 DisplayObject::getGlobalTransformation ()
{
if (_parent != nullptr) return _parent->getGlobalTransformation() * getLocalTransformation();
return getLocalTransformation();
}
void RobotNodeFixed::updateTransformationMatrices(const Eigen::Matrix4f &parentPose)
{
this->globalPose = parentPose * getLocalTransformation();
}
