diff --git a/Angle.cpp b/Angle.cpp index 5b71d0d..00ea7cb 100644 --- a/Angle.cpp +++ b/Angle.cpp @@ -76,9 +76,8 @@ AngleOf AngleOf::Degrees(float angle) { template <> AngleOf AngleOf::Radians(float angle) { - if (!isfinite(angle)) { + if (!isfinite(angle)) return AngleOf(0); - } // map float [-PI..PI) to integer [-32768..32767] signed short value = (signed short)(angle / pi * 32768.0F); @@ -130,6 +129,13 @@ float AngleOf::InRadians() const { //===== Generic +template +const AngleOf AngleOf::zero = AngleOf(); +template +const AngleOf AngleOf::deg90 = AngleOf::Degrees(90); +template +const AngleOf AngleOf::deg180 = AngleOf::Degrees(180); + template bool AngleOf::operator==(const AngleOf a) const { return this->value == a.value; @@ -267,6 +273,11 @@ AngleOf AngleOf::Atan(float f) { return AngleOf::Radians(atanf(f)); } +template +AngleOf Passer::LinearAlgebra::AngleOf::Atan2(float f1, float f2) { + return AngleOf::Radians(atan2f(f1, f2)); +} + // template <> // float AngleOf::CosineRuleSide(float a, float b, AngleOf gamma) // { diff --git a/Angle.h b/Angle.h index 59ba151..384c66f 100644 --- a/Angle.h +++ b/Angle.h @@ -21,6 +21,10 @@ class AngleOf { static AngleOf Degrees(float f); static AngleOf Radians(float f); + const static AngleOf zero; + const static AngleOf deg90; + const static AngleOf deg180; + float InDegrees() const; float InRadians() const; @@ -64,6 +68,7 @@ class AngleOf { static AngleOf Acos(float f); static AngleOf Asin(float f); static AngleOf Atan(float f); + static AngleOf Atan2(float f1, float f2); static float CosineRuleSide(float a, float b, AngleOf gamma); static AngleOf CosineRuleAngle(float a, float b, float c); diff --git a/Direction.cpp b/Direction.cpp index 8dc6bad..e4a26f9 100644 --- a/Direction.cpp +++ b/Direction.cpp @@ -24,8 +24,13 @@ DirectionOf::DirectionOf(AngleOf horizontal, AngleOf vertical) { template DirectionOf::DirectionOf(Vector3 v) { - this->horizontal = AngleOf::Radians(atan2f(v.Right(), v.Forward())); - this->vertical = AngleOf::Radians(-(0.5f * pi) - acosf(v.Up())); + this->horizontal = AngleOf::Atan2( + v.Right(), + v.Forward()); // AngleOf::Radians(atan2f(v.Right(), v.Forward())); + this->vertical = + -AngleOf::deg90 - + AngleOf::Acos( + v.Up()); // AngleOf::Radians(-(0.5f * pi) - acosf(v.Up())); Normalize(); } @@ -34,19 +39,19 @@ const DirectionOf DirectionOf::forward = DirectionOf(AngleOf(), AngleOf()); template const DirectionOf DirectionOf::back = - DirectionOf(AngleOf::Degrees(180), AngleOf()); + DirectionOf(AngleOf::deg180, AngleOf()); template const DirectionOf DirectionOf::up = - DirectionOf(AngleOf(), AngleOf::Degrees(90)); + DirectionOf(AngleOf(), AngleOf::deg90); template const DirectionOf DirectionOf::down = - DirectionOf(AngleOf(), AngleOf::Degrees(-90)); + DirectionOf(AngleOf(), -AngleOf::deg90); template const DirectionOf DirectionOf::left = - DirectionOf(AngleOf::Degrees(-90), AngleOf()); + DirectionOf(-AngleOf::deg90, AngleOf()); template const DirectionOf DirectionOf::right = - DirectionOf(AngleOf::Degrees(90), AngleOf()); + DirectionOf(AngleOf::deg90, AngleOf()); template DirectionOf Passer::LinearAlgebra::DirectionOf::Degrees(float horizontal, @@ -70,14 +75,14 @@ bool Passer::LinearAlgebra::DirectionOf::operator==( template DirectionOf Passer::LinearAlgebra::DirectionOf::operator-() const { - DirectionOf r = DirectionOf(this->horizontal + AngleOf::Degrees(180), - -this->vertical); + DirectionOf r = + DirectionOf(this->horizontal + AngleOf::deg180, -this->vertical); return r; } template Vector3 DirectionOf::ToVector3() { - Vector3 v = Quaternion::Euler(-(this->vertical.InDegrees()), + Vector3 v = Quaternion::Euler(-this->vertical.InDegrees(), this->horizontal.InDegrees(), 0) * Vector3::forward; return v; @@ -85,10 +90,10 @@ Vector3 DirectionOf::ToVector3() { template void DirectionOf::Normalize() { - if (this->vertical > AngleOf::Degrees(90) || - this->vertical < AngleOf::Degrees(-90)) { - this->horizontal += AngleOf::Degrees(180); - this->vertical = AngleOf::Degrees(180) - this->vertical; + if (this->vertical > AngleOf::deg90 || + this->vertical < -AngleOf::deg90) { + this->horizontal += AngleOf::deg180; + this->vertical = AngleOf::deg180 - this->vertical; } } diff --git a/SwingTwist.cpp b/SwingTwist.cpp index a82097c..4d9fc1f 100644 --- a/SwingTwist.cpp +++ b/SwingTwist.cpp @@ -31,8 +31,7 @@ template SwingTwistOf SwingTwistOf::Degrees(float horizontal, float vertical, float twist) { - DirectionOf swing = DirectionOf(AngleOf::Degrees(horizontal), - AngleOf::Degrees(vertical)); + DirectionOf swing = DirectionOf::Degrees(horizontal, vertical); AngleOf twistAngle = AngleOf::Degrees(twist); SwingTwistOf orientation = SwingTwistOf(swing, twistAngle); return orientation; @@ -50,9 +49,8 @@ template SwingTwistOf Passer::LinearAlgebra::SwingTwistOf::FromQuaternion( Quaternion q) { Vector3 angles = Quaternion::ToAngles(q); - SwingTwistOf r = SwingTwistOf(AngleOf::Degrees(angles.Up()), - AngleOf::Degrees(angles.Right()), - AngleOf::Degrees(angles.Forward())); + SwingTwistOf r = + SwingTwistOf::Degrees(angles.Up(), angles.Right(), angles.Forward()); return r; }