257 lines
9.0 KiB
C++
257 lines
9.0 KiB
C++
/*
|
|
#include "Spherical16.h"
|
|
|
|
#include "Quaternion.h"
|
|
#include "Spherical.h"
|
|
|
|
#include <math.h>
|
|
|
|
Spherical16::Spherical16() {
|
|
this->distance = 0.0f;
|
|
this->horizontalAngle = Angle16(0);
|
|
this->verticalAngle = Angle16(0);
|
|
}
|
|
|
|
Spherical16::Spherical16(Polar polar) {
|
|
this->distance = polar.distance;
|
|
this->horizontalAngle = Angle16(polar.angle.ToFloat());
|
|
this->verticalAngle = Angle16(0);
|
|
}
|
|
|
|
Spherical16::Spherical16(float distance,
|
|
Angle16 horizontalAngle,
|
|
Angle16 verticalAngle) {
|
|
if (distance < 0) {
|
|
this->distance = -distance;
|
|
this->horizontalAngle = horizontalAngle.ToFloat() - Angle16(180).ToFloat();
|
|
this->verticalAngle = verticalAngle;
|
|
} else {
|
|
this->distance = distance;
|
|
this->horizontalAngle = horizontalAngle;
|
|
this->verticalAngle = verticalAngle;
|
|
}
|
|
}
|
|
|
|
Spherical16::Spherical16(Vector3 v) {
|
|
this->distance = v.magnitude();
|
|
if (distance == 0.0f) {
|
|
this->verticalAngle = 0.0f;
|
|
this->horizontalAngle = 0.0f;
|
|
} else {
|
|
this->verticalAngle = (90.0f - acosf(v.Up() / this->distance) *
|
|
Passer::LinearAlgebra::Rad2Deg);
|
|
this->horizontalAngle =
|
|
atan2f(v.Right(), v.Forward()) * Passer::LinearAlgebra::Rad2Deg;
|
|
}
|
|
}
|
|
|
|
Vector3 Passer::LinearAlgebra::Spherical16::ToVector3() {
|
|
float verticalRad =
|
|
(90.0f - this->verticalAngle.ToFloat()) * Passer::LinearAlgebra::Deg2Rad;
|
|
float horizontalRad =
|
|
this->horizontalAngle.ToFloat() * Passer::LinearAlgebra::Deg2Rad;
|
|
float cosVertical = cosf(verticalRad);
|
|
float sinVertical = sinf(verticalRad);
|
|
float cosHorizontal = cosf(horizontalRad);
|
|
float sinHorizontal = sinf(horizontalRad);
|
|
|
|
float x = this->distance * sinVertical * sinHorizontal;
|
|
float y = this->distance * cosVertical;
|
|
float z = this->distance * sinVertical * cosHorizontal;
|
|
|
|
Vector3 v = Vector3(x, y, z);
|
|
return Vector3();
|
|
}
|
|
|
|
const Spherical16 Spherical16::zero = Spherical16(0.0f, 0.0f, 0.0f);
|
|
const Spherical16 Spherical16::forward = Spherical16(1.0f, 0.0f, 0.0f);
|
|
const Spherical16 Spherical16::back = Spherical16(1.0f, 180.0f, 0.0f);
|
|
const Spherical16 Spherical16::right = Spherical16(1.0f, 90.0f, 0.0f);
|
|
const Spherical16 Spherical16::left = Spherical16(1.0f, -90.0f, 0.0f);
|
|
const Spherical16 Spherical16::up = Spherical16(1.0f, 0.0f, 90.0f);
|
|
const Spherical16 Spherical16::down = Spherical16(1.0f, 0.0f, -90.0f);
|
|
|
|
bool Spherical16::operator==(const Spherical16& v) const {
|
|
return (this->distance == v.distance &&
|
|
this->horizontalAngle.ToFloat() == v.horizontalAngle.ToFloat() &&
|
|
this->verticalAngle.ToFloat() == v.verticalAngle.ToFloat());
|
|
}
|
|
|
|
Spherical16 Spherical16::Normalize(const Spherical16& v) {
|
|
Spherical16 r = Spherical16(1, v.horizontalAngle, v.verticalAngle);
|
|
return r;
|
|
}
|
|
Spherical16 Spherical16::normalized() const {
|
|
Spherical16 r = Spherical16(1, this->horizontalAngle, this->verticalAngle);
|
|
return r;
|
|
}
|
|
|
|
Spherical16 Spherical16::operator-() const {
|
|
Spherical16 v =
|
|
Spherical16(this->distance, this->horizontalAngle.ToFloat() + 180.0f,
|
|
this->verticalAngle.ToFloat() + 180.0f);
|
|
return v;
|
|
}
|
|
|
|
Spherical16 Spherical16::operator-(const Spherical16& s2) const {
|
|
Spherical thisSpherical =
|
|
Spherical(this->distance, this->horizontalAngle.ToFloat(),
|
|
this->verticalAngle.ToFloat());
|
|
Spherical spherical2 = Spherical(s2.distance, s2.horizontalAngle.ToFloat(),
|
|
s2.verticalAngle.ToFloat());
|
|
|
|
// let's do it the easy way...
|
|
Vector3 v1 = Vector3(thisSpherical);
|
|
Vector3 v2 = Vector3(spherical2);
|
|
Vector3 v = v1 - v2;
|
|
Spherical16 r = Spherical16(v);
|
|
return r;
|
|
}
|
|
Spherical16 Spherical16::operator-=(const Spherical16& v) {
|
|
*this = *this - v;
|
|
return *this;
|
|
}
|
|
|
|
Spherical16 Spherical16::operator+(const Spherical16& s2) const {
|
|
// let's do it the easy way...
|
|
Vector3 v1 =
|
|
Vector3(Spherical(this->distance, this->horizontalAngle.ToFloat(),
|
|
this->verticalAngle.ToFloat()));
|
|
Vector3 v2 = Vector3(Spherical(s2.distance, s2.horizontalAngle.ToFloat(),
|
|
s2.verticalAngle.ToFloat()));
|
|
Vector3 v = v1 + v2;
|
|
Spherical16 r = Spherical16(v);
|
|
return r;
|
|
|
|
// This is the hard way...
|
|
if (v2.distance <= 0)
|
|
return Spherical(this->distance, this->horizontalAngle,
|
|
this->verticalAngle);
|
|
if (this->distance <= 0)
|
|
return v2;
|
|
|
|
float deltaHorizontalAngle =
|
|
(float)Angle::Normalize(v2.horizontalAngle - this->horizontalAngle);
|
|
float horizontalRotation = deltaHorizontalAngle < 0
|
|
? 180 + deltaHorizontalAngle
|
|
: 180 - deltaHorizontalAngle;
|
|
float deltaVerticalAngle =
|
|
Angle::Normalize(v2.verticalAngle - this->verticalAngle);
|
|
float verticalRotation = deltaVerticalAngle < 0 ? 180 + deltaVerticalAngle
|
|
: 180 - deltaVerticalAngle;
|
|
|
|
if (horizontalRotation == 180 && verticalRotation == 180)
|
|
// angle is too small, take this angle and add the distances
|
|
return Spherical(this->distance + v2.distance, this->horizontalAngle,
|
|
this->verticalAngle);
|
|
|
|
Angle rotation = AngleBetween(*this, v2);
|
|
float newDistance =
|
|
Angle::CosineRuleSide(v2.distance, this->distance, rotation);
|
|
float angle =
|
|
Angle::CosineRuleAngle(newDistance, this->distance, v2.distance);
|
|
|
|
// Now we have to project the angle to the horizontal and vertical planes...
|
|
// The axis for the angle is the cross product of the two spherical vectors
|
|
// (which function we do not have either...)
|
|
float horizontalAngle = 0;
|
|
float verticalAngle = 0;
|
|
|
|
float newHorizontalAngle =
|
|
deltaHorizontalAngle < 0
|
|
? Angle::Normalize(this->horizontalAngle - horizontalAngle)
|
|
: Angle::Normalize(this->horizontalAngle + horizontalAngle);
|
|
float newVerticalAngle =
|
|
deltaVerticalAngle < 0
|
|
? Angle::Normalize(this->verticalAngle - verticalAngle)
|
|
: Angle::Normalize(this->verticalAngle + verticalAngle);
|
|
|
|
Spherical v = Spherical(newDistance, newHorizontalAngle, newVerticalAngle);
|
|
|
|
}
|
|
Spherical16 Spherical16::operator+=(const Spherical16& v) {
|
|
*this = *this + v;
|
|
return *this;
|
|
}
|
|
|
|
// Spherical Passer::LinearAlgebra::operator*(const Spherical &v, float f) {
|
|
// return Spherical(v.distance * f, v.horizontalAngle, v.verticalAngle);
|
|
// }
|
|
// Spherical Passer::LinearAlgebra::operator*(float f, const Spherical &v) {
|
|
// return Spherical(v.distance * f, v.horizontalAngle, v.verticalAngle);
|
|
// }
|
|
Spherical16 Spherical16::operator*=(float f) {
|
|
this->distance *= f;
|
|
return *this;
|
|
}
|
|
|
|
// Spherical Passer::LinearAlgebra::operator/(const Spherical &v, float f) {
|
|
// return Spherical(v.distance / f, v.horizontalAngle, v.verticalAngle);
|
|
// }
|
|
// Spherical Passer::LinearAlgebra::operator/(float f, const Spherical &v) {
|
|
// return Spherical(v.distance / f, v.horizontalAngle, v.verticalAngle);
|
|
// }
|
|
Spherical16 Spherical16::operator/=(float f) {
|
|
this->distance /= f;
|
|
return *this;
|
|
}
|
|
|
|
// float Spherical::GetSwing() {
|
|
// // Not sure if this is correct
|
|
// return sqrtf(horizontalAngle * horizontalAngle +
|
|
// verticalAngle * verticalAngle);
|
|
// }
|
|
|
|
// float Spherical::Distance(const Spherical &s1, const Spherical &s2) {
|
|
// float d = 0;
|
|
// return d;
|
|
// }
|
|
|
|
#include "AngleUsing.h"
|
|
#include "FloatSingle.h"
|
|
#include "Vector3.h"
|
|
|
|
const float epsilon = 1E-05f;
|
|
const float Rad2Deg = 57.29578F;
|
|
|
|
Angle Spherical16::AngleBetween(const Spherical16& v1, const Spherical16& v2) {
|
|
// float denominator = sqrtf(v1_3.sqrMagnitude() * v2_3.sqrMagnitude());
|
|
float denominator =
|
|
v1.distance * v2.distance; // sqrtf(v1.distance * v1.distance *
|
|
// v2.distance * v2.distance);
|
|
if (denominator < epsilon)
|
|
return 0.0f;
|
|
|
|
Vector3 v1_3 = Vector3(Spherical(v1.distance, v1.horizontalAngle.ToFloat(),
|
|
v1.verticalAngle.ToFloat()));
|
|
Vector3 v2_3 = Vector3(Spherical(v2.distance, v2.horizontalAngle.ToFloat(),
|
|
v2.verticalAngle.ToFloat()));
|
|
float dot = Vector3::Dot(v1_3, v2_3);
|
|
float fraction = dot / denominator;
|
|
if (isnan(fraction))
|
|
return fraction; // short cut to returning NaN universally
|
|
|
|
float cdot = Float::Clamp(fraction, -1.0, 1.0);
|
|
float r = ((float)acos(cdot)) * Rad2Deg;
|
|
return r;
|
|
}
|
|
|
|
Spherical16 Spherical16::Rotate(const Spherical16& v,
|
|
Angle horizontalAngle,
|
|
Angle verticalAngle) {
|
|
Spherical16 r = Spherical16(
|
|
v.distance, v.horizontalAngle.ToFloat() + horizontalAngle.ToFloat(),
|
|
v.verticalAngle.ToFloat() + verticalAngle.ToFloat());
|
|
return r;
|
|
}
|
|
Spherical16 Spherical16::RotateHorizontal(const Spherical16& v, Angle a) {
|
|
Spherical16 r = Spherical16(
|
|
v.distance, v.horizontalAngle.ToFloat() + a.ToFloat(), v.verticalAngle);
|
|
return r;
|
|
}
|
|
Spherical16 Spherical16::RotateVertical(const Spherical16& v, Angle a) {
|
|
Spherical16 r = Spherical16(v.distance, v.horizontalAngle,
|
|
v.verticalAngle.ToFloat() + a.ToFloat());
|
|
return r;
|
|
}
|
|
*/ |