From fdd228071a3112aeebda20766c7df3b20b8651aa Mon Sep 17 00:00:00 2001 From: chai Date: Thu, 2 Dec 2021 14:44:36 +0800 Subject: +Fix32 --- Client/Project/VisualStudio/Phy2D/Phy2D.vcxproj | 19 +- .../VisualStudio/Phy2D/Phy2D.vcxproj.filters | 60 +- .../VisualStudio/Phy2Dlite/Phy2Dlite.vcxproj | 3 + .../Phy2Dlite/Phy2Dlite.vcxproj.filters | 15 + Client/Source/Phy2D/Common/Math.h | 191 ---- Client/Source/Phy2D/Common/Settings.h | 82 +- Client/Source/Phy2D/Common/Type.h | 0 Client/Source/Phy2D/Dynamic/Arbiter.cpp | 189 ---- Client/Source/Phy2D/Dynamic/Arbiter.h | 91 -- Client/Source/Phy2D/Dynamic/Body.cpp | 47 - Client/Source/Phy2D/Dynamic/Body.h | 34 - Client/Source/Phy2D/Dynamic/Collide.cpp | 326 ------- Client/Source/Phy2D/Dynamic/Joint.cpp | 102 --- Client/Source/Phy2D/Dynamic/Joint.h | 34 - Client/Source/Phy2D/Dynamic/World.cpp | 129 --- Client/Source/Phy2D/Dynamic/World.h | 36 - Client/Source/Phy2D/Phy2D.h | 4 - Client/Source/Phy2D/Shapes/p2CircleShape.cpp | 0 Client/Source/Phy2D/Shapes/p2CircleShape.h | 8 - Client/Source/Phy2D/Shapes/p2PolygonShape.cpp | 0 Client/Source/Phy2D/Shapes/p2PolygonShape.h | 0 Client/Source/Phy2D/Shapes/p2Shape.h | 7 - Client/Source/Phy2D/Tests/test.h | 2 +- Client/Source/Phy2D/Tests/test_math.cpp | 9 +- Client/Source/Phy2D/Tests/test_p2d.cpp | 625 -------------- Client/Source/Phy2DLite/Constants.h | 24 +- Client/Source/Phy2DLite/Settings.h | 23 +- Client/Source/Phy2DLite/World.cpp | 2 +- Client/ThirdParty/fix32/fix32.cpp | 0 Client/ThirdParty/fix32/fix32.h | 7 - Client/ThirdParty/fix32/fix32.hpp | 161 ++++ Client/ThirdParty/math-sll/LICENSE | 23 + Client/ThirdParty/math-sll/Makefile | 69 ++ Client/ThirdParty/math-sll/README | 51 ++ Client/ThirdParty/math-sll/math-sll.c | 957 +++++++++++++++++++++ Client/ThirdParty/math-sll/math-sll.h | 770 +++++++++++++++++ 36 files changed, 2192 insertions(+), 1908 deletions(-) create mode 100644 Client/Source/Phy2D/Common/Type.h delete mode 100644 Client/Source/Phy2D/Dynamic/Arbiter.cpp delete mode 100644 Client/Source/Phy2D/Dynamic/Arbiter.h delete mode 100644 Client/Source/Phy2D/Dynamic/Body.cpp delete mode 100644 Client/Source/Phy2D/Dynamic/Body.h delete mode 100644 Client/Source/Phy2D/Dynamic/Collide.cpp delete mode 100644 Client/Source/Phy2D/Dynamic/Joint.cpp delete mode 100644 Client/Source/Phy2D/Dynamic/Joint.h delete mode 100644 Client/Source/Phy2D/Dynamic/World.cpp delete mode 100644 Client/Source/Phy2D/Dynamic/World.h delete mode 100644 Client/Source/Phy2D/Shapes/p2CircleShape.cpp delete mode 100644 Client/Source/Phy2D/Shapes/p2CircleShape.h delete mode 100644 Client/Source/Phy2D/Shapes/p2PolygonShape.cpp delete mode 100644 Client/Source/Phy2D/Shapes/p2PolygonShape.h delete mode 100644 Client/Source/Phy2D/Shapes/p2Shape.h delete mode 100644 Client/ThirdParty/fix32/fix32.cpp delete mode 100644 Client/ThirdParty/fix32/fix32.h create mode 100644 Client/ThirdParty/fix32/fix32.hpp create mode 100644 Client/ThirdParty/math-sll/LICENSE create mode 100644 Client/ThirdParty/math-sll/Makefile create mode 100644 Client/ThirdParty/math-sll/README create mode 100644 Client/ThirdParty/math-sll/math-sll.c create mode 100644 Client/ThirdParty/math-sll/math-sll.h diff --git a/Client/Project/VisualStudio/Phy2D/Phy2D.vcxproj b/Client/Project/VisualStudio/Phy2D/Phy2D.vcxproj index 54f9787..84b0365 100644 --- a/Client/Project/VisualStudio/Phy2D/Phy2D.vcxproj +++ b/Client/Project/VisualStudio/Phy2D/Phy2D.vcxproj @@ -137,13 +137,6 @@ - - - - - - - @@ -163,21 +156,16 @@ + - - - - + - - - - + @@ -195,6 +183,7 @@ + diff --git a/Client/Project/VisualStudio/Phy2D/Phy2D.vcxproj.filters b/Client/Project/VisualStudio/Phy2D/Phy2D.vcxproj.filters index 5a36dba..cf32e27 100644 --- a/Client/Project/VisualStudio/Phy2D/Phy2D.vcxproj.filters +++ b/Client/Project/VisualStudio/Phy2D/Phy2D.vcxproj.filters @@ -31,17 +31,11 @@ {596062aa-a263-4c8c-acf2-1267b28a0f97} + + {fc4d6710-2d01-480a-892c-79a461ad0ec4} + - - Shapes - - - Shapes - - - Dynamic - Common @@ -51,18 +45,6 @@ Test - - Dynamic - - - Dynamic - - - Dynamic - - - Dynamic - libs\glad @@ -114,24 +96,15 @@ libs\libfixmath + + libs\sll + - - Shapes - - - Shapes - - - Shapes - Rendering - - Dynamic - Common @@ -141,24 +114,12 @@ Test - - Dynamic - - - Dynamic - - - Dynamic - libs\glad libs\glad - - libs\fix32 - libs\imgui @@ -204,6 +165,15 @@ libs\libfixmath + + Common + + + libs\sll + + + libs\fix32 + diff --git a/Client/Project/VisualStudio/Phy2Dlite/Phy2Dlite.vcxproj b/Client/Project/VisualStudio/Phy2Dlite/Phy2Dlite.vcxproj index 61da1c4..3f7e430 100644 --- a/Client/Project/VisualStudio/Phy2Dlite/Phy2Dlite.vcxproj +++ b/Client/Project/VisualStudio/Phy2Dlite/Phy2Dlite.vcxproj @@ -160,6 +160,7 @@ + @@ -171,6 +172,7 @@ + @@ -191,6 +193,7 @@ + diff --git a/Client/Project/VisualStudio/Phy2Dlite/Phy2Dlite.vcxproj.filters b/Client/Project/VisualStudio/Phy2Dlite/Phy2Dlite.vcxproj.filters index 104e093..3b49c68 100644 --- a/Client/Project/VisualStudio/Phy2Dlite/Phy2Dlite.vcxproj.filters +++ b/Client/Project/VisualStudio/Phy2Dlite/Phy2Dlite.vcxproj.filters @@ -60,6 +60,9 @@ Tests + + Libs\sll + @@ -133,6 +136,12 @@ Libs\fpm + + Libs\sll + + + Libs\fix32 + @@ -153,6 +162,12 @@ {4670ccf3-2964-42b4-bb00-6b48eae171ad} + + {1b250cf1-a3ca-4e90-9bc9-bc10df919ccf} + + + {e9dd5549-728e-408f-b740-15e38984d253} + diff --git a/Client/Source/Phy2D/Common/Math.h b/Client/Source/Phy2D/Common/Math.h index 9c0ff8f..e69de29 100644 --- a/Client/Source/Phy2D/Common/Math.h +++ b/Client/Source/Phy2D/Common/Math.h @@ -1,191 +0,0 @@ -#pragma once - -#include -#include -#include -#include - -#include "Settings.h" - -namespace Phy2D -{ - - struct Vec2 - { - Vec2() {} - Vec2(number x, number y) : x(x), y(y) {} - - void Set(number x_, number y_) { x = x_; y = y_; } - - Vec2 operator -() { return Vec2(-x, -y); } - - void operator += (const Vec2& v) - { - x += v.x; y += v.y; - } - - void operator -= (const Vec2& v) - { - x -= v.x; y -= v.y; - } - - void operator *= (number a) - { - x *= a; y *= a; - } - - number Length() const - { - return SQRT(x * x + y * y); - } - - std::string ToString() - { - return std::to_string((float)x) + "," + std::to_string((float)y); - } - - number x, y; - }; - - struct Mat22 - { - Mat22() {} - Mat22(number angle) - { - number c = COS(angle), s = SIN(angle); - col1.x = c; col2.x = -s; - col1.y = s; col2.y = c; - } - - Mat22(const Vec2& col1, const Vec2& col2) : col1(col1), col2(col2) {} - - Mat22 Transpose() const - { - return Mat22(Vec2(col1.x, col2.x), Vec2(col1.y, col2.y)); - } - - Mat22 Invert() const - { - number a = col1.x, b = col2.x, c = col1.y, d = col2.y; - Mat22 B; - number det = a * d - b * c; - assert(det != 0.0f); - det = (number)1.0f / det; - B.col1.x = det * d; B.col2.x = -det * b; - B.col1.y = -det * c; B.col2.y = det * a; - return B; - } - - Vec2 col1, col2; - }; - - inline number Dot(const Vec2& a, const Vec2& b) - { - return a.x * b.x + a.y * b.y; - } - - inline number Cross(const Vec2& a, const Vec2& b) - { - return a.x * b.y - a.y * b.x; - } - - inline Vec2 Cross(const Vec2& a, number s) - { - return Vec2(s * a.y, -s * a.x); - } - - inline Vec2 Cross(number s, const Vec2& a) - { - return Vec2(-s * a.y, s * a.x); - } - - inline Vec2 operator * (const Mat22& A, const Vec2& v) - { - return Vec2(A.col1.x * v.x + A.col2.x * v.y, A.col1.y * v.x + A.col2.y * v.y); - } - - inline Vec2 operator + (const Vec2& a, const Vec2& b) - { - return Vec2(a.x + b.x, a.y + b.y); - } - - inline Vec2 operator - (const Vec2& a, const Vec2& b) - { - return Vec2(a.x - b.x, a.y - b.y); - } - - inline Vec2 operator * (number s, const Vec2& v) - { - return Vec2(s * v.x, s * v.y); - } - - inline Mat22 operator + (const Mat22& A, const Mat22& B) - { - return Mat22(A.col1 + B.col1, A.col2 + B.col2); - } - - inline Mat22 operator * (const Mat22& A, const Mat22& B) - { - return Mat22(A * B.col1, A * B.col2); - } - - inline number Abs(number a) - { - return a > 0.0f ? a : -a; - } - - inline Vec2 Abs(const Vec2& a) - { - return Vec2(fabsf(a.x), fabsf(a.y)); - } - - inline Mat22 Abs(const Mat22& A) - { - return Mat22(Abs(A.col1), Abs(A.col2)); - } - - inline number Sign(number x) - { - return (float) x < 0.0f ? -1.0f : 1.0f; - } - - inline number Min(number a, number b) - { - return a < b ? a : b; - } - - inline number Max(number a, number b) - { - return a > b ? a : b; - } - - inline number Clamp(number a, number low, number high) - { - return Max(low, Min(a, high)); - } - - template inline void Swap(T& a, T& b) - { - T tmp = a; - a = b; - b = tmp; - } - - //// Random number in range [-1,1] - //inline number Random() - //{ - // number r = (number)rand(); - // r /= RAND_MAX; - // r = (number)2.0f * r - 1.0f; - // return r; - //} - - //inline number Random(number lo, number hi) - //{ - // number r = (number)rand(); - // r /= RAND_MAX; - // r = (hi - lo) * r + lo; - // return r; - //} - -} diff --git a/Client/Source/Phy2D/Common/Settings.h b/Client/Source/Phy2D/Common/Settings.h index 5c24b57..318a76c 100644 --- a/Client/Source/Phy2D/Common/Settings.h +++ b/Client/Source/Phy2D/Common/Settings.h @@ -1,27 +1,97 @@ #pragma once +#define NUMBER_FLOAT 1 +#define NUMBER_LIBFIX 2 +#define NUMBER_FPM 3 + +#define NUMBER_ALIAS NUMBER_FPM + +#if NUMBER_ALIAS == NUMBER_LIBFIX #include "libfixmath/libfixmath/fixmath.h" +#elif NUMBER_ALIAS == NUMBER_FPM +#include "fpm/include/fpm/fixed.hpp" +#include "fpm/include/fpm/math.hpp" +#endif namespace Phy2D { -#define NUMBER_FLOAT false +#if NUMBER_ALIAS == NUMBER_FLOAT -#if NUMBER_FLOAT -typedef float number; + typedef float fixed; #define NUMBER_MAX (FLT_MAX) #define NUMBER_MIN (FLT_MIN) #define SQRT(a) (sqrt((a))) #define SIN(a) (sin((a))) #define COS(a) (cos((a))) -#else -// 同时一定要开启内联函数扩展，否则执行效率会非常低 -typedef Fix16 number; +#define PI (3.1415925f) + +#elif NUMBER_ALIAS == NUMBER_LIBFIX + + // 同时一定要开启内联函数扩展，否则执行效率会非常低 + typedef Fix16 fixed; #define NUMBER_MAX (fix16_maximum) #define NUMBER_MIN (fix16_minimum) #define SQRT(a) ((a).sqrt()) #define SIN(a) ((a).sin()) #define COS(a) ((a).cos()) +#define PI (fix16_pi) + +#elif NUMBER_ALIAS == NUMBER_FPM + + template + struct Limits {}; + + template <> + struct Limits + { + static constexpr bool is_signed() noexcept { return true; } + static constexpr int digits() noexcept { return 31; } + static constexpr int max_digits10() noexcept { return 5 + 5; } + static constexpr int min_exponent() noexcept { return -15; } + static constexpr int max_exponent() noexcept { return 15; } + static constexpr int min_exponent10() noexcept { return -4; } + static constexpr int max_exponent10() noexcept { return 4; } + static constexpr fpm::fixed_16_16 min() noexcept { return fpm::fixed_16_16::from_raw_value(-2147483647 - 1); } + static constexpr fpm::fixed_16_16 max() noexcept { return fpm::fixed_16_16::from_raw_value(2147483647); } + }; + + template <> + struct Limits + { + static constexpr bool is_signed() noexcept { return true; } + static constexpr int digits() noexcept { return 31; } + static constexpr int max_digits10() noexcept { return 7 + 3; } + static constexpr int min_exponent() noexcept { return -7; } + static constexpr int max_exponent() noexcept { return 23; } + static constexpr int min_exponent10() noexcept { return -2; } + static constexpr int max_exponent10() noexcept { return 6; } + static constexpr fpm::fixed_24_8 min() noexcept { return fpm::fixed_24_8::from_raw_value(-2147483647 - 1); } + static constexpr fpm::fixed_24_8 max() noexcept { return fpm::fixed_24_8::from_raw_value(2147483647); } + }; + + template <> + struct Limits + { + static constexpr bool is_signed() noexcept { return true; } + static constexpr int digits() noexcept { return 31; } + static constexpr int max_digits10() noexcept { return 3 + 8; } + static constexpr int min_exponent() noexcept { return -23; } + static constexpr int max_exponent() noexcept { return 7; } + static constexpr int min_exponent10() noexcept { return -7; } + static constexpr int max_exponent10() noexcept { return 2; } + static constexpr fpm::fixed_8_24 min() noexcept { return fpm::fixed_8_24::from_raw_value(-2147483647 - 1); } + static constexpr fpm::fixed_8_24 max() noexcept { return fpm::fixed_8_24::from_raw_value(2147483647); } + }; + + typedef fpm::fixed_16_16 fixed; +#define NUMBER_MAX (Limits::max()) +#define NUMBER_MIN (Limits::min()) +#define SQRT(a) (fpm::sqrt((a))) +#define SIN(a) (fpm::sin((a))) +#define COS(a) (fpm::cos((a))) +#define PI (fixed::pi()) + #endif } \ No newline at end of file diff --git a/Client/Source/Phy2D/Common/Type.h b/Client/Source/Phy2D/Common/Type.h new file mode 100644 index 0000000..e69de29 diff --git a/Client/Source/Phy2D/Dynamic/Arbiter.cpp b/Client/Source/Phy2D/Dynamic/Arbiter.cpp deleted file mode 100644 index 4163154..0000000 --- a/Client/Source/Phy2D/Dynamic/Arbiter.cpp +++ /dev/null @@ -1,189 +0,0 @@ -#include "Arbiter.h" -#include "World.h" -#include "Body.h" -#include "Joint.h" - -using namespace Phy2D; - -Arbiter::Arbiter(Body* b1, Body* b2) -{ - if (b1 < b2) - { - body1 = b1; - body2 = b2; - } - else - { - body1 = b2; - body2 = b1; - } - - numContacts = Collide(contacts, body1, body2); - - friction = SQRT(body1->friction * body2->friction); -} - -void Arbiter::Update(Contact* newContacts, int numNewContacts) -{ - Contact mergedContacts[2]; - - for (int i = 0; i < numNewContacts; ++i) - { - Contact* cNew = newContacts + i; - int k = -1; - for (int j = 0; j < numContacts; ++j) - { - Contact* cOld = contacts + j; - if (cNew->feature.value == cOld->feature.value) - { - k = j; - break; - } - } - - if (k > -1) - { - Contact* c = mergedContacts + i; - Contact* cOld = contacts + k; - *c = *cNew; - if (World::warmStarting) - { - c->Pn = cOld->Pn; - c->Pt = cOld->Pt; - c->Pnb = cOld->Pnb; - } - else - { - c->Pn = 0.0f; - c->Pt = 0.0f; - c->Pnb = 0.0f; - } - } - else - { - mergedContacts[i] = newContacts[i]; - } - } - - for (int i = 0; i < numNewContacts; ++i) - contacts[i] = mergedContacts[i]; - - numContacts = numNewContacts; -} - - -void Arbiter::PreStep(number inv_dt) -{ - const number k_allowedPenetration = 0.01f; - number k_biasFactor = World::positionCorrection ? 0.2f : 0.0f; - - for (int i = 0; i < numContacts; ++i) - { - Contact* c = contacts + i; - - Vec2 r1 = c->position - body1->position; - Vec2 r2 = c->position - body2->position; - - // Precompute normal mass, tangent mass, and bias. - number rn1 = Dot(r1, c->normal); - number rn2 = Dot(r2, c->normal); - number kNormal = body1->invMass + body2->invMass; - kNormal += body1->invI * (Dot(r1, r1) - rn1 * rn1) + body2->invI * (Dot(r2, r2) - rn2 * rn2); - c->massNormal = (number) 1.0f / kNormal; - - Vec2 tangent = Cross(c->normal, 1.0f); - number rt1 = Dot(r1, tangent); - number rt2 = Dot(r2, tangent); - number kTangent = body1->invMass + body2->invMass; - kTangent += body1->invI * (Dot(r1, r1) - rt1 * rt1) + body2->invI * (Dot(r2, r2) - rt2 * rt2); - c->massTangent = (number) 1.0f / kTangent; - - c->bias = -k_biasFactor * inv_dt * Min(0.0f, c->separation + k_allowedPenetration); - - if (World::accumulateImpulses) - { - // Apply normal + friction impulse - Vec2 P = c->Pn * c->normal + c->Pt * tangent; - - body1->velocity -= body1->invMass * P; - body1->angularVelocity -= body1->invI * Cross(r1, P); - - body2->velocity += body2->invMass * P; - body2->angularVelocity += body2->invI * Cross(r2, P); - } - } -} - -void Arbiter::ApplyImpulse() -{ - Body* b1 = body1; - Body* b2 = body2; - - for (int i = 0; i < numContacts; ++i) - { - Contact* c = contacts + i; - c->r1 = c->position - b1->position; - c->r2 = c->position - b2->position; - - // Relative velocity at contact - Vec2 dv = b2->velocity + Cross(b2->angularVelocity, c->r2) - b1->velocity - Cross(b1->angularVelocity, c->r1); - - // Compute normal impulse - number vn = Dot(dv, c->normal); - - number dPn = c->massNormal * (-vn + c->bias); - - if (World::accumulateImpulses) - { - // Clamp the accumulated impulse - number Pn0 = c->Pn; - c->Pn = Max(Pn0 + dPn, 0.0f); - dPn = c->Pn - Pn0; - } - else - { - dPn = Max(dPn, 0.0f); - } - - // Apply contact impulse - Vec2 Pn = dPn * c->normal; - - b1->velocity -= b1->invMass * Pn; - b1->angularVelocity -= b1->invI * Cross(c->r1, Pn); - - b2->velocity += b2->invMass * Pn; - b2->angularVelocity += b2->invI * Cross(c->r2, Pn); - - // Relative velocity at contact - dv = b2->velocity + Cross(b2->angularVelocity, c->r2) - b1->velocity - Cross(b1->angularVelocity, c->r1); - - Vec2 tangent = Cross(c->normal, 1.0f); - number vt = Dot(dv, tangent); - number dPt = c->massTangent * (-vt); - - if (World::accumulateImpulses) - { - // Compute friction impulse - number maxPt = friction * c->Pn; - - // Clamp friction - number oldTangentImpulse = c->Pt; - c->Pt = Clamp(oldTangentImpulse + dPt, -maxPt, maxPt); - dPt = c->Pt - oldTangentImpulse; - } - else - { - number maxPt = friction * dPn; - dPt = Clamp(dPt, -maxPt, maxPt); - } - - // Apply contact impulse - Vec2 Pt = dPt * tangent; - - b1->velocity -= b1->invMass * Pt; - b1->angularVelocity -= b1->invI * Cross(c->r1, Pt); - - b2->velocity += b2->invMass * Pt; - b2->angularVelocity += b2->invI * Cross(c->r2, Pt); - } -} diff --git a/Client/Source/Phy2D/Dynamic/Arbiter.h b/Client/Source/Phy2D/Dynamic/Arbiter.h deleted file mode 100644 index 64bed72..0000000 --- a/Client/Source/Phy2D/Dynamic/Arbiter.h +++ /dev/null @@ -1,91 +0,0 @@ -#pragma once - -#include "../Common/Math.h" - -namespace Phy2D -{ - - struct Body; - - union FeaturePair - { - struct Edges - { - char inEdge1; - char outEdge1; - char inEdge2; - char outEdge2; - } e; - int value; - }; - - struct Contact - { - Contact() : Pn(0.0f), Pt(0.0f), Pnb(0.0f) {} - - Vec2 position; - Vec2 normal; - Vec2 r1, r2; - number separation; - number Pn; // accumulated normal impulse - number Pt; // accumulated tangent impulse - number Pnb; // accumulated normal impulse for position bias - number massNormal, massTangent; - number bias; - FeaturePair feature; - }; - - struct ArbiterKey - { - ArbiterKey(Body* b1, Body* b2) - { - if (b1 < b2) - { - body1 = b1; body2 = b2; - } - else - { - body1 = b2; body2 = b1; - } - } - - Body* body1; - Body* body2; - }; - - struct Arbiter - { - enum { MAX_POINTS = 2 }; - - Arbiter(Body* b1, Body* b2); - - void Update(Contact* contacts, int numContacts); - - void PreStep(number inv_dt); - void ApplyImpulse(); - - Contact contacts[MAX_POINTS]; - int numContacts; - - Body* body1; - Body* body2; - - // Combined friction - number friction; - }; - - // This is used by std::set - inline bool operator < (const ArbiterKey& a1, const ArbiterKey& a2) - { - if (a1.body1 < a2.body1) - return true; - - if (a1.body1 == a2.body1 && a1.body2 < a2.body2) - return true; - - return false; - } - - int Collide(Contact* contacts, Body* body1, Body* body2); - -} \ No newline at end of file diff --git a/Client/Source/Phy2D/Dynamic/Body.cpp b/Client/Source/Phy2D/Dynamic/Body.cpp deleted file mode 100644 index 1d896fd..0000000 --- a/Client/Source/Phy2D/Dynamic/Body.cpp +++ /dev/null @@ -1,47 +0,0 @@ -#include "Body.h" - -using namespace Phy2D; - -Body::Body() -{ - position.Set(0.0f, 0.0f); - rotation = 0.0f; - velocity.Set(0.0f, 0.0f); - angularVelocity = 0.0f; - force.Set(0.0f, 0.0f); - torque = 0.0f; - friction = 0.2f; - - width.Set(1.0f, 1.0f); - mass = NUMBER_MAX; - invMass = 0.0f; - I = NUMBER_MAX; - invI = 0.0f; -} - -void Body::Set(const Vec2& w, number m) -{ - position.Set(0.0f, 0.0f); - rotation = 0.0f; - velocity.Set(0.0f, 0.0f); - angularVelocity = 0.0f; - force.Set(0.0f, 0.0f); - torque = 0.0f; - friction = 0.2f; - - width = w; - mass = m; - - if (mass < NUMBER_MAX) - { - invMass = (number)1.0f / mass; - I = mass * (width.x * width.x + width.y * width.y) / 12.0f; - invI = (number)1.0f / I; - } - else - { - invMass = 0.0f; - I = NUMBER_MAX; - invI = 0.0f; - } -} diff --git a/Client/Source/Phy2D/Dynamic/Body.h b/Client/Source/Phy2D/Dynamic/Body.h deleted file mode 100644 index 8c2ac72..0000000 --- a/Client/Source/Phy2D/Dynamic/Body.h +++ /dev/null @@ -1,34 +0,0 @@ -#pragma once - -#include "../Common/Math.h" - -namespace Phy2D -{ - - struct Body - { - Body(); - void Set(const Vec2& w, number m); - - void AddForce(const Vec2& f) - { - force += f; - } - - Vec2 position; - number rotation; - - Vec2 velocity; - number angularVelocity; - - Vec2 force; - number torque; - - Vec2 width; - - number friction; - number mass, invMass; - number I, invI; - }; - -} \ No newline at end of file diff --git a/Client/Source/Phy2D/Dynamic/Collide.cpp b/Client/Source/Phy2D/Dynamic/Collide.cpp deleted file mode 100644 index 6849c0e..0000000 --- a/Client/Source/Phy2D/Dynamic/Collide.cpp +++ /dev/null @@ -1,326 +0,0 @@ -#include "Arbiter.h" -#include "Body.h" -#include "World.h" -#include "Joint.h" - -using namespace Phy2D; - -// Box vertex and edge numbering: -// -// ^ y -// | -// e1 -// v2 ------ v1 -// | | -// e2 | | e4 --> x -// | | -// v3 ------ v4 -// e3 - -enum Axis -{ - FACE_A_X, - FACE_A_Y, - FACE_B_X, - FACE_B_Y -}; - -enum EdgeNumbers -{ - NO_EDGE = 0, - EDGE1, - EDGE2, - EDGE3, - EDGE4 -}; - -struct ClipVertex -{ - ClipVertex() { fp.value = 0; } - Vec2 v; - FeaturePair fp; -}; - -void Flip(FeaturePair& fp) -{ - Swap(fp.e.inEdge1, fp.e.inEdge2); - Swap(fp.e.outEdge1, fp.e.outEdge2); -} - -int ClipSegmentToLine(ClipVertex vOut[2], ClipVertex vIn[2], - const Vec2& normal, number offset, char clipEdge) -{ - // Start with no output points - int numOut = 0; - - // Calculate the distance of end points to the line - number distance0 = Dot(normal, vIn[0].v) - offset; - number distance1 = Dot(normal, vIn[1].v) - offset; - - // If the points are behind the plane - if (distance0 <= 0.0f) vOut[numOut++] = vIn[0]; - if (distance1 <= 0.0f) vOut[numOut++] = vIn[1]; - - // If the points are on different sides of the plane - if (distance0 * distance1 < 0.0f) - { - // Find intersection point of edge and plane - number interp = distance0 / (distance0 - distance1); - vOut[numOut].v = vIn[0].v + interp * (vIn[1].v - vIn[0].v); - if (distance0 > 0.0f) - { - vOut[numOut].fp = vIn[0].fp; - vOut[numOut].fp.e.inEdge1 = clipEdge; - vOut[numOut].fp.e.inEdge2 = NO_EDGE; - } - else - { - vOut[numOut].fp = vIn[1].fp; - vOut[numOut].fp.e.outEdge1 = clipEdge; - vOut[numOut].fp.e.outEdge2 = NO_EDGE; - } - ++numOut; - } - - return numOut; -} - -static void ComputeIncidentEdge(ClipVertex c[2], const Vec2& h, const Vec2& pos, - const Mat22& Rot, const Vec2& normal) -{ - // The normal is from the reference box. Convert it - // to the incident boxe's frame and flip sign. - Mat22 RotT = Rot.Transpose(); - Vec2 n = -(RotT * normal); - Vec2 nAbs = Abs(n); - - if (nAbs.x > nAbs.y) - { - if (Sign(n.x) > 0.0f) - { - c[0].v.Set(h.x, -h.y); - c[0].fp.e.inEdge2 = EDGE3; - c[0].fp.e.outEdge2 = EDGE4; - - c[1].v.Set(h.x, h.y); - c[1].fp.e.inEdge2 = EDGE4; - c[1].fp.e.outEdge2 = EDGE1; - } - else - { - c[0].v.Set(-h.x, h.y); - c[0].fp.e.inEdge2 = EDGE1; - c[0].fp.e.outEdge2 = EDGE2; - - c[1].v.Set(-h.x, -h.y); - c[1].fp.e.inEdge2 = EDGE2; - c[1].fp.e.outEdge2 = EDGE3; - } - } - else - { - if (Sign(n.y) > 0.0f) - { - c[0].v.Set(h.x, h.y); - c[0].fp.e.inEdge2 = EDGE4; - c[0].fp.e.outEdge2 = EDGE1; - - c[1].v.Set(-h.x, h.y); - c[1].fp.e.inEdge2 = EDGE1; - c[1].fp.e.outEdge2 = EDGE2; - } - else - { - c[0].v.Set(-h.x, -h.y); - c[0].fp.e.inEdge2 = EDGE2; - c[0].fp.e.outEdge2 = EDGE3; - - c[1].v.Set(h.x, -h.y); - c[1].fp.e.inEdge2 = EDGE3; - c[1].fp.e.outEdge2 = EDGE4; - } - } - - c[0].v = pos + Rot * c[0].v; - c[1].v = pos + Rot * c[1].v; -} - -namespace Phy2D -{ - - // The normal points from A to B - int Collide(Contact* contacts, Body* bodyA, Body* bodyB) - { - // Setup - Vec2 hA = 0.5f * bodyA->width; - Vec2 hB = 0.5f * bodyB->width; - - Vec2 posA = bodyA->position; - Vec2 posB = bodyB->position; - - Mat22 RotA(bodyA->rotation), RotB(bodyB->rotation); - - Mat22 RotAT = RotA.Transpose(); - Mat22 RotBT = RotB.Transpose(); - - Vec2 dp = posB - posA; - Vec2 dA = RotAT * dp; - Vec2 dB = RotBT * dp; - - Mat22 C = RotAT * RotB; - Mat22 absC = Abs(C); - Mat22 absCT = absC.Transpose(); - - // Box A faces - Vec2 faceA = Abs(dA) - hA - absC * hB; - if (faceA.x > 0.0f || faceA.y > 0.0f) - return 0; - - // Box B faces - Vec2 faceB = Abs(dB) - absCT * hA - hB; - if (faceB.x > 0.0f || faceB.y > 0.0f) - return 0; - - // Find best axis - Axis axis; - number separation; - Vec2 normal; - - // Box A faces - axis = FACE_A_X; - separation = faceA.x; - normal = dA.x > 0.0f ? RotA.col1 : -RotA.col1; - - const number relativeTol = 0.95f; - const number absoluteTol = 0.01f; - - if (faceA.y > relativeTol * separation + absoluteTol * hA.y) - { - axis = FACE_A_Y; - separation = faceA.y; - normal = dA.y > 0.0f ? RotA.col2 : -RotA.col2; - } - - // Box B faces - if (faceB.x > relativeTol * separation + absoluteTol * hB.x) - { - axis = FACE_B_X; - separation = faceB.x; - normal = dB.x > 0.0f ? RotB.col1 : -RotB.col1; - } - - if (faceB.y > relativeTol * separation + absoluteTol * hB.y) - { - axis = FACE_B_Y; - separation = faceB.y; - normal = dB.y > 0.0f ? RotB.col2 : -RotB.col2; - } - - // Setup clipping plane data based on the separating axis - Vec2 frontNormal, sideNormal; - ClipVertex incidentEdge[2]; - number front, negSide, posSide; - char negEdge, posEdge; - - // Compute the clipping lines and the line segment to be clipped. - switch (axis) - { - case FACE_A_X: - { - frontNormal = normal; - front = Dot(posA, frontNormal) + hA.x; - sideNormal = RotA.col2; - number side = Dot(posA, sideNormal); - negSide = -side + hA.y; - posSide = side + hA.y; - negEdge = EDGE3; - posEdge = EDGE1; - ComputeIncidentEdge(incidentEdge, hB, posB, RotB, frontNormal); - } - break; - - case FACE_A_Y: - { - frontNormal = normal; - front = Dot(posA, frontNormal) + hA.y; - sideNormal = RotA.col1; - number side = Dot(posA, sideNormal); - negSide = -side + hA.x; - posSide = side + hA.x; - negEdge = EDGE2; - posEdge = EDGE4; - ComputeIncidentEdge(incidentEdge, hB, posB, RotB, frontNormal); - } - break; - - case FACE_B_X: - { - frontNormal = -normal; - front = Dot(posB, frontNormal) + hB.x; - sideNormal = RotB.col2; - number side = Dot(posB, sideNormal); - negSide = -side + hB.y; - posSide = side + hB.y; - negEdge = EDGE3; - posEdge = EDGE1; - ComputeIncidentEdge(incidentEdge, hA, posA, RotA, frontNormal); - } - break; - - case FACE_B_Y: - { - frontNormal = -normal; - front = Dot(posB, frontNormal) + hB.y; - sideNormal = RotB.col1; - number side = Dot(posB, sideNormal); - negSide = -side + hB.x; - posSide = side + hB.x; - negEdge = EDGE2; - posEdge = EDGE4; - ComputeIncidentEdge(incidentEdge, hA, posA, RotA, frontNormal); - } - break; - } - - // clip other face with 5 box planes (1 face plane, 4 edge planes) - - ClipVertex clipPoints1[2]; - ClipVertex clipPoints2[2]; - int np; - - // Clip to box side 1 - np = ClipSegmentToLine(clipPoints1, incidentEdge, -sideNormal, negSide, negEdge); - - if (np < 2) - return 0; - - // Clip to negative box side 1 - np = ClipSegmentToLine(clipPoints2, clipPoints1, sideNormal, posSide, posEdge); - - if (np < 2) - return 0; - - // Now clipPoints2 contains the clipping points. - // Due to roundoff, it is possible that clipping removes all points. - - int numContacts = 0; - for (int i = 0; i < 2; ++i) - { - number separation = Dot(frontNormal, clipPoints2[i].v) - front; - - if (separation <= 0) - { - contacts[numContacts].separation = separation; - contacts[numContacts].normal = normal; - // slide contact point onto reference face (easy to cull) - contacts[numContacts].position = clipPoints2[i].v - separation * frontNormal; - contacts[numContacts].feature = clipPoints2[i].fp; - if (axis == FACE_B_X || axis == FACE_B_Y) - Flip(contacts[numContacts].feature); - ++numContacts; - } - } - - return numContacts; - } -} \ No newline at end of file diff --git a/Client/Source/Phy2D/Dynamic/Joint.cpp b/Client/Source/Phy2D/Dynamic/Joint.cpp deleted file mode 100644 index 95f7c64..0000000 --- a/Client/Source/Phy2D/Dynamic/Joint.cpp +++ /dev/null @@ -1,102 +0,0 @@ -#include "Joint.h" -#include "Body.h" -#include "World.h" - -using namespace Phy2D; - -void Joint::Set(Body* b1, Body* b2, const Vec2& anchor) -{ - body1 = b1; - body2 = b2; - - Mat22 Rot1(body1->rotation); - Mat22 Rot2(body2->rotation); - Mat22 Rot1T = Rot1.Transpose(); - Mat22 Rot2T = Rot2.Transpose(); - - localAnchor1 = Rot1T * (anchor - body1->position); - localAnchor2 = Rot2T * (anchor - body2->position); - - P.Set(0.0f, 0.0f); - - softness = 0.0f; - biasFactor = 0.2f; -} - -void Joint::PreStep(number inv_dt) -{ - // Pre-compute anchors, mass matrix, and bias. - Mat22 Rot1(body1->rotation); - Mat22 Rot2(body2->rotation); - - r1 = Rot1 * localAnchor1; - r2 = Rot2 * localAnchor2; - - // deltaV = deltaV0 + K * impulse - // invM = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)] - // = [1/m1+1/m2 0 ] + invI1 * [r1.y*r1.y -r1.x*r1.y] + invI2 * [r1.y*r1.y -r1.x*r1.y] - // [ 0 1/m1+1/m2] [-r1.x*r1.y r1.x*r1.x] [-r1.x*r1.y r1.x*r1.x] - Mat22 K1; - K1.col1.x = body1->invMass + body2->invMass; K1.col2.x = 0.0f; - K1.col1.y = 0.0f; K1.col2.y = body1->invMass + body2->invMass; - - Mat22 K2; - K2.col1.x = body1->invI * r1.y * r1.y; K2.col2.x = -body1->invI * r1.x * r1.y; - K2.col1.y = -body1->invI * r1.x * r1.y; K2.col2.y = body1->invI * r1.x * r1.x; - - Mat22 K3; - K3.col1.x = body2->invI * r2.y * r2.y; K3.col2.x = -body2->invI * r2.x * r2.y; - K3.col1.y = -body2->invI * r2.x * r2.y; K3.col2.y = body2->invI * r2.x * r2.x; - - Mat22 K = K1 + K2 + K3; - K.col1.x += softness; - K.col2.y += softness; - - M = K.Invert(); - - Vec2 p1 = body1->position + r1; - Vec2 p2 = body2->position + r2; - Vec2 dp = p2 - p1; - - if (World::positionCorrection) - { - bias = -biasFactor * inv_dt * dp; - } - else - { - bias.Set(0.0f, 0.0f); - } - - if (World::warmStarting) - { - // Apply accumulated impulse. - body1->velocity -= body1->invMass * P; - body1->angularVelocity -= body1->invI * Cross(r1, P); - - body2->velocity += body2->invMass * P; - body2->angularVelocity += body2->invI * Cross(r2, P); - } - else - { - P.Set(0.0f, 0.0f); - } -} - -void Joint::ApplyImpulse() -{ - Vec2 dv = body2->velocity + Cross(body2->angularVelocity, r2) - body1->velocity - Cross(body1->angularVelocity, r1); - - Vec2 impulse; - - impulse = M * (bias - dv - softness * P); - - body1->velocity -= body1->invMass * impulse; - body1->angularVelocity -= body1->invI * Cross(r1, impulse); - - body2->velocity += body2->invMass * impulse; - body2->angularVelocity += body2->invI * Cross(r2, impulse); - - P += impulse; -} - - diff --git a/Client/Source/Phy2D/Dynamic/Joint.h b/Client/Source/Phy2D/Dynamic/Joint.h deleted file mode 100644 index ee08b40..0000000 --- a/Client/Source/Phy2D/Dynamic/Joint.h +++ /dev/null @@ -1,34 +0,0 @@ -#pragma once - -#include "../Common/Math.h" - -namespace Phy2D -{ - - struct Body; - - struct Joint - { - Joint() : - body1(0), body2(0), - P(0.0f, 0.0f), - biasFactor(0.2f), softness(0.0f) - {} - - void Set(Body* body1, Body* body2, const Vec2& anchor); - - void PreStep(number inv_dt); - void ApplyImpulse(); - - Mat22 M; - Vec2 localAnchor1, localAnchor2; - Vec2 r1, r2; - Vec2 bias; - Vec2 P; // accumulated impulse - Body* body1; - Body* body2; - number biasFactor; - number softness; - }; - -} diff --git a/Client/Source/Phy2D/Dynamic/World.cpp b/Client/Source/Phy2D/Dynamic/World.cpp deleted file mode 100644 index 4f48b69..0000000 --- a/Client/Source/Phy2D/Dynamic/World.cpp +++ /dev/null @@ -1,129 +0,0 @@ -#include "World.h" -#include "Body.h" -#include "Joint.h" -#include "Arbiter.h" - -using namespace Phy2D; - - -using std::vector; -using std::map; -using std::pair; - -typedef map::iterator ArbIter; -typedef pair ArbPair; - -bool World::accumulateImpulses = true; -bool World::warmStarting = true; -bool World::positionCorrection = true; - -void World::Add(Body* body) -{ - bodies.push_back(body); -} - -void World::Add(Joint* joint) -{ - joints.push_back(joint); -} - -void World::Clear() -{ - bodies.clear(); - joints.clear(); - arbiters.clear(); -} - -void World::BroadPhase() -{ - // O(n^2) broad-phase - for (int i = 0; i < (int)bodies.size(); ++i) - { - Body* bi = bodies[i]; - - for (int j = i + 1; j < (int)bodies.size(); ++j) - { - Body* bj = bodies[j]; - - if (bi->invMass == 0.0f && bj->invMass == 0.0f) - continue; - - Arbiter newArb(bi, bj); - ArbiterKey key(bi, bj); - - if (newArb.numContacts > 0) - { - ArbIter iter = arbiters.find(key); - if (iter == arbiters.end()) - { - arbiters.insert(ArbPair(key, newArb)); - } - else - { - iter->second.Update(newArb.contacts, newArb.numContacts); - } - } - else - { - arbiters.erase(key); - } - } - } -} - -void World::Step(number dt) -{ - number inv_dt = dt > 0.0f ? (number)1.0f / dt : (number)0.0f; - - // Determine overlapping bodies and update contact points. - BroadPhase(); - - // Integrate forces. - for (int i = 0; i < (int)bodies.size(); ++i) - { - Body* b = bodies[i]; - - if (b->invMass == 0.0f) - continue; - - b->velocity += dt * (gravity + b->invMass * b->force); - b->angularVelocity += dt * b->invI * b->torque; - } - - // Perform pre-steps. - for (ArbIter arb = arbiters.begin(); arb != arbiters.end(); ++arb) - { - arb->second.PreStep(inv_dt); - } - - for (int i = 0; i < (int)joints.size(); ++i) - { - joints[i]->PreStep(inv_dt); - } - - // Perform iterations - for (int i = 0; i < iterations; ++i) - { - for (ArbIter arb = arbiters.begin(); arb != arbiters.end(); ++arb) - { - arb->second.ApplyImpulse(); - } - - for (int j = 0; j < (int)joints.size(); ++j) - { - joints[j]->ApplyImpulse(); - } - } - - // Integrate Velocities - for (int i = 0; i < (int)bodies.size(); ++i) - { - Body* b = bodies[i]; - - b->position += dt * b->velocity; - b->rotation += dt * b->angularVelocity; - - b->force.Set(0.0f, 0.0f); - b->torque = 0.0f; - } -} diff --git a/Client/Source/Phy2D/Dynamic/World.h b/Client/Source/Phy2D/Dynamic/World.h deleted file mode 100644 index 93c883d..0000000 --- a/Client/Source/Phy2D/Dynamic/World.h +++ /dev/null @@ -1,36 +0,0 @@ -#pragma once - -#include -#include -#include "../Common/Math.h" -#include "Arbiter.h" - -namespace Phy2D -{ - - struct Body; - struct Joint; - - struct World - { - World(Vec2 gravity, int iterations) : gravity(gravity), iterations(iterations) {} - - void Add(Body* body); - void Add(Joint* joint); - void Clear(); - - void Step(number dt); - - void BroadPhase(); - - std::vector bodies; - std::vector joints; - std::map arbiters; - Vec2 gravity; - int iterations; - static bool accumulateImpulses; - static bool warmStarting; - static bool positionCorrection; - }; - -} diff --git a/Client/Source/Phy2D/Phy2D.h b/Client/Source/Phy2D/Phy2D.h index d2e97f8..0fc0011 100644 --- a/Client/Source/Phy2D/Phy2D.h +++ b/Client/Source/Phy2D/Phy2D.h @@ -2,7 +2,3 @@ // 定点数的物理引擎 - -#include "Dynamic/World.h" -#include "Dynamic/Body.h" -#include "Dynamic/Joint.h" diff --git a/Client/Source/Phy2D/Shapes/p2CircleShape.cpp b/Client/Source/Phy2D/Shapes/p2CircleShape.cpp deleted file mode 100644 index e69de29..0000000 diff --git a/Client/Source/Phy2D/Shapes/p2CircleShape.h b/Client/Source/Phy2D/Shapes/p2CircleShape.h deleted file mode 100644 index a13a6ab..0000000 --- a/Client/Source/Phy2D/Shapes/p2CircleShape.h +++ /dev/null @@ -1,8 +0,0 @@ -#pragma once - -#include "p2Shape.h" - -class p2CircleShape : public p2Shape -{ - -}; diff --git a/Client/Source/Phy2D/Shapes/p2PolygonShape.cpp b/Client/Source/Phy2D/Shapes/p2PolygonShape.cpp deleted file mode 100644 index e69de29..0000000 diff --git a/Client/Source/Phy2D/Shapes/p2PolygonShape.h b/Client/Source/Phy2D/Shapes/p2PolygonShape.h deleted file mode 100644 index e69de29..0000000 diff --git a/Client/Source/Phy2D/Shapes/p2Shape.h b/Client/Source/Phy2D/Shapes/p2Shape.h deleted file mode 100644 index 4546314..0000000 --- a/Client/Source/Phy2D/Shapes/p2Shape.h +++ /dev/null @@ -1,7 +0,0 @@ -#pragma once - -class p2Shape -{ - -}; - diff --git a/Client/Source/Phy2D/Tests/test.h b/Client/Source/Phy2D/Tests/test.h index c814e3f..f3459ea 100644 --- a/Client/Source/Phy2D/Tests/test.h +++ b/Client/Source/Phy2D/Tests/test.h @@ -3,4 +3,4 @@ #define TEST_MATH 1 #define TEST_P2D 2 -#define TEST TEST_P2D +#define TEST TEST_MATH diff --git a/Client/Source/Phy2D/Tests/test_math.cpp b/Client/Source/Phy2D/Tests/test_math.cpp index e7642c1..f88170d 100644 --- a/Client/Source/Phy2D/Tests/test_math.cpp +++ b/Client/Source/Phy2D/Tests/test_math.cpp @@ -4,16 +4,17 @@ #include #include "../Common/Math.h" +#include "fix32/fix32.hpp" -using namespace Phy2D; using namespace std; int main(int argc, char **argv) { - Vec2 a = Vec2(1.f, 2.f); - Vec2 b = a; + Fix32 a = 1.3; + Fix32 b = 2.4; + Fix32 c = a * b; - cout << (a+b).ToString(); + cout << (double)c; getchar(); diff --git a/Client/Source/Phy2D/Tests/test_p2d.cpp b/Client/Source/Phy2D/Tests/test_p2d.cpp index 9050ccd..e69de29 100644 --- a/Client/Source/Phy2D/Tests/test_p2d.cpp +++ b/Client/Source/Phy2D/Tests/test_p2d.cpp @@ -1,625 +0,0 @@ -#include "test.h" -#if TEST == TEST_P2D - -#include "libfixmath/libfixmath/fixmath.h" -#include -#include -#include -#include "SDL2/SDL.h" -#include "../Rendering/Visualize.h" -#include "imgui/imgui.h" -#include "imgui/backends/imgui_impl_opengl2.h" -#include "imgui/backends/imgui_impl_sdl.h" - -#include "../Phy2D.h" - -using namespace std; -using namespace Phy2D; - -namespace -{ - Body bodies[200]; - Joint joints[100]; - - Body* bomb = NULL; - - float timeStep = 1.0f / 60.0f; - int iterations = 10; - Vec2 gravity(0.0f, -10.0f); - - int numBodies = 0; - int numJoints = 0; - - int demoIndex = 0; - - int width = 1280; - int height = 720; - float zoom = 10.0f; - float pan_y = 8.0f; - - World world(gravity, iterations); -} - -static void DrawBody(Body* body) -{ - Mat22 R(body->rotation); - Vec2 x = body->position; - Vec2 h = 0.5f * body->width; - - Vec2 v1 = x + R * Vec2(-h.x, -h.y); - Vec2 v2 = x + R * Vec2(h.x, -h.y); - Vec2 v3 = x + R * Vec2(h.x, h.y); - Vec2 v4 = x + R * Vec2(-h.x, h.y); - - if (body == bomb) - glColor3f(0.4f, 0.9f, 0.4f); - else - glColor3f(0.8f, 0.8f, 0.9f); - - glBegin(GL_LINE_LOOP); - glVertex2f(v1.x, v1.y); - glVertex2f(v2.x, v2.y); - glVertex2f(v3.x, v3.y); - glVertex2f(v4.x, v4.y); - glEnd(); -} - -static void DrawJoint(Joint* joint) -{ - Body* b1 = joint->body1; - Body* b2 = joint->body2; - - Mat22 R1(b1->rotation); - Mat22 R2(b2->rotation); - - Vec2 x1 = b1->position; - Vec2 p1 = x1 + R1 * joint->localAnchor1; - - Vec2 x2 = b2->position; - Vec2 p2 = x2 + R2 * joint->localAnchor2; - - glColor3f(0.5f, 0.5f, 0.8f); - glBegin(GL_LINES); - glVertex2f(x1.x, x1.y); - glVertex2f(p1.x, p1.y); - glVertex2f(x2.x, x2.y); - glVertex2f(p2.x, p2.y); - glEnd(); -} - -static void Demo1(Body* b, Joint* j) -{ - float x, y; - - b->Set(Vec2(100.0f, 20.0f), NUMBER_MAX); - b->position.Set(0.0f, (number)-0.5f * b->width.y); - world.Add(b); - ++b; ++numBodies; - - b->Set(Vec2(1.0f, 1.0f), 200.0f); - b->position.Set(0.0f, 4.0f); - world.Add(b); - ++b; ++numBodies; -} - -// A simple pendulum -static void Demo2(Body* b, Joint* j) -{ - Body* b1 = b + 0; - b1->Set(Vec2(100.0f, 20.0f), NUMBER_MAX); - b1->friction = 0.2f; - b1->position.Set(0.0f, (number)-0.5f * b1->width.y); - b1->rotation = 0.0f; - world.Add(b1); - - Body* b2 = b + 1; - b2->Set(Vec2(1.0f, 1.0f), 100.0f); - b2->friction = 0.2f; - b2->position.Set(9.0f, 11.0f); - b2->rotation = 0.0f; - world.Add(b2); - - numBodies += 2; - - j->Set(b1, b2, Vec2(0.0f, 11.0f)); - world.Add(j); - - numJoints += 1; -} - -// Varying friction coefficients -static void Demo3(Body* b, Joint* j) -{ - b->Set(Vec2(100.0f, 20.0f), NUMBER_MAX); - b->position.Set(0.0f, (number)-0.5f * b->width.y); - world.Add(b); - ++b; ++numBodies; - - b->Set(Vec2(13.0f, 0.25f), NUMBER_MAX); - b->position.Set(-2.0f, 11.0f); - b->rotation = -0.25f; - world.Add(b); - ++b; ++numBodies; - - b->Set(Vec2(0.25f, 1.0f), NUMBER_MAX); - b->position.Set(5.25f, 9.5f); - world.Add(b); - ++b; ++numBodies; - - b->Set(Vec2(13.0f, 0.25f), NUMBER_MAX); - b->position.Set(2.0f, 7.0f); - b->rotation = 0.25f; - world.Add(b); - ++b; ++numBodies; - - b->Set(Vec2(0.25f, 1.0f), NUMBER_MAX); - b->position.Set(-5.25f, 5.5f); - world.Add(b); - ++b; ++numBodies; - - b->Set(Vec2(13.0f, 0.25f), NUMBER_MAX); - b->position.Set(-2.0f, 3.0f); - b->rotation = -0.25f; - world.Add(b); - ++b; ++numBodies; - - float friction[5] = { 0.75f, 0.5f, 0.35f, 0.1f, 0.0f }; - for (int i = 0; i < 5; ++i) - { - b->Set(Vec2(0.5f, 0.5f), 25.0f); - b->friction = friction[i]; - b->position.Set(-7.5f + 2.0f * i, 14.0f); - world.Add(b); - ++b; ++numBodies; - } -} - -// A vertical stack -static void Demo4(Body* b, Joint* j) -{ - b->Set(Vec2(100.0f, 20.0f), NUMBER_MAX); - b->friction = 0.2f; - b->position.Set(0.0f, (number)-0.5f * b->width.y); - b->rotation = 0.0f; - world.Add(b); - ++b; ++numBodies; - - for (int i = 0; i < 10; ++i) - { - b->Set(Vec2(1.0f, 1.0f), 1.0f); - b->friction = 0.2f; - //float x = Random(-0.1f, 0.1f); - float x = 0; - b->position.Set(x, 0.51f + 1.05f * i); - world.Add(b); - ++b; ++numBodies; - } -} - -// A pyramid -static void Demo5(Body* b, Joint* j) -{ - b->Set(Vec2(100.0f, 20.0f), NUMBER_MAX); - b->friction = 0.2f; - b->position.Set(0.0f, (number)-0.5f * b->width.y); - b->rotation = 0.0f; - world.Add(b); - ++b; ++numBodies; - - Vec2 x(-6.0f, 0.75f); - Vec2 y; - - for (int i = 0; i < 12; ++i) - { - y = x; - - for (int j = i; j < 12; ++j) - { - b->Set(Vec2(1.0f, 1.0f), 10.0f); - b->friction = 0.2f; - b->position = y; - world.Add(b); - ++b; ++numBodies; - - y += Vec2(1.125f, 0.0f); - } - - //x += Vec2(0.5625f, 1.125f); - x += Vec2(0.5625f, 2.0f); - } -} - -// A teeter -static void Demo6(Body* b, Joint* j) -{ - Body* b1 = b + 0; - b1->Set(Vec2(100.0f, 20.0f), NUMBER_MAX); - b1->position.Set(0.0f, (number)-0.5f * b1->width.y); - world.Add(b1); - - Body* b2 = b + 1; - b2->Set(Vec2(12.0f, 0.25f), 100.0f); - b2->position.Set(0.0f, 1.0f); - world.Add(b2); - - Body* b3 = b + 2; - b3->Set(Vec2(0.5f, 0.5f), 25.0f); - b3->position.Set(-5.0f, 2.0f); - world.Add(b3); - - Body* b4 = b + 3; - b4->Set(Vec2(0.5f, 0.5f), 25.0f); - b4->position.Set(-5.5f, 2.0f); - world.Add(b4); - - Body* b5 = b + 4; - b5->Set(Vec2(1.0f, 1.0f), 100.0f); - b5->position.Set(5.5f, 15.0f); - world.Add(b5); - - numBodies += 5; - - j->Set(b1, b2, Vec2(0.0f, 1.0f)); - world.Add(j); - - numJoints += 1; -} - -// A suspension bridge -static void Demo7(Body* b, Joint* j) -{ - b->Set(Vec2(100.0f, 20.0f), NUMBER_MAX); - b->friction = 0.2f; - b->position.Set(0.0f, (number)-0.5f * b->width.y); - b->rotation = 0.0f; - world.Add(b); - ++b; ++numBodies; - - const int numPlanks = 15; - float mass = 50.0f; - - for (int i = 0; i < numPlanks; ++i) - { - b->Set(Vec2(1.0f, 0.25f), mass); - b->friction = 0.2f; - b->position.Set(-8.5f + 1.25f * i, 5.0f); - world.Add(b); - ++b; ++numBodies; - } - - // Tuning - float frequencyHz = 2.0f; - float dampingRatio = 0.7f; - - // frequency in radians - float omega = (number)2.0f * fix16_pi * frequencyHz; - - // damping coefficient - float d = 2.0f * mass * dampingRatio * omega; - - // spring stifness - float k = mass * omega * omega; - - // magic formulas - float softness = 1.0f / (d + timeStep * k); - float biasFactor = timeStep * k / (d + timeStep * k); - - for (int i = 0; i < numPlanks; ++i) - { - j->Set(bodies + i, bodies + i + 1, Vec2(-9.125f + 1.25f * i, 5.0f)); - j->softness = softness; - j->biasFactor = biasFactor; - - world.Add(j); - ++j; ++numJoints; - } - - j->Set(bodies + numPlanks, bodies, Vec2(-9.125f + 1.25f * numPlanks, 5.0f)); - j->softness = softness; - j->biasFactor = biasFactor; - world.Add(j); - ++j; ++numJoints; -} - -// Dominos -static void Demo8(Body* b, Joint* j) -{ - Body* b1 = b; - b->Set(Vec2(100.0f, 20.0f), NUMBER_MAX); - b->position.Set(0.0f, (number)-0.5f * b->width.y); - world.Add(b); - ++b; ++numBodies; - - b->Set(Vec2(12.0f, 0.5f), NUMBER_MAX); - b->position.Set(-1.5f, 10.0f); - world.Add(b); - ++b; ++numBodies; - - for (int i = 0; i < 10; ++i) - { - b->Set(Vec2(0.2f, 2.0f), 10.0f); - b->position.Set(-6.0f + 1.0f * i, 11.125f); - b->friction = 0.1f; - world.Add(b); - ++b; ++numBodies; - } - - b->Set(Vec2(14.0f, 0.5f), NUMBER_MAX); - b->position.Set(1.0f, 6.0f); - b->rotation = 0.3f; - world.Add(b); - ++b; ++numBodies; - - Body* b2 = b; - b->Set(Vec2(0.5f, 3.0f), NUMBER_MAX); - b->position.Set(-7.0f, 4.0f); - world.Add(b); - ++b; ++numBodies; - - Body* b3 = b; - b->Set(Vec2(12.0f, 0.25f), 20.0f); - b->position.Set(-0.9f, 1.0f); - world.Add(b); - ++b; ++numBodies; - - j->Set(b1, b3, Vec2(-2.0f, 1.0f)); - world.Add(j); - ++j; ++numJoints; - - Body* b4 = b; - b->Set(Vec2(0.5f, 0.5f), 10.0f); - b->position.Set(-10.0f, 15.0f); - world.Add(b); - ++b; ++numBodies; - - j->Set(b2, b4, Vec2(-7.0f, 15.0f)); - world.Add(j); - ++j; ++numJoints; - - Body* b5 = b; - b->Set(Vec2(2.0f, 2.0f), 20.0f); - b->position.Set(6.0f, 2.5f); - b->friction = 0.1f; - world.Add(b); - ++b; ++numBodies; - - j->Set(b1, b5, Vec2(6.0f, 2.6f)); - world.Add(j); - ++j; ++numJoints; - - Body* b6 = b; - b->Set(Vec2(2.0f, 0.2f), 10.0f); - b->position.Set(6.0f, 3.6f); - world.Add(b); - ++b; ++numBodies; - - j->Set(b5, b6, Vec2(7.0f, 3.5f)); - world.Add(j); - ++j; ++numJoints; -} - -// A multi-pendulum -static void Demo9(Body* b, Joint* j) -{ - b->Set(Vec2(100.0f, 20.0f), NUMBER_MAX); - b->friction = 0.2f; - b->position.Set(0.0f, (number)-0.5f * b->width.y); - b->rotation = 0.0f; - world.Add(b); - - Body * b1 = b; - ++b; - ++numBodies; - - float mass = 10.0f; - - // Tuning - float frequencyHz = 4.0f; - float dampingRatio = 0.7f; - - // frequency in radians - float omega = (number) 2.0f * fix16_pi * frequencyHz; - - // damping coefficient - float d = 2.0f * mass * dampingRatio * omega; - - // spring stiffness - float k = mass * omega * omega; - - // magic formulas - float softness = 1.0f / (d + timeStep * k); - float biasFactor = timeStep * k / (d + timeStep * k); - - const float y = 12.0f; - - for (int i = 0; i < 15; ++i) - { - Vec2 x(0.5f + i, y); - b->Set(Vec2(0.75f, 0.25f), mass); - b->friction = 0.2f; - b->position = x; - b->rotation = 0.0f; - world.Add(b); - - j->Set(b1, b, Vec2(float(i), y)); - j->softness = softness; - j->biasFactor = biasFactor; - world.Add(j); - - b1 = b; - ++b; - ++numBodies; - ++j; - ++numJoints; - } -} - -static void InitDemo() -{ - world.Clear(); - numBodies = 0; - numJoints = 0; - bomb = NULL; - - demoIndex = 0; - Demo5(bodies, joints); -} - -int main(int argc, char **argv) { - - // Setup SDL - // (Some versions of SDL before <2.0.10 appears to have performance/stalling issues on a minority of Windows systems, - // depending on whether SDL_INIT_GAMECONTROLLER is enabled or disabled.. updating to latest version of SDL is recommended!) - if (SDL_Init(SDL_INIT_VIDEO | SDL_INIT_TIMER | SDL_INIT_GAMECONTROLLER) != 0) - { - printf("Error: %s\n", SDL_GetError()); - return -1; - } - - // Setup window - SDL_GL_SetAttribute(SDL_GL_DOUBLEBUFFER, 1); - SDL_GL_SetAttribute(SDL_GL_DEPTH_SIZE, 24); - SDL_GL_SetAttribute(SDL_GL_STENCIL_SIZE, 8); - SDL_GL_SetAttribute(SDL_GL_CONTEXT_MAJOR_VERSION, 2); - SDL_GL_SetAttribute(SDL_GL_CONTEXT_MINOR_VERSION, 2); - SDL_WindowFlags window_flags = (SDL_WindowFlags)(SDL_WINDOW_OPENGL | SDL_WINDOW_RESIZABLE | SDL_WINDOW_ALLOW_HIGHDPI); - SDL_Window* window = SDL_CreateWindow("Phy2D", SDL_WINDOWPOS_CENTERED, SDL_WINDOWPOS_CENTERED, width, height, window_flags); - SDL_GLContext gl_context = SDL_GL_CreateContext(window); - - if (!gladLoadGLLoader((GLADloadproc)SDL_GL_GetProcAddress)) { - std::cerr << "Failed to initialize the OpenGL context." << std::endl; - exit(1); - } - - SDL_GL_MakeCurrent(window, gl_context); - SDL_GL_SetSwapInterval(1); // Enable vsync - - // Setup Dear ImGui context - IMGUI_CHECKVERSION(); - ImGui::CreateContext(); - ImGuiIO& io = ImGui::GetIO(); (void)io; - //io.ConfigFlags |= ImGuiConfigFlags_NavEnableKeyboard; // Enable Keyboard Controls - //io.ConfigFlags |= ImGuiConfigFlags_NavEnableGamepad; // Enable Gamepad Controls - - // Setup Dear ImGui style - ImGui::StyleColorsDark(); - //ImGui::StyleColorsClassic(); - - // Setup Platform/Renderer backends - ImGui_ImplSDL2_InitForOpenGL(window, gl_context); - ImGui_ImplOpenGL2_Init(); - - // Load Fonts - // - If no fonts are loaded, dear imgui will use the default font. You can also load multiple fonts and use ImGui::PushFont()/PopFont() to select them. - // - AddFontFromFileTTF() will return the ImFont* so you can store it if you need to select the font among multiple. - // - If the file cannot be loaded, the function will return NULL. Please handle those errors in your application (e.g. use an assertion, or display an error and quit). - // - The fonts will be rasterized at a given size (w/ oversampling) and stored into a texture when calling ImFontAtlas::Build()/GetTexDataAsXXXX(), which ImGui_ImplXXXX_NewFrame below will call. - // - Read 'docs/FONTS.md' for more instructions and details. - // - Remember that in C/C++ if you want to include a backslash \ in a string literal you need to write a double backslash \\ ! - //io.Fonts->AddFontDefault(); - //io.Fonts->AddFontFromFileTTF("../../misc/fonts/Roboto-Medium.ttf", 16.0f); - //io.Fonts->AddFontFromFileTTF("../../misc/fonts/Cousine-Regular.ttf", 15.0f); - //io.Fonts->AddFontFromFileTTF("../../misc/fonts/DroidSans.ttf", 16.0f); - //io.Fonts->AddFontFromFileTTF("../../misc/fonts/ProggyTiny.ttf", 10.0f); - //ImFont* font = io.Fonts->AddFontFromFileTTF("c:\\Windows\\Fonts\\ArialUni.ttf", 18.0f, NULL, io.Fonts->GetGlyphRangesJapanese()); - //IM_ASSERT(font != NULL); - - // Our state - bool show_demo_window = true; - bool show_another_window = false; - ImVec4 clear_color = ImVec4(0.45f, 0.55f, 0.60f, 1.00f); - - glViewport(0, 0, (int)io.DisplaySize.x, (int)io.DisplaySize.y); - glMatrixMode(GL_PROJECTION); - glLoadIdentity(); - - float aspect = float(width) / float(height); - if (width >= height) - { - // aspect >= 1, set the height from -1 to 1, with larger width - glOrtho(-zoom * aspect, zoom * aspect, -zoom + pan_y, zoom + pan_y, -1.0, 1.0); - } - else - { - // aspect < 1, set the width to -1 to 1, with larger height - glOrtho(-zoom, zoom, -zoom / aspect + pan_y, zoom / aspect + pan_y, -1.0, 1.0); - } - - InitDemo(); - - // Main loop - bool done = false; - while (!done) - { - //glClearColor(clear_color.x * clear_color.w, clear_color.y * clear_color.w, clear_color.z * clear_color.w, clear_color.w); - glClearColor(0, 0, 0, 0); - glClear(GL_COLOR_BUFFER_BIT); - - // Poll and handle events (inputs, window resize, etc.) - // You can read the io.WantCaptureMouse, io.WantCaptureKeyboard flags to tell if dear imgui wants to use your inputs. - // - When io.WantCaptureMouse is true, do not dispatch mouse input data to your main application. - // - When io.WantCaptureKeyboard is true, do not dispatch keyboard input data to your main application. - // Generally you may always pass all inputs to dear imgui, and hide them from your application based on those two flags. - SDL_Event event; - while (SDL_PollEvent(&event)) - { - ImGui_ImplSDL2_ProcessEvent(&event); - if (event.type == SDL_QUIT) - done = true; - if (event.type == SDL_WINDOWEVENT && event.window.event == SDL_WINDOWEVENT_CLOSE && event.window.windowID == SDL_GetWindowID(window)) - done = true; - } - - // Start the Dear ImGui frame - ImGui_ImplOpenGL2_NewFrame(); - ImGui_ImplSDL2_NewFrame(); - ImGui::NewFrame(); - - // Rendering - ImGui::Render(); - - glMatrixMode(GL_MODELVIEW); - glLoadIdentity(); - - world.Step(timeStep); - - for (int i = 0; i < numBodies; ++i) - DrawBody(bodies + i); - - for (int i = 0; i < numJoints; ++i) - DrawJoint(joints + i); - - glPointSize(4.0f); - glColor3f(1.0f, 0.0f, 0.0f); - glBegin(GL_POINTS); - std::map::const_iterator iter; - for (iter = world.arbiters.begin(); iter != world.arbiters.end(); ++iter) - { - const Arbiter& arbiter = iter->second; - for (int i = 0; i < arbiter.numContacts; ++i) - { - Vec2 p = arbiter.contacts[i].position; - glVertex2f(p.x, p.y); - } - } - glEnd(); - glPointSize(1.0f); - - //glUseProgram(0); // You may want this if using this code in an OpenGL 3+ context where shaders may be bound - ImGui_ImplOpenGL2_RenderDrawData(ImGui::GetDrawData()); - SDL_GL_SwapWindow(window); - } - - // Cleanup - ImGui_ImplOpenGL2_Shutdown(); - ImGui_ImplSDL2_Shutdown(); - ImGui::DestroyContext(); - - SDL_GL_DeleteContext(gl_context); - SDL_DestroyWindow(window); - SDL_Quit(); - - return 0; -} - -#endif \ No newline at end of file diff --git a/Client/Source/Phy2DLite/Constants.h b/Client/Source/Phy2DLite/Constants.h index f116ca8..59a8c09 100644 --- a/Client/Source/Phy2DLite/Constants.h +++ b/Client/Source/Phy2DLite/Constants.h @@ -5,7 +5,7 @@ namespace Phy2D { #if NUMBER_ALIAS == NUMBER_FPM -#define CONSTANT(name, value) static fixed name = fixed::from_raw_value(value) +#define CONSTANT(name, value) static constexpr fixed name = fixed::from_raw_value(value) CONSTANT(_1, 65536); CONSTANT(_n1, -65536); @@ -16,5 +16,27 @@ namespace Phy2D CONSTANT(_0_95, 62259); CONSTANT(_12, 786432); +#elif NUMBER_ALIAS == NUMBER_FLOAT + + static const float _1 = 1; + static const float _n1 = -1; + static const float _0 = 0; + static const float _0_01 = 0.01; + static const float _0_2 = 0.2; + static const float _0_5 = 0.5; + static const float _0_95 = 0.95; + static const float _12 = 12; + +#elif NUMBER_ALIAS == NUMBER_FIX32 + + static const float _1 = 1; + static const float _n1 = -1; + static const float _0 = 0; + static const float _0_01 = 0.01; + static const float _0_2 = 0.2; + static const float _0_5 = 0.5; + static const float _0_95 = 0.95; + static const float _12 = 12; + #endif } \ No newline at end of file diff --git a/Client/Source/Phy2DLite/Settings.h b/Client/Source/Phy2DLite/Settings.h index 9345b32..6454943 100644 --- a/Client/Source/Phy2DLite/Settings.h +++ b/Client/Source/Phy2DLite/Settings.h @@ -1,16 +1,19 @@ #pragma once -#define NUMBER_FLOAT 1 -#define NUMBER_LIBFIX 2 -#define NUMBER_FPM 3 +#define NUMBER_FLOAT 1 +#define NUMBER_LIBFIX 2 +#define NUMBER_FPM 3 +#define NUMBER_FIX32 4 -#define NUMBER_ALIAS NUMBER_FPM +#define NUMBER_ALIAS NUMBER_FIX32 #if NUMBER_ALIAS == NUMBER_LIBFIX #include "libfixmath/libfixmath/fixmath.h" #elif NUMBER_ALIAS == NUMBER_FPM #include "fpm/include/fpm/fixed.hpp" #include "fpm/include/fpm/math.hpp" +#elif NUMBER_ALIAS == NUMBER_FIX32 +#include "fix32/fix32.hpp" #endif namespace Phy2D @@ -24,10 +27,10 @@ typedef float fixed; #define SQRT(a) (sqrt((a))) #define SIN(a) (sin((a))) #define COS(a) (cos((a))) +#define PI (3.1415925f) #elif NUMBER_ALIAS == NUMBER_LIBFIX -// 同时一定要开启内联函数扩展，否则执行效率会非常低 typedef Fix16 fixed; #define NUMBER_MAX (fix16_maximum) #define NUMBER_MIN (fix16_minimum) @@ -91,6 +94,16 @@ typedef fpm::fixed_16_16 fixed; #define COS(a) (fpm::cos((a))) #define PI (fixed::pi()) +#elif NUMBER_ALIAS == NUMBER_FIX32 + + typedef Fix32 fixed; +#define NUMBER_MAX (CONST_MAX) +#define NUMBER_MIN (CONST_MIN) +#define SQRT(a) ((a).sqrt()) +#define SIN(a) ((a).sin()) +#define COS(a) ((a).cos()) +#define PI (CONST_PI) + #endif } \ No newline at end of file diff --git a/Client/Source/Phy2DLite/World.cpp b/Client/Source/Phy2DLite/World.cpp index f20be5f..76e57ea 100644 --- a/Client/Source/Phy2DLite/World.cpp +++ b/Client/Source/Phy2DLite/World.cpp @@ -73,7 +73,7 @@ void World::BroadPhase() void World::Step(fixed dt) { - fixed inv_dt = dt > _0 ? _1 / dt : _0; + fixed inv_dt = dt > _0 ? (fixed)_1 / dt : (fixed)_0; // Determine overlapping bodies and update contact points. BroadPhase(); diff --git a/Client/ThirdParty/fix32/fix32.cpp b/Client/ThirdParty/fix32/fix32.cpp deleted file mode 100644 index e69de29..0000000 diff --git a/Client/ThirdParty/fix32/fix32.h b/Client/ThirdParty/fix32/fix32.h deleted file mode 100644 index fb3d65c..0000000 --- a/Client/ThirdParty/fix32/fix32.h +++ /dev/null @@ -1,7 +0,0 @@ -#pragma once - -#include - -// -2,147,483,648 to 2,147,483,647 -typedef int64_t fix32_t; - diff --git a/Client/ThirdParty/fix32/fix32.hpp b/Client/ThirdParty/fix32/fix32.hpp new file mode 100644 index 0000000..7999ad4 --- /dev/null +++ b/Client/ThirdParty/fix32/fix32.hpp @@ -0,0 +1,161 @@ +#pragma once + +#include +extern "C" { +#include "math-sll/math-sll.h" +} + +typedef sll fixed32_t; + +// Q32.32 +class Fix32 +{ +public: + sll value; + + inline Fix32() { value = 0; } + inline Fix32(const Fix32 &inValue) { value = inValue.value; } + inline Fix32(const float inValue) { value = dbl2sll(inValue); } + inline Fix32(const double inValue) { value = dbl2sll(inValue); } + inline Fix32(const int32_t inValue) { value = int2sll(inValue); } + inline Fix32(const sll inValue) { value = inValue; } + + inline operator sll() const { return value; } + inline operator double() const { return sll2dbl(value); } + inline operator float() const { return (float)sll2dbl(value); } + inline operator int32_t() const { return (int32_t)sll2int(value); } + + inline Fix32 & operator=(const Fix32 &rhs) { value = rhs.value; return *this; } + inline Fix32 & operator=(const sll rhs) { value = rhs; return *this; } + inline Fix32 & operator=(const double rhs) { value = dbl2sll(rhs); return *this; } + inline Fix32 & operator=(const float rhs) { value = (float)dbl2sll(rhs); return *this; } + inline Fix32 & operator=(const int32_t rhs) { value = int2sll(rhs); return *this; } + + inline Fix32 & operator+=(const Fix32 &rhs) { value += rhs.value; return *this; } + inline Fix32 & operator+=(const sll rhs) { value += rhs; return *this; } + inline Fix32 & operator+=(const double rhs) { value += dbl2sll(rhs); return *this; } + inline Fix32 & operator+=(const float rhs) { value += (float)dbl2sll(rhs); return *this; } + inline Fix32 & operator+=(const int32_t rhs) { value += int2sll(rhs); return *this; } + + inline Fix32 & operator-=(const Fix32 &rhs) { value -= rhs.value; return *this; } + inline Fix32 & operator-=(const sll rhs) { value -= rhs; return *this; } + inline Fix32 & operator-=(const double rhs) { value -= dbl2sll(rhs); return *this; } + inline Fix32 & operator-=(const float rhs) { value -= (float)dbl2sll(rhs); return *this; } + inline Fix32 & operator-=(const int32_t rhs) { value -= int2sll(rhs); return *this; } + + inline Fix32 & operator*=(const Fix32 &rhs) { value = sllmul(value, rhs.value); return *this; } + inline Fix32 & operator*=(const sll rhs) { value = sllmul(value, rhs); return *this; } + inline Fix32 & operator*=(const double rhs) { value = sllmul(value, dbl2sll(rhs)); return *this; } + inline Fix32 & operator*=(const float rhs) { value = sllmul(value, (float)dbl2sll(rhs)); return *this; } + inline Fix32 & operator*=(const int32_t rhs) { value *= rhs; return *this; } + + inline Fix32 & operator/=(const Fix32 &rhs) { value = slldiv(value, rhs.value); return *this; } + inline Fix32 & operator/=(const sll rhs) { value = slldiv(value, rhs); return *this; } + inline Fix32 & operator/=(const double rhs) { value = slldiv(value, dbl2sll(rhs)); return *this; } + inline Fix32 & operator/=(const float rhs) { value = slldiv(value, (float)dbl2sll(rhs)); return *this; } + inline Fix32 & operator/=(const int32_t rhs) { value /= rhs; return *this; } + + inline const Fix32 operator+(const Fix32 &other) const { Fix32 ret = *this; ret += other; return ret; } + inline const Fix32 operator+(const sll other) const { Fix32 ret = *this; ret += other; return ret; } + inline const Fix32 operator+(const double other) const { Fix32 ret = *this; ret += other; return ret; } + inline const Fix32 operator+(const float other) const { Fix32 ret = *this; ret += other; return ret; } + inline const Fix32 operator+(const int32_t other) const { Fix32 ret = *this; ret += other; return ret; } + +#ifndef FIXMATH_NO_OVERFLOW + inline const Fix32 sadd(const Fix32 &other) const { Fix32 ret = slladd(value, other.value); return ret; } + inline const Fix32 sadd(const sll other) const { Fix32 ret = slladd(value, other); return ret; } + inline const Fix32 sadd(const double other) const { Fix32 ret = slladd(value, dbl2sll(other)); return ret; } + inline const Fix32 sadd(const float other) const { Fix32 ret = slladd(value, (float)dbl2sll(other)); return ret; } + inline const Fix32 sadd(const int32_t other) const { Fix32 ret = slladd(value, int2sll(other)); return ret; } +#endif + + inline const Fix32 operator-(const Fix32 &other) const { Fix32 ret = *this; ret -= other; return ret; } + inline const Fix32 operator-(const sll other) const { Fix32 ret = *this; ret -= other; return ret; } + inline const Fix32 operator-(const double other) const { Fix32 ret = *this; ret -= other; return ret; } + inline const Fix32 operator-(const float other) const { Fix32 ret = *this; ret -= other; return ret; } + inline const Fix32 operator-(const int32_t other) const { Fix32 ret = *this; ret -= other; return ret; } + + inline const Fix32 operator-() const { Fix32 ret = sllsub(0, value); return ret; } + +#ifndef FIXMATH_NO_OVERFLOW + inline const Fix32 ssub(const Fix32 &other) const { Fix32 ret = slladd(value, -other.value); return ret; } + inline const Fix32 ssub(const sll other) const { Fix32 ret = slladd(value, -other); return ret; } + inline const Fix32 ssub(const double other) const { Fix32 ret = slladd(value, -dbl2sll(other)); return ret; } + inline const Fix32 ssub(const float other) const { Fix32 ret = slladd(value, -(float)dbl2sll(other)); return ret; } + inline const Fix32 ssub(const int32_t other) const { Fix32 ret = slladd(value, -int2sll(other)); return ret; } +#endif + + inline const Fix32 operator*(const Fix32 &other) const { Fix32 ret = *this; ret *= other; return ret; } + inline const Fix32 operator*(const sll other) const { Fix32 ret = *this; ret *= other; return ret; } + inline const Fix32 operator*(const double other) const { Fix32 ret = *this; ret *= other; return ret; } + inline const Fix32 operator*(const float other) const { Fix32 ret = *this; ret *= other; return ret; } + inline const Fix32 operator*(const int32_t other) const { Fix32 ret = *this; ret *= other; return ret; } + +#ifndef FIXMATH_NO_OVERFLOW + inline const Fix32 smul(const Fix32 &other) const { Fix32 ret = sllmul(value, other.value); return ret; } + inline const Fix32 smul(const sll other) const { Fix32 ret = sllmul(value, other); return ret; } + inline const Fix32 smul(const double other) const { Fix32 ret = sllmul(value, dbl2sll(other)); return ret; } + inline const Fix32 smul(const float other) const { Fix32 ret = sllmul(value, (float)dbl2sll(other)); return ret; } + inline const Fix32 smul(const int32_t other) const { Fix32 ret = sllmul(value, int2sll(other)); return ret; } +#endif + + inline const Fix32 operator/(const Fix32 &other) const { Fix32 ret = *this; ret /= other; return ret; } + inline const Fix32 operator/(const sll other) const { Fix32 ret = *this; ret /= other; return ret; } + inline const Fix32 operator/(const double other) const { Fix32 ret = *this; ret /= other; return ret; } + inline const Fix32 operator/(const float other) const { Fix32 ret = *this; ret /= other; return ret; } + inline const Fix32 operator/(const int32_t other) const { Fix32 ret = *this; ret /= other; return ret; } + +#ifndef FIXMATH_NO_OVERFLOW + inline const Fix32 sdiv(const Fix32 &other) const { Fix32 ret = slldiv(value, other.value); return ret; } + inline const Fix32 sdiv(const sll other) const { Fix32 ret = slldiv(value, other); return ret; } + inline const Fix32 sdiv(const double other) const { Fix32 ret = slldiv(value, dbl2sll(other)); return ret; } + inline const Fix32 sdiv(const float other) const { Fix32 ret = slldiv(value, (float)dbl2sll(other)); return ret; } + inline const Fix32 sdiv(const int32_t other) const { Fix32 ret = slldiv(value, int2sll(other)); return ret; } +#endif + + inline int operator==(const Fix32 &other) const { return (value == other.value); } + inline int operator==(const sll other) const { return (value == other); } + inline int operator==(const double other) const { return (value == dbl2sll(other)); } + inline int operator==(const float other) const { return (value == (float)dbl2sll(other)); } + inline int operator==(const int32_t other) const { return (value == int2sll(other)); } + + inline int operator!=(const Fix32 &other) const { return (value != other.value); } + inline int operator!=(const sll other) const { return (value != other); } + inline int operator!=(const double other) const { return (value != dbl2sll(other)); } + inline int operator!=(const float other) const { return (value != (float)dbl2sll(other)); } + inline int operator!=(const int32_t other) const { return (value != int2sll(other)); } + + inline int operator<=(const Fix32 &other) const { return (value <= other.value); } + inline int operator<=(const sll other) const { return (value <= other); } + inline int operator<=(const double other) const { return (value <= dbl2sll(other)); } + inline int operator<=(const float other) const { return (value <= (float)dbl2sll(other)); } + inline int operator<=(const int32_t other) const { return (value <= int2sll(other)); } + + inline int operator>=(const Fix32 &other) const { return (value >= other.value); } + inline int operator>=(const sll other) const { return (value >= other); } + inline int operator>=(const double other) const { return (value >= dbl2sll(other)); } + inline int operator>=(const float other) const { return (value >= (float)dbl2sll(other)); } + inline int operator>=(const int32_t other) const { return (value >= int2sll(other)); } + + inline int operator< (const Fix32 &other) const { return (value < other.value); } + inline int operator< (const sll other) const { return (value < other); } + inline int operator< (const double other) const { return (value < dbl2sll(other)); } + inline int operator< (const float other) const { return (value < (float)dbl2sll(other)); } + inline int operator< (const int32_t other) const { return (value < int2sll(other)); } + + inline int operator> (const Fix32 &other) const { return (value > other.value); } + inline int operator> (const sll other) const { return (value > other); } + inline int operator> (const double other) const { return (value > dbl2sll(other)); } + inline int operator> (const float other) const { return (value > (float)dbl2sll(other)); } + inline int operator> (const int32_t other) const { return (value > int2sll(other)); } + + inline Fix32 sin() const { return Fix32(sllsin(value)); } + inline Fix32 cos() const { return Fix32(sllcos(value)); } + inline Fix32 tan() const { return Fix32(slltan(value)); } + inline Fix32 asin() const { return Fix32(sllasin(value)); } + inline Fix32 acos() const { return Fix32(sllacos(value)); } + inline Fix32 atan() const { return Fix32(sllatan(value)); } + //inline Fix32 atan2(const Fix32 &inY) const { return Fix32(fix16_atan2(value, inY.value)); } + inline Fix32 sqrt() const { return Fix32(sllsqrt(value)); } + +}; \ No newline at end of file diff --git a/Client/ThirdParty/math-sll/LICENSE b/Client/ThirdParty/math-sll/LICENSE new file mode 100644 index 0000000..4e683cf --- /dev/null +++ b/Client/ThirdParty/math-sll/LICENSE @@ -0,0 +1,23 @@ + +Licensed under the terms of the MIT license: + +Copyright (c) 2000,2002,2006,2012,2016 Andrew E. Mileski + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to +deal in the Software without restriction, including without limitation the +rights to use, copy, modify, merge, publish, distribute, sublicense, and/or +sell copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The copyright notice, and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER +DEALINGS IN THE SOFTWARE. + diff --git a/Client/ThirdParty/math-sll/Makefile b/Client/ThirdParty/math-sll/Makefile new file mode 100644 index 0000000..b7b512f --- /dev/null +++ b/Client/ThirdParty/math-sll/Makefile @@ -0,0 +1,69 @@ +# +# Licensed under the terms of the MIT license: +# +# Copyright (c) 2000,2002,2006,2012,2016 Andrew E. Mileski +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to +# deal in the Software without restriction, including without limitation the +# rights to use, copy, modify, merge, publish, distribute, sublicense, and/or +# sell copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The copyright notice, and this permission notice shall be included in all +# copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +# FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER +# DEALINGS IN THE SOFTWARE. +# + +# +# Installation directories +# + +PREFIX := /usr/local +INCDIR := $(PREFIX)/include +LIBDIR := $(PREFIX)/lib + +# +# Executables +# + +AR := ar +CC := gcc +CFLAGS := -O2 -W -Wall +INSTALL := install +RANLIB := ranlib +RM := rm -f +STRIP := strip --strip-unneeded + +# +# Recipes +# + +OBJS := math-sll.o +LIBS := math-sll.a + +.PHONY: all clean install + +all: $(LIBS) + +clean: + $(RM) $(LIBS) $(OBJS) + +install: $(LIBS) math-sll.h + $(INSTALL) -m a=rx,u+w math-sll.a $(LIBDIR) + $(INSTALL) -m a=r,u+w math-sll.h $(INCDIR) + +math-sll.o: math-sll.c math-sll.h + +math-sll.a: math-sll.o + $(STRIP) $< + $(AR) rcs $@ $< + $(RANLIB) $@ + diff --git a/Client/ThirdParty/math-sll/README b/Client/ThirdParty/math-sll/README new file mode 100644 index 0000000..e53ca36 --- /dev/null +++ b/Client/ThirdParty/math-sll/README @@ -0,0 +1,51 @@ + +math-sll + + A fixed point (32.32 bit) math library. + + See math-sll.h for details. + +License + + Licensed under the terms of the MIT license as of revision v1.19 + + See the LICENSE file for details. + + Earlier revisions were licensed under the terms of GPL or LGPL. + +Installation + + The source can be added as-is into your project, or a library can optionally + be built and installed, then statically-linked into your project. + + To build: + + make clean + make all + + Optionally: + + make install + + The default installation path prefix is /usr/local + + See the Makefile for details. + +Repository + + A math-sll project is being maintained at: + + http://github.com/aemileski/math-sll + + You can also obtain a copy of the latest source code directly via GIT: + + git clone git://github.com/aemileski/math-sll.git + +Contact + + Feel free to contact me with questions, bug reports, patches, feature + requests, etc., or just to tell me about how you are using math-sll in + your project! + + Andrew E. Mileski + diff --git a/Client/ThirdParty/math-sll/math-sll.c b/Client/ThirdParty/math-sll/math-sll.c new file mode 100644 index 0000000..ecc4238 --- /dev/null +++ b/Client/ThirdParty/math-sll/math-sll.c @@ -0,0 +1,957 @@ +/* + * Revision v1.24 + * + * Credits + * + * Maintained, conceived, written, and fiddled with by: + * + * Andrew E. Mileski + * + * Other source code contributors: + * + * Kevin Rockel + * Kevin Michael Woley + * Mark Anthony Lisher + * Nicolas Pitre + * Anonymous + * + * License + * + * Licensed under the terms of the MIT license: + * + * Copyright (c) 2000,2002,2006,2012,2016 Andrew E. Mileski + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to + * deal in the Software without restriction, including without limitation the + * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or + * sell copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The copyright notice, and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING + * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER + * DEALINGS IN THE SOFTWARE. + */ + +/* See header for full details */ +#include "math-sll.h" + +/* + * Local prototypes + */ + +static sll _sllcos(sll x); +static sll _sllsin(sll x); + +static sll _sllexp(sll x); + +/* + * Unpack IEEE 754 floating point double format into fixed point sll format + * + * Description + * + * IEEE 754 specifies the binary64 type ("double" in C) as having: + * + * 1 bit sign + * 11 bit exponent + * 53 bit significand + * + * The first bit of the significand is an implied 1 which is not stored. + * The decimal would be to the right of that implied 1, or to the left of + * the stored significand. + * + * The exponent is unsigned, and biased with an offset of 1023. + * + * The IEEE 754 standard does not specify endianess, but the endian used is + * traditionally the same endian that the processor uses. + */ + +sll dbl2sll(double dbl) +{ + union { + double d; + unsigned u[2]; + ull ull; + sll sll; + } in, retval; + register unsigned exp; + + /* Move into memory as args might be passed in regs */ + in.d = dbl; + +#if defined(BROKEN_IEEE754_DOUBLE) + + exp = in.u[0]; + in.u[0] = in.u[1]; + in.u[1] = exp; + +#endif /* defined(BROKEN_IEEE754_DOUBLE) */ + + /* Leading 1 is assumed by IEEE */ + retval.u[1] = 0x40000000; + + /* Unpack the mantissa into the unsigned long */ + retval.u[1] |= (in.u[1] << 10) & 0x3ffffc00; + retval.u[1] |= (in.u[0] >> 22) & 0x000003ff; + retval.u[0] = in.u[0] << 10; + + /* Extract the exponent and align the decimals */ + exp = (in.u[1] >> 20) & 0x7ff; + if (exp) + /* IEEE 754 decimal begins at right of bit position 30 */ + retval.ull >>= (1023 + 30) - exp; + else + return 0L; + + /* Negate if negative flag set */ + if (in.u[1] & 0x80000000) + retval.sll = _sllneg(retval.sll); + + return retval.sll; +} + +/* + * Pack fixed point sll format into IEEE 754 floating point double format + * + * Description + * + * IEEE 754 specifies the binary64 type ("double" in C) as having: + * + * 1 bit sign + * 11 bit exponent + * 53 bit significand + * + * The first bit of the significand is an implied 1 which is not stored. + * The decimal would be to the right of that implied 1, or to the left of + * the stored significand. + * + * The exponent is unsigned, and biased with an offset of 1023. + * + * The IEEE 754 standard does not specify endianess, but the endian used is + * traditionally the same endian that the processor uses. + */ + +double sll2dbl(sll s) +{ + union { + double d; + unsigned u[2]; + ull ull; + sll sll; + } in, retval; + register unsigned exp; + register unsigned flag; + + if (s == 0) + return 0.0; + + /* Move into memory as args might be passed in regs */ + in.sll = s; + + /* Handle the negative flag */ + if (in.sll < 1) { + flag = 0x80000000; + in.ull = _sllneg(in.sll); + } else + flag = 0x00000000; + + /* + * Normalize + * + * IEEE 754 decimal-point begins at right of bit position 30 + */ + for (exp = (1023 + 30); in.ull && (in.u[1] & 0x80000000) == 0; exp--) { + in.ull <<= 1; + } + in.ull <<= 1; + exp++; + in.ull >>= 12; + retval.ull = in.ull; + retval.u[1] |= flag | (exp << 20); + +#if defined(BROKEN_IEEE754_DOUBLE) + + exp = retval.u[0]; + retval.u[0] = retval.u[1]; + retval.u[1] = exp; + +#endif /* defined(BROKEN_IEEE754_DOUBLE) */ + + return retval.d; +} + +/* + * Multiply two sll values + * + * Description + * + * When multiplying two 64 bit sll numbers, the result is 128 bits, but there + * is only room for a 64 bit result with sll! + * + * The 128 bit result has 64 bits on either side of the decimal, so 32 bits + * of overflow to the left of the decimal, and 32 bits of underflow to the + * right of the decmial. + * + * 32.32 * 32.32 = 64.64 = overflow(32) + 32.32 + underflow(32) + * + * However, a "long long" multiply has 64 bits of overflow to the left of the + * decimal, resulting in the entire integer part being lost! + * + * 32.32 * 32.32 = 64.64 = overflow(64) + .64 + * + * Hence a custom multiply routine is required, to preserve the parts + * of the result that sll needs. + * + * Consider two sll numbers, x and y: + * + * Let x = x_hi * 2^0 + x_lo * 2^(-32) + * Let y = y_hi * 2^0 + y_lo * 2^(-32) + * + * Where: + * + * *_hi is the signed 32 bit integer part to the left of the decimal + * *_lo is the unsigned 32 bit fractional part to the right of the decimal + * + * x * y = (x_hi * 2^0 + x_lo * 2^(-32)) + * * (y_hi * 2^0 + y_lo * 2^(-32)) + * + * Expanding the terms, we get: + * + * = x_hi * y_hi * 2^0 + x_hi * y_lo * 2^(-32) + * + x_lo * y_hi * 2^(-32) + x_lo * y_lo * 2^(-64) + * + * Grouping by powers of 2, we get: + * + * (x_hi * y_hi) * 2^0 + * We only need the low 32 bits of this term, as the rest is overflow + * + * (x_hi * y_lo + x_lo * y_hi) * 2^-32 + * We need all bits of this term + * + * x_lo * y_lo * 2^-64 + * We only need the high 32 bits of this term, as the rest is underflow + */ + +sll sllmul(sll x, sll y) +{ + register unsigned int x_lo; + register signed int x_hi; + + register unsigned int y_lo; + register signed int y_hi; + + x_hi = (signed int) ((ull) x >> 32); // Discard lower 32 bits + x_lo = (unsigned int) x; // Discard upper 32 bits + + y_hi = (signed int) ((ull) y >> 32); // Discard lower 32 bits + y_lo = (unsigned int) y; // Discard upper 32 bits + + return (sll) ( + ((ull) (x_hi * y_hi) << 32) + + ((ull) x_hi * y_lo + x_lo * (ull) y_hi) + + (((ull) x_lo * y_lo) >> 32) + ); +} + +/* + * Calculate cos x where -pi/4 <= x <= pi/4 + * + * Description + * + * cos x = 1 - x^2 / 2! + x^4 / 4! - ... + x^(2N) / (2N)! + * Note that (pi/4)^12 / 12! < 2^-32 which is the smallest possible number. + * + * cos x = t0 + t1 + t2 + t3 + t4 + t5 + t6 + * + * Consider only the factorials: + * f0 = 0! = 1 + * f1 = 2! = 2 * 1 * f0 = 2 * f0 + * f2 = 4! = 4 * 3 * f1 = 12 * f1 + * f3 = 6! = 6 * 5 * f2 = 30 * f2 + * f4 = 8! = 8 * 7 * f3 = 56 * f3 + * f5 = 10! = 10 * 9 * f4 = 90 * f4 + * f6 = 12! = 12 * 11 * f5 = 132 * f5 + * + * Now consider each term of the series: + * t0 = 1 + * t1 = -t0 * x^2 / f1 = -t0 * x^2 * CONST_1_2 + * t2 = -t1 * x^2 / f2 = -t1 * x^2 * CONST_1_12 + * t3 = -t2 * x^2 / f3 = -t2 * x^2 * CONST_1_30 + * t4 = -t3 * x^2 / f4 = -t3 * x^2 * CONST_1_56 + * t5 = -t4 * x^2 / f5 = -t4 * x^2 * CONST_1_90 + * t6 = -t5 * x^2 / f6 = -t5 * x^2 * CONST_1_132 + */ + +sll _sllcos(sll x) +{ + sll retval; + sll x2; + + x2 = sllmul(x, x); + + retval = _sllsub(CONST_1, sllmul(x2, CONST_1_132)); + retval = _sllsub(CONST_1, sllmul(sllmul(x2, retval), CONST_1_90)); + retval = _sllsub(CONST_1, sllmul(sllmul(x2, retval), CONST_1_56)); + retval = _sllsub(CONST_1, sllmul(sllmul(x2, retval), CONST_1_30)); + retval = _sllsub(CONST_1, sllmul(sllmul(x2, retval), CONST_1_12)); + retval = _sllsub(CONST_1, slldiv2(sllmul(x2, retval))); + + return retval; +} + +/* + * Calculate sin x where -pi/4 <= x <= pi/4 + * + * Description + * + * sin x = x - x^3 / 3! + x^5 / 5! - ... + x^(2N+1) / (2N+1)! + * Note that (pi/4)^13 / 13! < 2^-32 which is the smallest possible number. + * + * sin x = t0 + t1 + t2 + t3 + t4 + t5 + t6 + * + * Consider only the factorials: + * f0 = 0! = 1 + * f1 = 3! = 3 * 2 * f0 = 6 * f0 + * f2 = 5! = 5 * 4 * f1 = 20 * f1 + * f3 = 7! = 7 * 6 * f2 = 42 * f2 + * f4 = 9! = 9 * 8 * f3 = 72 * f3 + * f5 = 11! = 11 * 10 * f4 = 110 * f4 + * f6 = 13! = 13 * 12 * f5 = 156 * f5 + * + * Now consider each term of the series: + * t0 = 1 + * t1 = -t0 * x^2 / 6 = -t0 * x^2 * CONST_1_6 + * t2 = -t1 * x^2 / 20 = -t1 * x^2 * CONST_1_20 + * t3 = -t2 * x^2 / 42 = -t2 * x^2 * CONST_1_42 + * t4 = -t3 * x^2 / 72 = -t3 * x^2 * CONST_1_72 + * t5 = -t4 * x^2 / 110 = -t4 * x^2 * CONST_1_110 + * t6 = -t5 * x^2 / 156 = -t5 * x^2 * CONST_1_156 + */ + +sll _sllsin(sll x) +{ + sll retval; + sll x2; + + x2 = sllmul(x, x); + + retval = _sllsub(x, sllmul(x2, CONST_1_156)); + retval = _sllsub(x, sllmul(sllmul(x2, retval), CONST_1_110)); + retval = _sllsub(x, sllmul(sllmul(x2, retval), CONST_1_72)); + retval = _sllsub(x, sllmul(sllmul(x2, retval), CONST_1_42)); + retval = _sllsub(x, sllmul(sllmul(x2, retval), CONST_1_20)); + retval = _sllsub(x, sllmul(sllmul(x2, retval), CONST_1_6)); + + return retval; +} + +/* + * Calculate cos x for any value of x, by quadrant + */ + +sll sllcos(sll x) +{ + int i; + sll retval; + + /* Calculate cos (x - i * pi/2), where -pi/4 <= x - i * pi/2 <= pi/4 */ + i = _sll2int(_slladd(sllmul(x, CONST_2_PI), CONST_1_2)); + x = _sllsub(x, sllmul(_int2sll(i), CONST_PI_2)); + + /* Locate the quadrant */ + switch (i & 3) { + default: + case 0: + retval = _sllcos(x); + break; + case 1: + retval = sllneg(_sllsin(x)); + break; + case 2: + retval = sllneg(_sllcos(x)); + break; + case 3: + retval = _sllsin(x); + break; + } + + return retval; +} + +/* + * Calculate sin x for any value of x, by quadrant + */ + +sll sllsin(sll x) +{ + int i; + sll retval; + + /* Calculate sin (x - n * pi/2), where -pi/4 <= x - i * pi/2 <= pi/4 */ + i = _sll2int(_slladd(sllmul(x, CONST_2_PI), CONST_1_2)); + x = _sllsub(x, sllmul(_int2sll(i), CONST_PI_2)); + + /* Locate the quadrant */ + switch (i & 3) { + default: + case 0: + retval = _sllsin(x); + break; + case 1: + retval = _sllcos(x); + break; + case 2: + retval = sllneg(_sllsin(x)); + break; + case 3: + retval = sllneg(_sllcos(x)); + break; + } + + return retval; +} + +/* + * Calculate tan x for any value of x, by quadrant + */ + +sll slltan(sll x) +{ + int i; + sll retval; + + i = _sll2int(_slladd(sllmul(x, CONST_2_PI), CONST_1_2)); + x = _sllsub(x, sllmul(_int2sll(i), CONST_PI_2)); + + /* Locate the quadrant */ + switch (i & 3) { + default: + case 0: + case 2: + retval = slldiv(_sllsin(x), _sllcos(x)); + break; + case 1: + case 3: + retval = _sllneg(slldiv(_sllcos(x), _sllsin(x))); + break; + } + + return retval; +} + +/* + * + * Calculate asin x, where |x| <= 1 + * + * Description + * + * asin x = SUM[n=0,) C(2 * n, n) * x^(2 * n + 1) / (4^n * (2 * n + 1)), |x| <= 1 + * + * where C(n, r) = nCr = n! / (r! * (n - r)!) + * + * Using a two term approximation: + * [1] a = x + x^3 / 6 + * + * Results in: + * asin x = a + D + * where D is the difference from the exact result + * + * Letting D = asin d results in: + * [2] asin x = a + asin d + * + * Re-arranging: + * asin x - a = asin d + * + * Applying sin to both sides: + * sin (asin x - a) = sin asin d + * sin (asin x - a) = d + * d = sin (asin x - a) + * + * Applying the standard identity: + * sin (u - v) = sin u * cos v - cos u * sin v + * + * Results in: + * d = sin asin x * cos a - cos asin x * sin a + * d = x * cos a - cos asin x * sin a + * + * Applying the standard identity: + * cos asin u = (1 - u^2)^(1 / 2) + * + * Results in: + * [3] d = x * cos a - (1 - x^2)^(1 / 2) * sin a + * + * Putting the pieces together: + * [1] a = x + x^3 / 6 + * [3] d = x * cos a - (1 - x^2)^(1 / 2) * sin a + * [2] asin x = a + asin d + * + * The worst case is x = 1.0 which converges after 2 iterations. + */ + +sll sllasin(sll x) +{ + int left_side; + sll a; + sll retval; + + /* asin -x = -asin x */ + if ((left_side = x < 0)) + x = _sllneg(x); + + /* Out-of-range */ + if (x > CONST_1) + return 0; + + /* Initial approximate */ + a = sllmul(x, _slladd(CONST_1, sllmul(x, sllmul(x, CONST_1_6)))); + retval = a; + + /* First iteration */ + x = _sllsub(sllmul(x, sllcos(a)), sllmul(sllsqrt(_sllsub(CONST_1, sllmul(x, x))), sllsin(a))); + a = sllmul(x, _slladd(CONST_1, sllmul(x, sllmul(x, CONST_1_6)))); + retval = _slladd(retval, a); + + /* Second iteration */ + x = _sllsub(sllmul(x, sllcos(a)), sllmul(sllsqrt(_sllsub(CONST_1, sllmul(x, x))), sllsin(a))); + a = sllmul(x, _slladd(CONST_1, sllmul(x, sllmul(x, CONST_1_6)))); + retval = _slladd(retval, a); + + /* Negate result if necessary */ + return (left_side ? _sllneg(retval): retval); +} + +/* + * Calculate atan x + * + * Description + * + * atan x = SUM[n=0,) (-1)^n * x^(2 * n + 1) / (2 * n + 1), |x| <= 1 + * + * Using a two term approximation: + * [1] a = x - x^3 / 3 + * + * Results in: + * atan x = a + D + * where D is the difference from the exact result + * + * Letting D = atan d results in: + * [2] atan x = a + atan d + * + * Re-arranging: + * atan x - a = atan d + * + * Applying tan to both sides: + * tan (atan x - a) = tan atan d + * tan (atan x - a) = d + * d = tan (atan x - a) + * + * Applying the standard identity: + * tan (u - v) = (tan u - tan v) / (1 + tan u * tan v) + * + * Results in: + * d = tan (atan x - a) = (tan atan x - tan a) / (1 + tan atan x * tan a) + * d = tan (atan x - a) = (x - tan a) / (1 + x * tan a) + * + * Let: + * [3] t = tan a + * + * Results in: + * [4] d = (x - t) / (1 + x * t) + * + * So putting the pieces together: + * [1] a = x - x^3 / 3 + * [3] t = tan a + * [4] d = (x - t) / (1 + x * t) + * [2] atan x = a + atan d + * atan x = a + atan ((x - t) / (1 + x * t)) + * + * The worst case is x = 1.0 which converges after 2 iterations. + */ + +sll sllatan(sll x) +{ + int side; + sll a; + sll t; + sll retval; + + + if (x < CONST_1) { + + /* Left: if (x < -1) then atan x = pi / 2 + atan 1 / x */ + side = -1; + x = sllinv(x); + + } else if (x > CONST_1) { + + /* Right: if (x > 1) then atan x = pi / 2 - atan 1 / x */ + side = 1; + x = sllinv(x); + + } else { + /* Middle: -1 <= x <= 1 */ + side = 0; + } + + /* Initial approximate */ + a = sllmul(x, _sllsub(CONST_1, sllmul(x, sllmul(x, CONST_1_3)))); + retval = a; + + /* First iteration */ + t = _slldiv(_sllsin(a), _sllcos(a)); + x = _slldiv(_sllsub(x, t), _slladd(CONST_1, sllmul(x, t))); + a = sllmul(x, _sllsub(CONST_1, sllmul(x, sllmul(x, CONST_1_3)))); + retval = _slladd(retval, a); + + /* Second iteration */ + t = _slldiv(_sllsin(a), _sllcos(a)); + x = _slldiv(_sllsub(x, t), _slladd(CONST_1, sllmul(x, t))); + a = sllmul(x, _sllsub(CONST_1, sllmul(x, sllmul(x, CONST_1_3)))); + retval = _slladd(retval, a); + + if (side == -1) { + + /* Left: if (x < -1) then atan x = pi / 2 + atan 1 / x */ + retval = _slladd(CONST_PI_2, retval); + + } else if (side == 1) { + + /* Right: if (x > 1) then atan x = pi / 2 - atan 1 / x */ + retval = _sllsub(CONST_PI_2, retval); + } + + return retval; +} + +/* + * Calculate e^x where -0.5 <= x <= 0.5 + * + * Description: + * e^x = x^0 / 0! + x^1 / 1! + ... + x^N / N! + * Note that 0.5^11 / 11! < 2^-32 which is the smallest possible number. + */ + +sll _sllexp(sll x) +{ + sll retval; + + retval = CONST_1; + + retval = _slladd(CONST_1, sllmul(retval, sllmul(x, CONST_1_11))); + retval = _slladd(CONST_1, sllmul(retval, sllmul(x, CONST_1_10))); + retval = _slladd(CONST_1, sllmul(retval, sllmul(x, CONST_1_9))); + retval = _slladd(CONST_1, sllmul(retval, slldiv2n(x, 3))); + retval = _slladd(CONST_1, sllmul(retval, sllmul(x, CONST_1_7))); + retval = _slladd(CONST_1, sllmul(retval, sllmul(x, CONST_1_6))); + retval = _slladd(CONST_1, sllmul(retval, sllmul(x, CONST_1_5))); + retval = _slladd(CONST_1, sllmul(retval, slldiv4(x))); + retval = _slladd(CONST_1, sllmul(retval, sllmul(x, CONST_1_3))); + retval = _slladd(CONST_1, sllmul(retval, slldiv2(x))); + retval = _slladd(CONST_1, sllmul(retval, x)); + + return retval; +} + +/* + * Calculate e^x for any value of x + */ + +sll sllexp(sll x) +{ + int i; + sll e; + sll retval; + + e = CONST_E; + + /* -0.5 <= x <= 0.5 */ + i = _sll2int(_slladd(x, CONST_1_2)); + retval = _sllexp(_sllsub(x, _int2sll(i))); + + /* i >= 0 */ + if (i < 0) { + i = -i; + e = CONST_1_E; + } + + /* Scale the result */ + for (; i; i >>= 1) { + if (i & 1) + retval = sllmul(retval, e); + e = sllmul(e, e); + } + + return retval; +} + +/* + * Calculate natural logarithm using Netwton-Raphson method + */ + +sll slllog(sll x) +{ + sll x1; + sll ln; + + ln = 0; + + /* Scale: e^(-1/2) <= x <= e^(1/2) */ + while (x < CONST_1_SQRTE) { + ln = _sllsub(ln, CONST_1); + x = sllmul(x, CONST_E); + } + while (x > CONST_SQRTE) { + ln = _slladd(ln, CONST_1); + x = sllmul(x, CONST_1_E); + } + + /* First iteration */ + x1 = sllmul(_sllsub(x, CONST_1), slldiv2(_sllsub(x, CONST_3))); + ln = _sllsub(ln, x1); + x = sllmul(x, _sllexp(x1)); + + /* Second iteration */ + x1 = sllmul(_sllsub(x, CONST_1), slldiv2(_sllsub(x, CONST_3))); + ln = _sllsub(ln, x1); + x = sllmul(x, _sllexp(x1)); + + /* Third iteration */ + x1 = sllmul(_sllsub(x, CONST_1), slldiv2(_sllsub(x, CONST_3))); + ln = _sllsub(ln, x1); + + return ln; +} + +/* + * Calculate the inverse for non-zero values + */ + +sll sllinv(sll x) +{ + int sgn; + sll u; + ull s; + + /* Use positive numbers, or the approximation won't work */ + if (x < CONST_0) { + x = _sllneg(x); + sgn = 1; + } else { + sgn = 0; + } + + /* Starting-point (gets shifted right to become positive) */ + s = -1; + + /* An approximation - must be larger than the actual value */ + for (u = x; u; u = ((ull) u) >> 1) + s >>= 1; + + /* Newton's Method */ + u = sllmul(s, _sllsub(CONST_2, sllmul(x, s))); + u = sllmul(u, _sllsub(CONST_2, sllmul(x, u))); + u = sllmul(u, _sllsub(CONST_2, sllmul(x, u))); + u = sllmul(u, _sllsub(CONST_2, sllmul(x, u))); + u = sllmul(u, _sllsub(CONST_2, sllmul(x, u))); + u = sllmul(u, _sllsub(CONST_2, sllmul(x, u))); + + return ((sgn) ? _sllneg(u): u); +} + +/* + * Calculate x^y + * + * Description + * + * The standard identity: + * ln x^y = y * log x + * + * Raising e to the power of either sides: + * e^(ln x^y) = e^(y * log x) + * + * Which simplifies to: + * x^y = e^(y * ln x) + */ + +sll sllpow(sll x, sll y) +{ + if (y == CONST_0) + return CONST_1; + if (y == CONST_1) + return x; + if (y == CONST_2) + return sllmul(x, x); + + return sllexp(sllmul(y, slllog(x))); +} + +/* + * Calculate the square-root + * + * Description + * + * Consider a parabola centered on the y-axis: + * y = a * x^2 + b + * + * Has zeros (y = 0) located at: + * a * x^2 + b = 0 + * a * x^2 = -b + * x^2 = -b / a + * x = +- (-b / a)^(1 / 2) + * + * Letting a = 1 and b = -X results in: + * y = x^2 - X + * x = +- X^(1 / 2) + * + * Which is a convenient result, since we want to find the square root of X, + * and we can * use Newton's Method to find the zeros of any f(x): + * xn = x - f(x) / f'(x) + * + * Applying Newton's Method to our parabola: + * f(x) = x^2 - X + * xn = x - (x^2 - X) / (2 * x) + * xn = x - (x - X / x) / 2 + * + * To make this converge quickly, we scale X so that: + * X = 4^N * z + * + * Taking the roots of both sides + * X^(1 / 2) = (4^n * z)^(1 / 2) + * X^(1 / 2) = 2^n * z^(1 / 2) + * + * Letting N = 2^n results in: + * x^(1 / 2) = N * z^(1 / 2) + * + * We want this to converge to the positive root, so we must start at a point + * 0 < start <= x^(1 / 2) + * or + * x^(1/2) <= start <= infinity + * + * Since: + * (1/2)^(1/2) = 0.707 + * 2^(1/2) = 1.414 + * + * A good choice is 1 which lies in the middle, and takes 4 iterations to + * converge from either extreme. + */ + +sll sllsqrt(sll x) +{ + sll n; + sll xn; + + /* Quick solutions for the simple cases */ + if (x <= CONST_0 || x == CONST_1) + return x; + + /* Start with a scaling factor of 1 */ + n = CONST_1; + + /* Scale x so that 0.5 <= x < 2 */ + while (x >= CONST_2) { + x = slldiv4(x); + n = sllmul2(n); + } + while (x < CONST_1_2) { + x = sllmul4(x); + n = slldiv2(n); + } + + /* Simple solution if x = 4^n */ + if (x == CONST_1) + return n; + + /* The starting point */ + xn = CONST_1; + + /* Four iterations will be enough */ + xn = _sllsub(xn, slldiv2(_sllsub(xn, slldiv(x, xn)))); + xn = _sllsub(xn, slldiv2(_sllsub(xn, slldiv(x, xn)))); + xn = _sllsub(xn, slldiv2(_sllsub(xn, slldiv(x, xn)))); + xn = _sllsub(xn, slldiv2(_sllsub(xn, slldiv(x, xn)))); + + /* Scale the result */ + return sllmul(n, xn); +} + +sll slld2dsqrt(sll num) +{ + if (num <= CONST_0 || num == CONST_1) + { + return num; + } + sll result = 0; + sll bit; + + // Many numbers will be less than 15, so + // this gives a good balance between time spent + // in if vs. time spent in the while loop + // when searching for the starting value. + if (num & 0xFFFFFF0000000000LL) + bit = 0x4000000000000000LL; //(sll)1 << 62; + else + bit = 0x0000004000000000LL; //(sll)1 << 38; + + while (bit > num) bit >>= 2; + + while (bit) + { + if (num >= result + bit) + { + num -= result + bit; + result = (result >> 1) + bit; + } + else + { + result = (result >> 1); + } + bit >>= 2; + } + + // Then process it again to get the lowest 8 bits. + // 灏炬暟瀹氱偣鏁板ぇ浜庣瓑浜1 + if (num > CONST_P9999) + { + // The remainder 'num' is too large to be shifted left + // by 32, so we have to add 0.5 to result manually and + // adjust 'num' accordingly. a鏄師鏈殑鏁 + // why 0.5 is enough? because: result^2 <= a < (result+1)^2 + // num = a - (result + 0.5)^2 + // = num + result^2 - (result + 0.5)^2 + // = num - result - 0.25 + num -= result; + // 娉ㄦ剰锛屼弗鏍兼潵璇达紝杩欓噷瑕佸乏绉16浣嶆墠缂╁皬鍒扮粨鏋滐紝浣嗘槸涓轰簡鍚庣画灏忔暟涓嶇敤鍐嶆斁澶э紝杩欓噷鎻愬墠鍋氫簡鏀惧ぇ + num = (num << 32) - CONST_1_4; + result = (result << 32) + CONST_1_2; + } + else + { + num <<= 32; + result <<= 32; + } + + bit = 0x0000000040000000LL; //(sll)1 << 30; + + while (bit) + { + if (num >= result + bit) + { + num -= result + bit; + result = (result >> 1) + bit; + } + else + { + result = (result >> 1); + } + bit >>= 2; + } + + return result; +} \ No newline at end of file diff --git a/Client/ThirdParty/math-sll/math-sll.h b/Client/ThirdParty/math-sll/math-sll.h new file mode 100644 index 0000000..b10dbe7 --- /dev/null +++ b/Client/ThirdParty/math-sll/math-sll.h @@ -0,0 +1,770 @@ +#if !defined(MATH_SLL_H) +# define MATH_SLL_H + +/* + * Revision v1.24 + * + * A fixed point (32.32 bit) math library. + * + * Description + * + * Floating point packs the most accuracy in the available bits, but it + * often provides more accuracy than is required. It is time consuming to + * carry the extra precision around, particularly on platforms that don't + * have a dedicated floating point processor. + * + * This library is a compromise. All math is done using the 64 bit signed + * "long long" format (sll), and is not intended to be portable, just as + * simple and as fast as possible. + * + * As some processors lack division instructions but have multiplication + * instructions, multiplication is favored over division. This can be a + * penalty when used on a processor with a division instruction, so it is + * recommended to modify the division functions and macros in that case. + * + * On procesors without multiplication instructions, other algorithms, for + * example CORDIC, are probably faster. + * + * Since "long long" is a elementary type, it can be passed around without + * resorting to the use of pointers. Since the format used is fixed point, + * there is never a need to do time consuming checks and adjustments to + * maintain normalized numbers, as is the case in floating point. + * + * Simply put, this library is limited to handling numbers with a whole + * part of up to 2^31 - 1 = 2.147483647e9 in magnitude, and fractional + * parts down to 2^-32 = 2.3283064365e-10 in magnitude. This yields a + * decent range and accuracy for many applications. + * + * IMPORTANT + * + * No checking for arguments out of range (error). + * No checking for divide by zero (error). + * No checking for overflow (error). + * No checking for underflow (warning). + * Chops, doesn't round. + * + * Functions + * + * sll dbl2sll(double d) double to sll + * double sll2dbl(sll s) sll to double + * + * sll int2sll(int i) integer to sll + * int sll2int(sll s) sll to integer + * + * sll sllint(sll s) Set fractional-part to 0 + * sll sllfrac(sll s) Set integer-part to 0 + * + * sll slladd(sll x, sll y) x + y + * sll sllneg(sll x) -x + * sll sllsub(sll x, sll y) x - y + * + * sll sllmul(sll x, sll y) x * y + * sll sllmul2(sll x) x * 2 + * sll sllmul2n(sll x, int n) x * 2^n, 0 <= n <= 31 + * sll sllmul4(sll x) x * 4 + * + * sll slldiv(sll x, sll y) x / y + * sll slldiv2(sll x) x / 2 + * sll slldiv2n(sll x, int n) x / 2^n, 0 <= n <= 31 + * sll slldiv4(sll x) x / 4 + * + * sll sllcos(sll x) cos x + * sll sllsin(sll x) sin x + * sll slltan(sll x) tan x + * + * sll sllsec(sll x) sec x = 1 / cos x + * sll sllcsc(sll x) csc x = 1 / sin x + * sll sllcot(sll x) cot x = 1 / tan x = cos x / sin x + * + * sll sllacos(sll x) acos x + * sll sllasin(sll x) asin x + * sll sllatan(sll x) atan x + * + * sll sllcosh(sll x) cosh x + * sll sllsinh(sll x) sinh x + * sll slltanh(sll x) tanh x + * + * sll sllsech(sll x) sech x + * sll sllcsch(sll x) cosh x + * sll sllcoth(sll x) coth x + * + * sll sllexp(sll x) e^x + * sll slllog(sll x) ln x + * + * sll sllinv(sll v) 1 / x + * sll sllpow(sll x, sll y) x^y + * sll sllsqrt(sll x) x^(1 / 2) + * + * sll sllfloor(sll x) floor x + * sll sllceil(sll x) ceiling x + * + * Macros + * + * Use of the following macros is optional, but may be beneficial with + * some compilers. Using the non-macro versions is strongly recommended. + * + * WARNING: macros do not type-check their arguments! + * + * _int2sll(X) See function int2sll() + * _sll2int(X) See function sll2int() + * + * _sllint(X) See function sllint() + * _sllfrac(X) See function sllfrac() + * + * _slladd(X,Y) See function slladd() + * _sllneg(X) See function sllneg() + * _sllsub(X,Y) See function sllsub() + * + * _sllmul2(X) See function sllmul2() + * _sllmul2n(X) See function sllmul2n() + * _sllmul4(X) See function sllmul4() + * + * _slldiv(X,Y) See function slldiv() + * _slldiv2(X,Y) See function slldiv2() + * _slldiv2n(X,Y) See function slldiv2n() + * _slldiv4(X,Y) See function slldiv4() + * + * Credits + * + * Maintained, conceived, written, and fiddled with by: + * + * Andrew E. Mileski + * + * Other source code contributors: + * + * Kevin Rockel + * Kevin Michael Woley + * Mark Anthony Lisher + * Nicolas Pitre + * Anonymous + * + * License + * + * Licensed under the terms of the MIT license: + * + * Copyright (c) 2000,2002,2006,2012,2016 Andrew E. Mileski + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to + * deal in the Software without restriction, including without limitation the + * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or + * sell copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The copyright notice, and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING + * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER + * DEALINGS IN THE SOFTWARE. + */ + +/* DEC SA-110 "StrongARM" (armv4l) architecture has a big-endian double */ +#if defined(__arm__) +# if (!defined(__BYTE_ORDER__) || (__BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__)) +# define BROKEN_IEEE754_DOUBLE +# endif +#endif + +#ifdef _MSC_VER +# define __inline__ __inline +# ifndef _MSC_STDINT_H_ +typedef signed __int64 int64_t; +typedef unsigned __int64 uint64_t; +#endif +#define __extension__ +#else +#include +#if defined(__GNUC__) + +#else + #define __inline__ inline +#endif +#endif +/* + * Data types + */ + +__extension__ typedef int64_t sll; +__extension__ typedef uint64_t ull; + +/* + * Function prototypes + */ + +sll dbl2sll(double d); +double sll2dbl(sll s); + +static __inline__ sll int2sll(int i); +static __inline__ int sll2int(sll s); + +static __inline__ sll sllint(sll s); +static __inline__ sll sllfrac(sll s); + +static __inline__ sll slladd(sll x, sll y); +static __inline__ sll sllneg(sll s); +static __inline__ sll sllsub(sll x, sll y); +sll sllmul(sll x, sll y); +static __inline__ sll sllmul2(sll x); +static __inline__ sll sllmul4(sll x); +static __inline__ sll sllmul2n(sll x, int n); + +static __inline__ sll slldiv(sll x, sll y); +static __inline__ sll slldiv2(sll x); +static __inline__ sll slldiv4(sll x); +static __inline__ sll slldiv2n(sll x, int n); + +sll sllcos(sll x); +sll sllsin(sll x); +sll slltan(sll x); + +static __inline__ sll sllacos(sll x); +sll sllasin(sll x); +sll sllatan(sll x); + +static __inline__ sll sllsec(sll x); +static __inline__ sll sllcsc(sll x); +static __inline__ sll sllcot(sll x); + +static __inline__ sll sllcosh(sll x); +static __inline__ sll sllsinh(sll x); +static __inline__ sll slltanh(sll x); + +static __inline__ sll sllsech(sll x); +static __inline__ sll sllcsch(sll x); +static __inline__ sll sllcoth(sll x); + +sll sllexp(sll x); +sll slllog(sll x); + +sll sllpow(sll x, sll y); +sll sllinv(sll v); +sll sllsqrt(sll x); +sll slld2dsqrt(sll x); + +static __inline__ sll sllfloor(sll x); +static __inline__ sll sllceil(sll x); + +/* + * Macros + * + * WARNING - Macros don't type-check! + */ + +#define _int2sll(X) (((sll) (X)) << 32) +#define _sll2int(X) ((int) ((X) >> 32)) + +#define _sllint(X) ((X) & 0xffffffff00000000LL) +#define _sllfrac(X) ((X) & 0x00000000ffffffffLL) + +#define _slladd(X,Y) ((X) + (Y)) +#define _sllneg(X) (-(X)) +#define _sllsub(X,Y) ((X) - (Y)) + +#define _sllmul2(X) ((X) << 1) +#define _sllmul4(X) ((X) << 2) +#define _sllmul2n(X,N) ((X) << (N)) + +#define _slldiv(X,Y) sllmul((X), sllinv(Y)) +#define _slldiv2(X) ((X) >> 1) +#define _slldiv4(X) ((X) >> 2) +#define _slldiv2n(X,N) ((X) >> (N)) + +/* + * Constants (converted from double) +*/ + +#define CONST_0 0x0000000000000000LL // 0.0 +#define CONST_1 0x0000000100000000LL // 1.0 +#define CONST_neg1 0xffffffff00000000LL // -1.0 +#define CONST_2 0x0000000200000000LL // 2.0 +#define CONST_3 0x0000000300000000LL // 3.0 +#define CONST_4 0x0000000400000000LL // 4.0 +#define CONST_10 0x0000000a00000000LL // 10.0 +#define CONST_10 0x0000000a00000000LL // 10.0 +#define CONST_1_2 0x0000000080000000LL // 1.0 / 2.0 +#define CONST_1_3 0x0000000055555555LL // 1.0 / 3.0 +#define CONST_1_4 0x0000000040000000LL // 1.0 / 4.0 +#define CONST_1_5 0x0000000033333333LL // 1.0 / 5.0 +#define CONST_1_6 0x000000002aaaaaaaLL // 1.0 / 6.0 +#define CONST_1_7 0x0000000024924924LL // 1.0 / 7.0 +#define CONST_1_8 0x0000000020000000LL // 1.0 / 8.0 +#define CONST_1_9 0x000000001c71c71cLL // 1.0 / 9.0 +#define CONST_1_10 0x0000000019999999LL // 1.0 / 10.0 +#define CONST_1_11 0x000000001745d174LL // 1.0 / 11.0 +#define CONST_1_12 0x0000000015555555LL // 1.0 / 12.0 +#define CONST_1_20 0x000000000cccccccLL // 1.0 / 20.0 +#define CONST_1_30 0x0000000008888888LL // 1.0 / 30.0 +#define CONST_1_42 0x0000000006186186LL // 1.0 / 42.0 +#define CONST_1_56 0x0000000004924924LL // 1.0 / 56.0 +#define CONST_1_72 0x00000000038e38e3LL // 1.0 / 72.0 +#define CONST_1_90 0x0000000002d82d82LL // 1.0 / 90.0 +#define CONST_1_110 0x000000000253c825LL // 1.0 / 110.0 +#define CONST_1_132 0x0000000001f07c1fLL // 1.0 / 132.0 +#define CONST_1_156 0x0000000001a41a41LL // 1.0 / 156.0 +#define CONST_P9999 0x00000000FFFFFFFFLL // 0.999999999999 + +#define CONST_E 0x00000002b7e15162LL // E +#define CONST_1_E 0x000000005e2d58d8LL // 1 / E +#define CONST_SQRTE 0x00000001a61298e1LL // sqrt(E) +#define CONST_1_SQRTE 0x000000009b4597e3LL // 1 / sqrt(E) +#define CONST_LOG2_E 0x0000000171547652LL // ln(E) +#define CONST_LOG10_E 0x000000006f2dec54LL // log(E) +#define CONST_LN2 0x00000000b17217f7LL // ln(2) +#define CONST_LN10 0x000000024d763776LL // ln(10) + +#define CONST_PI 0x00000003243f6a88LL // PI +#define CONST_2PI 0x00000006487ED510LL // PI +#define CONST_PI_2 0x00000001921fb544LL // PI / 2 +#define CONST_PI_4 0x00000000c90fdaa2LL // PI / 4 +#define CONST_1_PI 0x00000000517cc1b7LL // 1 / PI +#define CONST_2_PI 0x00000000a2f9836eLL // 2 / PI +#define CONST_180_PI 0x000000394BB834C7LL // 180 / PI 246083499207.51537232162612011973 +#define CONST_PI_180 0x000000000477d1a8LL // PI / 180 74961320.580677883103327681382757 +#define CONST_2_SQRTPI 0x0000000120dd7504LL // 2 / sqrt(PI) +#define CONST_SQRT2 0x000000016a09e667LL // sqrt(2) +#define CONST_1_SQRT2 0x00000000b504f333LL // 1 / sqrt(2) + +#define CONST_FACT_0 0x0000000100000000LL // 0! +#define CONST_FACT_1 0x0000000100000000LL // 1! +#define CONST_FACT_2 0x0000000200000000LL // 2! +#define CONST_FACT_3 0x0000000600000000LL // 3! +#define CONST_FACT_4 0x0000001800000000LL // 4! +#define CONST_FACT_5 0x0000007800000000LL // 5! +#define CONST_FACT_6 0x000002d000000000LL // 6! +#define CONST_FACT_7 0x000013b000000000LL // 7! +#define CONST_FACT_8 0x00009d8000000000LL // 8! +#define CONST_FACT_9 0x0005898000000000LL // 9! +#define CONST_FACT_10 0x00375f0000000000LL // 10! +#define CONST_FACT_11 0x0261150000000000LL // 11! +#define CONST_FACT_12 0x1c8cfc0000000000LL // 12! + +#define CONST_MAX 0x7FFFFFFFFFFFFFFFLL // 鏈澶 +#define CONST_MIN 0x8000000000000000LL // 鏈灏 + +/* + * Convert integer to sll + */ + +static __inline__ sll int2sll(int i) +{ + return _int2sll(i); +} + +/* + * Convert sll to integer (truncates) + */ + +static __inline__ int sll2int(sll s) +{ + return _sll2int(s); +} + +/* + * Integer-part of sll (fractional-part set to 0) + */ + +static __inline__ sll sllint(sll s) +{ + return _sllint(s); +} + +/* + * Fractional-part of sll (integer-part set to 0) + */ + +static __inline__ sll sllfrac(sll s) +{ + return _sllfrac(s); +} + +/* + * Addition + */ + +static __inline__ sll slladd(sll x, sll y) +{ + return _slladd(x, y); +} + +/* + * Negate + */ + +static __inline__ sll sllneg(sll s) +{ + return _sllneg(s); +} + +/* + * Subtraction + */ + +static __inline__ sll sllsub(sll x, sll y) +{ + return _sllsub(x, y); +} + +/* + * Multiply two sll values + * + * Description + * + * Let a = A * 2^32 + a_h * 2^0 + a_l * 2^(-32) + * Let b = B * 2^32 + b_h * 2^0 + b_l * 2^(-32) + * + * Where: + * + * *_h is the integer part + * *_l the fractional part + * A and B are the sign (0 for positive, -1 for negative). + * + * a * b = (A * 2^32 + a_h * 2^0 + a_l * 2^-32) + * * (B * 2^32 + b_h * 2^0 + b_l * 2^-32) + * + * Expanding the terms, we get: + * + * = A * B * 2^64 + A * b_h * 2^32 + A * b_l * 2^0 + * + a_h * B * 2^32 + a_h * b_h * 2^0 + a_h * b_l * 2^-32 + * + a_l * B * 2^0 + a_l * b_h * 2^-32 + a_l * b_l * 2^-64 + * + * Grouping by powers of 2, we get: + * + * = A * B * 2^64 + * Meaningless overflow from sign extension - ignore + * + * + (A * b_h + a_h * B) * 2^32 + * Overflow which we can't handle - ignore + * + * + (A * b_l + a_h * b_h + a_l * B) * 2^0 + * We only need the low 32 bits of this term, as the rest is overflow + * + * + (a_h * b_l + a_l * b_h) * 2^-32 + * We need all 64 bits of this term + * + * + a_l * b_l * 2^-64 + * We only need the high 32 bits of this term, as the rest is underflow + * + * Note that: + * a > 0 && b > 0: A = 0, B = 0 and the third term is a_h * b_h + * a < 0 && b > 0: A = -1, B = 0 and the third term is a_h * b_h - b_l + * a > 0 && b < 0: A = 0, B = -1 and the third term is a_h * b_h - a_l + * a < 0 && b < 0: A = -1, B = -1 and the third term is a_h * b_h - a_l - b_l + */ + +/* + * Multiplication by 2 + */ + +static __inline__ sll sllmul2(sll x) +{ + return _sllmul2(x); +} + +/* + * Multiplication by 4 + */ + +static __inline__ sll sllmul4(sll x) +{ + return _sllmul4(x); +} + +/* + * Multiplication by power of 2 + */ + +static __inline__ sll sllmul2n(sll x, int n) +{ + return _sllmul2n(x, n); +} + +/* + * Division + */ + +static __inline__ sll slldiv(sll x, sll y) +{ + return _slldiv(x, y); +} + +/* + * Division by 2 + */ + +static __inline__ sll slldiv2(sll x) +{ + return _slldiv2(x); +} + +/* + * Division by 4 + */ + +static __inline__ sll slldiv4(sll x) +{ + return _slldiv4(x); +} + +/* + * Division by power of 2 + */ + +static __inline__ sll slldiv2n(sll x, int n) +{ + return _slldiv2n(x, n); +} + +/* + * + * Calculate acos x, where |x| <= 1 + * + * Description + * + * acos x = pi / 2 - asin x + * acos x = pi / 2 - SUM[n=0,) C(2 * n, n) * x^(2 * n + 1) / (4^n * (2 * n + 1)), |x| <= 1 + * + * where C(n, r) = nCr = n! / (r! * (n - r)!) + */ + +static __inline__ sll sllacos(sll x) +{ + return _sllsub(CONST_PI_2, sllasin(x)); +} + +/* + * Trigonometric secant + * + * Description + * + * sec x = 1 / cos x + * + * An alternate algorithm, like a power series, would be more accurate. + */ + +static __inline__ sll sllsec(sll x) +{ + return sllinv(sllcos(x)); +} + +/* + * Trigonometric cosecant + * + * Description + * + * csc x = 1 / sin x + * + * An alternate algorithm, like a power series, would be more accurate. + */ + +static __inline__ sll sllcsc(sll x) +{ + return sllinv(sllsin(x)); +} + +/* + * Trigonometric cotangent + * + * Description + * + * cot x = 1 / tan x + * + * cot x = cos x / sin x + * + * An alternate algorithm, like a power series, would be more accurate. + */ + +static __inline__ sll sllcot(sll x) +{ + return _slldiv(sllcos(x), sllsin(x)); +} + +/* + * Hyperbolic cosine + * + * Description + * + * cosh x = (e^x + e^(-x)) / 2 + * + * cosh x = 1 + x^2 / 2! + ... + x^(2 * N) / (2 * N)! + * + * An alternate algorithm, like a power series, would be more accurate. + */ + +static __inline__ sll sllcosh(sll x) +{ + return _slldiv2(_slladd(sllexp(x), sllexp(_sllneg(x)))); +} + +/* + * Hyperbolic sine + * + * Description + * + * sinh x = (e^x - e^(-x)) / 2 + * + * sinh x = 1 + x^3 / 3! + ... + x^(2 * N + 1) / (2 * N + 1)! + * + * An alternate algorithm, like a power series, would be more accurate. + */ + +static __inline__ sll sllsinh(sll x) +{ + return _slldiv2(_sllsub(sllexp(x), sllexp(_sllneg(x)))); +} + +/* + * Hyperbolic tangent + * + * Description + * + * tanh x = sinh x / cosh x + * + * tanh x = (e^x - e^(-x)) / (e^x + e^(-x)) + * + * tanh x = (e^(2 * x) - 1) / (e^(2 * x) + 1) + * + * An alternate algorithm, like a power series, would be more accurate. + */ + +static __inline__ sll slltanh(sll x) +{ + register sll e2x; + + e2x = sllexp(_sllmul2(x)); + + return _slldiv(_sllsub(e2x, CONST_1), _slladd(e2x, CONST_1)); +} + +/* + * Hyperbolic secant + * + * Description + * + * sech x = 1 / cosh x + * + * sech x = 2 / (e^x + e^(-x)) + * + * sech x = 2 * e^x / (e^(2 * x) + 1) + * + * An alternate algorithm, like a power series, would be more accurate. + */ + +static __inline__ sll sllsech(sll x) +{ + return _slldiv(_sllmul2(sllexp(x)), _slladd(sllexp(_sllmul2(x)), CONST_1)); +} + +/* + * Hyperbolic cosecant + * + * Description + * + * csch x = = 1 / sinh x + * + * csch x = 2 / (e^x - e^(-x)) + * + * csch x = 2 * e^x / (e^(2 * x) - 1) + * + * An alternate algorithm, like a power series, would be more accurate. + */ + +static __inline__ sll sllcsch(sll x) +{ + return _slldiv(_sllmul2(sllexp(x)), _sllsub(sllexp(_sllmul2(x)), CONST_1)); +} + +/* + * Hyperbolic cotangent + * + * Description + * + * coth x = 1 / tanh x + * + * coth x = cosh x / sinh x + * + * coth x = (e^x + e^(-x)) / (e^x - e^(-x)) + * + * coth x = (e^(2 * x) + 1) / (e^(2 * x) - 1) + * + * An alternate algorithm, like a power series, would be more accurate. + */ + +static __inline__ sll sllcoth(sll x) +{ + register sll e2x; + + e2x = sllexp(sllmul2(x)); + + return _slldiv(_slladd(e2x, CONST_1), _sllsub(e2x, CONST_1)); +} + +/* + * Floor + * + * Description + * + * floor x = largest integer not larger than x + */ + +static __inline__ sll sllfloor(sll x) +{ + register sll retval; + + retval = _sllint(x); + + return ((retval > x) ? _sllsub(retval, CONST_1): retval); +} + +/* + * Ceiling + * + * Description + * + * ceil x = smallest integer not smaller than x + */ + +static __inline__ sll sllceil(sll x) +{ + register sll retval; + + retval = _sllint(x); + + return ((retval < x) ? _slladd(retval, CONST_1): retval); +} + +#define sllabs(x) ((x) < 0 ? -(x) : (x)) +#define __VECTOR2_META__ "__VECTOR2_META__" +#define __VECTOR3_META__ "__VECTOR3_META__" +#define __ROT2_META__ "__ROT2_META__" +#define __ROT4_META__ "__ROT4_META__" +#define __METATABLE_NAME "__FIX_METATABLE__" + +static int _mul[] = {1, 10, 100, 1000, 10000, 100000, 1000000}; + +typedef struct Vector2 +{ + sll x; + sll y; +}Vector2; + +typedef struct Vector3 +{ + sll x; + sll y; + sll z; +}Vector3; + +typedef struct Vector4 +{ + sll x; + sll y; + sll z; + sll w; + +}Vector4; +#endif -- cgit v1.1-26-g67d0