SimoxMotionState::SimoxMotionState( VirtualRobot::SceneObjectPtr sceneObject)
:btDefaultMotionState()
{
this->sceneObject = sceneObject;
initalGlobalPose.setIdentity();
if (sceneObject)
{
initalGlobalPose = sceneObject->getGlobalPoseVisualization();
}
_transform.setIdentity();
_graphicsTransfrom.setIdentity();
_comOffset.setIdentity();
Eigen::Vector3f com = sceneObject->getCoMLocal();
RobotNodePtr rn = boost::dynamic_pointer_cast<RobotNode>(sceneObject);
if (rn)
{
// we are operating on a RobotNode, so we need a RobtoNodeActuator to move it later on
robotNodeActuator.reset(new RobotNodeActuator(rn));
// localcom is given in coord sytsem of robotnode (== endpoint of internal transformation)
// we need com in visualization coord system (== without postjointtransform)
Eigen::Matrix4f t;
t.setIdentity();
t.block(0,3,3,1)=rn->getCoMGlobal();
t = rn->getGlobalPoseVisualization().inverse() * t;
com = t.block(0,3,3,1);
}
_setCOM(com);
setGlobalPose(initalGlobalPose);
}
void phx::core::actor::rigidactor::base::setGlobalPosition(vec3 p) {
/** @todo if is nested actor, then this is wrong... */
pose_.pos_ = vec4(p,1);
assert(px_actor_);
auto px_rigidactor = px_actor_->isRigidActor();
assert(px_rigidactor);
px_rigidactor->setGlobalPose(physx::PxTransform(
phx::util::convert(p),
phx::util::convert(pose_.rot_)
));
}
