mirror of
https://github.com/copyrighttxt/watrbx-game-engine.git
synced 2026-09-05 05:07:48 +00:00
GEEKING
This commit is contained in:
watrabi
@@ -0,0 +1,469 @@
|
||||
/**
|
||||
@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/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::fromXYZYPRRadians(float x, float y, float z, float yaw,
|
||||
float pitch, float roll) {
|
||||
Matrix3 rotation = Matrix3::fromAxisAngleFast(Vector3::unitY(), yaw);
|
||||
|
||||
rotation = Matrix3::fromAxisAngleFast(rotation.column(0), pitch) * rotation;
|
||||
rotation = Matrix3::fromAxisAngleFast(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;
|
||||
}
|
||||
|
||||
// ROBLOX
|
||||
bool CoordinateFrame::fuzzyEq(const CoordinateFrame& other, double absepsilon) 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], absepsilon)) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
if (! G3D::fuzzyEq(translation[c], other.translation[c], absepsilon)) {
|
||||
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);
|
||||
}
|
||||
|
||||
//ROBLOX
|
||||
#if 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);
|
||||
}
|
||||
#else
|
||||
Plane CoordinateFrame::toObjectSpace(const Plane& p) const
|
||||
{
|
||||
return Plane( normalToObjectSpace(p.normal()),
|
||||
pointToObjectSpace(p.normal() * p.distance()) );
|
||||
}
|
||||
|
||||
|
||||
Plane CoordinateFrame::toWorldSpace(const Plane& p) const {
|
||||
return Plane( normalToWorldSpace(p.normal()),
|
||||
pointToWorldSpace(p.normal() * p.distance()) );
|
||||
}
|
||||
#endif
|
||||
// =============
|
||||
|
||||
Triangle CoordinateFrame::toObjectSpace(const Triangle& t) const {
|
||||
return Triangle(pointToObjectSpace(t.vertex(0)),
|
||||
pointToObjectSpace(t.vertex(1)),
|
||||
pointToObjectSpace(t.vertex(2)));
|
||||
}
|
||||
|
||||
//ROBLOX
|
||||
Line CoordinateFrame::toWorldSpace(const Line& l) const {
|
||||
return Line::fromPointAndUnitDirection(pointToWorldSpace(l.point()), vectorToWorldSpace(l.direction()));
|
||||
}
|
||||
//========
|
||||
|
||||
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));
|
||||
}
|
||||
|
||||
AABox CoordinateFrame::AABBtoWorldSpace(const AABox& b) const {
|
||||
Vector3 center = b.center();
|
||||
Vector3 halfSize = b.extent()*0.5f;
|
||||
|
||||
Vector3 newCenter = pointToWorldSpace(center);
|
||||
Vector3 newHalfSize = Vector3(
|
||||
fabs(rotation[0][0]) * halfSize[0] + fabs(rotation[0][1]) * halfSize[1] + fabs(rotation[0][2]) * halfSize[2],
|
||||
fabs(rotation[1][0]) * halfSize[0] + fabs(rotation[1][1]) * halfSize[1] + fabs(rotation[1][2]) * halfSize[2],
|
||||
fabs(rotation[2][0]) * halfSize[0] + fabs(rotation[2][1]) * halfSize[1] + fabs(rotation[2][2]) * halfSize[2]);
|
||||
|
||||
return AABox(newCenter - newHalfSize, newCenter + newHalfSize);
|
||||
}
|
||||
|
||||
AABox CoordinateFrame::AABBtoObjectSpace(const AABox& b) const {
|
||||
Vector3 center = b.center();
|
||||
Vector3 halfSize = b.extent()*0.5f;
|
||||
|
||||
Vector3 newCenter = pointToObjectSpace(center);
|
||||
Vector3 newHalfSize = Vector3(
|
||||
fabs(rotation[0][0]) * halfSize[0] + fabs(rotation[1][0]) * halfSize[1] + fabs(rotation[2][0]) * halfSize[2],
|
||||
fabs(rotation[0][1]) * halfSize[0] + fabs(rotation[1][1]) * halfSize[1] + fabs(rotation[2][1]) * halfSize[2],
|
||||
fabs(rotation[0][2]) * halfSize[0] + fabs(rotation[1][2]) * halfSize[1] + fabs(rotation[2][2]) * halfSize[2]);
|
||||
|
||||
return AABox(newCenter - newHalfSize, newCenter + newHalfSize);
|
||||
}
|
||||
|
||||
|
||||
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()));
|
||||
}
|
||||
|
||||
RBX::RbxRay CoordinateFrame::toObjectSpace(const RBX::RbxRay& r) const {
|
||||
return RBX::RbxRay::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);
|
||||
}
|
||||
}
|
||||
|
||||
CoordinateFrame CoordinateFrame::nlerp(
|
||||
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.nlerp(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]);
|
||||
}
|
||||
}
|
||||
|
||||
btTransform CoordinateFrame::transformFromCFrame() const
|
||||
{
|
||||
btMatrix3x3 rot(
|
||||
rotation[0][0], rotation[0][1], rotation[0][2],
|
||||
rotation[1][0], rotation[1][1], rotation[1][2],
|
||||
rotation[2][0], rotation[2][1], rotation[2][2]);
|
||||
|
||||
return btTransform(rot, btVector3(translation.x, translation.y, translation.z));
|
||||
}
|
||||
|
||||
} // namespace
|
||||
Reference in New Issue
Block a user