const Quatf Entity::rotation() const {
const RotationInfo info = rotationInfo();
switch (info.type) {
case RTZAngle: {
const PropertyValue* angleValue = propertyForKey(info.property);
if (angleValue == NULL)
return Quatf(0.0f, Vec3f::PosZ);
float angle = static_cast<float>(std::atof(angleValue->c_str()));
return Quatf(Math<float>::radians(angle), Vec3f::PosZ);
}
case RTZAngleWithUpDown: {
const PropertyValue* angleValue = propertyForKey(info.property);
if (angleValue == NULL)
return Quatf(0.0f, Vec3f::PosZ);
float angle = static_cast<float>(std::atof(angleValue->c_str()));
if (angle == -1.0f)
return Quatf(-Math<float>::Pi / 2.0f, Vec3f::PosY);
if (angle == -2.0f)
return Quatf(Math<float>::Pi / 2.0f, Vec3f::PosY);
return Quatf(Math<float>::radians(angle), Vec3f::PosZ);
}
case RTEulerAngles: {
const PropertyValue* angleValue = propertyForKey(info.property);
Vec3f angles = angleValue != NULL ? Vec3f(*angleValue) : Vec3f::Null;
Quatf zRotation(Math<float>::radians( angles.x()), Vec3f::PosZ);
Quatf yRotation(Math<float>::radians(-angles.y()), Vec3f::PosY);
return zRotation * yRotation;
}
default:
return Quatf(0.0f, Vec3f::PosZ);
}
}
void Camera::Rotate(const float xAmount, const float yAmount, const float zAmount)
{
quat xRotation(DegToRad(xAmount), m_right.x, m_right.y, m_right.z);
quat yRotation(DegToRad(yAmount), m_up.x, m_up.y, m_up.z);
quat zRotation(DegToRad(zAmount), m_facing.x, m_facing.y, m_facing.z);
quat rotation = xRotation * yRotation * zRotation;
m_right = normalize(rotation * m_right);
m_up = normalize(rotation * m_up);
m_facing = normalize(rotation * m_facing);
}
void Ring::buildModules(EndcapModule* templ, int numMods, double smallDelta) {
double alignmentRotation = alignEdges() ? 0.5 : 0.;
for (int i = 0, parity = smallParity(); i < numMods; i++, parity *= -1) {
EndcapModule* mod = GeometryFactory::clone(*templ);
mod->myid(i+1);
mod->rotateZ(2*M_PI*(i+alignmentRotation)/numMods); // CUIDADO had a rotation offset of PI/2
mod->rotateZ(zRotation());
mod->translateZ(parity*smallDelta);
mod->flipped(parity != 1);
modules_.push_back(mod);
}
}
Matrix3 Matrix3::rotation(float x, float y, float z)
{
Matrix3 xRotation(
1, 0, 0,
0, cos(x), sin(x),
0, -sin(x), cos(x));
Matrix3 yRotation(
cos(y), 0, -sin(y),
0, 1, 0,
sin(y), 0, cos(y));
Matrix3 zRotation(
cos(z), sin(z), 0,
-sin(z), cos(z), 0,
0, 0, 1);
return xRotation * yRotation * zRotation;
}
void Camera::RotateAroundPoint(const float xAmount, const float yAmount, const float zAmount)
{
quat xRotation(DegToRad(xAmount), m_right.x, m_right.y, m_right.z);
quat yRotation(DegToRad(yAmount), m_up.x, m_up.y, m_up.z);
quat zRotation(DegToRad(zAmount), m_facing.x, m_facing.y, m_facing.z);
quat rotation = xRotation * yRotation * zRotation;
// Get the view position, based on the facing and the zoom amount
vec3 view = m_position + (m_facing*m_zoomAmount);
m_position -= view; // Translate the position to the origin, relative to the view position (that is the facing zoomed)
m_position = (rotation * m_position);
m_position += view; // Translate back to relative view position
m_right = normalize(rotation * m_right);
m_facing = normalize(rotation * m_facing);
m_up = normalize(rotation * m_up);
}
Matrix4 Matrix4::rotation(float x, float y, float z)
{
Matrix4 xRotation(
1, 0, 0, 0,
0, cos(x), sin(x), 0,
0, -sin(x), cos(x), 0,
0, 0, 0, 1);
Matrix4 yRotation(
cos(y), 0, -sin(y), 0,
0, 1, 0, 0,
sin(y), 0, cos(y), 0,
0, 0, 0, 1);
Matrix4 zRotation(
cos(z), sin(z), 0, 0,
-sin(z), cos(z), 0, 0,
0, 0, 1, 0,
0, 0, 0, 1);
return xRotation * yRotation * zRotation;
}
//=============================================================
// rpyRotation
//=============================================================
Transform rpyRotation(double roll, double pitch, double yaw)
{
Transform rpyRotation;
rpyRotation = (zRotation(roll) * yRotation(pitch)) * xRotation(yaw);
return rpyRotation;
}
void Entity::applyRotation(const Mat4f& rotation) {
const RotationInfo info = rotationInfo();
switch (info.type) {
case RTZAngle: {
const PropertyValue* angleValue = propertyForKey(info.property);
float angle = angleValue != NULL ? static_cast<float>(std::atof(angleValue->c_str())) : 0.0f;
Vec3f direction;
direction[0] = std::cos(Math<float>::radians(angle));
direction[1] = std::sin(Math<float>::radians(angle));
direction = rotation * direction;
direction[2] = 0.0f;
direction.normalize();
angle = Math<float>::round(Math<float>::degrees(std::acos(direction.x())));
if (direction.y() < 0.0f)
angle = 360.0f - angle;
setProperty(info.property, angle, true);
break;
}
case RTZAngleWithUpDown: {
const PropertyValue* angleValue = propertyForKey(info.property);
float angle = angleValue != NULL ? static_cast<float>(std::atof(angleValue->c_str())) : 0.0f;
Vec3f direction;
if (angle == -1.0f) {
direction = Vec3f::PosZ;
} else if (angle == -2.0f) {
direction = Vec3f::NegZ;
} else {
direction[0] = std::cos(Math<float>::radians(angle));
direction[1] = std::sin(Math<float>::radians(angle));
}
direction = rotation * direction;
direction.normalize();
if (direction.z() > 0.9f) {
setProperty(info.property, -1.0f, true);
} else if (direction.z() < -0.9f) {
setProperty(info.property, -2.0f, true);
} else {
direction[2] = 0.0f;
direction.normalize();
angle = Math<float>::round(Math<float>::degrees(std::acos(direction.x())));
if (direction.y() < 0.0f)
angle = 360.0f - angle;
while (angle < 0.0f)
angle += 360.0f;
setProperty(info.property, angle, true);
}
break;
}
case RTEulerAngles: {
const PropertyValue* angleValue = propertyForKey(info.property);
Vec3f angles = angleValue != NULL ? Vec3f(*angleValue) : Vec3f::Null;
Vec3f direction = Vec3f::PosX;
Quatf zRotation(Math<float>::radians(angles.x()), Vec3f::PosZ);
Quatf yRotation(Math<float>::radians(-angles.y()), Vec3f::PosY);
direction = zRotation * yRotation * direction;
direction = rotation * direction;
direction.normalize();
float zAngle, xAngle;
// FIXME: this is still buggy
Vec3f xyDirection = direction;
if (std::abs(xyDirection.z()) == 1.0f) {
zAngle = 0.0f;
} else {
xyDirection[2] = 0.0f;
xyDirection.normalize();
zAngle = Math<float>::round(Math<float>::degrees(std::acos(xyDirection.x())));
if (xyDirection.y() < 0.0f)
zAngle = 360.0f - zAngle;
}
Vec3f xzDirection = direction;
if (std::abs(xzDirection.y()) == 1.0f) {
xAngle = 0.0f;
} else {
xzDirection[1] = 0.0f;
xzDirection.normalize();
xAngle = Math<float>::round(Math<float>::degrees(std::acos(xzDirection.x())));
if (xzDirection.z() < 0.0f)
xAngle = 360.0f - xAngle;
}
angles = Vec3f(zAngle, xAngle, 0.0f);
setProperty(info.property, angles, true);
break;
}
default:
break;
}
}
