void Robot::updateFrom(tf::TransformListener *tfListener) {
using namespace Ogre;
static tf::StampedTransform baseTF;
static Vector3 translation = Vector3::ZERO;
static Quaternion orientation = Quaternion::IDENTITY;
static Quaternion qRot = Quaternion(-sqrt(0.5), 0.0f, sqrt(0.5), 0.0f)*Quaternion(-sqrt(0.5), sqrt(0.5), 0.0f, 0.0f);
static Quaternion qYn90 = Quaternion(Degree(90), Vector3::NEGATIVE_UNIT_Y);
static tfScalar yaw,pitch,roll;
static Matrix3 mRot;
try {
tfListener->lookupTransform("map","base_footprint",ros::Time(0), baseTF);
translation.x = baseTF.getOrigin().x();
translation.y = baseTF.getOrigin().y();
translation.z = baseTF.getOrigin().z();
translation = qRot*translation + Vector3(0.0f, 1.0f, 0.0f);
baseTF.getBasis().getEulerYPR(yaw,pitch,roll);
mRot.FromEulerAnglesYXZ(Radian(yaw),Radian(0.0f),Radian(0.0f));
orientation.FromRotationMatrix(mRot);
orientation = qYn90*orientation;
robot->setPosition(translation);
robot->setOrientation(orientation);
} catch (tf::TransformException ex){
ROS_ERROR("%s",ex.what());
}
}
void RoadPositionSystem::PositionChanged(GameObject* object) {
auto road = object->GetComponent<Road>();
auto roadPosition = object->GetComponent<RoadPosition>();
auto transform = object->GetComponent<Transform>();
float width;
Matrix4x4 world = road->GetWorld(roadPosition->Position(), width);
transform->Position = world.TransformPosition(0);
Quaternion rot;
rot.FromRotationMatrix(world);
transform->Rotation = rot;
}
Quaternion OgreMaxUtilities::LoadRotation(const TiXmlElement* objectElement)
{
Quaternion rotation = Quaternion::IDENTITY;
if (objectElement->Attribute("qx") != 0)
{
//The rotation is specified as a quaternion
rotation.x = GetRealAttribute(objectElement, "qx", 0);
rotation.y = GetRealAttribute(objectElement, "qy", 0);
rotation.z = GetRealAttribute(objectElement, "qz", 0);
rotation.w = GetRealAttribute(objectElement, "qw", 0);
}
else if (objectElement->Attribute("axisX") != 0)
{
//The rotation is specified as an axis and angle
Real angle = GetRealAttribute(objectElement, "angle", 0);
Vector3 axis;
axis.x = GetRealAttribute(objectElement, "axisX", 0);
axis.y = GetRealAttribute(objectElement, "axisY", 0);
axis.z = GetRealAttribute(objectElement, "axisZ", 0);
//Convert the angle and axis into the rotation quaternion
rotation.FromAngleAxis(Radian(angle), axis);
}
else if (objectElement->Attribute("angleX") != 0)
{
//Assume the rotation is specified as three Euler angles
Vector3 euler;
euler.x = GetRealAttribute(objectElement, "angleX", 0);
euler.y = GetRealAttribute(objectElement, "angleY", 0);
euler.z = GetRealAttribute(objectElement, "angleZ", 0);
String order = GetStringAttribute(objectElement, "order");
//Convert Euler angles to a matrix
Matrix3 rotationMatrix;
if (order.length() < 2)
rotationMatrix.FromEulerAnglesXYZ(Radian(euler.x), Radian(euler.y), Radian(euler.z));
else
{
if (order[0] == 'x')
{
if (order[1] == 'y')
rotationMatrix.FromEulerAnglesXYZ(Radian(euler.x), Radian(euler.y), Radian(euler.z));
else
rotationMatrix.FromEulerAnglesXZY(Radian(euler.x), Radian(euler.y), Radian(euler.z));
}
else if (order[0] == 'y')
{
if (order[1] == 'x')
rotationMatrix.FromEulerAnglesYXZ(Radian(euler.x), Radian(euler.y), Radian(euler.z));
else
rotationMatrix.FromEulerAnglesYZX(Radian(euler.x), Radian(euler.y), Radian(euler.z));
}
else
{
if (order[1] == 'x')
rotationMatrix.FromEulerAnglesZXY(Radian(euler.x), Radian(euler.y), Radian(euler.z));
else
rotationMatrix.FromEulerAnglesZYX(Radian(euler.x), Radian(euler.y), Radian(euler.z));
}
}
//Convert the matrix into the rotation quaternion
rotation.FromRotationMatrix(rotationMatrix);
}
return rotation;
}