mirror of
https://github.com/mod-playerbots/azerothcore-wotlk.git
synced 2025-11-29 17:38:24 +08:00
466 lines
12 KiB
C++
466 lines
12 KiB
C++
/**
|
|
@file CoordinateFrame.cpp
|
|
|
|
Coordinate frame class
|
|
|
|
@maintainer Morgan McGuire, http://graphics.cs.williams.edu
|
|
|
|
@created 2001-06-02
|
|
@edited 2010-03-13
|
|
|
|
Copyright 2000-2010, Morgan McGuire.
|
|
All rights reserved.
|
|
*/
|
|
|
|
#include "G3D/platform.h"
|
|
#include "G3D/CoordinateFrame.h"
|
|
#include "G3D/Quat.h"
|
|
#include "G3D/Matrix4.h"
|
|
#include "G3D/Box.h"
|
|
#include "G3D/AABox.h"
|
|
#include "G3D/Sphere.h"
|
|
#include "G3D/Triangle.h"
|
|
#include "G3D/Ray.h"
|
|
#include "G3D/Capsule.h"
|
|
#include "G3D/Cylinder.h"
|
|
#include "G3D/UprightFrame.h"
|
|
#include "G3D/Any.h"
|
|
#include "G3D/stringutils.h"
|
|
#include "G3D/PhysicsFrame.h"
|
|
#include "G3D/UprightFrame.h"
|
|
|
|
namespace G3D {
|
|
|
|
|
|
std::string CoordinateFrame::toXYZYPRDegreesString() const {
|
|
UprightFrame uframe(*this);
|
|
|
|
return format("CFrame::fromXYZYPRDegrees(% 5.1ff, % 5.1ff, % 5.1ff, % 5.1ff, % 5.1ff, % 5.1ff)",
|
|
uframe.translation.x, uframe.translation.y, uframe.translation.z,
|
|
toDegrees(uframe.yaw), toDegrees(uframe.pitch), 0.0f);
|
|
}
|
|
|
|
|
|
CoordinateFrame::CoordinateFrame(const Any& any) {
|
|
*this = CFrame();
|
|
|
|
const std::string& n = toUpper(any.name());
|
|
|
|
if (beginsWith(n, "VECTOR3")) {
|
|
translation = any;
|
|
} else if (beginsWith(n, "MATRIX3")) {
|
|
rotation = any;
|
|
} else if ((n == "CFRAME") || (n == "COORDINATEFRAME")) {
|
|
any.verifyType(Any::TABLE, Any::ARRAY);
|
|
if (any.type() == Any::ARRAY) {
|
|
any.verifySize(2);
|
|
rotation = any[0];
|
|
translation = any[1];
|
|
} else {
|
|
for (Any::AnyTable::Iterator it = any.table().begin(); it.hasMore(); ++it) {
|
|
const std::string& n = toLower(it->key);
|
|
if (n == "translation") {
|
|
translation = Vector3(it->value);
|
|
} else if (n == "rotation") {
|
|
rotation = Matrix3(it->value);
|
|
} else {
|
|
any.verify(false, "Illegal table key: " + it->key);
|
|
}
|
|
}
|
|
}
|
|
} else if (beginsWith(n, "PHYSICSFRAME") || beginsWith(n, "PFRAME")) {
|
|
*this = PhysicsFrame(any);
|
|
} else {
|
|
any.verifyName("CFrame::fromXYZYPRDegrees", "CoordinateFrame::fromXYZYPRDegrees");
|
|
any.verifyType(Any::ARRAY);
|
|
any.verifySize(3, 6);
|
|
|
|
int s = any.size();
|
|
|
|
*this = fromXYZYPRDegrees(any[0], any[1], any[2],
|
|
(s > 3) ? any[3].number() : 0.0f,
|
|
(s > 4) ? any[4].number() : 0.0f,
|
|
(s > 5) ? any[5].number() : 0.0f);
|
|
}
|
|
}
|
|
|
|
|
|
CoordinateFrame::operator Any() const {
|
|
float x, y, z, yaw, pitch, roll;
|
|
getXYZYPRDegrees(x, y, z, yaw, pitch, roll);
|
|
Any a(Any::ARRAY, "CFrame::fromXYZYPRDegrees");
|
|
a.append(x, y, z, yaw);
|
|
if ( ! G3D::fuzzyEq(yaw, 0.0f) || ! G3D::fuzzyEq(pitch, 0.0f) || ! G3D::fuzzyEq(roll, 0.0f)) {
|
|
a.append(yaw);
|
|
if (! G3D::fuzzyEq(pitch, 0.0f) || ! G3D::fuzzyEq(roll, 0.0f)) {
|
|
a.append(pitch);
|
|
if (! G3D::fuzzyEq(roll, 0.0f)) {
|
|
a.append(roll);
|
|
}
|
|
}
|
|
}
|
|
return a;
|
|
}
|
|
|
|
|
|
CoordinateFrame::CoordinateFrame(const class UprightFrame& f) {
|
|
*this = f.toCoordinateFrame();
|
|
}
|
|
|
|
|
|
CoordinateFrame::CoordinateFrame() :
|
|
rotation(Matrix3::identity()), translation(Vector3::zero()) {
|
|
}
|
|
|
|
CoordinateFrame CoordinateFrame::fromXYZYPRRadians(float x, float y, float z, float yaw,
|
|
float pitch, float roll) {
|
|
Matrix3 rotation = Matrix3::fromAxisAngle(Vector3::unitY(), yaw);
|
|
|
|
rotation = Matrix3::fromAxisAngle(rotation.column(0), pitch) * rotation;
|
|
rotation = Matrix3::fromAxisAngle(rotation.column(2), roll) * rotation;
|
|
|
|
const Vector3 translation(x, y, z);
|
|
|
|
return CoordinateFrame(rotation, translation);
|
|
}
|
|
|
|
|
|
void CoordinateFrame::getXYZYPRRadians(float& x, float& y, float& z,
|
|
float& yaw, float& pitch, float& roll) const {
|
|
x = translation.x;
|
|
y = translation.y;
|
|
z = translation.z;
|
|
|
|
const Vector3& look = lookVector();
|
|
|
|
if (abs(look.y) > 0.99f) {
|
|
// Looking nearly straight up or down
|
|
|
|
yaw = G3D::pi() + atan2(look.x, look.z);
|
|
pitch = asin(look.y);
|
|
roll = 0.0f;
|
|
|
|
} else {
|
|
|
|
// Yaw cannot be affected by others, so pull it first
|
|
yaw = G3D::pi() + atan2(look.x, look.z);
|
|
|
|
// Pitch is the elevation of the yaw vector
|
|
pitch = asin(look.y);
|
|
|
|
Vector3 actualRight = rightVector();
|
|
Vector3 expectedRight = look.cross(Vector3::unitY());
|
|
|
|
roll = 0;//acos(actualRight.dot(expectedRight)); TODO
|
|
}
|
|
}
|
|
|
|
|
|
void CoordinateFrame::getXYZYPRDegrees(float& x, float& y, float& z,
|
|
float& yaw, float& pitch, float& roll) const {
|
|
getXYZYPRRadians(x, y, z, yaw, pitch, roll);
|
|
yaw = toDegrees(yaw);
|
|
pitch = toDegrees(pitch);
|
|
roll = toDegrees(roll);
|
|
}
|
|
|
|
|
|
CoordinateFrame CoordinateFrame::fromXYZYPRDegrees(float x, float y, float z,
|
|
float yaw, float pitch, float roll) {
|
|
return fromXYZYPRRadians(x, y, z, toRadians(yaw), toRadians(pitch), toRadians(roll));
|
|
}
|
|
|
|
|
|
Ray CoordinateFrame::lookRay() const {
|
|
return Ray::fromOriginAndDirection(translation, lookVector());
|
|
}
|
|
|
|
|
|
bool CoordinateFrame::fuzzyEq(const CoordinateFrame& other) const {
|
|
|
|
for (int c = 0; c < 3; ++c) {
|
|
for (int r = 0; r < 3; ++r) {
|
|
if (! G3D::fuzzyEq(other.rotation[r][c], rotation[r][c])) {
|
|
return false;
|
|
}
|
|
}
|
|
if (! G3D::fuzzyEq(translation[c], other.translation[c])) {
|
|
return false;
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
|
|
bool CoordinateFrame::fuzzyIsIdentity() const {
|
|
const Matrix3& I = Matrix3::identity();
|
|
|
|
for (int c = 0; c < 3; ++c) {
|
|
for (int r = 0; r < 3; ++r) {
|
|
if (fuzzyNe(I[r][c], rotation[r][c])) {
|
|
return false;
|
|
}
|
|
}
|
|
if (fuzzyNe(translation[c], 0)) {
|
|
return false;
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
|
|
bool CoordinateFrame::isIdentity() const {
|
|
return
|
|
(translation == Vector3::zero()) &&
|
|
(rotation == Matrix3::identity());
|
|
}
|
|
|
|
|
|
Matrix4 CoordinateFrame::toMatrix4() const {
|
|
return Matrix4(*this);
|
|
}
|
|
|
|
|
|
std::string CoordinateFrame::toXML() const {
|
|
return G3D::format(
|
|
"<COORDINATEFRAME>\n %lf,%lf,%lf,%lf,\n %lf,%lf,%lf,%lf,\n %lf,%lf,%lf,%lf,\n %lf,%lf,%lf,%lf\n</COORDINATEFRAME>\n",
|
|
rotation[0][0], rotation[0][1], rotation[0][2], translation.x,
|
|
rotation[1][0], rotation[1][1], rotation[1][2], translation.y,
|
|
rotation[2][0], rotation[2][1], rotation[2][2], translation.z,
|
|
0.0, 0.0, 0.0, 1.0);
|
|
}
|
|
|
|
|
|
Plane CoordinateFrame::toObjectSpace(const Plane& p) const {
|
|
Vector3 N, P;
|
|
double d;
|
|
p.getEquation(N, d);
|
|
P = N * (float)d;
|
|
P = pointToObjectSpace(P);
|
|
N = normalToObjectSpace(N);
|
|
return Plane(N, P);
|
|
}
|
|
|
|
|
|
Plane CoordinateFrame::toWorldSpace(const Plane& p) const {
|
|
Vector3 N, P;
|
|
double d;
|
|
p.getEquation(N, d);
|
|
P = N * (float)d;
|
|
P = pointToWorldSpace(P);
|
|
N = normalToWorldSpace(N);
|
|
return Plane(N, P);
|
|
}
|
|
|
|
|
|
Triangle CoordinateFrame::toObjectSpace(const Triangle& t) const {
|
|
return Triangle(pointToObjectSpace(t.vertex(0)),
|
|
pointToObjectSpace(t.vertex(1)),
|
|
pointToObjectSpace(t.vertex(2)));
|
|
}
|
|
|
|
|
|
Triangle CoordinateFrame::toWorldSpace(const Triangle& t) const {
|
|
return Triangle(pointToWorldSpace(t.vertex(0)),
|
|
pointToWorldSpace(t.vertex(1)),
|
|
pointToWorldSpace(t.vertex(2)));
|
|
}
|
|
|
|
|
|
Cylinder CoordinateFrame::toWorldSpace(const Cylinder& c) const {
|
|
return Cylinder(
|
|
pointToWorldSpace(c.point(0)),
|
|
pointToWorldSpace(c.point(1)),
|
|
c.radius());
|
|
}
|
|
|
|
|
|
Capsule CoordinateFrame::toWorldSpace(const Capsule& c) const {
|
|
return Capsule(
|
|
pointToWorldSpace(c.point(0)),
|
|
pointToWorldSpace(c.point(1)),
|
|
c.radius());
|
|
}
|
|
|
|
|
|
Box CoordinateFrame::toWorldSpace(const AABox& b) const {
|
|
Box b2(b);
|
|
return toWorldSpace(b2);
|
|
}
|
|
|
|
|
|
Box CoordinateFrame::toWorldSpace(const Box& b) const {
|
|
Box out(b);
|
|
|
|
for (int i = 0; i < 8; ++i) {
|
|
out._corner[i] = pointToWorldSpace(b._corner[i]);
|
|
debugAssert(! isNaN(out._corner[i].x));
|
|
}
|
|
|
|
for (int i = 0; i < 3; ++i) {
|
|
out._axis[i] = vectorToWorldSpace(b._axis[i]);
|
|
}
|
|
|
|
out._center = pointToWorldSpace(b._center);
|
|
|
|
return out;
|
|
}
|
|
|
|
|
|
Box CoordinateFrame::toObjectSpace(const Box &b) const {
|
|
return inverse().toWorldSpace(b);
|
|
}
|
|
|
|
|
|
Box CoordinateFrame::toObjectSpace(const AABox& b) const {
|
|
return toObjectSpace(Box(b));
|
|
}
|
|
|
|
|
|
CoordinateFrame::CoordinateFrame(class BinaryInput& b) : rotation(Matrix3::zero()) {
|
|
deserialize(b);
|
|
}
|
|
|
|
|
|
void CoordinateFrame::deserialize(class BinaryInput& b) {
|
|
rotation.deserialize(b);
|
|
translation.deserialize(b);
|
|
}
|
|
|
|
|
|
void CoordinateFrame::serialize(class BinaryOutput& b) const {
|
|
rotation.serialize(b);
|
|
translation.serialize(b);
|
|
}
|
|
|
|
|
|
Sphere CoordinateFrame::toWorldSpace(const Sphere &b) const {
|
|
return Sphere(pointToWorldSpace(b.center), b.radius);
|
|
}
|
|
|
|
|
|
Sphere CoordinateFrame::toObjectSpace(const Sphere &b) const {
|
|
return Sphere(pointToObjectSpace(b.center), b.radius);
|
|
}
|
|
|
|
|
|
Ray CoordinateFrame::toWorldSpace(const Ray& r) const {
|
|
return Ray::fromOriginAndDirection(pointToWorldSpace(r.origin()), vectorToWorldSpace(r.direction()));
|
|
}
|
|
|
|
|
|
Ray CoordinateFrame::toObjectSpace(const Ray& r) const {
|
|
return Ray::fromOriginAndDirection(pointToObjectSpace(r.origin()), vectorToObjectSpace(r.direction()));
|
|
}
|
|
|
|
|
|
void CoordinateFrame::lookAt(const Vector3 &target) {
|
|
lookAt(target, Vector3::unitY());
|
|
}
|
|
|
|
|
|
void CoordinateFrame::lookAt(
|
|
const Vector3& target,
|
|
Vector3 up) {
|
|
|
|
up = up.direction();
|
|
|
|
Vector3 look = (target - translation).direction();
|
|
if (fabs(look.dot(up)) > .99f) {
|
|
up = Vector3::unitX();
|
|
if (fabs(look.dot(up)) > .99f) {
|
|
up = Vector3::unitY();
|
|
}
|
|
}
|
|
|
|
up -= look * look.dot(up);
|
|
up.unitize();
|
|
|
|
Vector3 z = -look;
|
|
Vector3 x = -z.cross(up);
|
|
x.unitize();
|
|
|
|
Vector3 y = z.cross(x);
|
|
|
|
rotation.setColumn(0, x);
|
|
rotation.setColumn(1, y);
|
|
rotation.setColumn(2, z);
|
|
}
|
|
|
|
|
|
CoordinateFrame CoordinateFrame::lerp(
|
|
const CoordinateFrame& other,
|
|
float alpha) const {
|
|
|
|
if (alpha == 1.0f) {
|
|
return other;
|
|
} else if (alpha == 0.0f) {
|
|
return *this;
|
|
} else {
|
|
const Quat q1(this->rotation);
|
|
const Quat q2(other.rotation);
|
|
|
|
return CoordinateFrame(
|
|
q1.slerp(q2, alpha).toRotationMatrix(),
|
|
translation * (1 - alpha) + other.translation * alpha);
|
|
}
|
|
}
|
|
|
|
|
|
void CoordinateFrame::pointToWorldSpace(const Array<Vector3>& v, Array<Vector3>& vout) const {
|
|
vout.resize(v.size());
|
|
|
|
for (int i = 0; i < v.size(); ++i) {
|
|
vout[i] = pointToWorldSpace(v[i]);
|
|
}
|
|
}
|
|
|
|
|
|
void CoordinateFrame::normalToWorldSpace(const Array<Vector3>& v, Array<Vector3>& vout) const {
|
|
vout.resize(v.size());
|
|
|
|
for (int i = 0; i < v.size(); ++i) {
|
|
vout[i] = normalToWorldSpace(v[i]);
|
|
}
|
|
}
|
|
|
|
|
|
void CoordinateFrame::vectorToWorldSpace(const Array<Vector3>& v, Array<Vector3>& vout) const {
|
|
vout.resize(v.size());
|
|
|
|
for (int i = v.size() - 1; i >= 0; --i) {
|
|
vout[i] = vectorToWorldSpace(v[i]);
|
|
}
|
|
}
|
|
|
|
|
|
void CoordinateFrame::pointToObjectSpace(const Array<Vector3>& v, Array<Vector3>& vout) const {
|
|
vout.resize(v.size());
|
|
|
|
for (int i = v.size() - 1; i >= 0; --i) {
|
|
vout[i] = pointToObjectSpace(v[i]);
|
|
}
|
|
}
|
|
|
|
|
|
void CoordinateFrame::normalToObjectSpace(const Array<Vector3>& v, Array<Vector3>& vout) const {
|
|
vout.resize(v.size());
|
|
|
|
for (int i = v.size() - 1; i >= 0; --i) {
|
|
vout[i] = normalToObjectSpace(v[i]);
|
|
}
|
|
}
|
|
|
|
|
|
void CoordinateFrame::vectorToObjectSpace(const Array<Vector3>& v, Array<Vector3>& vout) const {
|
|
vout.resize(v.size());
|
|
|
|
for (int i = v.size() - 1; i >= 0; --i) {
|
|
vout[i] = vectorToObjectSpace(v[i]);
|
|
}
|
|
}
|
|
|
|
} // namespace
|