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);
}
}
int GLWidget::setCenter(){
if(!isCenChanged){
return(0);
}
isCenChanged = false;
isFocusChanged = true;
curCen.x = BOXSIZE/2.0;
curCen.y = BOXSIZE/2.0;
curCen.z = BOXSIZE/2.0;
float d[3];
d[0] = curCen.x-lastCen.x;
d[1] = curCen.y-lastCen.y;
d[2] = curCen.z-lastCen.z;
lastCen.x = curCen.x;
lastCen.y = curCen.y;
lastCen.z = curCen.z;
yRotation(lastRotY, d);
xRotation(lastRotX, d);
curFocus.x = lastFocus.x+d[0];
curFocus.y = lastFocus.y+d[1];
curFocus.z = lastFocus.z+d[2];
lastFocus.x = curFocus.x;
lastFocus.y = curFocus.y;
lastFocus.z = curFocus.z;
return(0);
}
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 GraphicRootTracker::evaluate()
{
if (!IsConnected()) return;
double alpha = gc::Pi - GetAzimuth();
SbVec3f yAxis( 0.0, 1.0, 0.0 );
SbRotation yRotation( yAxis, alpha );
SbVec3f xAxis( 1.0, 0.0, 0.0 );
SbRotation xRotation( xAxis, GetZenith() );
SbRotation rotation = xRotation * yRotation;
SetEngineOutputRotation(rotation);
}
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 TSceneTracker::evaluate()
{
if (!IsConnected()) return;
SetAnglesToScene();
double alpha = gc::Pi - GetAzimuth();
SbVec3f yAxis( 0.0, 1.0, 0.0 );
SbRotation yRotation( yAxis, -alpha );
SbVec3f xAxis( 1.0, 0.0, 0.0 );
SbRotation xRotation( xAxis, -GetZenith() );
SbRotation rotation = yRotation * xRotation;
SetEngineOutputRotation(rotation);
}
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;
}
void Player::MouseMove(int x, int y)
{
double deltaX = -(x - _resolution.x / 2) / _resolution.x * PI * 4;
double deltaY = -(y - _resolution.y / 2) / _resolution.y * PI;
Mat3 xRotation(cos(deltaX), -sin(deltaX), 0,
sin(deltaX), cos(deltaX), 0,
0, 0, 1);
_lookat = xRotation * Vec3(-1, -1, 0);
_lookat.normalize();
Vec3 rn = Vec3(0, 0, 1).cross(_lookat); //rotate axis
Mat3 yRotation(rn.x*rn.x*(1 - cos(deltaY)) + cos(deltaY), rn.x*rn.y*(1 - cos(deltaY)) + rn.z*sin(deltaY), rn.x*rn.z*(1 - cos(deltaY)) - rn.y*sin(deltaY),
rn.x*rn.y*(1 - cos(deltaY)) - rn.z*sin(deltaY), rn.y*rn.y*(1 - cos(deltaY)) + cos(deltaY), rn.y*rn.z*(1 - cos(deltaY)) + rn.x*sin(deltaY),
rn.x*rn.z*(1 - cos(deltaY)) + rn.y*sin(deltaY), rn.y*rn.z*(1 - cos(deltaY)) - rn.x*sin(deltaY), rn.z*rn.z*(1 - cos(deltaY)) + cos(deltaY));
_lookat = yRotation * _lookat;
_lookat.normalize();
_lookChanged = true;
}
int GLWidget::setFocus(){
if(!(isFocusChanged)){
return(0);
}
isFocusChanged = false;
isCenChanged = true;
curFocus.x = lookAt.x;
curFocus.y = lookAt.y;
curFocus.z = lookAt.z-0.1;
float d[3];
d[0] = curFocus.x-lastFocus.x;
d[1] = curFocus.y-lastFocus.y;
d[2] = curFocus.z-lastFocus.z;
lastFocus.x = curFocus.x;
lastFocus.y = curFocus.y;
lastFocus.z = curFocus.z;
xRotation(-lastRotX, d);
yRotation(-lastRotY, d);
// zRotation(-lastRotZ, d);
curCen.x = lastCen.x+d[0];
curCen.y = lastCen.y+d[1];
curCen.z = lastCen.z+d[2];
lastCen.x = curCen.x;
lastCen.y = curCen.y;
lastCen.z = curCen.z;
return(0);
}
//=============================================================
// 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;
}
}
