sid/NazaraEngine
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Upgrade to Newton 3.14 and make it a thirdparty lib

Browse Source
This commit is contained in:
Jérôme Leclercq
2020-09-06 17:09:19 +02:00
parent 8913d5c1d1
commit 67b0d70b7c
240 changed files with 103390 additions and 311 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
SDK/include/NDK/Components/CollisionComponent3D.hpp
View File

@@ -32,7 +32,7 @@ namespace Ndk
void SetGeom(Nz::Collider3DRef geom);
CollisionComponent3D& operator=(Nz::Collider3DRef geom);
CollisionComponent3D& operator=(CollisionComponent3D&& collision) = default;
CollisionComponent3D& operator=(CollisionComponent3D&& collision) = delete;
static ComponentIndex componentIndex;

16
build/scripts/common.lua
View File

@@ -122,6 +122,10 @@ function NazaraBuild:Execute()
end
filter({})
if (libTable.Custom) then
libTable.Custom()
end
end
end
@@ -180,6 +184,10 @@ function NazaraBuild:Execute()
end
filter({})
if (moduleTable.Custom) then
moduleTable.Custom()
end
end
-- Tools
@@ -251,6 +259,10 @@ function NazaraBuild:Execute()
end
filter({})
if (toolTable.Custom) then
toolTable.Custom()
end
end
group("Examples")
@@ -301,6 +313,10 @@ function NazaraBuild:Execute()
end
filter({})
if (exampleTable.Custom) then
exampleTable.Custom()
end
end
end
end

20
build/scripts/modules/physics3d.lua
View File

@@ -1,6 +1,24 @@
MODULE.Name = "Physics3D"
MODULE.Defines = {
"_NEWTON_STATIC_LIB"
}
MODULE.OsDefines.Windows = {
"_WINDOWS"
}
MODULE.Libraries = {
"NazaraCore",
"Newton" -- Newton Game Dynamics
"newton" -- Newton Game Dynamics
}
MODULE.Custom = function()
vectorextensions("SSE3")
filter({"architecture:x86_64", "system:linux"})
defines("_POSIX_VER_64")
filter({"architecture:x86", "system:linux"})
defines("_POSIX_VER")
end

3
include/Nazara/Physics3D/PhysWorld3D.hpp
View File

@@ -49,7 +49,6 @@ namespace Nz
void SetGravity(const Vector3f& gravity);
void SetMaxStepCount(std::size_t maxStepCount);
void SetSolverModel(unsigned int model);
void SetStepSize(float stepSize);
void SetThreadCount(unsigned int threadCount);
@@ -72,7 +71,7 @@ namespace Nz
CollisionCallback collisionCallback;
};
static int OnAABBOverlap(const NewtonMaterial* const material, const NewtonBody* const body0, const NewtonBody* const body1, int threadIndex);
static int OnAABBOverlap(const NewtonJoint* const contact, float timestep, int threadIndex);
static void ProcessContact(const NewtonJoint* const contact, float timestep, int threadIndex);
std::unordered_map<Nz::UInt64, std::unique_ptr<Callback>> m_callbacks;

4
src/Nazara/Physics3D/Collider3D.cpp
View File

@@ -259,7 +259,7 @@ namespace Nz
NewtonCollision* CapsuleCollider3D::CreateHandle(PhysWorld3D* world) const
{
return NewtonCreateCapsule(world->GetHandle(), m_radius, m_length, 0, m_matrix);
return NewtonCreateCapsule(world->GetHandle(), m_radius, m_radius, m_length, 0, m_matrix);
}
/******************************* CompoundCollider3D ********************************/
@@ -396,7 +396,7 @@ namespace Nz
NewtonCollision* CylinderCollider3D::CreateHandle(PhysWorld3D* world) const
{
return NewtonCreateCylinder(world->GetHandle(), m_radius, m_length, 0, m_matrix);
return NewtonCreateCylinder(world->GetHandle(), m_radius, m_radius, m_length, 0, m_matrix);
}
/********************************* NullCollider3D **********************************/

36
src/Nazara/Physics3D/PhysWorld3D.cpp
View File

@@ -91,11 +91,6 @@ namespace Nz
m_maxStepCount = maxStepCount;
}
void PhysWorld3D::SetSolverModel(unsigned int model)
{
NewtonSetSolverModel(m_world, model);
}
void PhysWorld3D::SetStepSize(float stepSize)
{
m_stepSize = stepSize;
@@ -114,7 +109,8 @@ namespace Nz
callbackPtr->aabbOverlapCallback = std::move(aabbOverlapCallback);
callbackPtr->collisionCallback = std::move(collisionCallback);
NewtonMaterialSetCollisionCallback(m_world, firstMaterial, secondMaterial, callbackPtr.get(), (callbackPtr->aabbOverlapCallback) ? OnAABBOverlap : nullptr, (callbackPtr->collisionCallback) ? ProcessContact : nullptr);
NewtonMaterialSetCollisionCallback(m_world, firstMaterial, secondMaterial, (callbackPtr->aabbOverlapCallback) ? OnAABBOverlap : nullptr, (callbackPtr->collisionCallback) ? ProcessContact : nullptr);
NewtonMaterialSetCallbackUserData(m_world, firstMaterial, secondMaterial, callbackPtr.get());
UInt64 firstMaterialId(firstMaterial);
UInt64 secondMaterialId(secondMaterial);
@@ -161,17 +157,31 @@ namespace Nz
}
}
int PhysWorld3D::OnAABBOverlap(const NewtonMaterial* const material, const NewtonBody* const body0, const NewtonBody* const body1, int threadIndex)
int PhysWorld3D::OnAABBOverlap(const NewtonJoint* const contactJoint, dFloat timestep, int threadIndex)
{
RigidBody3D* bodyA = static_cast<RigidBody3D*>(NewtonBodyGetUserData(body0));
RigidBody3D* bodyB = static_cast<RigidBody3D*>(NewtonBodyGetUserData(body1));
RigidBody3D* bodyA = static_cast<RigidBody3D*>(NewtonBodyGetUserData(NewtonJointGetBody0(contactJoint)));
RigidBody3D* bodyB = static_cast<RigidBody3D*>(NewtonBodyGetUserData(NewtonJointGetBody1(contactJoint)));
assert(bodyA && bodyB);
Callback* callbackData = static_cast<Callback*>(NewtonMaterialGetMaterialPairUserData(material));
assert(callbackData);
assert(callbackData->aabbOverlapCallback);
using ContactJoint = void*;
return callbackData->aabbOverlapCallback(*bodyA, *bodyB);
// Query all joints first, to prevent removing a joint from the list while iterating on it
StackVector<ContactJoint> contacts = NazaraStackVector(ContactJoint, NewtonContactJointGetContactCount(contactJoint));
for (ContactJoint contact = NewtonContactJointGetFirstContact(contactJoint); contact; contact = NewtonContactJointGetNextContact(contactJoint, contact))
contacts.push_back(contact);
for (ContactJoint contact : contacts)
{
NewtonMaterial* material = NewtonContactGetMaterial(contact);
Callback* callbackData = static_cast<Callback*>(NewtonMaterialGetMaterialPairUserData(material));
assert(callbackData);
assert(callbackData->collisionCallback);
if (!callbackData->collisionCallback(*bodyA, *bodyB))
return 0;
}
return 1;
}
void PhysWorld3D::ProcessContact(const NewtonJoint* const contactJoint, float timestep, int threadIndex)

2
src/Nazara/Physics3D/RigidBody3D.cpp
View File

@@ -315,7 +315,7 @@ namespace Nz
{
// If we already have a mass, we already have an inertial matrix as well, just rescale it
float Ix, Iy, Iz;
NewtonBodyGetMassMatrix(m_body, &m_mass, &Ix, &Iy, &Iz);
NewtonBodyGetMass(m_body, &m_mass, &Ix, &Iy, &Iz);
float scale = mass / m_mass;
NewtonBodySetMassMatrix(m_body, mass, Ix*scale, Iy*scale, Iz*scale);

38
thirdparty/build/newton.lua vendored Normal file
View File

@@ -0,0 +1,38 @@
LIBRARY.Name = "newton"
LIBRARY.Defines = {
"_CRT_SECURE_NO_WARNINGS",
"_NEWTON_STATIC_LIB",
}
LIBRARY.OsDefines.Windows = {
"_WINDOWS"
}
LIBRARY.Language = "C++"
LIBRARY.Files = {
"../thirdparty/include/newton/**.h",
"../thirdparty/src/newton/**.h",
"../thirdparty/src/newton/**.c",
"../thirdparty/src/newton/**.cpp",
}
LIBRARY.Includes = {
"../thirdparty/src/newton/dgCore",
"../thirdparty/src/newton/dgMeshUtil",
"../thirdparty/src/newton/dgPhysics",
"../thirdparty/src/newton/dgNewton",
"../thirdparty/src/newton/dContainers",
"../thirdparty/src/newton/dMath"
}
LIBRARY.Custom = function()
vectorextensions("SSE3")
filter({"architecture:x86_64", "system:linux"})
defines("_POSIX_VER_64")
filter({"architecture:x86", "system:linux"})
defines("_POSIX_VER")
end

596
thirdparty/include/Newton/Newton.h vendored
View File

File diff suppressed because it is too large Load Diff

696
thirdparty/include/Newton/dgTypes.h vendored Normal file
View File

@@ -0,0 +1,696 @@
/* Copyright (c) <2003-2019> <Julio Jerez, Newton Game Dynamics>
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
*
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
*
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
*
* 3. This notice may not be removed or altered from any source distribution.
*/
#ifndef __DGTYPES_H__
#define __DGTYPES_H__
#ifdef _MSC_VER
#ifdef _M_ARM
#ifndef _ARM_VER
#define _ARM_VER
#endif
#elif defined (_M_X64)
#ifndef _WIN_64_VER
#define _WIN_64_VER
#endif
#elif defined (_M_IX86)
#ifndef _WIN_32_VER
#define _WIN_32_VER
#endif
#else
#error target platform not defined
#endif
#if _MSC_VER >= 1400
#define HAVE_STRUCT_TIMESPEC
#endif
#endif
#if (defined (_WIN_32_VER) || defined (_WIN_64_VER) || (defined (_MSC_VER ) && defined (_ARM_VER)) )
#pragma warning (disable: 4100) //unreferenced formal parameter
#pragma warning (disable: 4201) //nonstandard extension used : nameless struct/union
#pragma warning (disable: 4324) //structure was padded due to __declspec(align())
#pragma warning (disable: 4514) //unreferenced inline function has been removed
#pragma warning (disable: 4530) //C++ exception handler used, but unwind semantics are not enabled. Specify /EHsc
#pragma warning (disable: 4625) //copy constructor could not be generated because a base class copy constructor is inaccessible or deleted
#pragma warning (disable: 4626) //assignment operator could not be generated because a base class assignment operator is inaccessible or deleted
#pragma warning (disable: 4640) //construction of local static object is not thread-safe
#pragma warning (disable: 4820) //bytes padding added after data member
#pragma warning (disable: 4005) //'__useHeader': macro redefinition
//#pragma warning (disable: 4577) // 'noexcept' used with no exception handling mode specified; termination on exception is not guaranteed. Specify /EHsc
#include <io.h>
#include <direct.h>
#include <malloc.h>
#include <float.h>
#include <stdarg.h>
#include <process.h>
#ifdef _DEBUG
#pragma warning (disable: 4127) //conditional expression is constant
#endif
#pragma warning (push, 3)
#include <windows.h>
#include <crtdbg.h>
//#ifndef _DURANGO
// #include <tlhelp32.h>
//#endif
#pragma warning (pop)
#endif
#include <new>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <stdarg.h>
#include <time.h>
#include <math.h>
#include <float.h>
#include <ctype.h>
#if (defined (__MINGW32__) || defined (__MINGW64__))
#include <io.h>
#include <direct.h>
#include <malloc.h>
#include <float.h>
#include <windows.h>
#include <process.h>
#endif
#if (defined (_WIN_32_VER) || defined (_WIN_64_VER))
#include <intrin.h>
#include <emmintrin.h>
#include <pmmintrin.h>
#endif
#if (defined (_POSIX_VER) || defined (_POSIX_VER_64) || defined (__MINGW32__) || defined (__MINGW64__))
/* CMake defines NDEBUG for _not_ debug builds. Therefore, set
Newton's _DEBUG flag only when NDEBUG is not defined.
*/
#ifndef NDEBUG
#define _DEBUG 1
#endif
#include <unistd.h>
#include <assert.h>
#if (!defined(__arm__) && !defined(__aarch64__)) // it was __ARMCC_VERSION before, it should be __ARM__ or aarch64, otherwise cross compiling in gcc fails.
extern "C"
{
// for SSE3 and up
#include <pmmintrin.h>
#include <emmintrin.h>
#include <mmintrin.h>
}
#endif
#endif
#ifdef _MACOSX_VER
#include <unistd.h>
#include <sys/sysctl.h>
#include <assert.h>
#if (defined __i386__ || defined __x86_64__)
#include <fenv.h>
#include <pmmintrin.h>
#include <emmintrin.h> //sse3
#include <mmintrin.h>
#endif
#endif
// uncomment out D_PROFILER to enable profiler frame capture profiler traces
// alternatively the end application can use a command line option to enable this define
//#define D_PROFILER
// uncomment this for Scalar floating point
// alternatively the end application can use a command line option to enable this define
//#define DG_SCALAR_VECTOR_CLASS
// uncomment this for Scalar floating point
// alternatively the end application can use a command line option to enable this define
//#define __ANDROID__
// by default newton run on a separate thread and
// optionally concurrent with the calling thread,
// it also uses a thread job pool for multi core systems.
// define DG_USE_THREAD_EMULATION on the command line for
// platform that do not support hardware multi threading or
// if the and application want to control threading at the application level
//#define DG_USE_THREAD_EMULATION
#if (defined (_WIN_32_VER) || defined (_WIN_64_VER))
#if _MSC_VER < 1700
#ifndef DG_USE_THREAD_EMULATION
#define DG_USE_THREAD_EMULATION
#endif
#endif
#endif
#ifndef DG_USE_THREAD_EMULATION
#include <mutex>
#include <thread>
#include <condition_variable>
#endif
//************************************************************
#ifdef DG_DISABLE_ASSERT
#define dgAssert(x)
#else
#if defined (_WIN_32_VER) || defined (_WIN_64_VER)
#define dgAssert(x) _ASSERTE(x)
#elif defined (_MSC_VER ) && defined (_ARM_VER)
#define dgAssert(x) _ASSERTE(x)
#else
#ifdef _DEBUG
#define dgAssert(x) assert(x)
#else
#define dgAssert(x)
#endif
#endif
#endif
#define DG_MAX_THREADS_HIVE_COUNT 16
#ifdef _DEBUG
//#define __ENABLE_DG_CONTAINERS_SANITY_CHECK
#endif
#ifdef DLL_DECLSPEC
#undef DLL_DECLSPEC
#endif
#ifdef _DEBUG
#define DG_INLINE inline
#else
#if defined(_MSC_VER)
#define DG_INLINE __forceinline
#else
#define DG_INLINE inline
//#define DG_INLINE __attribute__((always_inline))
#endif
#endif
#define DG_VECTOR_SIMD_SIZE 16
#define DG_VECTOR_AVX2_SIZE 32
#if defined(_MSC_VER)
#define DG_GCC_VECTOR_ALIGNMENT
#define DG_MSC_VECTOR_ALIGNMENT __declspec(align(DG_VECTOR_SIMD_SIZE))
#else
#define DG_GCC_VECTOR_ALIGNMENT __attribute__ ((aligned (DG_VECTOR_SIMD_SIZE)))
#define DG_MSC_VECTOR_ALIGNMENT
#endif
#if defined(_MSC_VER)
#define DG_GCC_AVX_ALIGNMENT
#define DG_MSC_AVX_ALIGNMENT __declspec(align(DG_VECTOR_AVX2_SIZE))
#else
#define DG_GCC_AVX_ALIGNMENT __attribute__ ((aligned (DG_VECTOR_AVX2_SIZE)))
#define DG_MSC_AVX_ALIGNMENT
#endif
#if defined(_MSC_VER)
#define DG_LIBRARY_EXPORT __declspec(dllexport)
#define DG_LIBRARY_IMPORT __declspec(dllimport)
#define DG_LIBRARY_STATIC
#else
#define DG_LIBRARY_EXPORT __attribute__((visibility("default")))
#define DG_LIBRARY_IMPORT __attribute__((visibility("default")))
#define DG_LIBRARY_STATIC
#endif
#if ((defined (_WIN_32_VER) || defined (_WIN_64_VER)) && (_MSC_VER >= 1600))
#include <stdint.h>
typedef int8_t dgInt8;
typedef uint8_t dgUnsigned8;
typedef int16_t dgInt16;
typedef uint16_t dgUnsigned16;
typedef int32_t dgInt32;
typedef uint32_t dgUnsigned32;
typedef int64_t dgInt64;
typedef uint64_t dgUnsigned64;
#else
typedef char dgInt8;
typedef unsigned char dgUnsigned8;
typedef short dgInt16;
typedef unsigned short dgUnsigned16;
typedef int dgInt32;
typedef unsigned dgUnsigned32;
typedef unsigned int dgUnsigned32;
typedef long long dgInt64;
typedef unsigned long long dgUnsigned64;
typedef double dgFloat64;
#endif
typedef double dgFloat64;
#ifdef _NEWTON_USE_DOUBLE
typedef double dgFloat32;
#else
typedef float dgFloat32;
#endif
class dgTriplex
{
public:
dgFloat32 m_x;
dgFloat32 m_y;
dgFloat32 m_z;
};
#define dgPi dgFloat32 (3.141592f)
#define dgPI2 dgFloat32 (dgPi * 2.0f)
#define dgEXP dgFloat32 (2.71828f)
#define dgEpsilon dgFloat32 (1.0e-5f)
#define dgDegreeToRad dgFloat32 (dgPi / 180.0f)
#define dgRadToDegree dgFloat32 (180.0f / dgPi)
class dgBigVector;
#ifndef _NEWTON_USE_DOUBLE
class dgVector;
#endif
#if (defined (_WIN_32_VER) || defined (_WIN_64_VER))
#define dgApi __cdecl
#define dgStdApi __stdcall
#else
#define dgApi
#define dgStdApi
#endif
#if (defined (_WIN_32_VER) || defined (_WIN_64_VER))
#define dgCheckFloat(x) (_finite(x) && !_isnan(x))
// #define dgCheckFloat(x) 1
#else
#define dgCheckFloat(x) (isfinite(x) && !isnan(x))
// #define dgCheckFloat(x) 1
#endif
typedef void (dgApi *dgDeserialize) (void* const userData, void* buffer, dgInt32 size);
typedef void (dgApi *dgSerialize) (void* const userData, const void* const buffer, dgInt32 size);
typedef bool (dgApi *dgReportProgress) (dgFloat32 progressNormalzedPercent, void* const userData);
// assume this function returns memory aligned to 16 bytes
#define dgAlloca(type, count) (type*) alloca (sizeof (type) * (count))
DG_INLINE dgInt32 dgExp2 (dgInt32 x)
{
dgInt32 exp;
for (exp = -1; x; x >>= 1) {
exp ++;
}
return exp;
}
DG_INLINE dgInt32 dgBitReversal(dgInt32 v, dgInt32 base)
{
dgInt32 x = 0;
dgInt32 power = dgExp2 (base) - 1;
do {
x += (v & 1) << power;
v >>= 1;
power--;
} while (v);
dgAssert(x < base);
return x;
}
template <class T>
DG_INLINE T dgMin(T A, T B)
{
return (A < B) ? A : B;
}
template <class T>
DG_INLINE T dgMax(T A, T B)
{
return (A > B) ? A : B;
}
template <class T>
DG_INLINE T dgMin(T A, T B, T C)
{
return dgMin(dgMin (A, B), C);
}
template <class T>
DG_INLINE T dgMax(T A, T B, T C)
{
return dgMax(dgMax (A, B), C);
}
template <class T>
DG_INLINE T dgClamp(T val, T min, T max)
{
return dgMax (min, dgMin (max, val));
}
template <class T>
DG_INLINE void dgSwap(T& A, T& B)
{
T tmp (A);
A = B;
B = tmp;
}
template <class T>
DG_INLINE T dgAbs(T A)
{
// according to Intel this is better because is does not read after write
return (A >= T(0)) ? A : -A;
}
template <class T>
DG_INLINE T dgSign(T A)
{
return (A >= T(0)) ? T(1) : T(-1);
}
template <class T>
DG_INLINE bool dgAreEqual(T A, T B, T tol)
{
if ((dgAbs(A) < tol) && (dgAbs(B) < tol)) {
return true;
}
/*
dgInt32 exp0;
dgFloat64 mantissa0 = frexp(dgFloat64 (A), &exp0);
dgInt32 exp1;
dgFloat64 mantissa1 = frexp(dgFloat64(B), &exp1);
if ((exp0 < -12) && (exp1 < -12)) {
return true;
}
if (exp0 != exp1) {
return false;
}
return dgAbs(mantissa0 - mantissa1) < tol;
*/
T den = dgMax(dgAbs(A), dgAbs(B)) + tol;
A /= den;
B /= den;
return dgAbs(A - B) < tol;
}
#ifdef _NEWTON_USE_DOUBLE
union dgFloatSign
{
struct {
dgInt32 m_dommy;
dgInt32 m_iVal;
} m_integer;
dgFloat64 m_fVal;
};
#else
union dgFloatSign
{
struct {
dgInt32 m_iVal;
} m_integer;
dgFloat32 m_fVal;
};
#endif
union dgDoubleInt
{
struct {
dgInt32 m_intL;
dgInt32 m_intH;
};
void* m_ptr;
dgInt64 m_int;
dgFloat64 m_float;
};
void dgGetMinMax (dgBigVector &Min, dgBigVector &Max, const dgFloat64* const vArray, dgInt32 vCount, dgInt32 strideInBytes);
dgInt32 dgVertexListToIndexList (dgFloat64* const vertexList, dgInt32 strideInBytes, dgInt32 compareCount, dgInt32 vertexCount, dgInt32* const indexListOut, dgFloat64 tolerance = dgEpsilon);
dgInt32 dgVertexListToIndexList (dgFloat32* const vertexList, dgInt32 strideInBytes, dgInt32 floatSizeInBytes, dgInt32 unsignedSizeInBytes, dgInt32 vertexCount, dgInt32* const indexListOut, dgFloat32 tolerance = dgEpsilon);
#define PointerToInt(x) ((size_t)x)
#define IntToPointer(x) ((void*)(size_t(x)))
#ifndef _MSC_VER
#define _stricmp(x,y) strcasecmp(x,y)
#endif
#define dgSqrt(x) dgFloat32 (sqrt(x))
#define dgSin(x) dgFloat32 (sin(x))
#define dgCos(x) dgFloat32 (cos(x))
#define dgAsin(x) dgFloat32 (asin(x))
#define dgAcos(x) dgFloat32 (acos(x))
#define dgLog(x) dgFloat32 (log(x))
#define dgCeil(x) dgFloat32 (ceil(x))
#define dgFloor(x) dgFloat32 (floor(x))
#define dgPow(x,y) dgFloat32 (pow(x,y))
#define dgFmod(x,y) dgFloat32 (fmod(x,y))
#define dgAtan2(x,y) dgFloat32 (atan2(x,y))
#define dgRsqrt(x) (dgFloat32 (1.0f) / dgSqrt(x))
#define dgClearFP() _clearfp()
#define dgControlFP(x,y) _controlfp(x,y)
enum dgSerializeRevisionNumber
{
m_firstRevision = 100,
// add new serialization revision number here
m_currentRevision
};
dgUnsigned64 dgGetTimeInMicrosenconds();
dgFloat64 dgRoundToFloat(dgFloat64 val);
void dgSerializeMarker(dgSerialize serializeCallback, void* const userData);
dgInt32 dgDeserializeMarker(dgDeserialize serializeCallback, void* const userData);
class dgFloatExceptions
{
public:
#ifdef _MSC_VER
#define DG_FLOAT_EXECTIONS_MASK (EM_INVALID | EM_DENORMAL | EM_ZERODIVIDE)
#else
#define DG_FLOAT_EXECTIONS_MASK 0
#endif
dgFloatExceptions(dgUnsigned32 mask = DG_FLOAT_EXECTIONS_MASK);
~dgFloatExceptions();
private:
#if (defined (_MSC_VER) && defined (_WIN_32_VER))
dgUnsigned32 m_mask;
#endif
};
class dgSetPrecisionDouble
{
public:
dgSetPrecisionDouble();
~dgSetPrecisionDouble();
#if (defined (_MSC_VER) && defined (_WIN_32_VER))
dgInt32 m_mask;
#endif
};
DG_INLINE dgInt32 dgAtomicExchangeAndAdd (dgInt32* const addend, dgInt32 amount)
{
#if (defined (_WIN_32_VER) || defined (_WIN_64_VER))
return _InterlockedExchangeAdd((long*)addend, long(amount));
#elif defined (_MSC_VER ) && defined (_ARM_VER)
return _InterlockedExchangeAdd((long*)addend, long(amount));
#elif (defined (__MINGW32__) || defined (__MINGW64__))
return InterlockedExchangeAdd((long*)addend, long(amount));
#elif (defined (_POSIX_VER) || defined (_POSIX_VER_64) ||defined (_MACOSX_VER)|| defined ANDROID)
return __sync_fetch_and_add((int32_t*)addend, amount);
#else
#error "dgAtomicExchangeAndAdd implementation required"
#endif
}
DG_INLINE dgInt32 dgInterlockedExchange(dgInt32* const ptr, dgInt32 value)
{
#if (defined (_WIN_32_VER) || defined (_WIN_64_VER))
return _InterlockedExchange((long*) ptr, value);
#elif defined (_MSC_VER ) && defined (_ARM_VER)
return _InterlockedExchange((long*)ptr, value);
#elif (defined (__MINGW32__) || defined (__MINGW64__))
return InterlockedExchange((long*) ptr, value);
#elif (defined (_POSIX_VER) || defined (_POSIX_VER_64) ||defined (_MACOSX_VER))
return __sync_lock_test_and_set((int32_t*)ptr, value);
#else
#error "dgInterlockedExchange implementation required"
#endif
}
/*
DG_INLINE void* dgInterlockedExchange(void** const ptr, void* value)
{
#if defined(_WIN32)
#ifdef _M_X64
return (void*)_InterlockedExchange64((dgInt64*)ptr, dgInt64 (value));
#else
return (void*)_InterlockedExchange((long*) ptr, dgInt32(value));
#endif
#elif defined(__linux__) || defined(_MACOSX_VER)
#if defined(__ANDROID__)
return __sync_lock_test_and_set(ptr, value);
#else
#ifdef __x86_64__
return (void*)__sync_lock_test_and_set((dgInt64*) ptr, (dgInt64)value);
#else
return (void*)__sync_lock_test_and_set((dgInt32*) ptr, (dgInt32) value);
#endif
#endif
#else
#error "dgInterlockedExchange implementation required"
#endif
}
*/
DG_INLINE dgInt32 dgInterlockedTest(dgInt32* const ptr, dgInt32 value)
{
#if (defined (_WIN_32_VER) || defined (_WIN_64_VER))
return _InterlockedCompareExchange((long*)ptr, value, value);
#elif defined (_MSC_VER ) && defined (_ARM_VER)
return _InterlockedCompareExchange((long*)ptr, value, value);
#elif (defined (__MINGW32__) || defined (__MINGW64__))
return InterlockedCompareExchange((long*)ptr, value, value);
#elif (defined (_POSIX_VER) || defined (_POSIX_VER_64) ||defined (_MACOSX_VER))
return __sync_lock_test_and_set((int32_t*)ptr, value);
#else
#error "dgInterlockedTest implementation required"
#endif
}
DG_INLINE void dgThreadYield()
{
#ifndef DG_USE_THREAD_EMULATION
std::this_thread::yield();
#endif
}
DG_INLINE void dgThreadPause()
{
#ifndef DG_USE_THREAD_EMULATION
//#if defined (_WIN_32_VER) || defined (_WIN_64_VER) || defined (WIN32) || defined (i386_) || defined (x86_64_)
#if defined (_WIN_32_VER) || defined (_WIN_64_VER)
_mm_pause();
#else
std::this_thread::yield();
#endif
#endif
}
DG_INLINE void dgSpinLock(dgInt32* const ptr)
{
#ifndef DG_USE_THREAD_EMULATION
while (dgInterlockedExchange(ptr, 1)) {
dgThreadYield();
}
#endif
}
DG_INLINE void dgSpinUnlock (dgInt32* const ptr)
{
#ifndef DG_USE_THREAD_EMULATION
dgInterlockedExchange(ptr, 0);
#else
*ptr = 0;
#endif
}
class dgScopeSpinLock
{
public:
DG_INLINE dgScopeSpinLock(dgInt32* const lock)
:m_atomicLock(lock)
{
dgSpinLock(m_atomicLock);
}
DG_INLINE ~dgScopeSpinLock()
{
dgSpinUnlock(m_atomicLock);
}
dgInt32* m_atomicLock;
};
class dgScopeSpinPause
{
public:
DG_INLINE dgScopeSpinPause(dgInt32* const lock)
:m_atomicLock(lock)
{
while (dgInterlockedExchange(m_atomicLock, 1)) {
dgThreadPause();
}
}
DG_INLINE ~dgScopeSpinPause()
{
dgSpinUnlock(m_atomicLock);
}
dgInt32* m_atomicLock;
};
#ifdef _MACOSX_VER
#include <sys/time.h>
#ifndef CLOCK_REALTIME
#define CLOCK_REALTIME 0
#endif
#ifndef CLOCK_MONOTONIC
#define CLOCK_MONOTONIC 0
#endif
//clock_gettime is not implemented on OSX
DG_INLINE int clock_gettime(int /*clk_id*/, struct timespec* t) {
struct timeval now;
int rv = gettimeofday(&now, NULL);
if (rv) return rv;
t->tv_sec = now.tv_sec;
t->tv_nsec = now.tv_usec * 1000;
return 0;
}
#endif
#endif

1
thirdparty/lib/common/x64/Newton_d.dll.REMOVED.git-id vendored
View File

@@ -1 +0,0 @@
0248a702d5e35c14dc8adfb6f0d4ba7d18d90c8b

1
thirdparty/lib/common/x86/Newton_d.dll.REMOVED.git-id vendored
View File

@@ -1 +0,0 @@
c1629ecc8c31a6a8f6648448bc655824e35435b0

1
thirdparty/lib/gmake/x64/libNewton.a.REMOVED.git-id vendored
View File

@@ -1 +0,0 @@
f79ffcc0c10c1bffafba367583e3675a15fa9167

1
thirdparty/lib/gmake/x86/libNewton.a.REMOVED.git-id vendored
View File

@@ -1 +0,0 @@
50c86d0a541fd1757e41ef7f8a368d969395fdf6

61
thirdparty/src/newton/dContainers/CMakeLists.txt vendored Normal file
View File

@@ -0,0 +1,61 @@
# Copyright (c) <2014-2017> <Newton Game Dynamics>
#
# This software is provided 'as-is', without any express or implied
# warranty. In no event will the authors be held liable for any damages
# arising from the use of this software.
#
# Permission is granted to anyone to use this software for any purpose,
# including commercial applications, and to alter it and redistribute it
# freely.
cmake_minimum_required(VERSION 3.4.0)
set (projectName "dContainers")
message (${projectName})
#source and header files
file(GLOB CPP_SOURCE *.h *.cpp)
file(GLOB HEADERS *.h)
source_group(TREE "${CMAKE_CURRENT_SOURCE_DIR}/" FILES ${CPP_SOURCE})
if(NEWTON_BUILD_SHARED_LIBS)
if(MSVC OR MINGW)
add_definitions(-D_DCONTAINERS_DLL)
add_definitions(-D_DCONTAINERS_EXPORT)
endif()
add_library(${projectName} SHARED ${CPP_SOURCE})
else(NEWTON_BUILD_SHARED_LIBS)
if (MSVC OR MINGW)
add_definitions(-D_NEWTON_STATIC_LIB)
endif()
add_library(${projectName} STATIC ${CPP_SOURCE})
endif(NEWTON_BUILD_SHARED_LIBS)
target_include_directories(${projectName} PUBLIC . ../dMath ../dgCore)
if (NEWTON_BUILD_PROFILER)
target_link_libraries (${projectName} dProfiler)
endif()
if (MSVC)
if(CMAKE_VS_MSBUILD_COMMAND OR CMAKE_VS_DEVENV_COMMAND)
set_target_properties(${projectName} PROPERTIES COMPILE_FLAGS "/YudContainersStdAfx.h")
set_source_files_properties(dContainersStdAfx.cpp PROPERTIES COMPILE_FLAGS "/YcdContainersStdAfx.h")
endif()
if (NEWTON_BUILD_SANDBOX_DEMOS)
add_custom_command(
TARGET ${projectName} POST_BUILD
COMMAND ${CMAKE_COMMAND}
ARGS -E copy $<TARGET_FILE:${projectName}> ${PROJECT_BINARY_DIR}/applications/demosSandbox/${CMAKE_CFG_INTDIR}/$<TARGET_FILE_NAME:${projectName}>)
endif ()
endif(MSVC)
install(TARGETS ${projectName}
LIBRARY DESTINATION lib
ARCHIVE DESTINATION lib
RUNTIME DESTINATION bin)
install(FILES ${HEADERS} DESTINATION include/${projectName})

114
thirdparty/src/newton/dContainers/dArray.h vendored Normal file
View File

@@ -0,0 +1,114 @@
/* Copyright (c) <2003-2019> <Newton Game Dynamics>
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely
*/
#ifndef __D_ARRAY__
#define __D_ARRAY__
#include "dContainersAlloc.h"
template<class T>
class dArray: public dContainersAlloc
{
public:
dArray();
dArray(int size);
~dArray();
T& operator[] (int i);
const T& operator[] (int i) const;
int GetSize() const;
void Resize(int size) const;
protected:
mutable int m_capacity;
mutable T* m_data;
};
template<class T>
dArray<T>::dArray()
:m_capacity(0)
,m_data(NULL)
{
}
template<class T>
dArray<T>::dArray(int size)
:dContainersAlloc()
,m_capacity(0)
,m_data(NULL)
{
Resize(size);
}
template<class T>
dArray<T>::~dArray()
{
if (m_data) {
delete[] m_data;
}
}
template<class T>
T& dArray<T>::operator[] (int i)
{
dAssert(i >= 0);
while (i >= m_capacity) {
Resize(dMax (i * 2, 1));
}
return m_data[i];
}
template<class T>
const T& dArray<T>::operator[] (int i) const
{
dAssert(i >= 0);
while (i >= m_capacity) {
Resize(dMax (i * 2, 1));
}
return m_data[i];
}
template<class T>
int dArray <T>::GetSize() const
{
return m_capacity;
}
template<class T>
void dArray <T>::Resize(int size) const
{
if (size >= m_capacity) {
T* const newArray = new T[size];
if (m_data) {
for (int i = 0; i < m_capacity; i++) {
newArray[i] = m_data[i];
}
delete[] m_data;
}
m_data = newArray;
m_capacity = size;
} else if (size < m_capacity) {
T* const newArray = new T[size];
if (m_data) {
for (int i = 0; i < size; i++) {
newArray[i] = m_data[i];
}
delete[] m_data;
}
m_data = newArray;
m_capacity = size;
}
}
#endif

148
thirdparty/src/newton/dContainers/dBaseHierarchy.cpp vendored Normal file
View File

@@ -0,0 +1,148 @@
/* Copyright (c) <2003-2019> <Newton Game Dynamics>
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely
*/
#include "dContainersStdAfx.h"
#include "dBaseHierarchy.h"
dBaseHierarchy::dBaseHierarchy (const dBaseHierarchy &clone)
{
Clear ();
SetNameID (clone.m_name.GetStr());
for (dBaseHierarchy* obj = clone.m_child; obj; obj = obj->m_sibling) {
dBaseHierarchy* const newObj = obj->CreateClone ();
newObj->Attach (this);
}
}
dBaseHierarchy::~dBaseHierarchy ()
{
Detach();
while (m_child) {
delete m_child;
}
}
void dBaseHierarchy::Attach (dBaseHierarchy* const parentArg, bool addFirst)
{
m_parent = parentArg;
if (m_parent->m_child) {
if (addFirst) {
m_sibling = m_parent->m_child;
m_parent->m_child = this;
} else {
dBaseHierarchy* obj = m_parent->m_child;
for (; obj->m_sibling; obj = obj->m_sibling);
obj->m_sibling = this;
}
} else {
m_parent->m_child = this;
}
}
void dBaseHierarchy::Detach ()
{
if (m_parent) {
if (m_parent->m_child == this) {
m_parent->m_child = m_sibling;
} else {
dBaseHierarchy* ptr = m_parent->m_child;
for (; ptr->m_sibling != this; ptr = ptr->m_sibling);
ptr->m_sibling = m_sibling;
}
m_parent = NULL;
m_sibling = NULL;
}
}
dBaseHierarchy* dBaseHierarchy::GetRoot() const
{
const dBaseHierarchy* root = this;
for (; root->m_parent; root = root->m_parent);
return (dBaseHierarchy*)root;
}
dBaseHierarchy* dBaseHierarchy::GetFirst() const
{
dBaseHierarchy* ptr = (dBaseHierarchy*) this;
for (; ptr->m_child; ptr = ptr->m_child);
return ptr;
}
dBaseHierarchy* dBaseHierarchy::GetNext() const
{
if (m_sibling) {
return m_sibling->GetFirst();
}
dBaseHierarchy* ptr = m_parent;
dBaseHierarchy* x = (dBaseHierarchy *)this;
for (; ptr && (x == ptr->m_sibling); ptr = ptr->m_parent) {
x = ptr;
}
return ptr;
}
dBaseHierarchy* dBaseHierarchy::GetLast() const
{
dBaseHierarchy* ptr = (dBaseHierarchy*) this;
for (; ptr->m_sibling; ptr = ptr->m_sibling);
return ptr;
}
dBaseHierarchy* dBaseHierarchy::GetPrev() const
{
if (m_child) {
return m_child->GetNext();
}
dBaseHierarchy* ptr = m_parent;
dBaseHierarchy* x = (dBaseHierarchy *)this;
for (; ptr && (x == ptr->m_child); ptr = ptr->m_child) {
x = ptr;
}
return ptr;
}
dBaseHierarchy* dBaseHierarchy::Find (dCRCTYPE nameCRC) const
{
if (nameCRC == GetNameID()) {
return (dBaseHierarchy*)this;
} else {
for (dBaseHierarchy* ptr = GetFirst(); ptr && (ptr != this); ptr = ptr->GetNext()) {
if (nameCRC == ptr->GetNameID()) {
return ptr;
}
}
}
return NULL;
}

256
thirdparty/src/newton/dContainers/dBaseHierarchy.h vendored Normal file
View File

@@ -0,0 +1,256 @@
/* Copyright (c) <2003-2019> <Newton Game Dynamics>
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely
*/
#ifndef __Hierarchy__
#define __Hierarchy__
#include "dCRC.h"
#include "dString.h"
#include "dContainersAlloc.h"
class dBaseHierarchy: public dContainersAlloc
{
public:
DCONTAINERS_API dBaseHierarchy* GetChild () const;
DCONTAINERS_API dBaseHierarchy* GetParent () const;
DCONTAINERS_API dBaseHierarchy* GetSibling () const;
DCONTAINERS_API void Detach ();
DCONTAINERS_API void Attach (dBaseHierarchy* const parent, bool addFirst = false);
DCONTAINERS_API dBaseHierarchy* GetRoot () const;
DCONTAINERS_API dBaseHierarchy* GetFirst() const;
DCONTAINERS_API dBaseHierarchy* GetLast() const;
DCONTAINERS_API dBaseHierarchy* GetNext() const;
DCONTAINERS_API dBaseHierarchy* GetPrev() const;
DCONTAINERS_API dBaseHierarchy* Find (dCRCTYPE nameCRC) const;
DCONTAINERS_API dBaseHierarchy* Find (const char* const name) const;
DCONTAINERS_API long long GetNameID() const;
DCONTAINERS_API const dString& GetName() const;
void SetNameID(const char* const name);
protected:
DCONTAINERS_API dBaseHierarchy ();
DCONTAINERS_API dBaseHierarchy (const char* const name);
DCONTAINERS_API dBaseHierarchy (const dBaseHierarchy &clone);
virtual DCONTAINERS_API ~dBaseHierarchy ();
virtual DCONTAINERS_API dBaseHierarchy* CreateClone () const = 0;
private:
inline void Clear();
dString m_name;
long long m_nameID;
dBaseHierarchy* m_parent;
dBaseHierarchy* m_child;
dBaseHierarchy* m_sibling;
};
template<class T>
class dHierarchy: public dBaseHierarchy
{
public:
dHierarchy ();
dHierarchy (const char* const name);
void Attach (T* const parent, bool addFirst = false);
void Detach ();
T* GetChild () const;
T* GetParent () const;
T* GetSibling () const;
T* GetRoot () const;
T* GetFirst() const;
T* GetLast() const;
T* GetNext() const;
T* GetPrev() const;
T* Find (long long nameCRC) const;
T* Find (const char* const name) const;
protected:
dHierarchy (const T &clone);
virtual ~dHierarchy ();
};
inline dBaseHierarchy::dBaseHierarchy ()
{
Clear ();
}
inline dBaseHierarchy::dBaseHierarchy (const char* const name)
{
Clear ();
SetNameID (name);
}
inline void dBaseHierarchy::Clear()
{
m_child = NULL;
m_parent = NULL;
m_sibling = NULL;
m_nameID = 0;
m_name = (char*)NULL;
}
inline dBaseHierarchy* dBaseHierarchy::GetChild () const
{
return m_child;
}
inline dBaseHierarchy* dBaseHierarchy::GetSibling () const
{
return m_sibling;
}
inline dBaseHierarchy* dBaseHierarchy::GetParent () const
{
return m_parent;
}
inline dBaseHierarchy* dBaseHierarchy::Find (const char* const name) const
{
return Find (dCRC64 (name));
}
inline void dBaseHierarchy::SetNameID(const char* const name)
{
m_nameID = dCRC64 (name);
m_name = name;
}
inline long long dBaseHierarchy::GetNameID() const
{
return m_nameID;
}
inline const dString& dBaseHierarchy::GetName() const
{
return m_name;
}
template<class T>
dHierarchy<T>::dHierarchy ()
:dBaseHierarchy ()
{
}
template<class T>
dHierarchy<T>::dHierarchy (const T &clone)
:dBaseHierarchy (clone)
{
}
template<class T>
dHierarchy<T>::dHierarchy (const char* const name)
:dBaseHierarchy (name)
{
}
template<class T>
dHierarchy<T>::~dHierarchy ()
{
}
//template<class T>
//dBaseHierarchy* dHierarchy<T>::CreateClone () const
//{
// return new T (*(T*)this);
//}
template<class T>
void dHierarchy<T>::Attach (T* const parent, bool addFirst)
{
dBaseHierarchy::Attach(parent, addFirst);
}
template<class T>
void dHierarchy<T>::Detach ()
{
dBaseHierarchy::Detach ();
}
template<class T>
T* dHierarchy<T>::GetChild () const
{
return (T*) dBaseHierarchy::GetChild();
}
template<class T>
T* dHierarchy<T>::GetSibling () const
{
return (T*) dBaseHierarchy::GetSibling ();
}
template<class T>
T* dHierarchy<T>::GetParent () const
{
return (T*) dBaseHierarchy::GetParent ();
}
template<class T>
T* dHierarchy<T>::GetRoot () const
{
return (T*) dBaseHierarchy::GetRoot ();
}
template<class T>
T* dHierarchy<T>::GetFirst() const
{
return (T*) dBaseHierarchy::GetFirst ();
}
template<class T>
T* dHierarchy<T>::GetLast() const
{
return (T*) dBaseHierarchy::GetLast ();
}
template<class T>
T* dHierarchy<T>::GetNext() const
{
return (T*) dBaseHierarchy::GetNext ();
}
template<class T>
T* dHierarchy<T>::GetPrev() const
{
return (T*) dBaseHierarchy::GetPrev ();
}
template<class T>
T* dHierarchy<T>::Find (dCRCTYPE nameCRC) const
{
return (T*) dBaseHierarchy::Find (nameCRC);
}
template<class T>
T* dHierarchy<T>::Find (const char* const name) const
{
return (T*) dBaseHierarchy::Find (name);
}
#endif

641
thirdparty/src/newton/dContainers/dBezierSpline.cpp vendored Normal file
View File

@@ -0,0 +1,641 @@
/* Copyright (c) <2003-2019> <Newton Game Dynamics>
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely
*/
#include "dContainersStdAfx.h"
#include "dBezierSpline.h"
dBezierSpline::dBezierSpline ()
:dContainersAlloc()
,m_knotVector(16)
,m_controlPoints(16)
,m_degree(0)
,m_knotsCount(0)
,m_controlPointsCount(0)
{
}
dBezierSpline::dBezierSpline (const dBezierSpline& src)
:m_knotVector(src.m_knotsCount)
,m_controlPoints(src.m_controlPointsCount)
,m_degree(0)
,m_knotsCount(0)
,m_controlPointsCount(0)
{
if (src.m_knotsCount) {
CreateFromKnotVectorAndControlPoints (src.m_degree, src.m_knotsCount - 2 * src.m_degree, &src.m_knotVector[src.m_degree], &src.m_controlPoints[0]);
}
}
dBezierSpline::~dBezierSpline ()
{
Clear();
}
void dBezierSpline::Clear()
{
m_degree = 0;
m_knotsCount = 0;
m_controlPointsCount = 0;
}
dBezierSpline& dBezierSpline::operator = (const dBezierSpline &copy)
{
Clear();
if (copy.m_knotsCount) {
CreateFromKnotVectorAndControlPoints (copy.m_degree, copy.m_knotsCount - 2 * copy.m_degree, &copy.m_knotVector[copy.m_degree], &copy.m_controlPoints[0]);
}
return *this;
}
int dBezierSpline::GetDegree () const
{
return m_degree;
}
void dBezierSpline::CreateFromKnotVectorAndControlPoints (int degree, int knotCount, const dFloat64* const knotVector, const dBigVector* const controlPoints)
{
Clear();
dAssert (knotCount);
dAssert (knotVector[0] == 0.0f);
dAssert (knotVector[knotCount - 1] == 1.0f);
m_degree = degree;
m_knotsCount = knotCount + 2 * degree;
m_controlPointsCount = knotCount + m_degree - 1;
for (int i = 0; i < m_controlPointsCount; i++) {
m_controlPoints[i] = controlPoints[i];
}
for (int i = 0; i < m_degree; i ++) {
m_knotVector[i] = dFloat64(0.0f);
m_knotVector[i + m_knotsCount - m_degree] = dFloat64(1.0f);
}
for (int i = 0; i < knotCount; i ++) {
m_knotVector[i + m_degree] = knotVector[i];
dAssert (m_knotVector[i + m_degree] >= m_knotVector[i + m_degree - 1]);
}
}
int dBezierSpline::GetKnotCount() const
{
return m_knotsCount;
}
dArray<dFloat64>& dBezierSpline::GetKnotArray()
{
return m_knotVector;
}
const dArray<dFloat64>& dBezierSpline::GetKnotArray() const
{
return m_knotVector;
}
dFloat64 dBezierSpline::GetKnot(int i) const
{
return m_knotVector[i];
}
int dBezierSpline::GetControlPointCount() const
{
return m_controlPointsCount;
}
dBigVector dBezierSpline::GetControlPoint(int i) const
{
return m_controlPoints[i];
}
void dBezierSpline::SetControlPoint(int i, const dBigVector& point)
{
m_controlPoints[i] = point;
}
dArray<dBigVector>& dBezierSpline::GetControlPointArray()
{
return m_controlPoints;
}
const dArray<dBigVector>& dBezierSpline::GetControlPointArray() const
{
return m_controlPoints;
}
int dBezierSpline::GetSpan(dFloat64 u) const
{
int low = m_degree;
int high = m_knotsCount - m_degree - 1;
dAssert (u >= 0.0f);
dAssert (u <= 1.0f);
while ((high - low) >= 4) {
int mid = (low + high) >> 1;
if (u > m_knotVector[mid]) {
low = mid;
} else {
high = mid;
}
}
dAssert (m_knotVector[low] <= u);
for (int i = low; i < m_degree + m_knotsCount + 1; i ++) {
if (m_knotVector[i + 1] >= u) {
return i;
}
}
dAssert (0);
return 0;
}
void dBezierSpline::BasicsFunctions (dFloat64 u, int span, dFloat64* const BasicFunctionsOut) const
{
BasicFunctionsOut[0] = 1.0f;
dFloat64* const left = dAlloca(dFloat64, m_knotsCount + 32);
dFloat64* const right = dAlloca(dFloat64, m_knotsCount + 32);
for (int j = 1; j <= m_degree; j ++) {
left[j] = u - m_knotVector[span + 1 - j];
right[j] = m_knotVector[span + j] - u;
dFloat64 saved = 0.0f;
for (int r = 0; r < j; r ++) {
dFloat64 temp = BasicFunctionsOut[r] / (right[r + 1] + left[j - r]);
BasicFunctionsOut[r] = saved + temp * right[r + 1];
saved = temp * left[j - r];
}
BasicFunctionsOut[j] = saved;
}
}
void dBezierSpline::BasicsFunctionsDerivatives (dFloat64 u, int span, dFloat64* const derivativesOut) const
{
dFloat64* const a = dAlloca(dFloat64, m_knotsCount + 32);
dFloat64* const ndu = dAlloca(dFloat64, m_knotsCount + 32);
dFloat64* const left = dAlloca(dFloat64, m_knotsCount + 32);
dFloat64* const right = dAlloca(dFloat64, m_knotsCount + 32);
const int width = m_degree + 1;
ndu[0] = 1.0f;
for (int j = 1; j <= m_degree; j ++) {
left[j] = u - m_knotVector[span + 1 - j];
right[j] = m_knotVector[span + j] - u;
dFloat64 saved = 0.0f;
for (int r = 0; r < j; r ++) {
ndu[j * width + r] = right[r + 1] + left[j - r];
dFloat64 temp = ndu[r * width + j - 1] / ndu[j * width + r];
ndu[r * width + j] = saved + temp * right[r + 1];
saved = temp * left[j - r];
}
ndu[j * width + j] = saved;
}
for (int j = 0; j <= m_degree; j ++) {
derivativesOut[width * 0 + j] = ndu [width * j + m_degree];
}
for (int r = 0; r <= m_degree; r ++) {
int s1 = 0;
int s2 = 1;
a[0] = 1.0f;
for (int k = 1; k <= m_degree; k ++) {
dFloat64 d = 0.0f;
int rk = r - k;
int pk = m_degree - k;
if (r >= k) {
a[width * s2 + 0] = a[width * s1 + 0] / ndu[width * (pk + 1) + rk];
d = a[width * s2 + 0] * ndu[width * rk + pk];
}
int j1 = 0;
int j2 = 0;
if (rk >= -1) {
j1 = 1;
} else {
j1 = -rk;
}
if ((r - 1) <= pk) {
j2 = k-1;
} else {
j2 = m_degree-r;
}
for (int j = j1; j <= j2; j ++) {
a[width * s2 + j] = (a[width * s1 + j] - a[width * s1 + j - 1]) / ndu[width * (pk + 1) + rk + j];
d += a[width * s2 + j] * ndu[width * (rk + j) + pk];
}
if (r <= pk) {
a[width * s2 + k] = -a[width * s1 + k - 1] / ndu[width * (pk + 1) + r];
d += a[width * s2 + k] * ndu[width * r + pk];
}
derivativesOut[width * k + r] = d;
dSwap(s1, s2);
}
}
int s = m_degree;
for (int k = 1; k <= m_degree; k ++) {
for (int j = 0; j <= m_degree; j ++) {
derivativesOut[width * k + j] *= s;
}
s *= (m_degree - k);
}
}
dBigVector dBezierSpline::CurvePoint (dFloat64 u, int span) const
{
dBigVector point (0.0f);
dFloat64* const basicFunctions = dAlloca(dFloat64, m_knotsCount + 32);
BasicsFunctions (u, span, basicFunctions);
for (int i = 0; i <= m_degree; i ++) {
point += m_controlPoints[span - m_degree + i].Scale (basicFunctions[i]);
}
return point;
}
dBigVector dBezierSpline::CurvePoint (dFloat64 u) const
{
u = dClamp (u, dFloat64 (0.0f), dFloat64 (1.0f));
int span = GetSpan(u);
return CurvePoint (u, span);
}
dBigVector dBezierSpline::CurveDerivative (dFloat64 u, int index) const
{
u = dClamp (u, dFloat64 (0.0f), dFloat64 (1.0f));
dAssert (index <= m_degree);
dFloat64* const basicsFuncDerivatives = dAlloca(dFloat64, m_knotsCount + 32);
int span = GetSpan(u);
BasicsFunctionsDerivatives (u, span, basicsFuncDerivatives);
const int with = m_degree + 1;
dBigVector point (0.0f);
for (int i = 0; i <= m_degree; i ++) {
point += m_controlPoints[span - m_degree + i].Scale (basicsFuncDerivatives[with * index + i]);
}
return point;
}
int dBezierSpline::CurveAllDerivatives (dFloat64 u, dBigVector* const derivatives) const
{
u = dMod (u, dFloat64(1.0f));
dFloat64* const basicsFuncDerivatives = dAlloca(dFloat64, m_knotsCount + 32);
int span = GetSpan(u);
BasicsFunctionsDerivatives (u, span, basicsFuncDerivatives);
const int with = m_degree + 1;
dBigVector point (0.0f);
for (int j = 0; j <= m_degree; j ++) {
dBigVector ck (0.0f);
for (int i = 0; i <= m_degree; i ++) {
ck += m_controlPoints[span - m_degree + i].Scale (basicsFuncDerivatives[with * j + i]);
}
derivatives[j] = ck;
}
return m_degree + 1;
}
void dBezierSpline::GlobalCubicInterpolation (int count, const dBigVector* const points, const dBigVector& firstTangent, const dBigVector& lastTangent)
{
CreateCubicKnotVector (count, points);
CreateCubicControlPoints (count, points, firstTangent, lastTangent);
}
void dBezierSpline::CreateCubicKnotVector(int count, const dBigVector* const points)
{
dFloat64 d = dFloat64(0.0f);
dAssert (count >= 2);
dFloat64* const u = dAlloca(dFloat64, m_knotsCount + 32);
u[0] = dFloat64(0.0f);
for (int i = 1; i < count; i ++) {
dBigVector step (points[i] - points[i - 1]);
dFloat64 len = dSqrt (step.DotProduct3(step));
u[i] = dSqrt (len);
d += u[i];
}
for (int i = 1; i < count; i ++) {
u[i] = u[i-1] + u[i] / d;
}
u[0] = dFloat64 (0.0f);
u[count - 1] = dFloat64(1.0f);
m_degree = 3;
m_knotsCount = count + 2 * m_degree;
for (int i = 0; i < (m_degree + 1); i ++) {
m_knotVector[i] = dFloat64(0.0f);
m_knotVector[i + m_knotsCount - m_degree - 1] = dFloat64(1.0f);
}
for (int i = 1; i < (count - 1); i ++) {
dFloat64 acc = dFloat64 (0.0f);
for (int j = 0; j < m_degree; j ++) {
acc += u[j + i - 1];
}
m_knotVector[m_degree + i] = acc / dFloat64 (3.0f);
}
}
void dBezierSpline::CreateCubicControlPoints(int count, const dBigVector* const points, const dBigVector& firstTangent, const dBigVector& lastTangent)
{
dFloat64 abc[4];
if ((m_knotsCount - 2 * (m_degree - 1)) != m_controlPointsCount) {
m_controlPointsCount = m_knotsCount - 2 * (m_degree - 1);
}
m_controlPoints[0] = points[0];
m_controlPoints[m_controlPointsCount - 1] = points[count - 1];
m_controlPoints[1] = m_controlPoints[0] + firstTangent.Scale (m_knotVector[m_degree + 1] / 3.0f);
m_controlPoints[m_controlPointsCount - 2] = m_controlPoints[m_controlPointsCount - 1] - lastTangent.Scale ((1.0f - m_knotVector[m_knotsCount - m_degree - 2]) / 3.0f);
if (count == 3) {
BasicsFunctions (m_knotVector[m_degree + 1], m_degree + 1, abc);
m_controlPoints[2] = points[1] - m_controlPoints[1].Scale (abc[0]) - m_controlPoints[3].Scale (abc[2]);
m_controlPoints[2] = m_controlPoints[2].Scale (1.0f / abc[1]);
} else {
dFloat64* const dd = dAlloca(dFloat64, m_knotsCount + 32);
BasicsFunctions (m_knotVector[m_degree + 1], m_degree + 1, abc);
dFloat64 den = abc[1];
m_controlPoints[2] = (points[1] - m_controlPoints[1].Scale (abc[0])).Scale (1.0f / den);
for (int i = 3; i < (count - 1); i ++) {
dd[i + 1] = abc[2] / den;
BasicsFunctions (m_knotVector[i + 2], i + 2, abc);
den = abc[1] - abc[0] * dd[i + 1];
m_controlPoints[i] = (points[i - 1] - m_controlPoints[i - 1].Scale (abc[0])).Scale (1.0f / den);
}
dd[count] = abc[2] / den;
BasicsFunctions (m_knotVector[count + 1], count + 1, abc);
den = abc[1] - abc[0] * dd[count];
m_controlPoints[count - 1] = (points[count - 2] - m_controlPoints[count].Scale (abc[2]) - m_controlPoints[count - 2].Scale (abc[0])).Scale (1.0f / den);
for (int i = count - 2; i >= 2; i --) {
m_controlPoints[i] -= m_controlPoints[i + 1].Scale (dd[i + 2]);
}
}
}
dFloat64 dBezierSpline::CalculateLength (dFloat64 tol) const
{
dBigVector stackPool[32][3];
int stack = 0;
dFloat64 length = 0.0f;
dFloat64 tol2 = tol * tol;
dFloat64 u0 = m_knotVector[m_degree];
dBigVector p0 (CurvePoint (u0));
for (int i = m_degree; i < (m_knotsCount - m_degree - 1); i ++) {
dFloat64 u1 = m_knotVector[i + 1];
dBigVector p1 (CurvePoint (u1));
stackPool[stack][0] = p0;
stackPool[stack][1] = p1;
stackPool[stack][2] = dBigVector (u0, u1, dFloat64(0.0f), dFloat64(0.0f));
stack ++;
while (stack) {
stack --;
dBigVector q0 (stackPool[stack][0]);
dBigVector q1 (stackPool[stack][1]);
dFloat64 t0 = stackPool[stack][2][0];
dFloat64 t1 = stackPool[stack][2][1];
dFloat64 t01 = (t1 + t0) * 0.5f;
dBigVector p01 ((q1 + q0).Scale (0.5f));
dBigVector q01 (CurvePoint (t01));
dBigVector err (q01 - p01);
dFloat64 err2 = err.DotProduct3(err);
if (err2 < tol2) {
dBigVector step (q1 - q0);
length += dSqrt (step.DotProduct3(step));
} else {
stackPool[stack][0] = q01;
stackPool[stack][1] = q1;
stackPool[stack][2] = dBigVector (t01, t1, dFloat64(0.0f), dFloat64(0.0f));
stack ++;
stackPool[stack][0] = q0;
stackPool[stack][1] = q01;
stackPool[stack][2] = dBigVector (t0, t01, dFloat64(0.0f), dFloat64(0.0f));
stack ++;
}
}
u0 = u1;
p0 = p1;
}
return length;
}
void dBezierSpline::InsertKnot (dFloat64 u)
{
const int k = GetSpan(u);
int multiplicity = 0;
for (int i = 0; i < m_degree; i ++) {
multiplicity += (dAbs (m_knotVector[k + i + 1] - u) < dFloat64 (1.0e-5f)) ? 1 : 0;
}
if (multiplicity == m_degree) {
return;
}
for (int i = m_knotsCount; i > (k + 1); i --) {
m_knotVector[i] = m_knotVector[i - 1];
}
m_knotVector[k + 1] = u;
dBigVector Rw[16];
for (int i = 0; i <= m_degree; i ++) {
Rw[i] = m_controlPoints[k - m_degree + i];
}
const int m = k - m_degree + 1;
dAssert(m >= 0);
dAssert((k + 1 - 1 - 0) >= 0);
dAssert((m_degree - 1 - 0) >= 0);
for (int i = 0; i <= (m_degree - 1); i ++) {
dFloat64 alpha = (u - m_knotVector[m + i]) / (m_knotVector[i + k + 1] - m_knotVector[m + i]);
Rw[i] = Rw[i + 1].Scale (alpha) + Rw[i].Scale (dFloat64 (1.0f) - alpha);
}
for (int i = m_controlPointsCount; i > k; i--) {
m_controlPoints[i] = m_controlPoints[i - 1];
}
m_controlPoints[m] = Rw[0];
m_controlPoints[k + 1 - 1 - 0] = Rw[m_degree - 1 - 0];
for (int i = m + 1; i < k; i++) {
dAssert((i - m) >= 0);
m_controlPoints[i] = Rw[i - m];
}
m_knotsCount ++;
m_controlPointsCount ++;
}
bool dBezierSpline::RemoveKnot (dFloat64 u, dFloat64 tol)
{
int r = GetSpan(u) + 1;
dAssert (m_knotVector[r - 1] < u);
if (dAbs (m_knotVector[r] - u) > 1.0e-5f) {
return false;
}
int s = 1;
int last = r - s;
int first = r - m_degree;
int ord = m_degree + 1;
dBigVector temp[16];
bool removableFlag = false;
int t = 0;
for ( ; t < m_degree; t ++) {
int off = first - 1;
temp[0] = m_controlPoints[off];
temp[last + 1 - off] = m_controlPoints[last + 1];
int i = first;
int j = last;
int ii = 1;
int jj = last - off;
while ((j - i) > t) {
dFloat64 alpha_i = (u - m_knotVector[i]) / (m_knotVector[i + ord + t] - m_knotVector[i]);
dFloat64 alpha_j = (u - m_knotVector[j - t]) / (m_knotVector[j + ord] - m_knotVector[j - t]);
temp[ii] = (m_controlPoints[i] - temp[ii - 1].Scale (dFloat64 (1.0f) - alpha_i)).Scale (dFloat64 (1.0f) / alpha_i);
temp[jj] = (m_controlPoints[j] - temp[jj + 1].Scale (alpha_j)).Scale (dFloat64 (1.0f) / (dFloat64 (1.0f) - alpha_j));
i ++;
j --;
ii ++;
jj --;
}
if ((j - i) < t) {
dBigVector diff (temp[ii - 1] - temp[jj + 1]);
removableFlag = diff.DotProduct3(diff) < (tol * tol);
} else {
dFloat64 alpha_i = (u - m_knotVector[i]) / (m_knotVector[i + ord + t] - m_knotVector[i]);
dBigVector p (temp[ii + t + 1].Scale (alpha_i) + temp[ii - 1].Scale (dFloat64 (1.0f) - alpha_i));
dBigVector diff (m_controlPoints[i] - p);
removableFlag = diff.DotProduct3(diff) < (tol * tol);
}
if (!removableFlag) {
break;
}
i = first;
j = last;
while ((j - 1) > t) {
m_controlPoints[i] = temp[i - off];
m_controlPoints[j] = temp[j - off];
i ++;
j --;
}
first --;
last ++;
}
if (t) {
for (int k = r + t; k < m_knotsCount; k ++) {
m_knotVector[k - t] = m_knotVector[k];
}
int fOut = (2 * r - s - m_degree) / 2;
int j = fOut;
int i = j;
for (int k = 1; k < t; k ++) {
if ((k % 2) == 1) {
i ++;
} else {
j = j - 1;
}
}
for (int k = i + 1; k < m_controlPointsCount; k ++) {
m_controlPoints[j] = m_controlPoints[k];
j ++;
}
m_knotsCount -= t;
m_controlPointsCount -= t;
}
return removableFlag;
}
dFloat64 dBezierSpline::FindClosestKnot(dBigVector& closestPoint, const dBigVector& point, int subdivitionSteps) const
{
int startSpan = m_degree;
dFloat64 bestU = 0.0f;
dFloat64 distance2 = 1.0e10f;
dBigVector closestControlPoint(m_controlPoints[0]);
subdivitionSteps = dMax(subdivitionSteps, 1);
dFloat64 scale = 1.0f / subdivitionSteps;
for (int span = m_degree; span < (m_knotsCount - m_degree - 1); span++) {
dFloat64 param = 0.0f;
for (int i = 0; i < subdivitionSteps; i++) {
dFloat64 u = m_knotVector[span] + (m_knotVector[span + 1] - m_knotVector[span]) * param;
param += scale;
dBigVector p(CurvePoint(u, span));
dBigVector dp(p - point);
dFloat64 dist2 = dp.DotProduct3(dp);
if (dist2 < distance2) {
bestU = u;
startSpan = span;
distance2 = dist2;
closestControlPoint = p;
}
}
}
dBigVector p(CurvePoint(0.999f));
dBigVector dp(p - point);
dFloat64 dist2 = dp.DotProduct3(dp);
if (dist2 < distance2) {
bestU = dFloat64(0.999f);
startSpan = m_knotsCount - m_degree - 2;
closestControlPoint = p;
}
dBigVector derivatives[32];
dFloat64 u0 = bestU;
bool stop = false;
for (int i = 0; (i < 20) && !stop; i++) {
CurveAllDerivatives(u0, derivatives);
dBigVector dist(closestControlPoint - point);
dFloat64 num = derivatives[1].DotProduct3(dist);
dFloat64 den = derivatives[2].DotProduct3(dist) + derivatives[1].DotProduct3(derivatives[1]);
dFloat64 u1 = dClamp(u0 - num / den, dFloat64(0.0), dFloat64(1.0));
if (u1 < m_knotVector[startSpan]) {
startSpan--;
dAssert(startSpan >= 0);
} else if (u1 >= m_knotVector[startSpan + 1]) {
startSpan++;
dAssert(startSpan < (m_knotsCount - m_degree));
}
dAssert(startSpan >= m_degree);
dAssert(startSpan <= (m_knotsCount - m_degree - 1));
closestControlPoint = CurvePoint(u1, startSpan);
stop |= (dAbs(u1 - u0) < 1.0e-10) || (num * num < ((dist.DotProduct3(dist)) * (derivatives[1].DotProduct3(derivatives[1])) * 1.0e-10));
u0 = u1;
}
closestPoint = closestControlPoint;
return u0;
}

79
thirdparty/src/newton/dContainers/dBezierSpline.h vendored Normal file
View File

@@ -0,0 +1,79 @@
/* Copyright (c) <2003-2019> <Newton Game Dynamics>
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely
*/
#include "dContainersStdAfx.h"
#include "dArray.h"
#include "dContainersAlloc.h"
#ifndef __D_BEZIER_SPLINE_H__
#define __D_BEZIER_SPLINE_H__
class dBezierSpline: public dContainersAlloc
{
public:
// empty spline
DCONTAINERS_API dBezierSpline ();
DCONTAINERS_API dBezierSpline (const dBezierSpline& src);
// create from knot vector and control points
DCONTAINERS_API virtual ~dBezierSpline ();
DCONTAINERS_API dBezierSpline& operator = (const dBezierSpline &copy) ;
DCONTAINERS_API int GetDegree () const;
DCONTAINERS_API dBigVector CurvePoint (dFloat64 u) const;
DCONTAINERS_API dBigVector CurveDerivative (dFloat64 u, int index = 1) const;
DCONTAINERS_API int CurveAllDerivatives (dFloat64 u, dBigVector* const defivatives) const;
DCONTAINERS_API dFloat64 CalculateLength (dFloat64 tol) const;
DCONTAINERS_API void GlobalCubicInterpolation (int count, const dBigVector* const points, const dBigVector& firstTangent, const dBigVector& lastTangent);
DCONTAINERS_API void CreateFromKnotVectorAndControlPoints (int degree, int knotCount, const dFloat64* const knotVector, const dBigVector* const controlPoints);
DCONTAINERS_API void InsertKnot (dFloat64 u);
DCONTAINERS_API bool RemoveKnot (dFloat64 u, dFloat64 tol);
DCONTAINERS_API int GetControlPointCount() const;
DCONTAINERS_API dArray<dBigVector>& GetControlPointArray();
DCONTAINERS_API const dArray<dBigVector>& GetControlPointArray() const;
DCONTAINERS_API dBigVector GetControlPoint(int i) const;
DCONTAINERS_API void SetControlPoint(int i, const dBigVector& point);
DCONTAINERS_API int GetKnotCount() const;
DCONTAINERS_API dArray<dFloat64>& GetKnotArray();
DCONTAINERS_API const dArray<dFloat64>& GetKnotArray() const;
DCONTAINERS_API dFloat64 GetKnot(int i) const;
DCONTAINERS_API dFloat64 FindClosestKnot (dBigVector& closestPointOnCurve, const dBigVector& point, int subdivitionSteps = 2) const;
private:
void Clear();
int GetSpan(dFloat64 u) const;
dBigVector CurvePoint (dFloat64 u, int span) const;
void CreateCubicKnotVector(int count, const dBigVector* const points);
void CreateCubicControlPoints(int count, const dBigVector* const points, const dBigVector& firstTangent, const dBigVector& lastTangent);
void BasicsFunctions (dFloat64 u, int span, dFloat64* const functionOut) const;
void BasicsFunctionsDerivatives (dFloat64 u, int span, dFloat64* const derivatyivesOut) const;
dArray<dFloat64> m_knotVector;
dArray<dBigVector> m_controlPoints;
int m_degree;
int m_knotsCount;
int m_controlPointsCount;
};
#endif

135
thirdparty/src/newton/dContainers/dCRC.cpp vendored Normal file
View File

@@ -0,0 +1,135 @@
/* Copyright (c) <2003-2019> <Newton Game Dynamics>
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely
*/
#include "dContainersStdAfx.h"
#include "dCRC.h"
static dCRCTYPE randBits0[] =
{
static_cast<long long>(7266447313870364031ULL), static_cast<long long>(4946485549665804864ULL), static_cast<long long>(16945909448695747420ULL), static_cast<long long>(16394063075524226720ULL),
static_cast<long long>(4873882236456199058ULL), static_cast<long long>(14877448043947020171ULL), static_cast<long long>(6740343660852211943ULL), static_cast<long long>(13857871200353263164ULL),
static_cast<long long>(5249110015610582907ULL), static_cast<long long>(10205081126064480383ULL), static_cast<long long>(1235879089597390050ULL), static_cast<long long>(17320312680810499042ULL),
static_cast<long long>(16489141110565194782ULL), static_cast<long long>(8942268601720066061ULL), static_cast<long long>(13520575722002588570ULL), static_cast<long long>(14226945236717732373ULL),
static_cast<long long>(9383926873555417063ULL), static_cast<long long>(15690281668532552105ULL), static_cast<long long>(11510704754157191257ULL), static_cast<long long>(15864264574919463609ULL),
static_cast<long long>(6489677788245343319ULL), static_cast<long long>(5112602299894754389ULL), static_cast<long long>(10828930062652518694ULL), static_cast<long long>(15942305434158995996ULL),
static_cast<long long>(15445717675088218264ULL), static_cast<long long>(4764500002345775851ULL), static_cast<long long>(14673753115101942098ULL), static_cast<long long>(236502320419669032ULL),
static_cast<long long>(13670483975188204088ULL), static_cast<long long>(14931360615268175698ULL), static_cast<long long>(8904234204977263924ULL), static_cast<long long>(12836915408046564963ULL),
static_cast<long long>(12120302420213647524ULL), static_cast<long long>(15755110976537356441ULL), static_cast<long long>(5405758943702519480ULL), static_cast<long long>(10951858968426898805ULL),
static_cast<long long>(17251681303478610375ULL), static_cast<long long>(4144140664012008120ULL), static_cast<long long>(18286145806977825275ULL), static_cast<long long>(13075804672185204371ULL),
static_cast<long long>(10831805955733617705ULL), static_cast<long long>(6172975950399619139ULL), static_cast<long long>(12837097014497293886ULL), static_cast<long long>(12903857913610213846ULL),
static_cast<long long>(560691676108914154ULL), static_cast<long long>(1074659097419704618ULL), static_cast<long long>(14266121283820281686ULL), static_cast<long long>(11696403736022963346ULL),
static_cast<long long>(13383246710985227247ULL), static_cast<long long>(7132746073714321322ULL), static_cast<long long>(10608108217231874211ULL), static_cast<long long>(9027884570906061560ULL),
static_cast<long long>(12893913769120703138ULL), static_cast<long long>(15675160838921962454ULL), static_cast<long long>(2511068401785704737ULL), static_cast<long long>(14483183001716371453ULL),
static_cast<long long>(3774730664208216065ULL), static_cast<long long>(5083371700846102796ULL), static_cast<long long>(9583498264570933637ULL), static_cast<long long>(17119870085051257224ULL),
static_cast<long long>(5217910858257235075ULL), static_cast<long long>(10612176809475689857ULL), static_cast<long long>(1924700483125896976ULL), static_cast<long long>(7171619684536160599ULL),
static_cast<long long>(10949279256701751503ULL), static_cast<long long>(15596196964072664893ULL), static_cast<long long>(14097948002655599357ULL), static_cast<long long>(615821766635933047ULL),
static_cast<long long>(5636498760852923045ULL), static_cast<long long>(17618792803942051220ULL), static_cast<long long>(580805356741162327ULL), static_cast<long long>(425267967796817241ULL),
static_cast<long long>(8381470634608387938ULL), static_cast<long long>(13212228678420887626ULL), static_cast<long long>(16993060308636741960ULL), static_cast<long long>(957923366004347591ULL),
static_cast<long long>(6210242862396777185ULL), static_cast<long long>(1012818702180800310ULL), static_cast<long long>(15299383925974515757ULL), static_cast<long long>(17501832009465945633ULL),
static_cast<long long>(17453794942891241229ULL), static_cast<long long>(15807805462076484491ULL), static_cast<long long>(8407189590930420827ULL), static_cast<long long>(974125122787311712ULL),
static_cast<long long>(1861591264068118966ULL), static_cast<long long>(997568339582634050ULL), static_cast<long long>(18046771844467391493ULL), static_cast<long long>(17981867688435687790ULL),
static_cast<long long>(3809841506498447207ULL), static_cast<long long>(9460108917638135678ULL), static_cast<long long>(16172980638639374310ULL), static_cast<long long>(958022432077424298ULL),
static_cast<long long>(4393365126459778813ULL), static_cast<long long>(13408683141069553686ULL), static_cast<long long>(13900005529547645957ULL), static_cast<long long>(15773550354402817866ULL),
static_cast<long long>(16475327524349230602ULL), static_cast<long long>(6260298154874769264ULL), static_cast<long long>(12224576659776460914ULL), static_cast<long long>(6405294864092763507ULL),
static_cast<long long>(7585484664713203306ULL), static_cast<long long>(5187641382818981381ULL), static_cast<long long>(12435998400285353380ULL), static_cast<long long>(13554353441017344755ULL),
static_cast<long long>(646091557254529188ULL), static_cast<long long>(11393747116974949255ULL), static_cast<long long>(16797249248413342857ULL), static_cast<long long>(15713519023537495495ULL),
static_cast<long long>(12823504709579858843ULL), static_cast<long long>(4738086532119935073ULL), static_cast<long long>(4429068783387643752ULL), static_cast<long long>(585582692562183870ULL),
static_cast<long long>(1048280754023674130ULL), static_cast<long long>(6788940719869959076ULL), static_cast<long long>(11670856244972073775ULL), static_cast<long long>(2488756775360218862ULL),
static_cast<long long>(2061695363573180185ULL), static_cast<long long>(6884655301895085032ULL), static_cast<long long>(3566345954323888697ULL), static_cast<long long>(12784319933059041817ULL),
static_cast<long long>(4772468691551857254ULL), static_cast<long long>(6864898938209826895ULL), static_cast<long long>(7198730565322227090ULL), static_cast<long long>(2452224231472687253ULL),
static_cast<long long>(13424792606032445807ULL), static_cast<long long>(10827695224855383989ULL), static_cast<long long>(11016608897122070904ULL), static_cast<long long>(14683280565151378358ULL),
static_cast<long long>(7077866519618824360ULL), static_cast<long long>(17487079941198422333ULL), static_cast<long long>(3956319990205097495ULL), static_cast<long long>(5804870313319323478ULL),
static_cast<long long>(8017203611194497730ULL), static_cast<long long>(3310931575584983808ULL), static_cast<long long>(5009341981771541845ULL), static_cast<long long>(11772020174577005930ULL),
static_cast<long long>(3537640779967351792ULL), static_cast<long long>(6801855569284252424ULL), static_cast<long long>(17687268231192623388ULL), static_cast<long long>(12968358613633237218ULL),
static_cast<long long>(1429775571144180123ULL), static_cast<long long>(10427377732172208413ULL), static_cast<long long>(12155566091986788996ULL), static_cast<long long>(16465954421598296115ULL),
static_cast<long long>(12710429690464359999ULL), static_cast<long long>(9547226351541565595ULL), static_cast<long long>(12156624891403410342ULL), static_cast<long long>(2985938688676214686ULL),
static_cast<long long>(18066917785985010959ULL), static_cast<long long>(5975570403614438776ULL), static_cast<long long>(11541343163022500560ULL), static_cast<long long>(11115388652389704592ULL),
static_cast<long long>(9499328389494710074ULL), static_cast<long long>(9247163036769651820ULL), static_cast<long long>(3688303938005101774ULL), static_cast<long long>(2210483654336887556ULL),
static_cast<long long>(15458161910089693228ULL), static_cast<long long>(6558785204455557683ULL), static_cast<long long>(1288373156735958118ULL), static_cast<long long>(18433986059948829624ULL),
static_cast<long long>(3435082195390932486ULL), static_cast<long long>(16822351800343061990ULL), static_cast<long long>(3120532877336962310ULL), static_cast<long long>(16681785111062885568ULL),
static_cast<long long>(7835551710041302304ULL), static_cast<long long>(2612798015018627203ULL), static_cast<long long>(15083279177152657491ULL), static_cast<long long>(6591467229462292195ULL),
static_cast<long long>(10592706450534565444ULL), static_cast<long long>(7438147750787157163ULL), static_cast<long long>(323186165595851698ULL), static_cast<long long>(7444710627467609883ULL),
static_cast<long long>(8473714411329896576ULL), static_cast<long long>(2782675857700189492ULL), static_cast<long long>(3383567662400128329ULL), static_cast<long long>(3200233909833521327ULL),
static_cast<long long>(12897601280285604448ULL), static_cast<long long>(3612068790453735040ULL), static_cast<long long>(8324209243736219497ULL), static_cast<long long>(15789570356497723463ULL),
static_cast<long long>(1083312926512215996ULL), static_cast<long long>(4797349136059339390ULL), static_cast<long long>(5556729349871544986ULL), static_cast<long long>(18266943104929747076ULL),
static_cast<long long>(1620389818516182276ULL), static_cast<long long>(172225355691600141ULL), static_cast<long long>(3034352936522087096ULL), static_cast<long long>(1266779576738385285ULL),
static_cast<long long>(3906668377244742888ULL), static_cast<long long>(6961783143042492788ULL), static_cast<long long>(17159706887321247572ULL), static_cast<long long>(4676208075243319061ULL),
static_cast<long long>(10315634697142985816ULL), static_cast<long long>(13435140047933251189ULL), static_cast<long long>(716076639492622016ULL), static_cast<long long>(13847954035438697558ULL),
static_cast<long long>(7195811275139178570ULL), static_cast<long long>(10815312636510328870ULL), static_cast<long long>(6214164734784158515ULL), static_cast<long long>(16412194511839921544ULL),
static_cast<long long>(3862249798930641332ULL), static_cast<long long>(1005482699535576005ULL), static_cast<long long>(4644542796609371301ULL), static_cast<long long>(17600091057367987283ULL),
static_cast<long long>(4209958422564632034ULL), static_cast<long long>(5419285945389823940ULL), static_cast<long long>(11453701547564354601ULL), static_cast<long long>(9951588026679380114ULL),
static_cast<long long>(7425168333159839689ULL), static_cast<long long>(8436306210125134906ULL), static_cast<long long>(11216615872596820107ULL), static_cast<long long>(3681345096403933680ULL),
static_cast<long long>(5770016989916553752ULL), static_cast<long long>(11102855936150871733ULL), static_cast<long long>(11187980892339693935ULL), static_cast<long long>(396336430216428875ULL),
static_cast<long long>(6384853777489155236ULL), static_cast<long long>(7551613839184151117ULL), static_cast<long long>(16527062023276943109ULL), static_cast<long long>(13429850429024956898ULL),
static_cast<long long>(9901753960477271766ULL), static_cast<long long>(9731501992702612259ULL), static_cast<long long>(5217575797614661659ULL), static_cast<long long>(10311708346636548706ULL),
static_cast<long long>(15111747519735330483ULL), static_cast<long long>(4353415295139137513ULL), static_cast<long long>(1845293119018433391ULL), static_cast<long long>(11952006873430493561ULL),
static_cast<long long>(3531972641585683893ULL), static_cast<long long>(16852246477648409827ULL), static_cast<long long>(15956854822143321380ULL), static_cast<long long>(12314609993579474774ULL),
static_cast<long long>(16763911684844598963ULL), static_cast<long long>(16392145690385382634ULL), static_cast<long long>(1545507136970403756ULL), static_cast<long long>(17771199061862790062ULL),
static_cast<long long>(12121348462972638971ULL), static_cast<long long>(12613068545148305776ULL), static_cast<long long>(954203144844315208ULL), static_cast<long long>(1257976447679270605ULL),
static_cast<long long>(3664184785462160180ULL), static_cast<long long>(2747964788443845091ULL), static_cast<long long>(15895917007470512307ULL), static_cast<long long>(15552935765724302120ULL),
static_cast<long long>(16366915862261682626ULL), static_cast<long long>(8385468783684865323ULL), static_cast<long long>(10745343827145102946ULL), static_cast<long long>(2485742734157099909ULL),
static_cast<long long>(916246281077683950ULL), static_cast<long long>(15214206653637466707ULL), static_cast<long long>(12895483149474345798ULL), static_cast<long long>(1079510114301747843ULL),
static_cast<long long>(10718876134480663664ULL), static_cast<long long>(1259990987526807294ULL), static_cast<long long>(8326303777037206221ULL), static_cast<long long>(14104661172014248293ULL),
};
dCRCTYPE dCombineCRC (dCRCTYPE a, dCRCTYPE b)
{
return (a << 8) ^ b;
}
// calculate a 32 bit crc of a string
dCRCTYPE dCRC64 (const char* const name, dCRCTYPE crcAcc)
{
if (name) {
const int bitshift = (sizeof (dCRCTYPE)<<3) - 8;
for (int i = 0; name[i]; i ++) {
char c = name[i];
dCRCTYPE val = randBits0[((crcAcc >> bitshift) ^ c) & 0xff];
crcAcc = (crcAcc << 8) ^ val;
}
}
return crcAcc;
}
dCRCTYPE dCRC64 (const void* const buffer, int size, dCRCTYPE crcAcc)
{
const unsigned char* const ptr = (unsigned char*)buffer;
const int bitshift = (sizeof (dCRCTYPE)<<3) - 8;
for (int i = 0; i < size; i ++) {
char c = ptr[i];
dCRCTYPE val = randBits0[((crcAcc >> bitshift) ^ c) & 0xff];
crcAcc = (crcAcc << 8) ^ val;
}
return crcAcc;
}

25
thirdparty/src/newton/dContainers/dCRC.h vendored Normal file
View File

@@ -0,0 +1,25 @@
/* Copyright (c) <2003-2019> <Newton Game Dynamics>
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely
*/
#ifndef __dCRC__
#define __dCRC__
#include "dContainersAlloc.h"
#define dCRCTYPE long long
DCONTAINERS_API dCRCTYPE dCRC64 (const char* const string, dCRCTYPE crcAcc = 0);
DCONTAINERS_API dCRCTYPE dCRC64 (const void* const buffer, int size, dCRCTYPE crcAcc);
DCONTAINERS_API dCRCTYPE dCombineCRC (dCRCTYPE a, dCRCTYPE b);
#endif

18
thirdparty/src/newton/dContainers/dClassInfo.cpp vendored Normal file
View File

@@ -0,0 +1,18 @@
/* Copyright (c) <2003-2019> <Newton Game Dynamics>
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely
*/
#include "dContainersStdAfx.h"
#include "dClassInfo.h"
//dRtti dClassInfo::m_rtti ("dClassInfo");
dRttiRootClassSupportImplement(dClassInfo);

32
thirdparty/src/newton/dContainers/dClassInfo.h vendored Normal file
View File

@@ -0,0 +1,32 @@
/* Copyright (c) <2003-2019> <Newton Game Dynamics>
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely
*/
#ifndef __DCLASS_INFO_H__
#define __DCLASS_INFO_H__
#include "dRtti.h"
#include "dRefCounter.h"
class dClassInfo: public dRefCounter
{
public:
dClassInfo(void)
{
}
virtual ~dClassInfo()
{
}
dRttiRootClassSupportDeclare(dClassInfo,DCONTAINERS_API);
};
#endif

215
thirdparty/src/newton/dContainers/dContainersAlloc.cpp vendored Normal file
View File

@@ -0,0 +1,215 @@
/* Copyright (c) <2003-2019> <Julio Jerez, Newton Game Dynamics>
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
*
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
*
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
*
* 3. This notice may not be removed or altered from any source distribution.
*/
#include "dContainersStdAfx.h"
#include "dContainersAlloc.h"
#pragma warning (disable: 4100) //warning C4100: 'lpReserved' : unreferenced formal parameter
#ifdef _DCONTAINERS_DLL
void* operator new (size_t size)
{
void* const ptr = malloc (size);
dAssert (ptr);
return ptr;
}
void operator delete (void* ptr)
{
free (ptr);
}
BOOL APIENTRY DllMain( HMODULE hModule, DWORD ul_reason_for_call, LPVOID lpReserved)
{
switch (ul_reason_for_call)
{
case DLL_PROCESS_ATTACH:
case DLL_THREAD_ATTACH:
// check for memory leaks
#if defined(_DEBUG) && defined(_MSC_VER)
// Track all memory leaks at the operating system level.
// make sure no Newton tool or utility leaves leaks behind.
_CrtSetDbgFlag(_CRTDBG_LEAK_CHECK_DF|_CrtSetDbgFlag(_CRTDBG_LEAK_CHECK_DF));
#endif
case DLL_THREAD_DETACH:
case DLL_PROCESS_DETACH:
break;
}
return TRUE;
}
#endif
void* dContainersAlloc::operator new (size_t size)
{
//return ::new char[size];
return Alloc (size);
}
void dContainersAlloc::operator delete (void* ptr)
{
// delete[] (char*) ptr;
Free(ptr);
}
void* dContainersAlloc::Alloc (size_t size)
{
char* const ptr = ::new char[size];
return ptr;
}
void dContainersAlloc::Free(void* const ptr)
{
delete[] (char*) ptr;
}
dContainerFixSizeAllocator::dContainerFixSizeAllocator(int size, int poolSize)
:m_freeListNode(NULL)
,m_size(size)
,m_poolSize(poolSize)
{
Prefetch ();
}
dContainerFixSizeAllocator::~dContainerFixSizeAllocator()
{
Flush();
}
dContainerFixSizeAllocator* dContainerFixSizeAllocator::Create (int size, int poolSize)
{
class AllocatorsFactory
{
public:
AllocatorsFactory()
:m_count(0)
,m_maxSize(0)
,m_pool (NULL)
{
m_maxSize = 1;
m_pool = new dContainerFixSizeAllocator*[1];
}
~AllocatorsFactory()
{
for (int i = 0; i < m_count; i ++) {
delete m_pool[i];
}
delete[] m_pool;
}
dContainerFixSizeAllocator* FindCreate (int size, int poolSize)
{
int i0 = 0;
int i2 = m_count -1;
while ((i2 - i0) > 4) {
int i1 = (i0 + i2) >> 1;
if (size < m_pool[i1]->m_size) {
i2 = i1;
} else {
i0 = i1;
}
}
for (int i = i0; (i < m_count) && (m_pool[i]->m_size <= size); i ++) {
if (m_pool[i]->m_size == size) {
return m_pool[i];
}
}
if (m_count == m_maxSize) {
m_maxSize *= 2;
dContainerFixSizeAllocator** pool = new dContainerFixSizeAllocator*[m_maxSize];
memcpy (pool, m_pool, m_count * sizeof (dContainerFixSizeAllocator*));
delete[] m_pool;
m_pool = pool;
}
dContainerFixSizeAllocator* const allocator = new dContainerFixSizeAllocator(size, poolSize);
int entry = m_count;
for (; entry && (m_pool[entry - 1]->m_size > size); entry --) {
m_pool[entry] = m_pool[entry - 1];
}
m_pool[entry] = allocator;
m_count ++;
return allocator;
}
int m_count;
int m_maxSize;
dContainerFixSizeAllocator** m_pool;
};
static AllocatorsFactory factories;
return factories.FindCreate(size, poolSize);
}
void dContainerFixSizeAllocator::Prefetch ()
{
for (int i = 0; i < m_poolSize; i ++) {
dFreeListNode* const data = (dFreeListNode*) dContainersAlloc::Alloc (m_size);
data->m_count = i + 1;
data->m_next = m_freeListNode;
m_freeListNode = data;
}
}
void dContainerFixSizeAllocator::Flush ()
{
for (int i = 0; m_freeListNode && (i < m_poolSize); i ++) {
dFreeListNode* const ptr = m_freeListNode;
m_freeListNode = m_freeListNode->m_next;
dContainersAlloc::Free (ptr);
}
}
void* dContainerFixSizeAllocator::Alloc()
{
if (!m_freeListNode) {
Prefetch ();
}
dFreeListNode* const data = m_freeListNode;
m_freeListNode = m_freeListNode->m_next;
return data;
}
void dContainerFixSizeAllocator::Free(void* const ptr)
{
dFreeListNode* const data = (dFreeListNode*) ptr;
data->m_count = m_freeListNode ? m_freeListNode->m_count + 1 : 1;
data->m_next = m_freeListNode;
m_freeListNode = data;
if (data->m_count >= 2 * m_poolSize) {
Flush();
}
}

75
thirdparty/src/newton/dContainers/dContainersAlloc.h vendored Normal file
View File

@@ -0,0 +1,75 @@
/* Copyright (c) <2003-2019> <Julio Jerez, Newton Game Dynamics>
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
*
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
*
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
*
* 3. This notice may not be removed or altered from any source distribution.
*/
#ifndef _D_CONTAINERS_ALLOC_H_
#define _D_CONTAINERS_ALLOC_H_
#include "dContainersStdAfx.h"
#define D_MAX_ENTRIES_IN_FREELIST 32
class dContainersAlloc
{
public:
DCONTAINERS_API void *operator new (size_t size);
DCONTAINERS_API void operator delete (void* ptr);
dContainersAlloc()
{
}
virtual ~dContainersAlloc()
{
}
static DCONTAINERS_API void* Alloc (size_t size);
static DCONTAINERS_API void Free (void* const ptr);
};
class dContainerFixSizeAllocator
{
public:
static DCONTAINERS_API dContainerFixSizeAllocator* Create (int size, int poolSize);
DCONTAINERS_API ~dContainerFixSizeAllocator();
DCONTAINERS_API void* Alloc();
DCONTAINERS_API void Free(void* const ptr);
// DCONTAINERS_API bool IsAlive() const;
DCONTAINERS_API void Flush ();
private:
DCONTAINERS_API dContainerFixSizeAllocator(int size, int poolSize);
class dFreeListNode
{
public:
int m_count;
dFreeListNode* m_next;
};
DCONTAINERS_API void Prefetch ();
dFreeListNode* m_freeListNode;
int m_size;
int m_poolSize;
};
#endif

21
thirdparty/src/newton/dContainers/dContainersStdAfx.cpp vendored Normal file
View File

@@ -0,0 +1,21 @@
/* Copyright (c) <2003-2019> <Newton Game Dynamics>
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely
*/
// stdafx.cpp : source file that includes just the standard includes
// containers.pch will be the pre-compiled header
// stdafx.obj will contain the pre-compiled type information
#include "dContainersStdAfx.h"
// TODO: reference any additional headers you need in STDAFX.H
// and not in this file

55
thirdparty/src/newton/dContainers/dContainersStdAfx.h vendored Normal file
View File

@@ -0,0 +1,55 @@
/* Copyright (c) <2003-2019> <Newton Game Dynamics>
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely
*/
// stdafx.h : include file for standard system include files,
// or project specific include files that are used frequently, but
// are changed infrequently
//
#ifndef __D_CONTAINERS_STDAFX__
#define __D_CONTAINERS_STDAFX__
#include <new>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <assert.h>
#include <dMathDefines.h>
#include <dVector.h>
#include <dMatrix.h>
#include <dgTypes.h>
#ifdef _DCONTAINERS_DLL
#ifdef _DCONTAINERS_EXPORT
#define DCONTAINERS_API DG_LIBRARY_EXPORT
#else
#define DCONTAINERS_API DG_LIBRARY_IMPORT
#endif
#else
#define DCONTAINERS_API DG_LIBRARY_STATIC
#endif
#ifdef _WIN32
#include <windows.h>
#include <crtdbg.h>
#endif
#ifdef _MACOSX_VER
#include <unistd.h>
#include <libkern/OSAtomic.h>
#include <sys/sysctl.h>
#endif
#include <dMathDefines.h>
#endif

421
thirdparty/src/newton/dContainers/dHeap.h vendored Normal file
View File

@@ -0,0 +1,421 @@
/* Copyright (c) <2003-2019> <Julio Jerez, Newton Game Dynamics>
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
*
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
*
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
*
* 3. This notice may not be removed or altered from any source distribution.
*/
#ifndef __dHeapBase__
#define __dHeapBase__
#include "dContainersStdAfx.h"
#include "dContainersAlloc.h"
//#define D_CHECK_HEAP
template <class OBJECT, class KEY>
class dHeapBase: public dContainersAlloc
{
protected:
struct dHeapRecord
{
dHeapRecord ()
:m_key(), m_obj()
{
}
dHeapRecord (KEY key, const OBJECT& obj)
:m_key(key), m_obj(obj)
{
}
~dHeapRecord ()
{
}
KEY m_key;
OBJECT m_obj;
};
dHeapBase (int maxElements);
virtual ~dHeapBase ();
public:
void Flush ();
KEY MaxValue() const;
KEY Value(int i = 0) const;
int GetCount() const;
int GetMaxCount() const;
const OBJECT& operator[] (int i) const;
int Find (OBJECT &obj);
int Find (KEY key);
int m_curCount;
int m_maxCount;
bool m_allocated;
dHeapRecord* m_pool;
};
template <class OBJECT, class KEY>
class dDownHeap: public dHeapBase<OBJECT, KEY>
{
public:
dDownHeap (int maxElements);
void Pop () {Remove (0);}
void Push (OBJECT &obj, KEY key);
void Sort ();
void Remove (int Index);
bool SanityCheck();
};
template <class OBJECT, class KEY>
class dUpHeap: public dHeapBase<OBJECT, KEY>
{
public:
dUpHeap (int maxElements);
void Pop () {Remove (0);}
void Push (OBJECT &obj, KEY key);
void Sort ();
void Remove (int Index);
bool SanityCheck();
};
template <class OBJECT, class KEY>
dHeapBase<OBJECT,KEY>::dHeapBase (int maxElements)
:m_curCount(0)
,m_maxCount(maxElements)
,m_allocated(true)
,m_pool(new dHeapRecord[maxElements])
{
Flush();
}
template <class OBJECT, class KEY>
dHeapBase<OBJECT,KEY>::~dHeapBase ()
{
if (m_allocated == true) {
delete[] m_pool;
}
}
template <class OBJECT, class KEY>
KEY dHeapBase<OBJECT,KEY>::Value(int i) const
{
return m_pool[i].m_key;
}
template <class OBJECT, class KEY>
int dHeapBase<OBJECT,KEY>::GetCount() const
{
return m_curCount;
}
template <class OBJECT, class KEY>
void dHeapBase<OBJECT,KEY>::Flush ()
{
m_curCount = 0;
#ifdef _DEBUG
// dHeapBase<OBJECT,KEY>::m_pool[dHeapBase<OBJECT,KEY>::m_curCount].m_key = KEY (0);
#endif
}
template <class OBJECT, class KEY>
KEY dHeapBase<OBJECT,KEY>::MaxValue() const
{
return m_pool[0].m_key;
}
template <class OBJECT, class KEY>
int dHeapBase<OBJECT,KEY>::GetMaxCount() const
{
return m_maxCount;
}
template <class OBJECT, class KEY>
int dHeapBase<OBJECT,KEY>::Find (OBJECT &obj)
{
// For now let perform a linear search
// this is efficient if the size of the heap is small
// ex: m_curCount < 32
// this will be change to a binary search in the heap should the
// the size of the heap get larger than 32
// dAssert (m_curCount <= 32);
for (int i = 0; i < m_curCount; i ++) {
if (m_pool[i].obj == obj) {
return i;
}
}
return - 1;
}
template <class OBJECT, class KEY>
int dHeapBase<OBJECT,KEY>::Find (KEY key)
{
// ex: m_curCount < 32
// this will be change to a binary search in the heap shoud the
// the size of the heap get larger than 32
dAssert (m_curCount <= 32);
for (int i = 0; i < m_curCount; i ++) {
if (m_pool[i].m_key == key) {
return i;
}
}
return - 1;
}
template <class OBJECT, class KEY>
const OBJECT& dHeapBase<OBJECT,KEY>::operator[] (int i) const
{
dAssert (i<= m_curCount);
return m_pool[i].m_obj;
}
// **************************************************************************
//
// down Heap
//
// **************************************************************************
template <class OBJECT, class KEY>
dDownHeap<OBJECT,KEY>::dDownHeap (int maxElements)
:dHeapBase<OBJECT, KEY> (maxElements)
{
}
template <class OBJECT, class KEY>
void dDownHeap<OBJECT,KEY>::Push (OBJECT &obj, KEY key)
{
dAssert ((dHeapBase<OBJECT,KEY>::m_curCount < dHeapBase<OBJECT,KEY>::m_maxCount));
dHeapBase<OBJECT,KEY>::m_curCount ++;
int j;
int i = dHeapBase<OBJECT,KEY>::m_curCount;
for (; i; i = j) {
j = i >> 1;
if (!j || (dHeapBase<OBJECT,KEY>::m_pool[j - 1].m_key > key)) {
break;
}
dHeapBase<OBJECT,KEY>::m_pool[i - 1] = dHeapBase<OBJECT,KEY>::m_pool[j - 1];
}
dAssert (i);
dHeapBase<OBJECT,KEY>::m_pool[i - 1].m_key = key;
dHeapBase<OBJECT,KEY>::m_pool[i - 1].m_obj = obj;
dAssert (SanityCheck());
}
template <class OBJECT, class KEY>
void dDownHeap<OBJECT,KEY>::Remove (int index)
{
dHeapBase<OBJECT, KEY>::m_curCount--;
dHeapBase<OBJECT, KEY>::m_pool[index] = dHeapBase<OBJECT, KEY>::m_pool[dHeapBase<OBJECT, KEY>::m_curCount];
while (index && dHeapBase<OBJECT, KEY>::m_pool[(index - 1) >> 1].m_key < dHeapBase<OBJECT, KEY>::m_pool[index].m_key) {
dSwap(dHeapBase<OBJECT, KEY>::m_pool[(index - 1) >> 1], dHeapBase<OBJECT, KEY>::m_pool[index]);
index = (index - 1) >> 1;
}
while ((2 * index + 1) < dHeapBase<OBJECT, KEY>::m_curCount) {
int i0 = 2 * index + 1;
int i1 = 2 * index + 2;
if (i1 < dHeapBase<OBJECT, KEY>::m_curCount) {
i0 = (dHeapBase<OBJECT, KEY>::m_pool[i0].m_key > dHeapBase<OBJECT, KEY>::m_pool[i1].m_key) ? i0 : i1;
if (dHeapBase<OBJECT, KEY>::m_pool[i0].m_key <= dHeapBase<OBJECT, KEY>::m_pool[index].m_key) {
break;
}
dSwap(dHeapBase<OBJECT, KEY>::m_pool[i0], dHeapBase<OBJECT, KEY>::m_pool[index]);
index = i0;
} else {
if (dHeapBase<OBJECT, KEY>::m_pool[i0].m_key > dHeapBase<OBJECT, KEY>::m_pool[index].m_key) {
dSwap(dHeapBase<OBJECT, KEY>::m_pool[i0], dHeapBase<OBJECT, KEY>::m_pool[index]);
}
index = i0;
}
}
dAssert(SanityCheck());
}
template <class OBJECT, class KEY>
void dDownHeap<OBJECT,KEY>::Sort ()
{
int count = dHeapBase<OBJECT,KEY>::m_curCount;
for (int i = 1; i < count; i ++) {
KEY key (dHeapBase<OBJECT,KEY>::m_pool[0].m_key);
OBJECT obj (dHeapBase<OBJECT,KEY>::m_pool[0].m_obj);
Pop();
dHeapBase<OBJECT,KEY>::m_pool[dHeapBase<OBJECT,KEY>::m_curCount].m_key = key;
dHeapBase<OBJECT,KEY>::m_pool[dHeapBase<OBJECT,KEY>::m_curCount].m_obj = obj;
}
dHeapBase<OBJECT,KEY>::m_curCount = count;
for (int i = 0; i < count / 2; i ++) {
KEY key (dHeapBase<OBJECT,KEY>::m_pool[i].m_key);
OBJECT obj (dHeapBase<OBJECT,KEY>::m_pool[i].m_obj);
dHeapBase<OBJECT,KEY>::m_pool[i].m_key = dHeapBase<OBJECT,KEY>::m_pool[count - i - 1].m_key;
dHeapBase<OBJECT,KEY>::m_pool[i].m_obj = dHeapBase<OBJECT,KEY>::m_pool[count - i - 1].m_obj;
dHeapBase<OBJECT,KEY>::m_pool[count - i - 1].m_key = key;
dHeapBase<OBJECT,KEY>::m_pool[count - i - 1].m_obj = obj;
}
dAssert (SanityCheck());
}
template <class OBJECT, class KEY>
bool dDownHeap<OBJECT, KEY>::SanityCheck()
{
#ifdef D_CHECK_HEAP
for (int i = 0; i < this->m_curCount; i++) {
int i1 = 2 * i + 1;
int i2 = 2 * i + 2;
if ((i1 < this->m_curCount) && (dHeapBase<OBJECT, KEY>::m_pool[i].m_key < dHeapBase<OBJECT, KEY>::m_pool[i1].m_key)) {
return false;
}
if ((i2 < this->m_curCount) && (dHeapBase<OBJECT, KEY>::m_pool[i].m_key < dHeapBase<OBJECT, KEY>::m_pool[i2].m_key)) {
return false;
}
}
#endif
return true;
}
// **************************************************************************
//
// Up Heap
//
// **************************************************************************
template <class OBJECT, class KEY>
dUpHeap<OBJECT,KEY>::dUpHeap (int maxElements)
:dHeapBase<OBJECT, KEY> (maxElements)
{
}
template <class OBJECT, class KEY>
void dUpHeap<OBJECT,KEY>::Push (OBJECT &obj, KEY key)
{
dAssert ((dHeapBase<OBJECT,KEY>::m_curCount < dHeapBase<OBJECT,KEY>::m_maxCount));
dHeapBase<OBJECT,KEY>::m_curCount ++;
int j;
int i = dHeapBase<OBJECT,KEY>::m_curCount;
for (; i; i = j) {
j = i >> 1;
if (!j || (dHeapBase<OBJECT,KEY>::m_pool[j - 1].m_key < key)) {
break;
}
dHeapBase<OBJECT,KEY>::m_pool[i - 1] = dHeapBase<OBJECT,KEY>::m_pool[j - 1];
}
dAssert (i);
dHeapBase<OBJECT,KEY>::m_pool[i - 1].m_key = key;
dHeapBase<OBJECT,KEY>::m_pool[i - 1].m_obj = obj;
dAssert (SanityCheck());
}
template <class OBJECT, class KEY>
void dUpHeap<OBJECT,KEY>::Sort ()
{
int count = dHeapBase<OBJECT,KEY>::m_curCount;
for (int i = 1; i < count; i ++) {
KEY key (dHeapBase<OBJECT,KEY>::m_pool[0].m_key);
OBJECT obj (dHeapBase<OBJECT,KEY>::m_pool[0].m_obj);
Pop();
dHeapBase<OBJECT,KEY>::m_pool[dHeapBase<OBJECT,KEY>::m_curCount].m_key = key;
dHeapBase<OBJECT,KEY>::m_pool[dHeapBase<OBJECT,KEY>::m_curCount].m_obj = obj;
}
dHeapBase<OBJECT,KEY>::m_curCount = count;
for (int i = 0; i < count / 2; i ++) {
KEY key (dHeapBase<OBJECT,KEY>::m_pool[i].m_key);
OBJECT obj (dHeapBase<OBJECT,KEY>::m_pool[i].m_obj);
dHeapBase<OBJECT,KEY>::m_pool[i].m_key = dHeapBase<OBJECT,KEY>::m_pool[count - i - 1].m_key;
dHeapBase<OBJECT,KEY>::m_pool[i].m_obj = dHeapBase<OBJECT,KEY>::m_pool[count - i - 1].m_obj;
dHeapBase<OBJECT,KEY>::m_pool[count - i - 1].m_key = key;
dHeapBase<OBJECT,KEY>::m_pool[count - i - 1].m_obj = obj;
}
dAssert (SanityCheck());
}
template <class OBJECT, class KEY>
void dUpHeap<OBJECT,KEY>::Remove (int index)
{
dHeapBase<OBJECT, KEY>::m_curCount--;
dHeapBase<OBJECT, KEY>::m_pool[index] = dHeapBase<OBJECT, KEY>::m_pool[dHeapBase<OBJECT, KEY>::m_curCount];
while (index && dHeapBase<OBJECT, KEY>::m_pool[(index - 1) >> 1].m_key > dHeapBase<OBJECT, KEY>::m_pool[index].m_key) {
dSwap(dHeapBase<OBJECT, KEY>::m_pool[(index - 1) >> 1], dHeapBase<OBJECT, KEY>::m_pool[index]);
index = (index - 1) >> 1;
}
while ((2 * index + 1) < dHeapBase<OBJECT, KEY>::m_curCount) {
int i0 = 2 * index + 1;
int i1 = 2 * index + 2;
if (i1 < dHeapBase<OBJECT, KEY>::m_curCount) {
i0 = (dHeapBase<OBJECT, KEY>::m_pool[i0].m_key < dHeapBase<OBJECT, KEY>::m_pool[i1].m_key) ? i0 : i1;
if (dHeapBase<OBJECT, KEY>::m_pool[i0].m_key >= dHeapBase<OBJECT, KEY>::m_pool[index].m_key) {
break;
}
dSwap(dHeapBase<OBJECT, KEY>::m_pool[i0], dHeapBase<OBJECT, KEY>::m_pool[index]);
index = i0;
} else {
if (dHeapBase<OBJECT, KEY>::m_pool[i0].m_key < dHeapBase<OBJECT, KEY>::m_pool[index].m_key) {
dSwap(dHeapBase<OBJECT, KEY>::m_pool[i0], dHeapBase<OBJECT, KEY>::m_pool[index]);
}
index = i0;
}
}
dAssert (SanityCheck());
}
template <class OBJECT, class KEY>
bool dUpHeap<OBJECT, KEY>::SanityCheck()
{
#ifdef D_CHECK_HEAP
for (int i = 0; i < this->m_curCount; i++) {
int i1 = 2 * i + 1;
int i2 = 2 * i + 2;
if ((i1 < this->m_curCount) && (dHeapBase<OBJECT, KEY>::m_pool[i].m_key > dHeapBase<OBJECT, KEY>::m_pool[i1].m_key)) {
return false;
}
if ((i2 < this->m_curCount) && (dHeapBase<OBJECT, KEY>::m_pool[i].m_key > dHeapBase<OBJECT, KEY>::m_pool[i2].m_key)) {
return false;
}
}
#endif
return true;
}
#endif

483
thirdparty/src/newton/dContainers/dList.h vendored Normal file
View File

@@ -0,0 +1,483 @@
/* Copyright (c) <2003-2019> <Newton Game Dynamics>
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely
*/
#ifndef __dList__
#define __dList__
#include "dContainersStdAfx.h"
#include "dContainersAlloc.h"
// a small double link list container similar to STL,
template<class T, int poolSize = D_MAX_ENTRIES_IN_FREELIST>
class dList: public dContainersAlloc
{
public:
class dListNode
{
dListNode (dListNode* const prev, dListNode* const next)
:m_info ()
{
m_prev = prev;
m_next = next;
if (m_prev) {
m_prev->m_next = this;
}
if (m_next) {
m_next->m_prev = this;
}
}
dListNode (const T &info, dListNode* prev, dListNode* next)
:m_info (info)
{
m_prev = prev;
m_next = next;
if (m_prev) {
m_prev->m_next = this;
}
if (m_next) {
m_next->m_prev = this;
}
}
virtual ~dListNode()
{
}
void Unlink ()
{
if (m_prev) {
m_prev->m_next = m_next;
}
if (m_next) {
m_next->m_prev = m_prev;
}
m_prev = NULL;
m_next = NULL;
}
void Remove()
{
Unlink();
this->~dListNode();
}
void AddLast(dListNode* const node)
{
m_next = node;
node->m_prev = this;
}
void AddFirst(dListNode* const node)
{
m_prev = node;
node->m_next = this;
}
public:
T& GetInfo()
{
return m_info;
}
const T& GetInfo() const
{
return m_info;
}
dListNode* GetNext() const
{
return m_next;
}
dListNode* GetPrev() const
{
return m_prev;
}
private:
T m_info;
dListNode* m_next;
dListNode* m_prev;
friend class dList<T, poolSize>;
};
class Iterator
{
public:
Iterator (const dList<T, poolSize> &me)
{
m_ptr = NULL;
m_list = (dList *)&me;
}
~Iterator ()
{
}
operator int() const
{
return m_ptr != NULL;
}
bool operator== (const Iterator &target) const
{
return (m_ptr == target.m_ptr) && (m_list == target.m_list);
}
void Begin()
{
m_ptr = m_list->GetFirst();
}
void End()
{
m_ptr = m_list->GetLast();
}
void Set (dListNode* const node)
{
m_ptr = node;
}
void operator++ ()
{
m_ptr = m_ptr->m_next();
}
void operator++ (int)
{
m_ptr = m_ptr->GetNext();
}
void operator-- ()
{
m_ptr = m_ptr->GetPrev();
}
void operator-- (int)
{
m_ptr = m_ptr->GetPrev();
}
T &operator* () const
{
return m_ptr->GetInfo();
}
dListNode* GetNode() const
{
return m_ptr;
}
private:
dList *m_list;
dListNode* m_ptr;
};
// ***********************************************************
// member functions
// ***********************************************************
public:
dList ();
virtual ~dList ();
operator int() const;
int GetCount() const;
dListNode* GetLast() const;
dListNode* GetFirst() const;
dListNode* Append ();
dListNode* Append (const T &element);
dListNode* Addtop ();
dListNode* Addtop (const T &element);
dListNode* AppendAfter (dListNode* const node);
void RotateToEnd (dListNode* const node);
void RotateToBegin (dListNode* const node);
void InsertAfter (dListNode* const root, dListNode* const node);
dListNode* Find (const T &element) const;
dListNode* GetNodeFromInfo (T &m_info) const;
void Remove (dListNode* const node);
void Remove (const T &element);
void RemoveAll ();
// special routines
// move the data to the target list and set to zero m_count, m_first and m_last
void TranferDataToTarget (dList& target);
// ***********************************************************
// member variables
// ***********************************************************
private:
/*
bool Sanity() const
{
int count = 0;
for (dListNode* ptr = GetFirst(); ptr; ptr = ptr->GetNext())
count ++;
return count == m_count;
}
*/
bool Sanity() const {return true;}
dContainerFixSizeAllocator& GetAllocator()
{
static dContainerFixSizeAllocator* allocator = NULL;
if (!allocator) {
allocator = dContainerFixSizeAllocator::Create (sizeof (dList<T, poolSize>::dListNode), poolSize);
}
return *allocator;
}
int m_count;
dListNode* m_first;
dListNode* m_last;
friend class dListNode;
};
template<class T, int poolSize>
dList<T, poolSize>::dList ()
{
m_count = 0;
m_first = NULL;
m_last = NULL;
GetAllocator();
}
template<class T, int poolSize>
dList<T, poolSize>::~dList ()
{
RemoveAll ();
}
template<class T, int poolSize>
int dList<T, poolSize>::GetCount() const
{
return m_count;
}
template<class T, int poolSize>
dList<T, poolSize>::operator int() const
{
return m_first != NULL;
}
template<class T, int poolSize>
typename dList<T, poolSize>::dListNode* dList<T, poolSize>::GetFirst() const
{
return m_first;
}
template<class T, int poolSize>
typename dList<T, poolSize>::dListNode* dList<T, poolSize>::GetLast() const
{
return m_last;
}
template<class T, int poolSize>
typename dList<T, poolSize>::dListNode* dList<T, poolSize>::Append ()
{
m_count ++;
if (m_first == NULL) {
m_first = new (GetAllocator().Alloc()) dListNode(NULL, NULL);
m_last = m_first;
} else {
m_last = new (GetAllocator().Alloc()) dListNode(m_last, NULL);
}
dAssert (Sanity());
return m_last;
}
template<class T, int poolSize>
typename dList<T, poolSize>::dListNode* dList<T, poolSize>::AppendAfter (dListNode* const node)
{
dListNode* const ptr = Append ();
InsertAfter (node, ptr);
return ptr;
}
template<class T, int poolSize>
typename dList<T, poolSize>::dListNode* dList<T, poolSize>::Append (const T &element)
{
m_count ++;
if (m_first == NULL) {
m_first = new (GetAllocator().Alloc()) dListNode(element, NULL, NULL);
m_last = m_first;
} else {
m_last = new (GetAllocator().Alloc()) dListNode(element, m_last, NULL);
}
dAssert (Sanity());
return m_last;
}
template<class T, int poolSize>
typename dList<T, poolSize>::dListNode* dList<T, poolSize>::Addtop ()
{
m_count ++;
if (m_last == NULL) {
m_last = new (GetAllocator().Alloc()) dListNode(NULL, NULL);
m_first = m_last;
} else {
m_first = new (GetAllocator().Alloc()) dListNode(NULL, m_first);
}
dAssert (Sanity());
return m_first;
}
template<class T, int poolSize>
typename dList<T, poolSize>::dListNode* dList<T, poolSize>::Addtop (const T &element)
{
m_count ++;
if (m_last == NULL) {
m_last = new (GetAllocator().Alloc()) dListNode(element, NULL, NULL);
m_first = m_last;
} else {
m_first = new (GetAllocator().Alloc()) dListNode(element, NULL, m_first);
}
dAssert (Sanity());
return m_first;
}
template<class T, int poolSize>
void dList<T, poolSize>::InsertAfter (dListNode* const root, dListNode* const node)
{
dAssert (root != node);
if (node == m_last) {
m_last = m_last->GetPrev();
}
node->Unlink();
node->m_prev = root;
node->m_next = root->m_next;
if (root->m_next) {
root->m_next->m_prev = node;
}
root->m_next = node;
if (root == m_last) {
m_last = node;
}
dAssert (Sanity());
}
template<class T, int poolSize>
void dList<T, poolSize>::RotateToEnd (dListNode* const node)
{
if (node != m_last) {
if (m_last != m_first) {
if (node == m_first) {
m_first = m_first->GetNext();
}
node->Unlink();
m_last->AddLast(node);
m_last = node;
}
}
}
template<class T, int poolSize>
void dList<T, poolSize>::RotateToBegin (dListNode* const node)
{
if (node != m_first) {
if (m_last != m_first) {
if (node == m_last) {
m_last = m_last->GetPrev();
}
node->Unlink();
m_first->AddFirst(node);
m_first = node;
}
}
}
template<class T, int poolSize>
typename dList<T, poolSize>::dListNode* dList<T, poolSize>::Find (const T &element) const
{
dListNode* node;
for (node = m_first; node; node = node->GetNext()) {
if (element == node->m_info) {
break;
}
}
return node;
}
template<class T, int poolSize>
typename dList<T, poolSize>::dListNode* dList<T, poolSize>::GetNodeFromInfo (T &info) const
{
dListNode* const node = (dListNode*) &info;
long long offset = ((char*) &node->m_info) - ((char*)node);
dListNode* const retnode = (dListNode*) (((char *) node) - offset);
dAssert (&retnode->GetInfo () == &info);
return retnode;
}
template<class T, int poolSize>
void dList<T, poolSize>::Remove (const T &element)
{
dListNode* const node = Find (element);
if (node) {
Remove (node);
}
}
template<class T, int poolSize>
void dList<T, poolSize>::Remove (dListNode* const node)
{
m_count --;
if (node == m_first) {
m_first = m_first->GetNext();
}
if (node == m_last) {
m_last = m_last->GetPrev();
}
node->Remove();
GetAllocator().Free (node);
dAssert (Sanity());
}
template<class T, int poolSize>
void dList<T, poolSize>::RemoveAll ()
{
while (m_first) {
Remove(m_first);
}
dAssert (!m_count);
}
template<class T, int poolSize>
void dList<T, poolSize>::TranferDataToTarget (dList& target)
{
dAssert (target.m_count == 0);
target.m_count = m_count;
target.m_first = m_first;
target.m_last = m_last;
m_count = 0;
m_first = NULL;
m_last = NULL;
}
#endif

1049
thirdparty/src/newton/dContainers/dMap.h vendored Normal file
View File

File diff suppressed because it is too large Load Diff

79
thirdparty/src/newton/dContainers/dPointer.h vendored Normal file
View File

@@ -0,0 +1,79 @@
/* Copyright (c) <2003-2019> <Newton Game Dynamics>
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely
*/
#ifndef __D_POINTER_H__
#define __D_POINTER_H__
template <typename T>
class dPointer
{
public:
dPointer();
dPointer(T* const data);
~dPointer();
T& operator* ();
T* operator-> ();
T* GetData() const;
void SetData(T* const data);
private:
T* m_data;
};
template<class T>
dPointer<T>::dPointer()
:m_data(NULL)
{
}
template<class T>
dPointer<T>::dPointer(T* const data)
:m_data(data)
{
}
template<class T>
dPointer<T>::~dPointer()
{
if (m_data) {
delete m_data;
}
}
template<class T>
T& dPointer<T>::operator* ()
{
return *m_data;
}
template<class T>
T* dPointer<T>::operator-> ()
{
return m_data;
}
template<class T>
T* dPointer<T>::GetData() const
{
return m_data;
}
template<class T>
void dPointer<T>::SetData(T* const data)
{
m_data = data;
}
#endif

46
thirdparty/src/newton/dContainers/dRefCounter.cpp vendored Normal file
View File

@@ -0,0 +1,46 @@
/* Copyright (c) <2003-2019> <Newton Game Dynamics>
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely
*/
#include "dContainersStdAfx.h"
#include "dRefCounter.h"
dRefCounter::dRefCounter(void)
:dContainersAlloc()
{
m_refCount = 1;
}
dRefCounter::~dRefCounter(void)
{
}
int dRefCounter::GetRef() const
{
return m_refCount;
}
int dRefCounter::Release()
{
m_refCount --;
dAssert (m_refCount >= 0);
if (!m_refCount) {
delete this;
return 0;
}
return m_refCount;
}
void dRefCounter::AddRef() const
{
m_refCount ++;
}

34
thirdparty/src/newton/dContainers/dRefCounter.h vendored Normal file
View File

@@ -0,0 +1,34 @@
/* Copyright (c) <2003-2019> <Newton Game Dynamics>
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely
*/
#ifndef __DREF_COUNTER_H__
#define __DREF_COUNTER_H__
#include "dContainersAlloc.h"
class dRefCounter: public dContainersAlloc
{
public:
DCONTAINERS_API dRefCounter(void);
DCONTAINERS_API int GetRef() const;
DCONTAINERS_API int Release();
DCONTAINERS_API void AddRef() const;
protected:
DCONTAINERS_API virtual ~dRefCounter(void);
private:
mutable int m_refCount;
};
#endif

62
thirdparty/src/newton/dContainers/dRtti.h vendored Normal file
View File

@@ -0,0 +1,62 @@
/* Copyright (c) <2003-2019> <Newton Game Dynamics>
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely
*/
#ifndef __DRTTI_H__
#define __DRTTI_H__
#include "dCRC.h"
#include "dContainersAlloc.h"
#define dRttiCommon(className,exportType) \
private: \
exportType static dCRCTYPE m_rtti; \
public: \
static dCRCTYPE GetRttiType() \
{ \
return m_rtti; \
} \
virtual dCRCTYPE GetTypeId () const \
{ \
return m_rtti; \
} \
// add these macros only to the root base class that you want to have rtti
#define dRttiRootClassSupportDeclare(className,exportType) \
dRttiCommon(className,exportType) \
virtual bool IsType (dCRCTYPE typeId) const \
{ \
return typeId == m_rtti; \
}
#define dRttiRootClassSupportImplement(className) \
dCRCTYPE className::m_rtti = dCRC64 (#className);
// add these macros to every derived class
#define dAddRtti(baseClass,exportType) \
dRttiCommon(baseClass,exportType) \
virtual bool IsType (dCRCTYPE typeId) const \
{ \
if (typeId == m_rtti) { \
return true; \
} \
return baseClass::IsType (typeId); \
}
#define dInitRtti(className) \
dRttiRootClassSupportImplement(className)
#endif

571
thirdparty/src/newton/dContainers/dString.cpp vendored Normal file
View File

@@ -0,0 +1,571 @@
/* Copyright (c) <2003-2019> <Newton Game Dynamics>
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely
*/
#include "dContainersStdAfx.h"
#include "dString.h"
#define D_USE_POOL_BUKECT_ALLOCATOR
#define D_STRING_MEM_GRANULARITY 16
#define D_STRING_MEM_MAX_BUCKET_SIZE 256
#define D_STRING_MEM_BUCKETS (D_STRING_MEM_MAX_BUCKET_SIZE / D_STRING_MEM_GRANULARITY)
#define D_DSTRING_ENTRIES_IN_FREELIST 32
class dString::dStringAllocator
{
public:
#ifdef D_USE_POOL_BUKECT_ALLOCATOR
class dMemBucket
{
public:
class dDataChunk
{
public:
int m_size;
int m_count;
dDataChunk* m_next;
};
dMemBucket()
:m_freeListDataChunk(NULL)
{
}
~dMemBucket()
{
}
void Prefetch (int chunckSize)
{
for (int i = 0; i < D_DSTRING_ENTRIES_IN_FREELIST; i ++) {
//dDataChunk* const data = (dDataChunk*) new char[chunckSize + sizeof (int)];
dDataChunk* const data = (dDataChunk*) dContainersAlloc::Alloc (chunckSize + sizeof (int));
data->m_count = i + 1;
data->m_size = chunckSize;
data->m_next = m_freeListDataChunk;
m_freeListDataChunk = data;
}
}
void Flush ()
{
for (int i = 0; m_freeListDataChunk && (i < D_DSTRING_ENTRIES_IN_FREELIST); i ++) {
dDataChunk* const ptr = m_freeListDataChunk;
m_freeListDataChunk = m_freeListDataChunk->m_next;
//delete[] (char*) ptr;
dContainersAlloc::Free (ptr);
}
}
char* Alloc(int size)
{
dAssert (size < 1024 * 4);
if (!m_freeListDataChunk) {
Prefetch (size);
}
dDataChunk* const data = m_freeListDataChunk;
dAssert (size == data->m_size);
m_freeListDataChunk = m_freeListDataChunk->m_next;
return ((char*)data) + sizeof (int);
}
void Free(char * const ptr)
{
char* const realPtr = ptr - sizeof (int);
dMemBucket::dDataChunk* const dataChunck = (dMemBucket::dDataChunk*) (realPtr);
dataChunck->m_count = m_freeListDataChunk ? m_freeListDataChunk->m_count + 1 : 1;
dataChunck->m_next = m_freeListDataChunk;
m_freeListDataChunk = dataChunck;
if (dataChunck->m_count >= 2 * D_DSTRING_ENTRIES_IN_FREELIST) {
Flush();
}
}
dDataChunk* m_freeListDataChunk;
};
dStringAllocator()
{
for (int i = 0; i < int (sizeof (m_buckects) / sizeof (m_buckects[0])); i ++) {
m_buckects[i].Prefetch ((i + 1)* D_STRING_MEM_GRANULARITY);
}
}
~dStringAllocator()
{
for (int i = 0; i < int (sizeof (m_buckects) / sizeof (m_buckects[0])); i ++) {
m_buckects[i].Flush();
}
}
char* Alloc(int size)
{
dAssert (size >= 1);
if (size <= D_STRING_MEM_MAX_BUCKET_SIZE) {
int buckectEntry = (size - 1) / D_STRING_MEM_GRANULARITY;
int buckectSize = (buckectEntry + 1) * D_STRING_MEM_GRANULARITY;
return m_buckects[buckectEntry].Alloc(buckectSize);
}
dMemBucket::dDataChunk* const ptr = (dMemBucket::dDataChunk*) dContainersAlloc::Alloc (size + sizeof (int));
ptr->m_size = size;
return ((char*)ptr) + sizeof (int);
}
void Free(char* const ptr)
{
char* const realPtr = ptr-sizeof (int);
dMemBucket::dDataChunk* const dataChunck = (dMemBucket::dDataChunk*) (realPtr);
if (dataChunck->m_size <= D_STRING_MEM_MAX_BUCKET_SIZE) {
int buckectEntry = dataChunck->m_size / D_STRING_MEM_GRANULARITY - 1;
m_buckects[buckectEntry].Free(ptr);
} else {
void* const ptr1 = ((char*)ptr) - sizeof (int);
dContainersAlloc::Free (ptr1);
}
}
dMemBucket m_buckects [D_STRING_MEM_BUCKETS];
#else
char* Alloc(int size)
{
//return new char[size];
return (char*) dContainersAlloc::Alloc (size);
}
void Free(char* const ptr)
{
//delete[] ptr;
dContainersAlloc::Free (ptr);
}
#endif
};
//dString::dStringAllocator dString::m_allocator;
dString::dString ()
:m_string(NULL)
,m_size(0)
,m_capacity(0)
{
}
dString::dString (const dString& src)
:m_string(NULL)
,m_size(0)
,m_capacity(0)
{
if (src.m_string) {
m_size = src.m_size;
m_capacity = m_size + 1;
m_string = AllocMem (src.m_size + 1);
CopyData (m_string, src.m_string, src.m_size + 1);
m_string[m_size] = 0;
}
}
dString::dString (const char* const data)
:m_string(NULL)
,m_size(0)
,m_capacity(0)
{
if (data) {
m_size = CalculateSize (data);
m_capacity = m_size + 1;
m_string = AllocMem (m_size + 1);
CopyData (m_string, data, m_size + 1);
m_string[m_size] = 0;
}
}
dString::dString (const char* const data, int maxSize)
:m_string(NULL)
,m_size(0)
,m_capacity(0)
{
if (data) {
m_size = dMin (CalculateSize (data), maxSize);
m_capacity = m_size + 1;
m_string = AllocMem (m_size + 1);
CopyData (m_string, data, m_size + 1);
m_string[m_size] = 0;
}
}
dString::dString (const dString& src, const char* const concatenate, int concatenateSize)
:m_string(NULL)
,m_size(0)
,m_capacity(0)
{
m_string = AllocMem (src.m_size + concatenateSize + 1);
memcpy (m_string, src.m_string, src.m_size);
memcpy (&m_string[src.m_size], concatenate, concatenateSize);
m_size = src.m_size + concatenateSize;
m_string[m_size] = 0;
m_capacity = m_size + 1;
}
dString::dString (char chr)
:m_string(NULL)
,m_size(0)
,m_capacity(0)
{
m_string = AllocMem (2);
m_string[0] = chr;
m_string[1] = 0;
m_size = 1;
m_capacity = m_size + 1;
}
dString::dString (int val)
:m_string(NULL)
,m_size(0)
,m_capacity(0)
{
char tmp[256];
int count = 0;
unsigned mag = abs (val);
do {
unsigned digit = mag % 10;
mag /= 10;
tmp[count] = '0' + char(digit);
count ++;
} while (mag > 0);
int offset = (val >= 0) ? 0: 1;
m_string = AllocMem (count + offset + 1);
if (offset) {
m_string[0] = '-';
}
for (int i = 0; i < count; i ++) {
m_string[i + offset] = tmp[count - i - 1];
}
m_string[count + offset] = 0;
m_size = count + offset;
m_capacity = m_size + 1;
}
dString::dString (long long val)
:m_string(NULL)
,m_size(0)
,m_capacity(0)
{
char tmp[256];
int count = 0;
unsigned long long mag = (val > 0ll) ? val : -val;
do {
unsigned long long digit = mag % 10ll;
mag /= 10ll;
tmp[count] = '0' + char(digit);
count ++;
} while (mag > 0);
int offset = (val >= 0ll) ? 0: 1;
m_string = AllocMem (count + offset + 1);
if (offset) {
m_string[0] = '-';
}
for (int i = 0; i < count; i ++) {
m_string[i + offset] = tmp[count - i - 1];
}
m_string[count + offset] = 0;
m_size = count + offset;
m_capacity = m_size + 1;
}
dString::~dString ()
{
Empty();
}
void dString::Empty()
{
if (m_capacity && m_string) {
FreeMem (m_string);
}
m_size = 0;
m_capacity = 0;
m_string = NULL;
}
void dString::LoadFile (FILE* const file)
{
Empty();
// fseek (file, 0, SEEK_END);
// int size = ftell (file);
int size = 0;
fseek (file, 0, SEEK_SET);
for (;!feof(file); size ++) {
fgetc (file);
}
fseek (file, 0, SEEK_SET);
Expand (size);
size_t ret = fread (m_string, 1, size, file);
ret = 0;
m_string[size-1] = 0;
m_size = size-1;
m_capacity = m_size + 1;
}
void dString::operator+= (const char* const src)
{
char* const oldData = m_string;
int size = CalculateSize (src);
m_string = AllocMem (m_size + size + 1);
memcpy (m_string, oldData, m_size);
memcpy (&m_string[m_size], src, size);
m_size = m_size + size;
m_string[m_size] = 0;
m_capacity = m_size + 1;
FreeMem(oldData);
}
int dString::ToInteger() const
{
int value = 0;
if (m_size) {
int base = (m_string[0] == '-') ? 1 : 0;
for (int i = base; i < m_size; i ++) {
char ch = m_string[i];
if ((ch >= '0') && (ch <= '9')) {
value = value * 10 + ch - '0';
} else {
break;
}
}
value *= base ? -1 : 1;
}
return value;
}
long long dString::ToInteger64() const
{
long long value = 0;
if (m_size) {
int base = (m_string[0] == '-') ? 1 : 0;
for (int i = base; i < m_size; i ++) {
char ch = m_string[i];
if ((ch >= '0') && (ch <= '9')) {
value = value * 10ll + ch - '0';
} else {
break;
}
}
value *= base ? -1 : 1;
}
return value;
}
double dString::ToFloat() const
{
double value = 0.0;
double power = 1.0;
double decimalBase = 1.0;
if (m_size) {
int base = (m_string[0] == '-') ? 1 : 0;
for (int i = base; i < m_size; i ++) {
char ch = m_string[i];
if ((ch >= '0') && (ch <= '9')) {
value = value * 10ll + ch - '0';
power *= decimalBase;
} else if (ch == '.') {
decimalBase = 10.0;
} else {
break;
}
}
value *= base ? -1 : 1;
}
value /= power;
return value;
}
dString& dString::operator= (const dString& src)
{
if (m_capacity && m_string) {
FreeMem (m_string);
}
m_string = NULL;
m_capacity = 0;
m_size = src.m_size;
if (src.m_string) {
m_capacity = src.m_size + 1;
m_string = AllocMem (src.m_size + 1);
CopyData (m_string, src.m_string, src.m_size + 1);
}
return *this;
}
int dString::CalculateSize (const char* const data) const
{
int size = 0;
if (data) {
for (int i = 0; data[i]; i ++) {
size ++;
}
}
return size;
}
void dString::ToUpper()
{
if (m_string) {
for (char * cp = m_string; *cp; ++cp) {
if ((*cp >= 'a') && (*cp <= 'z') )
*cp += 'A' - 'a';
}
}
}
void dString::ToLower()
{
if (m_string) {
for (char * cp = m_string; *cp; ++cp) {
if ((*cp >= 'A') && (*cp <= 'Z') )
*cp += 'a' - 'A';
}
}
}
int dString::Find (char ch, int from) const
{
for (int i = from; i < m_size; i ++) {
if (m_string[i] == ch) {
return i;
}
}
return -1;
}
//int dString::Find (const dString& subStream, int from) const
int dString::Find (const char* const subString, int subStringLength, int from, int lenght) const
{
dAssert (from >= 0);
//dAssert (subStream.m_size >= 0);
dAssert (subStringLength >= 1);
int location = -1;
if (m_size) {
const int str2Size = dMin (subStringLength, lenght);
if (str2Size == 1) {
char ch = subString[0];
const char* const ptr1 = m_string;
for (int i = 0; i < m_size; i ++) {
if (ch == ptr1[i]) {
return i;
}
}
} else if ((str2Size < 4) || (m_size < 64)) {
const int size = m_size - str2Size;
for (int j = from; j <= size; j ++) {
const char* const ptr1 = &m_string[j];
int i = 0;
while (subString[i] && (ptr1[i] == subString[i])) {
i ++;
}
if (!subString[i]) {
return j;
}
}
} else {
// for large strings smart search
short frequency[256];
memset (frequency, -1, sizeof (frequency));
for (int i = 0; i < str2Size; i ++) {
frequency[int (subString[i])] = short(i);
}
int j = from;
const int size = m_size - str2Size;
while (j <= size) {
const char* const ptr1 = &m_string[j];
int i = str2Size - 1;
while ((i >= 0) && (ptr1[i] == subString[i])) {
i --;
}
if (i < 0) {
return j;
}
j += dMax(i - frequency[int (ptr1[i])], 1);
}
}
}
return location;
}
void dString::Replace (int start, int size, const char* const str, int strSize)
{
char* const oldData = m_string;
m_string = AllocMem (m_size - size + strSize + 1);
memcpy (m_string, oldData, start);
memcpy (&m_string[start], str, strSize);
memcpy (&m_string[start + strSize], &oldData[start + size], m_size - (start + size));
m_size = m_size - size + strSize;
m_capacity = m_size - size + strSize + 1;
m_string[m_size] = 0;
FreeMem(oldData);
}
void dString::Expand (int size)
{
char* const oldData = m_string;
m_string = AllocMem (m_size + size + 1);
if (m_capacity) {
memcpy (m_string, oldData, m_size);
FreeMem(oldData);
}
m_string[m_size] = 0;
m_capacity = m_size + size + 1;
}
dString::dStringAllocator& dString::GetAllocator() const
{
static dStringAllocator allocator;
return allocator;
}
char* dString::AllocMem(int size)
{
return GetAllocator().Alloc(size);
}
void dString::FreeMem (char* const ptr)
{
if (ptr) {
GetAllocator().Free(ptr);
}
}

214
thirdparty/src/newton/dContainers/dString.h vendored Normal file
View File

@@ -0,0 +1,214 @@
/* Copyright (c) <2003-2019> <Newton Game Dynamics>
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely
*/
#ifndef __DSTRING_H_
#define __DSTRING_H_
#include "dContainersStdAfx.h"
#include "dContainersAlloc.h"
class dString: public dContainersAlloc
{
class dStringAllocator;
public:
DCONTAINERS_API dString ();
DCONTAINERS_API dString (char chr);
DCONTAINERS_API dString (const dString& src);
DCONTAINERS_API dString (const char* const data);
DCONTAINERS_API dString (const char* const data, int maxSize);
DCONTAINERS_API dString (int val);
DCONTAINERS_API dString (long long val);
DCONTAINERS_API ~dString ();
char& operator[] (int index);
char operator[] (int index) const;
DCONTAINERS_API dString& operator= (const dString& src);
bool operator== (const dString& src) const;
bool operator!= (const dString& src) const;
bool operator< (const dString& src) const;
bool operator> (const dString& src) const;
bool operator<= (const dString& src) const;
bool operator>= (const dString& src) const;
DCONTAINERS_API void operator+= (const char* const src);
void operator+= (const dString& src);
dString operator+ (const char* const src) const;
dString operator+ (const dString& src) const;
DCONTAINERS_API int Find (char ch, int from = 0) const;
int Find (const dString& subString, int from = 0) const;
DCONTAINERS_API int Find (const char* const subString, int from = 0, int lenght = 0x7ffffff) const;
DCONTAINERS_API void Replace (int start, int size, const char* const str, int strSize);
void Replace (int start, int size, const dString& str);
void Empty();
DCONTAINERS_API void ToUpper();
DCONTAINERS_API void ToLower();
DCONTAINERS_API int ToInteger() const;
DCONTAINERS_API double ToFloat() const;
DCONTAINERS_API long long ToInteger64() const;
int Size() const;
int Capacity() const;
DCONTAINERS_API void Expand (int size);
DCONTAINERS_API void LoadFile (FILE* const file);
dString SubString(int start = 0, int size = 0x7fffffff) const;
const char* GetStr () const;
private:
DCONTAINERS_API int CalculateSize (const char* const data) const;
int Compare (const char* const str0, const char* const str1) const;
void CopyData (char* const dst, const char* const src, int size) const;
DCONTAINERS_API int Find (const char* const subString, int stringSize, int from, int lenght) const;
protected:
char* AllocMem(int size);
void FreeMem (char* const ptr);
DCONTAINERS_API dString (const dString& src, const char* const concatenate, int maxSize);
char* m_string;
int m_size;
int m_capacity;
private:
dStringAllocator& GetAllocator() const;
};
inline char& dString::operator[] (int index)
{
dAssert (m_string);
dAssert (index >= 0);
dAssert (index < m_size);
return m_string[index];
}
inline char dString::operator[] (int index) const
{
dAssert (m_string);
dAssert (index >= 0);
dAssert (index < m_size);
return m_string[index];
}
inline const char* dString::GetStr () const
{
return m_string;
}
inline int dString::Size() const
{
return m_size;
}
inline int dString::Find (const char* const subString, int from, int lenght) const
{
return Find (subString, CalculateSize(subString), from, lenght);
}
inline int dString::Find (const dString& subStream, int from) const
{
dAssert (subStream.m_string);
return Find (subStream.m_string, subStream.m_size, from, subStream.m_size);
}
inline void dString::Replace (int start, int size, const dString& str)
{
Replace(start, size, str.m_string, str.m_size);
}
inline void dString::operator+= (const dString& src)
{
*this += src.m_string;
}
inline dString dString::operator+ (const dString& src) const
{
return dString (*this, src.m_string, src.m_size);
}
inline dString dString::operator+ (const char* const copy) const
{
return dString (*this, copy, CalculateSize (copy));
}
inline int dString::Capacity() const
{
return m_capacity;
}
inline void dString::CopyData (char* const dst, const char* const src, int size) const
{
dAssert (dst);
dAssert (src);
memcpy (dst, src, size);
}
inline int dString::Compare (const char* const str0, const char* const str1) const
{
dAssert (str0);
dAssert (str1);
return strcmp (str0, str1);
}
inline bool dString::operator== (const dString& src) const
{
return Compare (m_string, src.m_string) == 0;
}
inline bool dString::operator!= (const dString& src) const
{
return Compare (m_string, src.m_string) != 0;
}
inline bool dString::operator< (const dString& src) const
{
return Compare (m_string, src.m_string) < 0;
}
inline bool dString::operator> (const dString& src) const
{
return Compare (m_string, src.m_string) > 0;
}
inline bool dString::operator<= (const dString& src) const
{
return Compare (m_string, src.m_string) <= 0;
}
inline bool dString::operator>= (const dString& src) const
{
return Compare (m_string, src.m_string) >= 0;
}
inline dString dString::SubString(int start, int size) const
{
dAssert (m_string);
return dString (&m_string[start], size);
}
#endif

369
thirdparty/src/newton/dContainers/dTree.cpp vendored Normal file
View File

@@ -0,0 +1,369 @@
/* Copyright (c) <2003-2019> <Newton Game Dynamics>
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely
*/
#include "dContainersStdAfx.h"
#include "dTree.h"
dRedBackNode* dRedBackNode::Minimum () const
{
dRedBackNode* ptr = (dRedBackNode*) this;
for (; ptr->m_left; ptr = ptr->m_left);
return ptr;
}
dRedBackNode* dRedBackNode::Maximum () const
{
dRedBackNode* ptr = (dRedBackNode*) this;
for (; ptr->m_right; ptr = ptr->m_right);
return ptr;
}
dRedBackNode* dRedBackNode::Prev () const
{
if (m_left) {
return m_left->Maximum ();
}
dRedBackNode* me = (dRedBackNode*) this;
dRedBackNode* ptr = m_parent;
for (; ptr && me == ptr->m_left; ptr = ptr->m_parent) {
me = ptr;
}
return ptr;
}
dRedBackNode* dRedBackNode::Next () const
{
if (m_right) {
return m_right->Minimum ();
}
dRedBackNode* node = (dRedBackNode*) this;
dRedBackNode* ptr = m_parent;
for (; ptr && node == ptr->m_right; ptr = ptr->m_parent) {
node = ptr;
}
return ptr;
}
// rotate node me to left
void dRedBackNode::RotateLeft(dRedBackNode** const head)
{
dRedBackNode* const me = this;
dRedBackNode* const child = me->m_right;
dAssert (child);
//establish me->m_right link
me->m_right = child->m_left;
if (child->m_left != NULL) {
child->m_left->m_parent = me;
}
// establish child->m_parent link
if (child != NULL) {
child->m_parent = me->m_parent;
}
if (me->m_parent) {
if (me == me->m_parent->m_left) {
me->m_parent->m_left = child;
} else {
me->m_parent->m_right = child;
}
} else {
*head = child;
}
// link child and me
child->m_left = me;
if (me != NULL) {
me->m_parent = child;
}
}
// rotate node me to right *
void dRedBackNode::RotateRight(dRedBackNode** const head)
{
dRedBackNode* const me = this;
dRedBackNode* const child = me->m_left;
dAssert (child);
// establish me->m_left link
me->m_left = child->m_right;
if (child->m_right != NULL) {
child->m_right->m_parent = me;
}
// establish child->m_parent link
if (child != NULL) {
child->m_parent = me->m_parent;
}
if (me->m_parent) {
if (me == me->m_parent->m_right) {
me->m_parent->m_right = child;
} else {
me->m_parent->m_left = child;
}
} else {
*head = child;
}
// link me and child
child->m_right = me;
if (me != NULL) {
me->m_parent = child;
}
}
// maintain Red-Black tree balance after inserting node ptr
void dRedBackNode::InsertFixup(dRedBackNode** const head)
{
dRedBackNode* ptr = this;
// check Red-Black properties
dAssert ((ptr == *head) || ptr->m_parent);
while ((ptr != *head) && (ptr->m_parent->GetColor() == RED)) {
// we have a violation
dAssert (ptr->m_parent);
dAssert (ptr->m_parent->m_parent);
if (ptr->m_parent == ptr->m_parent->m_parent->m_left) {
dRedBackNode* const tmp = ptr->m_parent->m_parent->m_right;
if (tmp && (tmp->GetColor() == RED)) {
// uncle is RED
ptr->m_parent->SetColor(BLACK);
tmp->SetColor(BLACK) ;
ptr->m_parent->m_parent->SetColor(RED) ;
ptr = ptr->m_parent->m_parent;
} else {
// uncle is BLACK
if (ptr == ptr->m_parent->m_right) {
// make ptr a left child
ptr = ptr->m_parent;
ptr->RotateLeft(head);
}
ptr->m_parent->SetColor(BLACK);
if (ptr->m_parent->m_parent) {
ptr->m_parent->m_parent->SetColor(RED);
ptr->m_parent->m_parent->RotateRight(head);
}
}
} else {
//dAssert (ptr->m_parent == ptr->m_parent->m_parent->m_right);
// mirror image of above code
dRedBackNode* const tmp = ptr->m_parent->m_parent->m_left;
if (tmp && (tmp->GetColor() == RED)) {
//uncle is RED
ptr->m_parent->SetColor(BLACK);
tmp->SetColor(BLACK) ;
ptr->m_parent->m_parent->SetColor(RED) ;
ptr = ptr->m_parent->m_parent;
} else {
// uncle is BLACK
if (ptr == ptr->m_parent->m_left) {
ptr = ptr->m_parent;
ptr->RotateRight(head);
}
ptr->m_parent->SetColor(BLACK);
if (ptr->m_parent->m_parent->GetColor() == BLACK) {
ptr->m_parent->m_parent->SetColor(RED) ;
ptr->m_parent->m_parent->RotateLeft (head);
}
}
}
}
(*head)->SetColor(BLACK);
}
//maintain Red-Black tree balance after deleting node x
void dRedBackNode::RemoveFixup (dRedBackNode* const me, dRedBackNode** const head)
{
dRedBackNode* ptr = this;
dRedBackNode* node = me;
while ((node != *head) && (!node || node->GetColor() == BLACK)) {
if (node == ptr->m_left) {
if (!ptr) {
return;
}
dRedBackNode* tmp = ptr->m_right;
if (!tmp) {
return;
}
if (tmp->GetColor() == RED) {
tmp->SetColor(BLACK) ;
ptr->SetColor(RED) ;
ptr->RotateLeft (head);
tmp = ptr->m_right;
//if (!ptr || !tmp) {
if (!tmp) {
return;
}
}
if ((!tmp->m_left || (tmp->m_left->GetColor() == BLACK)) &&
(!tmp->m_right || (tmp->m_right->GetColor() == BLACK))) {
tmp->SetColor(RED);
node = ptr;
ptr = ptr->m_parent;
continue;
} else if (!tmp->m_right || (tmp->m_right->GetColor() == BLACK)) {
tmp->m_left->SetColor(BLACK);
tmp->SetColor(RED);
tmp->RotateRight (head);
tmp = ptr->m_right;
//if (!ptr || !tmp) {
if (!tmp) {
return;
}
}
tmp->SetColor (ptr->GetColor());
if (tmp->m_right) {
tmp->m_right->SetColor(BLACK) ;
}
if (ptr) {
ptr->SetColor(BLACK) ;
ptr->RotateLeft (head);
}
node = *head;
} else {
if (!ptr) {
return;
}
dRedBackNode* tmp = ptr->m_left;
if (!tmp) {
return;
}
if (tmp->GetColor() == RED) {
tmp->SetColor(BLACK) ;
ptr->SetColor(RED) ;
ptr->RotateRight (head);
tmp = ptr->m_left;
//if (!ptr || !tmp) {
if (!tmp) {
return;
}
}
if ((!tmp->m_right || (tmp->m_right->GetColor() == BLACK)) &&
(!tmp->m_left || (tmp->m_left->GetColor() == BLACK))) {
tmp->SetColor(RED) ;
node = ptr;
ptr = ptr->m_parent;
continue;
} else if (!tmp->m_left || (tmp->m_left->GetColor() == BLACK)) {
tmp->m_right->SetColor(BLACK) ;
tmp->SetColor(RED) ;
tmp->RotateLeft (head);
tmp = ptr->m_left;
//if (!ptr || !tmp) {
if (!tmp) {
return;
}
}
tmp->SetColor (ptr->GetColor());
if (tmp->m_left) {
tmp->m_left->SetColor(BLACK);
}
if (ptr) {
ptr->SetColor(BLACK) ;
ptr->RotateRight (head);
}
node = *head;
}
}
if (node) {
node->SetColor(BLACK);
}
}
void dRedBackNode::Unlink (dRedBackNode** const head)
{
dRedBackNode* const node = this;
node->SetInTreeFlag(false);
if (!node->m_left || !node->m_right) {
// y has a NULL node as a child
dRedBackNode* const endNode = node;
// x is y's only child
dRedBackNode* child = endNode->m_right;
if (endNode->m_left) {
child = endNode->m_left;
}
// remove y from the parent chain
if (child) {
child->m_parent = endNode->m_parent;
}
if (endNode->m_parent) {
if (endNode == endNode->m_parent->m_left) {
endNode->m_parent->m_left = child;
} else {
endNode->m_parent->m_right = child;
}
} else {
*head = child;
}
if (endNode->GetColor() == BLACK) {
endNode->m_parent->RemoveFixup (child, head);
}
} else {
// find tree successor with a NULL node as a child
dRedBackNode* endNode = node->m_right;
while (endNode->m_left != NULL) {
endNode = endNode->m_left;
}
// x is y's only child
dRedBackNode* const child = endNode->m_right;
endNode->m_left = node->m_left;
node->m_left->m_parent = endNode;
dRedBackNode* endNodeParent = endNode;
if (endNode != node->m_right) {
if (child) {
child->m_parent = endNode->m_parent;
}
endNode->m_parent->m_left = child;
endNode->m_right = node->m_right;
node->m_right->m_parent = endNode;
endNodeParent = endNode->m_parent;
}
if (node == *head) {
*head = endNode;
} else if (node == node->m_parent->m_left) {
node->m_parent->m_left = endNode;
} else {
node->m_parent->m_right = endNode;
}
endNode->m_parent = node->m_parent;
bool oldColor = endNode->GetColor();
endNode->SetColor (node->GetColor());
node->SetColor (oldColor);
if (oldColor == BLACK) {
endNodeParent->RemoveFixup (child, head);
}
}
}

796
thirdparty/src/newton/dContainers/dTree.h vendored Normal file
View File

@@ -0,0 +1,796 @@
/* Copyright (c) <2003-2019> <Newton Game Dynamics>
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely
*/
#ifndef __dTree__
#define __dTree__
#include "dContainersStdAfx.h"
#include "dContainersAlloc.h"
// Note: this is a low level class for dTree use only
// unpredictable result will happen if you attempt to manipulate
// any member of this class
class dRedBackNode
{
public:
enum REDBLACK_COLOR
{
RED = true,
BLACK = false
};
public:
dRedBackNode* GetLeft() const;
dRedBackNode* GetRight() const;
dRedBackNode* GetParent() const;
dRedBackNode (dRedBackNode* const parent);
DCONTAINERS_API dRedBackNode* Prev() const;
DCONTAINERS_API dRedBackNode* Next() const;
DCONTAINERS_API dRedBackNode* Minimum() const;
DCONTAINERS_API dRedBackNode* Maximum() const;
protected:
virtual ~dRedBackNode ()
{
}
void Initdata (dRedBackNode* const parent);
void SetColor (bool color);
bool GetColor () const;
bool IsInTree () const;
void SetInTreeFlag (bool flag);
DCONTAINERS_API void RotateLeft(dRedBackNode** const head);
DCONTAINERS_API void RotateRight(dRedBackNode** const head);
DCONTAINERS_API void RemoveFixup (dRedBackNode* const node, dRedBackNode** const head);
DCONTAINERS_API void Unlink (dRedBackNode** const head);
DCONTAINERS_API void InsertFixup(dRedBackNode** const head);
bool m_color;
bool m_inTree;
dRedBackNode* m_left;
dRedBackNode* m_right;
dRedBackNode* m_parent;
};
template<class OBJECT, class KEY, int poolSize = D_MAX_ENTRIES_IN_FREELIST>
class dTree: public dContainersAlloc
{
public:
class dTreeNode: public dRedBackNode
{
dTreeNode (
const KEY &key,
dTreeNode* parentNode)
:dRedBackNode(parentNode), m_info (), m_key (key)
{
}
dTreeNode (
const OBJECT &info,
const KEY &key,
dTreeNode* parentNode)
:dRedBackNode(parentNode), m_info (info), m_key (key)
{
}
virtual ~dTreeNode ()
{
}
dTreeNode& operator= (dTreeNode& src)
{
dAssert (0);
return* this;
}
dTreeNode* GetLeft () const
{
return (dTreeNode*) dRedBackNode::m_left;
}
dTreeNode* GetRight () const
{
return (dTreeNode*) dRedBackNode::m_right;
}
dTreeNode* GetParent ()
{
return (dTreeNode*) dRedBackNode::m_parent;
}
void SetLeft (dTreeNode* const node)
{
dRedBackNode::m_left = node;
}
void SetRight (dTreeNode* const node)
{
dRedBackNode::m_right = node;
}
void SetParent (dTreeNode* const node)
{
dRedBackNode::m_parent = node;
}
public:
const KEY& GetKey() const
{
return m_key;
}
OBJECT& GetInfo()
{
return m_info;
}
private:
OBJECT m_info;
KEY m_key;
friend class dTree<OBJECT, KEY>;
};
class Iterator
{
public:
Iterator(const dTree<OBJECT,KEY> &me)
{
m_ptr = NULL;
m_tree = &me;
}
~Iterator()
{
}
void Begin()
{
m_ptr = m_tree->Minimum();
}
void End()
{
m_ptr = m_tree->Maximum();
}
void Set (dTreeNode* const node)
{
m_ptr = node;
}
operator int() const
{
return m_ptr != NULL;
}
void operator++ ()
{
//dAssert (m_ptr);
m_ptr = m_ptr->Next();
}
void operator++ (int)
{
//dAssert (m_ptr);
m_ptr = m_ptr->Next();
}
void operator-- ()
{
//dAssert (m_ptr);
m_ptr = m_ptr->Prev();
}
void operator-- (int)
{
//dAssert (m_ptr);
m_ptr = m_ptr->Prev();
}
OBJECT &operator* () const
{
return ((dTreeNode*)m_ptr)->GetInfo();
}
dTreeNode* GetNode() const
{
return (dTreeNode*)m_ptr;
}
KEY GetKey () const
{
dTreeNode* const tmp = (dTreeNode*)m_ptr;
return tmp ? tmp->GetKey() : KEY(0);
}
private:
dRedBackNode* m_ptr;
const dTree* m_tree;
};
// ***********************************************************
// member functions
// ***********************************************************
public:
dTree ();
virtual ~dTree ();
operator int() const;
int GetCount() const;
dTreeNode* GetRoot () const;
dTreeNode* Minimum () const;
dTreeNode* Maximum () const;
dTreeNode* Find (KEY key) const;
dTreeNode* FindGreater (KEY key) const;
dTreeNode* FindGreaterEqual (KEY key) const;
dTreeNode* FindLessEqual (KEY key) const;
dTreeNode* GetNodeFromInfo (OBJECT &info) const;
dTreeNode* Insert (KEY key);
dTreeNode* Insert (const OBJECT &element, KEY key);
dTreeNode* Insert (const OBJECT &element, KEY key, bool& elementWasInTree);
dTreeNode* Insert (dTreeNode* const node, KEY key);
dTreeNode* Replace (OBJECT &element, KEY key);
dTreeNode* ReplaceKey (KEY oldKey, KEY newKey);
dTreeNode* ReplaceKey (dTreeNode* const node, KEY key);
void Unlink (dTreeNode* const node);
void Remove (KEY key);
void Remove (dTreeNode* const node);
void RemoveAll ();
bool SanityCheck () const;
// ***********************************************************
// member variables
// ***********************************************************
private:
dContainerFixSizeAllocator& GetAllocator()
{
static dContainerFixSizeAllocator* allocator = NULL;
if (!allocator) {
allocator = dContainerFixSizeAllocator::Create (sizeof (dTree<OBJECT, KEY, poolSize>::dTreeNode), poolSize);
}
return *allocator;
}
void RemoveAllLow (dTreeNode* const root);
int CompareKeys (const KEY &key0, const KEY &key1) const;
bool SanityCheck (dTreeNode* const ptr, int height) const;
int m_count;
dTreeNode* m_head;
friend class dTreeNode;
};
inline dRedBackNode::dRedBackNode (dRedBackNode* const parent)
{
Initdata (parent);
}
inline void dRedBackNode::Initdata (dRedBackNode* const parent)
{
SetColor (RED);
SetInTreeFlag (true);
m_left = NULL;
m_right = NULL;
m_parent = parent;
}
inline void dRedBackNode::SetColor (bool color)
{
m_color = color;
}
inline bool dRedBackNode::GetColor () const
{
return m_color;
}
inline bool dRedBackNode::IsInTree () const
{
return m_inTree;
}
inline void dRedBackNode::SetInTreeFlag (bool flag)
{
m_inTree = flag;
}
template<class OBJECT, class KEY, int poolSize>
dTree<OBJECT, KEY, poolSize>::dTree ()
{
m_count = 0;
m_head = NULL;
GetAllocator();
}
template<class OBJECT, class KEY, int poolSize>
dTree<OBJECT, KEY, poolSize>::~dTree ()
{
RemoveAll();
}
template<class OBJECT, class KEY, int poolSize>
dTree<OBJECT, KEY, poolSize>::operator int() const
{
return m_head != NULL;
}
template<class OBJECT, class KEY, int poolSize>
int dTree<OBJECT, KEY, poolSize>::GetCount() const
{
return m_count;
}
template<class OBJECT, class KEY, int poolSize>
typename dTree<OBJECT, KEY, poolSize>::dTreeNode* dTree<OBJECT, KEY, poolSize>::Minimum () const
{
return m_head ? (dTreeNode*) m_head->Minimum() : NULL;
}
template<class OBJECT, class KEY, int poolSize>
typename dTree<OBJECT, KEY, poolSize>::dTreeNode* dTree<OBJECT, KEY, poolSize>::Maximum () const
{
return m_head ? (dTreeNode*) m_head->Maximum() : NULL;
}
template<class OBJECT, class KEY, int poolSize>
typename dTree<OBJECT, KEY, poolSize>::dTreeNode* dTree<OBJECT, KEY, poolSize>::GetRoot () const
{
return m_head;
}
template<class OBJECT, class KEY, int poolSize>
typename dTree<OBJECT, KEY, poolSize>::dTreeNode* dTree<OBJECT, KEY, poolSize>::Find (KEY key) const
{
if (m_head == NULL) {
return NULL;
}
dTreeNode* ptr = m_head;
while (ptr != NULL) {
int val = CompareKeys (ptr->m_key, key);
if (!val) {
break;
}
if (val < 0) {
ptr = ptr->GetLeft();
} else {
ptr = ptr->GetRight();
}
}
return ptr;
}
template<class OBJECT, class KEY, int poolSize>
typename dTree<OBJECT, KEY, poolSize>::dTreeNode* dTree<OBJECT, KEY, poolSize>::GetNodeFromInfo (OBJECT &info) const
{
dTreeNode* node = (dTreeNode*) &info;
int offset = ((char*) &node->m_info) - ((char *) node);
node = (dTreeNode*) (((char *) node) - offset);
// dAssert (node->IsInTree ());
dAssert (&node->GetInfo () == &info);
return (node->IsInTree ()) ? node : NULL;
}
template<class OBJECT, class KEY, int poolSize>
typename dTree<OBJECT, KEY, poolSize>::dTreeNode* dTree<OBJECT, KEY, poolSize>::FindGreater (KEY key) const
{
if (m_head == NULL) {
return NULL;
}
dTreeNode* prev = NULL;
dTreeNode* ptr = m_head;
int val = 0;
while (ptr != NULL) {
val = CompareKeys (ptr->m_key, key);
if (!val) {
return (dTreeNode*) ptr->Next();
}
prev = ptr;
if (val < 0) {
ptr = ptr->GetLeft();
} else {
ptr = ptr->GetRight();
}
}
if (val > 0) {
while (prev->m_parent && (prev->m_parent->m_right == prev)) {
prev = prev->GetParent();
}
prev = prev->GetParent();
}
return (dTreeNode*) prev;
}
template<class OBJECT, class KEY, int poolSize>
typename dTree<OBJECT, KEY, poolSize>::dTreeNode* dTree<OBJECT, KEY, poolSize>::FindGreaterEqual (KEY key) const
{
if (m_head == NULL) {
return NULL;
}
dTreeNode* prev = NULL;
dTreeNode* ptr = m_head;
int val = 0;
while (ptr != NULL) {
val = CompareKeys (ptr->m_key, key);
if (!val) {
return ptr;
}
prev = ptr;
if (val < 0) {
ptr = ptr->GetLeft();
} else {
ptr = ptr->GetRight();
}
}
if (val > 0) {
while (prev->m_parent && (prev->m_parent->m_right == prev)) {
prev = prev->GetParent();
}
prev = prev->GetParent();
}
return (dTreeNode*) prev;
}
template<class OBJECT, class KEY, int poolSize>
typename dTree<OBJECT, KEY, poolSize>::dTreeNode* dTree<OBJECT, KEY, poolSize>::FindLessEqual (KEY key) const
{
if (m_head == NULL) {
return NULL;
}
dTreeNode* prev = NULL;
dTreeNode* ptr = m_head;
int val = 0;
while (ptr != NULL) {
val = CompareKeys (ptr->m_key, key);
if (!val) {
return ptr;
}
prev = ptr;
if (val < 0) {
ptr = ptr->GetLeft();
} else {
ptr = ptr->GetRight();
}
}
if (val < 0) {
while (prev->m_parent && (prev->m_parent->m_left == prev)) {
prev = prev->GetParent();
}
prev = prev->GetParent();
}
return (dTreeNode*) prev;
}
template<class OBJECT, class KEY, int poolSize>
typename dTree<OBJECT, KEY, poolSize>::dTreeNode* dTree<OBJECT, KEY, poolSize>::Insert (KEY key)
{
dTreeNode* parent = NULL;
dTreeNode* ptr = m_head;
int val = 0;
while (ptr != NULL) {
parent = ptr;
val = CompareKeys (ptr->m_key, key);
if (val < 0) {
ptr = ptr->GetLeft();
} else if (val > 0) {
ptr = ptr->GetRight();
} else {
return ptr;
}
}
m_count ++;
ptr = new (GetAllocator().Alloc()) dTreeNode (key, parent);
if (!parent) {
m_head = ptr;
} else {
if (val < 0) {
parent->m_left = ptr;
} else {
parent->m_right = ptr;
}
}
ptr->InsertFixup ((dRedBackNode**)&m_head);
return ptr;
}
template<class OBJECT, class KEY, int poolSize>
typename dTree<OBJECT, KEY, poolSize>::dTreeNode* dTree<OBJECT, KEY, poolSize>::Insert (const OBJECT &element, KEY key, bool& elementWasInTree)
{
dTreeNode* parent = NULL;
dTreeNode* ptr = m_head;
int val = 0;
elementWasInTree = false;
while (ptr != NULL) {
parent = ptr;
val = CompareKeys (ptr->m_key, key);
if (val < 0) {
ptr = ptr->GetLeft();
} else if (val > 0) {
ptr = ptr->GetRight();
} else {
elementWasInTree = true;
return ptr;
}
}
m_count ++;
ptr = new (GetAllocator().Alloc()) dTreeNode (element, key, parent);
if (!parent) {
m_head = ptr;
} else {
if (val < 0) {
parent->m_left = ptr;
} else {
parent->m_right = ptr;
}
}
ptr->InsertFixup ((dRedBackNode**)&m_head);
return ptr;
}
template<class OBJECT, class KEY, int poolSize>
typename dTree<OBJECT, KEY, poolSize>::dTreeNode* dTree<OBJECT, KEY, poolSize>::Insert (const OBJECT &element, KEY key)
{
bool foundState;
dTreeNode* node = Insert (element, key, foundState);
if (foundState) {
node = NULL;
}
return node;
}
template<class OBJECT, class KEY, int poolSize>
typename dTree<OBJECT, KEY, poolSize>::dTreeNode* dTree<OBJECT, KEY, poolSize>::Insert (typename dTree<OBJECT, KEY, poolSize>::dTreeNode* const node, KEY key)
{
int val = 0;
dTreeNode* ptr = m_head;
dTreeNode* parent = NULL;
while (ptr != NULL) {
parent = ptr;
val = CompareKeys (ptr->m_key, key);
if (val < 0) {
ptr = ptr->GetLeft();
} else if (val > 0) {
ptr = ptr->GetRight();
} else {
return NULL;
}
}
m_count ++;
ptr = node;
ptr->m_key = key;
ptr->Initdata (parent);
if (!parent) {
m_head = ptr;
} else {
if (val < 0) {
parent->m_left = ptr;
} else {
parent->m_right = ptr;
}
}
ptr->InsertFixup ((dRedBackNode**)&m_head);
return ptr;
}
template<class OBJECT, class KEY, int poolSize>
typename dTree<OBJECT, KEY, poolSize>::dTreeNode* dTree<OBJECT, KEY, poolSize>::Replace (OBJECT &element, KEY key)
{
dTreeNode* parent = NULL;
dTreeNode* ptr = m_head;
int val = 0;
while (ptr != NULL) {
parent = ptr;
val = CompareKeys (ptr->m_key, key);
if (val == 0) {
ptr->m_info = element;
return ptr;
}
if (val < 0) {
ptr = ptr->GetLeft();
} else {
ptr = ptr->GetRight();
}
}
ptr = new (GetAllocator().Alloc()) dTreeNode (element, key, parent);
if (!parent) {
m_head = ptr;
} else {
if (val < 0) {
parent->m_left = ptr;
} else {
parent->m_right = ptr;
}
}
ptr->InsertFixup ((dRedBackNode**)&m_head);
return ptr;
}
template<class OBJECT, class KEY, int poolSize>
typename dTree<OBJECT, KEY, poolSize>::dTreeNode* dTree<OBJECT, KEY, poolSize>::ReplaceKey (typename dTree<OBJECT, KEY, poolSize>::dTreeNode* node, KEY key)
{
Unlink(node);
dTreeNode* const ptr = Insert (node, key);
dAssert (ptr);
return ptr;
}
template<class OBJECT, class KEY, int poolSize>
typename dTree<OBJECT, KEY, poolSize>::dTreeNode* dTree<OBJECT, KEY, poolSize>::ReplaceKey (KEY oldKey, KEY newKey)
{
dTreeNode* const node = Find (oldKey);
return node ? ReplaceKey (node, newKey) : NULL;
}
template<class OBJECT, class KEY, int poolSize>
void dTree<OBJECT, KEY, poolSize>::Unlink (typename dTree<OBJECT, KEY, poolSize>::dTreeNode* const node)
{
m_count --;
node->Unlink((dRedBackNode** )&m_head);
}
template<class OBJECT, class KEY, int poolSize>
void dTree<OBJECT, KEY, poolSize>::Remove (typename dTree<OBJECT, KEY, poolSize>::dTreeNode* const node)
{
m_count --;
node->Unlink ((dRedBackNode** )&m_head);
node->~dTreeNode();
GetAllocator().Free (node);
}
template<class OBJECT, class KEY, int poolSize>
void dTree<OBJECT, KEY, poolSize>::Remove (KEY key)
{
// find node in tree
dTreeNode* const node = Find (key);
if (node) {
Remove (node);
}
}
template<class OBJECT, class KEY, int poolSize>
void dTree<OBJECT, KEY, poolSize>::RemoveAllLow (dTreeNode* const root)
{
if (root->m_left) {
RemoveAllLow((dTreeNode*)root->m_left);
}
if (root->m_right) {
RemoveAllLow ((dTreeNode*)root->m_right);
}
root->SetInTreeFlag(false);
root->~dTreeNode();
GetAllocator().Free (root);
}
template<class OBJECT, class KEY, int poolSize>
void dTree<OBJECT, KEY, poolSize>::RemoveAll ()
{
if (m_head) {
m_count = 0;
dTreeNode* root;
for (root = m_head; root->m_parent; root = (dTreeNode*)root->m_parent);
RemoveAllLow(root);
m_head = NULL;
}
}
template<class OBJECT, class KEY, int poolSize>
bool dTree<OBJECT, KEY, poolSize>::SanityCheck () const
{
return SanityCheck (m_head, 0);
}
template<class OBJECT, class KEY, int poolSize>
bool dTree<OBJECT, KEY, poolSize>::SanityCheck (typename dTree<OBJECT, KEY, poolSize>::dTreeNode* ptr, int height) const
{
if (!ptr) {
return true;
}
if (ptr->m_left) {
if (CompareKeys (ptr->m_key, ptr->GetLeft()->m_key) > 0) {
return false;
}
}
if (ptr->m_right) {
if (CompareKeys (ptr->m_key, ptr->GetRight()->m_key) < 0) {
return false;
}
}
if (ptr->GetColor() == dTreeNode::BLACK) {
height ++;
} else if (!((!ptr->m_left || (ptr->m_left->GetColor() == dTreeNode::BLACK)) &&
(!ptr->m_right || (ptr->m_right->GetColor() == dTreeNode::BLACK)))) {
return false;
}
if (!ptr->m_left && !ptr->m_right) {
int bh = 0;
for (dTreeNode* x = ptr; x; x = x->GetParent()) {
if (x->GetColor() == dTreeNode::BLACK) {
bh ++;
}
}
if (bh != height) {
return false;
}
}
if (ptr->m_left && !SanityCheck (ptr->GetLeft(), height)) {
return false;
}
if (ptr->m_right && !SanityCheck (ptr->GetRight(), height)) {
return false;
}
return true;
}
template<class OBJECT, class KEY, int poolSize>
int dTree<OBJECT, KEY, poolSize>::CompareKeys (const KEY &key0, const KEY &key1) const
{
if (key1 < key0) {
return - 1;
}
if (key1 > key0) {
return 1;
}
return 0;
}
#endif

48
thirdparty/src/newton/dMath/CMakeLists.txt vendored Normal file
View File

@@ -0,0 +1,48 @@
# Copyright (c) <2014-2017> <Newton Game Dynamics>
#
# This software is provided 'as-is', without any express or implied
# warranty. In no event will the authors be held liable for any damages
# arising from the use of this software.
#
# Permission is granted to anyone to use this software for any purpose,
# including commercial applications, and to alter it and redistribute it
# freely.
cmake_minimum_required(VERSION 3.4.0)
set (projectName "dMath")
message (${projectName})
# low level core
file(GLOB CPP_SOURCE *.h *.cpp)
file(GLOB HEADERS *.h)
if (UNIX OR MINGW)
if(NEWTON_BUILD_SHARED_LIBS)
add_library(${projectName} SHARED ${CPP_SOURCE})
else(NEWTON_BUILD_SHARED_LIBS)
add_library(${projectName} STATIC ${CPP_SOURCE})
endif(NEWTON_BUILD_SHARED_LIBS)
endif (UNIX OR MINGW)
if (MSVC)
add_library(${projectName} STATIC ${CPP_SOURCE})
if(CMAKE_VS_MSBUILD_COMMAND OR CMAKE_VS_DEVENV_COMMAND)
set_target_properties(${projectName} PROPERTIES COMPILE_FLAGS "/YudStdAfxMath.h")
set_source_files_properties(dStdAfxMath.cpp PROPERTIES COMPILE_FLAGS "/YcdStdAfxMath.h")
endif()
endif(MSVC)
target_include_directories(${projectName} PUBLIC .)
if (NEWTON_BUILD_PROFILER)
target_link_libraries (${projectName} dProfiler)
endif()
install(TARGETS ${projectName}
LIBRARY DESTINATION lib
ARCHIVE DESTINATION lib
RUNTIME DESTINATION bin)
install(FILES ${HEADERS} DESTINATION include/${projectName})

948
thirdparty/src/newton/dMath/dLinearAlgebra.cpp vendored Normal file
View File

@@ -0,0 +1,948 @@
/* Copyright (c) <2003-2019> <Newton Game Dynamics>
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely
*/
#include "dStdAfxMath.h"
#include "dMathDefines.h"
#include "dLinearAlgebra.h"
#define COMPLEMENTARITY_VEL_DAMP dFloat(100.0f)
#define COMPLEMENTARITY_POS_DAMP dFloat(1500.0f)
#define COMPLEMENTARITY_PSD_DAMP_TOL dFloat(1.0e-4f)
#define COMPLEMENTARITY_STACK_ENTRIES 64
void dSymmetricBiconjugateGradientSolve::ScaleAdd (int size, dFloat64* const a, const dFloat64* const b, dFloat64 scale, const dFloat64* const c) const
{
for (int i = 0; i < size; i ++) {
a[i] = b[i] + scale * c[i];
}
}
void dSymmetricBiconjugateGradientSolve::Sub (int size, dFloat64* const a, const dFloat64* const b, const dFloat64* const c) const
{
for (int i = 0; i < size; i ++) {
a[i] = b[i] - c[i];
}
}
dFloat64 dSymmetricBiconjugateGradientSolve::DotProduct (int size, const dFloat64* const b, const dFloat64* const c) const
{
dFloat64 product = dFloat64 (0.0f);
for (int i = 0; i < size; i ++) {
product += b[i] * c[i];
}
return product;
}
dFloat64 dSymmetricBiconjugateGradientSolve::Solve (int size, dFloat64 tolerance, dFloat64* const x, const dFloat64* const b) const
{
dFloat64* const r0 = new dFloat64 [size];
dFloat64* const p0 = new dFloat64 [size];
dFloat64* const MinvR0 = new dFloat64 [size];
dFloat64* const matrixP0 = new dFloat64 [size];
MatrixTimeVector (matrixP0, x);
Sub(size, r0, b, matrixP0);
bool continueExecution = InversePrecoditionerTimeVector (p0, r0);
int iter = 0;
dFloat64 num = DotProduct (size, r0, p0);
dFloat64 error2 = num;
for (int j = 0; (j < size) && (error2 > tolerance) && continueExecution; j ++) {
MatrixTimeVector (matrixP0, p0);
dFloat64 den = DotProduct (size, p0, matrixP0);
dAssert (fabs(den) > dFloat64 (0.0f));
dFloat64 alpha = num / den;
ScaleAdd (size, x, x, alpha, p0);
if ((j % 50) != 49) {
ScaleAdd (size, r0, r0, -alpha, matrixP0);
} else {
MatrixTimeVector (matrixP0, x);
Sub(size, r0, b, matrixP0);
}
continueExecution = InversePrecoditionerTimeVector (MinvR0, r0);
dFloat64 num1 = DotProduct (size, r0, MinvR0);
dFloat64 beta = num1 / num;
ScaleAdd (size, p0, MinvR0, beta, p0);
num = DotProduct (size, r0, MinvR0);
iter ++;
error2 = num;
if (j > 10) {
error2 = dFloat64 (0.0f);
for (int i = 0; i < size; i ++) {
error2 = dMax (error2, r0[i] * r0[i]);
}
}
}
delete[] matrixP0;
delete[] MinvR0;
delete[] p0;
delete[] r0;
dAssert (iter <= size);
return num;
}
dComplementaritySolver::dBodyState::dBodyState()
:m_matrix(dGetIdentityMatrix())
,m_localFrame(dGetIdentityMatrix())
,m_inertia(dGetZeroMatrix())
,m_invInertia(dGetZeroMatrix())
,m_localInertia (0.0f)
,m_localInvInertia(0.0f)
,m_veloc(0.0f)
,m_omega(0.0f)
,m_externalForce(0.0f)
,m_externalTorque(0.0f)
,m_globalCentreOfMass(0.0f)
,m_mass(0.0f)
,m_invMass(0.0f)
,m_myIndex(0)
{
}
const dVector& dComplementaritySolver::dBodyState::GetOmega() const
{
return m_omega;
}
const dVector& dComplementaritySolver::dBodyState::GetVelocity() const
{
return m_veloc;
}
dVector dComplementaritySolver::dBodyState::CalculatePointVelocity (const dVector& point) const
{
return m_veloc + m_omega.CrossProduct(point - m_globalCentreOfMass);
}
dFloat dComplementaritySolver::dBodyState::GetMass () const
{
return m_mass;
}
dFloat dComplementaritySolver::dBodyState::GetInvMass () const
{
return m_invMass;
}
void dComplementaritySolver::dBodyState::SetMass (dFloat mass)
{
m_mass = mass;
m_invMass = mass > (1.0e-3f) ? 1.0f / mass : 0.0f;
}
void dComplementaritySolver::dBodyState::SetInertia (dFloat Ixx, dFloat Iyy, dFloat Izz)
{
m_localInertia[0] = Ixx;
m_localInertia[1] = Iyy;
m_localInertia[2] = Izz;
m_localInvInertia[0] = Ixx ? 1.0f / Ixx : 0.0f;
m_localInvInertia[1] = Iyy ? 1.0f / Iyy : 0.0f;
m_localInvInertia[2] = Izz ? 1.0f / Izz : 0.0f;
}
void dComplementaritySolver::dBodyState::GetInertia (dFloat& Ixx, dFloat& Iyy, dFloat& Izz) const
{
Ixx = m_localInertia[0];
Iyy = m_localInertia[1];
Izz = m_localInertia[2];
}
const dMatrix& dComplementaritySolver::dBodyState::GetInertia() const
{
return m_inertia;
}
const dMatrix& dComplementaritySolver::dBodyState::GetInvInertia() const
{
return m_invInertia;
}
void dComplementaritySolver::dBodyState::SetMatrix (const dMatrix& matrix)
{
m_matrix = matrix;
m_globalCentreOfMass = m_matrix.TransformVector(m_localFrame.m_posit);
}
const dMatrix& dComplementaritySolver::dBodyState::GetMatrix () const
{
return m_matrix;
}
void dComplementaritySolver::dBodyState::SetLocalMatrix (const dMatrix& matrix)
{
m_localFrame = matrix;
m_globalCentreOfMass = m_matrix.TransformVector(m_localFrame.m_posit);
}
const dMatrix& dComplementaritySolver::dBodyState::GetLocalMatrix () const
{
return m_localFrame;
}
const dVector& dComplementaritySolver::dBodyState::GetCOM () const
{
return m_localFrame.m_posit;
}
void dComplementaritySolver::dBodyState::SetVeloc (const dVector& veloc)
{
m_veloc = veloc;
}
void dComplementaritySolver::dBodyState::SetOmega (const dVector& omega)
{
m_omega = omega;
}
void dComplementaritySolver::dBodyState::SetForce (const dVector& force)
{
m_externalForce = force;
}
void dComplementaritySolver::dBodyState::SetTorque (const dVector& torque)
{
m_externalTorque = torque;
}
const dVector& dComplementaritySolver::dBodyState::GetForce () const
{
return m_externalForce;
}
const dVector& dComplementaritySolver::dBodyState::GetTorque () const
{
return m_externalTorque;
}
void dComplementaritySolver::dBodyState::UpdateInertia()
{
dMatrix tmpMatrix (dGetZeroMatrix());
tmpMatrix[0] = m_localInertia * dVector (m_matrix[0][0], m_matrix[1][0], m_matrix[2][0], dFloat(0.0f));
tmpMatrix[1] = m_localInertia * dVector (m_matrix[0][1], m_matrix[1][1], m_matrix[2][1], dFloat(0.0f));
tmpMatrix[2] = m_localInertia * dVector (m_matrix[0][2], m_matrix[1][2], m_matrix[2][2], dFloat(0.0f));
m_inertia = tmpMatrix * m_matrix;
tmpMatrix[0] = m_localInvInertia * dVector (m_matrix[0][0], m_matrix[1][0], m_matrix[2][0], dFloat(0.0f));
tmpMatrix[1] = m_localInvInertia * dVector (m_matrix[0][1], m_matrix[1][1], m_matrix[2][1], dFloat(0.0f));
tmpMatrix[2] = m_localInvInertia * dVector (m_matrix[0][2], m_matrix[1][2], m_matrix[2][2], dFloat(0.0f));
m_invInertia = tmpMatrix * m_matrix;
}
void dComplementaritySolver::dBodyState::IntegrateForce (dFloat timestep, const dVector& force, const dVector& torque)
{
dVector accel (force.Scale (m_invMass));
dVector alpha (m_invInertia.RotateVector(torque));
m_veloc += accel.Scale (timestep);
m_omega += alpha.Scale (timestep);
}
void dComplementaritySolver::dBodyState::IntegrateVelocity (dFloat timestep)
{
// this is the fucking bug cause the vehicle malfunction
const dFloat D_MAX_ANGLE_STEP = dFloat (90.0f * dDegreeToRad);
while ((m_omega.DotProduct3(m_omega) * timestep * timestep) > (D_MAX_ANGLE_STEP * D_MAX_ANGLE_STEP)) {
dAssert (0);
m_omega = m_omega.Scale (dFloat (0.8f));
}
// this is correct
const dFloat D_ANGULAR_TOL = dFloat (0.0125f * dDegreeToRad);
m_globalCentreOfMass += m_veloc.Scale (timestep);
dFloat omegaMag2 = m_omega.DotProduct3(m_omega);
if (omegaMag2 > (D_ANGULAR_TOL * D_ANGULAR_TOL)) {
dFloat invOmegaMag = 1.0f / dSqrt (omegaMag2);
dVector omegaAxis (m_omega.Scale (invOmegaMag));
dFloat omegaAngle = invOmegaMag * omegaMag2 * timestep;
dQuaternion rotation (omegaAxis, omegaAngle);
dQuaternion rotMatrix (m_matrix);
rotMatrix = rotMatrix * rotation;
rotMatrix.Scale( 1.0f / dSqrt (rotMatrix.DotProduct (rotMatrix)));
m_matrix = dMatrix (rotMatrix, m_matrix.m_posit);
}
m_matrix.m_posit = m_globalCentreOfMass - m_matrix.RotateVector(m_localFrame.m_posit);
#ifdef _DEBUG
int j0 = 1;
int j1 = 2;
for (int i = 0; i < 3; i ++) {
dAssert (m_matrix[i][3] == 0.0f);
dFloat val = m_matrix[i].DotProduct3(m_matrix[i]);
dAssert (dAbs (val - 1.0f) < 1.0e-5f);
dVector tmp (m_matrix[j0].CrossProduct(m_matrix[j1]));
val = tmp.DotProduct3(m_matrix[i]);
dAssert (dAbs (val - 1.0f) < 1.0e-5f);
j0 = j1;
j1 = i;
}
#endif
}
void dComplementaritySolver::dBodyState::ApplyNetForceAndTorque (dFloat invTimestep, const dVector& veloc, const dVector& omega)
{
dVector accel = (m_veloc - veloc).Scale(invTimestep);
dVector alpha = (m_omega - omega).Scale(invTimestep);
m_externalForce = accel.Scale(m_mass);
alpha = m_matrix.UnrotateVector(alpha);
m_externalTorque = m_matrix.RotateVector(alpha * m_localInertia);
}
void dComplementaritySolver::dBilateralJoint::Init(dBodyState* const state0, dBodyState* const state1)
{
m_start = 0;
m_count = 0;
memset (m_rowIsMotor, 0, sizeof (m_rowIsMotor));
memset (m_motorAcceleration, 0, sizeof (m_motorAcceleration));
//memset (m_jointFeebackForce, 0, sizeof (m_jointFeebackForce));
m_state0 = state0;
m_state1 = state1;
}
void dComplementaritySolver::dBilateralJoint::AddContactRowJacobian (dParamInfo* const constraintParams, const dVector& pivot, const dVector& dir, dFloat restitution)
{
dVector r0 (pivot - m_state0->m_globalCentreOfMass);
dVector r1 (pivot - m_state1->m_globalCentreOfMass);
int index = constraintParams->m_count;
dAssert(dir.m_w == 0.0f);
dJacobian &jacobian0 = constraintParams->m_jacobians[index].m_jacobian_J01;
dJacobian &jacobian1 = constraintParams->m_jacobians[index].m_jacobian_J10;
jacobian0.m_linear = dir;
jacobian0.m_angular = r0.CrossProduct(jacobian0.m_linear);
jacobian1.m_linear = dir.Scale(-1.0f);
jacobian1.m_angular = r1.CrossProduct(jacobian1.m_linear);
const dVector& omega0 = m_state0->m_omega;
const dVector& omega1 = m_state1->m_omega;
const dVector& veloc0 = m_state0->m_veloc;
const dVector& veloc1 = m_state1->m_veloc;
const dVector veloc(jacobian0.m_linear * veloc0 + jacobian0.m_angular * omega0 + jacobian1.m_linear * veloc1 + jacobian1.m_angular * omega1);
const dVector relAccel(veloc.Scale(constraintParams->m_timestepInv * (1.0f + restitution)));
dAssert(relAccel.m_w == 0.0f);
constraintParams->m_frictionCallback[index] = NULL;
constraintParams->m_diagonalRegularizer[index] = 0.0f;
constraintParams->m_jointAccel[index] = -(relAccel.m_x + relAccel.m_y + relAccel.m_z);
constraintParams->m_jointLowFrictionCoef[index] = D_COMPLEMENTARITY_MIN_FRICTION_BOUND;
constraintParams->m_jointHighFrictionCoef[index] = D_COMPLEMENTARITY_MAX_FRICTION_BOUND;
constraintParams->m_count = index + 1;
}
void dComplementaritySolver::dBilateralJoint::AddLinearRowJacobian(dParamInfo* const constraintParams, const dVector& pivot, const dVector& dir)
{
dVector r0 (pivot - m_state0->m_globalCentreOfMass);
dVector r1 (pivot - m_state1->m_globalCentreOfMass);
int index = constraintParams->m_count;
dAssert(dir.m_w == 0.0f);
dJacobian &jacobian0 = constraintParams->m_jacobians[index].m_jacobian_J01;
dJacobian &jacobian1 = constraintParams->m_jacobians[index].m_jacobian_J10;
jacobian0.m_linear = dir;
jacobian0.m_angular = r0.CrossProduct(jacobian0.m_linear);
jacobian1.m_linear = dir.Scale(-1.0f);
jacobian1.m_angular = r1.CrossProduct(jacobian1.m_linear);
const dVector& omega0 = m_state0->m_omega;
const dVector& omega1 = m_state1->m_omega;
const dVector& veloc0 = m_state0->m_veloc;
const dVector& veloc1 = m_state1->m_veloc;
dVector centripetal0(omega0.CrossProduct(omega0.CrossProduct(r0)));
dVector centripetal1(omega1.CrossProduct(omega1.CrossProduct(r1)));
const dVector accel(jacobian0.m_linear * centripetal0 + jacobian1.m_linear * centripetal1);
const dVector veloc(jacobian0.m_linear * veloc0 + jacobian0.m_angular * omega0 + jacobian1.m_linear * veloc1 + jacobian1.m_angular * omega1);
const dVector relAccel(accel + veloc.Scale(constraintParams->m_timestepInv));
dAssert(relAccel.m_w == 0.0f);
constraintParams->m_frictionCallback[index] = NULL;
constraintParams->m_diagonalRegularizer[index] = 0.0f;
constraintParams->m_jointAccel[index] = -(relAccel.m_x + relAccel.m_y + relAccel.m_z);
constraintParams->m_jointLowFrictionCoef[index] = D_COMPLEMENTARITY_MIN_FRICTION_BOUND;
constraintParams->m_jointHighFrictionCoef[index] = D_COMPLEMENTARITY_MAX_FRICTION_BOUND;
constraintParams->m_count = index + 1;
}
void dComplementaritySolver::dBilateralJoint::AddAngularRowJacobian (dParamInfo* const constraintParams, const dVector& dir, dFloat jointAngle)
{
int index = constraintParams->m_count;
dAssert(dir.m_w == 0.0f);
dJacobian &jacobian0 = constraintParams->m_jacobians[index].m_jacobian_J01;
dJacobian &jacobian1 = constraintParams->m_jacobians[index].m_jacobian_J10;
jacobian0.m_linear = dVector(0.0f);
jacobian1.m_linear = dVector(0.0f);
jacobian0.m_angular = dir;
jacobian1.m_angular = dir.Scale (-1.0f);
const dVector& omega0 = m_state0->m_omega;
const dVector& omega1 = m_state1->m_omega;
const dVector omega (omega0 * jacobian0.m_angular + omega1 * jacobian1.m_angular);
const dVector alpha (omega.Scale (constraintParams->m_timestepInv));
constraintParams->m_frictionCallback[index] = NULL;
constraintParams->m_diagonalRegularizer[index] = 0.0f;
constraintParams->m_jointAccel[index] = -(alpha.m_x + alpha.m_y + alpha.m_z);
constraintParams->m_jointLowFrictionCoef[index] = D_COMPLEMENTARITY_MIN_FRICTION_BOUND;
constraintParams->m_jointHighFrictionCoef[index] = D_COMPLEMENTARITY_MAX_FRICTION_BOUND;
constraintParams->m_count = index + 1;
}
dFloat dComplementaritySolver::dBilateralJoint::GetRowAccelaration(dParamInfo* const constraintParams) const
{
dAssert(constraintParams->m_count > 0);
return constraintParams->m_jointAccel[constraintParams->m_count - 1];
}
void dComplementaritySolver::dBilateralJoint::SetRowAccelaration(dParamInfo* const constraintParams, dFloat accel)
{
dAssert(constraintParams->m_count > 0);
constraintParams->m_jointAccel[constraintParams->m_count - 1] = accel;
}
dFloat dComplementaritySolver::dBilateralJoint::CalculateMassMatrixDiagonal(dParamInfo* const constraintParams) const
{
/*
dAssert(constraintParams->m_count > 0);
const int index = constraintParams->m_count - 1;
const dJacobian &jacobian0 = constraintParams->m_jacobians[index].m_jacobian_J01;
const dJacobian &jacobian1 = constraintParams->m_jacobians[index].m_jacobian_J10;
const dMatrix& invInertia0 = m_state0->m_invInertia;
const dMatrix& invInertia1 = m_state1->m_invInertia;
const dFloat invMass0 = m_state0->m_invMass;
const dFloat invMass1 = m_state1->m_invMass;
const dVector JMinvIM0linear(jacobian0.m_linear.Scale(invMass0));
const dVector JMinvIM1linear(jacobian1.m_linear.Scale(invMass1));
const dVector JMinvIM0angular = invInertia0.UnrotateVector(jacobian0.m_angular);
const dVector JMinvIM1angular = invInertia1.UnrotateVector(jacobian1.m_angular);
const dVector tmpDiag(JMinvIM0linear * jacobian0.m_linear + JMinvIM0angular * jacobian0.m_angular +
JMinvIM1linear * jacobian1.m_linear + JMinvIM1angular * jacobian1.m_angular);
dFloat invEffectiveMass = (tmpDiag[0] + tmpDiag[1] + tmpDiag[2]);
springConst *= invEffectiveMass;
damperConst *= invEffectiveMass;
springConst *= 2.0f;
damperConst *= 0.5f;
const dFloat timestep = constraintParams->m_timestep;
//at = [- ks (x2 - x1) - kd * (v2 - v1) - dt * ks * (v2 - v1)] / [1 + dt * kd + dt * dt * ks]
dFloat accel = springConst * posit + damperConst * speed + timestep * springConst * speed;
// yes I know this is the correct implicit term, but made the behavior too soft, so I am fudging it
//dFloat den = dFloat(1.0f) + timestep * kd + timestep * ksd;
//dFloat den = timestep * damperConst + timestep * ksd;
dFloat den = 0.25f * timestep * (timestep * springConst + damperConst);
constraintParams->m_jointAccel[index] = - accel;
constraintParams->m_diagonalRegularizer[index] = den;
*/
dAssert(constraintParams->m_count > 0);
const int index = constraintParams->m_count - 1;
const dJacobian &jacobian0 = constraintParams->m_jacobians[index].m_jacobian_J01;
const dJacobian &jacobian1 = constraintParams->m_jacobians[index].m_jacobian_J10;
const dMatrix& invInertia0 = m_state0->m_invInertia;
const dMatrix& invInertia1 = m_state1->m_invInertia;
const dFloat invMass0 = m_state0->m_invMass;
const dFloat invMass1 = m_state1->m_invMass;
const dVector JMinvIM0linear(jacobian0.m_linear.Scale(invMass0));
const dVector JMinvIM1linear(jacobian1.m_linear.Scale(invMass1));
const dVector JMinvIM0angular = invInertia0.UnrotateVector(jacobian0.m_angular);
const dVector JMinvIM1angular = invInertia1.UnrotateVector(jacobian1.m_angular);
const dVector tmpDiag(JMinvIM0linear * jacobian0.m_linear + JMinvIM0angular * jacobian0.m_angular +
JMinvIM1linear * jacobian1.m_linear + JMinvIM1angular * jacobian1.m_angular);
return tmpDiag[0] + tmpDiag[1] + tmpDiag[2];
}
/*
dFloat dComplementaritySolver::dBilateralJoint::CalculateRowZeroAccelaration (dParamInfo* const constraintParams) const
{
const int i = constraintParams->m_count - 1;
dAssert (i >= 0);
const dVector& omega0 = m_state0->GetOmega();
const dVector& omega1 = m_state1->GetOmega();
const dVector& veloc0 = m_state0->GetVelocity();
const dVector& veloc1 = m_state1->GetVelocity();
dVector accel(constraintParams->m_jacobians[i].m_jacobian_J01.m_linear * veloc0 +
constraintParams->m_jacobians[i].m_jacobian_J01.m_angular * omega0 +
constraintParams->m_jacobians[i].m_jacobian_J10.m_linear * veloc1 +
constraintParams->m_jacobians[i].m_jacobian_J10.m_angular * omega1);
return -(accel.m_x + accel.m_y + accel.m_z) * constraintParams->m_timestepInv;
}
*/
dFloat dComplementaritySolver::dBilateralJoint::CalculateAngle (const dVector& planeDir, const dVector& cosDir, const dVector& sinDir) const
{
dFloat cosAngle = planeDir.DotProduct3(cosDir);
dFloat sinAngle = sinDir.DotProduct3(planeDir.CrossProduct(cosDir));
return dAtan2(sinAngle, cosAngle);
}
/*
void dComplementaritySolver::dBilateralJoint::AddAngularRowJacobian (dParamInfo* const constraintParams, const dVector& dir0, const dVector& dir1, dFloat accelerationRatio)
{
int index = constraintParams->m_count;
dJacobian &jacobian0 = constraintParams->m_jacobians[index].m_jacobian_IM0;
jacobian0.m_linear[0] = 0.0f;
jacobian0.m_linear[1] = 0.0f;
jacobian0.m_linear[2] = 0.0f;
jacobian0.m_linear[3] = 0.0f;
jacobian0.m_angular[0] = dir0.m_x;
jacobian0.m_angular[1] = dir0.m_y;
jacobian0.m_angular[2] = dir0.m_z;
jacobian0.m_angular[3] = 0.0f;
dJacobian &jacobian1 = constraintParams->m_jacobians[index].m_jacobian_IM1;
jacobian1.m_linear[0] = 0.0f;
jacobian1.m_linear[1] = 0.0f;
jacobian1.m_linear[2] = 0.0f;
jacobian1.m_linear[3] = 0.0f;
jacobian1.m_angular[0] = dir1.m_x;
jacobian1.m_angular[1] = dir1.m_y;
jacobian1.m_angular[2] = dir1.m_z;
jacobian1.m_angular[3] = 0.0f;
m_rowIsMotor[index] = true;
m_motorAcceleration[index] = accelerationRatio;
constraintParams->m_jointAccel[index] = 0.0f;
constraintParams->m_jointLowFriction[index] = D_COMPLEMENTARITY_MIN_FRICTION_BOUND;
constraintParams->m_jointHighFriction[index] = D_COMPLEMENTARITY_MAX_FRICTION_BOUND;
constraintParams->m_count = index + 1;
}
*/
void dComplementaritySolver::dBilateralJoint::JointAccelerations (dJointAccelerationDecriptor* const params)
{
dJacobianColum* const jacobianColElements = params->m_colMatrix;
dJacobianPair* const jacobianRowElements = params->m_rowMatrix;
const dVector& bodyVeloc0 = m_state0->m_veloc;
const dVector& bodyOmega0 = m_state0->m_omega;
const dVector& bodyVeloc1 = m_state1->m_veloc;
const dVector& bodyOmega1 = m_state1->m_omega;
dFloat timestep = params->m_timeStep;
dFloat kd = COMPLEMENTARITY_VEL_DAMP * dFloat (4.0f);
dFloat ks = COMPLEMENTARITY_POS_DAMP * dFloat (0.25f);
for (int k = 0; k < params->m_rowsCount; k ++) {
if (m_rowIsMotor[k]) {
jacobianColElements[k].m_coordenateAccel = m_motorAcceleration[k] + jacobianColElements[k].m_deltaAccel;
} else {
const dJacobianPair& Jt = jacobianRowElements[k];
dVector relVeloc (Jt.m_jacobian_J01.m_linear * bodyVeloc0 +
Jt.m_jacobian_J01.m_angular * bodyOmega0 +
Jt.m_jacobian_J10.m_linear * bodyVeloc1 +
Jt.m_jacobian_J10.m_angular * bodyOmega1);
dFloat vRel = relVeloc.m_x + relVeloc.m_y + relVeloc.m_z;
dFloat aRel = jacobianColElements[k].m_deltaAccel;
dFloat ksd = timestep * ks;
dFloat relPosit = 0.0f - vRel * timestep * params->m_firstPassCoefFlag;
dFloat num = ks * relPosit - kd * vRel - ksd * vRel;
dFloat den = dFloat (1.0f) + timestep * kd + timestep * ksd;
dFloat aRelErr = num / den;
jacobianColElements[k].m_coordenateAccel = aRelErr + aRel;
}
}
}
int dComplementaritySolver::dFrictionLessContactJoint::CompareContact (const dContact* const contactA, const dContact* const contactB, void* dommy)
{
if (contactA->m_point[0] < contactB->m_point[0]) {
return -1;
} else if (contactA->m_point[0] > contactB->m_point[0]) {
return 1;
} else {
return 0;
}
}
int dComplementaritySolver::dFrictionLessContactJoint::ReduceContacts (int count, dContact* const contacts, dFloat tol)
{
int mask[D_MAX_PLACEMENT_CONTACTS];
int index = 0;
int packContacts = 0;
dFloat window = tol;
dFloat window2 = window * window;
memset (mask, 0, size_t (count));
dSort (contacts, count, CompareContact, NULL);
dAssert (count <= D_MAX_PLACEMENT_CONTACTS);
for (int i = 0; i < count; i ++) {
if (!mask[i]) {
dFloat val = contacts[i].m_point[index] + window;
for (int j = i + 1; (j < count) && (contacts[j].m_point[index] < val) ; j ++) {
if (!mask[j]) {
dVector dp (contacts[j].m_point - contacts[i].m_point);
dFloat dist2 = dp.DotProduct3(dp);
if (dist2 < window2) {
mask[j] = 1;
packContacts = 1;
}
}
}
}
}
if (packContacts) {
int j = 0;
for (int i = 0; i < count; i ++) {
dAssert (i < D_MAX_PLACEMENT_CONTACTS);
if (!mask[i]) {
contacts[j] = contacts[i];
j ++;
}
}
count = j;
}
return count;
}
void dComplementaritySolver::dFrictionLessContactJoint::SetContacts (int count, dContact* const contacts, dFloat restitution)
{
dFloat tol = 5.0e-3f;
count = ReduceContacts(count, contacts, tol);
while (count > D_MAX_PRAM_INFO_SIZE) {
tol *= 2.0f;
count = ReduceContacts(count, contacts, tol);
}
m_count = count;
m_restitution = restitution;
memcpy (m_contacts, contacts, count * sizeof (dContact));
}
void dComplementaritySolver::dFrictionLessContactJoint::JacobianDerivative (dParamInfo* const constraintParams)
{
for (int i = 0; i < m_count; i ++) {
AddLinearRowJacobian(constraintParams, m_contacts[i].m_point, m_contacts[i].m_point);
dAssert(0);
/*
dVector velocError (pointData.m_veloc1 - pointData.m_veloc0);
//dFloat restitution = 0.05f;
dFloat relVelocErr = velocError.DotProduct3(m_contacts[i].m_normal);
dFloat penetration = 0.0f;
dFloat penetrationStiffness = 0.0f;
dFloat penetrationVeloc = penetration * penetrationStiffness;
if (relVelocErr > dFloat(1.0e-3f)) {
relVelocErr *= (m_restitution + dFloat (1.0f));
}
constraintParams->m_normalIndex[i] = 0;
constraintParams->m_frictionCallback[index] = NULL;
constraintParams->m_jointLowFrictionCoef[i] = dFloat (0.0f);
constraintParams->m_jointAccel[i] = dMax (dFloat (-4.0f), relVelocErr + penetrationVeloc) * constraintParams->m_timestepInv;
*/
}
}
void dComplementaritySolver::dFrictionLessContactJoint::JointAccelerations (dJointAccelerationDecriptor* const params)
{
dJacobianPair* const rowMatrix = params->m_rowMatrix;
dJacobianColum* const jacobianColElements = params->m_colMatrix;
const dVector& bodyVeloc0 = m_state0->GetVelocity();
const dVector& bodyOmega0 = m_state0->GetOmega();
const dVector& bodyVeloc1 = m_state1->GetVelocity();
const dVector& bodyOmega1 = m_state1->GetOmega();
int count = params->m_rowsCount;
dAssert (params->m_timeStep > dFloat (0.0f));
for (int k = 0; k < count; k ++) {
const dJacobianPair& Jt = rowMatrix[k];
dJacobianColum& element = jacobianColElements[k];
dVector relVeloc (Jt.m_jacobian_J01.m_linear * bodyVeloc0 + Jt.m_jacobian_J01.m_angular * bodyOmega0 + Jt.m_jacobian_J10.m_linear * bodyVeloc1 + Jt.m_jacobian_J10.m_angular * bodyOmega1);
dFloat vRel = relVeloc.m_x + relVeloc.m_y + relVeloc.m_z;
dFloat aRel = element.m_deltaAccel;
//dFloat restitution = (vRel <= 0.0f) ? 1.05f : 1.0f;
dFloat restitution = (vRel <= 0.0f) ? (dFloat (1.0f) + m_restitution) : dFloat(1.0f);
vRel *= restitution;
vRel = dMin (dFloat (4.0f), vRel);
element.m_coordenateAccel = (aRel - vRel * params->m_invTimeStep);
}
}
int dComplementaritySolver::BuildJacobianMatrix (int jointCount, dBilateralJoint** const jointArray, dFloat timestep, dJacobianPair* const jacobianArray, dJacobianColum* const jacobianColumnArray, int maxRowCount)
{
int rowCount = 0;
dParamInfo constraintParams;
constraintParams.m_timestep = timestep;
constraintParams.m_timestepInv = 1.0f / timestep;
// calculate Jacobian derivative for each active joint
for (int j = 0; j < jointCount; j ++) {
dBilateralJoint* const joint = jointArray[j];
constraintParams.m_count = 0;
joint->JacobianDerivative (&constraintParams);
int dofCount = constraintParams.m_count;
joint->m_count = dofCount;
joint->m_start = rowCount;
// complete the derivative matrix for this joint
int index = joint->m_start;
dBodyState* const state0 = joint->m_state0;
dBodyState* const state1 = joint->m_state1;
const dMatrix& invInertia0 = state0->m_invInertia;
const dMatrix& invInertia1 = state1->m_invInertia;
dFloat invMass0 = state0->m_invMass;
dFloat invMass1 = state1->m_invMass;
dFloat weight = 0.9f;
for (int i = 0; i < dofCount; i ++) {
dJacobianPair* const row = &jacobianArray[index];
dJacobianColum* const col = &jacobianColumnArray[index];
jacobianArray[rowCount] = constraintParams.m_jacobians[i];
dVector JMinvIM0linear (row->m_jacobian_J01.m_linear.Scale (invMass0));
dVector JMinvIM1linear (row->m_jacobian_J10.m_linear.Scale (invMass1));
dVector JMinvIM0angular = invInertia0.UnrotateVector(row->m_jacobian_J01.m_angular);
dVector JMinvIM1angular = invInertia1.UnrotateVector(row->m_jacobian_J10.m_angular);
dVector tmpDiag (JMinvIM0linear * row->m_jacobian_J01.m_linear + JMinvIM0angular * row->m_jacobian_J01.m_angular + JMinvIM1linear * row->m_jacobian_J10.m_linear + JMinvIM1angular * row->m_jacobian_J10.m_angular);
dVector tmpAccel (JMinvIM0linear * state0->m_externalForce + JMinvIM0angular * state0->m_externalTorque + JMinvIM1linear * state1->m_externalForce + JMinvIM1angular * state1->m_externalTorque);
dFloat extenalAcceleration = -(tmpAccel[0] + tmpAccel[1] + tmpAccel[2]);
col->m_diagDamp = 1.0f;
col->m_coordenateAccel = constraintParams.m_jointAccel[i];
col->m_normalIndex = constraintParams.m_normalIndex[i];
col->m_frictionCallback = constraintParams.m_frictionCallback[i];
col->m_jointLowFriction = constraintParams.m_jointLowFrictionCoef[i];
col->m_jointHighFriction = constraintParams.m_jointHighFrictionCoef[i];
col->m_deltaAccel = extenalAcceleration;
col->m_coordenateAccel += extenalAcceleration;
col->m_force = joint->m_jointFeebackForce[i] * weight;
dFloat stiffness = COMPLEMENTARITY_PSD_DAMP_TOL * col->m_diagDamp;
dFloat diag = (tmpDiag[0] + tmpDiag[1] + tmpDiag[2]);
dAssert (diag > dFloat (0.0f));
col->m_diagDamp = diag * stiffness;
diag *= (dFloat(1.0f) + stiffness);
col->m_invDJMinvJt = dFloat(1.0f) / diag;
index ++;
rowCount ++;
dAssert (rowCount < maxRowCount);
}
}
return rowCount;
}
void dComplementaritySolver::CalculateReactionsForces (int bodyCount, dBodyState** const bodyArray, int jointCount, dBilateralJoint** const jointArray, dFloat timestepSrc, dJacobianPair* const jacobianArray, dJacobianColum* const jacobianColumnArray)
{
dJacobian stateVeloc[COMPLEMENTARITY_STACK_ENTRIES];
dJacobian internalForces [COMPLEMENTARITY_STACK_ENTRIES];
int stateIndex = 0;
dVector zero(dFloat (0.0f));
for (int i = 0; i < bodyCount; i ++) {
dBodyState* const state = bodyArray[i];
stateVeloc[stateIndex].m_linear = state->m_veloc;
stateVeloc[stateIndex].m_angular = state->m_omega;
internalForces[stateIndex].m_linear = zero;
internalForces[stateIndex].m_angular = zero;
state->m_myIndex = stateIndex;
stateIndex ++;
dAssert (stateIndex < int (sizeof (stateVeloc)/sizeof (stateVeloc[0])));
}
for (int i = 0; i < jointCount; i ++) {
dJacobian y0;
dJacobian y1;
y0.m_linear = zero;
y0.m_angular = zero;
y1.m_linear = zero;
y1.m_angular = zero;
dBilateralJoint* const constraint = jointArray[i];
int first = constraint->m_start;
int count = constraint->m_count;
for (int j = 0; j < count; j ++) {
dJacobianPair* const row = &jacobianArray[j + first];
const dJacobianColum* const col = &jacobianColumnArray[j + first];
dFloat val = col->m_force;
y0.m_linear += row->m_jacobian_J01.m_linear.Scale(val);
y0.m_angular += row->m_jacobian_J01.m_angular.Scale(val);
y1.m_linear += row->m_jacobian_J10.m_linear.Scale(val);
y1.m_angular += row->m_jacobian_J10.m_angular.Scale(val);
}
int m0 = constraint->m_state0->m_myIndex;
int m1 = constraint->m_state1->m_myIndex;
internalForces[m0].m_linear += y0.m_linear;
internalForces[m0].m_angular += y0.m_angular;
internalForces[m1].m_linear += y1.m_linear;
internalForces[m1].m_angular += y1.m_angular;
}
dFloat invTimestepSrc = dFloat (1.0f) / timestepSrc;
dFloat invStep = dFloat (0.25f);
dFloat timestep = timestepSrc * invStep;
dFloat invTimestep = invTimestepSrc * dFloat (4.0f);
int maxPasses = 5;
dFloat firstPassCoef = dFloat (0.0f);
dFloat maxAccNorm = dFloat (1.0e-2f);
for (int step = 0; step < 4; step ++) {
dJointAccelerationDecriptor joindDesc;
joindDesc.m_timeStep = timestep;
joindDesc.m_invTimeStep = invTimestep;
joindDesc.m_firstPassCoefFlag = firstPassCoef;
for (int i = 0; i < jointCount; i ++) {
dBilateralJoint* const constraint = jointArray[i];
joindDesc.m_rowsCount = constraint->m_count;
joindDesc.m_rowMatrix = &jacobianArray[constraint->m_start];
joindDesc.m_colMatrix = &jacobianColumnArray[constraint->m_start];
constraint->JointAccelerations (&joindDesc);
}
firstPassCoef = dFloat (1.0f);
dFloat accNorm = dFloat (1.0e10f);
for (int passes = 0; (passes < maxPasses) && (accNorm > maxAccNorm); passes ++) {
accNorm = dFloat (0.0f);
for (int i = 0; i < jointCount; i ++) {
dBilateralJoint* const constraint = jointArray[i];
int index = constraint->m_start;
int rowsCount = constraint->m_count;
int m0 = constraint->m_state0->m_myIndex;
int m1 = constraint->m_state1->m_myIndex;
dVector linearM0 (internalForces[m0].m_linear);
dVector angularM0 (internalForces[m0].m_angular);
dVector linearM1 (internalForces[m1].m_linear);
dVector angularM1 (internalForces[m1].m_angular);
dBodyState* const state0 = constraint->m_state0;
dBodyState* const state1 = constraint->m_state1;
const dMatrix& invInertia0 = state0->m_invInertia;
const dMatrix& invInertia1 = state1->m_invInertia;
dFloat invMass0 = state0->m_invMass;
dFloat invMass1 = state1->m_invMass;
for (int k = 0; k < rowsCount; k ++) {
dJacobianPair* const row = &jacobianArray[index];
dJacobianColum* const col = &jacobianColumnArray[index];
dVector JMinvIM0linear (row->m_jacobian_J01.m_linear.Scale (invMass0));
dVector JMinvIM1linear (row->m_jacobian_J10.m_linear.Scale (invMass1));
dVector JMinvIM0angular = invInertia0.UnrotateVector(row->m_jacobian_J01.m_angular);
dVector JMinvIM1angular = invInertia1.UnrotateVector(row->m_jacobian_J10.m_angular);
dVector acc (JMinvIM0linear * linearM0 + JMinvIM0angular * angularM0 + JMinvIM1linear * linearM1 + JMinvIM1angular * angularM1);
dFloat a = col->m_coordenateAccel - acc.m_x - acc.m_y - acc.m_z - col->m_force * col->m_diagDamp;
dFloat f = col->m_force + col->m_invDJMinvJt * a;
dFloat lowerFrictionForce = col->m_jointLowFriction;
dFloat upperFrictionForce = col->m_jointHighFriction;
if (f > upperFrictionForce) {
a = dFloat (0.0f);
f = upperFrictionForce;
} else if (f < lowerFrictionForce) {
a = dFloat (0.0f);
f = lowerFrictionForce;
}
accNorm = dMax (accNorm, dAbs (a));
dFloat prevValue = f - col->m_force;
col->m_force = f;
linearM0 += row->m_jacobian_J01.m_linear.Scale (prevValue);
angularM0 += row->m_jacobian_J01.m_angular.Scale (prevValue);
linearM1 += row->m_jacobian_J10.m_linear.Scale (prevValue);
angularM1 += row->m_jacobian_J10.m_angular.Scale (prevValue);
index ++;
}
internalForces[m0].m_linear = linearM0;
internalForces[m0].m_angular = angularM0;
internalForces[m1].m_linear = linearM1;
internalForces[m1].m_angular = angularM1;
}
}
for (int i = 0; i < bodyCount; i ++) {
dBodyState* const state = bodyArray[i];
//int index = state->m_myIndex;
dAssert (state->m_myIndex == i);
dVector force (state->m_externalForce + internalForces[i].m_linear);
dVector torque (state->m_externalTorque + internalForces[i].m_angular);
state->IntegrateForce(timestep, force, torque);
}
}
for (int i = 0; i < jointCount; i ++) {
dBilateralJoint* const constraint = jointArray[i];
int first = constraint->m_start;
int count = constraint->m_count;
for (int j = 0; j < count; j ++) {
const dJacobianColum* const col = &jacobianColumnArray[j + first];
dFloat val = col->m_force;
constraint->m_jointFeebackForce[j] = val;
}
}
for (int i = 0; i < jointCount; i ++) {
dBilateralJoint* const constraint = jointArray[i];
constraint->UpdateSolverForces (jacobianArray);
}
for (int i = 0; i < bodyCount; i ++) {
dBodyState* const state = bodyArray[i];
dAssert (state->m_myIndex == i);
state->ApplyNetForceAndTorque (invTimestepSrc, stateVeloc[i].m_linear, stateVeloc[i].m_angular);
}
}

294
thirdparty/src/newton/dMath/dLinearAlgebra.h vendored Normal file
View File

@@ -0,0 +1,294 @@
/* Copyright (c) <2003-2019> <Newton Game Dynamics>
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely
*/
#include "dStdAfxMath.h"
#include "dMathDefines.h"
#include "dVector.h"
#include "dMatrix.h"
#include "dQuaternion.h"
#ifndef __D_LINEAR_ALGEBRA_H__
#define __D_LINEAR_ALGEBRA_H__
#ifdef _MSC_VER
#pragma warning (disable: 4100) //unreferenced formal parameter
#endif
#define D_MAX_PRAM_INFO_SIZE 16
#define D_MAX_PLACEMENT_CONTACTS 128
#define D_COMPLEMENTARITY_MAX_FRICTION_BOUND dFloat(1.0e15f)
#define D_COMPLEMENTARITY_MIN_FRICTION_BOUND (-D_COMPLEMENTARITY_MAX_FRICTION_BOUND)
class dSymmetricBiconjugateGradientSolve
{
public:
dSymmetricBiconjugateGradientSolve () {}
virtual ~dSymmetricBiconjugateGradientSolve () {}
virtual dFloat64 Solve (int size, dFloat64 tolerance, dFloat64* const x, const dFloat64* const b) const;
protected:
virtual void MatrixTimeVector (dFloat64* const out, const dFloat64* const v) const = 0;
virtual bool InversePrecoditionerTimeVector (dFloat64* const out, const dFloat64* const v) const = 0;
private:
dFloat64 DotProduct (int size, const dFloat64* const b, const dFloat64* const c) const;
void ScaleAdd (int size, dFloat64* const a, const dFloat64* const b, dFloat64 scale, const dFloat64* const c) const;
void Sub (int size, dFloat64* const a, const dFloat64* const b, const dFloat64* const c) const;
};
class dComplementaritySolver
{
public:
class dBodyState;
class dBilateralJoint;
class dContact
{
public:
dContact()
:m_point(0.0f)
,m_normal(0.0f)
{
}
dVector m_point;
dVector m_normal;
};
class dJacobian
{
public:
dJacobian ()
:m_linear(0.0f)
,m_angular(0.0f)
{
}
dJacobian (const dVector& linear, const dVector& angular)
:m_linear(linear)
,m_angular(angular)
{
}
dVector m_linear;
dVector m_angular;
};
class dJacobianPair
{
public:
dJacobian m_jacobian_J01;
dJacobian m_jacobian_J10;
};
class dJacobianColum
{
public:
dFloat m_force;
dFloat m_diagDamp;
dFloat m_deltaAccel;
dFloat m_invDJMinvJt;
dFloat m_coordenateAccel;
dFloat m_jointLowFriction;
dFloat m_jointHighFriction;
dBilateralJoint* m_frictionCallback;
int m_normalIndex;
};
class dParamInfo
{
public:
dJacobianPair m_jacobians[D_MAX_PRAM_INFO_SIZE];
dFloat m_jointAccel[D_MAX_PRAM_INFO_SIZE];
dFloat m_jointLowFrictionCoef[D_MAX_PRAM_INFO_SIZE];
dFloat m_jointHighFrictionCoef[D_MAX_PRAM_INFO_SIZE];
dFloat m_diagonalRegularizer[D_MAX_PRAM_INFO_SIZE];
int m_normalIndex[D_MAX_PRAM_INFO_SIZE];
dBilateralJoint* m_frictionCallback[D_MAX_PRAM_INFO_SIZE];
dFloat m_timestep;
dFloat m_timestepInv;
int m_count;
};
class dJointAccelerationDecriptor
{
public:
int m_rowsCount;
dFloat m_timeStep;
dFloat m_invTimeStep;
dFloat m_firstPassCoefFlag;
dJacobianPair* m_rowMatrix;
dJacobianColum* m_colMatrix;
};
class dBilateralJoint
{
public:
dBilateralJoint()
:m_state0(NULL)
,m_state1(NULL)
,m_ordinals(0x050403020100ll)
,m_start(0)
,m_count(0)
,m_dof(0)
{
memset(m_rowIsMotor, 0, sizeof (m_rowIsMotor));
memset(m_motorAcceleration, 0, sizeof (m_motorAcceleration));
memset(m_jointFeebackForce, 0, sizeof (m_jointFeebackForce));
}
virtual ~dBilateralJoint(){}
virtual void Init (dBodyState* const state0, dBodyState* const state1);
virtual void JacobianDerivative (dParamInfo* const constraintParams) = 0;
virtual void UpdateSolverForces (const dJacobianPair* const jacobians) const = 0;
virtual void JointAccelerations (dJointAccelerationDecriptor* const accelParam);
virtual void SpecialSolverFrictionCallback(const dFloat* const force, dFloat* const lowFriction, dFloat* const highFriction) const {}
void AddAngularRowJacobian (dParamInfo* const constraintParams, const dVector& dir, dFloat jointAngle);
void AddLinearRowJacobian (dParamInfo* const constraintParams, const dVector& pivot, const dVector& dir);
void AddContactRowJacobian (dParamInfo* const constraintParams, const dVector& pivot, const dVector& dir, dFloat restitution);
dFloat GetRowAccelaration(dParamInfo* const constraintParams) const;
void SetRowAccelaration(dParamInfo* const constraintParams, dFloat accel);
dFloat CalculateMassMatrixDiagonal(dParamInfo* const constraintParams) const;
dFloat CalculateAngle (const dVector& planeDir, const dVector& cosDir, const dVector& sinDir) const;
dFloat m_motorAcceleration[D_MAX_PRAM_INFO_SIZE];
dFloat m_jointFeebackForce[D_MAX_PRAM_INFO_SIZE];
int m_rowIsMotor[D_MAX_PRAM_INFO_SIZE];
dBodyState* m_state0;
dBodyState* m_state1;
union
{
long long m_ordinals;
char m_sourceJacobianIndex[8];
};
int m_start;
int m_count;
int m_dof;
friend class dBodyState;
friend class dComplementaritySolver;
};
class dFrictionLessContactJoint: public dBilateralJoint
{
public:
dFrictionLessContactJoint()
:dBilateralJoint()
,m_restitution(0.0f)
,m_count (0)
{}
virtual ~dFrictionLessContactJoint(){}
void SetContacts (int count, dContact* const contacts, dFloat restitution);
protected:
void UpdateSolverForces (const dJacobianPair* const jacobians) const {}
static inline int CompareContact (const dContact* const contactA, const dContact* const contactB, void* dommy);
int ReduceContacts (int count, dContact* const contacts, dFloat tol);
void JacobianDerivative (dParamInfo* const constraintParams);
void JointAccelerations (dJointAccelerationDecriptor* const params);
dContact m_contacts[D_MAX_PRAM_INFO_SIZE];
dFloat m_restitution;
int m_count;
};
class dBodyState
{
public:
dBodyState();
virtual ~dBodyState() {}
dFloat GetMass () const;
void SetMass (dFloat mass);
dFloat GetInvMass () const;
void SetInertia (dFloat Ixx, dFloat Iyy, dFloat Izz);
void GetInertia (dFloat& Ixx, dFloat& Iyy, dFloat& Izz) const;
const dMatrix& GetInertia() const;
const dMatrix& GetInvInertia() const;
void SetVeloc (const dVector& veloc);
void SetOmega (const dVector& omega);
const dVector& GetOmega() const;
const dVector& GetVelocity() const;
dVector CalculatePointVelocity (const dVector& point) const;
void UpdateInertia();
void SetMatrix (const dMatrix& matrix);
const dMatrix& GetMatrix () const;
void SetLocalMatrix (const dMatrix& matrix);
const dMatrix& GetLocalMatrix () const;
void SetForce (const dVector& force);
void SetTorque (const dVector& torque);
const dVector& GetForce () const;
const dVector& GetTorque () const;
const dVector& GetCOM () const;
void IntegrateVelocity (dFloat timestep);
void IntegrateForce (dFloat timestep, const dVector& force, const dVector& torque);
protected:
virtual void ApplyNetForceAndTorque (dFloat invTimestep, const dVector& veloc, const dVector& omega);
dMatrix m_matrix;
dMatrix m_localFrame;
dMatrix m_inertia;
dMatrix m_invInertia;
dVector m_localInertia;
dVector m_localInvInertia;
dVector m_veloc;
dVector m_omega;
dVector m_externalForce;
dVector m_externalTorque;
dVector m_globalCentreOfMass;
dFloat m_mass;
dFloat m_invMass;
int m_myIndex;
friend class dBilateralJoint;
friend class dComplementaritySolver;
};
public:
dComplementaritySolver() {};
virtual ~dComplementaritySolver() {};
virtual int GetActiveJoints (dBilateralJoint** const jointArray, int bufferSize)
{
return 0;
}
virtual int BuildJacobianMatrix (int jointCount, dBilateralJoint** const jointArray, dFloat timestep, dJacobianPair* const jacobianArray, dJacobianColum* const jacobianColumnArray, int maxRowCount);
virtual void CalculateReactionsForces (int bodyCount, dBodyState** const bodyArray, int jointCount, dBilateralJoint** const jointArray, dFloat timestep, dJacobianPair* const jacobianArray, dJacobianColum* const jacobianColumnArray);
};
#endif

15
thirdparty/src/newton/dMath/dMathDefines.cpp vendored Normal file
View File

@@ -0,0 +1,15 @@
/* Copyright (c) <2003-2019> <Newton Game Dynamics>
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely
*/
#include "dStdAfxMath.h"
#include "dMathDefines.h"

925
thirdparty/src/newton/dMath/dMathDefines.h vendored Normal file
View File

@@ -0,0 +1,925 @@
/* Copyright (c) <2003-2019> <Newton Game Dynamics>
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely
*/
#ifndef __dMathDefined__
#define __dMathDefined__
#include <math.h>
#include <float.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <assert.h>
#ifdef _MSC_VER
#include <malloc.h>
#include <windows.h>
#ifdef _DEBUG
#include <stdarg.h>
inline void dExpandTraceMessage (const char* const fmt, ...)
{
va_list v_args;
char text[4096];
text[0] = 0;
va_start (v_args, fmt);
vsprintf_s(text, fmt, v_args);
va_end (v_args);
OutputDebugStringA (text);
}
#define dTrace(x) \
{ \
dExpandTraceMessage x; \
}
#else
#define dTrace(x)
#endif
#else
#define dTrace(x)
#endif
#if ( defined (_MSC_VER) || defined (__MINGW32__) || defined (__MINGW64__) )
#include <crtdbg.h>
#define dAssert(x) _ASSERTE(x)
#else
#define dAssert(x) assert(x)
#endif
#ifndef dFloat
#ifdef _NEWTON_USE_DOUBLE
typedef double dFloat;
#else
typedef float dFloat;
#endif
#endif
#ifndef dFloat64
typedef double dFloat64;
#endif
// some constants
#define dPi dFloat (3.141592f)
#define dRadToDegree (dFloat (180.0f) / dPi)
#define dDegreeToRad (dFloat (1.0f) / dRadToDegree)
// transcendental functions
#define dSqrt(x) dFloat (sqrt (dFloat(x)))
#define dCiel(x) dFloat (ceil (dFloat(x)))
#define dFloor(x) dFloat (floor (dFloat(x)))
#define dLog(x) dFloat (log (dFloat(x)))
#define dPow(x,y) dFloat (pow (dFloat(x), dFloat(y)))
#define dSin(x) dFloat (sin (dFloat(x)))
#define dCos(x) dFloat (cos (dFloat(x)))
#define dTan(x) dFloat (tan (dFloat(x)))
#define dAsin(x) dFloat (asin (dFloat(x)))
#define dAcos(x) dFloat (acos (dFloat(x)))
#define dAtan(x) dFloat (atan (dFloat(x)))
#define dAtan2(x,y) dFloat (atan2 (dFloat(x), dFloat(y)))
#ifdef D_PROFILER
#include <dProfiler.h>
#define D_TRACKTIME() dProfilerZoneScoped(__FUNCTION__)
#define D_SET_TRACK_NAME(trackName) dProfilerSetTrackName(trackName)
#else
#define D_TRACKTIME()
#define D_SET_TRACK_NAME(trackName)
#endif
#define D_MSC_VECTOR_ALIGNMENT
enum dEulerAngleOrder
{
m_pitchYawRoll,
m_pitchRollYaw,
PYR = (0 << 8) + (1 << 4) + (2 << 0),
PRY = (0 << 8) + (2 << 4) + (1 << 0),
YPR = (1 << 8) + (0 << 4) + (2 << 0),
YRP = (1 << 8) + (2 << 4) + (0 << 0),
RYP = (2 << 8) + (1 << 4) + (0 << 0),
RPY = (2 << 8) + (0 << 4) + (1 << 0),
};
#define dAlloca(type,size) (type*) alloca ((size) * sizeof (type))
template <class T>
T dAbs(T A)
{
// this is far faster than the standard function (does not mess with cpu rounding mode)
return (A >= T(0.0f)) ? A : -A;
}
template <class T>
T dSign (T A)
{
return (A >= T(0)) ? T(1) : T(-1);
}
template <class T>
void dSwap(T& A, T& B)
{
T tmp (A);
A = B;
B = tmp;
}
template <class T>
T dMax(T A, T B)
{
return (A > B) ? A : B;
}
template <class T>
T dMin(T A, T B)
{
return (A < B) ? A : B;
}
template <class T>
T dClamp(T val, T min, T max)
{
return dMax (min, dMin (max, val));
}
template <class T>
T dMod(T val, T mod)
{
return T (fmod (T(val), T(mod)));
}
template <class T>
void dSort (T* const array, int elements, int (*compare) (const T* const A, const T* const B, void* const context), void* const context = NULL)
{
int stride = 8;
int stack[1024][2];
stack[0][0] = 0;
stack[0][1] = elements - 1;
int stackIndex = 1;
while (stackIndex) {
stackIndex --;
int lo = stack[stackIndex][0];
int hi = stack[stackIndex][1];
if ((hi - lo) > stride) {
int i = lo;
int j = hi;
T val (array[(lo + hi) >> 1]);
do {
while (compare (&array[i], &val, context) < 0) i ++;
while (compare (&array[j], &val, context) > 0) j --;
if (i <= j) {
dSwap(array[i], array[j]);
i++;
j--;
}
} while (i <= j);
if (i < hi) {
stack[stackIndex][0] = i;
stack[stackIndex][1] = hi;
stackIndex ++;
}
if (lo < j) {
stack[stackIndex][0] = lo;
stack[stackIndex][1] = j;
stackIndex ++;
}
dAssert (stackIndex < int (sizeof (stack) / (2 * sizeof (stack[0][0]))));
}
}
stride = stride * 2;
if (elements < stride) {
stride = elements;
}
for (int i = 1; i < stride; i ++) {
if (compare (&array[0], &array[i], context) > 0) {
dSwap(array[0], array[i]);
}
}
for (int i = 1; i < elements; i ++) {
int j = i;
T tmp (array[i]);
for (; compare (&array[j - 1], &tmp, context) > 0; j --) {
dAssert (j > 0);
array[j] = array[j - 1];
}
array[j] = tmp;
}
#ifdef _DEBUG
for (int i = 0; i < (elements - 1); i ++) {
dAssert (compare (&array[i], &array[i + 1], context) <= 0);
}
#endif
}
// return dot product
template<class T>
T dDotProduct(int size, const T* const A, const T* const B)
{
T val(0.0f);
for (int i = 0; i < size; i++) {
val = val + A[i] * B[i];
}
return val;
}
template<class T>
void dMatrixTimeVector(int size, const T* const matrix, const T* const v, T* const out)
{
int stride = 0;
for (int i = 0; i < size; i++) {
out[i] = dDotProduct(size, &matrix[stride], v);
stride += size;
}
}
template<class T>
bool dSolveGaussian(int size, T* const matrix, T* const b)
{
for (int i = 0; i < size - 1; i++) {
const T* const rowI = &matrix[i * size];
int m = i;
T maxVal(dAbs(rowI[i]));
for (int j = i + 1; j < size - 1; j++) {
T val(dAbs(matrix[size * j + i]));
if (val > maxVal) {
m = j;
maxVal = val;
}
}
if (maxVal < T(1.0e-12f)) {
return false;
}
if (m != i) {
T* const rowK = &matrix[m * size];
T* const rowJ = &matrix[i * size];
for (int j = 0; j < size; j++) {
dSwap(rowK[j], rowJ[j]);
}
dSwap(b[i], b[m]);
}
T den = T(1.0f) / rowI[i];
for (int k = i + 1; k < size; k++) {
T* const rowK = &matrix[size * k];
T factor(-rowK[i] * den);
for (int j = i + 1; j < size; j++) {
rowK[j] += rowI[j] * factor;
}
rowK[i] = T(0.0f);
b[k] += b[i] * factor;
}
}
for (int i = size - 1; i >= 0; i--) {
T acc(0);
T* const rowI = &matrix[i * size];
for (int j = i + 1; j < size; j++) {
acc = acc + rowI[j] * b[j];
}
b[i] = (b[i] - acc) / rowI[i];
}
return true;
}
template<class T>
void dCholeskySolve(int size, int stride, const T* const choleskyMatrix, T* const x)
{
int rowStart = 0;
for (int i = 0; i < size; i++) {
T acc(0.0f);
const T* const row = &choleskyMatrix[rowStart];
for (int j = 0; j < i; j++) {
acc = acc + row[j] * x[j];
}
x[i] = (x[i] - acc) / row[i];
rowStart += stride;
}
for (int i = size - 1; i >= 0; i--) {
T acc = 0.0f;
for (int j = i + 1; j < size; j++) {
acc = acc + choleskyMatrix[stride * j + i] * x[j];
}
x[i] = (x[i] - acc) / choleskyMatrix[stride * i + i];
}
}
template<class T>
bool dCholeskyFactorization(int size, int stride, T* const matrix)
{
T* const invDiagonal = dAlloca(T, size);
for (int i = 0; i < size; i++) {
T* const rowN = &matrix[stride * i];
int rowStart = 0;
for (int j = 0; j <= i; j++) {
T s(0.0f);
T* const rowJ = &matrix[rowStart];
for (int k = 0; k < j; k++) {
s += rowN[k] * rowJ[k];
}
if (i == j) {
T diag = rowN[i] - s;
if (diag < T(1.0e-8f)) {
return false;
}
rowN[i] = T(sqrt(diag));
invDiagonal[i] = T (1.0f) / rowN[i];
} else {
//rowN[j] = ((T(1.0f) / rowJ[j]) * (rowN[j] - s));
rowN[j] = invDiagonal[j] * (rowN[j] - s);
}
rowStart += stride;
}
}
return true;
}
template<class T>
bool dTestPSDmatrix(int size, int stride, const T* const matrix)
{
T* const copy = dAlloca(T, size * size);
int row = 0;
for (int i = 0; i < size; i ++)
{
memcpy (&copy[i * size], &matrix[row], size * sizeof (T));
row += stride;
}
return dCholeskyFactorization(size, size, copy);
}
template<class T>
void dPermuteRows(int size, int i, int j, T* const matrix, T* const choleskyMatrix, T* const x, T* const r, T* const low, T* const high, int* const permute)
{
if (i != j) {
T* const matrixRowA = &matrix[size * i];
T* const matrixRowB = &matrix[size * j];
T* const choleskyRowA = &choleskyMatrix[size * i];
T* const choleskyRowB = &choleskyMatrix[size * j];
for (int k = 0; k < size; k++) {
dSwap(matrixRowA[k], matrixRowB[k]);
dSwap(choleskyRowA[k], choleskyRowB[k]);
}
int stride = 0;
for (int k = 0; k < size; k++) {
dSwap(matrix[stride + i], matrix[stride + j]);
stride += size;
}
dSwap(x[i], x[j]);
dSwap(r[i], r[j]);
dSwap(low[i], low[j]);
dSwap(high[i], high[j]);
dSwap(permute[i], permute[j]);
}
}
template<class T>
void dCholeskyUpdate(int size, int row, int colum, T* const choleskyMatrix, T* const tmp, T* const reflexion)
{
if (row != colum) {
dAssert(row < colum);
for (int i = row; i < size; i++) {
T* const rowI = &choleskyMatrix[size * i];
T mag(T(dFloat(0.0f)));
for (int j = i + 1; j < size; j++) {
mag += rowI[j] * rowI[j];
reflexion[j] = rowI[j];
}
if (mag > T(dFloat(1.0e-9f))) {
reflexion[i] = rowI[i] - T(sqrt(mag + rowI[i] * rowI[i]));
T vMag2(mag + reflexion[i] * reflexion[i]);
T den(dFloat(1.0f) / T(sqrt(vMag2)));
for (int j = i; j < size; j++) {
reflexion[j] *= den;
}
for (int j = i; j < size; j++) {
T acc(0.0f);
T* const rowJ = &choleskyMatrix[size * j];
for (int k = i; k < size; k++) {
acc += rowJ[k] * reflexion[k];
}
tmp[j] = acc * T(dFloat(2.0f));
}
for (int j = i + 1; j < size; j++) {
T* const rowJ = &choleskyMatrix[size * j];
for (int k = i; k < size; k++) {
rowJ[k] -= tmp[j] * reflexion[k];
}
}
rowI[i] -= tmp[i] * reflexion[i];
}
for (int k = i + 1; k < size; k++) {
rowI[k] = T(dFloat(0.0f));
}
if (rowI[i] < T(dFloat(0.0f))) {
for (int k = i; k < size; k++) {
choleskyMatrix[size * k + i] = -choleskyMatrix[size * k + i];
}
}
}
for (int i = row; i < size; i++) {
choleskyMatrix[size * i + i] = dMax(choleskyMatrix[size * i + i], T(dFloat(1.0e-6f)));
}
}
}
template<class T>
void dCalculateDelta_x(int size, T dir, int n, const T* const matrix, const T* const choleskyMatrix, T* const delta_x)
{
const T* const row = &matrix[size * n];
for (int i = 0; i < n; i++) {
delta_x[i] = -row[i] * dir;
}
dCholeskySolve(size, n, choleskyMatrix, delta_x);
delta_x[n] = dir;
}
// calculate delta_r = A * delta_x
template<class T>
void dCalculateDelta_r(int size, int n, const T* const matrix, const T* const delta_x, T* const delta_r)
{
int stride = n * size;
for (int i = n; i < size; i++) {
delta_r[i] = dDotProduct(size, &matrix[stride], delta_x);
stride += size;
}
}
// solve a general Linear complementary program (LCP)
// A * x = b + r
// subjected to constraints
// x(i) = low(i), if r(i) >= 0
// x(i) = high(i), if r(i) <= 0
// low(i) <= x(i) <= high(i), if r(i) == 0
//
// return true is the system has a solution.
// in return
// x is the solution,
// r is return in vector b
// note: although the system is called LCP, the solve is far more general than a strict LCP
// to solve a strict LCP set the following
// low(i) = 0
// high(i) = infinity.
// this the same as enforcing the constrain: x(i) * r(i) = 0
template <class T>
bool dSolveDantzigLCP(int size, T* const matrix, T* const x, T* const b, T* const low, T* const high, T regularizer = T(1.e-4f))
{
T* const choleskyMatrix = dAlloca(T, size * size);
T* const x0 = dAlloca(T, size);
T* const r0 = dAlloca(T, size);
T* const delta_r = dAlloca(T, size);
T* const delta_x = dAlloca(T, size);
T* const tmp0 = dAlloca(T, size);
T* const tmp1 = dAlloca(T, size);
int* const permute = dAlloca(int, size);
memcpy(choleskyMatrix, matrix, sizeof(T) * size * size);
bool pass = dCholeskyFactorization(size, choleskyMatrix);
while (!pass) {
int stride = 0;
for (int i = 0; i < size; i ++) {
matrix[stride] += matrix[stride] * regularizer;
stride += size + 1;
}
memcpy(choleskyMatrix, matrix, sizeof(T) * size * size);
pass = dCholeskyFactorization(size, choleskyMatrix);
}
for (int i = 0; i < size; i++) {
T* const row = &choleskyMatrix[i * size];
for (int j = i + 1; j < size; j++) {
row[j] = T(0.0f);
}
}
int index = 0;
int count = size;
int clampedIndex = size;
for (int i = 0; i < size; i++) {
permute[i] = short(i);
r0[i] = -b[i];
x0[i] = dFloat(0.0f);
delta_x[i] = T(dFloat(0.0f));
delta_r[i] = T(dFloat(0.0f));
}
bool findInitialGuess = size >= 6;
if (findInitialGuess) {
int initialGuessCount = size;
for (int j = 0; (j < 5) && findInitialGuess; j++) {
findInitialGuess = false;
for (int i = 0; i < initialGuessCount; i++) {
x0[i] = -r0[i];
}
dCholeskySolve(size, initialGuessCount, choleskyMatrix, x0);
int permuteStart = initialGuessCount;
for (int i = 0; i < initialGuessCount; i++) {
T f = x0[i];
if ((f < low[i]) || (f > high[i])) {
findInitialGuess = true;
x0[i] = T(dFloat(0.0f));
dPermuteRows(size, i, initialGuessCount - 1, matrix, choleskyMatrix, x0, r0, low, high, permute);
permuteStart = dMin(permuteStart, i);
i--;
initialGuessCount--;
}
}
if (findInitialGuess) {
dCholeskyUpdate(size, permuteStart, size - 1, choleskyMatrix, tmp0, tmp1);
}
}
if (initialGuessCount == size) {
for (int i = 0; i < size; i++) {
x[i] = x0[i];
b[i] = T(dFloat(0.0f));
}
return true;
} else {
if (!findInitialGuess) {
for (int i = 0; i < initialGuessCount; i++) {
r0[i] = T(dFloat(0.0f));
}
for (int i = initialGuessCount; i < size; i++) {
r0[i] += dDotProduct(size, &matrix[i * size], x0);
}
index = initialGuessCount;
count = size - initialGuessCount;
} else {
//dAssert(0);
for (int i = 0; i < size; i++) {
x0[i] = dFloat(0.0f);
}
}
}
}
while (count) {
bool loop = true;
bool calculateDelta_x = true;
while (loop) {
loop = false;
T clamp_x(0.0f);
int swapIndex = -1;
if (dAbs(r0[index]) > T(1.0e-12f)) {
if (calculateDelta_x) {
T dir(dFloat(1.0f));
dCalculateDelta_x(size, dir, index, matrix, choleskyMatrix, delta_x);
}
calculateDelta_x = true;
dCalculateDelta_r(size, index, matrix, delta_x, delta_r);
dAssert(delta_r[index] != T(dFloat(0.0f)));
dAssert(dAbs(delta_x[index]) == T(1.0f));
delta_r[index] = (delta_r[index] == T(dFloat(0.0f))) ? T(dFloat(1.0e-12f)) : delta_r[index];
T s = -r0[index] / delta_r[index];
dAssert(dAbs(s) >= T(dFloat(0.0f)));
for (int i = 0; i <= index; i++) {
T x1 = x0[i] + s * delta_x[i];
if (x1 > high[i]) {
swapIndex = i;
clamp_x = high[i];
s = (high[i] - x0[i]) / delta_x[i];
} else if (x1 < low[i]) {
swapIndex = i;
clamp_x = low[i];
s = (low[i] - x0[i]) / delta_x[i];
}
}
dAssert(dAbs(s) >= T(dFloat(0.0f)));
for (int i = clampedIndex; (i < size) && (s > T(1.0e-12f)); i++) {
T r1 = r0[i] + s * delta_r[i];
if ((r1 * r0[i]) < T(dFloat(0.0f))) {
dAssert(dAbs(delta_r[i]) > T(dFloat(0.0f)));
T s1 = -r0[i] / delta_r[i];
dAssert(dAbs(s1) >= T(dFloat(0.0f)));
dAssert(dAbs(s1) <= dAbs(s));
if (dAbs(s1) < dAbs(s)) {
s = s1;
swapIndex = i;
}
}
}
for (int i = 0; i < size; i++) {
dAssert((x0[i] + dAbs(x0[i]) * T(dFloat(1.0e-4f))) >= low[i]);
dAssert((x0[i] - dAbs(x0[i]) * T(dFloat(1.0e-4f))) <= high[i]);
x0[i] += s * delta_x[i];
r0[i] += s * delta_r[i];
dAssert((x0[i] + dFloat(1.0f)) >= low[i]);
dAssert((x0[i] - dFloat(1.0f)) <= high[i]);
}
}
if (swapIndex == -1) {
r0[index] = T(dFloat(0.0f));
delta_r[index] = T(dFloat(0.0f));
index++;
count--;
loop = false;
} else if (swapIndex == index) {
count--;
clampedIndex--;
x0[index] = clamp_x;
dPermuteRows(size, index, clampedIndex, matrix, choleskyMatrix, x0, r0, low, high, permute);
dCholeskyUpdate(size, index, clampedIndex, choleskyMatrix, tmp0, tmp1);
loop = count ? true : false;
} else if (swapIndex > index) {
loop = true;
r0[swapIndex] = T(dFloat(0.0f));
dAssert(swapIndex < size);
dAssert(clampedIndex <= size);
if (swapIndex < clampedIndex) {
count--;
clampedIndex--;
dPermuteRows(size, clampedIndex, swapIndex, matrix, choleskyMatrix, x0, r0, low, high, permute);
dCholeskyUpdate(size, swapIndex, clampedIndex, choleskyMatrix, tmp0, tmp1);
dAssert(clampedIndex >= index);
} else {
count++;
dAssert(clampedIndex < size);
dPermuteRows(size, clampedIndex, swapIndex, matrix, choleskyMatrix, x0, r0, low, high, permute);
dCholeskyUpdate(size, clampedIndex, swapIndex, choleskyMatrix, tmp0, tmp1);
clampedIndex++;
dAssert(clampedIndex <= size);
dAssert(clampedIndex >= index);
}
calculateDelta_x = false;
} else {
dAssert(index > 0);
x0[swapIndex] = clamp_x;
delta_x[index] = T(dFloat(0.0f));
dAssert(swapIndex < index);
dPermuteRows(size, swapIndex, index - 1, matrix, choleskyMatrix, x0, r0, low, high, permute);
dPermuteRows(size, index - 1, index, matrix, choleskyMatrix, x0, r0, low, high, permute);
dPermuteRows(size, clampedIndex - 1, index, matrix, choleskyMatrix, x0, r0, low, high, permute);
dCholeskyUpdate(size, swapIndex, clampedIndex - 1, choleskyMatrix, tmp0, tmp1);
clampedIndex--;
index--;
loop = true;
}
}
}
for (int i = 0; i < size; i++) {
int j = permute[i];
x[j] = x0[i];
b[j] = r0[i];
}
return true;
}
template <class T>
bool dCholeskyWithRegularizer(int size, int block, T* const matrix, T regularizer)
{
T* const copy = dAlloca(T, size * size);
memcpy(copy, matrix, size * size * sizeof (T));
bool pass = dCholeskyFactorization(size, block, matrix);
int count = 0;
while (!pass && (count < 10)) {
int stride = 0;
for (int i = 0; i < block; i++) {
copy[stride] += copy[stride] * regularizer;
stride += size + 1;
}
memcpy(matrix, copy, sizeof(T)* size * size);
pass = dCholeskyFactorization(size, block, matrix);
}
return pass;
}
// solve a general Linear complementary program (LCP)
// A * x = b + r
// subjected to constraints
// x(i) = low(i), if r(i) >= 0
// x(i) = high(i), if r(i) <= 0
// low(i) <= x(i) <= high(i), if r(i) == 0
//
// return true is the system has a solution.
// in return
// x is the solution,
// b is zero
// note: although the system is called LCP, the solver is far more general than a strict LCP
// to solve a strict LCP, set the following
// low(i) = 0
// high(i) = infinity.
// this is the same as enforcing the constraint: x(i) * r(i) = 0
template <class T>
bool dSolvePartitionDantzigLCP(int size, T* const symmetricMatrixPSD, T* const x, T* const b, T* const low, T* const high, int unboundedSize, T regularizer = T(1.e-4f))
{
bool ret = false;
if (unboundedSize > 0) {
ret = dCholeskyWithRegularizer(size, unboundedSize, symmetricMatrixPSD, regularizer);
if (ret) {
memcpy (x, b, unboundedSize * sizeof (T));
dCholeskySolve(size, unboundedSize, symmetricMatrixPSD, x);
int base = unboundedSize * size;
for (int i = unboundedSize; i < size; i++) {
b[i] -= dDotProduct(unboundedSize, &symmetricMatrixPSD[base], x);
base += size;
}
const int boundedSize = size - unboundedSize;
T* const l = dAlloca(T, boundedSize);
T* const h = dAlloca(T, boundedSize);
T* const c = dAlloca(T, boundedSize);
T* const u = dAlloca(T, boundedSize);
T* const a11 = dAlloca(T, boundedSize * boundedSize);
T* const a10 = dAlloca(T, boundedSize * unboundedSize);
for (int i = 0; i < boundedSize; i++) {
T* const g = &a10[i * unboundedSize];
const T* const row = &symmetricMatrixPSD[(unboundedSize + i) * size];
for (int j = 0; j < unboundedSize; j++) {
g[j] = -row[j];
}
dCholeskySolve(size, unboundedSize, symmetricMatrixPSD, g);
T* const arow = &a11[i * boundedSize];
const T* const row2 = &symmetricMatrixPSD[(unboundedSize + i) * size];
arow[i] = row2[unboundedSize + i] + dDotProduct(unboundedSize, g, row2);
for (int j = i + 1; j < boundedSize; j++) {
const T* const row1 = &symmetricMatrixPSD[(unboundedSize + j) * size];
T elem = row1[unboundedSize + i] + dDotProduct(unboundedSize, g, row1);
arow[j] = elem;
a11[j * boundedSize + i] = elem;
}
u[i] = T(0.0f);
c[i] = b[i + unboundedSize];
l[i] = low[i + unboundedSize];
h[i] = high[i + unboundedSize];
}
if (dSolveDantzigLCP(boundedSize, a11, u, c, l, h, regularizer)) {
for (int i = 0; i < boundedSize; i++) {
const T s = u[i];
x[unboundedSize + i] = s;
const T* const g = &a10[i * unboundedSize];
for (int j = 0; j < unboundedSize; j++) {
x[j] += g[j] * s;
}
}
ret = true;
}
}
} else {
ret = dSolveDantzigLCP(size, symmetricMatrixPSD, x, b, low, high, regularizer);
}
return ret;
}
// solve a general Linear complementary program (LCP)
// A * x = b + r
// subjected to constraints
// x(i) = low(i), if r(i) >= 0
// x(i) = high(i), if r(i) <= 0
// low(i) <= x(i) <= high(i), if r(i) == 0
//
// return true is the system has a solution.
// in return
// x is the solution,
// r is return in vector b
// note: although the system is called LCP, the solver is far more general than a strict LCP
// to solve a strict LCP, set the following
// low(i) = 0
// high(i) = infinity.
// this the same as enforcing the constraint: x(i) * r(i) = 0
template <class T>
void dGaussSeidelLcpSor(const int size, const int stride, const T* const matrix, T* const x, const T* const b, const int* const normalIndex, const T* const low, const T* const high, T tol2, int maxIterCount, T sor)
{
const T* const me = matrix;
T* const invDiag1 = dAlloca(T, size);
T* const u = dAlloca(T, size + 1);
int* const index = dAlloca(int, size);
u[size] = T(1.0f);
int rowStart = 0;
for (int j = 0; j < size; j++) {
u[j] = x[j];
index[j] = normalIndex[j] ? j + normalIndex[j] : size;
}
for (int j = 0; j < size; j++) {
const T val = u[index[j]];
const T l = low[j] * val;
const T h = high[j] * val;
u[j] = dClamp(u[j], l, h);
invDiag1[j] = T(1.0f) / me[rowStart + j];
rowStart += stride;
}
T tolerance(tol2 * 2.0f);
const T* const invDiag = invDiag1;
const int maxCount = dMax (8, size);
for (int i = 0; (i < maxCount) && (tolerance > tol2); i++) {
int base = 0;
tolerance = T(0.0f);
for (int j = 0; j < size; j++) {
const T* const row = &me[base];
T r(b[j] - dDotProduct(size, row, u));
T f((r + row[j] * u[j]) * invDiag[j]);
const T val = u[index[j]];
const T l = low[j] * val;
const T h = high[j] * val;
if (f > h) {
u[j] = h;
} else if (f < l) {
u[j] = l;
} else {
tolerance += r * r;
u[j] = f;
}
base += stride;
}
}
#ifdef _DEBUG
int passes = 0;
#endif
for (int i = 0; (i < maxIterCount) && (tolerance > tol2); i++) {
int base = 0;
tolerance = T(0.0f);
#ifdef _DEBUG
passes++;
#endif
for (int j = 0; j < size; j++) {
const T* const row = &me[base];
T r(b[j] - dDotProduct(size, row, u));
T f((r + row[j] * u[j]) * invDiag[j]);
f = u[j] + (f - u[j]) * sor;
const T val = u[index[j]];
const T l = low[j] * val;
const T h = high[j] * val;
if (f > h) {
u[j] = h;
} else if (f < l) {
u[j] = l;
} else {
tolerance += r * r;
u[j] = f;
}
base += stride;
}
}
for (int j = 0; j < size; j++) {
x[j] = u[j];
}
}
#endif

1036
thirdparty/src/newton/dMath/dMatrix.cpp vendored Normal file
View File

File diff suppressed because it is too large Load Diff

181
thirdparty/src/newton/dMath/dMatrix.h vendored Normal file
View File

@@ -0,0 +1,181 @@
/* Copyright (c) <2003-2019> <Newton Game Dynamics>
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely
*/
#ifndef __dMatrix__
#define __dMatrix__
#include "dVector.h"
class dMatrix;
class dQuaternion;
class dQuaternion;
dMatrix dGetZeroMatrix ();
dMatrix dGetIdentityMatrix();
class dMatrix
{
public:
dMatrix ();
dMatrix (const dFloat* const array);
#ifndef _NEWTON_USE_DOUBLE
dMatrix (const dFloat64* const array);
#endif
dMatrix (const dVector &front, const dVector &up, const dVector &right, const dVector &posit);
dMatrix (const dQuaternion &rotation, const dVector &position);
dMatrix (dFloat pitch, dFloat yaw, dFloat roll, const dVector& location);
dVector& operator[] (int i);
const dVector& operator[] (int i) const;
dMatrix Inverse () const;
dMatrix Transpose () const;
dMatrix Transpose4X4 () const;
dVector RotateVector (const dVector &v) const;
dVector UnrotateVector (const dVector &v) const;
dVector TransformVector (const dVector &v) const;
dVector UntransformVector (const dVector &v) const;
dVector TransformPlane (const dVector &localPlane) const;
dVector UntransformPlane (const dVector &globalPlane) const;
void GetEulerAngles(dVector& euler1, dVector& euler2, dEulerAngleOrder order = m_pitchYawRoll) const;
bool TestIdentity() const;
bool TestOrthogonal() const;
dMatrix Inverse4x4() const;
dVector RotateVector4x4 (const dVector &v) const;
dMatrix JacobiDiagonalization (dVector& eigenValues, const dMatrix& initialMatrix = dGetIdentityMatrix()) const;
// decompose this matrix into [this = transpose(stretchAxis) * matrix(scale) * stretchAxis * transformMatrix];
void PolarDecomposition (dMatrix& transformMatrix, dVector& scale, dMatrix& stretchAxis, const dMatrix& initialStretchAxis = dGetIdentityMatrix()) const;
// constructor for polar composition
dMatrix (const dMatrix& transformMatrix, const dVector& scale, const dMatrix& stretchAxis);
void TransformTriplex (dFloat* const dst, int dstStrideInBytes, const dFloat* const src, int srcStrideInBytes, int count) const;
#ifndef _NEWTON_USE_DOUBLE
void TransformTriplex (dFloat64* const dst, int dstStrideInBytes, const dFloat64* const src, int srcStrideInBytes, int count) const;
#endif
dMatrix operator* (const dMatrix & B) const;
void Trace () const
{
dTrace (("(%ff, %ff, %ff, %ff)\n", m_front.m_x, m_front.m_y, m_front.m_z, m_front.m_w));
dTrace (("(%ff, %ff, %ff, %ff)\n", m_up.m_x, m_up.m_y, m_up.m_z, m_up.m_w));
dTrace (("(%ff, %ff, %ff, %ff)\n", m_right.m_x, m_right.m_y, m_right.m_z, m_right.m_w));
dTrace (("(%ff, %ff, %ff, %ff)\n", m_posit.m_x, m_posit.m_y, m_posit.m_z, m_posit.m_w));
}
bool SanityCheck() const;
dVector m_front;
dVector m_up;
dVector m_right;
dVector m_posit;
};
inline dMatrix::dMatrix ()
{
}
inline dMatrix::dMatrix (const dVector &front, const dVector &up, const dVector &right, const dVector &posit)
:m_front (front), m_up(up), m_right(right), m_posit(posit)
{
}
inline dMatrix::dMatrix (const dFloat* const array)
{
memcpy (&(*this)[0][0], array, sizeof (dMatrix));
}
#ifndef _NEWTON_USE_DOUBLE
inline dMatrix::dMatrix (const dFloat64* const array)
{
dFloat* const ptr = &(*this)[0][0];
for (int i = 0; i < 16; i ++) {
ptr[i] = dFloat (array[i]);
}
}
#endif
inline dVector& dMatrix::operator[] (int i)
{
return (&m_front)[i];
}
inline const dVector& dMatrix::operator[] (int i) const
{
return (&m_front)[i];
}
dMatrix dRollMatrix(dFloat ang);
dMatrix dYawMatrix(dFloat ang);
dMatrix dPitchMatrix(dFloat ang);
dMatrix dGrammSchmidt(const dVector& dir);
class dSpatialMatrix
{
public:
inline dSpatialMatrix()
{
}
inline dSpatialMatrix(dFloat val)
{
for (int i = 0; i < 6; i++) {
m_rows[i] = dSpatialVector(val);
}
}
inline dSpatialVector& operator[] (int i)
{
dAssert(i < 6);
dAssert(i >= 0);
return m_rows[i];
}
inline const dSpatialVector& operator[] (int i) const
{
dAssert(i < 6);
dAssert(i >= 0);
return m_rows[i];
}
dSpatialMatrix Inverse(int rows) const;
inline dSpatialVector VectorTimeMatrix(const dSpatialVector& jacobian) const
{
dSpatialVector tmp(m_rows[0].Scale(jacobian[0]));
for (int i = 1; i < 6; i++) {
tmp = tmp + m_rows[i].Scale(jacobian[i]);
}
return tmp;
}
inline dSpatialVector VectorTimeMatrix(const dSpatialVector& jacobian, int dof) const
{
dSpatialVector tmp(0.0f);
for (int i = 0; i < dof; i++) {
tmp = tmp + m_rows[i].Scale(jacobian[i]);
}
return tmp;
}
dSpatialVector m_rows[6];
};
#endif

206
thirdparty/src/newton/dMath/dQuaternion.cpp vendored Normal file
View File

@@ -0,0 +1,206 @@
/* Copyright (c) <2003-2019> <Newton Game Dynamics>
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely
*/
#include "dStdAfxMath.h"
#include "dMathDefines.h"
#include "dVector.h"
#include "dMatrix.h"
#include "dQuaternion.h"
enum QUAT_INDEX
{
X_INDEX = 0,
Y_INDEX = 1,
Z_INDEX = 2
};
static QUAT_INDEX QIndex[] = { Y_INDEX, Z_INDEX, X_INDEX };
dQuaternion::dQuaternion (const dMatrix &matrix)
{
dFloat trace = matrix[0][0] + matrix[1][1] + matrix[2][2];
dAssert (matrix[2].DotProduct3(matrix[0].CrossProduct(matrix[1])) > 0.0f);
if (trace > dFloat(0.0f)) {
trace = dSqrt (trace + dFloat(1.0f));
m_w = dFloat (0.5f) * trace;
trace = dFloat (0.5f) / trace;
m_x = (matrix[1][2] - matrix[2][1]) * trace;
m_y = (matrix[2][0] - matrix[0][2]) * trace;
m_z = (matrix[0][1] - matrix[1][0]) * trace;
} else {
QUAT_INDEX i = X_INDEX;
if (matrix[Y_INDEX][Y_INDEX] > matrix[X_INDEX][X_INDEX]) {
i = Y_INDEX;
}
if (matrix[Z_INDEX][Z_INDEX] > matrix[i][i]) {
i = Z_INDEX;
}
QUAT_INDEX j = QIndex [i];
QUAT_INDEX k = QIndex [j];
trace = dFloat(1.0f) + matrix[i][i] - matrix[j][j] - matrix[k][k];
trace = dSqrt (trace);
dFloat* const ptr = &m_x;
ptr[i] = dFloat (0.5f) * trace;
trace = dFloat (0.5f) / trace;
m_w = (matrix[j][k] - matrix[k][j]) * trace;
ptr[j] = (matrix[i][j] + matrix[j][i]) * trace;
ptr[k] = (matrix[i][k] + matrix[k][i]) * trace;
}
#if _DEBUG
dMatrix tmp (*this, matrix.m_posit);
dMatrix unitMatrix (tmp * matrix.Inverse());
for (int i = 0; i < 4; i ++) {
dFloat err = dAbs (unitMatrix[i][i] - dFloat(1.0f));
dAssert (err < dFloat (1.0e-3f));
}
dFloat err = dAbs (DotProduct(*this) - dFloat(1.0f));
dAssert (err < dFloat(1.0e-3f));
#endif
}
dQuaternion::dQuaternion (const dVector &unitAxis, dFloat angle)
{
angle *= dFloat (0.5f);
m_w = dCos (angle);
dFloat sinAng = dSin (angle);
#ifdef _DEBUG
if (dAbs (angle) > dFloat(1.0e-6f)) {
dAssert (dAbs (dFloat(1.0f) - unitAxis.DotProduct3(unitAxis)) < dFloat(1.0e-3f));
}
#endif
m_x = unitAxis.m_x * sinAng;
m_y = unitAxis.m_y * sinAng;
m_z = unitAxis.m_z * sinAng;
}
dVector dQuaternion::CalcAverageOmega (const dQuaternion &q1, dFloat invdt) const
{
dQuaternion q0 (*this);
if (q0.DotProduct (q1) < 0.0f) {
q0.Scale(-1.0f);
}
dQuaternion dq (q0.Inverse() * q1);
dVector omegaDir (dq.m_x, dq.m_y, dq.m_z);
dFloat dirMag2 = omegaDir.DotProduct3(omegaDir);
if (dirMag2 < dFloat(dFloat (1.0e-5f) * dFloat (1.0e-5f))) {
return dVector (dFloat(0.0f));
}
dFloat dirMagInv = dFloat (1.0f) / dSqrt (dirMag2);
dFloat dirMag = dirMag2 * dirMagInv;
dFloat omegaMag = dFloat(2.0f) * dAtan2 (dirMag, dq.m_w) * invdt;
return omegaDir.Scale (dirMagInv * omegaMag);
}
dQuaternion dQuaternion::Slerp (const dQuaternion &q1, dFloat t) const
{
dQuaternion q;
dFloat dot = DotProduct (q1);
dAssert (dot >= 0.0f);
if ((dot + dFloat(1.0f)) > dFloat(1.0e-5f)) {
dFloat Sclp;
dFloat Sclq;
if (dot < dFloat(0.995f)) {
dFloat ang = dAcos (dot);
dFloat sinAng = dSin (ang);
dFloat den = dFloat(1.0f) / sinAng;
Sclp = dSin ((dFloat(1.0f) - t ) * ang) * den;
Sclq = dSin (t * ang) * den;
} else {
Sclp = dFloat(1.0f) - t;
Sclq = t;
}
q.m_w = m_w * Sclp + q1.m_w * Sclq;
q.m_x = m_x * Sclp + q1.m_x * Sclq;
q.m_y = m_y * Sclp + q1.m_y * Sclq;
q.m_z = m_z * Sclp + q1.m_z * Sclq;
} else {
q.m_w = m_z;
q.m_x = -m_y;
q.m_y = m_x;
q.m_z = m_w;
dFloat Sclp = dSin ((dFloat(1.0f) - t) * dFloat (dPi *0.5f));
dFloat Sclq = dSin (t * dFloat (dPi * 0.5f));
q.m_w = m_w * Sclp + q.m_w * Sclq;
q.m_x = m_x * Sclp + q.m_x * Sclq;
q.m_y = m_y * Sclp + q.m_y * Sclq;
q.m_z = m_z * Sclp + q.m_z * Sclq;
}
dot = q.DotProduct (q);
if ((dot) < (1.0f - 1.0e-4f) ) {
dot = dFloat(1.0f) / dSqrt (dot);
//dot = dgRsqrt (dot);
q.m_w *= dot;
q.m_x *= dot;
q.m_y *= dot;
q.m_z *= dot;
}
return q;
}
dVector dQuaternion::RotateVector (const dVector& point) const
{
dMatrix matrix (*this, dVector (0.0f, 0.0f, 0.0f, 1.0f));
return matrix.RotateVector(point);
}
dVector dQuaternion::UnrotateVector (const dVector& point) const
{
dMatrix matrix (*this, dVector (0.0f, 0.0f, 0.0f, 1.0f));
return matrix.UnrotateVector(point);
}
void dQuaternion::GetEulerAngles(dVector& euler1, dVector& euler2, dEulerAngleOrder order) const
{
dMatrix matrix (*this, dVector (0.0f,0.0f,0.0f,1.0f));
//return matrix.GetEulerAngles (order);
matrix.GetEulerAngles (euler1, euler2, order);
}
dQuaternion dQuaternion::IntegrateOmega (const dVector& omega, dFloat timestep) const
{
// this is correct
dQuaternion rotation (*this);
dFloat omegaMag2 = omega.DotProduct3(omega);
const dFloat errAngle = 0.0125f * dDegreeToRad;
const dFloat errAngle2 = errAngle * errAngle;
if (omegaMag2 > errAngle2) {
dFloat invOmegaMag = 1.0f / dSqrt (omegaMag2);
dVector omegaAxis (omega.Scale (invOmegaMag));
dFloat omegaAngle = invOmegaMag * omegaMag2 * timestep;
dQuaternion deltaRotation (omegaAxis, omegaAngle);
rotation = rotation * deltaRotation;
rotation.Scale(1.0f / dSqrt (rotation.DotProduct (rotation)));
}
return rotation;
}

109
thirdparty/src/newton/dMath/dQuaternion.h vendored Normal file
View File

@@ -0,0 +1,109 @@
/* Copyright (c) <2003-2019> <Newton Game Dynamics>
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely
*/
#ifndef __dQuaternion__
#define __dQuaternion__
#include "dVector.h"
class dMatrix;
D_MSC_VECTOR_ALIGNMENT
class dQuaternion
{
public:
dQuaternion ();
dQuaternion (const dMatrix& matrix);
//dQuaternion (dFloat w, dFloat x, dFloat y, dFloat z);
dQuaternion (dFloat x, dFloat y, dFloat z, dFloat w);
dQuaternion (const dVector& unit_Axis, dFloat Angle = 0.0f);
void Scale (dFloat scale);
void Normalize ();
inline dFloat DotProduct (const dQuaternion& q) const;
dQuaternion Inverse () const;
dVector RotateVector (const dVector& point) const;
dVector UnrotateVector (const dVector& point) const;
void GetEulerAngles(dVector& euler1, dVector& euler2, dEulerAngleOrder order = m_pitchYawRoll) const;
dVector CalcAverageOmega (const dQuaternion &q1, dFloat invdt) const;
dQuaternion Slerp (const dQuaternion &q1, dFloat t) const;
dQuaternion IntegrateOmega (const dVector& omega, dFloat timestep) const;
dQuaternion operator* (const dQuaternion &q) const;
dQuaternion operator+ (const dQuaternion &q) const;
dQuaternion operator- (const dQuaternion &q) const;
dFloat m_x;
dFloat m_y;
dFloat m_z;
dFloat m_w;
};
inline dQuaternion::dQuaternion ()
:m_x(0.0f)
,m_y(0.0f)
,m_z(0.0f)
,m_w(1.0f)
{
}
inline dQuaternion::dQuaternion (dFloat x, dFloat y, dFloat z, dFloat w)
:m_x(x)
,m_y(y)
,m_z(z)
,m_w(w)
{
}
inline void dQuaternion::Scale (dFloat scale)
{
m_w *= scale;
m_x *= scale;
m_y *= scale;
m_z *= scale;
}
inline void dQuaternion::Normalize ()
{
Scale (1.0f / dSqrt (DotProduct (*this)));
}
inline dFloat dQuaternion::DotProduct (const dQuaternion &q1) const
{
return m_w * q1.m_w + m_x * q1.m_x + m_y * q1.m_y + m_z * q1.m_z;
}
inline dQuaternion dQuaternion::Inverse () const
{
return dQuaternion (-m_x, -m_y, -m_z, m_w);
}
inline dQuaternion dQuaternion::operator+ (const dQuaternion &q) const
{
return dQuaternion (m_x + q.m_x, m_y + q.m_y, m_z + q.m_z, m_w + q.m_w);
}
inline dQuaternion dQuaternion::operator- (const dQuaternion &B) const
{
return dQuaternion (m_x - B.m_x, m_y - B.m_y, m_z - B.m_z, m_w - B.m_w);
}
inline dQuaternion dQuaternion::operator* (const dQuaternion &q) const
{
return dQuaternion (q.m_x * m_w + q.m_w * m_x - q.m_z * m_y + q.m_y * m_z,
q.m_y * m_w + q.m_z * m_x + q.m_w * m_y - q.m_x * m_z,
q.m_z * m_w - q.m_y * m_x + q.m_x * m_y + q.m_w * m_z,
q.m_w * m_w - q.m_x * m_x - q.m_y * m_y - q.m_z * m_z);
}
#endif

20
thirdparty/src/newton/dMath/dStdAfxMath.cpp vendored Normal file
View File

@@ -0,0 +1,20 @@
/* Copyright (c) <2003-2019> <Newton Game Dynamics>
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely
*/
// stdafx.cpp : source file that includes just the standard includes
// MinimalMath.pch will be the pre-compiled header
// stdafx.obj will contain the pre-compiled type information
#include "dStdAfxMath.h"
// TODO: reference any additional headers you need in STDAFX.H
// and not in this file

36
thirdparty/src/newton/dMath/dStdAfxMath.h vendored Normal file
View File

@@ -0,0 +1,36 @@
/* Copyright (c) <2003-2019> <Newton Game Dynamics>
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely
*/
// stdafx.h : include file for standard system include files,
// or project specific include files that are used frequently, but
// are changed infrequently
//
#ifndef __STDAFXMATH_H__
#define __STDAFXMATH_H__
#ifdef _MSC_VER
#ifndef WIN32_LEAN_AND_MEAN
#define WIN32_LEAN_AND_MEAN // Exclude rarely-used stuff from Windows headers
#endif
#include <windows.h>
#include <crtdbg.h>
#endif
#if ( defined (__MINGW32__) || defined (__MINGW64__) )
#include <crtdbg.h>
#endif
#include <stdio.h>
#include <assert.h>
#endif

13
thirdparty/src/newton/dMath/dVector.cpp vendored Normal file
View File

@@ -0,0 +1,13 @@
/* Copyright (c) <2003-2019> <Newton Game Dynamics>
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely
*/
#include "dStdAfxMath.h"

328
thirdparty/src/newton/dMath/dVector.h vendored Normal file
View File

@@ -0,0 +1,328 @@
/* Copyright (c) <2003-2019> <Newton Game Dynamics>
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely
*/
#ifndef __dVector__
#define __dVector__
#include "dMathDefines.h"
// small but very effective 4 dimensional template vector class
template<class T>
class TemplateVector
{
public:
TemplateVector ();
TemplateVector (const T val);
TemplateVector (const T* const ptr);
TemplateVector (T m_x, T m_y, T m_z, T m_w = T(1.0f));
TemplateVector Scale (T s) const;
T& operator[] (int i);
const T& operator[] (int i) const;
TemplateVector operator+ (const TemplateVector &A) const;
TemplateVector operator- (const TemplateVector &A) const;
TemplateVector operator* (const TemplateVector &A) const;
TemplateVector& operator+= (const TemplateVector &A);
TemplateVector& operator-= (const TemplateVector &A);
TemplateVector& operator*= (const TemplateVector &A);
TemplateVector Abs() const;
TemplateVector Min(const TemplateVector &A) const;
TemplateVector Max(const TemplateVector &A) const;
T DotProduct3 (const TemplateVector &A) const;
TemplateVector Normalize () const;
TemplateVector CrossProduct (const TemplateVector &A) const;
T m_x;
T m_y;
T m_z;
T m_w;
};
class dVector: public TemplateVector<dFloat>
{
public:
dVector()
:TemplateVector<dFloat>()
{
}
dVector(dFloat val)
:TemplateVector<dFloat>(val)
{
}
dVector (const TemplateVector<dFloat>& v)
:TemplateVector<dFloat>(v)
{
}
dVector (const dFloat* const ptr)
:TemplateVector<dFloat>(ptr)
{
}
dVector (dFloat x, dFloat y, dFloat z, dFloat w = 1.0f)
:TemplateVector<dFloat>(x, y, z, w)
{
}
#ifndef _NEWTON_USE_DOUBLE
dVector (const TemplateVector<dFloat64>& v)
:TemplateVector<dFloat>(dFloat(v.m_x), dFloat(v.m_y), dFloat(v.m_z), dFloat(v.m_w))
{
}
#endif
};
class dBigVector: public TemplateVector<dFloat64>
{
public:
dBigVector(){};
dBigVector(dFloat64 val)
:TemplateVector<dFloat64> (val)
{
}
dBigVector (const dFloat64* const ptr)
:TemplateVector<dFloat64> (ptr)
{
}
dBigVector (const TemplateVector<dFloat64>& v)
:TemplateVector<dFloat64> (v.m_x, v.m_y, v.m_z, v.m_w)
{
}
dBigVector(const dVector& v)
:TemplateVector<dFloat64>(v.m_x, v.m_y, v.m_z, v.m_w)
{
}
dBigVector (dFloat64 x, dFloat64 y, dFloat64 z, dFloat64 w = dFloat(1.0f))
:TemplateVector<dFloat64> (x, y, z, w)
{
}
};
template<class T>
TemplateVector<T>::TemplateVector()
{
}
template<class T>
TemplateVector<T>::TemplateVector(const T val)
:m_x (val), m_y(val), m_z(val), m_w(val)
{
}
template<class T>
TemplateVector<T>::TemplateVector(const T *ptr)
:m_x (ptr[0]), m_y(ptr[1]), m_z(ptr[2]), m_w(0.0f)
{
}
template<class T>
TemplateVector<T>::TemplateVector(T x, T y, T z, T w)
:m_x (x), m_y(y), m_z(z), m_w(w)
{
}
template<class T>
T& TemplateVector<T>::operator[] (int i)
{
return (&m_x)[i];
}
template<class T>
const T& TemplateVector<T>::operator[] (int i) const
{
return (&m_x)[i];
}
template<class T>
TemplateVector<T> TemplateVector<T>::Scale (T scale) const
{
return TemplateVector<T> (m_x * scale, m_y * scale, m_z * scale, m_w);
}
template<class T>
TemplateVector<T> TemplateVector<T>::operator+ (const TemplateVector<T>& B) const
{
return TemplateVector<T> (m_x + B.m_x, m_y + B.m_y, m_z + B.m_z, m_w);
}
template<class T>
TemplateVector<T> TemplateVector<T>::operator* (const TemplateVector<T>& B) const
{
return TemplateVector<T>(m_x * B.m_x, m_y * B.m_y, m_z * B.m_z, m_w);
}
template<class T>
TemplateVector<T>& TemplateVector<T>::operator+= (const TemplateVector<T>& A)
{
m_x += A.m_x;
m_y += A.m_y;
m_z += A.m_z;
return *this;
}
template<class T>
TemplateVector<T> TemplateVector<T>::operator- (const TemplateVector<T>& A) const
{
return TemplateVector<T> (m_x - A.m_x, m_y - A.m_y, m_z - A.m_z, m_w);
}
template<class T>
TemplateVector<T>& TemplateVector<T>::operator-= (const TemplateVector<T>& A)
{
m_x -= A.m_x;
m_y -= A.m_y;
m_z -= A.m_z;
return *this;
}
template<class T>
TemplateVector<T>& TemplateVector<T>::operator*= (const TemplateVector<T>& A)
{
m_x *= A.m_x;
m_y *= A.m_y;
m_z *= A.m_z;
//m_w *= A.m_w;
return *this;
}
template<class T>
TemplateVector<T> TemplateVector<T>::Abs() const
{
return dVector(dAbs(m_x), dAbs(m_y), dAbs(m_z), dAbs(m_w));
}
template<class T>
TemplateVector<T> TemplateVector<T>::Min(const TemplateVector &A) const
{
return dVector (dMin (m_x, A.m_x), dMin (m_y, A.m_y), dMin (m_z, A.m_z), dMin (m_w, A.m_w));
}
template<class T>
TemplateVector<T> TemplateVector<T>::Max(const TemplateVector &A) const
{
return dVector (dMax (m_x, A.m_x), dMax (m_y, A.m_y), dMax (m_z, A.m_z), dMax (m_w, A.m_w));
}
template<class T>
T TemplateVector<T>::DotProduct3 (const TemplateVector<T>& A) const
{
return m_x * A.m_x + m_y * A.m_y + m_z * A.m_z;
}
template<class T>
TemplateVector<T> TemplateVector<T>::Normalize () const
{
T mag (DotProduct3(*this));
return Scale (1.0f / T(sqrt (mag)));
}
template<class T>
TemplateVector<T> TemplateVector<T>::CrossProduct (const TemplateVector<T>& A) const
{
return TemplateVector<T> (m_y * A.m_z - m_z * A.m_y, m_z * A.m_x - m_x * A.m_z, m_x * A.m_y - m_y * A.m_x, m_w);
}
class dSpatialVector
{
public:
inline dSpatialVector()
{
}
inline dSpatialVector(const dFloat a)
{
for (int i = 0; i < 6; i++) {
m_d[i] = a;
}
}
inline dSpatialVector(const dVector& low, const dVector& high)
{
m_d[0] = low[0];
m_d[1] = low[1];
m_d[2] = low[2];
m_d[3] = high[0];
m_d[4] = high[1];
m_d[5] = high[2];
}
inline dFloat& operator[] (int i)
{
dAssert(i < 6);
dAssert(i >= 0);
return m_d[i];
}
inline const dFloat& operator[] (int i) const
{
dAssert(i < 6);
dAssert(i >= 0);
return m_d[i];
}
inline dSpatialVector operator+ (const dSpatialVector& A) const
{
dSpatialVector tmp;
for (int i = 0; i < 6; i++) {
tmp[i] = m_d[i] + A.m_d[i];
}
return tmp;
}
inline dSpatialVector operator* (const dSpatialVector& A) const
{
dSpatialVector tmp;
for (int i = 0; i < 6; i++) {
tmp[i] = m_d[i] * A.m_d[i];
}
return tmp;
}
inline dFloat DotProduct(const dSpatialVector& v) const
{
dFloat acc = dFloat (0.0f);
for (int i = 0; i < 6; i++) {
acc += m_d[i] * v.m_d[i];
}
return acc;
}
inline dSpatialVector Scale(dFloat s) const
{
dSpatialVector tmp;
for (int i = 0; i < 6; i++) {
tmp[i] = m_d[i] * s;
}
return tmp;
}
dFloat m_d[6];
};
#endif

27
thirdparty/src/newton/dgCore/dg.cpp vendored Normal file
View File

@@ -0,0 +1,27 @@
/* Copyright (c) <2003-2019> <Julio Jerez, Newton Game Dynamics>
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
*
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
*
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
*
* 3. This notice may not be removed or altered from any source distribution.
*/
#include "dgStdafx.h"

67
thirdparty/src/newton/dgCore/dg.h vendored Normal file
View File

@@ -0,0 +1,67 @@
/* Copyright (c) <2003-2019> <Julio Jerez, Newton Game Dynamics>
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
*
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
*
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
*
* 3. This notice may not be removed or altered from any source distribution.
*/
#if !defined(AFX_DG_H__FGRTY_5GR39H_98TQ_H)
#define AFX_DG_H__FGRTY_5GR39H_98TQ_H
#include "dgTypes.h"
#include "dgRef.h"
#include "dgCRC.h"
#include "dgObb.h"
#include "dgRtti.h"
#include "dgList.h"
#include "dgTree.h"
#include "dgNode.h"
#include "dgHeap.h"
#include "dgSort.h"
#include "dgArray.h"
#include "dgStack.h"
#include "dgDebug.h"
#include "dgGraph.h"
#include "dgPlane.h"
#include "dgGoogol.h"
#include "dgVector.h"
#include "dgMatrix.h"
#include "dgMemory.h"
#include "dgRandom.h"
#include "dgThread.h"
#include "dgProfiler.h"
#include "dgFastQueue.h"
#include "dgPolyhedra.h"
#include "dgThreadHive.h"
#include "dgPathFinder.h"
#include "dgRefCounter.h"
#include "dgQuaternion.h"
#include "dgMutexThread.h"
#include "dgConvexHull3d.h"
#include "dgConvexHull4d.h"
#include "dgIntersections.h"
#include "dgGeneralVector.h"
#include "dgGeneralMatrix.h"
#include "dgAABBPolygonSoup.h"
#include "dgSmallDeterminant.h"
#include "dgPolygonSoupBuilder.h"
#include "dgPolygonSoupDatabase.h"
#include "dgPolyhedraMassProperties.h"
#include "dgDelaunayTetrahedralization.h"
#endif

1390
thirdparty/src/newton/dgCore/dgAABBPolygonSoup.cpp vendored Normal file
View File

File diff suppressed because it is too large Load Diff

251
thirdparty/src/newton/dgCore/dgAABBPolygonSoup.h vendored Normal file
View File

@@ -0,0 +1,251 @@
/* Copyright (c) <2003-2019> <Julio Jerez, Newton Game Dynamics>
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
*
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
*
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
*
* 3. This notice may not be removed or altered from any source distribution.
*/
#ifndef __DG_AABB_POLYGON_SOUP_H_
#define __DG_AABB_POLYGON_SOUP_H_
#include "dgStdafx.h"
#include "dgIntersections.h"
#include "dgPolygonSoupDatabase.h"
class dgPolygonSoupDatabaseBuilder;
class dgAABBPolygonSoup: public dgPolygonSoupDatabase
{
public:
class dgNode
{
public:
enum dgNodeType
{
m_binary = 0,
m_leaf,
};
class dgLeafNodePtr
{
#define DG_INDEX_COUNT_BITS 6
public:
DG_INLINE dgLeafNodePtr ()
{
dgAssert (0);
}
DG_INLINE dgLeafNodePtr (dgUnsigned32 node)
{
m_node = node;
dgAssert (!IsLeaf());
}
DG_INLINE dgUnsigned32 IsLeaf () const
{
return m_node & 0x80000000;
}
DG_INLINE dgUnsigned32 GetCount() const
{
dgAssert (IsLeaf());
return (m_node & (~0x80000000)) >> (32 - DG_INDEX_COUNT_BITS - 1);
}
DG_INLINE dgUnsigned32 GetIndex() const
{
dgAssert (IsLeaf());
return m_node & (~(-(1 << (32 - DG_INDEX_COUNT_BITS - 1))));
}
DG_INLINE dgLeafNodePtr (dgUnsigned32 faceIndexCount, dgUnsigned32 faceIndexStart)
{
dgAssert (faceIndexCount < (1<<DG_INDEX_COUNT_BITS));
m_node = 0x80000000 | (faceIndexCount << (32 - DG_INDEX_COUNT_BITS - 1)) | faceIndexStart;
}
DG_INLINE dgNode* GetNode (const void* const root) const
{
return ((dgNode*) root) + m_node;
}
union {
dgUnsigned32 m_node;
};
};
dgNode ()
:m_indexBox0(0)
,m_indexBox1(0)
,m_left(0)
,m_right(0)
{
}
DG_INLINE dgFloat32 RayDistance (const dgFastRayTest& ray, const dgTriplex* const vertexArray) const
{
dgVector minBox (&vertexArray[m_indexBox0].m_x);
dgVector maxBox (&vertexArray[m_indexBox1].m_x);
minBox = minBox & dgVector::m_triplexMask;
maxBox = maxBox & dgVector::m_triplexMask;
return ray.BoxIntersect(minBox, maxBox);
}
DG_INLINE dgFloat32 BoxPenetration (const dgFastAABBInfo& obb, const dgTriplex* const vertexArray) const
{
dgVector p0 (&vertexArray[m_indexBox0].m_x);
dgVector p1 (&vertexArray[m_indexBox1].m_x);
p0 = p0 & dgVector::m_triplexMask;
p1 = p1 & dgVector::m_triplexMask;
dgVector minBox (p0 - obb.m_p1);
dgVector maxBox (p1 - obb.m_p0);
dgAssert(maxBox.m_x >= minBox.m_x);
dgAssert(maxBox.m_y >= minBox.m_y);
dgAssert(maxBox.m_z >= minBox.m_z);
dgVector mask ((minBox * maxBox) < dgVector::m_zero);
dgVector dist (maxBox.GetMin (minBox.Abs()) & mask);
dist = dist.GetMin(dist.ShiftTripleRight());
dist = dist.GetMin(dist.ShiftTripleRight());
if (dist.GetScalar() > dgFloat32 (0.0f)) {
dgVector origin (dgVector::m_half * (p1 + p0));
dgVector size (dgVector::m_half * (p1 - p0));
origin = obb.UntransformVector(origin);
size = obb.m_absDir.RotateVector(size);
dgVector q0 (origin - size);
dgVector q1 (origin + size);
dgVector minBox1 (q0 - obb.m_size);
dgVector maxBox1 (q1 + obb.m_size);
dgAssert(maxBox1.m_x >= minBox1.m_x);
dgAssert(maxBox1.m_y >= minBox1.m_y);
dgAssert(maxBox1.m_z >= minBox1.m_z);
dgVector mask1 ((minBox1 * maxBox1) < dgVector::m_zero);
dgVector dist1 (maxBox1.GetMin (minBox1.Abs()) & mask1);
dist1 = dist1.GetMin(dist1.ShiftTripleRight());
dist1 = dist1.GetMin(dist1.ShiftTripleRight());
dist = dist.GetMin(dist1);
} else {
dgVector p1p0((minBox.Abs()).GetMin(maxBox.Abs()).AndNot(mask));
dist = p1p0.DotProduct(p1p0);
dist = dist.Sqrt() * dgVector::m_negOne;
}
return dist.GetScalar();
}
DG_INLINE dgFloat32 BoxIntersect (const dgFastRayTest& ray, const dgFastRayTest& obbRay, const dgFastAABBInfo& obb, const dgTriplex* const vertexArray) const
{
dgVector p0 (&vertexArray[m_indexBox0].m_x);
dgVector p1 (&vertexArray[m_indexBox1].m_x);
p0 = p0 & dgVector::m_triplexMask;
p1 = p1 & dgVector::m_triplexMask;
dgVector minBox (p0 - obb.m_p1);
dgVector maxBox (p1 - obb.m_p0);
dgFloat32 dist = ray.BoxIntersect(minBox, maxBox);
if (dist < dgFloat32 (1.0f)) {
dgVector origin (dgVector::m_half * (p1 + p0));
dgVector size (dgVector::m_half * (p1 - p0));
origin = obb.UntransformVector(origin);
size = obb.m_absDir.RotateVector(size);
dgVector q0 (origin - size);
dgVector q1 (origin + size);
dgVector minBox1 (q0 - obb.m_size);
dgVector maxBox1 (q1 + obb.m_size);
dgFloat32 dist1 = obbRay.BoxIntersect(minBox1, maxBox1);
dist = (dist1 > dgFloat32 (1.0f)) ? dist1 : dgMax (dist1, dist);
}
return dist;
}
dgInt32 m_indexBox0;
dgInt32 m_indexBox1;
dgLeafNodePtr m_left;
dgLeafNodePtr m_right;
};
class dgSpliteInfo;
class dgNodeBuilder;
virtual void GetAABB (dgVector& p0, dgVector& p1) const;
virtual void Serialize (dgSerialize callback, void* const userData) const;
virtual void Deserialize (dgDeserialize callback, void* const userData, dgInt32 revisionNumber);
protected:
dgAABBPolygonSoup ();
virtual ~dgAABBPolygonSoup ();
void Create (const dgPolygonSoupDatabaseBuilder& builder, bool optimizedBuild);
void CalculateAdjacendy ();
virtual void ForAllSectorsRayHit (const dgFastRayTest& ray, dgFloat32 maxT, dgRayIntersectCallback callback, void* const context) const;
virtual void ForAllSectors (const dgFastAABBInfo& obbAabb, const dgVector& boxDistanceTravel, dgFloat32 m_maxT, dgAABBIntersectCallback callback, void* const context) const;
DG_INLINE void* GetRootNode() const
{
return m_aabb;
}
DG_INLINE void* GetBackNode(const void* const root) const
{
dgNode* const node = (dgNode*) root;
return node->m_left.IsLeaf() ? NULL : node->m_left.GetNode(m_aabb);
}
DG_INLINE void* GetFrontNode(const void* const root) const
{
dgNode* const node = (dgNode*) root;
return node->m_right.IsLeaf() ? NULL : node->m_right.GetNode(m_aabb);
}
DG_INLINE void GetNodeAABB(const void* const root, dgVector& p0, dgVector& p1) const
{
const dgNode* const node = (dgNode*)root;
p0 = dgVector (&((dgTriplex*)m_localVertex)[node->m_indexBox0].m_x);
p1 = dgVector (&((dgTriplex*)m_localVertex)[node->m_indexBox1].m_x);
p0 = p0 & dgVector::m_triplexMask;
p1 = p1 & dgVector::m_triplexMask;
}
virtual dgVector ForAllSectorsSupportVectex (const dgVector& dir) const;
private:
dgNodeBuilder* BuildTopDown (dgNodeBuilder* const leafArray, dgInt32 firstBox, dgInt32 lastBox, dgNodeBuilder** const allocator) const;
dgFloat32 CalculateFaceMaxSize (const dgVector* const vertex, dgInt32 indexCount, const dgInt32* const indexArray) const;
// static dgIntersectStatus CalculateManifoldFaceEdgeNormals (void* const context, const dgFloat32* const polygon, dgInt32 strideInBytes, const dgInt32* const indexArray, dgInt32 indexCount);
static dgIntersectStatus CalculateDisjointedFaceEdgeNormals (void* const context, const dgFloat32* const polygon, dgInt32 strideInBytes, const dgInt32* const indexArray, dgInt32 indexCount, dgFloat32 hitDistance);
static dgIntersectStatus CalculateAllFaceEdgeNormals (void* const context, const dgFloat32* const polygon, dgInt32 strideInBytes, const dgInt32* const indexArray, dgInt32 indexCount, dgFloat32 hitDistance);
void ImproveNodeFitness (dgNodeBuilder* const node) const;
dgInt32 m_nodesCount;
dgInt32 m_indexCount;
dgNode* m_aabb;
dgInt32* m_indices;
};
#endif

227
thirdparty/src/newton/dgCore/dgArray.h vendored Normal file
View File

@@ -0,0 +1,227 @@
/* Copyright (c) <2003-2019> <Julio Jerez, Newton Game Dynamics>
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
*
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
*
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
*
* 3. This notice may not be removed or altered from any source distribution.
*/
/****************************************************************************
*
* Visual C++ 6.0 created by: Julio Jerez
*
****************************************************************************/
#ifndef __dgArray__
#define __dgArray__
#include "dgStdafx.h"
template<class T>
class dgArray
{
public:
dgArray ();
dgArray (dgMemoryAllocator* const allocator, dgInt32 aligmentInBytes = DG_MEMORY_GRANULARITY);
dgArray (const dgArray& source, dgInt32 itemsToCopy);
dgArray(const dgArray& source);
~dgArray ();
DG_CLASS_ALLOCATOR(allocator)
DG_INLINE T& operator[] (dgInt32 i);
DG_INLINE const T& operator[] (dgInt32 i) const;
void Clear () const;
void Resize (dgInt32 size) const;
DG_INLINE void ResizeIfNecessary (dgInt32 index) const;
dgInt32 GetElementSize() const;
dgInt32 GetBytesCapacity () const;
dgInt32 GetElementsCapacity () const;
dgMemoryAllocator* GetAllocator() const;
void SetAllocator(dgMemoryAllocator* const allocator);
protected:
mutable T *m_array;
private:
mutable dgInt32 m_maxSize;
dgInt32 m_aligmentInBytes;
dgMemoryAllocator* m_allocator;
};
template<class T>
dgArray<T>::dgArray()
:m_array(NULL)
,m_maxSize(0)
,m_aligmentInBytes(DG_MEMORY_GRANULARITY)
,m_allocator(NULL)
{
m_aligmentInBytes = 1 << dgExp2(m_aligmentInBytes);
}
template<class T>
dgArray<T>::dgArray (dgMemoryAllocator* const allocator, dgInt32 aligmentInBytes)
:m_array(NULL)
,m_maxSize(0)
,m_aligmentInBytes(aligmentInBytes)
,m_allocator(allocator)
{
if (m_aligmentInBytes <= 0) {
m_aligmentInBytes = DG_MEMORY_GRANULARITY;
}
m_aligmentInBytes = 1 << dgExp2(m_aligmentInBytes);
}
template<class T>
dgArray<T>::dgArray (const dgArray& source, dgInt32 itemsToCopy)
:m_array(NULL)
,m_maxSize(itemsToCopy)
,m_aligmentInBytes(source.m_aligmentInBytes)
,m_allocator(source.m_allocator)
{
if (source.m_array) {
m_array = (T*) m_allocator->MallocLow (sizeof (T) * itemsToCopy, m_aligmentInBytes);
for (dgInt32 i = 0; i < itemsToCopy; i++) {
m_array[i] = source.m_array[i];
}
}
}
template<class T>
dgArray<T>::dgArray(const dgArray& source)
:m_array(NULL)
,m_maxSize(source.m_maxSize)
,m_aligmentInBytes(source.m_aligmentInBytes)
,m_allocator(source.m_allocator)
{
dgAssert(0);
// use dgArray<T>::dgArray(const dgArray& source, dgInt32 itemsToCopy)
if (source.m_array) {
m_array = (T*)m_allocator->MallocLow(sizeof(T) * m_maxSize, m_aligmentInBytes);
for (dgInt32 i = 0; i < m_maxSize; i++) {
m_array[i] = source.m_array[i];
}
}
}
template<class T>
dgArray<T>::~dgArray ()
{
if (m_array) {
m_allocator->FreeLow (m_array);
}
}
template<class T>
DG_INLINE const T& dgArray<T>::operator[] (dgInt32 i) const
{
dgAssert (i >= 0);
while (i >= m_maxSize) {
Resize (i * 2);
}
return m_array[i];
}
template<class T>
DG_INLINE T& dgArray<T>::operator[] (dgInt32 i)
{
dgAssert (i >= 0);
while (i >= m_maxSize) {
Resize (i * 2);
}
return m_array[i];
}
template<class T>
dgInt32 dgArray<T>::GetElementSize() const
{
return sizeof (T);
}
template<class T>
dgInt32 dgArray<T>::GetElementsCapacity () const
{
return m_maxSize;
}
template<class T>
dgInt32 dgArray<T>::GetBytesCapacity () const
{
return m_maxSize * GetElementSize();
}
template<class T>
void dgArray<T>::Clear () const
{
if (m_array) {
m_allocator->FreeLow (m_array);
m_array = NULL;
}
m_maxSize = 0;
}
template<class T>
dgMemoryAllocator* dgArray<T>::GetAllocator() const
{
return m_allocator;
}
template<class T>
void dgArray<T>::SetAllocator(dgMemoryAllocator* const allocator)
{
dgAssert (!m_allocator);
m_allocator = allocator;
}
template<class T>
void dgArray<T>::Resize (dgInt32 size) const
{
if (size >= m_maxSize) {
size = dgMax (size, 16);
T* const newArray = (T*) m_allocator->MallocLow (dgInt32 (sizeof (T) * size), m_aligmentInBytes);
if (m_array) {
for (dgInt32 i = 0; i < m_maxSize; i ++) {
newArray[i] = m_array[i];
}
m_allocator->FreeLow (m_array);
}
m_array = newArray;
m_maxSize = size;
} else if (size < m_maxSize) {
size = dgMax (size, 16);
T* const newArray = (T*) m_allocator->MallocLow (dgInt32 (sizeof (T) * size), m_aligmentInBytes);
if (m_array) {
for (dgInt32 i = 0; i < size; i ++) {
newArray[i] = m_array[i];
}
m_allocator->FreeLow (m_array);
}
m_array = newArray;
m_maxSize = size;
}
}
template<class T>
DG_INLINE void dgArray<T>::ResizeIfNecessary (dgInt32 size) const
{
while (size >= m_maxSize) {
Resize (m_maxSize * 2);
}
}
#endif

390
thirdparty/src/newton/dgCore/dgCRC.cpp vendored Normal file
View File

@@ -0,0 +1,390 @@
/* Copyright (c) <2003-2019> <Julio Jerez, Newton Game Dynamics>
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
*
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
*
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
*
* 3. This notice may not be removed or altered from any source distribution.
*/
#include "dgStdafx.h"
#include "dgCRC.h"
#include "dgList.h"
#include "dgTree.h"
#include "dgDebug.h"
#include "dgRandom.h"
namespace InternalCRC
{
const dgInt32 STRING_POOL_SIZE = 1024 * 8 - 256;
static dgUnsigned32 randBits[] = {
0x00000001, 0x2C11F801, 0xDFD8F60E, 0x6C8FA2B7,
0xB573754C, 0x1522DCDD, 0x21615D3A, 0xE1B307F3,
0x12AFA158, 0x53D18179, 0x70950126, 0x941702EF,
0x756FE824, 0x694801D5, 0x36DF4DD2, 0x63D80FAB,
0xB8AE95B0, 0x902439F1, 0x090C6F3E, 0x2B7C6A27,
0x8344B5FC, 0x67D3C5CD, 0x22F5516A, 0x2FB00E63,
0xFC761508, 0x87B00169, 0x27EBA056, 0x8CC0B85F,
0xE33D3ED4, 0x95DA08C5, 0x13E5C802, 0x9DD9E41B,
0xD4577F60, 0x3DD6B7E1, 0x096AF46E, 0x1A00CD97,
0x4B10E2AC, 0x22EAAABD, 0x683F119A, 0x62D070D3,
0xA8D034B8, 0xAA363D59, 0x58CECB86, 0x40F589CF,
0x4F630184, 0x38918BB5, 0xB85B8E32, 0x0A6A948B,
0x9A099510, 0x402871D1, 0x11E7859E, 0xEE73CD07,
0x4142FB5C, 0x39D68BAD, 0x0FE19DCA, 0xE35B2F43,
0x75590068, 0x66433549, 0x929182B6, 0x71EC773F,
0xBBAC3034, 0xF2BD8AA5, 0x5743A062, 0x5AB120FB,
0x5ABFD6C0, 0xDDD867C1, 0xDC3522CE, 0xD0EC6877,
0xE106000C, 0xB7C6689D, 0xED3FF5FA, 0xC75749B3,
0x126B7818, 0x1A75E939, 0x0546C5E6, 0x8A9C80AF,
0x48A3CAE4, 0x756D0595, 0x7060FE92, 0xA594896B,
0x12354470, 0x896599B1, 0xDAC6CBFE, 0xCB419FE7,
0x9C44F0BC, 0xAFA9418D, 0xB87D1A2A, 0x428BC023,
0x33229BC8, 0xC92D5929, 0xB1C19516, 0x0FBCA61F,
0xE594D194, 0x716EFC85, 0x0036A8C2, 0xD7BBCDDB,
0x16E4DE20, 0xD10F07A1, 0x68CF812E, 0x390A7357,
0x8BAACD6C, 0x2C2E167D, 0x3E7C0A5A, 0x167F9293,
0x3D596B78, 0x08888519, 0x9994F046, 0x0FC3E78F,
0x008A4444, 0x87526F75, 0xB0079EF2, 0x238DEE4B,
0xCA09A3D0, 0x4ED3B191, 0xFA42425E, 0x379DE2C7,
0x1EA2961C, 0x1FC3E76D, 0x90DFC68A, 0x0279C103,
0xF9AAE728, 0xF2666D09, 0xEF13D776, 0x92E944FF,
0x364F22F4, 0x37665E65, 0x05D6E122, 0x7131EABB,
0x479E9580, 0x98729781, 0x4BD20F8E, 0x1612EE37,
0xCB574ACC, 0x5499B45D, 0x360B4EBA, 0x33814B73,
0x43720ED8, 0x146610F9, 0x45514AA6, 0x0B23BE6F,
0x026E6DA4, 0xD1B9C955, 0x94676F52, 0xCE8EC32B,
0x165EB330, 0x2F6AB971, 0x92F1E8BE, 0xC54095A7,
0xBEB3EB7C, 0x5C9E7D4D, 0x5921A2EA, 0xB45D31E3,
0xB8C9E288, 0x5FE670E9, 0xC02049D6, 0xC42A53DF,
0x6F332454, 0x661BB045, 0x2B3C4982, 0xDF4B779B,
0xD7C4FCE0, 0x70FB1761, 0xADD4CDEE, 0x47BDD917,
0x8C63782C, 0x8181423D, 0xFA05C31A, 0xDD947453,
0x6A8D6238, 0x1A068CD9, 0x4413D506, 0x5374054F,
0xC5A84704, 0xB41B1335, 0x06986FB2, 0x4CCF080B,
0xF80C7290, 0x8622B151, 0x536DBF1E, 0x21E1B887,
0xDED0F0DC, 0xB4B1032D, 0x1D5AAF4A, 0xC56E12C3,
0x8C578DE8, 0xCBA564C9, 0xA67EEC36, 0x0837D2BF,
0x3D98D5B4, 0x1B06F225, 0xFF7EE1E2, 0x3640747B,
0x5E301440, 0x53A08741, 0x436FBC4E, 0xC9C333F7,
0x2727558C, 0x7F5CC01D, 0xFC83677A, 0xAFF10D33,
0x24836598, 0x3161F8B9, 0xDD748F66, 0x5B6CBC2F,
0xAD8FD064, 0x89EE4D15, 0xBBB2A012, 0xA086BCEB,
0x1BEAE1F0, 0x69F39931, 0x764DC57E, 0x17394B67,
0x4D51A63C, 0xF273790D, 0x35A2EBAA, 0x7EE463A3,
0xBC2BE948, 0x2B9B48A9, 0x2FC7BE96, 0x5FC9C19F,
0x3AD83714, 0x6FA02405, 0xDDB6AA42, 0xE648E15B,
0x1DB7DBA0, 0xF55AE721, 0x4D3ADAAE, 0xB3DAFED7,
0x5FFAE2EC, 0x96A42DFD, 0xFB9C3BDA, 0x21CF1613,
0x0F2C18F8, 0xAE705499, 0x650B79C6, 0x31C5E30F,
0x097D09C4, 0xAAAB76F5, 0x34CE0072, 0x27EDE1CB,
0xDAD20150, 0xADD57111, 0xC229FBDE, 0x8AFF4E47,
0x448E0B9C, 0x5C5DDEED, 0x4612580A, 0x05F82483,
0xBC1EF4A8, 0xB1C01C89, 0xF592C0F6, 0x6798207F,
0xEC494874, 0x795F45E5, 0xECFBA2A2, 0xBB9CBE3B,
0xF567104f, 0x47289407, 0x25683fa6, 0x2fde5836,
};
struct StringPool
{
char buff[STRING_POOL_SIZE];
StringPool ()
{
}
StringPool (const StringPool &arg)
{
}
};
/*
class CRCStringLookup: public dgTree<char *, dgUnsigned32>, public dgList<StringPool>
{
dgInt32 size;
char *ptr;
void AddContainer ()
{
StringPool tmp;
dgListNode *node;
// Addtop (tmp);
// node = GetFirst();
node = Addtop (tmp);
ptr = node->GetInfo().buff;
size = STRING_POOL_SIZE;
}
public:
CRCStringLookup ()
:dgTree<char *, dgUnsigned32>(NULL), dgList<StringPool>()
{
AddContainer();
}
~CRCStringLookup ()
{
}
void AddString (dgUnsigned32 crc, const char *string)
{
dgInt32 length;
if (dgTree<char *, dgUnsigned32>::Find(crc)) {
return;
}
length = dgInt32 (strlen (string)) + 1;
if (size < length) {
AddContainer();
}
strcpy (ptr, string);
Insert (ptr, crc);
ptr += length;
size -= length;
}
const char* FindString (dgUnsigned32 crc) const
{
dgTreeNode *node;
node = dgTree<char *, dgUnsigned32>::Find(crc);
// return node ? node->GetInfo() : NULL;
return node ? node->GetInfo() : NULL;
}
};
static CRCStringLookup& GetDatabase ()
{
static CRCStringLookup database;
return database;
}
*/
/*
dgUnsigned32 RSHash(char* str, dgUnsigned32 len)
{
dgUnsigned32 b = 378551;
dgUnsigned32 a = 63689;
dgUnsigned32 hash = 0;
dgUnsigned32 i = 0;
for(i = 0; i < len; str++, i++) {
hash = hash * a + (*str);
a = a * b;
}
return (hash & 0x7FFFFFFF);
}
// End Of RS Hash Function
dgUnsigned32 JSHash(char* str, dgUnsigned32 len)
{
dgUnsigned32 hash = 1315423911;
dgUnsigned32 i = 0;
for(i = 0; i < len; str++, i++)
{
hash ^= ((hash << 5) + (*str) + (hash >> 2));
}
return (hash & 0x7FFFFFFF);
}
// End Of JS Hash Function
dgUnsigned32 PJWHash(char* str, dgUnsigned32 len)
{
dgUnsigned32 BitsInUnsignedInt = (dgUnsigned32)(sizeof(dgUnsigned32) * 8);
dgUnsigned32 ThreeQuarters = (dgUnsigned32)((BitsInUnsignedInt * 3) / 4);
dgUnsigned32 OneEighth = (dgUnsigned32)(BitsInUnsignedInt / 8);
dgUnsigned32 HighBits = (dgUnsigned32)(0xFFFFFFFF) << (BitsInUnsignedInt - OneEighth);
dgUnsigned32 hash = 0;
dgUnsigned32 test = 0;
dgUnsigned32 i = 0;
for(i = 0; i < len; str++, i++)
{
hash = (hash << OneEighth) + (*str);
if((test = hash & HighBits) != 0)
{
hash = (( hash ^ (test >> ThreeQuarters)) & (~HighBits));
}
}
return (hash & 0x7FFFFFFF);
}
// End Of P. J. Weinberger Hash Function
dgUnsigned32 ELFHash(char* str, dgUnsigned32 len)
{
dgUnsigned32 hash = 0;
dgUnsigned32 x = 0;
dgUnsigned32 i = 0;
for(i = 0; i < len; str++, i++)
{
hash = (hash << 4) + (*str);
if((x = hash & 0xF0000000L) != 0)
{
hash ^= (x >> 24);
hash &= ~x;
}
}
return (hash & 0x7FFFFFFF);
}
// End Of ELF Hash Function
dgUnsigned32 BKDRHash(char* str, dgUnsigned32 len)
{
dgUnsigned32 seed = 131; // 31 131 1313 13131 131313 etc..
dgUnsigned32 hash = 0;
dgUnsigned32 i = 0;
for(i = 0; i < len; str++, i++)
{
hash = (hash * seed) + (*str);
}
return (hash & 0x7FFFFFFF);
}
// End Of BKDR Hash Function
dgUnsigned32 SDBMHash(char* str, dgUnsigned32 len)
{
dgUnsigned32 hash = 0;
dgUnsigned32 i = 0;
for(i = 0; i < len; str++, i++)
{
hash = (*str) + (hash << 6) + (hash << 16) - hash;
}
return (hash & 0x7FFFFFFF);
}
// End Of SDBM Hash Function
dgUnsigned32 DEKHash(char* str, dgUnsigned32 len)
{
dgUnsigned32 hash = len;
dgUnsigned32 i = 0;
for(i = 0; i < len; str++, i++)
{
hash = ((hash << 5) ^ (hash >> 27)) ^ (*str);
}
return (hash & 0x7FFFFFFF);
}
// End Of DEK Hash Function
dgUnsigned32 APHash(char* str, dgUnsigned32 len)
{
dgUnsigned32 hash = 0;
dgUnsigned32 i = 0;
for(i = 0; i < len; str++, i++)
{
hash ^= ((i & 1) == 0) ? ( (hash << 7) ^ (*str) ^ (hash >> 3)) :
(~((hash << 11) ^ (*str) ^ (hash >> 5)));
}
return (hash & 0x7FFFFFFF);
}
*/
// End Of DJB Hash Function
dgUnsigned32 DJBHash(const char* const str, dgInt32 len)
{
dgUnsigned32 hash = 5381;
for(dgInt32 i = 0; i < len; i++)
{
//hash = ((hash << 5) + hash) + (*str);
hash = ((hash << 5) + hash) + str[i];
}
return (hash & 0x7FFFFFFF);
}
}
dgUnsigned32 dgApi dgCRC (const char* const name)
{
// dgUnsigned32 c;
// dgUnsigned32 crc;
// unsigned char *ptr;
// dgUnsigned32 val;
if (!name) {
return 0;
}
dgUnsigned32 crc = 0;
//for (ptr = (unsigned char*)name; *ptr; ptr ++) {
for (dgInt32 i = 0; name[i]; i ++) {
char c = name[i];
dgUnsigned32 val = InternalCRC::randBits[((crc >> 24) ^ c) & 0xff];
crc = (crc << 8) ^ val;
}
dgAssert (0);
// InternalCRC::GetDatabase().AddString (crc, name);
return crc;
}
dgUnsigned32 dgApi dgCRC (const void* const buffer, dgInt32 size, dgUnsigned32 crcAcc)
{
dgAssert (buffer);
unsigned char* const ptr = (unsigned char*)buffer;
for (dgInt32 i = 0; i < size; i ++) {
dgUnsigned32 c = ptr[i];
dgUnsigned32 val = InternalCRC::randBits[((crcAcc >> 24) ^ c) & 0xff];
crcAcc = (crcAcc << 8) ^ val;
}
return crcAcc;
}
const char* dgApi dgInverseCRC (dgUnsigned32 crc)
{
dgAssert (0);
return NULL;
// return InternalCRC::GetDatabase().FindString (crc);
}
dgUnsigned32 dgApi dgHash (const void* const string, int size)
{
return InternalCRC::DJBHash ((char*)string, size);
}

35
thirdparty/src/newton/dgCore/dgCRC.h vendored Normal file
View File

@@ -0,0 +1,35 @@
/* Copyright (c) <2003-2019> <Julio Jerez, Newton Game Dynamics>
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
*
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
*
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
*
* 3. This notice may not be removed or altered from any source distribution.
*/
#ifndef __dgCRC__
#define __dgCRC__
#include "dgStdafx.h"
dgUnsigned32 dgApi dgCRC (const char* const string);
dgUnsigned32 dgApi dgCRC (const void* const buffer, dgInt32 size, dgUnsigned32 crcAcc = 0);
const char* dgApi dgInverseCRC (dgUnsigned32 CRC);
dgUnsigned32 dgApi dgHash (const void* const string, dgInt32 size);
#endif

1100
thirdparty/src/newton/dgCore/dgConvexHull3d.cpp vendored Normal file
View File

File diff suppressed because it is too large Load Diff

136
thirdparty/src/newton/dgCore/dgConvexHull3d.h vendored Normal file
View File

@@ -0,0 +1,136 @@
/* Copyright (c) <2003-2019> <Julio Jerez, Newton Game Dynamics>
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
*
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
*
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
*
* 3. This notice may not be removed or altered from any source distribution.
*/
#ifndef __DG_CONVEXHULL_3D__
#define __DG_CONVEXHULL_3D__
#include "dgStdafx.h"
#include "dgList.h"
#include "dgArray.h"
#include "dgPlane.h"
#include "dgVector.h"
#include "dgMatrix.h"
#include "dgQuaternion.h"
#define DG_OLD_CONVEXHULL_3D
class dgMemoryAllocator;
class dgConvexHull3DVertex;
class dgConvexHull3dAABBTreeNode;
class dgConvexHull3DFace
{
public:
dgConvexHull3DFace();
void SetMark(dgInt32 mark) {m_mark = mark;}
dgInt32 GetMark() const {return m_mark;}
dgList<dgConvexHull3DFace>::dgListNode* GetTwin(dgInt32 index) const { return m_twin[index];}
private:
dgFloat64 Evalue (const dgBigVector* const pointArray, const dgBigVector& point) const;
dgBigPlane GetPlaneEquation (const dgBigVector* const pointArray) const;
public:
dgInt32 m_index[3];
private:
dgInt32 m_mark;
dgList<dgConvexHull3DFace>::dgListNode* m_twin[3];
friend class dgConvexHull3d;
};
class dgConvexHull3d: public dgList<dgConvexHull3DFace>
{
#ifdef DG_OLD_CONVEXHULL_3D
class dgNormalMap;
#endif
public:
dgConvexHull3d(const dgConvexHull3d& source);
dgConvexHull3d(dgMemoryAllocator* const allocator, const dgFloat64* const vertexCloud, dgInt32 strideInBytes, dgInt32 count, dgFloat64 distTol, dgInt32 maxVertexCount = 0x7fffffff);
virtual ~dgConvexHull3d();
dgInt32 GetVertexCount() const;
const dgBigVector* GetVertexPool() const;
const dgBigVector& GetVertex(dgInt32 i) const;
dgFloat64 GetDiagonal() const;
void GetAABB (dgBigVector& boxP0, dgBigVector& boxP1) const;
dgFloat64 RayCast (const dgBigVector& localP0, const dgBigVector& localP1) const;
void CalculateVolumeAndSurfaceArea (dgFloat64& volume, dgFloat64& surcafeArea) const;
protected:
dgConvexHull3d(dgMemoryAllocator* const allocator);
void BuildHull (const dgFloat64* const vertexCloud, dgInt32 strideInBytes, dgInt32 count, dgFloat64 distTol, dgInt32 maxVertexCount);
virtual dgListNode* AddFace (dgInt32 i0, dgInt32 i1, dgInt32 i2);
virtual void DeleteFace (dgListNode* const node) ;
virtual dgInt32 InitVertexArray(dgConvexHull3DVertex* const points, const dgFloat64* const vertexCloud, dgInt32 strideInBytes, dgInt32 count, void* const memoryPool, dgInt32 maxMemSize);
bool CheckFlatSurface(dgConvexHull3dAABBTreeNode* vertexTree, dgConvexHull3DVertex* const points, dgInt32 count, dgFloat64 distTol, dgInt32 maxVertexCount);
void CalculateConvexHull2d (dgConvexHull3dAABBTreeNode* vertexTree, dgConvexHull3DVertex* const points, dgInt32 count, dgFloat64 distTol, dgInt32 maxVertexCount);
void CalculateConvexHull3d (dgConvexHull3dAABBTreeNode* vertexTree, dgConvexHull3DVertex* const points, dgInt32 count, dgFloat64 distTol, dgInt32 maxVertexCount);
dgInt32 SupportVertex (dgConvexHull3dAABBTreeNode** const tree, const dgConvexHull3DVertex* const points, const dgBigVector& dir, const bool removeEntry = true) const;
dgFloat64 TetrahedrumVolume (const dgBigVector& p0, const dgBigVector& p1, const dgBigVector& p2, const dgBigVector& p3) const;
dgInt32 GetUniquePoints(dgConvexHull3DVertex* const points, const dgFloat64* const vertexCloud, dgInt32 strideInBytes, dgInt32 count, void* const memoryPool, dgInt32 maxMemSize);
dgConvexHull3dAABBTreeNode* BuildTree (dgConvexHull3dAABBTreeNode* const parent, dgConvexHull3DVertex* const points, dgInt32 count, dgInt32 baseIndex, dgInt8** const memoryPool, dgInt32& maxMemSize) const;
static dgInt32 ConvexCompareVertex(const dgConvexHull3DVertex* const A, const dgConvexHull3DVertex* const B, void* const context);
bool Sanity() const;
void Save (const char* const filename) const;
dgInt32 m_count;
dgFloat64 m_diag;
dgBigVector m_aabbP0;
dgBigVector m_aabbP1;
dgArray<dgBigVector> m_points;
};
inline dgInt32 dgConvexHull3d::GetVertexCount() const
{
return m_count;
}
inline const dgBigVector* dgConvexHull3d::GetVertexPool() const
{
return &m_points[0];
}
inline const dgBigVector& dgConvexHull3d::GetVertex(dgInt32 index) const
{
return m_points[index];
}
inline dgFloat64 dgConvexHull3d::GetDiagonal() const
{
return m_diag;
}
inline void dgConvexHull3d::GetAABB (dgBigVector& boxP0, dgBigVector& boxP1) const
{
boxP0 = m_aabbP0;
boxP1 = m_aabbP1;
}
#endif

1056
thirdparty/src/newton/dgCore/dgConvexHull4d.cpp vendored Normal file
View File

File diff suppressed because it is too large Load Diff

193
thirdparty/src/newton/dgCore/dgConvexHull4d.h vendored Normal file
View File

@@ -0,0 +1,193 @@
/* Copyright (c) <2003-2019> <Julio Jerez, Newton Game Dynamics>
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
*
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
*
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
*
* 3. This notice may not be removed or altered from any source distribution.
*/
#ifndef __DG_CONVEXHULL_4D__
#define __DG_CONVEXHULL_4D__
#include "dgStdafx.h"
#include "dgList.h"
#include "dgArray.h"
#include "dgPlane.h"
#include "dgVector.h"
#include "dgMatrix.h"
#include "dgQuaternion.h"
class dgMemoryAllocator;
class dgConvexHull4dAABBTreeNode;
class dgConvexHull4dVector: public dgBigVector
{
public:
void operator = (const dgBigVector& a)
{
m_x = a.m_x;
m_y = a.m_y;
m_z = a.m_z;
m_w = a.m_w;
m_index = 0;
m_mark = 0;
}
dgInt32 m_index;
dgInt32 m_mark;
};
class dgConvexHull4dTetraherum
{
public:
class dgTetrahedrumFace
{
public:
dgInt32 m_index[4];
dgList<dgConvexHull4dTetraherum>::dgListNode* m_twin;
};
class dgTetrahedrumPlane: public dgBigVector
{
public:
dgTetrahedrumPlane (const dgBigVector& p0, const dgBigVector& p1, const dgBigVector& p2, const dgBigVector& p3);
dgFloat64 Evalue (const dgBigVector& point) const;
dgFloat64 m_dist;
};
dgConvexHull4dTetraherum();
dgTetrahedrumPlane GetPlaneEquation (const dgConvexHull4dVector* const points) const;
dgFloat64 GetTetraVolume (const dgConvexHull4dVector* const pointArray) const;
dgBigVector CircumSphereCenter (const dgConvexHull4dVector* const pointArray) const;
dgFloat64 Evalue (const dgConvexHull4dVector* const pointArray, const dgBigVector& point) const;
dgInt32 GetMark() const { return m_mark; }
void SetMark(dgInt32 mark) { m_mark = mark; }
private:
void Init (const dgConvexHull4dVector* const points, dgInt32 v0, dgInt32 v1, dgInt32 v2, dgInt32 v3);
public:
dgTetrahedrumFace m_faces[4];
dgInt32 m_mark;
dgInt32 m_uniqueID;
#ifdef _DEBUG
dgInt32 m_debugID;
#endif
friend class dgConvexHull4d;
friend class dgDelaunayTetrahedralization;
};
class dgConvexHull4d: public dgList<dgConvexHull4dTetraherum>
{
public:
dgConvexHull4d(dgMemoryAllocator* const allocator, const dgFloat64* const vertexCloud, dgInt32 strideInBytes, dgInt32 count, dgFloat64 distTol);
virtual ~dgConvexHull4d();
dgInt32 GetVertexCount() const;
dgInt32 GetVertexIndex(dgInt32 i) const;
const dgBigVector& GetVertex(dgInt32 i) const;
const dgConvexHull4dVector* GetHullVertexArray() const;
dgFloat64 GetTetraVolume (const dgConvexHull4dTetraherum* const tetra) const;
dgInt32 IncMark ();
void Save (const char* const filename) const;
protected:
dgConvexHull4d(dgMemoryAllocator* const allocator);
void BuildHull (dgMemoryAllocator* const allocator, const dgFloat64* const vertexCloud, dgInt32 strideInBytes, dgInt32 count, dgFloat64 distTol);
virtual dgInt32 AddVertex (const dgBigVector& vertex);
virtual dgInt32 InitVertexArray(dgConvexHull4dVector* const points, const dgFloat64* const vertexCloud, dgInt32 strideInBytes, dgInt32 count, void* const memoryPool, dgInt32 maxMemSize);
virtual dgListNode* AddFace (dgInt32 i0, dgInt32 i1, dgInt32 i2, dgInt32 i3);
virtual void DeleteFace (dgListNode* const node);
dgListNode* FindFacingNode(const dgBigVector& vertex);
void InsertNewVertex(dgInt32 vertexIndex, dgListNode* const frontFace, dgList<dgListNode*>& deletedFaces, dgList<dgListNode*>& newFaces);
dgInt32 SupportVertex (dgConvexHull4dAABBTreeNode** const tree, const dgConvexHull4dVector* const points, const dgBigVector& dir, const bool removeEntry = true) const;
void CalculateConvexHull (dgConvexHull4dAABBTreeNode* vertexTree, dgConvexHull4dVector* const points, dgInt32 count, dgFloat64 distTol);
void LinkSibling (dgListNode* node0, dgListNode* node1) const;
bool Sanity() const;
dgConvexHull4dAABBTreeNode* BuildTree (dgConvexHull4dAABBTreeNode* const parent, dgConvexHull4dVector* const points, dgInt32 count, dgInt32 baseIndex, dgInt8** const memoryPool, dgInt32& maxMemSize) const;
static dgInt32 ConvexCompareVertex(const dgConvexHull4dVector* const A, const dgConvexHull4dVector* const B, void* const context);
class dgNormalMap
{
public:
dgNormalMap();
private:
void TessellateTriangle(dgInt32 level, const dgVector& p0, const dgVector& p1, const dgVector& p2, dgBigVector* const buffer, dgInt32& count);
dgBigVector m_normal[1024];
dgInt32 m_count;
friend class dgConvexHull4d;
};
static const dgNormalMap& GetNormaMap();
dgInt32 m_mark;
dgInt32 m_count;
dgFloat64 m_diag;
dgArray<dgConvexHull4dVector> m_points;
};
inline dgInt32 dgConvexHull4d::IncMark ()
{
m_mark ++;
return m_mark;
}
inline dgInt32 dgConvexHull4d::GetVertexCount() const
{
return m_count;
}
inline dgInt32 dgConvexHull4d::GetVertexIndex(dgInt32 index) const
{
dgAssert (index >= 0);
dgAssert (index < m_count);
return m_points[index].m_index;
}
inline const dgBigVector& dgConvexHull4d::GetVertex(dgInt32 index) const
{
dgAssert (index >= 0);
dgAssert (index < m_count);
return m_points[index];
}
inline const dgConvexHull4dVector* dgConvexHull4d::GetHullVertexArray() const
{
return &m_points[0];
}
inline dgFloat64 dgConvexHull4d::GetTetraVolume (const dgConvexHull4dTetraherum* const tetra) const
{
return tetra->GetTetraVolume (&m_points[0]);
}
#endif

26
thirdparty/src/newton/dgCore/dgDebug.cpp vendored Normal file
View File

@@ -0,0 +1,26 @@
/* Copyright (c) <2003-2019> <Julio Jerez, Newton Game Dynamics>
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
*
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
*
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
*
* 3. This notice may not be removed or altered from any source distribution.
*/
#include "dgStdafx.h"
#include "dgDebug.h"

75
thirdparty/src/newton/dgCore/dgDebug.h vendored Normal file
View File

@@ -0,0 +1,75 @@
/* Copyright (c) <2003-2019> <Julio Jerez, Newton Game Dynamics>
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
*
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
*
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
*
* 3. This notice may not be removed or altered from any source distribution.
*/
#ifndef __dgDebug__
#define __dgDebug__
#include "dgStdafx.h"
inline void dgApi dgExpandTraceMessage(const char *fmt, ...)
{
va_list v_args;
char text[4096];
text[0] = 0;
va_start(v_args, fmt);
vsprintf(text, fmt, v_args);
va_end(v_args);
#if defined (_WIN_32_VER) || defined (_WIN_64_VER)
OutputDebugStringA(text);
#else
printf("%s\n", text);
#endif
}
#ifdef _MSC_VER
#ifdef _DEBUG
#define DG_TRACE
#endif
#endif
#ifdef DG_TRACE
#define dgTrace(x) dgExpandTraceMessage x;
#else
#define dgTrace(x);
#endif
#ifdef _DEBUG
inline void TraceFuntionName (const char *name)
{
// static int trace;
// dgTrace (("%d %s\n", trace, name));
dgTrace (("%s\n", name));
}
//#define TRACE_FUNCTION(name) TraceFuntionName (name)
#define TRACE_FUNCTION(name)
#else
#define TRACE_FUNCTION(name)
#endif
#endif

178
thirdparty/src/newton/dgCore/dgDelaunayTetrahedralization.cpp vendored Normal file
View File

@@ -0,0 +1,178 @@
/* Copyright (c) <2003-2019> <Julio Jerez, Newton Game Dynamics>
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
*
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
*
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
*
* 3. This notice may not be removed or altered from any source distribution.
*/
#include "dgStdafx.h"
#include "dgSort.h"
#include "dgStack.h"
#include "dgGoogol.h"
#include "dgSmallDeterminant.h"
#include "dgDelaunayTetrahedralization.h"
dgDelaunayTetrahedralization::dgDelaunayTetrahedralization(dgMemoryAllocator* const allocator, const dgFloat64* const vertexCloud, dgInt32 count, dgInt32 strideInByte, dgFloat64 distTol)
:dgConvexHull4d(allocator)
{
dgSetPrecisionDouble precision;
dgStack<dgBigVector> pool(count + 2);
dgBigVector* const points = &pool[0];
dgInt32 stride = dgInt32 (strideInByte / sizeof (dgFloat64));
for (dgInt32 i = 0; i < count; i ++) {
dgFloat64 x = dgRoundToFloat (vertexCloud[i * stride + 0]);
dgFloat64 y = dgRoundToFloat (vertexCloud[i * stride + 1]);
dgFloat64 z = dgRoundToFloat (vertexCloud[i * stride + 2]);
points[i] = dgBigVector (x, y, z, x * x + y * y + z * z);
}
dgInt32 oldCount = count;
BuildHull (allocator, &pool[0].m_x, sizeof (dgBigVector), count, distTol);
if (oldCount > m_count) {
// the mesh is convex, need to add two steiners point to make tractable
dgBigVector origin (dgFloat64 (0.0f));
dgFloat64 maxW = dgFloat64 (-1.0e20f);
for (dgInt32 i = 0; i < count; i++) {
dgFloat64 x = dgRoundToFloat(vertexCloud[i * stride + 0]);
dgFloat64 y = dgRoundToFloat(vertexCloud[i * stride + 1]);
dgFloat64 z = dgRoundToFloat(vertexCloud[i * stride + 2]);
points[i] = dgBigVector(x, y, z, x * x + y * y + z * z);
origin += points[i];
maxW = dgMax (points[i].m_w, maxW);
}
origin = origin.Scale (dgFloat64 (1.0f) / count);
points[count + 0] = origin;
points[count + 1] = origin;
points[count + 0].m_w += dgFloat64 (1.0f);
points[count + 1].m_w -= dgFloat64 (1.0f);
BuildHull (allocator, &pool[0].m_x, sizeof (dgBigVector), count + 2, distTol);
}
if (oldCount > m_count) {
// this is probably a regular convex solid, which will have a zero volume hull
// add the rest of the points by incremental insertion with small perturbation
dgInt32 hullCount = m_count;
for (dgInt32 i = 0; i < count; i ++) {
bool inHull = false;
const dgConvexHull4dVector* const hullPoints = &m_points[0];
for (dgInt32 j = 0; j < hullCount; j ++) {
if (hullPoints[j].m_index == i) {
inHull = true;
break;
}
}
if (!inHull) {
dgBigVector q (points[i]);
dgInt32 index = AddVertex(q);
if (index == -1) {
q.m_x += dgFloat64 (1.0e-3f);
q.m_y += dgFloat64 (1.0e-3f);
q.m_z += dgFloat64 (1.0e-3f);
index = AddVertex(q);
dgAssert (index != -1);
}
dgAssert (index != -1);
m_points[index].m_index = i;
}
}
}
SortVertexArray ();
}
dgDelaunayTetrahedralization::~dgDelaunayTetrahedralization()
{
}
dgInt32 dgDelaunayTetrahedralization::AddVertex (const dgBigVector& vertex)
{
dgSetPrecisionDouble precision;
dgBigVector p (vertex);
dgAssert(p.m_w == dgFloat32(0.0f));
p.m_w = p.DotProduct(p).GetScalar();
dgInt32 index = dgConvexHull4d::AddVertex(p);
return index;
}
dgInt32 dgDelaunayTetrahedralization::CompareVertexByIndex(const dgConvexHull4dVector* const A, const dgConvexHull4dVector* const B, void* const context)
{
if (A->m_index < B ->m_index) {
return -1;
} else if (A->m_index > B->m_index) {
return 1;
}
return 0;
}
void dgDelaunayTetrahedralization::SortVertexArray ()
{
dgConvexHull4dVector* const points = &m_points[0];
for (dgListNode* node = GetFirst(); node; node = node->GetNext()) {
dgConvexHull4dTetraherum* const tetra = &node->GetInfo();
for (dgInt32 i = 0; i < 4; i ++) {
dgConvexHull4dTetraherum::dgTetrahedrumFace& face = tetra->m_faces[i];
for (dgInt32 j = 0; j < 4; j ++) {
dgInt32 index = face.m_index[j];
face.m_index[j] = points[index].m_index;
}
}
}
dgSort(points, m_count, CompareVertexByIndex);
}
void dgDelaunayTetrahedralization::RemoveUpperHull ()
{
dgSetPrecisionDouble precision;
dgListNode* nextNode = NULL;
for (dgListNode* node = GetFirst(); node; node = nextNode) {
nextNode = node->GetNext();
dgConvexHull4dTetraherum* const tetra = &node->GetInfo();
tetra->SetMark(0);
dgFloat64 w = GetTetraVolume (tetra);
if (w >= dgFloat64 (0.0f)) {
DeleteFace(node);
}
}
}
void dgDelaunayTetrahedralization::DeleteFace (dgListNode* const node)
{
dgConvexHull4dTetraherum* const tetra = &node->GetInfo();
for (dgInt32 i = 0; i < 4; i ++) {
dgListNode* const twinNode = tetra->m_faces[i].m_twin;
if (twinNode) {
dgConvexHull4dTetraherum* const twinTetra = &twinNode->GetInfo();
for (dgInt32 j = 0; j < 4; j ++) {
if (twinTetra->m_faces[j].m_twin == node) {
twinTetra->m_faces[j].m_twin = NULL;
break;
}
}
}
}
dgConvexHull4d::DeleteFace (node);
}

45
thirdparty/src/newton/dgCore/dgDelaunayTetrahedralization.h vendored Normal file
View File

@@ -0,0 +1,45 @@
/* Copyright (c) <2003-2019> <Julio Jerez, Newton Game Dynamics>
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
*
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
*
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
*
* 3. This notice may not be removed or altered from any source distribution.
*/
#ifndef __DG_DEALUNAY_TETRAHEDRALIZAION_4D__
#define __DG_DEALUNAY_TETRAHEDRALIZAION_4D__
#include "dgStdafx.h"
#include "dgConvexHull4d.h"
class dgDelaunayTetrahedralization: public dgConvexHull4d
{
public:
dgDelaunayTetrahedralization(dgMemoryAllocator* const allocator, const dgFloat64* const vertexCloud, dgInt32 count, dgInt32 strideInByte, dgFloat64 distTol);
virtual ~dgDelaunayTetrahedralization();
void RemoveUpperHull ();
dgInt32 AddVertex (const dgBigVector& vertex);
protected:
virtual void DeleteFace (dgListNode* const node) ;
void SortVertexArray ();
static dgInt32 CompareVertexByIndex(const dgConvexHull4dVector* const A, const dgConvexHull4dVector* const B, void* const context);
};
#endif

113
thirdparty/src/newton/dgCore/dgFastQueue.h vendored Normal file
View File

@@ -0,0 +1,113 @@
/* Copyright (c) <2003-2019> <Julio Jerez, Newton Game Dynamics>
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
*
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
*
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
*
* 3. This notice may not be removed or altered from any source distribution.
*/
/****************************************************************************
*
* Visual C++ 6.0 created by: Julio Jerez
*
****************************************************************************/
#ifndef __dgFastQueue__
#define __dgFastQueue__
#include "dgStdafx.h"
template<class T, dgInt32 sizeInPowerOfTwo>
class dgFastQueue
{
public:
DG_CLASS_ALLOCATOR_NEW(allocator)
dgFastQueue (dgMemoryAllocator* const allocator);
~dgFastQueue ();
bool IsEmpty() const;
bool IsFull() const;
const T& GetHead() const;
void Pop();
void Push(T& object);
private:
T* m_pool;
dgMemoryAllocator* m_allocator;
dgInt32 m_head;
dgInt32 m_tail;
};
template<class T, dgInt32 sizeInPowerOfTwo>
dgFastQueue<T, sizeInPowerOfTwo>::dgFastQueue (dgMemoryAllocator* const allocator)
:m_allocator(allocator)
,m_head(0)
,m_tail(0)
{
dgAssert (((sizeInPowerOfTwo -1) & (-sizeInPowerOfTwo)) == 0);
m_pool = (T*) m_allocator->MallocLow(sizeInPowerOfTwo * sizeof (T));
}
template<class T, dgInt32 sizeInPowerOfTwo>
dgFastQueue<T, sizeInPowerOfTwo>::~dgFastQueue ()
{
m_allocator->FreeLow(m_pool);
}
template<class T, dgInt32 sizeInPowerOfTwo>
bool dgFastQueue<T, sizeInPowerOfTwo>::IsEmpty() const
{
return (m_head == m_tail);
}
template<class T, dgInt32 sizeInPowerOfTwo>
bool dgFastQueue<T, sizeInPowerOfTwo>::IsFull() const
{
return (((m_tail + 1) & (sizeInPowerOfTwo - 1)) == m_head);
}
template<class T, dgInt32 sizeInPowerOfTwo>
const T& dgFastQueue<T, sizeInPowerOfTwo>::GetHead() const
{
dgAssert (!IsEmpty());
return m_pool[m_head];
}
template<class T, dgInt32 sizeInPowerOfTwo>
void dgFastQueue<T, sizeInPowerOfTwo>::Pop()
{
dgAssert (!IsEmpty());
m_head = (m_head + 1) & (sizeInPowerOfTwo - 1);
}
template<class T, dgInt32 sizeInPowerOfTwo>
void dgFastQueue<T, sizeInPowerOfTwo>::Push(T& object)
{
dgAssert (!IsFull());
m_pool[m_tail] = object;
m_tail = (m_tail + 1) & (sizeInPowerOfTwo - 1);
}
#endif

66
thirdparty/src/newton/dgCore/dgGeneralMatrix.cpp vendored Normal file
View File

@@ -0,0 +1,66 @@
/* Copyright (c) <2003-2019> <Julio Jerez, Newton Game Dynamics>
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
*
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
*
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
*
* 3. This notice may not be removed or altered from any source distribution.
*/
#include "dgStdafx.h"
#include "dgGeneralMatrix.h"
#include "dgStack.h"
#include "dgMemory.h"
/*
DG_INLINE bool dgCholeskyFactorizationAddRow(dgInt32 size, dgInt32 n, dgFloat32* const matrix, dgInt32 rowStride)
{
dgFloat32* const rowN = &matrix[rowStride * n];
dgInt32 stride = 0;
for (dgInt32 j = 0; j <= n; j++) {
dgFloat32 s = dgFloat32(0.0f);
dgFloat32* const rowJ = &matrix[stride];
for (dgInt32 k = 0; k < j; k++) {
s += rowN[k] * rowJ[k];
}
if (n == j) {
dgFloat32 diag = rowN[n] - s;
if (diag < dgFloat32(dgFloat32(1.0e-6f))) {
return false;
}
rowN[n] = dgFloat32(sqrt(diag));
} else {
rowN[j] = (rowN[j] - s) / rowJ[j];
}
stride += rowStride;
}
return true;
}
bool dgCholeskyFactorization(dgInt32 size, dgFloat32* const psdMatrix, dgInt32 rowStride)
{
bool state = true;
for (dgInt32 i = 0; (i < size) && state; i++) {
state = state && dgCholeskyFactorizationAddRow(size, i, psdMatrix, rowStride);
}
return state;
}
*/

1337
thirdparty/src/newton/dgCore/dgGeneralMatrix.h vendored Normal file
View File

File diff suppressed because it is too large Load Diff

25
thirdparty/src/newton/dgCore/dgGeneralVector.cpp vendored Normal file
View File

@@ -0,0 +1,25 @@
/* Copyright (c) <2003-2019> <Julio Jerez, Newton Game Dynamics>
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
*
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
*
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
*
* 3. This notice may not be removed or altered from any source distribution.
*/
#include "dgStdafx.h"
#include "dgGeneralVector.h"

89
thirdparty/src/newton/dgCore/dgGeneralVector.h vendored Normal file
View File

@@ -0,0 +1,89 @@
/* Copyright (c) <2003-2019> <Julio Jerez, Newton Game Dynamics>
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
*
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
*
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
*
* 3. This notice may not be removed or altered from any source distribution.
*/
#ifndef __dgGeneralVector__
#define __dgGeneralVector__
#include "dgStdafx.h"
#include "dgDebug.h"
#include "dgMemory.h"
template <class T>
DG_INLINE T dgSQRH(const T num, const T den)
{
T r(num / den);
return T(sqrt(T(1.0f) + r * r));
}
template <class T>
DG_INLINE T dgPythag(const T a, const T b)
{
T absa(dgAbs(a));
T absb(dgAbs(b));
return (absa > absb) ? (absa * dgSQRH(absb, absa)) : ((absb == T(0.0f) ? T(0.0f) : (absb * dgSQRH(absa, absb))));
}
template <class T>
DG_INLINE T dgSign(const T a, const T b)
{
return (b >= T(0.0f)) ? (a >= T(0.0f) ? a : -a) : (a >= T(0.0f) ? -a : a);
}
// return dot product
template<class T>
DG_INLINE T dgDotProduct(dgInt32 size, const T* const A, const T* const B)
{
T val(0.0f);
for (dgInt32 i = 0; i < size; i++) {
val = val + A[i] * B[i];
}
return val;
}
template<class T>
DG_INLINE void dgAdd(dgInt32 size, T* const X, const T* const A, const T* const B)
{
for (dgInt32 i = 0; i < size; i++) {
X[i] = A[i] + B[i];
}
}
template<class T>
DG_INLINE void dgSub(dgInt32 size, T* const X, const T* const A, const T* const B)
{
for (dgInt32 i = 0; i < size; i++) {
X[i] = A[i] - B[i];
}
}
template<class T>
DG_INLINE void dgMulAdd(dgInt32 size, T* const X, const T* const A, const T* const B, T C)
{
for (dgInt32 i = 0; i < size; i++) {
X[i] = A[i] + B[i] * C;
}
}
#endif

584
thirdparty/src/newton/dgCore/dgGoogol.cpp vendored Normal file
View File

@@ -0,0 +1,584 @@
/* Copyright (c) <2003-2019> <Julio Jerez, Newton Game Dynamics>
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
*
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
*
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
*
* 3. This notice may not be removed or altered from any source distribution.
*/
#include "dgStdafx.h"
#include "dgGoogol.h"
dgGoogol dgGoogol::m_zero(0.0);
dgGoogol dgGoogol::m_one(1.0);
dgGoogol dgGoogol::m_two(2.0);
dgGoogol dgGoogol::m_three(3.0);
dgGoogol dgGoogol::m_half(0.5);
#ifdef _IMPLEMENT_USING_INTEGER_ARITHMETIC_
dgGoogol::dgGoogol(void)
:m_sign(0)
,m_exponent(0)
{
memset (m_mantissa, 0, sizeof (m_mantissa));
}
dgGoogol::dgGoogol(dgFloat64 value)
:m_sign(0)
,m_exponent(0)
{
dgInt32 exp;
dgFloat64 mantissa = fabs (frexp(value, &exp));
m_exponent = dgInt16 (exp);
m_sign = (value >= 0) ? 0 : 1;
memset (m_mantissa, 0, sizeof (m_mantissa));
m_mantissa[0] = dgUnsigned64 (dgFloat64 (dgUnsigned64(1)<<62) * mantissa);
// it looks like GCC have problems with this
//dgAssert (m_mantissa[0] >= 0);
dgAssert ((m_mantissa[0] & dgUnsigned64(1)<<63) == 0);
}
void dgGoogol::CopySignedMantissa (dgUnsigned64* const mantissa) const
{
memcpy (mantissa, m_mantissa, sizeof (m_mantissa));
if (m_sign) {
NegateMantissa (mantissa);
}
}
dgGoogol::operator double() const
{
dgFloat64 mantissa = (dgFloat64(1.0f) / dgFloat64 (dgUnsigned64(1)<<62)) * dgFloat64 (m_mantissa[0]);
mantissa = ldexp(mantissa, m_exponent) * (m_sign ? dgFloat64 (-1.0f) : dgFloat64 (1.0f));
return mantissa;
}
dgGoogol dgGoogol::operator+ (const dgGoogol &A) const
{
dgGoogol tmp;
dgAssert (dgInt64 (m_mantissa[0]) >= 0);
dgAssert (dgInt64 (A.m_mantissa[0]) >= 0);
if (m_mantissa[0] && A.m_mantissa[0]) {
dgUnsigned64 mantissa0[DG_GOOGOL_SIZE];
dgUnsigned64 mantissa1[DG_GOOGOL_SIZE];
dgUnsigned64 mantissa[DG_GOOGOL_SIZE];
CopySignedMantissa (mantissa0);
A.CopySignedMantissa (mantissa1);
dgInt32 exponetDiff = m_exponent - A.m_exponent;
dgInt32 exponent = m_exponent;
if (exponetDiff > 0) {
ShiftRightMantissa (mantissa1, exponetDiff);
} else if (exponetDiff < 0) {
exponent = A.m_exponent;
ShiftRightMantissa (mantissa0, -exponetDiff);
}
dgUnsigned64 carrier = 0;
for (dgInt32 i = DG_GOOGOL_SIZE - 1; i >= 0; i --) {
dgUnsigned64 m0 = mantissa0[i];
dgUnsigned64 m1 = mantissa1[i];
mantissa[i] = m0 + m1 + carrier;
carrier = CheckCarrier (m0, m1) | CheckCarrier (m0 + m1, carrier);
}
dgInt8 sign = 0;
if (dgInt64 (mantissa[0]) < 0) {
sign = 1;
NegateMantissa (mantissa);
}
dgInt32 bits = NormalizeMantissa (mantissa);
if (bits <= (-64 * DG_GOOGOL_SIZE)) {
tmp.m_sign = 0;
tmp.m_exponent = 0;
} else {
tmp.m_sign = sign;
tmp.m_exponent = dgInt16 (exponent + bits);
}
memcpy (tmp.m_mantissa, mantissa, sizeof (m_mantissa));
} else if (A.m_mantissa[0]) {
tmp = A;
} else {
tmp = *this;
}
dgAssert (dgInt64 (tmp.m_mantissa[0]) >= 0);
return tmp;
}
dgGoogol dgGoogol::operator- (const dgGoogol &A) const
{
dgGoogol tmp (A);
tmp.m_sign = !tmp.m_sign;
return *this + tmp;
}
void dgGoogol::ScaleMantissa (dgUnsigned64* const dst, dgUnsigned64 scale) const
{
dgUnsigned64 carrier = 0;
for (dgInt32 i = DG_GOOGOL_SIZE - 1; i >= 0; i --) {
if (m_mantissa[i]) {
dgUnsigned64 low;
dgUnsigned64 high;
ExtendeMultiply (scale, m_mantissa[i], high, low);
dgUnsigned64 acc = low + carrier;
carrier = CheckCarrier (low, carrier);
dgAssert (CheckCarrier (carrier, high) == 0);
carrier += high;
dst[i + 1] = acc;
} else {
dst[i + 1] = carrier;
carrier = 0;
}
}
dst[0] = carrier;
}
dgGoogol dgGoogol::operator* (const dgGoogol &A) const
{
dgAssert (dgInt64 (m_mantissa[0]) >= 0);
dgAssert (dgInt64 (A.m_mantissa[0]) >= 0);
if (m_mantissa[0] && A.m_mantissa[0]) {
dgUnsigned64 mantissaAcc[DG_GOOGOL_SIZE * 2];
memset (mantissaAcc, 0, sizeof (mantissaAcc));
for (dgInt32 i = DG_GOOGOL_SIZE - 1; i >= 0; i --) {
dgUnsigned64 a = m_mantissa[i];
if (a) {
dgUnsigned64 mantissaScale[2 * DG_GOOGOL_SIZE];
memset (mantissaScale, 0, sizeof (mantissaScale));
A.ScaleMantissa (&mantissaScale[i], a);
dgUnsigned64 carrier = 0;
for (dgInt32 j = 0; j < 2 * DG_GOOGOL_SIZE; j ++) {
const dgInt32 k = 2 * DG_GOOGOL_SIZE - 1 - j;
dgUnsigned64 m0 = mantissaAcc[k];
dgUnsigned64 m1 = mantissaScale[k];
mantissaAcc[k] = m0 + m1 + carrier;
carrier = CheckCarrier (m0, m1) | CheckCarrier (m0 + m1, carrier);
}
}
}
dgUnsigned64 carrier = 0;
//dgInt32 bits = dgUnsigned64(LeadingZeros (mantissaAcc[0]) - 2);
dgInt32 bits = LeadingZeros (mantissaAcc[0]) - 2;
for (dgInt32 i = 0; i < 2 * DG_GOOGOL_SIZE; i ++) {
const dgInt32 k = 2 * DG_GOOGOL_SIZE - 1 - i;
dgUnsigned64 a = mantissaAcc[k];
mantissaAcc[k] = (a << dgUnsigned64(bits)) | carrier;
carrier = a >> dgUnsigned64(64 - bits);
}
dgInt32 exp = m_exponent + A.m_exponent - (bits - 2);
dgGoogol tmp;
tmp.m_sign = m_sign ^ A.m_sign;
tmp.m_exponent = dgInt16 (exp);
memcpy (tmp.m_mantissa, mantissaAcc, sizeof (m_mantissa));
return tmp;
}
return dgGoogol(0.0);
}
dgGoogol dgGoogol::operator/ (const dgGoogol &A) const
{
dgGoogol tmp (1.0 / A);
tmp = tmp * (m_two - A * tmp);
tmp = tmp * (m_two - A * tmp);
int test = 0;
dgInt32 passes = 0;
do {
passes ++;
dgGoogol tmp0 (tmp);
tmp = tmp * (m_two - A * tmp);
test = memcmp (&tmp0, &tmp, sizeof (dgGoogol));
} while (test && (passes < (2 * DG_GOOGOL_SIZE)));
dgAssert (passes <= (2 * DG_GOOGOL_SIZE));
return (*this) * tmp;
}
dgGoogol dgGoogol::Abs () const
{
dgGoogol tmp (*this);
tmp.m_sign = 0;
return tmp;
}
dgGoogol dgGoogol::Floor () const
{
if (m_exponent < 1) {
return dgGoogol (0.0);
}
dgInt32 bits = m_exponent + 2;
dgInt32 start = 0;
while (bits >= 64) {
bits -= 64;
start ++;
}
dgGoogol tmp (*this);
for (dgInt32 i = DG_GOOGOL_SIZE - 1; i > start; i --) {
tmp.m_mantissa[i] = 0;
}
// some compilers do no like this and I do not know why is that
//dgUnsigned64 mask = (-1LL) << (64 - bits);
dgUnsigned64 mask (~0ULL);
mask <<= (64 - bits);
tmp.m_mantissa[start] &= mask;
if (m_sign) {
dgAssert (0);
}
return tmp;
}
dgGoogol dgGoogol::InvSqrt () const
{
const dgGoogol& me = *this;
dgGoogol x (1.0f / sqrt (me));
dgInt32 test = 0;
dgInt32 passes = 0;
do {
passes ++;
dgGoogol tmp (x);
x = m_half * x * (m_three - me * x * x);
test = memcmp (&x, &tmp, sizeof (dgGoogol));
} while (test && (passes < (2 * DG_GOOGOL_SIZE)));
dgAssert (passes <= (2 * DG_GOOGOL_SIZE));
return x;
}
dgGoogol dgGoogol::Sqrt () const
{
return *this * InvSqrt();
}
void dgGoogol::ToString (char* const string) const
{
dgGoogol tmp (*this);
dgGoogol base (10.0);
while (dgFloat64 (tmp) > 1.0) {
tmp = tmp/base;
}
dgInt32 index = 0;
while (tmp.m_mantissa[0]) {
tmp = tmp * base;
dgGoogol digit (tmp.Floor());
tmp -= digit;
dgFloat64 val = digit;
string[index] = char (val) + '0';
index ++;
}
string[index] = 0;
}
#else
dgGoogol::dgGoogol(void)
:m_value(0.0)
{
}
dgGoogol::dgGoogol(dgFloat64 value)
:m_value (value)
{
}
void dgGoogol::CopySignedMantissa (dgUnsigned64* const mantissa) const
{
}
dgGoogol::operator double() const
{
return m_value;
}
dgGoogol dgGoogol::operator+ (const dgGoogol &A) const
{
return m_value + A.m_value;
}
dgGoogol dgGoogol::operator- (const dgGoogol &A) const
{
return m_value - A.m_value;
}
dgGoogol dgGoogol::operator* (const dgGoogol &A) const
{
return m_value * A.m_value;
}
dgGoogol dgGoogol::operator/ (const dgGoogol &A) const
{
return m_value / A.m_value;
}
dgGoogol dgGoogol::Abs () const
{
return fabs (m_value);
}
dgGoogol dgGoogol::InvSqrt () const
{
return 1.0 / sqrt (m_value);
}
dgGoogol dgGoogol::Sqrt () const
{
return sqrt(m_value);
}
dgGoogol dgGoogol::Floor () const
{
return floor (m_value);
}
void dgGoogol::ToString (char* const string) const
{
sprintf (string, "%f", m_value);
}
void dgGoogol::ScaleMantissa (dgUnsigned64* const dst, dgUnsigned64 scale) const
{
}
#endif
void dgGoogol::NegateMantissa (dgUnsigned64* const mantissa) const
{
dgUnsigned64 carrier = 1;
for (dgInt32 i = DG_GOOGOL_SIZE - 1; i >= 0; i --) {
dgUnsigned64 a = ~mantissa[i] + carrier;
if (a) {
carrier = 0;
}
mantissa[i] = a;
}
}
void dgGoogol::ShiftRightMantissa (dgUnsigned64* const mantissa, dgInt32 bits) const
{
dgUnsigned64 carrier = 0;
if (dgInt64 (mantissa[0]) < dgInt64 (0)) {
carrier = dgUnsigned64 (-1);
}
while (bits >= 64) {
for (dgInt32 i = DG_GOOGOL_SIZE - 2; i >= 0; i --) {
mantissa[i + 1] = mantissa[i];
}
mantissa[0] = carrier;
bits -= 64;
}
if (bits > 0) {
carrier <<= (64 - bits);
for (dgInt32 i = 0; i < DG_GOOGOL_SIZE; i ++) {
dgUnsigned64 a = mantissa[i];
mantissa[i] = (a >> bits) | carrier;
carrier = a << (64 - bits);
}
}
}
dgInt32 dgGoogol::LeadingZeros (dgUnsigned64 a) const
{
#define dgCOUNTBIT(mask,add) \
{ \
dgUnsigned64 test = a & mask; \
n += test ? 0 : add; \
a = test ? test : (a & ~mask); \
}
dgInt32 n = 0;
dgAssert (a);
dgCOUNTBIT (0xffffffff00000000LL, 32);
dgCOUNTBIT (0xffff0000ffff0000LL, 16);
dgCOUNTBIT (0xff00ff00ff00ff00LL, 8);
dgCOUNTBIT (0xf0f0f0f0f0f0f0f0LL, 4);
dgCOUNTBIT (0xccccccccccccccccLL, 2);
dgCOUNTBIT (0xaaaaaaaaaaaaaaaaLL, 1);
return n;
}
dgInt32 dgGoogol::NormalizeMantissa (dgUnsigned64* const mantissa) const
{
dgAssert (dgInt64 (mantissa[0]) >= 0);
dgInt32 bits = 0;
if(dgInt64 (mantissa[0] * 2) < 0) {
bits = 1;
ShiftRightMantissa (mantissa, 1);
} else {
while (!mantissa[0] && bits > (-64 * DG_GOOGOL_SIZE)) {
bits -= 64;
for (dgInt32 i = 1; i < DG_GOOGOL_SIZE; i ++) {
mantissa[i - 1] = mantissa[i];
}
mantissa[DG_GOOGOL_SIZE - 1] = 0;
}
if (bits > (-64 * DG_GOOGOL_SIZE)) {
dgInt32 n = LeadingZeros (mantissa[0]) - 2;
if (n > 0) {
dgAssert (n > 0);
dgUnsigned64 carrier = 0;
for (dgInt32 i = DG_GOOGOL_SIZE-1; i >= 0; i --) {
dgUnsigned64 a = mantissa[i];
mantissa[i] = (a << n) | carrier;
carrier = a >> (64 - n);
}
bits -= n;
} else if (n < 0) {
// this is very rare but it does happens, whee the leading zeros of the mantissa is an exact multiple of 64
dgAssert (mantissa[0] & dgUnsigned64(3)<<62);
dgUnsigned64 carrier = 0;
dgInt32 shift = -n;
for (dgInt32 i = 0; i < DG_GOOGOL_SIZE; i ++) {
dgUnsigned64 a = mantissa[i];
mantissa[i] = (a >> shift) | carrier;
carrier = a << (64 - shift);
}
bits -= n;
}
}
}
return bits;
}
dgUnsigned64 dgGoogol::CheckCarrier (dgUnsigned64 a, dgUnsigned64 b) const
{
return ((dgUnsigned64 (-1) - b) < a) ? dgUnsigned64(1) : 0;
}
void dgGoogol::ExtendeMultiply (dgUnsigned64 a, dgUnsigned64 b, dgUnsigned64& high, dgUnsigned64& low) const
{
dgUnsigned64 bLow = b & 0xffffffff;
dgUnsigned64 bHigh = b >> 32;
dgUnsigned64 aLow = a & 0xffffffff;
dgUnsigned64 aHigh = a >> 32;
dgUnsigned64 l = bLow * aLow;
dgUnsigned64 c1 = bHigh * aLow;
dgUnsigned64 c2 = bLow * aHigh;
dgUnsigned64 m = c1 + c2;
dgUnsigned64 carrier = CheckCarrier (c1, c2) << 32;
dgUnsigned64 h = bHigh * aHigh + carrier;
dgUnsigned64 ml = m << 32;
dgUnsigned64 ll = l + ml;
dgUnsigned64 mh = (m >> 32) + CheckCarrier (l, ml);
dgAssert ((mh & ~0xffffffff) == 0);
dgUnsigned64 hh = h + mh;
low = ll;
high = hh;
}
dgGoogol dgGoogol::operator+= (const dgGoogol &A)
{
*this = *this + A;
return *this;
}
dgGoogol dgGoogol::operator-= (const dgGoogol &A)
{
*this = *this - A;
return *this;
}
bool dgGoogol::operator> (const dgGoogol &A) const
{
dgGoogol tmp (*this - A);
return dgFloat64(tmp) > 0.0;
}
bool dgGoogol::operator>= (const dgGoogol &A) const
{
dgGoogol tmp (*this - A);
return dgFloat64 (tmp) >= 0.0;
}
bool dgGoogol::operator< (const dgGoogol &A) const
{
dgGoogol tmp (*this - A);
return dgFloat64 (tmp) < 0.0;
}
bool dgGoogol::operator<= (const dgGoogol &A) const
{
dgGoogol tmp (*this - A);
return dgFloat64 (tmp) <= 0.0;
}
bool dgGoogol::operator== (const dgGoogol &A) const
{
dgGoogol tmp (*this - A);
return dgFloat64 (tmp) == 0.0;
}
bool dgGoogol::operator!= (const dgGoogol &A) const
{
dgGoogol tmp (*this - A);
return dgFloat64 (tmp) != 0.0;
}
void dgGoogol::Trace () const
{
dgTrace (("%f ", dgFloat64 (*this)));
}

144
thirdparty/src/newton/dgCore/dgGoogol.h vendored Normal file
View File

@@ -0,0 +1,144 @@
/* Copyright (c) <2003-2019> <Julio Jerez, Newton Game Dynamics>
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
*
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
*
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
*
* 3. This notice may not be removed or altered from any source distribution.
*/
#ifndef __dgGoogol__
#define __dgGoogol__
#include "dgStdafx.h"
#include "dgMemory.h"
#include "dgArray.h"
#include "dgVector.h"
#define _IMPLEMENT_USING_INTEGER_ARITHMETIC_
//#define DG_GOOGOL_SIZE 16
#define DG_GOOGOL_SIZE 4
class dgGoogol
{
public:
dgGoogol(void);
dgGoogol(dgFloat64 value);
operator double() const;
dgGoogol operator+ (const dgGoogol &A) const;
dgGoogol operator- (const dgGoogol &A) const;
dgGoogol operator* (const dgGoogol &A) const;
dgGoogol operator/ (const dgGoogol &A) const;
dgGoogol operator+= (const dgGoogol &A);
dgGoogol operator-= (const dgGoogol &A);
bool operator> (const dgGoogol &A) const;
bool operator>= (const dgGoogol &A) const;
bool operator< (const dgGoogol &A) const;
bool operator<= (const dgGoogol &A) const;
bool operator== (const dgGoogol &A) const;
bool operator!= (const dgGoogol &A) const;
dgGoogol Abs () const;
dgGoogol Sqrt () const;
dgGoogol InvSqrt () const;
dgGoogol Floor () const;
void Trace () const;
void ToString (char* const string) const;
private:
void InitFloatFloat (dgFloat64 value);
void NegateMantissa (dgUnsigned64* const mantissa) const;
void CopySignedMantissa (dgUnsigned64* const mantissa) const;
dgInt32 NormalizeMantissa (dgUnsigned64* const mantissa) const;
dgUnsigned64 CheckCarrier (dgUnsigned64 a, dgUnsigned64 b) const;
void ShiftRightMantissa (dgUnsigned64* const mantissa, dgInt32 bits) const;
dgInt32 LeadingZeros (dgUnsigned64 a) const;
void ExtendeMultiply (dgUnsigned64 a, dgUnsigned64 b, dgUnsigned64& high, dgUnsigned64& low) const;
void ScaleMantissa (dgUnsigned64* const out, dgUnsigned64 scale) const;
#ifdef _IMPLEMENT_USING_INTEGER_ARITHMETIC_
dgInt32 m_sign;
dgInt32 m_exponent;
dgUnsigned64 m_mantissa[DG_GOOGOL_SIZE];
#else
dgFloat64 m_value;
#endif
public:
static dgGoogol m_zero;
static dgGoogol m_one;
static dgGoogol m_two;
static dgGoogol m_three;
static dgGoogol m_half;
};
class dgHugeVector: public dgTemplateVector<dgGoogol>
{
public:
dgHugeVector ()
:dgTemplateVector<dgGoogol>()
{
}
dgHugeVector (const dgBigVector& a)
:dgTemplateVector<dgGoogol>(dgGoogol (a.m_x), dgGoogol (a.m_y), dgGoogol (a.m_z), dgGoogol (a.m_w))
{
}
dgHugeVector (const dgTemplateVector<dgGoogol>& a)
:dgTemplateVector<dgGoogol>(a)
{
}
dgHugeVector (dgFloat64 x, dgFloat64 y, dgFloat64 z, dgFloat64 w)
:dgTemplateVector<dgGoogol>(x, y, z, w)
{
}
dgHugeVector(const dgGoogol& x, const dgGoogol& y, const dgGoogol& z, const dgGoogol& w)
:dgTemplateVector<dgGoogol>(x, y, z, w)
{
}
dgGoogol EvaluePlane (const dgHugeVector& point) const
{
//return (point % (*this)) + m_w;
return DotProduct3(point) + m_w;
}
#ifdef _DEBUG
void Trace () const
{
m_x.Trace();
m_y.Trace();
m_z.Trace();
m_w.Trace();
dgTrace (("\n"));
}
#endif
};
#endif

191
thirdparty/src/newton/dgCore/dgGraph.h vendored Normal file
View File

@@ -0,0 +1,191 @@
/* Copyright (c) <2003-2019> <Julio Jerez, Newton Game Dynamics>
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
*
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
*
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
*
* 3. This notice may not be removed or altered from any source distribution.
*/
/****************************************************************************
*
* Visual C++ 6.0 created by: Julio Jerez
*
****************************************************************************/
#ifndef __dgGraph__
#define __dgGraph__
#include "dgStdafx.h"
#include "dgRef.h"
#include "dgList.h"
template<class dgNodeData, class dgEdgeData> class dgGraphEdge;
template<class dgNodeData, class dgEdgeData> class dgGraphNode;
template<class dgNodeData, class dgEdgeData>
class dgGraph: public dgList<dgGraphNode<dgNodeData, dgEdgeData> >
{
public:
dgGraph (dgMemoryAllocator* const allocator);
~dgGraph ();
typename dgGraph<dgNodeData, dgEdgeData>::dgListNode* AddNode ();
void DeleteNode (typename dgGraph<dgNodeData, dgEdgeData>::dgListNode* const node);
typename dgGraph<dgNodeData, dgEdgeData>::dgListNode* GetNodeFromNodeData(dgNodeData* const nodeData);
//void Trace () const;
};
template<class dgNodeData, class dgEdgeData>
class dgGraphNode: public dgList<dgGraphEdge<dgNodeData, dgEdgeData> >
{
public:
dgGraphNode ();
~dgGraphNode ();
typename dgGraphNode<dgNodeData, dgEdgeData>::dgListNode* AddEdge(typename dgGraph<dgNodeData, dgEdgeData>::dgListNode* const node);
void DeleteHalfEdge(typename dgGraphNode<dgNodeData, dgEdgeData>::dgListNode* const edge);
void DeleteEdge(typename dgGraphNode<dgNodeData, dgEdgeData>::dgListNode* const edge);
//void Trace () const;
dgNodeData m_nodeData;
};
template<class dgNodeData, class dgEdgeData>
class dgGraphEdge
{
public:
dgGraphEdge();
~dgGraphEdge();
typename dgGraph<dgNodeData, dgEdgeData>::dgListNode* m_node;
dgEdgeData m_edgeData;
};
template<class dgNodeData, class dgEdgeData>
dgGraph<dgNodeData, dgEdgeData>::dgGraph (dgMemoryAllocator* const allocator)
:dgList<dgGraphNode<dgNodeData, dgEdgeData> >(allocator)
{
}
template<class dgNodeData, class dgEdgeData>
dgGraph<dgNodeData, dgEdgeData>::~dgGraph ()
{
}
template<class dgNodeData, class dgEdgeData>
typename dgGraph<dgNodeData, dgEdgeData>::dgListNode* dgGraph<dgNodeData, dgEdgeData>::AddNode ()
{
typename dgGraph<dgNodeData, dgEdgeData>::dgListNode* const node = dgGraph<dgNodeData, dgEdgeData>::Append();
node->GetInfo().SetAllocator(dgGraph<dgNodeData, dgEdgeData>::GetAllocator());
return node;
}
template<class dgNodeData, class dgEdgeData>
void dgGraph<dgNodeData, dgEdgeData>::DeleteNode (typename dgGraph<dgNodeData, dgEdgeData>::dgListNode* const node)
{
for (typename dgGraphNode<dgNodeData, dgEdgeData>::dgListNode* link = node->GetInfo().GetFirst(); link; link = link->GetNext()) {
typename dgGraph<dgNodeData, dgEdgeData>::dgListNode* const twinNode = link->GetInfo().m_node;
for (typename dgGraphNode<dgNodeData, dgEdgeData>::dgListNode* link1 = twinNode->GetInfo().GetFirst(); link1; link1 = link1->GetNext()) {
if (link1->GetInfo().m_node == node) {
twinNode->GetInfo().Remove (link1);
break;
}
}
}
dgList<dgGraphNode<dgNodeData, dgEdgeData> >::Remove (node);
}
template<class dgNodeData, class dgEdgeData>
typename dgGraph<dgNodeData, dgEdgeData>::dgListNode* dgGraph<dgNodeData, dgEdgeData>::GetNodeFromNodeData(dgNodeData* const nodeData)
{
dgInt32 size1 = sizeof (*nodeData);
dgInt32 size0 = sizeof (dgGraphNode<dgNodeData, dgEdgeData>);
void* const ptr = ((char*)nodeData) - (size0 - size1);
dgGraphNode<dgNodeData, dgEdgeData>* const nodeInfo = (dgGraphNode<dgNodeData, dgEdgeData>*) ptr;
return this->GetNodeFromInfo(*nodeInfo);
}
template<class dgNodeData, class dgEdgeData>
dgGraphNode<dgNodeData, dgEdgeData>::dgGraphNode()
:dgList<dgGraphEdge<dgNodeData, dgEdgeData> >(NULL)
{
}
template<class dgNodeData, class dgEdgeData>
dgGraphNode<dgNodeData, dgEdgeData>::~dgGraphNode()
{
}
template<class dgNodeData, class dgEdgeData>
typename dgGraphNode<dgNodeData, dgEdgeData>::dgListNode* dgGraphNode<dgNodeData, dgEdgeData>::AddEdge (typename dgGraph<dgNodeData, dgEdgeData>::dgListNode* const node)
{
typename dgGraphNode<dgNodeData, dgEdgeData>::dgListNode* const edge = dgGraphNode<dgNodeData, dgEdgeData>::Append();
edge->GetInfo().m_node = node;
return edge;
}
template<class dgNodeData, class dgEdgeData>
void dgGraphNode<dgNodeData, dgEdgeData>::DeleteHalfEdge(typename dgGraphNode<dgNodeData, dgEdgeData>::dgListNode* const edge)
{
dgList<dgGraphEdge<dgNodeData, dgEdgeData> >::Remove (edge);
}
template<class dgNodeData, class dgEdgeData>
void dgGraphNode<dgNodeData, dgEdgeData>::DeleteEdge(typename dgGraphNode<dgNodeData, dgEdgeData>::dgListNode* const edge)
{
typename dgGraph<dgNodeData, dgEdgeData>::dgListNode* const node = edge->GetInfo().m_node;
for (typename dgGraphNode<dgNodeData, dgEdgeData>::dgListNode* twinEdge = node->GetInfo().GetFirst(); twinEdge; twinEdge = twinEdge->GetNext()) {
if (&twinEdge->GetInfo().m_node->GetInfo() == this) {
node->GetInfo().DeleteHalfEdge(twinEdge);
break;
}
}
DeleteHalfEdge(edge);
}
template<class dgNodeData, class dgEdgeData>
dgGraphEdge<dgNodeData, dgEdgeData>::dgGraphEdge()
{
}
template<class dgNodeData, class dgEdgeData>
dgGraphEdge<dgNodeData, dgEdgeData>::~dgGraphEdge()
{
}
#endif

435
thirdparty/src/newton/dgCore/dgHeap.h vendored Normal file
View File

@@ -0,0 +1,435 @@
/* Copyright (c) <2003-2019> <Julio Jerez, Newton Game Dynamics>
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
*
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
*
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
*
* 3. This notice may not be removed or altered from any source distribution.
*/
/****************************************************************************
*
* Visual C++ 6.0 created by: Julio Jerez
*
****************************************************************************/
#ifndef __dgHeapBase__
#define __dgHeapBase__
#include "dgStdafx.h"
#include "dgMemory.h"
//#define DG_HEAP_DEBUG_CHECK
template <class OBJECT, class KEY>
class dgHeapBase
{
protected:
struct RECORD
{
KEY m_key;
OBJECT m_obj;
RECORD (KEY key, const OBJECT& obj)
:m_key(key), m_obj(obj)
{
}
};
dgHeapBase (dgInt32 maxElements, dgMemoryAllocator* const allocator);
dgHeapBase (const void * const buffer, dgInt32 sizeInBytes);
~dgHeapBase ();
public:
DG_CLASS_ALLOCATOR(allocator)
void Flush ();
KEY MaxValue() const;
KEY Value(dgInt32 i = 0) const;
dgInt32 GetCount() const;
dgInt32 GetMaxCount() const;
const OBJECT& operator[] (dgInt32 i) const;
dgInt32 Find (OBJECT &obj);
dgInt32 Find (KEY key);
dgInt32 m_curCount;
dgInt32 m_maxCount;
dgMemoryAllocator* m_allocator;
RECORD *m_pool;
};
template <class OBJECT, class KEY>
class dgDownHeap: public dgHeapBase<OBJECT, KEY>
{
public:
dgDownHeap (dgInt32 maxElements, dgMemoryAllocator* const allocator);
dgDownHeap (const void * const buffer, dgInt32 sizeInBytes);
void Pop () {Remove (0);}
void Push (OBJECT &obj, KEY key);
void Sort ();
void Remove (dgInt32 Index);
bool SanityCheck();
};
template <class OBJECT, class KEY>
class dgUpHeap: public dgHeapBase<OBJECT, KEY>
{
public:
dgUpHeap (dgInt32 maxElements, dgMemoryAllocator* const allocator);
dgUpHeap (const void * const buffer, dgInt32 sizeInBytes);
void Pop () {Remove (0);}
void Push (OBJECT &obj, KEY key);
void Sort ();
void Remove (dgInt32 Index);
bool SanityCheck();
};
template <class OBJECT, class KEY>
dgHeapBase<OBJECT,KEY>::dgHeapBase (dgInt32 maxElements, dgMemoryAllocator* const allocator)
{
m_allocator = allocator;
m_pool = (RECORD *)m_allocator->Malloc (maxElements * sizeof (RECORD));
m_maxCount = maxElements;
Flush();
}
template <class OBJECT, class KEY>
dgHeapBase<OBJECT,KEY>::dgHeapBase (const void * const buffer, dgInt32 sizeInBytes)
{
m_allocator = NULL;
m_pool = (RECORD *) buffer;
m_maxCount = dgInt32 (sizeInBytes / sizeof (RECORD));
Flush();
}
template <class OBJECT, class KEY>
dgHeapBase<OBJECT,KEY>::~dgHeapBase ()
{
if (m_allocator) {
m_allocator->Free (m_pool);
}
}
template <class OBJECT, class KEY>
KEY dgHeapBase<OBJECT,KEY>::Value(dgInt32 i) const
{
return m_pool[i].m_key;
}
template <class OBJECT, class KEY>
dgInt32 dgHeapBase<OBJECT,KEY>::GetCount() const
{
return m_curCount;
}
template <class OBJECT, class KEY>
void dgHeapBase<OBJECT,KEY>::Flush ()
{
m_curCount = 0;
#ifdef _DEBUG
// dgHeapBase<OBJECT,KEY>::m_pool[dgHeapBase<OBJECT,KEY>::m_curCount].m_key = KEY (0);
#endif
}
template <class OBJECT, class KEY>
KEY dgHeapBase<OBJECT,KEY>::MaxValue() const
{
return m_pool[0].m_key;
}
template <class OBJECT, class KEY>
dgInt32 dgHeapBase<OBJECT,KEY>::GetMaxCount() const
{
return m_maxCount;
}
template <class OBJECT, class KEY>
dgInt32 dgHeapBase<OBJECT,KEY>::Find (OBJECT &obj)
{
// For now let perform a linear search
// this is efficient if the size of the heap is small
// ex: m_curCount < 32
// this will be change to a binary search in the heap should the
// the size of the heap get larger than 32
// dgAssert (m_curCount <= 32);
for (dgInt32 i = 0; i < m_curCount; i ++) {
if (m_pool[i].obj == obj) {
return i;
}
}
return - 1;
}
template <class OBJECT, class KEY>
dgInt32 dgHeapBase<OBJECT,KEY>::Find (KEY key)
{
// ex: m_curCount < 32
// this will be change to a binary search in the heap shoud the
// the size of the heap get larger than 32
dgAssert (m_curCount <= 32);
for (dgInt32 i = 0; i < m_curCount; i ++) {
if (m_pool[i].m_key == key) {
return i;
}
}
return - 1;
}
template <class OBJECT, class KEY>
const OBJECT& dgHeapBase<OBJECT,KEY>::operator[] (dgInt32 i) const
{
dgAssert (i<= m_curCount);
return m_pool[i].m_obj;
}
// **************************************************************************
//
// down Heap
//
// **************************************************************************
template <class OBJECT, class KEY>
dgDownHeap<OBJECT,KEY>::dgDownHeap (dgInt32 maxElements, dgMemoryAllocator* const allocator)
:dgHeapBase<OBJECT, KEY> (maxElements, allocator)
{
}
template <class OBJECT, class KEY>
dgDownHeap<OBJECT,KEY>::dgDownHeap (const void * const buffer, dgInt32 sizeInBytes)
:dgHeapBase<OBJECT, KEY> (buffer, sizeInBytes)
{
}
template <class OBJECT, class KEY>
void dgDownHeap<OBJECT,KEY>::Push (OBJECT &obj, KEY key)
{
dgHeapBase<OBJECT,KEY>::m_curCount ++;
dgInt32 j;
dgInt32 i = dgHeapBase<OBJECT,KEY>::m_curCount;
for (; i; i = j) {
j = i >> 1;
if (!j || (dgHeapBase<OBJECT,KEY>::m_pool[j - 1].m_key > key)) {
break;
}
dgHeapBase<OBJECT,KEY>::m_pool[i - 1] = dgHeapBase<OBJECT,KEY>::m_pool[j - 1];
}
dgAssert (i);
dgHeapBase<OBJECT,KEY>::m_pool[i - 1].m_key = key;
dgHeapBase<OBJECT,KEY>::m_pool[i - 1].m_obj = obj;
dgAssert (SanityCheck());
}
template <class OBJECT, class KEY>
void dgDownHeap<OBJECT,KEY>::Remove (dgInt32 index)
{
dgHeapBase<OBJECT, KEY>::m_curCount--;
dgHeapBase<OBJECT, KEY>::m_pool[index] = dgHeapBase<OBJECT, KEY>::m_pool[dgHeapBase<OBJECT, KEY>::m_curCount];
while (index && dgHeapBase<OBJECT, KEY>::m_pool[(index - 1) >> 1].m_key < dgHeapBase<OBJECT, KEY>::m_pool[index].m_key) {
dgSwap(dgHeapBase<OBJECT, KEY>::m_pool[(index - 1) >> 1], dgHeapBase<OBJECT, KEY>::m_pool[index]);
index = (index - 1) >> 1;
}
while ((2 * index + 1) < dgHeapBase<OBJECT, KEY>::m_curCount) {
dgInt32 i0 = 2 * index + 1;
dgInt32 i1 = 2 * index + 2;
if (i1 < dgHeapBase<OBJECT, KEY>::m_curCount) {
i0 = (dgHeapBase<OBJECT, KEY>::m_pool[i0].m_key > dgHeapBase<OBJECT, KEY>::m_pool[i1].m_key) ? i0 : i1;
if (dgHeapBase<OBJECT, KEY>::m_pool[i0].m_key <= dgHeapBase<OBJECT, KEY>::m_pool[index].m_key) {
break;
}
dgSwap(dgHeapBase<OBJECT, KEY>::m_pool[i0], dgHeapBase<OBJECT, KEY>::m_pool[index]);
index = i0;
} else {
if (dgHeapBase<OBJECT, KEY>::m_pool[i0].m_key > dgHeapBase<OBJECT, KEY>::m_pool[index].m_key) {
dgSwap(dgHeapBase<OBJECT, KEY>::m_pool[i0], dgHeapBase<OBJECT, KEY>::m_pool[index]);
}
index = i0;
}
}
dgAssert (SanityCheck());
}
template <class OBJECT, class KEY>
void dgDownHeap<OBJECT,KEY>::Sort ()
{
dgInt32 count = dgHeapBase<OBJECT,KEY>::m_curCount;
for (dgInt32 i = 1; i < count; i ++) {
KEY key (dgHeapBase<OBJECT,KEY>::m_pool[0].m_key);
OBJECT obj (dgHeapBase<OBJECT,KEY>::m_pool[0].m_obj);
Pop();
dgHeapBase<OBJECT,KEY>::m_pool[dgHeapBase<OBJECT,KEY>::m_curCount].m_key = key;
dgHeapBase<OBJECT,KEY>::m_pool[dgHeapBase<OBJECT,KEY>::m_curCount].m_obj = obj;
}
dgHeapBase<OBJECT,KEY>::m_curCount = count;
for (dgInt32 i = 0; i < count / 2; i ++) {
KEY key (dgHeapBase<OBJECT,KEY>::m_pool[i].m_key);
OBJECT obj (dgHeapBase<OBJECT,KEY>::m_pool[i].m_obj);
dgHeapBase<OBJECT,KEY>::m_pool[i].m_key = dgHeapBase<OBJECT,KEY>::m_pool[count - i - 1].m_key;
dgHeapBase<OBJECT,KEY>::m_pool[i].m_obj = dgHeapBase<OBJECT,KEY>::m_pool[count - i - 1].m_obj;
dgHeapBase<OBJECT,KEY>::m_pool[count - i - 1].m_key = key;
dgHeapBase<OBJECT,KEY>::m_pool[count - i - 1].m_obj = obj;
}
dgAssert (SanityCheck());
}
template <class OBJECT, class KEY>
bool dgDownHeap<OBJECT,KEY>::SanityCheck()
{
#ifdef DG_HEAP_DEBUG_CHECK
for (dgInt32 i = 0; i < m_curCount; i++) {
dgInt32 i1 = 2 * i + 1;
dgInt32 i2 = 2 * i + 2;
if ((i1 < m_curCount) && (dgHeapBase<OBJECT, KEY>::m_pool[i].m_key < dgHeapBase<OBJECT, KEY>::m_pool[i1].m_key)) {
return false;
}
if ((i2 < m_curCount) && (dgHeapBase<OBJECT, KEY>::m_pool[i].m_key < dgHeapBase<OBJECT, KEY>::m_pool[i2].m_key)) {
return false;
}
}
#endif
return true;
}
// **************************************************************************
//
// down Heap
//
// **************************************************************************
template <class OBJECT, class KEY>
dgUpHeap<OBJECT,KEY>::dgUpHeap (dgInt32 maxElements, dgMemoryAllocator* const allocator)
:dgHeapBase<OBJECT, KEY> (maxElements, allocator)
{
}
template <class OBJECT, class KEY>
dgUpHeap<OBJECT,KEY>::dgUpHeap (const void * const buffer, dgInt32 sizeInBytes)
:dgHeapBase<OBJECT, KEY> (buffer, sizeInBytes)
{
}
template <class OBJECT, class KEY>
bool dgUpHeap<OBJECT,KEY>::SanityCheck()
{
#ifdef DG_HEAP_DEBUG_CHECK
for (dgInt32 i = 0; i < m_curCount; i ++) {
dgInt32 i1 = 2 * i + 1;
dgInt32 i2 = 2 * i + 2;
if ((i1 < m_curCount) && (dgHeapBase<OBJECT,KEY>::m_pool[i].m_key > dgHeapBase<OBJECT,KEY>::m_pool[i1].m_key)) {
return false;
}
if ((i2 < m_curCount) && (dgHeapBase<OBJECT,KEY>::m_pool[i].m_key > dgHeapBase<OBJECT,KEY>::m_pool[i2].m_key)) {
return false;
}
}
#endif
return true;
}
template <class OBJECT, class KEY>
void dgUpHeap<OBJECT,KEY>::Push (OBJECT &obj, KEY key)
{
dgHeapBase<OBJECT,KEY>::m_curCount ++;
dgInt32 j;
dgInt32 i = dgHeapBase<OBJECT,KEY>::m_curCount;
for (; i; i = j) {
j = i >> 1;
if (!j || (dgHeapBase<OBJECT,KEY>::m_pool[j - 1].m_key < key)) {
break;
}
dgHeapBase<OBJECT,KEY>::m_pool[i - 1] = dgHeapBase<OBJECT,KEY>::m_pool[j - 1];
}
dgAssert (i);
dgHeapBase<OBJECT,KEY>::m_pool[i - 1].m_key = key;
dgHeapBase<OBJECT,KEY>::m_pool[i - 1].m_obj = obj;
dgAssert (SanityCheck());
}
template <class OBJECT, class KEY>
void dgUpHeap<OBJECT,KEY>::Sort ()
{
dgInt32 count = dgHeapBase<OBJECT,KEY>::m_curCount;
for (dgInt32 i = 1; i < count; i ++) {
KEY key (dgHeapBase<OBJECT,KEY>::m_pool[0].m_key);
OBJECT obj (dgHeapBase<OBJECT,KEY>::m_pool[0].m_obj);
Pop();
dgHeapBase<OBJECT,KEY>::m_pool[dgHeapBase<OBJECT,KEY>::m_curCount].m_key = key;
dgHeapBase<OBJECT,KEY>::m_pool[dgHeapBase<OBJECT,KEY>::m_curCount].m_obj = obj;
}
dgHeapBase<OBJECT,KEY>::m_curCount = count;
for (dgInt32 i = 0; i < count / 2; i ++) {
KEY key (dgHeapBase<OBJECT,KEY>::m_pool[i].m_key);
OBJECT obj (dgHeapBase<OBJECT,KEY>::m_pool[i].m_obj);
dgHeapBase<OBJECT,KEY>::m_pool[i].m_key = dgHeapBase<OBJECT,KEY>::m_pool[count - i - 1].m_key;
dgHeapBase<OBJECT,KEY>::m_pool[i].m_obj = dgHeapBase<OBJECT,KEY>::m_pool[count - i - 1].m_obj;
dgHeapBase<OBJECT,KEY>::m_pool[count - i - 1].m_key = key;
dgHeapBase<OBJECT,KEY>::m_pool[count - i - 1].m_obj = obj;
}
dgAssert (SanityCheck());
}
template <class OBJECT, class KEY>
void dgUpHeap<OBJECT,KEY>::Remove (dgInt32 index)
{
dgHeapBase<OBJECT, KEY>::m_curCount--;
dgHeapBase<OBJECT, KEY>::m_pool[index] = dgHeapBase<OBJECT, KEY>::m_pool[dgHeapBase<OBJECT, KEY>::m_curCount];
while (index && dgHeapBase<OBJECT, KEY>::m_pool[(index - 1) >> 1].m_key > dgHeapBase<OBJECT, KEY>::m_pool[index].m_key) {
dgSwap(dgHeapBase<OBJECT, KEY>::m_pool[(index - 1) >> 1], dgHeapBase<OBJECT, KEY>::m_pool[index]);
index = (index - 1) >> 1;
}
while ((2 * index + 1) < dgHeapBase<OBJECT, KEY>::m_curCount) {
dgInt32 i0 = 2 * index + 1;
dgInt32 i1 = 2 * index + 2;
if (i1 < dgHeapBase<OBJECT, KEY>::m_curCount) {
i0 = (dgHeapBase<OBJECT, KEY>::m_pool[i0].m_key < dgHeapBase<OBJECT, KEY>::m_pool[i1].m_key) ? i0 : i1;
if (dgHeapBase<OBJECT, KEY>::m_pool[i0].m_key >= dgHeapBase<OBJECT, KEY>::m_pool[index].m_key) {
break;
}
dgSwap(dgHeapBase<OBJECT, KEY>::m_pool[i0], dgHeapBase<OBJECT, KEY>::m_pool[index]);
index = i0;
} else {
if (dgHeapBase<OBJECT, KEY>::m_pool[i0].m_key < dgHeapBase<OBJECT, KEY>::m_pool[index].m_key) {
dgSwap(dgHeapBase<OBJECT, KEY>::m_pool[i0], dgHeapBase<OBJECT, KEY>::m_pool[index]);
}
index = i0;
}
}
dgAssert (SanityCheck());
}
#endif

400
thirdparty/src/newton/dgCore/dgIntersections.cpp vendored Normal file
View File

@@ -0,0 +1,400 @@
/* Copyright (c) <2003-2019> <Julio Jerez, Newton Game Dynamics>
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
*
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
*
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
*
* 3. This notice may not be removed or altered from any source distribution.
*/
#include "dgStdafx.h"
#include "dgMemory.h"
#include "dgVector.h"
#include "dgPlane.h"
#include "dgGoogol.h"
#include "dgIntersections.h"
#define USE_FLOAT_VERSION
#define DG_RAY_TOL_ERROR (dgFloat32 (-1.0e-3f))
#define DG_RAY_TOL_ADAPTIVE_ERROR (dgFloat32 (1.0e-1f))
dgFloat32 dgFastRayTest::PolygonIntersect (const dgVector& faceNormal, dgFloat32 maxT, const dgFloat32* const polygon, dgInt32 strideInBytes, const dgInt32* const indexArray, dgInt32 indexCount) const
{
dgAssert (m_p0.m_w == dgFloat32 (0.0f));
dgAssert (m_p1.m_w == dgFloat32 (0.0f));
if (faceNormal.DotProduct(m_unitDir).GetScalar() < dgFloat32 (0.0f)) {
dgInt32 stride = dgInt32(strideInBytes / sizeof (dgFloat32));
dgBigVector v0(dgVector(&polygon[indexArray[indexCount - 1] * stride]) & dgVector::m_triplexMask);
dgBigVector p0(m_p0);
dgBigVector p0v0(v0 - p0);
dgBigVector diff(m_diff);
dgBigVector normal(faceNormal);
dgFloat64 tOut = normal.DotProduct(p0v0).GetScalar() / normal.DotProduct(diff).GetScalar();
if ((tOut >= dgFloat64(0.0f)) && (tOut <= maxT)) {
dgBigVector p (p0 + diff.Scale (tOut));
dgBigVector unitDir(m_unitDir);
for (dgInt32 i = 0; i < indexCount; i++) {
dgInt32 i2 = indexArray[i] * stride;
dgBigVector v1(dgVector(&polygon[i2]) & dgVector::m_triplexMask);
dgBigVector edge0(p - v0);
dgBigVector edge1(v1 - v0);
dgFloat64 area = unitDir.DotProduct (edge0.CrossProduct(edge1)).GetScalar();
if (area < dgFloat32 (0.0f)) {
return 1.2f;
}
v0 = v1;
}
return dgFloat32(tOut);
}
}
return dgFloat32 (1.2f);
}
bool dgApi dgRayBoxClip (dgVector& p0, dgVector& p1, const dgVector& boxP0, const dgVector& boxP1)
{
dgAssert (p0.m_w == dgFloat32(0.0f));
dgAssert (p1.m_w == dgFloat32(0.0f));
dgAssert (boxP0.m_w == dgFloat32(0.0f));
dgAssert (boxP1.m_w == dgFloat32(0.0f));
for (int i = 0; i < 3; i ++) {
dgFloat32 tmp0 = boxP1[i] - p0[i];
if (tmp0 > dgFloat32 (0.0f)) {
dgFloat32 tmp1 = boxP1[i] - p1[i];
if (tmp1 < dgFloat32 (0.0f)) {
p1 = p0 + (p1 - p0).Scale (tmp0 / (p1[i] - p0[i]));
p1[i] = boxP1[i];
}
} else {
dgFloat32 tmp1 = boxP1[i] - p1[i];
if (tmp1 > dgFloat32 (0.0f)) {
p0 += (p1 - p0).Scale (tmp0 / (p1[i] - p0[i]));
p0[i] = boxP1[i];
} else {
return false;
}
}
tmp0 = boxP0[i] - p0[i];
if (tmp0 < dgFloat32 (0.0f)) {
dgFloat32 tmp1 = boxP0[i] - p1[i];
if (tmp1 > dgFloat32 (0.0f)) {
p1 = p0 + (p1 - p0).Scale (tmp0 / (p1[i] - p0[i]));
p1[i] = boxP0[i];
}
} else {
dgFloat32 tmp1 = boxP0[i] - p1[i];
if (tmp1 < dgFloat32 (0.0f)) {
p0 += (p1 - p0).Scale (tmp0 / (p1[i] - p0[i]));
p0[i] = boxP0[i];
} else {
return false;
}
}
}
return true;
}
dgBigVector dgPointToRayDistance (const dgBigVector& point, const dgBigVector& ray_p0, const dgBigVector& ray_p1)
{
dgBigVector dp (ray_p1 - ray_p0);
dgAssert (dp.m_w == dgFloat32 (0.0f));
dgFloat64 t = dgClamp (dp.DotProduct(point - ray_p0).GetScalar() / dp.DotProduct(dp).GetScalar(), dgFloat64(0.0f), dgFloat64 (1.0f));
return ray_p0 + dp.Scale (t);
}
dgBigVector dgPointToTriangleDistance(const dgBigVector& point, const dgBigVector& p0, const dgBigVector& p1, const dgBigVector& p2)
{
const dgBigVector e10(p1 - p0);
const dgBigVector e20(p2 - p0);
const dgFloat64 a00 = e10.DotProduct(e10).GetScalar();
const dgFloat64 a11 = e20.DotProduct(e20).GetScalar();
const dgFloat64 a01 = e10.DotProduct(e20).GetScalar();
const dgFloat64 det = a00 * a11 - a01 * a01;
dgAssert(det >= dgFloat32(0.0f));
if (dgAbs(det) > dgFloat32(1.0e-24f)) {
dgBigVector p0Point (point - p0);
const dgFloat64 b0 = e10.DotProduct(p0Point).GetScalar();
const dgFloat64 b1 = e20.DotProduct(p0Point).GetScalar();
const dgFloat64 beta = b1 * a00 - a01 * b0;
const dgFloat64 alpha = b0 * a11 - a01 * b1;
if (beta < dgFloat32(0.0f)) {
return dgPointToRayDistance (point, p0, p1);
} else if (alpha < dgFloat32(0.0f)) {
return dgPointToRayDistance (point, p0, p2);
} else if ((alpha + beta) > det) {
return dgPointToRayDistance (point, p1, p2);
}
return p0 + (e10.Scale(alpha) + e20.Scale(beta)).Scale(dgFloat64(1.0f) / det);
}
// this is a degenerated triangle. this should never happens
dgAssert(0);
return p0;
}
dgBigVector dgPointToTetrahedrumDistance (const dgBigVector& point, const dgBigVector& p0, const dgBigVector& p1, const dgBigVector& p2, const dgBigVector& p3)
{
const dgBigVector e10(p1 - p0);
const dgBigVector e20(p2 - p0);
const dgBigVector e30(p3 - p0);
const dgFloat64 d0 = sqrt(e10.DotProduct(e10).GetScalar());
if (d0 > dgFloat64(0.0f)) {
const dgFloat64 invd0 = dgFloat64(1.0f) / d0;
const dgFloat64 l10 = e20.DotProduct(e10).GetScalar() * invd0;
const dgFloat64 l20 = e30.DotProduct(e10).GetScalar() * invd0;
const dgFloat64 desc11 = e20.DotProduct(e20).GetScalar() - l10 * l10;
if (desc11 > dgFloat64(0.0f)) {
const dgFloat64 d1 = sqrt(desc11);
const dgFloat64 invd1 = dgFloat64(1.0f) / d1;
const dgFloat64 l21 = (e30.DotProduct(e20).GetScalar() - l20 * l10) * invd1;
const dgFloat64 desc22 = e30.DotProduct(e30).GetScalar() - l20 * l20 - l21 * l21;
if (desc22 > dgFloat64(0.0f)) {
dgBigVector p0Point (point - p0);
const dgFloat64 d2 = sqrt(desc22);
const dgFloat64 invd2 = dgFloat64(1.0f) / d2;
const dgFloat64 b0 = e10.DotProduct(p0Point).GetScalar();
const dgFloat64 b1 = e20.DotProduct(p0Point).GetScalar();
const dgFloat64 b2 = e30.DotProduct(p0Point).GetScalar();
dgFloat64 u1 = b0 * invd0;
dgFloat64 u2 = (b1 - l10 * u1) * invd1;
dgFloat64 u3 = (b2 - l20 * u1 - l21 * u2) * invd2 * invd2;
u2 = (u2 - l21 * u3) * invd1;
u1 = (u1 - l10 * u2 - l20 * u3) * invd0;
if (u3 < dgFloat64(0.0f)) {
// this looks funny but it is correct
return dgPointToTriangleDistance(point, p0, p1, p2);
} else if (u2 < dgFloat64(0.0f)) {
return dgPointToTriangleDistance(point, p0, p1, p3);
} else if (u1 < dgFloat64(0.0f)) {
return dgPointToTriangleDistance(point, p0, p2, p3);
} else if (u1 + u2 + u3 > dgFloat64(1.0f)) {
return dgPointToTriangleDistance(point, p1, p2, p3);
}
return p0 + e10.Scale(u1) + e20.Scale(u2) + e30.Scale(u3);
}
}
}
// this is a degenerated tetra. this should never happens
dgAssert(0);
return p0;
}
void dgApi dgRayToRayDistance (const dgVector& ray_p0, const dgVector& ray_p1, const dgVector& ray_q0, const dgVector& ray_q1, dgVector& pOut, dgVector& qOut)
{
dgFloat32 sN;
dgFloat32 tN;
dgVector u (ray_p1 - ray_p0);
dgVector v (ray_q1 - ray_q0);
dgVector w (ray_p0 - ray_q0);
dgAssert (u.m_w == dgFloat32 (0.0f));
dgAssert (v.m_w == dgFloat32 (0.0f));
dgAssert (w.m_w == dgFloat32 (0.0f));
dgFloat32 a = u.DotProduct(u).GetScalar();
dgFloat32 b = u.DotProduct(v).GetScalar();
dgFloat32 c = v.DotProduct(v).GetScalar();
dgFloat32 d = u.DotProduct(w).GetScalar();
dgFloat32 e = v.DotProduct(w).GetScalar();
dgFloat32 D = a*c - b*b;
dgFloat32 sD = D;
dgFloat32 tD = D;
// compute the line parameters of the two closest points
if (D < dgFloat32 (1.0e-8f)) {
sN = dgFloat32 (0.0f);
sD = dgFloat32 (1.0f);
tN = e;
tD = c;
} else {
// get the closest points on the infinite lines
sN = (b*e - c*d);
tN = (a*e - b*d);
if (sN < dgFloat32 (0.0f)) {
// sc < 0 => the s=0 edge is visible
sN = dgFloat32 (0.0f);
tN = e;
tD = c;
} else if (sN > sD) {
// sc > 1 => the s=1 edge is visible
sN = sD;
tN = e + b;
tD = c;
}
}
if (tN < dgFloat32 (0.0f)) {
// tc < 0 => the t=0 edge is visible
tN = dgFloat32 (0.0f);
// recompute sc for this edge
if (-d < dgFloat32 (0.0f)) {
sN = dgFloat32 (0.0f);
} else if (-d > a) {
sN = sD;
} else {
sN = -d;
sD = a;
}
} else if (tN > tD) {
// tc > 1 => the t=1 edge is visible
tN = tD;
// recompute sc for this edge
if ((-d + b) < dgFloat32 (0.0f)) {
sN = dgFloat32 (0.0f);
} else if ((-d + b) > a) {
sN = sD;
} else {
sN = (-d + b);
sD = a;
}
}
// finally do the division to get sc and tc
dgFloat32 sc = (dgAbs(sN) < dgFloat32(1.0e-8f) ? dgFloat32 (0.0f) : sN / sD);
dgFloat32 tc = (dgAbs(tN) < dgFloat32(1.0e-8f) ? dgFloat32 (0.0f) : tN / tD);
dgAssert (u.m_w == dgFloat32 (0.0f));
dgAssert (v.m_w == dgFloat32 (0.0f));
pOut = ray_p0 + u.Scale (sc);
qOut = ray_q0 + v.Scale (tc);
}
dgFloat32 dgRayCastSphere (const dgVector& p0, const dgVector& p1, const dgVector& origin, dgFloat32 radius)
{
dgVector p0Origin (p0 - origin);
dgAssert (p0Origin.m_w == dgFloat32 (0.0f));
if (p0Origin.DotProduct(p0Origin).GetScalar() < (dgFloat32 (100.0f) * radius * radius)) {
dgVector dp (p1 - p0);
dgAssert (dp.m_w == dgFloat32 (0.0f));
dgFloat32 a = dp.DotProduct(dp).GetScalar();
dgFloat32 b = dgFloat32 (2.0f) * p0Origin.DotProduct(dp).GetScalar();
dgFloat32 c = p0Origin.DotProduct(p0Origin).GetScalar() - radius * radius;
dgFloat32 desc = b * b - dgFloat32 (4.0f) * a * c;
if (desc >= 0.0f) {
desc = dgSqrt (desc);
dgFloat32 den = dgFloat32 (0.5f) / a;
dgFloat32 t0 = (-b + desc) * den;
dgFloat32 t1 = (-b - desc) * den;
if ((t0 >= dgFloat32 (0.0f)) && (t1 >= dgFloat32 (0.0f))) {
t0 = dgMin(t0, t1);
if (t0 <= dgFloat32 (1.0f)) {
return t0;
}
} else if (t0 >= dgFloat32 (0.0f)) {
if (t0 <= dgFloat32 (1.0f)) {
return t0;
}
} else {
if ((t1 >= dgFloat32 (0.0f)) && (t1 <= dgFloat32 (1.0f))) {
return t1;
}
}
}
} else {
dgBigVector p0Origin1 (p0Origin);
dgBigVector dp (p1 - p0);
dgAssert (dp.m_w == dgFloat32 (0.0f));
dgFloat64 a = dp.DotProduct(dp).GetScalar();
dgFloat64 b = dgFloat32 (2.0f) * p0Origin1.DotProduct(dp).GetScalar();
dgFloat64 c = p0Origin1.DotProduct(p0Origin1).GetScalar() - dgFloat64(radius) * radius;
dgFloat64 desc = b * b - dgFloat32 (4.0f) * a * c;
if (desc >= 0.0f) {
desc = sqrt (desc);
dgFloat64 den = dgFloat32 (0.5f) / a;
dgFloat64 t0 = (-b + desc) * den;
dgFloat64 t1 = (-b - desc) * den;
if ((t0 >= dgFloat32 (0.0f)) && (t1 >= dgFloat32 (0.0f))) {
t0 = dgMin(t0, t1);
if (t0 <= dgFloat32 (1.0f)) {
return dgFloat32 (t0);
}
} else if (t0 >= dgFloat32 (0.0f)) {
if (t0 <= dgFloat32 (1.0f)) {
return dgFloat32 (t0);
}
} else {
if ((t1 >= dgFloat32 (0.0f)) && (t1 <= dgFloat32 (1.0f))) {
return dgFloat32 (t1);
}
}
}
}
return dgFloat32 (1.2f);
}
dgFloat32 dgRayCastBox (const dgVector& p0, const dgVector& p1, const dgVector& boxP0, const dgVector& boxP1, dgVector& normalOut)
{
dgInt32 index = 0;
dgFloat32 signDir = dgFloat32 (0.0f);
dgFloat32 tmin = dgFloat32 (0.0f);
dgFloat32 tmax = dgFloat32 (1.0f);
//dgVector size (boxP1 - boxP0);
for (dgInt32 i = 0; i < 3; i++) {
dgFloat32 dp = p1[i] - p0[i];
if (dgAbs (dp) < dgFloat32 (1.0e-8f)) {
if (p0[i] <= boxP0[i] || p0[i] >= boxP1[i]) {
return dgFloat32 (1.2f);
}
} else {
dp = dgFloat32 (1.0f) / dp;
dgFloat32 t1 = (boxP0[i] - p0[i]) * dp;
dgFloat32 t2 = (boxP1[i] - p0[i]) * dp;
dgFloat32 sign = dgFloat32 (-1.0f);
if (t1 > t2) {
sign = 1;
dgSwap(t1, t2);
}
if (t1 > tmin) {
signDir = sign;
index = i;
tmin = t1;
}
if (t2 < tmax) {
tmax = t2;
}
if (tmin > tmax) {
return dgFloat32 (1.2f);
}
}
}
if (tmin > dgFloat32 (0.0f)) {
dgAssert (tmin < 1.0f);
normalOut = dgVector (dgFloat32 (0.0f));
normalOut[index] = signDir;
} else {
tmin = dgFloat32 (1.2f);
}
return tmin;
}

393
thirdparty/src/newton/dgCore/dgIntersections.h vendored Normal file
View File

@@ -0,0 +1,393 @@
/* Copyright (c) <2003-2019> <Julio Jerez, Newton Game Dynamics>
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
*
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
*
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
*
* 3. This notice may not be removed or altered from any source distribution.
*/
#ifndef __dgIntersections__
#define __dgIntersections__
#include "dgStdafx.h"
#include "dgObb.h"
#include "dgDebug.h"
#include "dgVector.h"
#include "dgMatrix.h"
class dgPlane;
class dgObject;
class dgPolyhedra;
enum dgIntersectStatus
{
t_StopSearh,
t_ContinueSearh
};
typedef dgIntersectStatus (*dgAABBIntersectCallback) (void* const context,
const dgFloat32* const polygon, dgInt32 strideInBytes,
const dgInt32* const indexArray, dgInt32 indexCount, dgFloat32 hitDistance);
typedef dgFloat32 (*dgRayIntersectCallback) (void* const context,
const dgFloat32* const polygon, dgInt32 strideInBytes,
const dgInt32* const indexArray, dgInt32 indexCount);
dgBigVector dgPointToRayDistance (const dgBigVector& point, const dgBigVector& ray_p0, const dgBigVector& ray_p1);
dgBigVector dgPointToTriangleDistance (const dgBigVector& point, const dgBigVector& p0, const dgBigVector& p1, const dgBigVector& p2);
dgBigVector dgPointToTetrahedrumDistance (const dgBigVector& point, const dgBigVector& p0, const dgBigVector& p1, const dgBigVector& p2, const dgBigVector& p3);
bool dgRayBoxClip (dgVector& ray_p0, dgVector& ray_p1, const dgVector& boxP0, const dgVector& boxP1);
void dgRayToRayDistance (const dgVector& ray_p0, const dgVector& ray_p1, const dgVector& ray_q0, const dgVector& ray_q1, dgVector& p0Out, dgVector& p1Out);
dgFloat32 dgRayCastBox (const dgVector& p0, const dgVector& p1, const dgVector& boxP0, const dgVector& boxP1, dgVector& normalOut);
dgFloat32 dgRayCastSphere (const dgVector& p0, const dgVector& p1, const dgVector& origin, dgFloat32 radius);
DG_INLINE dgInt32 dgOverlapTest (const dgVector& p0, const dgVector& p1, const dgVector& q0, const dgVector& q1)
{
dgVector val((p0 - q1) * (p1 - q0));
dgInt32 mask = val.GetSignMask();
return ((mask & 0x07) == 0x07);
}
DG_INLINE dgInt32 dgBoxInclusionTest (const dgVector& p0, const dgVector& p1, const dgVector& q0, const dgVector& q1)
{
dgVector val (dgVector::m_negOne & ((p0 >= q0) & (p1 <= q1)));
dgInt32 mask = val.GetSignMask();
return ((mask & 0x07) == 0x07);
}
DG_INLINE dgInt32 dgCompareBox (const dgVector& p0, const dgVector& p1, const dgVector& q0, const dgVector& q1)
{
dgAssert(0);
return (p0.m_x != q0.m_x) || (p0.m_y != q0.m_y) || (p0.m_z != q0.m_z) || (p1.m_x != q1.m_x) || (p1.m_y != q1.m_y) || (p1.m_z != q1.m_z);
}
DG_INLINE void dgMovingAABB (dgVector& p0, dgVector& p1, const dgVector& veloc, const dgVector& omega, dgFloat32 timestep, dgFloat32 maxRadius, dgFloat32 minRadius)
{
dgVector linearStep (veloc.Scale (timestep));
// estimate the maximum effect of the angular velocity and enlarge that box by that value (use 45 degrees as max angle not 90)
dgAssert (omega.m_w == dgFloat32 (0.0f));
dgFloat32 maxAngle = dgMin (dgSqrt (omega.DotProduct(omega).GetScalar() * timestep * timestep), dgFloat32 (45.0f * dgDegreeToRad));
dgFloat32 angularTravel = (maxRadius - minRadius) * maxAngle;
dgVector angularStep (angularTravel, angularTravel, angularTravel, dgFloat32 (0.0f));
dgVector r0 (p0 - angularStep);
dgVector r1 (p1 + angularStep);
dgVector q0 (r0 + linearStep);
dgVector q1 (r1 + linearStep);
p0 = r0.GetMin (q0) & dgVector::m_triplexMask;
p1 = r1.GetMax (q1) & dgVector::m_triplexMask;
}
DG_INLINE dgFloat32 BoxPenetration (const dgVector& minBox, const dgVector& maxBox)
{
dgAssert(maxBox.m_x >= minBox.m_x);
dgAssert(maxBox.m_y >= minBox.m_y);
dgAssert(maxBox.m_z >= minBox.m_z);
dgVector mask ((minBox * maxBox) < dgVector::m_zero);
dgVector dist (maxBox.GetMin (minBox.Abs()) & mask);
dist = dist.GetMin(dist.ShiftTripleRight());
dist = dist.GetMin(dist.ShiftTripleRight());
return dist.GetScalar();
}
DG_INLINE dgFloat32 dgBoxDistanceToOrigin2 (const dgVector& minBox, const dgVector& maxBox)
{
dgAssert(maxBox.m_x >= minBox.m_x);
dgAssert(maxBox.m_y >= minBox.m_y);
dgAssert(maxBox.m_z >= minBox.m_z);
dgVector mask ((minBox * maxBox) > dgVector::m_zero);
dgVector dist (maxBox.Abs().GetMin (minBox.Abs()) & mask);
return dist.DotProduct(dist).GetScalar();
}
DG_MSC_VECTOR_ALIGNMENT
class dgFastRayTest
{
public:
DG_INLINE dgFastRayTest(const dgVector& l0, const dgVector& l1)
:m_p0(l0)
,m_p1(l1)
,m_diff((l1 - l0) & dgVector::m_triplexMask)
,m_minT(dgFloat32(0.0f))
,m_maxT(dgFloat32(1.0f))
{
dgAssert(m_p0.m_w == dgFloat32(0.0f));
dgAssert(m_p1.m_w == dgFloat32(0.0f));
dgAssert(m_diff.m_w == dgFloat32(0.0f));
dgAssert (m_diff.DotProduct(m_diff).GetScalar() > dgFloat32 (0.0f));
m_isParallel = (m_diff.Abs() < dgVector(1.0e-8f));
//m_dpInv = (((dgVector(dgFloat32(1.0e-20)) & m_isParallel) | m_diff.AndNot(m_isParallel)).Reciproc()) & dgVector::m_triplexMask;
m_dpInv = m_diff.Select (dgVector(dgFloat32(1.0e-20f)), m_isParallel).Reciproc() & dgVector::m_triplexMask;
m_unitDir = m_diff.Normalize();
}
dgFloat32 PolygonIntersect(const dgVector& normal, dgFloat32 maxT, const dgFloat32* const polygon, dgInt32 strideInBytes, const dgInt32* const indexArray, dgInt32 indexCount) const;
DG_INLINE dgInt32 BoxTest(const dgVector& minBox, const dgVector& maxBox) const
{
#if 1
dgVector test(((m_p0 <= minBox) | (m_p0 >= maxBox)) & m_isParallel);
if (test.GetSignMask() & 0x07) {
return 0;
}
dgVector tt0(m_dpInv * (minBox - m_p0));
dgVector tt1(m_dpInv * (maxBox - m_p0));
dgVector t0(m_minT.GetMax(tt0.GetMin(tt1)));
dgVector t1(m_maxT.GetMin(tt0.GetMax(tt1)));
t0 = t0.GetMax(t0.ShiftTripleRight());
t1 = t1.GetMin(t1.ShiftTripleRight());
t0 = t0.GetMax(t0.ShiftTripleRight());
t1 = t1.GetMin(t1.ShiftTripleRight());
return ((t0 < t1).GetSignMask() & 1);
#else
dgFloat32 tmin = 0.0f;
dgFloat32 tmax = 1.0f;
for (dgInt32 i = 0; i < 3; i++) {
if (m_isParallel[i]) {
if (m_p0[i] <= minBox[i] || m_p0[i] >= maxBox[i]) {
return 0;
}
} else {
dgFloat32 t1 = (minBox[i] - m_p0[i]) * m_dpInv[i];
dgFloat32 t2 = (maxBox[i] - m_p0[i]) * m_dpInv[i];
if (t1 > t2) {
dgSwap(t1, t2);
}
if (t1 > tmin) {
tmin = t1;
}
if (t2 < tmax) {
tmax = t2;
}
if (tmin > tmax) {
return 0;
}
}
}
return 0x1;
#endif
}
DG_INLINE dgFloat32 BoxIntersect(const dgVector& minBox, const dgVector& maxBox) const
{
dgVector test(((m_p0 <= minBox) | (m_p0 >= maxBox)) & m_isParallel);
if (test.GetSignMask() & 0x07) {
return dgFloat32(1.2f);
}
dgVector tt0(m_dpInv * (minBox - m_p0));
dgVector tt1(m_dpInv * (maxBox - m_p0));
dgVector t0(m_minT.GetMax(tt0.GetMin(tt1)));
dgVector t1(m_maxT.GetMin(tt0.GetMax(tt1)));
t0 = t0.GetMax(t0.ShiftTripleRight());
t1 = t1.GetMin(t1.ShiftTripleRight());
t0 = t0.GetMax(t0.ShiftTripleRight());
t1 = t1.GetMin(t1.ShiftTripleRight());
dgVector mask(t0 < t1);
dgVector maxDist(dgFloat32(1.2f));
//t0 = (t0 & mask) | maxDist.AndNot(mask);
t0 = maxDist.Select(t0, mask);
dgAssert((mask.GetSignMask() & 1) == (t0.m_x < dgFloat32(1.0f)));
return t0.GetScalar();
}
dgVector m_p0;
dgVector m_p1;
dgVector m_diff;
dgVector m_dpInv;
dgVector m_minT;
dgVector m_maxT;
dgVector m_unitDir;
dgVector m_isParallel;
} DG_GCC_VECTOR_ALIGNMENT;
DG_MSC_VECTOR_ALIGNMENT
class dgFastAABBInfo: public dgObb
{
public:
DG_INLINE dgFastAABBInfo()
:dgObb()
,m_absDir(dgGetIdentityMatrix())
,m_separationDistance(dgFloat32(1.0e10f))
{
}
DG_INLINE dgFastAABBInfo(const dgMatrix& matrix, const dgVector& size)
:dgObb(matrix, size)
,m_separationDistance(dgFloat32(1.0e10f))
{
SetTransposeAbsMatrix (matrix);
dgVector size1 (matrix[0].Abs().Scale(size.m_x) + matrix[1].Abs().Scale(size.m_y) + matrix[2].Abs().Scale(size.m_z));
m_p0 = (matrix[3] - size1) & dgVector::m_triplexMask;
m_p1 = (matrix[3] + size1) & dgVector::m_triplexMask;
}
DG_INLINE dgFastAABBInfo(const dgVector& p0, const dgVector& p1)
:dgObb(dgGetIdentityMatrix(), dgVector::m_half * (p1 - p0))
,m_absDir(dgGetIdentityMatrix())
,m_separationDistance(dgFloat32(1.0e10f))
,m_p0(p0)
,m_p1(p1)
{
m_posit = ((dgVector::m_half * (p1 + p0)) & dgVector::m_triplexMask) | dgVector::m_wOne;
}
DG_INLINE void SetTransposeAbsMatrix (const dgMatrix& matrix)
{
m_absDir = matrix.Transpose();
m_absDir[0] = m_absDir[0].Abs();
m_absDir[1] = m_absDir[1].Abs();
m_absDir[2] = m_absDir[2].Abs();
//m_absDir[3] = dgVector::m_wOne;
}
DG_INLINE dgFloat32 PolygonBoxRayDistance (const dgVector& faceNormal, dgInt32 indexCount, const dgInt32* const indexArray, dgInt32 stride, const dgFloat32* const vertexArray, const dgFastRayTest& ray) const
{
dgVector minBox;
dgVector maxBox;
MakeBox1 (indexCount, indexArray, stride, vertexArray, minBox, maxBox);
dgFloat32 dist0 = ray.BoxIntersect(minBox, maxBox);
if (dist0 < dgFloat32 (1.0f)) {
dgMatrix faceMatrix (MakeFaceMatrix (faceNormal, indexCount, indexArray, stride, vertexArray));
MakeBox2 (faceMatrix, indexCount, indexArray, stride, vertexArray, minBox, maxBox);
dgVector veloc (faceMatrix.RotateVector(ray.m_diff) & dgVector::m_triplexMask);
dgFastRayTest localRay (dgVector (dgFloat32 (0.0f)), veloc);
dgFloat32 dist1 = localRay.BoxIntersect(minBox, maxBox);
dist0 = dgMax (dist1, dist0);
}
return dist0;
}
DG_INLINE dgFloat32 PolygonBoxDistance (const dgVector& faceNormal, dgInt32 indexCount, const dgInt32* const indexArray, dgInt32 stride, const dgFloat32* const vertexArray) const
{
dgVector minBox;
dgVector maxBox;
MakeBox1 (indexCount, indexArray, stride, vertexArray, minBox, maxBox);
dgVector mask(minBox * maxBox < dgVector(dgFloat32(0.0f)));
dgVector dist(maxBox.GetMin(minBox.Abs()) & mask);
dist = dist.GetMin(dist.ShiftTripleRight());
dist = dist.GetMin(dist.ShiftTripleRight());
dgFloat32 dist0 = dist.GetScalar();
if (dist0 > dgFloat32 (0.0f)) {
dgMatrix faceMatrix (MakeFaceMatrix (faceNormal, indexCount, indexArray, stride, vertexArray));
MakeBox2 (faceMatrix, indexCount, indexArray, stride, vertexArray, minBox, maxBox);
dgVector mask2(minBox * maxBox < dgVector(dgFloat32(0.0f)));
dgVector dist2(maxBox.GetMin(minBox.Abs()) & mask2);
dist2 = dist2.GetMin(dist2.ShiftTripleRight());
dist2 = dist2.GetMin(dist2.ShiftTripleRight());
dgFloat32 dist1 = dist2.GetScalar();
dist0 = (dist1 > dgFloat32 (0.0f)) ? dgMax (dist0, dist1) : dgFloat32 (0.0f);
if (dist0 <= dgFloat32(0.0f)) {
dgVector p1p0((minBox.Abs()).GetMin(maxBox.Abs()).AndNot(mask2));
dist2 = p1p0.DotProduct(p1p0);
dist2 = dist2.Sqrt() * dgVector::m_negOne;
dist0 = dist2.GetScalar();
}
} else {
dgVector p1p0((minBox.Abs()).GetMin(maxBox.Abs()).AndNot(mask));
dist = p1p0.DotProduct(p1p0);
dist = dist.Sqrt() * dgVector::m_negOne;
dist0 = dist.GetScalar();
}
return dist0;
}
private:
DG_INLINE void MakeBox1 (dgInt32 indexCount, const dgInt32* const indexArray, dgInt32 stride, const dgFloat32* const vertexArray, dgVector& minBox, dgVector& maxBox) const
{
dgVector faceBoxP0 (&vertexArray[indexArray[0] * stride]);
faceBoxP0 = faceBoxP0 & dgVector::m_triplexMask;
dgVector faceBoxP1 (faceBoxP0);
for (dgInt32 i = 1; i < indexCount; i ++) {
dgVector p (&vertexArray[indexArray[i] * stride]);
p = p & dgVector::m_triplexMask;
faceBoxP0 = faceBoxP0.GetMin(p);
faceBoxP1 = faceBoxP1.GetMax(p);
}
minBox = faceBoxP0 - m_p1;
maxBox = faceBoxP1 - m_p0;
}
DG_INLINE void MakeBox2 (const dgMatrix& faceMatrix, dgInt32 indexCount, const dgInt32* const indexArray, dgInt32 stride, const dgFloat32* const vertexArray, dgVector& minBox, dgVector& maxBox) const
{
dgVector faceBoxP0 (faceMatrix.TransformVector (dgVector (&vertexArray[indexArray[0] * stride]) & dgVector::m_triplexMask));
dgVector faceBoxP1 (faceBoxP0);
for (dgInt32 i = 1; i < indexCount; i ++) {
dgVector p (faceMatrix.TransformVector (dgVector (&vertexArray[indexArray[i] * stride]) & dgVector::m_triplexMask));
faceBoxP0 = faceBoxP0.GetMin(p);
faceBoxP1 = faceBoxP1.GetMax(p);
}
faceBoxP0 = faceBoxP0 & dgVector::m_triplexMask;
faceBoxP1 = faceBoxP1 & dgVector::m_triplexMask;
dgMatrix matrix = *this * faceMatrix;
dgVector size (matrix[0].Abs().Scale(m_size.m_x) + matrix[1].Abs().Scale(m_size.m_y) + matrix[2].Abs().Scale(m_size.m_z));
dgVector boxP0 ((matrix.m_posit - size) & dgVector::m_triplexMask);
dgVector boxP1 ((matrix.m_posit + size) & dgVector::m_triplexMask);
minBox = faceBoxP0 - boxP1;
maxBox = faceBoxP1 - boxP0;
}
DG_INLINE dgMatrix MakeFaceMatrix (const dgVector& faceNormal, dgInt32 indexCount, const dgInt32* const indexArray, dgInt32 stride, const dgFloat32* const vertexArray) const
{
dgMatrix faceMatrix;
dgVector origin (&vertexArray[indexArray[0] * stride]);
dgVector pin (&vertexArray[indexArray[0] * stride]);
pin = pin & dgVector::m_triplexMask;
origin = origin & dgVector::m_triplexMask;
dgVector pin1 (&vertexArray[indexArray[1] * stride]);
pin1 = pin1 & dgVector::m_triplexMask;
faceMatrix[0] = faceNormal;
faceMatrix[1] = pin1 - origin;
faceMatrix[1] = faceMatrix[1].Normalize();
faceMatrix[2] = faceMatrix[0].CrossProduct(faceMatrix[1]);
faceMatrix[3] = origin | dgVector::m_wOne;
return faceMatrix.Inverse();
}
protected:
dgMatrix m_absDir;
mutable dgVector m_separationDistance;
dgVector m_p0;
dgVector m_p1;
friend class dgAABBPolygonSoup;
friend class dgCollisionUserMesh;
friend class dgCollisionHeightField;
} DG_GCC_VECTOR_ALIGNMENT;
#endif

701
thirdparty/src/newton/dgCore/dgList.h vendored Normal file
View File

@@ -0,0 +1,701 @@
/* Copyright (c) <2003-2019> <Julio Jerez, Newton Game Dynamics>
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
*
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
*
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
*
* 3. This notice may not be removed or altered from any source distribution.
*/
#ifndef __dgList__
#define __dgList__
#include "dgStdafx.h"
#include "dgRef.h"
#include "dgDebug.h"
#include "dgMemory.h"
template<class T>
class dgList
{
public:
class dgListNode
{
DG_CLASS_ALLOCATOR(allocator)
dgListNode (dgListNode* const prev, dgListNode* const next)
:m_info ()
{
m_prev = prev;
m_next = next;
if (m_prev) {
m_prev->m_next = this;
}
if (m_next) {
m_next->m_prev = this;
}
}
dgListNode (const T &info, dgListNode* const prev, dgListNode* const next)
:m_info (info)
{
m_prev = prev;
m_next = next;
if (m_prev) {
m_prev->m_next = this;
}
if (m_next) {
m_next->m_prev = this;
}
}
virtual ~dgListNode()
{
}
void Unlink ()
{
if (m_prev) {
m_prev->m_next = m_next;
}
if (m_next) {
m_next->m_prev = m_prev;
}
m_prev = NULL;
m_next = NULL;
}
void AddLast(dgListNode* const node)
{
m_next = node;
node->m_prev = this;
}
void AddFirst(dgListNode* const node)
{
m_prev = node;
node->m_next = this;
}
public:
T& GetInfo()
{
return m_info;
}
dgListNode *GetNext() const
{
return m_next;
}
dgListNode *GetPrev() const
{
return m_prev;
}
private:
T m_info;
dgListNode *m_next;
dgListNode *m_prev;
friend class dgList<T>;
};
class Iterator
{
public:
Iterator (const dgList<T> &me)
{
m_ptr = NULL;
m_list = (dgList *)&me;
}
~Iterator ()
{
}
operator dgInt32() const
{
return m_ptr != NULL;
}
bool operator== (const Iterator &target) const
{
return (m_ptr == target.m_ptr) && (m_list == target.m_list);
}
void Begin()
{
m_ptr = m_list->GetFirst();
}
void End()
{
m_ptr = m_list->GetLast();
}
void Set (dgListNode* const node)
{
m_ptr = node;
}
void operator++ ()
{
dgAssert (m_ptr);
m_ptr = m_ptr->m_next();
}
void operator++ (dgInt32)
{
dgAssert (m_ptr);
m_ptr = m_ptr->GetNext();
}
void operator-- ()
{
dgAssert (m_ptr);
m_ptr = m_ptr->GetPrev();
}
void operator-- (dgInt32)
{
dgAssert (m_ptr);
m_ptr = m_ptr->GetPrev();
}
T &operator* () const
{
return m_ptr->GetInfo();
}
dgListNode *GetNode() const
{
return m_ptr;
}
private:
dgList *m_list;
dgListNode *m_ptr;
};
// ***********************************************************
// member functions
// ***********************************************************
public:
DG_CLASS_ALLOCATOR(allocator)
dgList (dgMemoryAllocator* const allocator);
virtual ~dgList ();
dgMemoryAllocator* GetAllocator () const;
void SetAllocator (dgMemoryAllocator* const allocator);
operator dgInt32() const;
dgInt32 GetCount() const;
dgListNode* GetLast() const;
dgListNode* GetFirst() const;
dgListNode* Append ();
dgListNode* Append (dgListNode* const node);
dgListNode* Append (const T &element);
dgListNode* Addtop ();
dgListNode* Addtop (dgListNode* const node);
dgListNode* Addtop (const T &element);
void RotateToEnd (dgListNode* const node);
void RotateToBegin (dgListNode* const node);
void InsertAfter (dgListNode* const root, dgListNode* const node);
void InsertBefore (dgListNode* const root, dgListNode* const node);
dgListNode* Find (const T &element) const;
dgListNode* GetNodeFromInfo (T &m_info) const;
void Remove (dgListNode* const node);
void Remove (const T &element);
void RemoveAll ();
void Merge (dgList<T>& list);
void Unlink (dgListNode* const node);
bool SanityCheck () const;
protected:
// dgListNode* SafeAddtop(const T &element);
// ***********************************************************
// member variables
// ***********************************************************
private:
dgInt32 m_count;
dgListNode* m_first;
dgListNode* m_last;
dgMemoryAllocator* m_allocator;
// static dgInt32 m_size;
// static dgMemoryAllocator* m_staticAllocator;
friend class dgListNode;
};
/*
template<class T>
dgList<T>::dgList ()
{
m_count = 0;
m_first = NULL;
m_last = NULL;
m_allocator = NULL;
}
*/
template<class T>
dgList<T>::dgList (dgMemoryAllocator* const allocator)
:m_count(0)
,m_first(NULL)
,m_last(NULL)
,m_allocator(allocator)
{
}
template<class T>
dgList<T>::~dgList ()
{
RemoveAll ();
}
template<class T>
void dgList<T>::SetAllocator (dgMemoryAllocator* const allocator)
{
if ((m_count == 0) && (m_allocator == NULL)) {
m_allocator = allocator;
}
}
template<class T>
dgMemoryAllocator* dgList<T>::GetAllocator () const
{
return m_allocator;
}
template<class T>
dgInt32 dgList<T>::GetCount() const
{
return m_count;
}
template<class T>
dgList<T>::operator dgInt32() const
{
return m_first != NULL;
}
template<class T>
typename dgList<T>::dgListNode *dgList<T>::GetFirst() const
{
return m_first;
}
template<class T>
typename dgList<T>::dgListNode *dgList<T>::GetLast() const
{
return m_last;
}
template<class T>
typename dgList<T>::dgListNode *dgList<T>::Append (dgListNode* const node)
{
dgAssert (node->m_next == NULL);
dgAssert (node->m_prev == NULL);
m_count ++;
if (m_first == NULL) {
m_last = node;
m_first = node;
} else {
m_last->AddLast (node);
m_last = node;
}
#ifdef __ENABLE_DG_CONTAINERS_SANITY_CHECK
dgAssert (SanityCheck ());
#endif
return m_last;
}
template<class T>
typename dgList<T>::dgListNode *dgList<T>::Append ()
{
m_count ++;
if (m_first == NULL) {
m_first = new (m_allocator) dgListNode(NULL, NULL);
m_last = m_first;
} else {
m_last = new (m_allocator) dgListNode(m_last, NULL);
}
#ifdef __ENABLE_DG_CONTAINERS_SANITY_CHECK
dgAssert (SanityCheck ());
#endif
return m_last;
}
template<class T>
typename dgList<T>::dgListNode *dgList<T>::Append (const T &element)
{
m_count ++;
if (m_first == NULL) {
m_first = new (m_allocator) dgListNode(element, NULL, NULL);
m_last = m_first;
} else {
m_last = new (m_allocator) dgListNode(element, m_last, NULL);
}
#ifdef __ENABLE_DG_CONTAINERS_SANITY_CHECK
dgAssert (SanityCheck ());
#endif
return m_last;
}
template<class T>
typename dgList<T>::dgListNode *dgList<T>::Addtop (dgListNode* const node)
{
dgAssert (node->m_next == NULL);
dgAssert (node->m_prev == NULL);
m_count ++;
if (m_last == NULL) {
m_last = node;
m_first = node;
} else {
m_first->AddFirst(node);
m_first = node;
}
#ifdef __ENABLE_DG_CONTAINERS_SANITY_CHECK
dgAssert (SanityCheck ());
#endif
return m_first;
}
template<class T>
typename dgList<T>::dgListNode *dgList<T>::Addtop ()
{
m_count ++;
if (m_last == NULL) {
m_last = new (m_allocator) dgListNode(NULL, NULL);
m_first = m_last;
} else {
m_first = new (m_allocator) dgListNode(NULL, m_first);
}
#ifdef __ENABLE_DG_CONTAINERS_SANITY_CHECK
dgAssert (SanityCheck ());
#endif
return m_first;
}
template<class T>
typename dgList<T>::dgListNode *dgList<T>::Addtop (const T &element)
{
m_count ++;
if (m_last == NULL) {
m_last = new (m_allocator) dgListNode(element, NULL, NULL);
m_first = m_last;
} else {
m_first = new (m_allocator) dgListNode(element, NULL, m_first);
}
#ifdef __ENABLE_DG_CONTAINERS_SANITY_CHECK
dgAssert (SanityCheck ());
#endif
return m_first;
}
/*
template<class T>
typename dgList<T>::dgListNode *dgList<T>::SafeAddtop(const T& element)
{
dgAssert (m_last);
m_count++;
dgListNode* const node = new (m_allocator) dgListNode(element, NULL, NULL);
dgListNode* const first = (dgListNode*) dgInterlockedExchange ((void**) &m_first, (void*) node);
node->m_next = first;
first->m_prev = node;
dgAssert (m_last);
#ifdef __ENABLE_DG_CONTAINERS_SANITY_CHECK
dgAssert(SanityCheck());
#endif
return node;
}
*/
template<class T>
void dgList<T>::InsertAfter (dgListNode* const root, dgListNode* const node)
{
dgAssert (root);
if (node != root) {
if (root->m_next != node) {
if (node == m_first) {
m_first = node->m_next;
}
if (node == m_last) {
m_last = node->m_prev;
}
node->Unlink ();
node->m_prev = root;
node->m_next = root->m_next;
if (root->m_next) {
root->m_next->m_prev = node;
}
root->m_next = node;
if (node->m_next == NULL) {
m_last = node;
}
dgAssert (m_last);
dgAssert (!m_last->m_next);
dgAssert (m_first);
dgAssert (!m_first->m_prev);
dgAssert (SanityCheck ());
}
}
}
template<class T>
void dgList<T>::InsertBefore (dgListNode* const root, dgListNode* const node)
{
dgAssert (root);
if (node != root) {
if (root->m_prev != node) {
if (node == m_last) {
m_last = node->m_prev;
}
if (node == m_first) {
m_first = node->m_next;
}
node->Unlink ();
node->m_next = root;
node->m_prev = root->m_prev;
if (root->m_prev) {
root->m_prev->m_next = node;
}
root->m_prev = node;
if (node->m_prev == NULL) {
m_first = node;
}
dgAssert (m_first);
dgAssert (!m_first->m_prev);
dgAssert (m_last);
dgAssert (!m_last->m_next);
dgAssert (SanityCheck ());
}
}
}
template<class T>
void dgList<T>::RotateToEnd (dgListNode* const node)
{
if (node != m_last) {
if (m_last != m_first) {
if (node == m_first) {
m_first = m_first->GetNext();
}
node->Unlink();
m_last->AddLast(node);
m_last = node;
}
}
#ifdef __ENABLE_DG_CONTAINERS_SANITY_CHECK
dgAssert (SanityCheck ());
#endif
}
template<class T>
void dgList<T>::RotateToBegin (dgListNode* const node)
{
if (node != m_first) {
if (m_last != m_first) {
if (node == m_last) {
m_last = m_last->GetPrev();
}
node->Unlink();
m_first->AddFirst(node);
m_first = node;
}
}
#ifdef __ENABLE_DG_CONTAINERS_SANITY_CHECK
dgAssert (SanityCheck ());
#endif
}
template<class T>
typename dgList<T>::dgListNode *dgList<T>::Find (const T &element) const
{
dgListNode *node;
for (node = m_first; node; node = node->GetNext()) {
if (element == node->m_info) {
break;
}
}
return node;
}
template<class T>
typename dgList<T>::dgListNode *dgList<T>::GetNodeFromInfo (T &info) const
{
dgListNode* const node = (dgListNode *) &info;
dgInt64 offset = ((char*) &node->m_info) - ((char *) node);
dgListNode* const retnode = (dgListNode *) (((char *) node) - offset);
dgAssert (&retnode->GetInfo () == &info);
return retnode;
}
template<class T>
void dgList<T>::Remove (const T &element)
{
dgListNode *const node = Find (element);
if (node) {
Remove (node);
}
}
template<class T>
void dgList<T>::Unlink (dgListNode* const node)
{
dgAssert (node);
m_count --;
dgAssert (m_count >= 0);
if (node == m_first) {
m_first = m_first->GetNext();
}
if (node == m_last) {
m_last = m_last->GetPrev();
}
node->Unlink();
#ifdef __ENABLE_DG_CONTAINERS_SANITY_CHECK
dgAssert (SanityCheck ());
#endif
}
template<class T>
void dgList<T>::Merge (dgList<T>& list)
{
m_count += list.m_count;
if (list.m_first) {
list.m_first->m_prev = m_last;
}
if (m_last) {
m_last->m_next = list.m_first;
}
m_last = list.m_last;
if (!m_first) {
m_first = list.m_first;
}
list.m_count = 0;
list.m_last = NULL;
list.m_first = NULL;
#ifdef __ENABLE_DG_CONTAINERS_SANITY_CHECK
dgAssert (SanityCheck ());
#endif
}
template<class T>
void dgList<T>::Remove (dgListNode* const node)
{
Unlink (node);
delete node;
}
template<class T>
void dgList<T>::RemoveAll ()
{
for (dgListNode *node = m_first; node; node = m_first) {
m_count --;
m_first = node->GetNext();
node->Unlink();
delete node;
}
dgAssert (m_count == 0);
m_last = NULL;
m_first = NULL;
}
template<class T>
bool dgList<T>::SanityCheck () const
{
#ifdef _DEBUG
dgInt32 tCount = 0;
for (dgListNode * node = m_first; node; node = node->GetNext()) {
tCount ++;
if (node->GetPrev()) {
dgAssert (node->GetPrev() != node->GetNext());
if (node->GetPrev()->GetNext() != node) {
dgAssert (0);
return false;
}
}
if (node->GetNext()) {
dgAssert (node->GetPrev() != node->GetNext());
if (node->GetNext()->GetPrev() != node) {
dgAssert (0);
return false;
}
}
}
if (tCount != m_count) {
dgAssert (0);
return false;
}
#endif
return true;
}
//template<class T>
//void dgList<T>::SetAllocator (dgMemoryAllocator * allocator)
//{
// m_allocator = allocator;
//}
//template<class T>
//dgMemoryAllocator* dgList<T>::GetAllocator () const
//{
// return m_allocator;
//}
//template<class T> dgInt32 dgList <T>::m_size = 0;
//template<class T> dgMemoryAllocator* dgList<T>::m_staticAllocator = NULL;
#endif

778
thirdparty/src/newton/dgCore/dgMatrix.cpp vendored Normal file
View File

@@ -0,0 +1,778 @@
/* Copyright (c) <2003-2019> <Julio Jerez, Newton Game Dynamics>
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
*
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
*
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
*
* 3. This notice may not be removed or altered from any source distribution.
*/
#include "dgStdafx.h"
#include "dgMatrix.h"
#include "dgQuaternion.h"
#include "dgGeneralMatrix.h"
#ifndef _NEWTON_USE_DOUBLE
dgVector dgVector::m_xMask(dgInt32(-1), dgInt32(0), dgInt32(0), dgInt32(0));
dgVector dgVector::m_yMask(dgInt32(0), dgInt32(-1), dgInt32(0), dgInt32(0));
dgVector dgVector::m_zMask(dgInt32(0), dgInt32(0), dgInt32(-1), dgInt32(0));
dgVector dgVector::m_wMask(dgInt32(0), dgInt32(0), dgInt32(0), dgInt32(-1));
dgVector dgVector::m_triplexMask(dgInt32(-1), dgInt32(-1), dgInt32(-1), dgInt32(0));
dgVector dgVector::m_signMask(dgVector(dgInt32(-1), dgInt32(-1), dgInt32(-1), dgInt32(-1)).ShiftRightLogical(1));
dgVector dgVector::m_zero(dgFloat32(0.0f));
dgVector dgVector::m_one(dgFloat32(1.0f));
dgVector dgVector::m_two(dgFloat32(2.0f));
dgVector dgVector::m_half(dgFloat32(0.5f));
dgVector dgVector::m_three(dgFloat32(3.0f));
dgVector dgVector::m_negOne(dgFloat32(-1.0f));
dgVector dgVector::m_epsilon(dgFloat32(1.0e-20f));
dgVector dgVector::m_wOne(dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(1.0f));
#endif
dgBigVector dgBigVector::m_zero (dgFloat64 (0.0f));
dgBigVector dgBigVector::m_one (dgFloat64 (1.0f));
dgBigVector dgBigVector::m_two (dgFloat64 (2.0f));
dgBigVector dgBigVector::m_half (dgFloat32 (0.5f));
dgBigVector dgBigVector::m_three (dgFloat32 (3.0f));
dgBigVector dgBigVector::m_negOne (dgFloat32 (-1.0f));
dgBigVector dgBigVector::m_epsilon(dgFloat32(1.0e-20f));
dgBigVector dgBigVector::m_wOne (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (1.0f));
dgBigVector dgBigVector::m_triplexMask (dgInt32 (-1), dgInt32 (-1), dgInt32 (-1), dgInt32 (0));
dgBigVector dgBigVector::m_signMask (dgBigVector(dgInt32 (-1), dgInt32 (-1), dgInt32 (-1), dgInt32 (-1)).ShiftRightLogical(1));
dgBigVector dgBigVector::m_xMask (dgInt32 (-1), dgInt32 ( 0), dgInt32 ( 0), dgInt32 ( 0));
dgBigVector dgBigVector::m_yMask (dgInt32 ( 0), dgInt32 (-1), dgInt32 ( 0), dgInt32 ( 0));
dgBigVector dgBigVector::m_zMask (dgInt32 ( 0), dgInt32 ( 0), dgInt32 (-1), dgInt32 ( 0));
dgBigVector dgBigVector::m_wMask (dgInt32 ( 0), dgInt32 ( 0), dgInt32 ( 0), dgInt32 (-1));
dgSpatialVector dgSpatialVector::m_zero (dgFloat32 (0.0f));
dgMatrix dgMatrix::m_zeroMatrix (dgVector (dgFloat32(0.0f)),
dgVector (dgFloat32(0.0f)),
dgVector (dgFloat32(0.0f)),
dgVector (dgFloat32(0.0f)));
dgMatrix dgMatrix::m_identityMatrix (dgVector (dgFloat32(1.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f)),
dgVector (dgFloat32(0.0f), dgFloat32(1.0f), dgFloat32(0.0f), dgFloat32(0.0f)),
dgVector (dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(1.0f), dgFloat32(0.0f)),
dgVector (dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(1.0f)));
#ifndef _NEWTON_USE_DOUBLE
#endif
const dgMatrix& dgGetIdentityMatrix()
{
return dgMatrix::m_identityMatrix;
}
const dgMatrix& dgGetZeroMatrix ()
{
return dgMatrix::m_zeroMatrix;
}
dgMatrix::dgMatrix (const dgQuaternion &quat0, const dgVector &position)
{
dgQuaternion quat1 (quat0);
quat1.Scale(dgFloat32 (2.0f));
dgFloat32 x2 = quat0.m_x * quat1.m_x;
dgFloat32 y2 = quat0.m_y * quat1.m_y;
dgFloat32 z2 = quat0.m_z * quat1.m_z;
#ifdef _DEBUG
dgFloat32 w2 = quat0.m_w * quat1.m_w;
dgAssert (dgAbs (w2 + x2 + y2 + z2 - dgFloat32(2.0f)) <dgFloat32 (1.0e-3f));
#endif
dgFloat32 xy = quat0.m_x * quat1.m_y;
dgFloat32 xz = quat0.m_x * quat1.m_z;
dgFloat32 xw = quat0.m_x * quat1.m_w;
dgFloat32 yz = quat0.m_y * quat1.m_z;
dgFloat32 yw = quat0.m_y * quat1.m_w;
dgFloat32 zw = quat0.m_z * quat1.m_w;
m_front = dgVector (dgFloat32(1.0f) - y2 - z2, xy + zw, xz - yw, dgFloat32(0.0f));
m_up = dgVector (xy - zw, dgFloat32(1.0f) - x2 - z2, yz + xw, dgFloat32(0.0f));
m_right = dgVector (xz + yw, yz - xw, dgFloat32(1.0f) - x2 - y2, dgFloat32(0.0f));
m_posit.m_x = position.m_x;
m_posit.m_y = position.m_y;
m_posit.m_z = position.m_z;
m_posit.m_w = dgFloat32(1.0f);
}
dgMatrix::dgMatrix (const dgMatrix& transformMatrix, const dgVector& scale, const dgMatrix& stretchAxis)
{
dgMatrix scaledAxis;
scaledAxis[0] = stretchAxis[0].Scale (scale[0]);
scaledAxis[1] = stretchAxis[1].Scale (scale[1]);
scaledAxis[2] = stretchAxis[2].Scale (scale[2]);
scaledAxis[3] = stretchAxis[3];
*this = stretchAxis.Transpose() * scaledAxis * transformMatrix;
}
dgMatrix dgMatrix::Multiply3X3 (const dgMatrix &B) const
{
return dgMatrix (B.m_front * m_front.BroadcastX() + B.m_up * m_front.BroadcastY() + B.m_right * m_front.BroadcastZ(),
B.m_front * m_up.BroadcastX() + B.m_up * m_up.BroadcastY() + B.m_right * m_up.BroadcastZ(),
B.m_front * m_right.BroadcastX() + B.m_up * m_right.BroadcastY() + B.m_right * m_right.BroadcastZ(),
dgVector::m_wOne);
}
dgMatrix dgMatrix::operator* (const dgMatrix &B) const
{
return dgMatrix (B.m_front * m_front.BroadcastX() + B.m_up * m_front.BroadcastY() + B.m_right * m_front.BroadcastZ() + B.m_posit * m_front.BroadcastW(),
B.m_front * m_up.BroadcastX() + B.m_up * m_up.BroadcastY() + B.m_right * m_up.BroadcastZ() + B.m_posit * m_up.BroadcastW(),
B.m_front * m_right.BroadcastX() + B.m_up * m_right.BroadcastY() + B.m_right * m_right.BroadcastZ() + B.m_posit * m_right.BroadcastW(),
B.m_front * m_posit.BroadcastX() + B.m_up * m_posit.BroadcastY() + B.m_right * m_posit.BroadcastZ() + B.m_posit * m_posit.BroadcastW());
}
void dgMatrix::TransformTriplex (dgFloat32* const dst, dgInt32 dstStrideInBytes, const dgFloat32* const src, dgInt32 srcStrideInBytes, dgInt32 count) const
{
dgInt32 dstStride = dgInt32 (dstStrideInBytes /sizeof (dgFloat32));
dgInt32 srcStride = dgInt32 (srcStrideInBytes / sizeof (dgFloat32));
dgInt32 dstIndex = 0;
dgInt32 srcIndex = 0;
for (dgInt32 i = 0 ; i < count; i ++ ) {
dgFloat32 x = src[srcIndex + 0];
dgFloat32 y = src[srcIndex + 1];
dgFloat32 z = src[srcIndex + 2];
srcIndex += srcStride;
dst[dstIndex + 0] = x * m_front.m_x + y * m_up.m_x + z * m_right.m_x + m_posit.m_x;
dst[dstIndex + 1] = x * m_front.m_y + y * m_up.m_y + z * m_right.m_y + m_posit.m_y;
dst[dstIndex + 2] = x * m_front.m_z + y * m_up.m_z + z * m_right.m_z + m_posit.m_z;
dstIndex += dstStride;
}
}
#ifndef _NEWTON_USE_DOUBLE
void dgMatrix::TransformTriplex (dgFloat64* const dst, dgInt32 dstStrideInBytes, const dgFloat64* const src, dgInt32 srcStrideInBytes, dgInt32 count) const
{
dgInt32 dstStride = dgInt32 (dstStrideInBytes /sizeof (dgFloat64));
dgInt32 srcStride = dgInt32 (srcStrideInBytes / sizeof (dgFloat64));
dgInt32 dstIndex = 0;
dgInt32 srcIndex = 0;
for (dgInt32 i = 0 ; i < count; i ++ ) {
dgFloat64 x = src[srcIndex + 0];
dgFloat64 y = src[srcIndex + 1];
dgFloat64 z = src[srcIndex + 2];
srcIndex += srcStride;
dst[dstIndex + 0] = x * m_front.m_x + y * m_up.m_x + z * m_right.m_x + m_posit.m_x;
dst[dstIndex + 1] = x * m_front.m_y + y * m_up.m_y + z * m_right.m_y + m_posit.m_y;
dst[dstIndex + 2] = x * m_front.m_z + y * m_up.m_z + z * m_right.m_z + m_posit.m_z;
dstIndex += dstStride;
}
}
void dgMatrix::TransformTriplex (dgFloat64* const dst, dgInt32 dstStrideInBytes, const dgFloat32* const src, dgInt32 srcStrideInBytes, dgInt32 count) const
{
dgInt32 dstStride = dgInt32 (dstStrideInBytes /sizeof (dgFloat64));
dgInt32 srcStride = dgInt32 (srcStrideInBytes / sizeof (dgFloat32));
dgInt32 dstIndex = 0;
dgInt32 srcIndex = 0;
for (dgInt32 i = 0 ; i < count; i ++ ) {
dgFloat64 x = src[srcIndex + 0];
dgFloat64 y = src[srcIndex + 1];
dgFloat64 z = src[srcIndex + 2];
srcIndex += srcStride;
dst[dstIndex + 0] = x * m_front.m_x + y * m_up.m_x + z * m_right.m_x + m_posit.m_x;
dst[dstIndex + 1] = x * m_front.m_y + y * m_up.m_y + z * m_right.m_y + m_posit.m_y;
dst[dstIndex + 2] = x * m_front.m_z + y * m_up.m_z + z * m_right.m_z + m_posit.m_z;
dstIndex += dstStride;
}
}
#endif
void dgMatrix::TransformBBox (const dgVector& p0local, const dgVector& p1local, dgVector& p0, dgVector& p1) const
{
const dgMatrix& matrix = *this;
//dgVector size ((p1local - p0local).Scale (dgFloat32 (0.5f)));
//dgVector center (TransformVector ((p1local + p0local).Scale (dgFloat32 (0.5f))));
dgVector size ((p1local - p0local) * dgVector::m_half);
dgVector center (TransformVector ((p1local + p0local) * dgVector::m_half));
dgVector extends (size.m_x * dgAbs(matrix[0][0]) + size.m_y * dgAbs(matrix[1][0]) + size.m_z * dgAbs(matrix[2][0]),
size.m_x * dgAbs(matrix[0][1]) + size.m_y * dgAbs(matrix[1][1]) + size.m_z * dgAbs(matrix[2][1]),
size.m_x * dgAbs(matrix[0][2]) + size.m_y * dgAbs(matrix[1][2]) + size.m_z * dgAbs(matrix[2][2]), dgFloat32 (0.0f));
p0 = center - extends;
p1 = center + extends;
}
dgMatrix dgMatrix::Inverse4x4 () const
{
dgMatrix tmp (*this);
dgMatrix inv (dgGetIdentityMatrix());
for (dgInt32 i = 0; i < 4; i++) {
dgFloat32 pivot = dgAbs(tmp[i][i]);
if (pivot < dgFloat32(0.01f)) {
dgInt32 permute = i;
for (dgInt32 j = i + 1; j < 4; j++) {
dgFloat32 pivot1 = dgAbs(tmp[j][i]);
if (pivot1 > pivot) {
permute = j;
pivot = pivot1;
}
}
if (permute != i) {
dgAssert(pivot > dgFloat32(0.0f));
dgAssert((pivot > dgFloat32(1.0e-6f)) || (dgConditionNumber(4, 4, (dgFloat32*)&(*this)[0][0]) < dgFloat32(1.0e5f)));
dgSwap(inv[i], inv[permute]);
dgSwap(tmp[i], tmp[permute]);
}
}
for (dgInt32 j = i + 1; j < 4; j++) {
dgVector scale (tmp[j][i] / tmp[i][i]);
tmp[j] -= tmp[i] * scale;
inv[j] -= inv[i] * scale;
tmp[j][i] = dgFloat32 (0.0f);
}
}
for (dgInt32 i = 3; i >= 0; i--) {
dgVector acc(dgVector::m_zero);
for (dgInt32 j = i + 1; j < 4; j++) {
dgVector pivot(tmp[i][j]);
acc += pivot * inv[j];
}
dgVector den(dgFloat32(1.0f) / tmp[i][i]);
inv[i] = den * (inv[i] - acc);
}
#ifdef _DEBUG
tmp = *this * inv;
for (dgInt32 i = 0; i < 4; i++) {
dgAssert(dgAbs(tmp[i][i] - dgFloat32(1.0f)) < dgFloat32(1.0e-6f));
for (dgInt32 j = i + 1; j < 4; j++) {
dgAssert(dgAbs(tmp[i][j]) < dgFloat32(1.0e-5f));
dgAssert(dgAbs(tmp[j][i]) < dgFloat32(1.0e-5f));
}
}
#endif
return inv;
}
dgVector dgMatrix::SolveByGaussianElimination(const dgVector &v) const
{
// return Inverse4x4().UnrotateVector(v);
dgMatrix tmp(*this);
dgVector ret(v);
for (dgInt32 i = 0; i < 4; i++) {
dgFloat32 pivot = dgAbs(tmp[i][i]);
if (pivot < dgFloat32(0.01f)) {
dgInt32 permute = i;
for (dgInt32 j = i + 1; j < 4; j++) {
dgFloat32 pivot1 = dgAbs(tmp[j][i]);
if (pivot1 > pivot) {
permute = j;
pivot = pivot1;
}
}
if (permute != i) {
dgAssert(pivot > dgFloat32(1.0e-6f));
dgSwap(ret[i], ret[permute]);
dgSwap(tmp[i], tmp[permute]);
}
}
for (dgInt32 j = i + 1; j < 4; j++) {
dgVector scale(tmp[j][i] / tmp[i][i]);
tmp[j] -= tmp[i] * scale;
ret[j] -= ret[i] * scale.GetScalar();
tmp[j][i] = dgFloat32(0.0f);
}
}
for (dgInt32 i = 3; i >= 0; i--) {
dgVector pivot(tmp[i] * ret);
ret[i] = (ret[i] - pivot.AddHorizontal().GetScalar() + tmp[i][i] * ret[i]) / tmp[i][i];
}
return ret;
}
void dgMatrix::CalcPitchYawRoll (dgVector& euler0, dgVector& euler1) const
{
const dgMatrix& matrix = *this;
dgAssert (matrix[2].DotProduct(matrix[0].CrossProduct(matrix[1])).GetScalar() > 0.0f);
dgAssert (dgAbs (matrix[2].DotProduct(matrix[0].CrossProduct(matrix[1])).GetScalar() - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f));
/*
// Assuming the angles are in radians.
if (matrix[0][2] > dgFloat32 (0.99995f)) {
dgFloat32 picth0 = dgFloat32 (0.0f);
dgFloat32 yaw0 = dgFloat32 (-dgPI * 0.5f);
dgFloat32 roll0 = - dgAtan2(matrix[2][1], matrix[1][1]);
euler0[0] = picth0;
euler0[1] = yaw0;
euler0[2] = roll0;
euler1[0] = picth0;
euler1[1] = yaw0;
euler1[2] = roll0;
} else if (matrix[0][2] < dgFloat32 (-0.99995f)) {
dgFloat32 picth0 = dgFloat32 (0.0f);
dgFloat32 yaw0 = dgFloat32 (dgPI * 0.5f);
dgFloat32 roll0 = dgAtan2(matrix[2][1], matrix[1][1]);
euler0[0] = picth0;
euler0[1] = yaw0;
euler0[2] = roll0;
euler1[0] = picth0;
euler1[1] = yaw0;
euler1[2] = roll0;
} else {
dgFloat32 yaw0 = -dgAsin ( matrix[0][2]);
dgFloat32 yaw1 = dgFloat32 (dgPI) - yaw0;
dgFloat32 sign0 = dgSign(dgCos (yaw0));
dgFloat32 sign1 = dgSign(dgCos (yaw1));
dgFloat32 picth0 = dgAtan2(matrix[1][2] * sign0, matrix[2][2] * sign0);
dgFloat32 picth1 = dgAtan2(matrix[1][2] * sign1, matrix[2][2] * sign1);
dgFloat32 roll0 = dgAtan2(matrix[0][1] * sign0, matrix[0][0] * sign0);
dgFloat32 roll1 = dgAtan2(matrix[0][1] * sign1, matrix[0][0] * sign1);
if (yaw1 > dgFloat32 (dgPI)) {
yaw1 -= dgFloat32 (2.0f * dgPI);
}
euler0[0] = picth0;
euler0[1] = yaw0;
euler0[2] = roll0;
euler1[0] = picth1;
euler1[1] = yaw1;
euler1[2] = roll1;
}
*/
// Assuming the angles are in radians.
if (matrix[0][2] > dgFloat32 (0.99995f)) {
dgFloat32 picth0 = dgFloat32(0.0f);
dgFloat32 yaw0 = dgFloat32(-dgPi * 0.5f);
dgFloat32 roll0 = -dgAtan2(matrix[2][1], matrix[1][1]);
euler0[0] = picth0;
euler0[1] = yaw0;
euler0[2] = roll0;
euler1[0] = picth0;
euler1[1] = yaw0;
euler1[2] = roll0;
} else if (matrix[0][2] < dgFloat32 (-0.99995f)) {
dgFloat32 picth0 = dgFloat32 (0.0f);
dgFloat32 yaw0 = dgFloat32(dgPi * 0.5f);
dgFloat32 roll0 = dgAtan2(matrix[2][1], matrix[1][1]);
euler0[0] = picth0;
euler0[1] = yaw0;
euler0[2] = roll0;
euler1[0] = picth0;
euler1[1] = yaw0;
euler1[2] = roll0;
} else {
dgFloat32 yaw0 = -dgAsin(matrix[0][2]);
dgFloat32 yaw1 = dgFloat32(dgPi) - yaw0;
dgFloat32 picth0 = dgAtan2( matrix[1][2], matrix[2][2]);
dgFloat32 picth1 = dgAtan2(-matrix[1][2], -matrix[2][2]);
dgFloat32 roll0 = dgAtan2( matrix[0][1], matrix[0][0]);
dgFloat32 roll1 = dgAtan2(-matrix[0][1], -matrix[0][0]);
if (yaw1 > dgFloat32 (dgPi)) {
yaw1 -= dgFloat32 (2.0f * dgPi);
}
euler0[0] = picth0;
euler0[1] = yaw0;
euler0[2] = roll0;
euler1[0] = picth1;
euler1[1] = yaw1;
euler1[2] = roll1;
}
euler0[3] = dgFloat32(0.0f);
euler1[3] = dgFloat32(0.0f);
#ifdef _DEBUG
dgMatrix m0 (dgPitchMatrix (euler0[0]) * dgYawMatrix(euler0[1]) * dgRollMatrix(euler0[2]));
dgMatrix m1 (dgPitchMatrix (euler1[0]) * dgYawMatrix(euler1[1]) * dgRollMatrix(euler1[2]));
for (dgInt32 i = 0; i < 3; i ++) {
for (dgInt32 j = 0; j < 3; j ++) {
dgFloat32 error = dgAbs (m0[i][j] - matrix[i][j]);
dgAssert (error < 5.0e-2f);
error = dgAbs (m1[i][j] - matrix[i][j]);
dgAssert (error < 5.0e-2f);
}
}
#endif
}
void dgMatrix::PolarDecomposition (dgMatrix& transformMatrix, dgVector& scale, dgMatrix& stretchAxis) const
{
// a polar decomposition decompose matrix A = O * S
// where S = sqrt (transpose (L) * L)
const dgMatrix& me = *this;
dgFloat32 sign = dgSign (me[2].DotProduct(me[0].CrossProduct(me[1])).GetScalar());
stretchAxis = me * Transpose();
scale = stretchAxis.EigenVectors();
// I need to deal with by seeing of some of the Scale are duplicated
// do this later (maybe by a given rotation around the non uniform axis but I do not know if it will work)
// for now just us the matrix
scale[0] = sign * dgSqrt (scale[0]);
scale[1] = sign * dgSqrt (scale[1]);
scale[2] = sign * dgSqrt (scale[2]);
scale[3] = dgFloat32 (0.0f);
dgMatrix scaledAxis;
scaledAxis[0] = stretchAxis[0].Scale (dgFloat32 (1.0f) / scale[0]);
scaledAxis[1] = stretchAxis[1].Scale (dgFloat32 (1.0f) / scale[1]);
scaledAxis[2] = stretchAxis[2].Scale (dgFloat32 (1.0f) / scale[2]);
scaledAxis[3] = stretchAxis[3];
dgMatrix symetricInv (stretchAxis.Transpose() * scaledAxis);
transformMatrix = symetricInv * (*this);
transformMatrix.m_posit = m_posit;
}
#if 0
dgVector dgMatrix::EigenVectors()
{
dgMatrix& mat = *this;
dgMatrix eigenVectors(dgGetIdentityMatrix());
dgVector d(mat[0][0], mat[1][1], mat[2][2], dgFloat32(0.0f));
dgVector b(d);
for (dgInt32 i = 0; i < 50; i++) {
dgFloat32 sm = mat[0][1] * mat[0][1] + mat[0][2] * mat[0][2] + mat[1][2] * mat[1][2];
if (sm < dgFloat32(1.0e-12f)) {
// check the eigenvalue vectors
//dgVector tmp(eigenVectors.m_front.CrossProduct(eigenVectors.m_up));
//if (tmp.DotProduct(eigenVectors.m_right).GetScalar() < dgFloat32(0.0f)) {
// eigenVectors.m_right = eigenVectors.m_right * dgVector::m_negOne;
//}
dgAssert (eigenVectors[0].DotProduct(eigenVectors[1].CrossProduct(eigenVectors[2])).GetScalar() > dgFloat32 (0.0f));
break;
}
dgFloat32 thresh = dgFloat32(0.0f);
if (i < 3) {
thresh = (dgFloat32)(0.2f / 9.0f) * sm;
}
dgVector z(dgVector::m_zero);
for (dgInt32 ip = 0; ip < 2; ip++) {
for (dgInt32 iq = ip + 1; iq < 3; iq++) {
dgFloat32 g = dgFloat32(100.0f) * dgAbs(mat[ip][iq]);
if ((i > 3) && ((dgAbs(d[ip]) + g) == dgAbs(d[ip])) && ((dgAbs(d[iq]) + g) == dgAbs(d[iq]))) {
mat[ip][iq] = dgFloat32(0.0f);
} else if (dgAbs(mat[ip][iq]) > thresh) {
dgFloat32 t;
dgFloat32 h = d[iq] - d[ip];
if (dgAbs(h) + g == dgAbs(h)) {
t = mat[ip][iq] / h;
} else {
dgFloat32 theta = dgFloat32(0.5f) * h / mat[ip][iq];
t = dgFloat32(1.0f) / (dgAbs(theta) + dgSqrt(dgFloat32(1.0f) + theta * theta));
if (theta < dgFloat32(0.0f)) {
t = -t;
}
}
dgFloat32 c = dgRsqrt(dgFloat32(1.0f) + t * t);
dgFloat32 s = t * c;
dgFloat32 tau = s / (dgFloat32(1.0f) + c);
h = t * mat[ip][iq];
z[ip] -= h;
z[iq] += h;
d[ip] -= h;
d[iq] += h;
mat[ip][iq] = dgFloat32(0.0f);
for (dgInt32 j = 0; j <= ip - 1; j++) {
dgFloat32 g0 = mat[j][ip];
dgFloat32 h0 = mat[j][iq];
mat[j][ip] = g0 - s * (h0 + g0 * tau);
mat[j][iq] = h0 + s * (g0 - h0 * tau);
}
for (dgInt32 j = ip + 1; j <= iq - 1; j++) {
dgFloat32 g0 = mat[ip][j];
dgFloat32 h0 = mat[j][iq];
mat[ip][j] = g0 - s * (h0 + g0 * tau);
mat[j][iq] = h0 + s * (g0 - h0 * tau);
}
for (dgInt32 j = iq + 1; j < 3; j++) {
dgFloat32 g0 = mat[ip][j];
dgFloat32 h0 = mat[iq][j];
mat[ip][j] = g0 - s * (h0 + g0 * tau);
mat[iq][j] = h0 + s * (g0 - h0 * tau);
}
dgVector sv(s);
dgVector tauv(tau);
dgVector gv(eigenVectors[ip]);
dgVector hv(eigenVectors[iq]);
eigenVectors[ip] -= sv * (hv + gv * tauv);
eigenVectors[iq] += sv * (gv - hv * tauv);
}
}
}
b += z;
d = b;
}
*this = eigenVectors;
return d;
}
#else
dgVector dgMatrix::EigenVectors ()
{
dgMatrix matrix (*this);
dgMatrix eigenVectors(dgGetIdentityMatrix());
#if 0
if (dgAbs(m_front.m_z) > dgFloat32(1.0e-6f)) {
// calculate initial guess by convert to tridiagonal matrix using householder
// but this fail since it changes the oder of the Eigen values and Eigen vectors
dgVector u(m_front);
u.m_x = dgFloat32(0.0f);
dgVector v(dgVector::m_zero);
v.m_y = dgSqrt(u.DotProduct(u).GetScalar());
dgVector w(u - v);
w = w.Normalize();
eigenVectors = dgMatrix(w, w);
dgMatrix ident(dgGetIdentityMatrix());
eigenVectors[0] = ident[0] - eigenVectors[0] * dgVector::m_two;
eigenVectors[1] = ident[1] - eigenVectors[1] * dgVector::m_two;
eigenVectors[2] = ident[2] - eigenVectors[2] * dgVector::m_two;
matrix = eigenVectors * matrix * eigenVectors;
}
matrix[0][2] = dgFloat32(0.0f);
matrix[2][0] = dgFloat32(0.0f);
#endif
// QR algorithm is really bad at converging matrices with very different eigenvalue.
// the solution is to use RD with double shift which I do not feel like implementing.
// using Jacobi diagonalize instead
dgVector d (matrix[0][0], matrix[1][1], matrix[2][2], dgFloat32 (0.0f));
dgVector b (d);
for (dgInt32 i = 0; i < 50; i++) {
dgFloat32 sm = matrix[0][1] * matrix[0][1] + matrix[0][2] * matrix[0][2] + matrix[1][2] * matrix[1][2];
if (sm < dgFloat32 (1.0e-12f)) {
// make sure the the Eigen vectors are orthonormal
//dgVector tmp (eigenVectors.m_front.CrossProduct(eigenVectors.m_up));
//if (tmp.DotProduct(eigenVectors.m_right).GetScalar() < dgFloat32(0.0f)) {
// eigenVectors.m_right = eigenVectors.m_right * dgVector::m_negOne;
//}
dgAssert (eigenVectors[0].DotProduct(eigenVectors[1].CrossProduct(eigenVectors[2])).GetScalar() > dgFloat32 (0.0f));
break;
}
dgFloat32 thresh = dgFloat32 (0.0f);
if (i < 3) {
thresh = (dgFloat32)(0.2f / 9.0f) * sm;
}
dgVector z (dgVector::m_zero);
for (dgInt32 j = 0; j < 2; j ++) {
for (dgInt32 k = j + 1; k < 3; k ++) {
dgFloat32 g = dgFloat32 (100.0f) * dgAbs(matrix[j][k]);
if ((i > 3) && ((dgAbs(d[j]) + g) == dgAbs(d[j])) && ((dgAbs(d[k]) + g) == dgAbs(d[k]))) {
matrix[j][k] = dgFloat32 (0.0f);
} else if (dgAbs(matrix[j][k]) > thresh) {
dgFloat32 t;
dgFloat32 h = d[k] - d[j];
if (dgAbs(h) + g == dgAbs(h)) {
t = matrix[j][k] / h;
} else {
dgFloat32 theta = dgFloat32 (0.5f) * h / matrix[j][k];
t = dgFloat32(1.0f) / (dgAbs(theta) + dgSqrt(dgFloat32(1.0f) + theta * theta));
if (theta < dgFloat32 (0.0f)) {
t = -t;
}
}
dgFloat32 c = dgRsqrt (dgFloat32 (1.0f) + t * t);
dgFloat32 s = t * c;
dgFloat32 tau = s / (dgFloat32(1.0f) + c);
h = t * matrix[j][k];
z[j] -= h;
z[k] += h;
d[j] -= h;
d[k] += h;
matrix[j][k] = dgFloat32(0.0f);
for (dgInt32 n = 0; n <= j - 1; n ++) {
dgFloat32 g0 = matrix[n][j];
dgFloat32 h0 = matrix[n][k];
matrix[n][j] = g0 - s * (h0 + g0 * tau);
matrix[n][k] = h0 + s * (g0 - h0 * tau);
}
for (dgInt32 n = j + 1; n <= k - 1; n ++) {
dgFloat32 g0 = matrix[j][n];
dgFloat32 h0 = matrix[n][k];
matrix[j][n] = g0 - s * (h0 + g0 * tau);
matrix[n][k] = h0 + s * (g0 - h0 * tau);
}
for (dgInt32 n = k + 1; n < 3; n ++) {
dgFloat32 g0 = matrix[j][n];
dgFloat32 h0 = matrix[k][n];
matrix[j][n] = g0 - s * (h0 + g0 * tau);
matrix[k][n] = h0 + s * (g0 - h0 * tau);
}
dgVector sv (s);
dgVector tauv (tau);
dgVector gv (eigenVectors[j]);
dgVector hv (eigenVectors[k]);
eigenVectors[j] -= sv * (hv + gv * tauv);
eigenVectors[k] += sv * (gv - hv * tauv);
}
}
}
b += z;
d = b;
}
#ifdef _DEBUG
dgMatrix diag(dgGetIdentityMatrix());
diag[0][0] = d[0];
diag[1][1] = d[1];
diag[2][2] = d[2];
dgMatrix E(eigenVectors.Transpose());
dgMatrix originalMatrix(*this);
dgMatrix tempMatrix(E * diag * E.Transpose());
for (dgInt32 j = 0; j < 3; j++) {
for (dgInt32 k = 0; k < 3; k++) {
dgFloat32 error = originalMatrix[j][k] - tempMatrix[j][k];
dgAssert((error * error) < dgFloat32(1.0e-4f));
}
}
#endif
*this = eigenVectors;
return d;
}
#endif
dgSpatialMatrix dgSpatialMatrix::Inverse(dgInt32 rows) const
{
dgSpatialMatrix tmp(*this);
dgSpatialMatrix inv(dgFloat64(0.0f));
for (dgInt32 i = 0; i < rows; i++) {
inv[i][i] = dgFloat32(1.0f);
}
for (dgInt32 i = 0; i < rows; i++) {
dgFloat64 pivot = dgAbs(tmp[i][i]);
if (pivot < dgFloat64(0.01f)) {
int permute = i;
for (dgInt32 j = i + 1; j < rows; j++) {
dgFloat64 pivot1 = dgAbs(tmp[j][i]);
if (pivot1 > pivot) {
permute = j;
pivot = pivot1;
}
}
dgAssert(pivot > dgFloat32(0.0f));
dgAssert((pivot > dgFloat32(1.0e-6f)) || (dgConditionNumber(rows, 6, (dgFloat64*)&m_rows[0]) < dgFloat32(1.0e5f)));
//if (!((pivot > dgFloat32(1.0e-6f)) || (dgConditionNumber(rows, 6, (dgFloat64*)&m_rows[0]) < dgFloat32(1.0e5f))))
//{
// for (dgInt32 m = 0; m < rows; m++) {
// for (dgInt32 n = 0; n < rows; n++) {
// dgTrace(("%f ", m_rows[m][n]));
// }
// dgTrace(("\n"));
// }
// dgAssert(0);
//}
if (permute != i) {
for (dgInt32 j = 0; j < rows; j++) {
dgSwap(tmp[i][j], tmp[permute][j]);
dgSwap(tmp[i][j], tmp[permute][j]);
}
}
}
for (dgInt32 j = i + 1; j < rows; j++) {
dgFloat64 scale = tmp[j][i] / tmp[i][i];
tmp[j][i] = dgFloat64(0.0f);
for (int k = i + 1; k < rows; k++) {
tmp[j][k] -= scale * tmp[i][k];
}
for (int k = 0; k <= i; k++) {
inv[j][k] -= scale * inv[i][k];
}
}
}
for (dgInt32 i = rows - 1; i >= 0; i--) {
dgSpatialVector acc(dgFloat64(0.0f));
for (dgInt32 j = i + 1; j < rows; j++) {
dgFloat64 pivot = tmp[i][j];
for (int k = 0; k < rows; k++) {
acc[k] += pivot * inv[j][k];
}
}
dgFloat64 den = dgFloat64(1.0f) / tmp[i][i];
for (dgInt32 k = 0; k < rows; k++) {
inv[i][k] = den * (inv[i][k] - acc[k]);
}
}
#ifdef _DEBUG
for (dgInt32 i = 0; i < rows; i++) {
for (dgInt32 j = 0; j < rows; j++) {
tmp[i][j] = m_rows[j][i];
}
}
for (dgInt32 i = 0; i < rows; i++) {
dgSpatialVector v(inv.VectorTimeMatrix(tmp[i], rows));
dgAssert(dgAbs(v[i] - dgFloat64(1.0f)) < dgFloat64(1.0e-6f));
for (dgInt32 j = 0; j < rows; j++) {
if (j != i) {
dgAssert(dgAbs(v[j]) < dgFloat64(1.0e-6f));
}
}
}
#endif
return inv;
}

380
thirdparty/src/newton/dgCore/dgMatrix.h vendored Normal file
View File

@@ -0,0 +1,380 @@
/* Copyright (c) <2003-2019> <Julio Jerez, Newton Game Dynamics>
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
*
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
*
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
*
* 3. This notice may not be removed or altered from any source distribution.
*/
#ifndef __dgMatrix__
#define __dgMatrix__
#include "dgStdafx.h"
#include "dgDebug.h"
#include "dgVector.h"
#include "dgPlane.h"
#include <math.h>
class dgMatrix;
class dgQuaternion;
const dgMatrix& dgGetZeroMatrix ();
const dgMatrix& dgGetIdentityMatrix();
DG_MSC_VECTOR_ALIGNMENT
class dgMatrix
{
public:
DG_CLASS_ALLOCATOR(allocator)
dgMatrix ();
dgMatrix (const dgFloat32* const array);
dgMatrix (const dgVector &front, const dgVector &up, const dgVector &right, const dgVector &posit);
dgMatrix (const dgQuaternion &rotation, const dgVector &position);
// create a orthonormal normal vector basis, front become m_front vector, and m_up and m_right are mutualiperpendicular to fron and to each other
dgMatrix (const dgVector &front);
// create a covariance Matrix = transpose(p) * q
dgMatrix (const dgVector& p, const dgVector& q);
dgVector& operator[] (dgInt32 i);
const dgVector& operator[] (dgInt32 i) const;
dgMatrix Inverse () const;
dgMatrix Inverse4x4 () const;
dgMatrix Transpose () const;
dgMatrix Transpose4X4 () const;
dgVector RotateVector (const dgVector &v) const;
dgVector UnrotateVector (const dgVector &v) const;
dgVector TransformVector (const dgVector &v) const;
dgVector UntransformVector (const dgVector &v) const;
dgPlane TransformPlane (const dgPlane &localPlane) const;
dgPlane UntransformPlane (const dgPlane &globalPlane) const;
dgVector SolveByGaussianElimination(const dgVector &v) const;
void TransformBBox (const dgVector& p0local, const dgVector& p1local, dgVector& p0, dgVector& p1) const;
void CalcPitchYawRoll (dgVector& euler0, dgVector& euler1) const;
void TransformTriplex (dgFloat32* const dst, dgInt32 dstStrideInBytes,
const dgFloat32* const src, dgInt32 srcStrideInBytes, dgInt32 count) const;
#ifndef _NEWTON_USE_DOUBLE
void TransformTriplex (dgFloat64* const dst, dgInt32 dstStrideInBytes,
const dgFloat64* const src, dgInt32 srcStrideInBytes, dgInt32 count) const;
void TransformTriplex (dgFloat64* const dst, dgInt32 dstStrideInBytes,
const dgFloat32* const src, dgInt32 srcStrideInBytes, dgInt32 count) const;
#endif
bool TestIdentity() const;
bool TestSymetric3x3() const;
bool TestOrthogonal(dgFloat32 tol = dgFloat32 (1.0e-4f)) const;
dgMatrix Multiply3X3 (const dgMatrix &B) const;
dgMatrix operator* (const dgMatrix &B) const;
// these function can only be called when dgMatrix is a PDS matrix
//void EigenVectors ();
dgVector EigenVectors ();
void PolarDecomposition (dgMatrix& transformMatrix, dgVector& scale, dgMatrix& stretchAxis) const;
// constructor for polar composition
dgMatrix (const dgMatrix& transformMatrix, const dgVector& scale, const dgMatrix& stretchAxis);
dgVector m_front;
dgVector m_up;
dgVector m_right;
dgVector m_posit;
static dgMatrix m_zeroMatrix;
static dgMatrix m_identityMatrix;
} DG_GCC_VECTOR_ALIGNMENT;
DG_INLINE dgMatrix::dgMatrix ()
{
}
DG_INLINE dgMatrix::dgMatrix (const dgFloat32* const array)
{
memcpy (&m_front.m_x, array, sizeof (dgMatrix)) ;
}
DG_INLINE dgMatrix::dgMatrix (const dgVector &front, const dgVector &up, const dgVector &right, const dgVector &posit)
:m_front (front), m_up(up), m_right(right), m_posit(posit)
{
}
DG_INLINE dgMatrix::dgMatrix (const dgVector& p, const dgVector& q)
:m_front(q * p.BroadcastX())
,m_up (q * p.BroadcastY())
,m_right(q * p.BroadcastZ())
,m_posit (dgVector::m_wOne)
{
}
DG_INLINE dgMatrix::dgMatrix (const dgVector& front)
{
m_front = front;
if (dgAbs (front.m_z) > dgFloat32 (0.577f)) {
m_right = front.CrossProduct(dgVector (-front.m_y, front.m_z, dgFloat32(0.0f), dgFloat32(0.0f)));
} else {
m_right = front.CrossProduct(dgVector (-front.m_y, front.m_x, dgFloat32(0.0f), dgFloat32(0.0f)));
}
//m_right = m_right.Scale (dgRsqrt (m_right.DotProduct(m_right).GetScalar()));
m_right = m_right.Normalize();
m_up = m_right.CrossProduct(m_front);
m_front.m_w = dgFloat32(0.0f);
m_up.m_w = dgFloat32(0.0f);
m_right.m_w = dgFloat32(0.0f);
m_posit = dgVector (dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(1.0f));
dgAssert ((dgAbs (m_front.DotProduct(m_front).GetScalar()) - dgFloat32(1.0f)) < dgFloat32(1.0e-5f));
dgAssert ((dgAbs (m_up.DotProduct(m_up).GetScalar()) - dgFloat32(1.0f)) < dgFloat32(1.0e-5f));
dgAssert ((dgAbs (m_right.DotProduct(m_right).GetScalar()) - dgFloat32(1.0f)) < dgFloat32(1.0e-5f));
dgAssert ((dgAbs (m_right.DotProduct(m_front.CrossProduct(m_up)).GetScalar()) - dgFloat32(1.0f)) < dgFloat32(1.0e-5f));
}
DG_INLINE dgVector& dgMatrix::operator[] (dgInt32 i)
{
dgAssert (i < 4);
dgAssert (i >= 0);
return (&m_front)[i];
}
DG_INLINE const dgVector& dgMatrix::operator[] (dgInt32 i) const
{
dgAssert (i < 4);
dgAssert (i >= 0);
return (&m_front)[i];
}
DG_INLINE dgMatrix dgMatrix::Transpose () const
{
dgMatrix inv;
dgVector::Transpose4x4(inv[0], inv[1], inv[2], inv[3], m_front, m_up, m_right, dgVector::m_wOne);
return inv;
}
DG_INLINE dgMatrix dgMatrix::Transpose4X4 () const
{
dgMatrix inv;
dgVector::Transpose4x4(inv[0], inv[1], inv[2], inv[3], m_front, m_up, m_right, m_posit);
return inv;
}
DG_INLINE dgVector dgMatrix::RotateVector (const dgVector &v) const
{
return m_front * v.BroadcastX() + m_up * v.BroadcastY() + m_right * v.BroadcastZ();
}
DG_INLINE dgVector dgMatrix::UnrotateVector (const dgVector &v) const
{
return dgVector ((m_front * v).AddHorizontal().GetScalar(), (m_up * v).AddHorizontal().GetScalar(), (m_right * v).AddHorizontal().GetScalar(), dgFloat32 (0.0f));
}
DG_INLINE dgVector dgMatrix::TransformVector (const dgVector &v) const
{
return RotateVector(v) + m_posit;
}
DG_INLINE dgVector dgMatrix::UntransformVector (const dgVector &v) const
{
return UnrotateVector(v - m_posit);
}
DG_INLINE dgPlane dgMatrix::TransformPlane (const dgPlane &localPlane) const
{
return dgPlane (RotateVector (localPlane), localPlane.m_w - (localPlane.DotProduct(UnrotateVector (m_posit)).GetScalar()));
}
DG_INLINE dgPlane dgMatrix::UntransformPlane (const dgPlane &globalPlane) const
{
return dgPlane (UnrotateVector (globalPlane), globalPlane.Evalue(m_posit));
}
/*
DG_INLINE void dgMatrix::EigenVectors ()
{
dgVector eigenValues;
EigenVectors (eigenValues);
}
*/
DG_INLINE dgMatrix dgMatrix::Inverse () const
{
// much faster inverse
dgMatrix inv;
dgVector::Transpose4x4 (inv[0], inv[1], inv[2], inv[3], m_front, m_up, m_right, dgVector::m_wOne);
inv.m_posit -= inv[0] * m_posit.BroadcastX() + inv[1] * m_posit.BroadcastY() + inv[2] * m_posit.BroadcastZ();
return inv;
}
DG_INLINE bool dgMatrix::TestIdentity() const
{
const dgMatrix& me = *this;
for (int i = 0; i < 4; i++) {
if (me[i][i] != dgFloat32 (1.0f)) {
return false;
}
for (int j = i + 1; j < 4; j++) {
if (me[i][j] != dgFloat32 (0.0f)) {
return false;
}
if (me[j][i] != dgFloat32(0.0f)) {
return false;
}
}
}
return true;
}
DG_INLINE bool dgMatrix::TestOrthogonal(dgFloat32 tol) const
{
dgVector n (m_front.CrossProduct(m_up));
dgFloat32 a = m_right.DotProduct(m_right).GetScalar();
dgFloat32 b = m_up.DotProduct(m_up).GetScalar();
dgFloat32 c = m_front.DotProduct(m_front).GetScalar();
dgFloat32 d = n.DotProduct(m_right).GetScalar();
#ifdef _DEBUG
const dgMatrix& me = *this;
for (int i = 0; i < 4; i++) {
for (int j = 0; j < 4; j++) {
dgAssert(dgCheckFloat(me[i][j]));
}
}
#endif
return (m_front[3] == dgFloat32 (0.0f)) &
(m_up[3] == dgFloat32 (0.0f)) &
(m_right[3] == dgFloat32 (0.0f)) &
(m_posit[3] == dgFloat32 (1.0f)) &
(dgAbs(a - dgFloat32 (1.0f)) < tol) &
(dgAbs(b - dgFloat32 (1.0f)) < tol) &
(dgAbs(c - dgFloat32 (1.0f)) < tol) &
(dgAbs(d - dgFloat32 (1.0f)) < tol);
}
DG_INLINE bool dgMatrix::TestSymetric3x3() const
{
const dgMatrix& me = *this;
return (dgAbs (me[0][1] - me[1][0]) < dgFloat32 (1.0e-5f)) &&
(dgAbs (me[0][2] - me[2][0]) < dgFloat32 (1.0e-5f)) &&
(dgAbs (me[1][2] - me[2][1]) < dgFloat32 (1.0e-5f)) &&
(me[0][3] == dgFloat32 (0.0f)) &&
(me[1][3] == dgFloat32 (0.0f)) &&
(me[2][3] == dgFloat32 (0.0f)) &&
(me[3][0] == dgFloat32 (0.0f)) &&
(me[3][1] == dgFloat32 (0.0f)) &&
(me[3][2] == dgFloat32 (0.0f)) &&
(me[3][3] == dgFloat32 (1.0f));
}
DG_INLINE dgMatrix dgPitchMatrix(dgFloat32 ang)
{
dgFloat32 sinAng = dgSin (ang);
dgFloat32 cosAng = dgCos (ang);
return dgMatrix (dgVector (dgFloat32(1.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f)),
dgVector (dgFloat32(0.0f), cosAng, sinAng, dgFloat32(0.0f)),
dgVector (dgFloat32(0.0f), -sinAng, cosAng, dgFloat32(0.0f)),
dgVector (dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(1.0f)));
}
DG_INLINE dgMatrix dgYawMatrix(dgFloat32 ang)
{
dgFloat32 sinAng = dgSin (ang);
dgFloat32 cosAng = dgCos (ang);
return dgMatrix (dgVector (cosAng, dgFloat32(0.0f), -sinAng, dgFloat32(0.0f)),
dgVector (dgFloat32(0.0f), dgFloat32(1.0f), dgFloat32(0.0f), dgFloat32(0.0f)),
dgVector (sinAng, dgFloat32(0.0f), cosAng, dgFloat32(0.0f)),
dgVector (dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(1.0f)));
}
DG_INLINE dgMatrix dgRollMatrix(dgFloat32 ang)
{
dgFloat32 sinAng = dgSin (ang);
dgFloat32 cosAng = dgCos (ang);
return dgMatrix (dgVector ( cosAng, sinAng, dgFloat32(0.0f), dgFloat32(0.0f)),
dgVector (-sinAng, cosAng, dgFloat32(0.0f), dgFloat32(0.0f)),
dgVector ( dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(1.0f), dgFloat32(0.0f)),
dgVector ( dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(1.0f)));
}
DG_MSC_VECTOR_ALIGNMENT
class dgSpatialMatrix
{
public:
DG_INLINE dgSpatialMatrix()
{
}
DG_INLINE dgSpatialMatrix(dgFloat32 val)
{
const dgSpatialVector row (val);
for (dgInt32 i = 0; i < 6; i++) {
m_rows[i] = row;
}
}
DG_INLINE dgSpatialVector& operator[] (dgInt32 i)
{
dgAssert(i < 6);
dgAssert(i >= 0);
return m_rows[i];
}
DG_INLINE const dgSpatialVector& operator[] (dgInt32 i) const
{
dgAssert(i < 6);
dgAssert(i >= 0);
return m_rows[i];
}
dgSpatialMatrix Inverse(dgInt32 rows) const;
DG_INLINE dgSpatialVector VectorTimeMatrix(const dgSpatialVector& jacobian) const
{
dgSpatialVector tmp(m_rows[0].Scale (jacobian[0]));
for (dgInt32 i = 1; i < 6; i++) {
tmp = tmp + m_rows[i].Scale(jacobian[i]);
}
return tmp;
}
DG_INLINE dgSpatialVector VectorTimeMatrix(const dgSpatialVector& jacobian, dgInt32 dof) const
{
dgSpatialVector tmp(dgFloat32 (0.0f));
for (dgInt32 i = 0; i < dof; i++) {
tmp = tmp + m_rows[i].Scale(jacobian[i]);
}
return tmp;
}
dgSpatialVector m_rows[6];
} DG_GCC_VECTOR_ALIGNMENT;
#endif

781
thirdparty/src/newton/dgCore/dgMemory.cpp vendored Normal file
View File

@@ -0,0 +1,781 @@
/* Copyright (c) <2003-2019> <Julio Jerez, Newton Game Dynamics>
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
*
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
*
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
*
* 3. This notice may not be removed or altered from any source distribution.
*/
#include "dgStdafx.h"
#include "dgList.h"
#include "dgDebug.h"
#include "dgMemory.h"
#ifdef DG_OLD_ALLOCATOR
dgInt32 dgMemoryAllocator::m_lock0 = 0;
dgInt32 dgMemoryAllocator::m_lock1 = 0;
#define DG_MEMORY_LOCK() dgScopeSpinPause lock (&dgMemoryAllocator::m_lock0);
#define DG_MEMORY_LOCK_LOW() dgScopeSpinPause lock (&dgMemoryAllocator::m_lock1);
class dgMemoryAllocator::dgMemoryBin
{
public:
class dgMemoryBinInfo
{
public:
dgInt32 m_count;
dgInt32 m_totalCount;
dgInt32 m_stepInBytes;
dgMemoryBin* m_next;
dgMemoryBin* m_prev;
};
char m_pool[DG_MEMORY_BIN_SIZE - sizeof (dgMemoryBinInfo)-DG_MEMORY_GRANULARITY * 2];
dgMemoryBinInfo m_info;
};
class dgMemoryAllocator::dgMemoryCacheEntry
{
public:
dgMemoryCacheEntry* m_next;
dgMemoryCacheEntry* m_prev;
};
class dgMemoryAllocator::dgMemoryInfo
{
public:
void *m_ptr;
dgMemoryAllocator* m_allocator;
dgInt32 m_size;
dgInt32 m_enum;
#ifdef _DEBUG
dgInt32 m_workingSize;
#endif
DG_INLINE void SaveInfo(dgMemoryAllocator* const allocator, void* const ptr, dgInt32 size, dgInt32& enumerator, dgInt32 workingSize = 0)
{
m_ptr = ptr;
m_size = size;
m_enum = enumerator;
enumerator++;
m_allocator = allocator;
#ifdef _DEBUG
m_workingSize = workingSize;
#endif
}
};
class dgGlobalAllocator: public dgMemoryAllocator, public dgList<dgMemoryAllocator*>
{
public:
dgGlobalAllocator ()
:dgMemoryAllocator (__malloc__, __free__), dgList<dgMemoryAllocator*> (NULL)
{
SetAllocator (this);
}
~dgGlobalAllocator ()
{
dgAssert (GetCount() == 0);
}
static void* dgApi __malloc__ (dgUnsigned32 size)
{
return malloc (size);
}
static void dgApi __free__ (void* const ptr, dgUnsigned32 size)
{
free (ptr);
}
void operator delete (void* const ptr)
{
dgAssert (0);
free (ptr);
}
dgInt32 GetMemoryUsed () const
{
dgInt32 mem = m_memoryUsed;
for (dgList<dgMemoryAllocator*>::dgListNode* node = GetFirst(); node; node = node->GetNext()) {
mem += node->GetInfo()->GetMemoryUsed();
}
return mem;
}
static dgGlobalAllocator& GetGlobalAllocator()
{
static dgGlobalAllocator m_globalAllocator;
return m_globalAllocator;
}
};
dgMemoryAllocator::dgMemoryAllocator ()
:m_free(NULL)
,m_malloc(NULL)
,m_enumerator(0)
,m_memoryUsed(0)
,m_isInList(1)
{
SetAllocatorsCallback (dgGlobalAllocator::GetGlobalAllocator().m_malloc, dgGlobalAllocator::GetGlobalAllocator().m_free);
memset (m_memoryDirectory, 0, sizeof (m_memoryDirectory));
dgGlobalAllocator::GetGlobalAllocator().Append(this);
}
dgMemoryAllocator::dgMemoryAllocator (dgMemAlloc memAlloc, dgMemFree memFree)
:m_free(NULL)
,m_malloc(NULL)
,m_enumerator(0)
,m_memoryUsed(0)
,m_isInList(0)
{
SetAllocatorsCallback (memAlloc, memFree);
memset (m_memoryDirectory, 0, sizeof (m_memoryDirectory));
}
dgMemoryAllocator::~dgMemoryAllocator ()
{
if (m_isInList) {
dgGlobalAllocator::GetGlobalAllocator().Remove(this);
}
dgAssert (m_memoryUsed == 0);
}
void *dgMemoryAllocator::operator new (size_t size)
{
return dgMallocStack(size);
}
void dgMemoryAllocator::operator delete (void* const ptr)
{
dgFreeStack(ptr);
}
dgInt32 dgMemoryAllocator::GetMemoryUsed() const
{
return m_memoryUsed;
}
void dgMemoryAllocator::SetAllocatorsCallback (dgMemAlloc memAlloc, dgMemFree memFree)
{
m_free = memFree;
m_malloc = memAlloc;
}
void *dgMemoryAllocator::MallocLow (dgInt32 workingSize, dgInt32 alignment)
{
DG_MEMORY_LOCK_LOW();
alignment = dgMax (alignment, DG_MEMORY_GRANULARITY);
dgAssert (((-alignment) & (alignment - 1)) == 0);
dgInt32 size = workingSize + alignment * 2;
void* const ptr = m_malloc(dgUnsigned32 (size));
dgAssert (ptr);
#ifdef _DEBUG
memset(ptr, 99, size_t(size));
#endif
dgUnsigned64 val = dgUnsigned64 (PointerToInt(ptr));
val = (val & dgUnsigned64(-alignment)) + alignment * 2;
void* const retPtr = IntToPointer (val);
dgMemoryInfo* const info = ((dgMemoryInfo*) (retPtr)) - 1;
info->SaveInfo(this, ptr, size, m_enumerator, workingSize);
dgAtomicExchangeAndAdd (&m_memoryUsed, size);
return retPtr;
}
void dgMemoryAllocator::FreeLow (void* const retPtr)
{
DG_MEMORY_LOCK_LOW();
dgMemoryInfo* const info = ((dgMemoryInfo*) (retPtr)) - 1;
dgAssert (info->m_allocator == this);
dgAtomicExchangeAndAdd (&m_memoryUsed, -info->m_size);
#ifdef _DEBUG
memset (retPtr, 0, size_t(info->m_workingSize));
#endif
m_free (info->m_ptr, dgUnsigned32 (info->m_size));
}
void *dgMemoryAllocator::Malloc (dgInt32 memsize)
{
dgAssert (dgInt32 (sizeof (dgMemoryCacheEntry) + sizeof (dgInt32) + sizeof(dgInt32)) <= DG_MEMORY_GRANULARITY);
dgInt32 size = memsize + DG_MEMORY_GRANULARITY - 1;
size &= (-DG_MEMORY_GRANULARITY);
dgInt32 paddedSize = size + DG_MEMORY_GRANULARITY;
dgInt32 entry = paddedSize >> DG_MEMORY_GRANULARITY_BITS;
void* ptr;
if (entry >= DG_MEMORY_BIN_ENTRIES) {
ptr = MallocLow (size);
} else {
DG_MEMORY_LOCK();
if (!m_memoryDirectory[entry].m_cache) {
dgMemoryBin* const bin = (dgMemoryBin*) MallocLow (sizeof (dgMemoryBin));
dgInt32 count = dgInt32 (sizeof (bin->m_pool) / paddedSize);
bin->m_info.m_count = 0;
bin->m_info.m_totalCount = count;
bin->m_info.m_stepInBytes = paddedSize;
bin->m_info.m_next = m_memoryDirectory[entry].m_first;
bin->m_info.m_prev = NULL;
if (bin->m_info.m_next) {
bin->m_info.m_next->m_info.m_prev = bin;
}
m_memoryDirectory[entry].m_first = bin;
dgInt8* charPtr = reinterpret_cast<dgInt8*>(bin->m_pool);
m_memoryDirectory[entry].m_cache = (dgMemoryCacheEntry*)charPtr;
for (dgInt32 i = 0; i < count; i ++) {
dgMemoryCacheEntry* const cashe = (dgMemoryCacheEntry*) charPtr;
cashe->m_next = (dgMemoryCacheEntry*) (charPtr + paddedSize);
cashe->m_prev = (dgMemoryCacheEntry*) (charPtr - paddedSize);
dgMemoryInfo* const info = ((dgMemoryInfo*) (charPtr + DG_MEMORY_GRANULARITY)) - 1;
info->SaveInfo(this, bin, entry, m_enumerator, memsize);
charPtr += paddedSize;
}
dgMemoryCacheEntry* const cashe = (dgMemoryCacheEntry*) (charPtr - paddedSize);
cashe->m_next = NULL;
m_memoryDirectory[entry].m_cache->m_prev = NULL;
}
dgAssert (m_memoryDirectory[entry].m_cache);
dgMemoryCacheEntry* const cashe = m_memoryDirectory[entry].m_cache;
m_memoryDirectory[entry].m_cache = cashe->m_next;
if (cashe->m_next) {
cashe->m_next->m_prev = NULL;
}
ptr = ((dgInt8*)cashe) + DG_MEMORY_GRANULARITY;
dgMemoryInfo* info;
info = ((dgMemoryInfo*) (ptr)) - 1;
dgAssert (info->m_allocator == this);
dgMemoryBin* const bin = (dgMemoryBin*) info->m_ptr;
bin->m_info.m_count ++;
}
return ptr;
}
void dgMemoryAllocator::Free (void* const retPtr)
{
dgMemoryInfo* const info = ((dgMemoryInfo*) (retPtr)) - 1;
dgAssert (info->m_allocator == this);
dgInt32 entry = info->m_size;
if (entry >= DG_MEMORY_BIN_ENTRIES) {
FreeLow (retPtr);
} else {
DG_MEMORY_LOCK();
dgMemoryCacheEntry* const cashe = (dgMemoryCacheEntry*) (((char*)retPtr) - DG_MEMORY_GRANULARITY) ;
dgMemoryCacheEntry* const tmpCashe = m_memoryDirectory[entry].m_cache;
if (tmpCashe) {
dgAssert (!tmpCashe->m_prev);
tmpCashe->m_prev = cashe;
}
cashe->m_next = tmpCashe;
cashe->m_prev = NULL;
m_memoryDirectory[entry].m_cache = cashe;
dgMemoryBin* const bin = (dgMemoryBin *) info->m_ptr;
dgAssert (bin);
#ifdef _DEBUG
dgAssert ((bin->m_info.m_stepInBytes - DG_MEMORY_GRANULARITY) > 0);
memset (retPtr, 0, size_t(bin->m_info.m_stepInBytes - DG_MEMORY_GRANULARITY));
#endif
bin->m_info.m_count --;
if (bin->m_info.m_count == 0) {
dgInt32 count = bin->m_info.m_totalCount;
dgInt32 sizeInBytes = bin->m_info.m_stepInBytes;
char* charPtr = bin->m_pool;
for (dgInt32 i = 0; i < count; i ++) {
dgMemoryCacheEntry* const tmpCashe1 = (dgMemoryCacheEntry*)charPtr;
charPtr += sizeInBytes;
if (tmpCashe1 == m_memoryDirectory[entry].m_cache) {
m_memoryDirectory[entry].m_cache = tmpCashe1->m_next;
}
if (tmpCashe1->m_prev) {
tmpCashe1->m_prev->m_next = tmpCashe1->m_next;
}
if (tmpCashe1->m_next) {
tmpCashe1->m_next->m_prev = tmpCashe1->m_prev;
}
}
if (m_memoryDirectory[entry].m_first == bin) {
m_memoryDirectory[entry].m_first = bin->m_info.m_next;
}
if (bin->m_info.m_next) {
bin->m_info.m_next->m_info.m_prev = bin->m_info.m_prev;
}
if (bin->m_info.m_prev) {
bin->m_info.m_prev->m_info.m_next = bin->m_info.m_next;
}
FreeLow (bin);
}
}
}
dgInt32 dgMemoryAllocator::GetSize (void* const retPtr)
{
dgMemoryInfo* const info = ((dgMemoryInfo*)(retPtr)) - 1;
return info->m_size;
}
void dgMemoryAllocator::SetGlobalAllocators (dgMemAlloc malloc, dgMemFree free)
{
dgGlobalAllocator::GetGlobalAllocator().SetAllocatorsCallback (malloc, free);
}
dgInt32 dgMemoryAllocator::GetGlobalMemoryUsed ()
{
return dgGlobalAllocator::GetGlobalAllocator().GetMemoryUsed();
}
// this can be used by function that allocates large memory pools memory locally on the stack
// this by pases the pool allocation because this should only be used for very large memory blocks.
// this was using virtual memory on windows but
// but because of many complaint I changed it to use malloc and free
void* dgApi dgMallocStack (size_t size)
{
void * const ptr = dgGlobalAllocator::GetGlobalAllocator().MallocLow (dgInt32 (size));
return ptr;
}
void* dgApi dgMallocAligned (size_t size, dgInt32 align)
{
void * const ptr = dgGlobalAllocator::GetGlobalAllocator().MallocLow (dgInt32 (size), align);
return ptr;
}
// this can be used by function that allocates large memory pools memory locally on the stack
// this by pases the pool allocation because this should only be used for very large memory blocks.
// this was using virtual memory on windows but
// but because of many complaint I changed it to use malloc and free
void dgApi dgFreeStack (void* const ptr)
{
dgGlobalAllocator::GetGlobalAllocator().FreeLow (ptr);
}
// general memory allocation for all data in the library
void* dgApi dgMalloc (size_t size, dgMemoryAllocator* const allocator)
{
void* ptr = NULL;
dgAssert (allocator);
if (size) {
ptr = allocator->Malloc (dgInt32 (size));
}
return ptr;
}
// general deletion allocation for all data in the library
void dgApi dgFree (void* const ptr)
{
if (ptr) {
dgMemoryAllocator::dgMemoryInfo* const info = ((dgMemoryAllocator::dgMemoryInfo*) ptr) - 1;
dgAssert (info->m_allocator);
info->m_allocator->Free (ptr);
}
}
#else
void* dgGlobalAllocator::__malloc__(dgUnsigned32 size)
{
return malloc(size);
}
void dgGlobalAllocator::__free__(void* const ptr, dgUnsigned32 size)
{
free(ptr);
}
void dgGlobalAllocator::SetAllocatorsCallback (dgMemAlloc malloc, dgMemFree free)
{
m_free = free;
m_malloc = malloc;
}
void* dgGlobalAllocator::Malloc(dgInt32 size)
{
dgInt32 paddedSize = size + sizeof (dgMemoryAllocator::dgMemoryGranularity) + sizeof (dgMemoryAllocator::dgMemoryHeader);
char* const ptr = (char*)m_malloc(paddedSize);
dgUnsigned64 address = dgUnsigned64(ptr + sizeof (dgMemoryAllocator::dgMemoryHeader) + sizeof (dgMemoryAllocator::dgMemoryGranularity)-1) & ~(sizeof (dgMemoryAllocator::dgMemoryGranularity)-1);
dgMemoryAllocator::dgMemoryHeader* const info = ((dgMemoryAllocator::dgMemoryHeader*)address) - 1;
info->m_ptr = ptr;
info->m_allocator = this;
info->m_size = size;
info->m_paddedSize = paddedSize;
dgAtomicExchangeAndAdd(&m_memoryUsed, paddedSize);
return &info[1];
}
void dgGlobalAllocator::Free(void* const ptr)
{
dgMemoryAllocator::dgMemoryHeader* const info = ((dgMemoryAllocator::dgMemoryHeader*)ptr) - 1;
dgAssert(info->m_allocator == this);
dgAtomicExchangeAndAdd(&m_memoryUsed, -info->m_paddedSize);
m_free(info->m_ptr, info->m_paddedSize);
}
dgMemoryAllocatorBase& dgGlobalAllocator::GetGlobalAllocator()
{
static dgGlobalAllocator m_globalAllocator;
return m_globalAllocator;
}
dgMemoryAllocator::dgMemoryPage::dgMemoryPage(dgInt32 size, dgMemoryPage* const root, dgMemoryAllocator* const allocator)
:m_next(root)
,m_prev(NULL)
,m_fullPageNext(NULL)
,m_fullPagePrev(NULL)
,m_freeList (NULL)
,m_count(0)
,m_capacity(0)
{
if (root) {
dgAssert (!root->m_prev);
root->m_prev = this;
}
dgInt32 paddSize = size + sizeof (dgMemoryHeader);
dgAssert ((paddSize & (sizeof (dgMemoryGranularity) - 1)) == 0);
const char* const ptr1 = &m_buffer[sizeof (m_buffer) - paddSize];
for (char* ptr = &m_buffer[sizeof (dgMemoryGranularity)]; ptr <= ptr1; ptr += paddSize) {
dgAssert ((dgUnsigned64(ptr) & (sizeof (dgMemoryGranularity) - 1)) == 0);
dgMemoryGranularity* const freeList = (dgMemoryGranularity*) ptr;
dgMemoryHeader* const header = ((dgMemoryHeader*)freeList) - 1;
header->m_page = this;
header->m_allocator = allocator;
header->m_size = 0;
header->m_paddedSize = size;
freeList->m_next = m_freeList;
m_freeList = freeList;
m_count ++;
}
m_capacity = m_count;
}
dgMemoryAllocator::dgMemoryPage::~dgMemoryPage()
{
dgAssert(m_count == m_capacity);
}
void *dgMemoryAllocator::dgMemoryPage::operator new (size_t size)
{
dgMemoryAllocatorBase& globalAllocator = dgGlobalAllocator::GetGlobalAllocator();
return globalAllocator.Malloc(dgInt32 (size));
}
void dgMemoryAllocator::dgMemoryPage::operator delete (void* const ptr)
{
dgMemoryAllocatorBase& globalAllocator = dgGlobalAllocator::GetGlobalAllocator();
globalAllocator.Free(ptr);
}
void* dgMemoryAllocator::dgMemoryPage::Malloc(dgInt32 size)
{
m_count --;
dgAssert (m_count >= 0);
dgMemoryGranularity* const freeList = m_freeList;
m_freeList = m_freeList->m_next;
dgMemoryHeader* const header = ((dgMemoryHeader*)freeList) - 1;
header->m_size = size;
return freeList;
}
void dgMemoryAllocator::dgMemoryPage::Free(void* const ptr)
{
m_count++;
dgAssert(m_count <= m_capacity);
dgMemoryHeader* const info = ((dgMemoryHeader*)ptr) - 1;
info->m_size = 0;
dgMemoryGranularity* const freeList = (dgMemoryGranularity*) ptr;
freeList->m_next = m_freeList;
m_freeList = freeList;
}
dgMemoryAllocator::dgMemoryBeam::dgMemoryBeam()
:m_firstPage(NULL)
,m_fullPage(NULL)
,m_beamSize(0)
,m_inUsedCount(0)
,m_fullPageCount(0)
{
}
dgMemoryAllocator::dgMemoryBeam::~dgMemoryBeam()
{
while (m_firstPage) {
if (m_firstPage->m_next) {
m_firstPage->m_next->m_prev = NULL;
}
dgMemoryPage* const firstPage = m_firstPage;
m_firstPage = m_firstPage->m_next;
delete firstPage;
}
while (m_fullPage) {
dgAssert(0);
if (m_fullPage->m_fullPageNext) {
m_fullPage->m_fullPageNext->m_fullPagePrev = NULL;
}
dgMemoryPage* const fullPage = m_fullPage;
m_fullPage = m_fullPage->m_fullPageNext;
delete fullPage;
}
m_fullPage = NULL;
m_firstPage = NULL;
}
void* dgMemoryAllocator::dgMemoryBeam::Malloc(dgInt32 size)
{
dgAssert (size <= m_beamSize);
if (!m_firstPage) {
m_firstPage = new dgMemoryPage (m_beamSize, m_firstPage, m_allocator);
m_inUsedCount ++;
}
dgAssert (m_firstPage->m_count);
//if (m_firstPage->m_count == 0) {
// m_firstPage = new dgMemoryPage (m_beamSize, m_firstPage, m_allocator);
//}
void* ptr = m_firstPage->Malloc(size);
if (m_firstPage->m_count == 0) {
dgMemoryPage* const page = m_firstPage;
m_inUsedCount --;
m_fullPageCount ++;
m_firstPage = page->m_next;
if (m_firstPage) {
m_firstPage->m_prev = NULL;
}
page->m_next = NULL;
page->m_prev = NULL;
dgAssert(!page->m_fullPageNext);
dgAssert(!page->m_fullPagePrev);
page->m_fullPageNext = m_fullPage;
if (m_fullPage) {
m_fullPage->m_fullPagePrev = page;
}
m_fullPage = page;
}
return ptr;
}
void dgMemoryAllocator::dgMemoryBeam::Free(void* const ptr)
{
dgMemoryHeader* const info = ((dgMemoryHeader*)ptr) - 1;
dgMemoryPage* const page = info->m_page;
page->Free(ptr);
dgAssert (page->m_count <= page->m_capacity);
if (page->m_count == page->m_capacity) {
m_inUsedCount --;
if (page == m_firstPage) {
m_firstPage = m_firstPage->m_next;
}
if (page->m_next) {
page->m_next->m_prev = page->m_prev;
}
if (page->m_prev) {
page->m_prev->m_next = page->m_next;
}
page->m_next = NULL;
page->m_prev = NULL;
delete page;
} else if (page->m_count == 1) {
m_inUsedCount++;
m_fullPageCount--;
if (page == m_fullPage) {
m_fullPage = m_fullPage->m_fullPageNext;
}
if (page->m_fullPageNext) {
page->m_fullPageNext->m_fullPagePrev = page->m_fullPagePrev;
}
if (page->m_fullPagePrev) {
page->m_fullPagePrev->m_fullPageNext = page->m_fullPageNext;
}
page->m_fullPagePrev = NULL;
page->m_fullPageNext = NULL;
dgAssert(!page->m_next);
dgAssert(!page->m_prev);
page->m_next = m_firstPage;
if (m_firstPage) {
m_firstPage->m_prev = page;
}
m_firstPage = page;
// } else if (page->m_prev) {
// dgInt32 key = page->GetSortKey();
// dgMemoryPage* prevPage = page->m_prev;
// while (prevPage && prevPage->GetSortKey() < key)
// {
// prevPage = prevPage->m_prev;
// }
//
// if (prevPage) {
// dgAssert(0);
// } else {
// dgAssert(0);
// }
}
}
void dgMemoryAllocator::dgMemoryBeam::Init(dgInt32 size, dgMemoryAllocator* const allocator)
{
m_beamSize = size;
m_allocator = allocator;
}
dgMemoryAllocator::dgMemoryAllocator()
{
// for (dgInt32 i = 0; i < DG_MEMORY_BEAMS_COUNT; i++) {
// dgInt32 size = ((dgInt32 (sizeof (dgMemoryGranularity) * (dgPow(dgFloat32(1.6f), i + 2) - dgPow(dgFloat32(1.6f), i + 1))) + sizeof(dgMemoryGranularity) - 1) & -dgInt32 (sizeof(dgMemoryGranularity))) - sizeof (dgMemoryHeader);
// m_beams[i].Init (size, this);
// }
dgInt32 index = 0;
dgInt32 size0 = 0;
dgFloat32 base = dgFloat32(1.3f);
dgFloat32 exp = dgFloat32 (0.0f);
while (index < DG_MEMORY_BEAMS_COUNT) {
dgFloat32 x0 = dgPow(base, exp);
dgFloat32 x1 = dgPow(base, exp + dgFloat32(1.0f));
dgInt32 size = ((dgInt32(sizeof(dgMemoryGranularity) * (x1 - x0) + sizeof(dgMemoryGranularity) - 1)) & -dgInt32(sizeof(dgMemoryGranularity))) - sizeof(dgMemoryHeader);
exp += dgFloat32(1.0f);
if (size > size0) {
size0 = size;
m_beams[index].Init(size, this);
index++;
}
}
}
dgMemoryAllocator::~dgMemoryAllocator()
{
}
void *dgMemoryAllocator::operator new (size_t size)
{
dgMemoryAllocatorBase& globalAllocator = dgGlobalAllocator::GetGlobalAllocator();
return globalAllocator.Malloc(dgInt32 (size));
}
void dgMemoryAllocator::operator delete (void* const ptr)
{
dgMemoryAllocatorBase& globalAllocator = dgGlobalAllocator::GetGlobalAllocator();
globalAllocator.Free (ptr);
}
void dgMemoryAllocator::SetGlobalAllocators(dgMemAlloc malloc, dgMemFree free)
{
dgGlobalAllocator* const globalAllocator = (dgGlobalAllocator*)&dgGlobalAllocator::GetGlobalAllocator();
globalAllocator->SetAllocatorsCallback(malloc, free);
}
dgInt32 dgMemoryAllocator::GetGlobalMemoryUsed()
{
dgGlobalAllocator* const globalAllocator = (dgGlobalAllocator*)&dgGlobalAllocator::GetGlobalAllocator();
return globalAllocator->GetMemoryUsed();
}
dgInt32 dgMemoryAllocator::GetSize (void* const ptr)
{
dgMemoryHeader* const info = ((dgMemoryHeader*)ptr) - 1;
return info->m_size;
}
void* dgMemoryAllocator::Malloc(dgInt32 size)
{
dgMemoryAllocatorBase& globalAllocator = dgGlobalAllocator::GetGlobalAllocator();
if (size > m_beams[DG_MEMORY_BEAMS_COUNT-1].m_beamSize) {
return globalAllocator.Malloc(size);
} else {
dgMemoryBeam* const beam = FindBeam(size);
return beam->Malloc(size);
}
}
void dgMemoryAllocator::Free(void* const ptr)
{
dgMemoryHeader* const info = ((dgMemoryHeader*)ptr) - 1;
dgMemoryAllocatorBase& globalAllocator = dgGlobalAllocator::GetGlobalAllocator();
if (info->m_size > m_beams[DG_MEMORY_BEAMS_COUNT - 1].m_beamSize) {
globalAllocator.Free (ptr);
} else {
dgMemoryBeam* const beam = FindBeam(info->m_size);
beam->Free(ptr);
}
}
dgMemoryAllocator::dgMemoryBeam* dgMemoryAllocator::FindBeam(dgInt32 size)
{
dgInt32 i = m_beams[DG_MEMORY_BEAMS_COUNT / 2].m_beamSize >= size ? 0 : DG_MEMORY_BEAMS_COUNT / 2;
for (; i < DG_MEMORY_BEAMS_COUNT; i++) {
if (m_beams[i].m_beamSize >= size) {
return &m_beams[i];
}
}
dgAssert(0);
return NULL;
}
#endif

352
thirdparty/src/newton/dgCore/dgMemory.h vendored Normal file
View File

@@ -0,0 +1,352 @@
/* Copyright (c) <2003-2019> <Julio Jerez, Newton Game Dynamics>
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
*
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
*
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
*
* 3. This notice may not be removed or altered from any source distribution.
*/
#ifndef __dgMemory__
#define __dgMemory__
#include "dgStdafx.h"
class dgMemoryAllocator;
#define DG_CLASS_ALLOCATOR_NEW(allocator) DG_INLINE void* operator new (size_t size, dgMemoryAllocator* const allocator) { return dgMalloc(size, allocator);}
#define DG_CLASS_ALLOCATOR_NEW_ARRAY(allocator) DG_INLINE void* operator new[] (size_t size, dgMemoryAllocator* const allocator) { return dgMalloc(size, allocator);}
#define DG_CLASS_ALLOCATOR_DELETE(allocator) DG_INLINE void operator delete (void* const ptr, dgMemoryAllocator* const allocator) { dgFree(ptr); }
#define DG_CLASS_ALLOCATOR_DELETE_ARRAY(allocator) DG_INLINE void operator delete[] (void* const ptr, dgMemoryAllocator* const allocator) { dgFree(ptr); }
#define DG_CLASS_ALLOCATOR_NEW_DUMMY DG_INLINE void* operator new (size_t size) { dgAssert (0); return dgMalloc(size, NULL);}
#define DG_CLASS_ALLOCATOR_NEW_ARRAY_DUMMY DG_INLINE void* operator new[] (size_t size) { dgAssert (0); return dgMalloc(size, NULL);}
#define DG_CLASS_ALLOCATOR_DELETE_DUMMY DG_INLINE void operator delete (void* const ptr) { dgFree(ptr); }
#define DG_CLASS_ALLOCATOR_DELETE_ARRAY_DUMMY DG_INLINE void operator delete[] (void* const ptr) { dgFree(ptr); }
#define DG_CLASS_ALLOCATOR(allocator) \
DG_CLASS_ALLOCATOR_DELETE(allocator) \
DG_CLASS_ALLOCATOR_DELETE_ARRAY(allocator) \
DG_CLASS_ALLOCATOR_NEW(allocator) \
DG_CLASS_ALLOCATOR_NEW_ARRAY(allocator) \
DG_CLASS_ALLOCATOR_NEW_DUMMY \
DG_CLASS_ALLOCATOR_NEW_ARRAY_DUMMY \
DG_CLASS_ALLOCATOR_DELETE_DUMMY \
DG_CLASS_ALLOCATOR_DELETE_ARRAY_DUMMY
typedef void* (dgApi *dgMemAlloc) (dgUnsigned32 size);
typedef void (dgApi *dgMemFree) (void* const ptr, dgUnsigned32 size);
#define DG_OLD_ALLOCATOR
#ifdef DG_OLD_ALLOCATOR
void* dgApi dgMalloc (size_t size, dgMemoryAllocator* const allocator);
void dgApi dgFree (void* const ptr);
void* dgApi dgMallocStack (size_t size);
void* dgApi dgMallocAligned (size_t size, dgInt32 alignmentInBytes);
void dgApi dgFreeStack (void* const ptr);
class dgMemoryAllocator
{
#if (defined (__LP64__) || defined (_WIN_64_VER) || defined (__MINGW64__) || defined (_POSIX_VER_64) || defined (_MACOSX_VER))
#define DG_MEMORY_GRANULARITY_BITS 6
#else
#define DG_MEMORY_GRANULARITY_BITS 5
#endif
#define DG_MEMORY_GRANULARITY (1 << DG_MEMORY_GRANULARITY_BITS)
#define DG_MEMORY_SIZE (1024 - 64)
#define DG_MEMORY_BIN_SIZE (1024 * 16)
#define DG_MEMORY_BIN_ENTRIES (DG_MEMORY_SIZE / DG_MEMORY_GRANULARITY)
public:
class dgMemoryBin;
class dgMemoryInfo;
class dgMemoryCacheEntry;
class dgMemDirectory
{
public:
dgMemoryBin* m_first;
dgMemoryCacheEntry* m_cache;
};
dgMemoryAllocator ();
virtual ~dgMemoryAllocator ();
void *operator new (size_t size);
void operator delete (void* const ptr);
dgInt32 GetMemoryUsed() const;
void SetAllocatorsCallback (dgMemAlloc memAlloc, dgMemFree memFree);
virtual void *MallocLow (dgInt32 size, dgInt32 alignment = DG_MEMORY_GRANULARITY);
virtual void FreeLow (void* const retPtr);
virtual void *Malloc (dgInt32 memsize);
virtual void Free (void* const retPtr);
virtual int GetSize (void* const retPtr);
static dgInt32 GetGlobalMemoryUsed ();
static void SetGlobalAllocators (dgMemAlloc alloc, dgMemFree free);
protected:
dgMemoryAllocator (bool init)
:m_free(NULL)
,m_malloc(NULL)
,m_enumerator(0)
,m_memoryUsed(0)
,m_isInList(0)
{
}
dgMemoryAllocator (dgMemAlloc memAlloc, dgMemFree memFree);
dgMemFree m_free;
dgMemAlloc m_malloc;
dgMemDirectory m_memoryDirectory[DG_MEMORY_BIN_ENTRIES + 1];
dgInt32 m_enumerator;
dgInt32 m_memoryUsed;
dgInt32 m_isInList;
public:
static dgInt32 m_lock0;
static dgInt32 m_lock1;
};
class dgStackMemoryAllocator: public dgMemoryAllocator
{
public:
DG_INLINE dgStackMemoryAllocator(void* const pool, dgInt32 size)
:dgMemoryAllocator (false)
,m_pool((dgInt8*) pool)
,m_index(0)
,m_size(size)
{
}
DG_INLINE ~dgStackMemoryAllocator()
{
}
DG_INLINE void* Alloc(dgInt32 size)
{
dgInt8* const ptr = (dgInt8*) (reinterpret_cast<uintptr_t>(m_pool + m_index + 15) & -0x10);
m_index = dgInt32 (ptr - m_pool) + size;
dgAssert (m_index < m_size);
return ptr;
}
DG_INLINE void* Malloc(dgInt32 size)
{
return Alloc(size);
}
DG_INLINE void* MallocLow(dgInt32 size, dgInt32 alignment)
{
return Alloc(size);
}
DG_INLINE void Free(void* const retPtr)
{
}
DG_INLINE void FreeLow(void* const retPtr)
{
}
dgInt8* m_pool;
dgInt32 m_index;
dgInt32 m_size;
};
#else
class dgMemoryAllocatorBase
{
public:
dgMemoryAllocatorBase() {}
virtual ~dgMemoryAllocatorBase() {}
virtual void* Malloc(dgInt32 size) = 0;
virtual void Free(void* const ptr) = 0;
};
class dgGlobalAllocator: public dgMemoryAllocatorBase
{
public:
dgGlobalAllocator()
:dgMemoryAllocatorBase()
,m_free(__free__)
,m_malloc(__malloc__)
,m_memoryUsed(0)
{
}
~dgGlobalAllocator()
{
}
void SetAllocatorsCallback(dgMemAlloc malloc, dgMemFree free);
static dgMemoryAllocatorBase& GetGlobalAllocator();
dgInt32 GetMemoryUsed() const { return m_memoryUsed; }
private:
static void* dgApi __malloc__(dgUnsigned32 size);
static void dgApi __free__(void* const ptr, dgUnsigned32 size);
void* Malloc(dgInt32 size);
void Free(void* const ptr);
dgMemFree m_free;
dgMemAlloc m_malloc;
dgInt32 m_memoryUsed;
};
class dgMemoryAllocator: public dgMemoryAllocatorBase
{
public:
#define DG_MEMORY_GRANULARITY_BITS 6
#define DG_MEMORY_GRANULARITY (1 << DG_MEMORY_GRANULARITY_BITS)
#define DG_MEMORY_BEAMS_COUNT 16
// #define DG_MEMORY_BEAMS_COUNT 1
#define DG_MEMORY_BEAMS_BUFFER_SIZE (1024 * 32)
class dgMemoryPage;
class dgMemoryHeader
{
public:
dgMemoryAllocatorBase* m_allocator;
union {
void* m_ptr;
dgMemoryPage* m_page;
};
dgInt32 m_size;
dgInt32 m_paddedSize;
};
class dgMemoryGranularity
{
public:
union
{
dgMemoryGranularity* m_next;
char m_padd1[DG_MEMORY_GRANULARITY];
};
};
class dgMemoryPage
{
public:
dgMemoryPage(dgInt32 size, dgMemoryPage* const root, dgMemoryAllocator* const allocator);
~dgMemoryPage();
void *operator new (size_t size);
void operator delete (void* const ptr);
void* Malloc(dgInt32 size);
void Free(void* const ptr);
char m_buffer[DG_MEMORY_BEAMS_BUFFER_SIZE];
dgMemoryPage* m_next;
dgMemoryPage* m_prev;
dgMemoryPage* m_fullPageNext;
dgMemoryPage* m_fullPagePrev;
dgMemoryGranularity* m_freeList;
dgInt32 m_count;
dgInt32 m_capacity;
};
class dgMemoryBeam
{
public:
dgMemoryBeam();
~dgMemoryBeam();
void Init(dgInt32 size, dgMemoryAllocator* const allocator);
void* Malloc(dgInt32 size);
void Free(void* const ptr);
dgMemoryPage* m_firstPage;
dgMemoryPage* m_fullPage;
dgMemoryAllocator* m_allocator;
dgInt32 m_beamSize;
dgInt32 m_inUsedCount;
dgInt32 m_fullPageCount;
};
dgMemoryAllocator();
virtual ~dgMemoryAllocator();
virtual void* Malloc(dgInt32 size);
virtual void Free(void* const ptr);
virtual dgInt32 GetSize (void* const ptr);
void* MallocLow(dgInt32 size, dgInt32 aligment=DG_MEMORY_GRANULARITY)
{
return Malloc(size);
}
void FreeLow(void* const ptr)
{
Free (ptr);
}
static dgInt32 GetGlobalMemoryUsed();
static void SetGlobalAllocators(dgMemAlloc alloc, dgMemFree free);
void *operator new (size_t size);
void operator delete (void* const ptr);
private:
dgMemoryBeam* FindBeam(dgInt32 size);
dgMemoryBeam m_beams[DG_MEMORY_BEAMS_COUNT];
};
DG_INLINE void* dgMalloc(size_t size, dgMemoryAllocatorBase* const allocator)
{
void* const ptr = allocator->Malloc(dgInt32(size));
return ptr;
}
DG_INLINE void dgFree(void* const ptr)
{
dgMemoryAllocator::dgMemoryHeader* const info = ((dgMemoryAllocator::dgMemoryHeader*)ptr) - 1;
dgAssert(info->m_allocator);
info->m_allocator->Free(ptr);
}
DG_INLINE void* dgMallocStack(size_t size)
{
return dgMalloc(size, (dgGlobalAllocator*) &dgGlobalAllocator::GetGlobalAllocator());
}
DG_INLINE void dgFreeStack(void* const ptr)
{
dgFree(ptr);
}
#endif
#endif

124
thirdparty/src/newton/dgCore/dgMutexThread.cpp vendored Normal file
View File

@@ -0,0 +1,124 @@
/* Copyright (c) <2003-2019> <Julio Jerez, Newton Game Dynamics>
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
*
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
*
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
*
* 3. This notice may not be removed or altered from any source distribution.
*/
#include "dgStdafx.h"
#include "dgThread.h"
#include "dgMutexThread.h"
dgMutexThread::dgMutexThread(const char* const name, dgInt32 id)
:dgThread(name, id)
,m_mutex()
,m_parentMutex()
{
Init ();
}
dgMutexThread::~dgMutexThread(void)
{
Terminate();
}
void dgMutexThread::Terminate()
{
if (IsThreadActive()) {
dgInterlockedExchange(&m_terminate, 1);
m_mutex.Release();
Close();
}
}
void dgMutexThread::Execute (dgInt32 threadID)
{
// suspend this tread until the call thread decide to
dgAssert (threadID == m_id);
while (!m_terminate) {
// wait for the main thread to signal an update
m_mutex.Wait();
if (!m_terminate) {
TickCallback(threadID);
}
m_parentMutex.Release();
}
}
void dgMutexThread::Tick()
{
// let the thread run until the update function return
m_mutex.Release();
m_parentMutex.Wait();
}
dgAsyncThread::dgAsyncThread(const char* const name, dgInt32 id)
:dgThread(name, id)
,m_mutex()
,m_inUpdate(0)
,m_beginUpdate(0)
{
Init();
}
dgAsyncThread::~dgAsyncThread(void)
{
Terminate();
}
void dgAsyncThread::Terminate()
{
if (IsThreadActive()) {
dgInterlockedExchange(&m_terminate, 1);
m_mutex.Release();
Close();
}
}
void dgAsyncThread::Sync()
{
while (m_inUpdate) {
dgThreadYield();
}
}
void dgAsyncThread::Tick()
{
// let the thread run until the update function return
Sync();
m_beginUpdate = 0;
m_mutex.Release();
while (!m_beginUpdate) {
dgThreadPause();
}
}
void dgAsyncThread::Execute(dgInt32 threadID)
{
dgAssert(threadID == m_id);
while (!m_terminate) {
m_mutex.Wait();
if (!m_terminate) {
dgInterlockedExchange(&m_inUpdate, 1);
dgInterlockedExchange(&m_beginUpdate, 1);
TickCallback(threadID);
dgInterlockedExchange(&m_inUpdate, 0);
}
}
}

66
thirdparty/src/newton/dgCore/dgMutexThread.h vendored Normal file
View File

@@ -0,0 +1,66 @@
/* Copyright (c) <2003-2019> <Julio Jerez, Newton Game Dynamics>
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
*
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
*
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
*
* 3. This notice may not be removed or altered from any source distribution.
*/
#ifndef __DG_MUTEX_THREAD_H__
#define __DG_MUTEX_THREAD_H__
#include "dgTypes.h"
#include "dgThread.h"
class dgMutexThread: public dgThread
{
public:
dgMutexThread(const char* const name, dgInt32 id);
virtual ~dgMutexThread(void);
void Tick();
void Terminate();
protected:
virtual void Execute (dgInt32 threadID);
virtual void TickCallback (dgInt32 threadID) = 0;
private:
dgSemaphore m_mutex;
dgSemaphore m_parentMutex;
};
class dgAsyncThread: public dgThread
{
public:
dgAsyncThread(const char* const name, dgInt32 id);
virtual ~dgAsyncThread(void);
void Tick();
void Sync();
void Terminate();
protected:
virtual void Execute(dgInt32 threadID);
virtual void TickCallback(dgInt32 threadID) = 0;
private:
dgSemaphore m_mutex;
dgInt32 m_inUpdate;
dgInt32 m_beginUpdate;
};
#endif

231
thirdparty/src/newton/dgCore/dgNode.cpp vendored Normal file
View File

@@ -0,0 +1,231 @@
/* Copyright (c) <2003-2019> <Julio Jerez, Newton Game Dynamics>
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
*
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
*
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
*
* 3. This notice may not be removed or altered from any source distribution.
*/
#include "dgStdafx.h"
#include "dgNode.h"
dgInitRtti(dgBaseNode);
dgBaseNode::dgBaseNode (const dgBaseNode &clone)
:dgRef (clone)
{
Clear ();
for (dgBaseNode* obj = clone.child; obj; obj = obj->sibling) {
dgBaseNode* newObj = (dgBaseNode *)obj->CreateClone ();
newObj->Attach (this);
newObj->Release();
}
}
dgBaseNode::~dgBaseNode ()
{
if (child) {
dgBaseNode* tmp;
for (dgBaseNode* ptr = child; ptr && ptr->Release(); ptr = tmp) {
ptr->Kill();
tmp = ptr->sibling;
ptr->parent = NULL;
ptr->sibling = NULL;
}
}
dgBaseNode* tmp;
for (dgBaseNode* ptr = sibling; ptr && ptr->Release(); ptr = tmp) {
ptr->Kill();
tmp = ptr->sibling;
ptr->parent = NULL;
ptr->sibling = NULL;
}
}
void dgBaseNode::CloneFixUp (const dgBaseNode &clone)
{
dgAssert (GetNameID() == clone.GetNameID());
dgBaseNode* cloneChild = clone.GetChild();
for (dgBaseNode* obj = child; obj; obj = obj->sibling) {
obj->CloneFixUp (*cloneChild);
cloneChild = cloneChild->GetSibling();
}
}
void dgBaseNode::Attach (dgBaseNode *parentArg, bool addFirst)
{
dgAssert (!parent);
dgAssert (!sibling);
dgAssert (parentArg);
parent = parentArg;
if (parent->child) {
if (addFirst) {
sibling = parent->child;
parent->child = this;
} else {
dgBaseNode* obj = parent->child;
for (; obj->sibling; obj = obj->sibling)
{
}
obj->sibling = this;
}
} else {
parent->child = this;
}
AddRef();
}
void dgBaseNode::Detach ()
{
if (parent) {
if (parent->child == this) {
parent->child = sibling;
} else {
dgBaseNode* ptr = parent->child;
for (; ptr->sibling != this; ptr = ptr->sibling)
{
}
ptr->sibling = sibling;
}
parent = NULL;
sibling = NULL;
Release();
}
}
dgBaseNode* dgBaseNode::GetRoot() const
{
const dgBaseNode* root = this;
for (; root->parent; root = root->parent)
{
}
return (dgBaseNode*)root;
}
dgBaseNode* dgBaseNode::GetFirst() const
{
dgBaseNode* ptr = (dgBaseNode *)this;
for (; ptr->child; ptr = ptr->child)
{
}
return ptr;
}
dgBaseNode* dgBaseNode::GetNext() const
{
if (sibling) {
return sibling->GetFirst();
}
dgBaseNode* ptr = parent;
dgBaseNode* x = (dgBaseNode *)this;
for (; ptr && (x == ptr->sibling); ptr = ptr->parent) {
x = ptr;
}
return ptr;
}
dgBaseNode* dgBaseNode::GetLast() const
{
dgBaseNode* ptr = (dgBaseNode *)this;
for (ptr = (dgBaseNode *)this; ptr->sibling; ptr = ptr->sibling)
{
}
return ptr;
}
dgBaseNode* dgBaseNode::GetPrev() const
{
if (child) {
return child->GetNext();
}
dgBaseNode* ptr = parent;
dgBaseNode* x = (dgBaseNode *)this;
for (; ptr && (x == ptr->child); ptr = ptr->child) {
x = ptr;
}
return ptr;
}
dgBaseNode* dgBaseNode::Find (dgUnsigned32 nameCRC) const
{
dgBaseNode *ptr = GetFirst();
for (; ptr; ptr = ptr->GetNext()) {
if (nameCRC == ptr->GetNameID()) {
break;
}
}
return ptr;
}
bool dgBaseNode::SanityCheck()
{
return true;
}
void dgBaseNode::PrintHierarchy (
dgFile &file,
char *indent) const
{
char newIndent[1024];
sprintf (newIndent, "%s ", indent);
for (dgBaseNode *node = child; node; node = node->sibling) {
node->PrintHierarchy (file, newIndent);
}
}
void dgBaseNode::DebugPrint (const char *fileName)
{
/*
char indent[512];
indent[0] = '\0';
dgFile file (fileName, "w");
PrintHierarchy (file, indent);
*/
}

262
thirdparty/src/newton/dgCore/dgNode.h vendored Normal file
View File

@@ -0,0 +1,262 @@
/* Copyright (c) <2003-2019> <Julio Jerez, Newton Game Dynamics>
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
*
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
*
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
*
* 3. This notice may not be removed or altered from any source distribution.
*/
#ifndef __dgNode__
#define __dgNode__
#include "dgStdafx.h"
#include "dgCRC.h"
#include "dgRef.h"
#include "dgRtti.h"
class dgFile;
//enum dgSaveType;
class dgBaseNode: public dgRef
{
public:
dgBaseNode *GetChild () const;
dgBaseNode *GetParent () const;
dgBaseNode *GetSibling () const;
void Detach ();
void Attach (dgBaseNode* const parent, bool addFirst = false);
dgBaseNode *GetRoot () const;
dgBaseNode *GetFirst() const;
dgBaseNode *GetLast() const;
dgBaseNode *GetNext() const;
dgBaseNode *GetPrev() const;
dgBaseNode *Find (dgUnsigned32 nameCRC) const;
dgBaseNode *Find (const char* const name) const;
void DebugPrint (const char* const fileName);
bool SanityCheck();
protected:
dgBaseNode ();
dgBaseNode (const char* const name);
dgBaseNode (const dgBaseNode &clone);
~dgBaseNode ();
// virtual void Save (dgFile &file, dgSaveType saveType, void* const context) const;
virtual void CloneFixUp (const dgBaseNode &clone);
virtual void PrintHierarchy (dgFile &file, char* indentation) const;
private:
inline void Clear();
dgAddRtti(dgRef);
dgBaseNode* parent;
dgBaseNode* child;
dgBaseNode* sibling;
};
template<class T>
class dgNode: public dgBaseNode
{
public:
dgNode ();
dgNode (const char* const name);
void Attach (T* parent, bool addFirst = false);
void Detach ();
T* GetChild () const;
T* GetSibling () const;
T* GetParent () const;
T* GetRoot () const;
T* GetFirst() const;
T* GetLast() const;
T* GetNext() const;
T* GetPrev() const;
T* Find (dgUnsigned32 nameCRC) const;
T* Find (const char* const name) const;
protected:
dgNode (const T &clone);
virtual ~dgNode ();
dgRef *CreateClone () const;
};
inline dgBaseNode::dgBaseNode ()
:dgRef ()
{
Clear ();
}
inline dgBaseNode::dgBaseNode (const char* const name)
:dgRef (name)
{
Clear ();
}
inline void dgBaseNode::Clear()
{
child = NULL;
parent = NULL;
sibling = NULL;
}
inline dgBaseNode *dgBaseNode::GetChild () const
{
return child;
}
inline dgBaseNode *dgBaseNode::GetSibling () const
{
return sibling;
}
inline dgBaseNode *dgBaseNode::GetParent () const
{
return parent;
}
inline dgBaseNode *dgBaseNode::Find (const char* const name) const
{
return Find (dgCRC (name));
}
template<class T>
dgNode<T>::dgNode ()
:dgBaseNode ()
{
}
template<class T>
dgNode<T>::dgNode (const T &clone)
:dgBaseNode (clone)
{
}
template<class T>
dgNode<T>::dgNode (const char* const name)
:dgBaseNode (name)
{
}
template<class T>
dgNode<T>::~dgNode ()
{
}
template<class T>
dgRef *dgNode<T>::CreateClone () const
{
return new T (*(T*)this);
}
template<class T>
void dgNode<T>::Attach (T* const parent, bool addFirst)
{
dgBaseNode::Attach(parent, addFirst);
}
template<class T>
void dgNode<T>::Detach ()
{
dgBaseNode::Detach ();
}
template<class T>
T* dgNode<T>::GetChild () const
{
return (T*) dgBaseNode::GetChild();
}
template<class T>
T* dgNode<T>::GetSibling () const
{
return (T*) dgBaseNode::GetSibling ();
}
template<class T>
T* dgNode<T>::GetParent () const
{
return (T*) dgBaseNode::GetParent ();
}
template<class T>
T* dgNode<T>::GetRoot () const
{
return (T*) dgBaseNode::GetRoot ();
}
template<class T>
T* dgNode<T>::GetFirst() const
{
return (T*) dgBaseNode::GetFirst ();
}
template<class T>
T* dgNode<T>::GetLast() const
{
return (T*) dgBaseNode::GetLast ();
}
template<class T>
T* dgNode<T>::GetNext() const
{
return (T*) dgBaseNode::GetNext ();
}
template<class T>
T* dgNode<T>::GetPrev() const
{
return (T*) dgBaseNode::GetPrev ();
}
template<class T>
T* dgNode<T>::Find (dgUnsigned32 nameCRC) const
{
return (T*) dgBaseNode::Find (nameCRC);
}
template<class T>
T* dgNode<T>::Find (const char* const name) const
{
return (T*) dgBaseNode::Find (name);
}
#endif

868
thirdparty/src/newton/dgCore/dgObb.cpp vendored Normal file
View File

@@ -0,0 +1,868 @@
/* Copyright (c) <2003-2019> <Julio Jerez, Newton Game Dynamics>
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
*
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
*
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
*
* 3. This notice may not be removed or altered from any source distribution.
*/
#include "dgStdafx.h"
#include "dgObb.h"
#include "dgDebug.h"
#include "dgPlane.h"
#include "dgMatrix.h"
namespace InternalSphere
{
const dgFloat32 SPHERE_TOL = 0.002f;
static dgFloat32 AspectRatio (dgFloat32 x, dgFloat32 y)
{
dgFloat32 tmp;
x = dgAbs (x);
y = dgAbs (y);
if (y < x) {
tmp = y;
y = x;
x = tmp;
}
if (y < 1.0e-12) {
y = 1.0e-12f;
}
return x / y;
}
static void BoundingBox (const dgMatrix& matrix, const dgFloat32 vertex[], dgInt32 stride, const dgInt32 index[], dgInt32 indexCount, dgVector &min, dgVector &max)
{
dgFloat32 xmin = dgFloat32 (1.0e10f);
dgFloat32 ymin = dgFloat32 (1.0e10f);
dgFloat32 zmin = dgFloat32 (1.0e10f);
dgFloat32 xmax = dgFloat32 (-1.0e10f);
dgFloat32 ymax = dgFloat32 (-1.0e10f);
dgFloat32 zmax = dgFloat32 (-1.0e10f);
const dgFloat32* const ptr = vertex;
for (dgInt32 j = 0 ; j < indexCount; j ++ ) {
dgInt32 i = index[j] * stride;
dgVector tmp (ptr[i + 0], ptr[i + 1], ptr[i + 2], dgFloat32 (0.0f));
tmp = matrix.UnrotateVector (tmp);
if (tmp.m_x < xmin) xmin = tmp.m_x;
if (tmp.m_y < ymin) ymin = tmp.m_y;
if (tmp.m_z < zmin) zmin = tmp.m_z;
if (tmp.m_x > xmax) xmax = tmp.m_x;
if (tmp.m_y > ymax) ymax = tmp.m_y;
if (tmp.m_z > zmax) zmax = tmp.m_z;
}
min = dgVector (xmin, ymin, zmin, dgFloat32 (0.0f));
max = dgVector (xmax, ymax, zmax, dgFloat32 (0.0f));
}
// Compute axis aligned box
static void BoundingBox (const dgMatrix &Mat, const dgFloat32 vertex[], dgInt32 vertexCount, dgInt32 stride, dgVector &min, dgVector &max)
{
dgFloat32 xmin = dgFloat32 (1.0e10f);
dgFloat32 ymin = dgFloat32 (1.0e10f);
dgFloat32 zmin = dgFloat32 (1.0e10f);
dgFloat32 xmax = dgFloat32 (-1.0e10f);
dgFloat32 ymax = dgFloat32 (-1.0e10f);
dgFloat32 zmax = dgFloat32 (-1.0e10f);
const dgFloat32* ptr = vertex;
for (dgInt32 i = 0 ; i < vertexCount; i ++ ) {
dgVector tmp (ptr[0], ptr[1], ptr[2], dgFloat32 (0.0f));
ptr += stride;
tmp = Mat.UnrotateVector (tmp);
if (tmp.m_x < xmin) xmin = tmp.m_x;
if (tmp.m_y < ymin) ymin = tmp.m_y;
if (tmp.m_z < zmin) zmin = tmp.m_z;
if (tmp.m_x > xmax) xmax = tmp.m_x;
if (tmp.m_y > ymax) ymax = tmp.m_y;
if (tmp.m_z > zmax) zmax = tmp.m_z;
}
min = dgVector (xmin, ymin, zmin, dgFloat32 (0.0f));
max = dgVector (xmax, ymax, zmax, dgFloat32 (0.0f));
}
static void Statistics (dgObb &sphere, dgVector &eigenValues, const dgVector &scale, const dgFloat32 vertex[], const dgInt32 faceIndex[], dgInt32 indexCount, dgInt32 stride)
{
dgVector var (dgFloat32 (0.0f));
dgVector cov (dgFloat32 (0.0f));
dgVector centre (dgFloat32 (0.0f));
dgVector massCenter (dgFloat32 (0.0f));
dgVector scaleVector (scale & dgVector::m_triplexMask);
dgFloat64 totalArea = dgFloat32 (0.0f);
const dgFloat32* const ptr = vertex;
for (dgInt32 i = 0; i < indexCount; i += 3) {
dgInt32 index = faceIndex[i] * stride;
dgVector p0 (&ptr[index]);
p0 = p0 & dgVector::m_triplexMask;
p0 = p0 * scaleVector;
index = faceIndex[i + 1] * stride;;
dgVector p1 (&ptr[index]);
p1 = p1 & dgVector::m_triplexMask;
p1 = p1 * scaleVector;
index = faceIndex[i + 2] * stride;;
dgVector p2 (&ptr[index]);
p2 = p2 & dgVector::m_triplexMask;
p2 = p2 * scaleVector;
dgVector normal ((p1 - p0).CrossProduct(p2 - p0));
dgAssert(normal.m_w == dgFloat32(0.0f));
dgFloat64 area = dgFloat32 (0.5f) * sqrt (normal.DotProduct(normal).GetScalar());
centre = p0 + p1 + p2;
centre = centre.Scale (dgFloat32 (1.0f / 3.0f));
// Inertia of each point in the triangle
dgFloat64 Ixx = p0.m_x * p0.m_x + p1.m_x * p1.m_x + p2.m_x * p2.m_x;
dgFloat64 Iyy = p0.m_y * p0.m_y + p1.m_y * p1.m_y + p2.m_y * p2.m_y;
dgFloat64 Izz = p0.m_z * p0.m_z + p1.m_z * p1.m_z + p2.m_z * p2.m_z;
dgFloat64 Ixy = p0.m_x * p0.m_y + p1.m_x * p1.m_y + p2.m_x * p2.m_y;
dgFloat64 Iyz = p0.m_y * p0.m_z + p1.m_y * p1.m_z + p2.m_y * p2.m_z;
dgFloat64 Ixz = p0.m_x * p0.m_z + p1.m_x * p1.m_z + p2.m_x * p2.m_z;
if (area > dgEpsilon * 10.0) {
dgFloat64 K = area / dgFloat64 (12.0);
//Coriolis theorem for Inertia of a triangle in an arbitrary orientation
Ixx = K * (Ixx + 9.0 * centre.m_x * centre.m_x);
Iyy = K * (Iyy + 9.0 * centre.m_y * centre.m_y);
Izz = K * (Izz + 9.0 * centre.m_z * centre.m_z);
Ixy = K * (Ixy + 9.0 * centre.m_x * centre.m_y);
Ixz = K * (Ixz + 9.0 * centre.m_x * centre.m_z);
Iyz = K * (Iyz + 9.0 * centre.m_y * centre.m_z);
centre = centre.Scale ((dgFloat32)area);
}
totalArea += area;
massCenter += centre;
var += dgVector ((dgFloat32)Ixx, (dgFloat32)Iyy, (dgFloat32)Izz, dgFloat32 (0.0f));
cov += dgVector ((dgFloat32)Ixy, (dgFloat32)Ixz, (dgFloat32)Iyz, dgFloat32 (0.0f));
}
if (totalArea > dgEpsilon * 10.0) {
dgFloat64 K = dgFloat64 (1.0) / totalArea;
var = var.Scale ((dgFloat32)K);
cov = cov.Scale ((dgFloat32)K);
massCenter = massCenter.Scale ((dgFloat32)K);
}
dgFloat64 Ixx = var.m_x - massCenter.m_x * massCenter.m_x;
dgFloat64 Iyy = var.m_y - massCenter.m_y * massCenter.m_y;
dgFloat64 Izz = var.m_z - massCenter.m_z * massCenter.m_z;
dgFloat64 Ixy = cov.m_x - massCenter.m_x * massCenter.m_y;
dgFloat64 Ixz = cov.m_y - massCenter.m_x * massCenter.m_z;
dgFloat64 Iyz = cov.m_z - massCenter.m_y * massCenter.m_z;
sphere.m_front = dgVector ((dgFloat32)Ixx, (dgFloat32)Ixy, (dgFloat32)Ixz, dgFloat32 (0.0f));
sphere.m_up = dgVector ((dgFloat32)Ixy, (dgFloat32)Iyy, (dgFloat32)Iyz, dgFloat32 (0.0f));
sphere.m_right = dgVector ((dgFloat32)Ixz, (dgFloat32)Iyz, (dgFloat32)Izz, dgFloat32 (0.0f));
eigenValues = sphere.EigenVectors();
}
static void Statistics (dgObb& sphere, dgVector &eigenValues, dgVector &scaleVector, const dgFloat32 vertex[], dgInt32 vertexCount, dgInt32 stride)
{
dgBigVector var (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
dgBigVector cov (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
dgBigVector massCenter (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
const dgFloat32* ptr = vertex;
for (dgInt32 i = 0; i < vertexCount; i ++) {
dgFloat32 x = ptr[0] * scaleVector.m_x;
dgFloat32 y = ptr[1] * scaleVector.m_y;
dgFloat32 z = ptr[2] * scaleVector.m_z;
ptr += stride;
massCenter += dgBigVector (x, y, z, dgFloat32 (0.0f));
var += dgBigVector (x * x, y * y, z * z, dgFloat32 (0.0f));
cov += dgBigVector (x * y, x * z, y * z, dgFloat32 (0.0f));
}
dgFloat64 k = dgFloat64 (1.0) / vertexCount;
var = var.Scale (k);
cov = cov.Scale (k);
massCenter = massCenter.Scale (k);
dgFloat64 Ixx = var.m_x - massCenter.m_x * massCenter.m_x;
dgFloat64 Iyy = var.m_y - massCenter.m_y * massCenter.m_y;
dgFloat64 Izz = var.m_z - massCenter.m_z * massCenter.m_z;
dgFloat64 Ixy = cov.m_x - massCenter.m_x * massCenter.m_y;
dgFloat64 Ixz = cov.m_y - massCenter.m_x * massCenter.m_z;
dgFloat64 Iyz = cov.m_z - massCenter.m_y * massCenter.m_z;
sphere.m_front = dgVector (dgFloat32(Ixx), dgFloat32(Ixy), dgFloat32(Ixz), dgFloat32 (0.0f));
sphere.m_up = dgVector (dgFloat32(Ixy), dgFloat32(Iyy), dgFloat32(Iyz), dgFloat32 (0.0f));
sphere.m_right = dgVector (dgFloat32(Ixz), dgFloat32(Iyz), dgFloat32(Izz), dgFloat32 (0.0f));
eigenValues = sphere.EigenVectors ();
}
/*
static void Statistics (
dgObb &sphere,
dgVector &eigenValues,
const dgVector &scaleVector,
const dgFloat32 vertex[],
dgInt32 stride,
const dgFace face[],
dgInt32 faceCount)
{
dgAssert (0);
dgInt32 i;
dgInt32 index;
const dgFloat32 *ptr;
dgFloat64 K;
dgFloat64 Ixx;
dgFloat64 Iyy;
dgFloat64 Izz;
dgFloat64 Ixy;
dgFloat64 Ixz;
dgFloat64 Iyz;
dgFloat64 area;
dgFloat64 totalArea;
const dgFace *Face;
dgVector var (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
dgVector cov (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
dgVector centre (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
dgVector massCenter (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
totalArea = 0.0;
ptr = vertex;
for (i = 0; i < faceCount; i ++) {
Face = &face[i];
index = Face->m_wireFrame[0] * stride;
dgVector p0 (&ptr[index]);
p0 = p0 * scaleVector;
index = Face->m_wireFrame[1] * stride;
dgVector p1 (&ptr[index]);
p1 = p1 * scaleVector;
index = Face->m_wireFrame[2] * stride;
dgVector p2 (&ptr[index]);
p2 = p2 * scaleVector;
dgVector normal ((p1 - p0) * (p2 - p0));
area = 0.5 * sqrt (normal % normal);
centre = p0 + p1 + p2;
centre = centre.Scale (1.0f / 3.0f);
// Inercia of each point in the triangle
Ixx = p0.m_x * p0.m_x + p1.m_x * p1.m_x + p2.m_x * p2.m_x;
Iyy = p0.m_y * p0.m_y + p1.m_y * p1.m_y + p2.m_y * p2.m_y;
Izz = p0.m_z * p0.m_z + p1.m_z * p1.m_z + p2.m_z * p2.m_z;
Ixy = p0.m_x * p0.m_y + p1.m_x * p1.m_y + p2.m_x * p2.m_y;
Iyz = p0.m_y * p0.m_z + p1.m_y * p1.m_z + p2.m_y * p2.m_z;
Ixz = p0.m_x * p0.m_z + p1.m_x * p1.m_z + p2.m_x * p2.m_z;
if (area > dgEPSILON * 10.0) {
K = area / 12.0;
//Coriolis theorem for Inertia of a triangle in an arbitrary orientation
Ixx = K * (Ixx + 9.0 * centre.m_x * centre.m_x);
Iyy = K * (Iyy + 9.0 * centre.m_y * centre.m_y);
Izz = K * (Izz + 9.0 * centre.m_z * centre.m_z);
Ixy = K * (Ixy + 9.0 * centre.m_x * centre.m_y);
Ixz = K * (Ixz + 9.0 * centre.m_x * centre.m_z);
Iyz = K * (Iyz + 9.0 * centre.m_y * centre.m_z);
centre = centre.Scale ((dgFloat32)area);
}
totalArea += area;
massCenter += centre;
var += dgVector ((dgFloat32)Ixx, (dgFloat32)Iyy, (dgFloat32)Izz);
cov += dgVector ((dgFloat32)Ixy, (dgFloat32)Ixz, (dgFloat32)Iyz);
}
if (totalArea > dgEPSILON * 10.0) {
K = 1.0 / totalArea;
var = var.Scale ((dgFloat32)K);
cov = cov.Scale ((dgFloat32)K);
massCenter = massCenter.Scale ((dgFloat32)K);
}
Ixx = var.m_x - massCenter.m_x * massCenter.m_x;
Iyy = var.m_y - massCenter.m_y * massCenter.m_y;
Izz = var.m_z - massCenter.m_z * massCenter.m_z;
Ixy = cov.m_x - massCenter.m_x * massCenter.m_y;
Ixz = cov.m_y - massCenter.m_x * massCenter.m_z;
Iyz = cov.m_z - massCenter.m_y * massCenter.m_z;
sphere.m_front = dgVector ((dgFloat32)Ixx, (dgFloat32)Ixy, (dgFloat32)Ixz);
sphere.m_up = dgVector ((dgFloat32)Ixy, (dgFloat32)Iyy, (dgFloat32)Iyz);
sphere.m_right = dgVector ((dgFloat32)Ixz, (dgFloat32)Iyz, (dgFloat32)Izz);
sphere.EigenVectors(eigenValues);
}
*/
}
void dgObb::SetDimensions (const dgFloat32 vertex[], dgInt32 strideInBytes, const dgInt32 triangles[], dgInt32 indexCount, const dgMatrix *basis)
{
dgVector eigen;
dgVector scaleVector (dgFloat32 (1.0f), dgFloat32 (1.0f), dgFloat32 (1.0f), dgFloat32 (0.0f));
if (indexCount < 3) {
return;
}
dgInt32 stride = dgInt32 (strideInBytes / sizeof (dgFloat32));
if (!basis) {
InternalSphere::Statistics (*this, eigen, scaleVector, vertex, triangles, indexCount, stride);
dgInt32 k = 0;
for (dgInt32 i = 0; i < 3; i ++) {
if (k >= 6) {
break;
}
for (dgInt32 j = i + 1; j < 3; j ++) {
dgFloat32 aspect = InternalSphere::AspectRatio (eigen[i], eigen[j]);
if (aspect > dgFloat32 (0.9f)) {
scaleVector[i] *= dgFloat32 (2.0f);
InternalSphere::Statistics (*this, eigen, scaleVector, vertex, triangles, indexCount, stride);
k ++;
i = -1;
break;
}
}
}
} else {
*this = *basis;
}
dgVector min;
dgVector max;
InternalSphere::BoundingBox (*this, vertex, stride, triangles, indexCount, min, max);
dgVector massCenter ((max + min) * dgVector::m_half);
//massCenter = massCenter.Scale (dgFloat32 (0.5f));
m_posit = TransformVector (massCenter);
dgVector dim ((max - min) * dgVector::m_half);
//dim = dim.Scale (dgFloat32(0.5f));
SetDimensions (dim.m_x, dim.m_y, dim.m_z);
}
void dgObb::SetDimensions (const dgFloat32 vertex[], dgInt32 strideInBytes, dgInt32 count, const dgMatrix *basis)
{
dgVector eigen;
dgVector scaleVector (dgFloat32(1.0f), dgFloat32(1.0f), dgFloat32(1.0f), dgFloat32 (0.0f));
dgInt32 stride = dgInt32 (strideInBytes / sizeof (dgFloat32));
if (!basis) {
InternalSphere::Statistics (*this, eigen, scaleVector, vertex, count, stride);
dgInt32 k = 0;
for (dgInt32 i = 0; i < 3; i ++) {
if (k >= 6) {
break;
}
for (dgInt32 j = i + 1; j < 3; j ++) {
dgFloat32 aspect = InternalSphere::AspectRatio (eigen[i], eigen[j]);
if (aspect > dgFloat32 (0.9f)) {
scaleVector[i] *= dgFloat32 (2.0f);
InternalSphere::Statistics (*this, eigen, scaleVector, vertex, count, stride);
k ++;
i = -1;
break;
}
}
}
} else {
*this = *basis;
}
dgVector min;
dgVector max;
InternalSphere::BoundingBox (*this, vertex, count, stride, min, max);
dgVector massCenter ((max + min) * dgVector::m_half);
//massCenter = massCenter.Scale (0.5);
m_posit = TransformVector (massCenter);
dgVector dim ((max - min) * dgVector::m_half);
//dim = dim.Scale (dgFloat32(0.5f));
SetDimensions (dim.m_x + InternalSphere::SPHERE_TOL,
dim.m_y + InternalSphere::SPHERE_TOL,
dim.m_z + InternalSphere::SPHERE_TOL);
}
/*
void dgObb::SetDimensions (
const dgFloat32 vertex[],
dgInt32 strideInBytes,
const dgInt32 index[],
dgInt32 indexCount,
const dgMatrix *basis)
{
dgInt32 i;
dgInt32 j;
dgInt32 k;
dgInt32 stride;
dgFloat32 aspect;
dgVector eigen;
dgVector scaleVector (dgFloat32(1.0f), dgFloat32(1.0f), dgFloat32(1.0f), dgFloat32 (0.0f));
stride = strideInBytes / sizeof (dgFloat32);
if (!basis) {
InternalSphere::Statistics (*this, eigen, scaleVector, vertex, index, indexCount, stride);
k = 0;
for (i = 0; i < 3; i ++) {
if (k >= 6) {
break;
}
for (j = i + 1; j < 3; j ++) {
aspect = InternalSphere::AspectRatio (eigen[i], eigen[j]);
if (aspect > dgFloat32 (0.9f)) {
scaleVector[i] *= dgFloat32 (2.0f);
InternalSphere::Statistics (*this, eigen, scaleVector, vertex, index, indexCount, stride);
i = -1;
k ++;
break;
}
}
}
} else {
*this = *basis;
}
dgVector min;
dgVector max;
InternalSphere::BoundingBox (*this, vertex, stride, index, indexCount, min, max);
dgVector massCenter (max + min);
massCenter = massCenter.Scale (dgFloat32(0.5f));
m_posit = TransformVector (massCenter);
dgVector dim (max - min);
dim = dim.Scale (dgFloat32(0.5f));
SetDimensions (dim.m_x + InternalSphere::SPHERE_TOL,
dim.m_y + InternalSphere::SPHERE_TOL,
dim.m_z + InternalSphere::SPHERE_TOL);
}
*/
/*
dgObb::dgObb (
const dgObb &dgObb,
const dgVector &Dir)
{
if ((Dir % Dir) < EPSILON * 0.01f) {
*this = dgObb;
return;
}
front = Dir;
front.Fast_Normalize();
if (dgAbs (front % dgObb.right) < 0.995) {
up = front * dgObb.right;
up.Fast_Normalize();
} else {
up = dgObb.up;
}
right = up * front;
dgVector Step (Dir.Scale(0.5));
size.m_x = (dgFloat32)(dgObb.size.m_x * dgAbs (right % dgObb.right) +
dgObb.size.m_y * dgAbs (right % dgObb.up) +
dgObb.size.m_z * dgAbs (right % dgObb.front));
size.m_y = (dgFloat32)(dgObb.size.m_x * dgAbs (up % dgObb.right) +
dgObb.size.m_y * dgAbs (up % dgObb.up) +
dgObb.size.m_z * dgAbs (up % dgObb.front));
size.m_z = (dgFloat32)(sqrt (Step % Step) +
dgObb.size.m_x * dgAbs (front % dgObb.right) +
dgObb.size.m_y * dgAbs (front % dgObb.up) +
dgObb.size.m_z * dgAbs (front % dgObb.front));
posit = dgObb.posit + Step;
}
bool dgObb::dgObb_Overlap_Test (const dgObb &dgObb)
{
dgFloat64 R;
dgVector Dir (dgObb.posit - posit);
R = size.m_x * dgAbs (right % Dir) + dgObb.size.m_x * dgAbs (dgObb.right % Dir) +
size.m_y * dgAbs (up % Dir) + dgObb.size.m_y * dgAbs (dgObb.up % Dir) +
size.m_z * dgAbs (front %Dir) + dgObb.size.m_z * dgAbs (dgObb.front % Dir);
if (R < (Dir % Dir)) {
return false;
}
R = size.m_x * dgAbs (right % dgObb.right) +
size.m_y * dgAbs (up % dgObb.right) +
size.m_z * dgAbs (front % dgObb.right) + dgObb.size.m_x;
if (R < dgAbs (Dir % dgObb.right)) {
return false;
}
R = size.m_x * dgAbs (right % dgObb.up) +
size.m_y * dgAbs (up % dgObb.up) +
size.m_z * dgAbs (front % dgObb.up) + dgObb.size.m_y;
if (R < dgAbs (Dir % dgObb.up)) {
return false;
}
R = size.m_x * dgAbs (right % dgObb.front) +
size.m_y * dgAbs (up % dgObb.front) +
size.m_z * dgAbs (front % dgObb.front) + dgObb.size.m_z;
if (R < dgAbs (Dir % dgObb.front)) {
return false;
}
R = dgObb.size.m_x * dgAbs (dgObb.right % right) +
dgObb.size.m_y * dgAbs (dgObb.up % right) +
dgObb.size.m_z * dgAbs (dgObb.front % right) + size.m_x;
if (R < dgAbs (Dir % right)) {
return false;
}
R = dgObb.size.m_x * dgAbs (dgObb.right % up) +
dgObb.size.m_y * dgAbs (dgObb.up % up) +
dgObb.size.m_z * dgAbs (dgObb.front % up) + size.m_y;
if (R < dgAbs (Dir % up)) {
return false;
}
R = dgObb.size.m_x * dgAbs (dgObb.right % front) +
dgObb.size.m_y * dgAbs (dgObb.up % front) +
dgObb.size.m_z * dgAbs (dgObb.front % front) + size.m_z;
if (R < dgAbs (Dir % front)) {
return false;
}
return true;
}
void dgObb::Swept_Volume (
dgVector &min,
dgVector &max)
{
dgFloat32 w;
dgFloat32 h;
dgFloat32 b;
w = (dgFloat32)(size.m_x * dgAbs(right.m_x) + size.m_y * dgAbs(up.m_x) + size.m_z * dgAbs(front.m_x));
h = (dgFloat32)(size.m_x * dgAbs(right.m_y) + size.m_y * dgAbs(up.m_y) + size.m_z * dgAbs(front.m_y));
b = (dgFloat32)(size.m_x * dgAbs(right.m_z) + size.m_y * dgAbs(up.m_z) + size.m_z * dgAbs(front.m_z));
min.m_x = posit.m_x - w;
min.m_y = posit.m_y - h;
min.m_z = posit.m_z - b;
max.m_x = posit.m_x + w;
max.m_y = posit.m_y + h;
max.m_z = posit.m_z + b;
}
*/
/*
dgInt32 dgObb::FrontTest (
const dgMatrix& matrix,
const dgPlane* plane) const
{
dgFloat32 R;
dgFloat32 dR;
InternalSphere::dgFloatSign flag0;
InternalSphere::dgFloatSign flag1;
dR = m_size.m_x * dgAbs (matrix.m_front.m_x) + m_size.m_y * dgAbs (matrix.m_up.m_x) + m_size.m_z * dgAbs (matrix.m_right.m_x);
R = plane[5].m_x * matrix.m_posit.m_x + plane[5].m_w;
flag0.f = R + dR;
flag1.f = R - dR;
flag0.i = flag0.i >> 30 & 2;
flag1.i = flag1.i >> 31 & 1;
return InternalSphere::CodeTbl[flag0.i | flag1.i];
}
dgInt32 dgObb::RearTest (const dgMatrix& matrix, const dgPlane* plane) const
{
dgFloat32 R;
dgFloat32 dR;
InternalSphere::dgFloatSign flag0;
InternalSphere::dgFloatSign flag1;
dR = m_size.m_x * dgAbs (matrix.m_front.m_x) + m_size.m_y * dgAbs (matrix.m_up.m_x) + m_size.m_z * dgAbs (matrix.m_right.m_x);
R = plane[4].m_x * matrix.m_posit.m_x + plane[4].m_w;
flag0.f = R + dR;
flag1.f = R - dR;
flag0.i = flag0.i >> 30 & 2;
flag1.i = flag1.i >> 31 & 1;
return InternalSphere::CodeTbl[flag0.i | flag1.i];
}
dgInt32 dgObb::LeftTest (const dgMatrix& matrix, const dgPlane* plane) const
{
dgFloat32 R;
dgFloat32 dR;
InternalSphere::dgFloatSign flag0;
InternalSphere::dgFloatSign flag1;
dR = m_size.m_x * dgAbs (matrix.m_front.m_x * plane[0].m_x + matrix.m_front.m_z * plane[0].m_z) +
m_size.m_y * dgAbs (matrix.m_up.m_x * plane[0].m_x + matrix.m_up.m_z * plane[0].m_z) +
m_size.m_z * dgAbs (matrix.m_right.m_x * plane[0].m_x + matrix.m_right.m_z * plane[0].m_z);
R = plane[0].m_x * matrix.m_posit.m_x + plane[0].m_z * matrix.m_posit.m_z;
flag0.f = R + dR;
flag1.f = R - dR;
flag0.i = (flag0.i >> 30) & 2;
flag1.i = (flag1.i >> 31) & 1;
return InternalSphere::CodeTbl[flag0.i | flag1.i];
}
dgInt32 dgObb::RightTest (const dgMatrix& matrix, const dgPlane* plane) const
{
dgFloat32 R;
dgFloat32 dR;
InternalSphere::dgFloatSign flag0;
InternalSphere::dgFloatSign flag1;
dR = m_size.m_x * dgAbs (matrix.m_front.m_x * plane[1].m_x + matrix.m_front.m_z * plane[1].m_z) +
m_size.m_y * dgAbs (matrix.m_up.m_x * plane[1].m_x + matrix.m_up.m_z * plane[1].m_z) +
m_size.m_z * dgAbs (matrix.m_right.m_x * plane[1].m_x + matrix.m_right.m_z * plane[1].m_z);
R = plane[1].m_x * matrix.m_posit.m_x + plane[1].m_z * matrix.m_posit.m_z;
flag0.f = R + dR;
flag1.f = R - dR;
flag0.i = (flag0.i >> 30) & 2;
flag1.i = (flag1.i >> 31) & 1;
return InternalSphere::CodeTbl[flag0.i | flag1.i];
}
dgInt32 dgObb::BottomTest (const dgMatrix& matrix, const dgPlane* plane) const
{
dgFloat32 R;
dgFloat32 dR;
InternalSphere::dgFloatSign flag0;
InternalSphere::dgFloatSign flag1;
dR = m_size.m_x * dgAbs (matrix.m_front.m_x * plane[2].m_x + matrix.m_front.m_y * plane[2].m_y) +
m_size.m_y * dgAbs (matrix.m_up.m_x * plane[2].m_x + matrix.m_up.m_y * plane[2].m_y) +
m_size.m_z * dgAbs (matrix.m_right.m_x * plane[2].m_x + matrix.m_right.m_y * plane[2].m_y);
R = plane[2].m_x * matrix.m_posit.m_x + plane[2].m_y * matrix.m_posit.m_y;
flag0.f = R + dR;
flag1.f = R - dR;
flag0.i = (flag0.i >> 30) & 2;
flag1.i = (flag1.i >> 31) & 1;
return InternalSphere::CodeTbl[flag0.i | flag1.i];
}
dgInt32 dgObb::TopTest (const dgMatrix& matrix, const dgPlane* plane) const
{
dgFloat32 R;
dgFloat32 dR;
InternalSphere::dgFloatSign flag0;
InternalSphere::dgFloatSign flag1;
dR = m_size.m_x * dgAbs (matrix.m_front.m_x * plane[3].m_x + matrix.m_front.m_y * plane[3].m_y) +
m_size.m_y * dgAbs (matrix.m_up.m_x * plane[3].m_x + matrix.m_up.m_y * plane[3].m_y) +
m_size.m_z * dgAbs (matrix.m_right.m_x * plane[3].m_x + matrix.m_right.m_y * plane[3].m_y);
R = plane[3].m_x * matrix.m_posit.m_x + plane[3].m_y * matrix.m_posit.m_y;
flag0.f = R + dR;
flag1.f = R - dR;
flag0.i = (flag0.i >> 30) & 2;
flag1.i = (flag1.i >> 31) & 1;
return InternalSphere::CodeTbl[flag0.i | flag1.i];
}
dgInt32 dgObb::VisibilityTestLow (
const dgCamera* camera,
const dgMatrix& matrix) const
{
dgInt32 i;
dgInt32 code;
const dgPlane* planes;
const dgPlane* guardPlanes;
planes = camera->GetViewVolume();
code = (this->*planeTest) (matrix, planes);
if (code != -1) {
for (i = 0; i < 6; i ++) {
code |= (this->*planeTestArray[i]) (matrix, planes);
if (code == -1) {
planeTest = planeTestArray[i];
return -1;
}
}
if (code) {
guardPlanes = camera->GetGuardViewVolume();
if (guardPlanes) {
code = 0;
for (i = 0; i < 6; i ++) {
code |= (this->*planeTestArray[i]) (matrix, guardPlanes);
dgAssert (code >= 0);
if (code) {
return code;
}
}
}
}
}
return code;
}
dgInt32 dgObb::VisibilityTest (const dgCamera* camera) const
{
dgMatrix viewMatrix (*this * camera->GetViewMatrix());
return VisibilityTestLow (camera, viewMatrix);
}
dgInt32 dgObb::VisibilityTest (const dgCamera* camera, const dgMatrix &worldMatrix) const
{
dgMatrix viewMatrix (*this * worldMatrix * camera->GetViewMatrix());
return VisibilityTestLow (camera, viewMatrix);
}
void dgObb::Render (
const dgCamera* camera,
const dgMatrix &worldMatrix,
dgUnsigned32 rgb) const
{
dgInt32 i;
struct ColorVertex
{
dgFloat32 m_x;
dgFloat32 m_y;
dgFloat32 m_z;
dgColor m_color;
};
dgUnsigned32 index [][2] = {
{0, 4}, {1, 5}, {2, 6}, {3, 7},
{0, 1}, {4, 5}, {7, 6}, {3, 2},
{1, 2}, {5, 6}, {4, 7}, {0, 3},
};
ColorVertex* ptr;
ColorVertex box[8];
box[0].m_x = -size.m_x;
box[0].m_y = -size.m_y;
box[0].m_z = -size.m_z;
box[0].m_color.m_val = rgb;
box[1].m_x = size.m_x;
box[1].m_y = -size.m_y;
box[1].m_z = -size.m_z;
box[1].m_color.m_val = rgb;
box[2].m_x = size.m_x;
box[2].m_y = -size.m_y;
box[2].m_z = size.m_z;
box[2].m_color.m_val = rgb;
box[3].m_x = -size.m_x;
box[3].m_y = -size.m_y;
box[3].m_z = size.m_z;
box[3].m_color.m_val = rgb;
box[4].m_x = -size.m_x;
box[4].m_y = size.m_y;
box[4].m_z = -size.m_z;
box[4].m_color.m_val = rgb;
box[5].m_x = size.m_x;
box[5].m_y = size.m_y;
box[5].m_z = -size.m_z;
box[5].m_color.m_val = rgb;
box[6].m_x = size.m_x;
box[6].m_y = size.m_y;
box[6].m_z = size.m_z;
box[6].m_color.m_val = rgb;
box[7].m_x = -size.m_x;
box[7].m_y = size.m_y;
box[7].m_z = size.m_z;
box[7].m_color.m_val = rgb;
dgRenderDescriptorParams param;
param.m_indexCount = 0;
param.m_vertexCount = sizeof (index) / sizeof (dgInt32);
param.m_descType = dgDynamicVertex;
param.m_primitiveType = RENDER_LINELIST;
param.m_vertexFlags = VERTEX_ENABLE_XYZ | COLOR_ENABLE;
dgRenderDescriptor desc (param);
dgMatrix tmpMat (*this * worldMatrix);
camera->SetWorldMatrix (&tmpMat);
desc.m_material = dgMaterial::UseDebugMaterial();
dgVertexRecord vertexRecord (desc.LockVertex());
ptr = (ColorVertex*) vertexRecord.vertex.ptr;
for (i = 0; i < (sizeof (index) / (2 * sizeof (dgUnsigned32))); i ++) {
ptr[0] = box[index[i][0]];
ptr[1] = box[index[i][1]];
ptr += 2;
}
desc.UnlockVertex();
camera->Render (desc);
desc.m_material->Release();
}
*/

115
thirdparty/src/newton/dgCore/dgObb.h vendored Normal file
View File

@@ -0,0 +1,115 @@
/* Copyright (c) <2003-2019> <Julio Jerez, Newton Game Dynamics>
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
*
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
*
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
*
* 3. This notice may not be removed or altered from any source distribution.
*/
#ifndef __dgOOBB_H__
#define __dgOOBB_H__
#include "dgStdafx.h"
#include "dgTypes.h"
#include "dgVector.h"
#include "dgMatrix.h"
#include "dgQuaternion.h"
class dgPlane;
DG_MSC_VECTOR_ALIGNMENT
class dgObb: public dgMatrix
{
public:
DG_INLINE dgObb (){};
dgObb (const dgQuaternion &quat, const dgVector &position, const dgVector& dim = dgVector(0.0f));
dgObb (const dgMatrix& matrix, const dgVector& dim = dgVector(0.0f));
dgObb &operator= (const dgMatrix &arg);
void Scale (dgFloat32 Ws, dgFloat32 Hs, dgFloat32 Bs) ;
void SetDimensions (dgFloat32 W, dgFloat32 H, dgFloat32 B);
void SetDimensions (const dgFloat32 vertex[], dgInt32 strideInBytes, dgInt32 vertexCount, const dgMatrix *basis = NULL);
void SetDimensions (const dgFloat32 vertex[], dgInt32 strideInBytes, const dgInt32 triangles[], dgInt32 indexCount, const dgMatrix *basis);
// void SetDimensions (const dgFloat32 vertex[], dgInt32 strideInBytes, const dgInt32 index[], dgInt32 indexCount, const dgMatrix *basis = NULL);
// return: 0 if the sphere is wholly inside the view port
// 1 if the sphere is partially inside the view port
// -1 if the sphere is wholly outside the view port
// dgInt32 VisibilityTest (const dgCamera* camera) const;
// dgInt32 VisibilityTest (const dgCamera* camera, const dgMatrix &worldMatrix) const;
// void Render (const dgCamera* camera, const dgMatrix &transform, unsigned rgb) const;
private:
/*
typedef dgInt32 (dgSphere::*CachedVisibilityTest) (const dgMatrix &point, const dgPlane* plane) const;
mutable CachedVisibilityTest planeTest;
static CachedVisibilityTest planeTestArray[6];
void ChangeCachedVisibilityTest (CachedVisibilityTest fnt);
dgInt32 FrontTest (const dgMatrix &point, const dgPlane* plane) const;
dgInt32 RearTest (const dgMatrix &point, const dgPlane* plane) const;
dgInt32 LeftTest (const dgMatrix &point, const dgPlane* plane) const;
dgInt32 RightTest (const dgMatrix &point, const dgPlane* plane) const;
dgInt32 TopTest (const dgMatrix &point, const dgPlane* plane) const;
dgInt32 BottomTest (const dgMatrix &point, const dgPlane* plane) const;
dgInt32 VisibilityTestLow (const dgCamera* camera, const dgMatrix& viewMNatrix) const;
*/
public:
dgVector m_size;
} DG_GCC_VECTOR_ALIGNMENT;
inline dgObb::dgObb (const dgQuaternion &quat, const dgVector &position, const dgVector& dim)
:dgMatrix(quat, position)
{
SetDimensions (dim.m_x, dim.m_y, dim.m_z);
dgAssert (0);
}
inline dgObb::dgObb(const dgMatrix& matrix, const dgVector& dim)
:dgMatrix(matrix)
{
SetDimensions (dim.m_x, dim.m_y, dim.m_z);
}
inline dgObb &dgObb::operator= (const dgMatrix &arg)
{
m_front = arg.m_front;
m_up = arg.m_up;
m_right = arg.m_right;
m_posit = arg.m_posit;
return *this;
}
inline void dgObb::SetDimensions (dgFloat32 W, dgFloat32 H, dgFloat32 B)
{
m_size = dgVector (dgAbs(W), dgAbs(H), dgAbs(B), dgSqrt (W * W + H * H + B * B));
}
inline void dgObb::Scale (dgFloat32 Ws, dgFloat32 Hs, dgFloat32 Bs)
{
SetDimensions (m_size.m_x * Ws, m_size.m_y * Hs, m_size.m_z * Bs);
}
#endif

226
thirdparty/src/newton/dgCore/dgPathFinder.h vendored Normal file
View File

@@ -0,0 +1,226 @@
/* Copyright (c) <2003-2019> <Julio Jerez, Newton Game Dynamics>
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
*
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
*
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
*
* 3. This notice may not be removed or altered from any source distribution.
*/
#ifndef __dgPathFinder__
#define __dgPathFinder__
#include "dgStdafx.h"
#include "dgTree.h"
#include "dgHeap.h"
template<class NODEID, class COST> class dgPathFinder;
#define DG_MAX_PATH_ENUMERATION_CHILDREN 128
template<class NODEID, class COST>
class dgPathNode
{
friend class dgPathFinder<NODEID, COST>;
NODEID m_Id;
COST m_NodeCostToGoal;
COST m_NodeCostFromSource;
dgPathNode* m_Next;
public:
dgPathNode();
NODEID GetId() const;
const dgPathNode* GetNext() const;
const dgPathNode* GetParent() const;
};
template<class NODEID, class COST>
class dgPathCloseList: public dgTree<dgPathNode<NODEID, COST>, NODEID>
{
protected:
dgPathCloseList(): dgTree<dgPathNode<NODEID, COST>, NODEID>() {}
};
template<class NODEID, class COST>
class dgPathOpenHeap: public dgUpHeap<typename dgPathCloseList<NODEID, COST>::dgTreeNode*, COST>
{
protected:
dgPathOpenHeap(dgInt32 maxElements)
:dgUpHeap<typename dgPathCloseList<NODEID, COST>::dgTreeNode*, COST>(maxElements)
{
}
friend class dgPathFinder<NODEID, COST>;
};
template<class NODEID, class COST>
class dgPathFinder: public dgPathCloseList<NODEID, COST>
{
dgInt32 maxChildren;
public:
dgPathFinder(dgInt32 maxElementsInOpenList, dgInt32 nodeMaxChildren);
virtual ~dgPathFinder();
virtual const dgPathNode<NODEID, COST>* CalCulatePath (NODEID source, NODEID goal);
// this funtions must be overloaded by the user
virtual COST GetCostFromParent(const dgPathNode<NODEID, COST>& node) const;
virtual COST GetEstimatedCostToGoal(NODEID id) const;
virtual dgInt32 EnumerateChildren(NODEID parent, NODEID array[]) const;
dgPathOpenHeap<NODEID, COST> m_openList;
};
template<class NODEID, class COST>
dgPathNode<NODEID, COST>::dgPathNode ()
{
}
template<class NODEID, class COST>
NODEID dgPathNode<NODEID, COST>::GetId () const
{
return m_Id;
}
template<class NODEID, class COST>
const dgPathNode<NODEID, COST>* dgPathNode<NODEID, COST>::GetNext() const
{
return m_Next;
}
template<class NODEID, class COST>
const dgPathNode<NODEID, COST>* dgPathNode<NODEID, COST>::GetParent() const
{
return m_Next;
}
template<class NODEID, class COST>
dgPathFinder<NODEID, COST>::dgPathFinder(
dgInt32 maxElementsInOpenList,
dgInt32 nodeMaxChildren)
:dgPathCloseList<NODEID, COST>(),
m_openList(maxElementsInOpenList)
{
maxChildren = nodeMaxChildren;
}
template<class NODEID, class COST>
dgPathFinder<NODEID, COST>::~dgPathFinder()
{
}
template<class NODEID, class COST>
const dgPathNode<NODEID, COST>* dgPathFinder<NODEID, COST>::CalCulatePath (NODEID source, NODEID goal)
{
dgInt32 count;
dgInt32 heapMaxCount;
dgPathNode<NODEID, COST> cell;
dgPathNode<NODEID, COST>* next;
dgPathNode<NODEID, COST>* prev;
dgPathNode<NODEID, COST>* link;
typename dgPathCloseList<NODEID, COST>::dgTreeNode* node;
NODEID idArray[DG_MAX_PATH_ENUMERATION_CHILDREN];
dgPathCloseList<NODEID, COST>& close = *this;
m_openList.Flush();
close.RemoveAll();
cell.m_Id = source;
cell.m_Next = NULL;
cell.m_NodeCostFromSource = COST (0);
cell.m_NodeCostToGoal = GetEstimatedCostToGoal(cell.m_Id);
node = close.Insert (cell, source);
heapMaxCount = m_openList.GetMaxCount();
m_openList.Push (node, cell.m_NodeCostFromSource + cell.m_NodeCostToGoal);
while (m_openList.GetCount()) {
node = m_openList[0];
dgPathNode<NODEID, COST>& parent = node->GetInfo();
if (parent.m_Id == goal) {
link = &parent;
next = NULL;
do {
prev = link->m_Next;
link->m_Next = next;
next = link;
link = prev;
} while (link);
return next;
}
m_openList.Pop();
count = EnumerateChildren(node->GetKey(), idArray);
cell.m_Next = &parent;
for (int i = 0; i < count; i ++) {
bool state;
cell.m_Id = idArray[i];
COST newCostFromSource (GetCostFromParent(cell) + parent.m_NodeCostFromSource);
node = close.Insert (cell, cell.m_Id, state);
dgPathNode<NODEID, COST>& newCell = node->GetInfo();
if (state) {
if (newCell.m_NodeCostFromSource <= newCostFromSource) {
continue;
}
//newCell.m_Next = cell.m_Next;
}
newCell.m_NodeCostFromSource = newCostFromSource;
newCell.m_NodeCostToGoal = GetEstimatedCostToGoal(newCell.m_Id);
if (m_openList.GetCount() >= heapMaxCount) {
m_openList.Remove (heapMaxCount);
}
m_openList.Push (node, newCell.m_NodeCostFromSource + newCell.m_NodeCostToGoal);
}
}
return NULL;
}
template<class NODEID, class COST>
COST dgPathFinder<NODEID, COST>::GetCostFromParent(const dgPathNode<NODEID, COST>& node) const
{
return COST (1);
}
template<class NODEID, class COST>
COST dgPathFinder<NODEID, COST>::GetEstimatedCostToGoal(NODEID id) const
{
return COST (1);
}
template<class NODEID, class COST>
dgInt32 dgPathFinder<NODEID, COST>::EnumerateChildren(NODEID parent, NODEID array[]) const
{
return 0;
}
#endif

159
thirdparty/src/newton/dgCore/dgPlane.h vendored Normal file
View File

@@ -0,0 +1,159 @@
/* Copyright (c) <2003-2019> <Julio Jerez, Newton Game Dynamics>
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
*
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
*
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
*
* 3. This notice may not be removed or altered from any source distribution.
*/
#ifndef __dgPlane__
#define __dgPlane__
#include "dgStdafx.h"
#include "dgVector.h"
#ifdef _NEWTON_USE_DOUBLE
#define dgPlane dgBigPlane
#else
DG_MSC_VECTOR_ALIGNMENT
class dgPlane: public dgVector
{
public:
dgPlane ();
dgPlane (const dgVector& point);
dgPlane (dgFloat32 x, dgFloat32 y, dgFloat32 z, dgFloat32 w);
dgPlane (const dgVector &normal, dgFloat32 distance);
dgPlane (const dgVector &P0, const dgVector &P1, const dgVector &P2);
dgPlane Scale (dgFloat32 s) const;
dgFloat32 Evalue (const dgFloat32* const point) const;
dgFloat32 Evalue (const dgVector &point) const;
} DG_GCC_VECTOR_ALIGNMENT;
#endif
DG_MSC_VECTOR_ALIGNMENT
class dgBigPlane: public dgBigVector
{
public:
dgBigPlane ();
dgBigPlane (const dgBigVector& point);
dgBigPlane (dgFloat64 x, dgFloat64 y, dgFloat64 z, dgFloat64 w);
dgBigPlane (const dgBigVector &normal, dgFloat64 distance);
dgBigPlane (const dgBigVector &P0, const dgBigVector &P1, const dgBigVector &P2);
dgBigPlane Scale (dgFloat64 s) const;
dgFloat64 Evalue (const dgFloat64* const point) const;
dgFloat64 Evalue (const dgBigVector &point) const;
} DG_GCC_VECTOR_ALIGNMENT;
#ifndef _NEWTON_USE_DOUBLE
DG_INLINE dgPlane::dgPlane ()
:dgVector ()
{
}
DG_INLINE dgPlane::dgPlane (const dgVector& point)
:dgVector (point)
{
}
DG_INLINE dgPlane::dgPlane (dgFloat32 x, dgFloat32 y, dgFloat32 z, dgFloat32 w)
:dgVector (x, y, z, w)
{
}
DG_INLINE dgPlane::dgPlane (const dgVector &normal, dgFloat32 distance)
:dgVector (normal)
{
m_w = distance;
}
DG_INLINE dgPlane::dgPlane (const dgVector &P0, const dgVector &P1, const dgVector &P2)
:dgVector ((P1 - P0).CrossProduct(P2 - P0))
{
m_w = - DotProduct(P0 & dgVector::m_triplexMask).GetScalar();
}
DG_INLINE dgPlane dgPlane::Scale (dgFloat32 s) const
{
return dgPlane(*this * dgVector(s));
}
DG_INLINE dgFloat32 dgPlane::Evalue (const dgFloat32* const point) const
{
dgVector p (point);
return DotProduct ((p & m_triplexMask) | m_wOne).GetScalar();
}
DG_INLINE dgFloat32 dgPlane::Evalue (const dgVector& point) const
{
return DotProduct ((point & m_triplexMask) | m_wOne).GetScalar();
}
#endif
DG_INLINE dgBigPlane::dgBigPlane ()
:dgBigVector ()
{
}
DG_INLINE dgBigPlane::dgBigPlane (const dgBigVector& point)
:dgBigVector (point)
{
}
DG_INLINE dgBigPlane::dgBigPlane (dgFloat64 x, dgFloat64 y, dgFloat64 z, dgFloat64 w)
:dgBigVector (x, y, z, w)
{
}
DG_INLINE dgBigPlane::dgBigPlane (const dgBigVector &normal, dgFloat64 distance)
:dgBigVector (normal)
{
m_w = distance;
}
DG_INLINE dgBigPlane::dgBigPlane (const dgBigVector &P0, const dgBigVector &P1, const dgBigVector &P2)
:dgBigVector ((P1 - P0).CrossProduct(P2 - P0))
{
m_w = - DotProduct(P0 & dgBigVector::m_triplexMask).GetScalar();
}
DG_INLINE dgBigPlane dgBigPlane::Scale (dgFloat64 s) const
{
return dgBigPlane (m_x * s, m_y * s, m_z * s, m_w * s);
}
DG_INLINE dgFloat64 dgBigPlane::Evalue (const dgFloat64* const point) const
{
return m_x * point[0] + m_y * point[1] + m_z * point[2] + m_w;
}
DG_INLINE dgFloat64 dgBigPlane::Evalue (const dgBigVector &point) const
{
return m_x * point.m_x + m_y * point.m_y + m_z * point.m_z + m_w;
}
#endif

1090
thirdparty/src/newton/dgCore/dgPolygonSoupBuilder.cpp vendored Normal file
View File

File diff suppressed because it is too large Load Diff

115
thirdparty/src/newton/dgCore/dgPolygonSoupBuilder.h vendored Normal file
View File

@@ -0,0 +1,115 @@
/* Copyright (c) <2003-2019> <Julio Jerez, Newton Game Dynamics>
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
*
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
*
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
*
* 3. This notice may not be removed or altered from any source distribution.
*/
/****************************************************************************
*
* Visual C++ 6.0 created by: Julio Jerez
*
****************************************************************************/
#ifndef __dgPolygonSoupDatabaseBuilder0x23413452233__
#define __dgPolygonSoupDatabaseBuilder0x23413452233__
#include "dgStdafx.h"
#include "dgRef.h"
#include "dgArray.h"
#include "dgIntersections.h"
class AdjacentdFace
{
public:
dgPlane m_normal;
dgInt32 m_count;
dgInt32 *m_index;
dgInt64 m_edgeMap[256];
};
class dgPolygonSoupDatabaseBuilder
{
class dgFaceMap;
class dgFaceInfo;
class dgFaceBucket;
class dgPolySoupFilterAllocator;
public:
dgPolygonSoupDatabaseBuilder (dgMemoryAllocator* const allocator);
dgPolygonSoupDatabaseBuilder (const dgPolygonSoupDatabaseBuilder& sopurce);
~dgPolygonSoupDatabaseBuilder ();
DG_CLASS_ALLOCATOR(allocator)
void Begin();
void End(bool optimize);
void AddMesh (const dgFloat32* const vertex, dgInt32 vertexCount, dgInt32 strideInBytes, dgInt32 faceCount,
const dgInt32* const faceArray, const dgInt32* const indexArray, const dgInt32* const faceTagsData, const dgMatrix& worldMatrix);
void SavePLY(const char* const fileName) const;
private:
void Optimize(dgInt32 faceId, const dgFaceBucket& faceBucket, const dgPolygonSoupDatabaseBuilder& source);
void Finalize();
void FinalizeAndOptimize();
void OptimizeByIndividualFaces();
dgInt32 FilterFace (dgInt32 count, dgInt32* const indexArray);
dgInt32 AddConvexFace (dgInt32 count, dgInt32* const indexArray, dgInt32* const facesArray);
void PackArray();
public:
class dgVertexArray: public dgArray<dgBigVector>
{
public:
dgVertexArray(dgMemoryAllocator* const allocator)
:dgArray<dgBigVector>(allocator)
{
}
};
class dgIndexArray: public dgArray<dgInt32>
{
public:
dgIndexArray(dgMemoryAllocator* const allocator)
:dgArray<dgInt32>(allocator)
{
}
};
dgInt32 m_run;
dgInt32 m_faceCount;
dgInt32 m_indexCount;
dgInt32 m_vertexCount;
dgInt32 m_normalCount;
dgIndexArray m_faceVertexCount;
dgIndexArray m_vertexIndex;
dgIndexArray m_normalIndex;
dgVertexArray m_vertexPoints;
dgVertexArray m_normalPoints;
dgMemoryAllocator* m_allocator;
};
#endif

79
thirdparty/src/newton/dgCore/dgPolygonSoupDatabase.cpp vendored Normal file
View File

@@ -0,0 +1,79 @@
/* Copyright (c) <2003-2019> <Julio Jerez, Newton Game Dynamics>
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
*
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
*
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
*
* 3. This notice may not be removed or altered from any source distribution.
*/
/****************************************************************************
*
* Visual C++ 6.0 created by: Julio Jerez
*
****************************************************************************/
#include "dgStdafx.h"
#include "dgPolygonSoupDatabase.h"
dgPolygonSoupDatabase::dgPolygonSoupDatabase(const char* const name)
{
m_vertexCount = 0;
m_strideInBytes = 0;
m_localVertex = NULL;
}
dgPolygonSoupDatabase::~dgPolygonSoupDatabase ()
{
if (m_localVertex) {
dgFreeStack (m_localVertex);
}
}
dgUnsigned32 dgPolygonSoupDatabase::GetTagId(const dgInt32* const face, dgInt32 indexCount) const
{
return dgUnsigned32 (face[indexCount]);
}
void dgPolygonSoupDatabase::SetTagId(const dgInt32* const facePtr, dgInt32 indexCount, dgUnsigned32 newID) const
{
dgUnsigned32* const face = (dgUnsigned32*) facePtr;
face[indexCount] = newID;
}
dgInt32 dgPolygonSoupDatabase::GetVertexCount() const
{
return m_vertexCount;
}
dgFloat32* dgPolygonSoupDatabase::GetLocalVertexPool() const
{
return m_localVertex;
}
dgInt32 dgPolygonSoupDatabase::GetStrideInBytes() const
{
return m_strideInBytes;
}
dgFloat32 dgPolygonSoupDatabase::GetRadius() const
{
return 0.0f;
}

66
thirdparty/src/newton/dgCore/dgPolygonSoupDatabase.h vendored Normal file
View File

@@ -0,0 +1,66 @@
/* Copyright (c) <2003-2019> <Julio Jerez, Newton Game Dynamics>
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
*
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
*
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
*
* 3. This notice may not be removed or altered from any source distribution.
*/
/****************************************************************************
*
* Visual C++ 6.0 created by: Julio Jerez
*
****************************************************************************/
#ifndef __dgPolygonSoupDatabase_H_
#define __dgPolygonSoupDatabase_H_
#include "dgStdafx.h"
#include "dgRef.h"
#include "dgArray.h"
#include "dgIntersections.h"
class dgMatrix;
class dgPolygonSoupDatabase
{
public:
dgFloat32 GetRadius() const;
dgInt32 GetVertexCount() const;
dgInt32 GetStrideInBytes() const;
dgFloat32* GetLocalVertexPool() const;
dgUnsigned32 GetTagId(const dgInt32* const face, dgInt32 indexCount) const;
void SetTagId(const dgInt32* const face, dgInt32 indexCount, dgUnsigned32 newID) const;
virtual void Serialize (dgSerialize callback, void* const userData) const = 0;
virtual void Deserialize (dgDeserialize callback, void* const userData, dgInt32 revisionNumber) = 0;
protected:
dgPolygonSoupDatabase(const char* const name = NULL);
virtual ~dgPolygonSoupDatabase ();
dgInt32 m_vertexCount;
dgInt32 m_strideInBytes;
dgFloat32* m_localVertex;
};
#endif

2864
thirdparty/src/newton/dgCore/dgPolyhedra.cpp vendored Normal file
View File

File diff suppressed because it is too large Load Diff

325
thirdparty/src/newton/dgCore/dgPolyhedra.h vendored Normal file
View File

@@ -0,0 +1,325 @@
/* Copyright (c) <2003-2019> <Julio Jerez, Newton Game Dynamics>
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
*
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
*
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
*
* 3. This notice may not be removed or altered from any source distribution.
*/
#ifndef __dgPolyhedra__
#define __dgPolyhedra__
#include "dgStdafx.h"
#include "dgList.h"
#include "dgTree.h"
#include "dgHeap.h"
#include "dgDebug.h"
class dgEdge;
class dgPlane;
class dgObb;
class dgMatrix;
class dgPolyhedra;
class dgVertexCollapseVertexMetric;
typedef dgInt64 dgEdgeKey;
DG_MSC_VECTOR_ALIGNMENT
class dgEdge
{
public:
dgEdge ();
dgEdge (dgInt32 vertex, dgInt32 face, dgUnsigned64 userdata = 0);
~dgEdge ();
dgInt32 m_incidentVertex;
dgInt32 m_incidentFace;
dgUnsigned64 m_userData;
dgEdge* m_next;
dgEdge* m_prev;
dgEdge* m_twin;
dgInt32 m_mark;
} DG_GCC_VECTOR_ALIGNMENT;
class dgPolyhedra: public dgTree <dgEdge, dgEdgeKey>
{
public:
class dgPairKey
{
public:
dgPairKey()
{
}
dgPairKey(dgInt64 key)
:m_key(dgUnsigned64(key))
{
}
dgPairKey(dgInt32 keyHigh, dgInt32 keyLow)
:m_keyLow(dgUnsigned32 (keyLow))
,m_keyHigh(dgUnsigned32 (keyHigh))
{
}
dgInt64 GetVal() const
{
return dgInt64(m_key);
}
dgInt32 GetLowKey() const
{
return dgInt32(m_keyLow);
}
dgInt32 GetHighKey() const
{
return dgInt32(m_keyHigh);
}
bool operator<(const dgPairKey& key) const
{
return m_key < key.m_key;
}
bool operator>(const dgPairKey& key) const
{
return m_key > key.m_key;
}
private:
union {
dgUnsigned64 m_key;
struct {
dgUnsigned32 m_keyLow;
dgUnsigned32 m_keyHigh;
};
};
};
dgPolyhedra (dgMemoryAllocator* const allocator);
dgPolyhedra (const dgPolyhedra &polyhedra);
virtual ~dgPolyhedra();
virtual void BeginFace();
dgEdge* AddFace (dgInt32 v0, dgInt32 v1, dgInt32 v2);
dgEdge* AddFace (dgInt32 count, const dgInt32* const index);
dgEdge* AddFace (dgInt32 count, const dgInt32* const index, const dgInt64* const userdata);
virtual bool EndFace ();
virtual void DeleteFace(dgEdge* const edge);
dgInt32 GetFaceCount() const;
dgInt32 GetEdgeCount() const;
dgInt32 GetLastVertexIndex() const;
dgInt32 IncLRU() const;
dgInt32 GetLRU() const;
void SetLRU(dgInt32 lru) const;
dgEdge* FindEdge (dgInt32 v0, dgInt32 v1) const;
dgTreeNode* FindEdgeNode (dgInt32 v0, dgInt32 v1) const;
dgEdge* AddHalfEdge (dgInt32 v0, dgInt32 v1);
void DeleteEdge (dgEdge* const edge);
void DeleteEdge (dgInt32 v0, dgInt32 v1);
dgEdge* ConnectVertex (dgEdge* const e0, dgEdge* const e1);
bool FlipEdge (dgEdge* const edge);
dgEdge* SpliteEdge (dgInt32 newIndex, dgEdge* const edge);
dgBigVector FaceNormal (const dgEdge* const face, const dgFloat64* const vertex, dgInt32 strideInBytes) const;
void SavePLY(const char* const fileName, const dgFloat64* const vertex, dgInt32 strideInBytes) const;
void BeginConectedSurface() const;
bool GetConectedSurface (dgPolyhedra &polyhedra) const;
void EndConectedSurface() const;
dgObb CalculateSphere (const dgFloat64* const vertex, dgInt32 strideInBytes, const dgMatrix* const basis = NULL) const;
void ChangeEdgeIncidentVertex (dgEdge* const edge, dgInt32 newIndex);
void DeleteDegenerateFaces (const dgFloat64* const pool, dgInt32 dstStrideInBytes, dgFloat64 minArea);
bool Optimize (const dgFloat64* const pool, dgInt32 strideInBytes, dgReportProgress normalizedProgress, void* const reportProgressUserData, dgFloat64 tol, dgInt32 maxFaceCount = 1<<28);
void Triangulate (const dgFloat64* const vertex, dgInt32 strideInBytes, dgPolyhedra* const leftOversOut);
void ConvexPartition (const dgFloat64* const vertex, dgInt32 strideInBytes, dgPolyhedra* const leftOversOut);
bool IsFaceConvex(dgEdge* const face, const dgFloat64* const pool, dgInt32 strideInBytes) const;
protected:
dgEdge* CollapseEdge(dgEdge* const edge);
bool PolygonizeFace(dgEdge* const face, const dgFloat64* const pool, dgInt32 stride);
bool TriangulateFace(dgEdge* const face, const dgFloat64* const pool, dgInt32 stride);
private:
void RefineTriangulation (const dgFloat64* const vertex, dgInt32 stride);
void RefineTriangulation (const dgFloat64* const vertex, dgInt32 stride, const dgBigVector& normal, dgInt32 perimeterCount, dgEdge** const perimeter);
void OptimizeTriangulation (const dgFloat64* const vertex, dgInt32 strideInBytes);
void RemoveInteriorEdges (dgPolyhedra& polyhedraOut, const dgFloat64* const vertex, dgInt32 strideInBytes);
void MarkAdjacentCoplanarFaces (dgPolyhedra& polyhedraOut, dgEdge* const face, const dgFloat64* const pool, dgInt32 strideInBytes);
dgEdge* FindEarTip (dgEdge* const face, const dgFloat64* const pool, dgInt32 stride, dgDownHeap<dgEdge*, dgFloat64>& heap, const dgBigVector &normal) const;
dgEdge* TriangulateFace (dgEdge* const face, const dgFloat64* const pool, dgInt32 stride, dgDownHeap<dgEdge*, dgFloat64>& heap, dgBigVector* const faceNormalOut);
void RemoveHalfEdge (dgEdge* const edge);
dgEdge* OptimizeCollapseEdge (dgEdge* const edge);
bool IsOkToCollapse (const dgBigVector* const pool, dgEdge* const edge) const;
dgFloat64 EdgePenalty (const dgBigVector* const pool, dgEdge* const edge, dgFloat64 dist) const;
dgBigPlane EdgePlane (dgInt32 i0, dgInt32 i1, dgInt32 i2, const dgBigVector* const pool) const;
void CalculateAllMetrics (dgVertexCollapseVertexMetric* const table, const dgBigVector* const pool) const;
void CalculateVertexMetrics (dgVertexCollapseVertexMetric* const table, const dgBigVector* const pool, dgEdge* const edge) const;
dgEdge* BestEdgePolygonizeFace(const dgBigVector& normal, dgEdge* const edge, const dgFloat64* const pool, dgInt32 stride, const dgBigVector& point) const;
static dgInt32 GetInteriorDiagonals (dgPolyhedra& polyhedra, dgEdge** const diagonals, dgInt32 maxCount);
static dgBigPlane UnboundedLoopPlane (dgInt32 i0, dgInt32 i1, dgInt32 i2, const dgBigVector* const pool);
static void RemoveOuterColinearEdges(dgPolyhedra& flatFace, const dgFloat64* const vertex, dgInt32 stride);
static void RemoveInteriorColinearEdges(dgPolyhedra& flatFace, const dgFloat64* const vertex, dgInt32 stride);
static bool IsEssensialDiagonal (dgEdge* const diagonal, const dgBigVector& normal, const dgFloat64* const pool, dgInt32 stride);
static bool IsEssensialPointDiagonal (dgEdge* const diagonal, const dgBigVector& normal, const dgFloat64* const pool, dgInt32 stride);
mutable dgInt32 m_baseMark;
mutable dgInt32 m_edgeMark;
mutable dgInt32 m_faceSecuence;
friend class dgPolyhedraDescriptor;
};
DG_INLINE dgEdge::dgEdge ()
{
}
DG_INLINE dgEdge::dgEdge (dgInt32 vertex, dgInt32 face, dgUnsigned64 userdata)
:m_incidentVertex(vertex)
,m_incidentFace(face)
,m_userData(userdata)
,m_next(NULL)
,m_prev(NULL)
,m_twin(NULL)
,m_mark(0)
{
}
DG_INLINE dgEdge::~dgEdge ()
{
}
DG_INLINE void dgPolyhedra::BeginFace ()
{
}
DG_INLINE dgEdge* dgPolyhedra::AddFace (dgInt32 count, const dgInt32* const index)
{
return AddFace (count, index, NULL);
}
DG_INLINE dgEdge* dgPolyhedra::AddFace (dgInt32 v0, dgInt32 v1, dgInt32 v2)
{
dgInt32 vertex[3];
vertex [0] = v0;
vertex [1] = v1;
vertex [2] = v2;
return AddFace (3, vertex, NULL);
}
DG_INLINE dgInt32 dgPolyhedra::GetEdgeCount() const
{
#ifdef _DEBUG
dgInt32 edgeCount = 0;
Iterator iter(*this);
for (iter.Begin(); iter; iter ++) {
edgeCount ++;
}
dgAssert (edgeCount == GetCount());;
#endif
return GetCount();
}
DG_INLINE dgInt32 dgPolyhedra::GetLastVertexIndex() const
{
dgInt32 maxVertexIndex = -1;
Iterator iter(*this);
for (iter.Begin(); iter; iter ++) {
const dgEdge* const edge = &(*iter);
if (edge->m_incidentVertex > maxVertexIndex) {
maxVertexIndex = edge->m_incidentVertex;
}
}
return maxVertexIndex + 1;
}
DG_INLINE dgInt32 dgPolyhedra::IncLRU() const
{
m_edgeMark ++;
dgAssert (m_edgeMark < 0x7fffffff);
return m_edgeMark;
}
DG_INLINE dgInt32 dgPolyhedra::GetLRU() const
{
return m_edgeMark;
}
DG_INLINE void dgPolyhedra::SetLRU(dgInt32 lru) const
{
if (lru > m_edgeMark) {
m_edgeMark = lru;
}
}
DG_INLINE void dgPolyhedra::BeginConectedSurface() const
{
m_baseMark = IncLRU();
}
DG_INLINE void dgPolyhedra::EndConectedSurface() const
{
}
DG_INLINE dgPolyhedra::dgTreeNode* dgPolyhedra::FindEdgeNode (dgInt32 i0, dgInt32 i1) const
{
dgPairKey key (i0, i1);
return Find (key.GetVal());
}
DG_INLINE dgEdge *dgPolyhedra::FindEdge (dgInt32 i0, dgInt32 i1) const
{
// dgTreeNode *node;
// dgPairKey key (i0, i1);
// node = Find (key.GetVal());
// return node ? &node->GetInfo() : NULL;
dgTreeNode* const node = FindEdgeNode (i0, i1);
return node ? &node->GetInfo() : NULL;
}
DG_INLINE void dgPolyhedra::DeleteEdge (dgInt32 v0, dgInt32 v1)
{
dgPairKey pairKey (v0, v1);
dgTreeNode* const node = Find(pairKey.GetVal());
dgEdge* const edge = node ? &node->GetInfo() : NULL;
if (!edge) {
return;
}
DeleteEdge (edge);
}
#endif

573
thirdparty/src/newton/dgCore/dgPolyhedraMassProperties.cpp vendored Normal file
View File

@@ -0,0 +1,573 @@
/* Copyright (c) <2003-2019> <Julio Jerez, Newton Game Dynamics>
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
*
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
*
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
*
* 3. This notice may not be removed or altered from any source distribution.
*/
#include "dgStdafx.h"
#include "dgVector.h"
#include "dgPolyhedraMassProperties.h"
#if 0
class dgPolyhedraMassProperties
{
#define X 0
#define Y 1
#define Z 2
#define SQR(x) ((x)*(x))
#define CUBE(x) ((x)*(x)*(x))
public:
dgPolyhedraMassProperties()
{
memset (this, 0, sizeof (dgPolyhedraMassProperties));
}
void ProjectionIntegrals(
dgInt32 indexCount,
const dgVector* faceVertex)
{
dgInt32 i0;
dgInt32 i1;
dgFloat32 a0, a1, da;
dgFloat32 b0, b1, db;
dgFloat32 C1;
dgFloat32 Ca;
dgFloat32 Cb;
dgFloat32 Caa;
dgFloat32 Cbb;
dgFloat32 Cab;
dgFloat32 Kab;
dgFloat32 a0_2;
dgFloat32 a0_3;
dgFloat32 a1_2;
dgFloat32 b0_2;
dgFloat32 b0_3;
m_P1 = dgFloat32 (0.0f);
m_Pa = dgFloat32 (0.0f);
m_Pb = dgFloat32 (0.0f);
m_Paa = dgFloat32 (0.0f);
m_Pbb = dgFloat32 (0.0f);
m_Pab = dgFloat32 (0.0f);
i0 = indexCount - 1;
for (i1 = 0; i1 < indexCount; i1 ++) {
a0 = faceVertex[i0][m_A];
b0 = faceVertex[i0][m_B];
a1 = faceVertex[i1][m_A];
b1 = faceVertex[i1][m_B];
i0 = i1;
da = a1 - a0;
db = b1 - b0;
a0_2 = a0 * a0;
a0_3 = a0_2 * a0;
b0_2 = b0 * b0;
b0_3 = b0_2 * b0;
a1_2 = a1 * a1;
C1 = a1 + a0;
Ca = a1 * C1 + a0_2;
Caa = a1*Ca + a0_3;
Cb = b1 * (b1 + b0) + b0_2;
Cbb = b1*Cb + b0_3;
Cab = dgFloat32 (3.0f) * a1_2 + dgFloat32 (2.0f) * a1 * a0 + a0_2;
Kab = a1_2 + dgFloat32 (2.0f) * a1 * a0 + dgFloat32 (3.0f) * a0_2;
m_P1 += db * C1;
m_Pa += db * Ca;
m_Paa += db * Caa;
m_Pb += da * Cb;
m_Pbb += da * Cbb;
m_Pab += db * (b1 * Cab + b0 * Kab);
}
m_P1 *= dgFloat32 (0.5f);
m_Pa *= dgFloat32 ( 1.0f / 6.0f);
m_Pb *= dgFloat32 (-1.0f / 6.0f);
m_Paa *= dgFloat32 (1.0f / 12.0f);
m_Pbb *= dgFloat32 (-1.0f / 12.0f);
m_Pab *= dgFloat32 ( 1.0f / 24.0f);
}
void FaceIntegrals (
dgInt32 count,
const dgPlane& plane,
const dgVector* faceVertex)
{
dgFloat32 k1, k2, k3, k4;
ProjectionIntegrals (count, faceVertex);
k1 = dgFloat32 (1.0f) / plane[m_C];
k2 = k1 * k1;
k3 = k2 * k1;
k4 = k3 * k1;
m_Fa = k1 * m_Pa;
m_Fb = k1 * m_Pb;
m_Fc = -k2 * (plane[m_A] * m_Pa + plane[m_B] * m_Pb + plane[3] * m_P1);
m_Faa = k1 * m_Paa;
m_Fbb = k1 * m_Pbb;
m_Fcc = k3 * (SQR(plane[m_A]) * m_Paa + dgFloat32 (2.0f) * plane[m_A] * plane[m_B] * m_Pab +
SQR(plane[m_B]) * m_Pbb + plane[3] * (dgFloat32 (2.0f) *(plane[m_A] * m_Pa + plane[m_B] * m_Pb) + plane[3] * m_P1));
}
void VolumeIntegrals(
dgInt32 indexCount,
const dgPlane& plane,
const dgVector* faceVertex)
{
dgFloat32 mag2;
mag2 = plane % plane;
if (mag2 > dgFloat32 (1.0e-8f)) {
if ((dgAbs (plane.m_x) > dgAbs (plane.m_y)) && (dgAbs (plane.m_x) > dgAbs (plane.m_z))) {
m_C = X;
} else {
m_C = (dgAbs (plane.m_y) > dgAbs (plane.m_z)) ? Y : Z;
}
m_A = (m_C + 1) % 3;
m_B = (m_A + 1) % 3;
FaceIntegrals (indexCount, plane, faceVertex);
m_T0 += plane[X] * ((m_A == X) ? m_Fa : ((m_B == X) ? m_Fb : m_Fc));
m_T1[m_A] += plane[m_A] * m_Faa;
m_T1[m_B] += plane[m_B] * m_Fbb;
m_T1[m_C] += plane[m_C] * m_Fcc;
//dgTrace (("(%f %f %f) (%f %f %f) (%f %f %f)\n", m_T1[m_A], m_T1[m_B], m_T1[m_C], plane[m_A], plane[m_B], plane[m_C], m_Faa, m_Fbb, m_Fcc))
}
}
void AddInertia (int indexCount, const dgFloat32* faceVertex)
{
dgInt32 i0;
dgInt32 i1;
dgFloat32 a0, a1, da;
dgFloat32 b0, b1, db;
dgFloat32 C1;
dgFloat32 Ca;
dgFloat32 Cb;
dgFloat32 Caa;
dgFloat32 Cbb;
dgFloat32 Cab;
dgFloat32 Kab;
dgFloat32 Caaa;
dgFloat32 Cbbb;
dgFloat32 Cabb;
dgFloat32 Caab;
dgFloat32 Kabb;
dgFloat32 Kaab;
dgFloat32 a0_2;
dgFloat32 a0_3;
dgFloat32 a0_4;
dgFloat32 a1_2;
dgFloat32 a1_3;
dgFloat32 b0_2;
dgFloat32 b0_3;
dgFloat32 b0_4;
dgFloat32 b1_2;
dgFloat32 b1_3;
dgFloat32 mag2;
dgVector p0 (&faceVertex[0]);
dgVector p1 (&faceVertex[3]);
dgVector p2 (&faceVertex[6]);
dgPlane plane (p0, p1, p2);
mag2 = plane % plane;
if (mag2 > dgFloat32 (1.0e-8f)) {
plane = plane.Scale (dgRsqrt ((plane % plane)));
if ((dgAbs (plane.m_x) > dgAbs (plane.m_y)) && (dgAbs (plane.m_x) > dgAbs (plane.m_z))) {
m_C = X;
} else {
m_C = (dgAbs (plane.m_y) > dgAbs (plane.m_z)) ? Y : Z;
}
m_A = (m_C + 1) % 3;
m_B = (m_A + 1) % 3;
// FaceIntegrals (indexCount, plane, faceVertex);
dgFloat32 k1, k2, k3, k4;
{
{
//ProjectionIntegrals (count, faceVertex);
m_P1 = dgFloat32 (0.0f);
m_Pa = dgFloat32 (0.0f);
m_Pb = dgFloat32 (0.0f);
m_Paa = dgFloat32 (0.0f);
m_Pbb = dgFloat32 (0.0f);
m_Pab = dgFloat32 (0.0f);
m_Paaa = dgFloat32 (0.0f);
m_Pbbb = dgFloat32 (0.0f);
m_Paab = dgFloat32 (0.0f);
m_Pabb = dgFloat32 (0.0f);
i0 = indexCount - 1;
for (i1 = 0; i1 < indexCount; i1 ++) {
a0 = faceVertex[i0 * 3 + m_A];
b0 = faceVertex[i0 * 3 + m_B];
a1 = faceVertex[i1 * 3 + m_A];
b1 = faceVertex[i1 * 3 + m_B];
i0 = i1;
da = a1 - a0;
db = b1 - b0;
a0_2 = a0 * a0;
a0_3 = a0_2 * a0;
a0_4 = a0_3 * a0;
b0_2 = b0 * b0;
b0_3 = b0_2 * b0;
b0_4 = b0_3 * b0;
a1_2 = a1 * a1;
a1_3 = a1_2 * a1;
b1_2 = b1 * b1;
b1_3 = b1_2 * b1;
C1 = a1 + a0;
Ca = a1 * C1 + a0_2;
Caa = a1 * Ca + a0_3;
Caaa = a1 * Caa + a0_4;
Cb = b1 * (b1 + b0) + b0_2;
Cbb = b1 * Cb + b0_3;
Cbbb = b1 * Cbb + b0_4;
Cab = dgFloat32 (3.0f) * a1_2 + dgFloat32 (2.0f) * a1 * a0 + a0_2;
Kab = a1_2 + dgFloat32 (2.0f) * a1 * a0 + dgFloat32 (3.0f) * a0_2;
Caab = a0 * Cab + dgFloat32 (4.0f) * a1_3;
Kaab = a1 * Kab + dgFloat32 (4.0f) * a0_3;
Cabb = dgFloat32 (4.0f) * b1_3 + dgFloat32 (3.0f) * b1_2 * b0 + dgFloat32 (2.0f) * b1 * b0_2 + b0_3;
Kabb = b1_3 + dgFloat32 (2.0f) * b1_2 * b0 + dgFloat32 (3.0f) * b1 * b0_2 + dgFloat32 (4.0f) * b0_3;
m_P1 += (db * C1);
m_Pa += (db * Ca);
m_Paa += (db * Caa);
m_Pb += (da * Cb);
m_Pbb += (da * Cbb);
m_Pab += (db * (b1 * Cab + b0 * Kab));
m_Paaa += (db * Caaa);
m_Pbbb += (da * Cbbb);
m_Paab += (db * (b1 * Caab + b0 * Kaab));
m_Pabb += (da * (a1 * Cabb + a0 * Kabb));
}
m_P1 *= dgFloat32 (0.5f);
m_Pa *= dgFloat32 ( 1.0f / 6.0f);
m_Pb *= dgFloat32 (-1.0f / 6.0f);
m_Paa *= dgFloat32 (1.0f / 12.0f);
m_Pbb *= dgFloat32 (-1.0f / 12.0f);
m_Pab *= dgFloat32 ( 1.0f / 24.0f);
m_Paaa *= dgFloat32 (1.0f / 20.0);
m_Pbbb *= dgFloat32 (-1.0f / 20.0);
m_Paab *= dgFloat32 (1.0f / 60.0);
m_Pabb *= dgFloat32 (-1.0f / 60.0);
}
k1 = dgFloat32 (1.0f) / plane[m_C];
k2 = k1 * k1;
k3 = k2 * k1;
k4 = k3 * k1;
m_Fa = k1 * m_Pa;
m_Fb = k1 * m_Pb;
m_Fc = -k2 * (plane[m_A] * m_Pa + plane[m_B] * m_Pb + plane[3] * m_P1);
m_Faa = k1 * m_Paa;
m_Fbb = k1 * m_Pbb;
m_Fcc = k3 * (SQR(plane[m_A]) * m_Paa + dgFloat32 (2.0f) * plane[m_A] * plane[m_B] * m_Pab +
SQR(plane[m_B]) * m_Pbb + plane[3] * (dgFloat32 (2.0f) *(plane[m_A] * m_Pa + plane[m_B] * m_Pb) + plane[3] * m_P1));
m_Faaa = k1 * m_Paaa;
m_Fbbb = k1 * m_Pbbb;
m_Fccc = -k4 * (CUBE(plane[m_A]) * m_Paaa +
dgFloat32(3.0f) * SQR(plane[m_A]) * plane[m_B] * m_Paab +
dgFloat32(3.0f) * plane[m_A] * SQR(plane[m_B]) * m_Pabb + CUBE(plane[m_B]) * m_Pbbb +
dgFloat32(3.0f) * plane[3] * (SQR(plane[m_A]) * m_Paa + dgFloat32 (2.0f) * plane[m_A] * plane[m_B] * m_Pab + SQR(plane[m_B]) * m_Pbb) +
SQR(plane[3]) * (dgFloat32(3.0f) * (plane[m_A] * m_Pa + plane[m_B] * m_Pb) + plane[3] * m_P1));
m_Faab = k1 * m_Paab;
m_Fbbc = -k2 * (plane[m_A] * m_Pabb + plane[m_B] * m_Pbbb + plane[3] * m_Pbb);
m_Fcca = k3 * (SQR(plane[m_A]) * m_Paaa + dgFloat32 (2.0f) * plane[m_A] * plane[m_B] * m_Paab + SQR(plane[m_B]) * m_Pabb +
plane[3] * (dgFloat32 (2.0f) * (plane[m_A] * m_Paa + plane[m_B] * m_Pab) + plane[3] * m_Pa));
}
m_T0 += (plane[X] * ((m_A == X) ? m_Fa : ((m_B == X) ? m_Fb : m_Fc)));
m_T1[m_A] += (plane[m_A] * m_Faa);
m_T1[m_B] += (plane[m_B] * m_Fbb);
m_T1[m_C] += (plane[m_C] * m_Fcc);
m_T2[m_A] += (plane[m_A] * m_Faaa);
m_T2[m_B] += (plane[m_B] * m_Fbbb);
m_T2[m_C] += (plane[m_C] * m_Fccc);
m_TP[m_A] += (plane[m_A] * m_Faab);
m_TP[m_B] += (plane[m_B] * m_Fbbc);
m_TP[m_C] += (plane[m_C] * m_Fcca);
}
}
dgInt32 m_A; // alpha
dgInt32 m_B; // beta
dgInt32 m_C; // gamma
dgFloat32 m_T0;
dgFloat32 m_T1[3];
dgFloat32 m_T2[3];
dgFloat32 m_TP[3];
dgFloat32 m_P1;
dgFloat32 m_Pa;
dgFloat32 m_Pb;
dgFloat32 m_Paa;
dgFloat32 m_Pbb;
dgFloat32 m_Pab;
dgFloat32 m_Paaa;
dgFloat32 m_Pbbb;
dgFloat32 m_Paab;
dgFloat32 m_Pabb;
dgFloat32 m_Fa;
dgFloat32 m_Fb;
dgFloat32 m_Fc;
dgFloat32 m_Faa;
dgFloat32 m_Fbb;
dgFloat32 m_Fcc;
dgFloat32 m_Faaa;
dgFloat32 m_Fbbb;
dgFloat32 m_Fccc;
dgFloat32 m_Faab;
dgFloat32 m_Fbbc;
dgFloat32 m_Fcca;
};
#endif
dgPolyhedraMassProperties::dgPolyhedraMassProperties()
{
memset (this, 0, sizeof (dgPolyhedraMassProperties));
mult[0] = dgFloat32 (1.0f/6.0f);
mult[1] = dgFloat32 (1.0f/24.0f);
mult[2] = dgFloat32 (1.0f/24.0f);
mult[3] = dgFloat32 (1.0f/24.0f);
mult[4] = dgFloat32 (1.0f/60.0f);
mult[5] = dgFloat32 (1.0f/60.0f);
mult[6] = dgFloat32 (1.0f/60.0f);
mult[7] = dgFloat32 (1.0f/120.0f);
mult[8] = dgFloat32 (1.0f/120.0f);
mult[9] = dgFloat32 (1.0f/120.0f);
}
void dgPolyhedraMassProperties::AddCGFace (dgInt32 indexCount, const dgVector* const faceVertex)
{
#define CDSubexpressions(w0,w1,w2,f1,f2) \
{ \
dgFloat32 temp0 = w0 + w1; \
f1 = temp0 + w2; \
f2 = w0 * w0 + w1 * temp0 + w2 * f1; \
}
const dgVector& p0 = faceVertex[0];
dgVector p1 (faceVertex[1]);
for (dgInt32 i = 2; i < indexCount; i++) {
const dgVector& p2 = faceVertex[i];
dgVector e01 (p1 - p0);
dgVector e02 (p2 - p0);
dgVector d (e01.CrossProduct(e02));
dgVector f1;
dgVector f2;
CDSubexpressions (p0.m_x, p1.m_x, p2.m_x, f1.m_x, f2.m_x);
CDSubexpressions (p0.m_y, p1.m_y, p2.m_y, f1.m_y, f2.m_y);
CDSubexpressions (p0.m_z, p1.m_z, p2.m_z, f1.m_z, f2.m_z);
// update integrals
intg[0] += d[0] * f1.m_x;
intg[1] += d[0] * f2.m_x;
intg[2] += d[1] * f2.m_y;
intg[3] += d[2] * f2.m_z;
p1 = p2;
}
}
void dgPolyhedraMassProperties::AddInertiaFace (dgInt32 indexCount, const dgFloat32* const faceVertex)
{
#define InertiaSubexpression(w0,w1,w2,f1,f2,f3) \
{ \
dgFloat32 temp0 = w0 + w1; \
dgFloat32 temp1 = w0 * w0; \
dgFloat32 temp2 = temp1 + w1 * temp0; \
f1 = temp0 + w2; \
f2 = temp2 + w2 * f1; \
f3 = w0 * temp1 + w1 * temp2 + w2 * f2; \
}
dgVector p0 (faceVertex[0], faceVertex[1], faceVertex[2], 0.0f);
dgVector p1 (faceVertex[3], faceVertex[4], faceVertex[5], 0.0f);
for (dgInt32 i = 2; i < indexCount; i++) {
dgVector p2 (faceVertex[i * 3], faceVertex[i * 3 + 1], faceVertex[i * 3 + 2], 0.0f);
dgVector e01 (p1 - p0);
dgVector e02 (p2 - p0);
dgVector d (e01.CrossProduct(e02));
dgVector f1;
dgVector f2;
dgVector f3;
InertiaSubexpression (p0.m_x, p1.m_x, p2.m_x, f1.m_x, f2.m_x, f3.m_x);
InertiaSubexpression (p0.m_y, p1.m_y, p2.m_y, f1.m_y, f2.m_y, f3.m_y);
InertiaSubexpression (p0.m_z, p1.m_z, p2.m_z, f1.m_z, f2.m_z, f3.m_z);
// update integrals
intg[0] += d[0] * f1.m_x;
intg[1] += d[0] * f2.m_x;
intg[2] += d[1] * f2.m_y;
intg[3] += d[2] * f2.m_z;
intg[4] += d[0] * f3.m_x;
intg[5] += d[1] * f3.m_y;
intg[6] += d[2] * f3.m_z;
p1 = p2;
}
}
void dgPolyhedraMassProperties::AddInertiaAndCrossFace (dgInt32 indexCount, const dgFloat32* const faceVertex)
{
#define Subexpressions(w0,w1,w2,f1,f2,f3,g0,g1,g2) \
{ \
dgFloat32 temp0 = w0 + w1; \
dgFloat32 temp1 = w0 * w0; \
dgFloat32 temp2 = temp1 + w1 * temp0; \
f1 = temp0 + w2; \
f2 = temp2 + w2 * f1; \
f3 = w0 * temp1 + w1 * temp2 + w2 * f2; \
g0 = f2 + w0 * (f1 + w0); \
g1 = f2 + w1 * (f1 + w1); \
g2 = f2 + w2 * (f1 + w2); \
}
dgVector p0 (&faceVertex[0]);
dgVector p1 (&faceVertex[3]);
p0 = p0 & dgVector::m_triplexMask;
p1 = p1 & dgVector::m_triplexMask;
for (dgInt32 i = 2; i < indexCount; i++) {
dgVector p2 (&faceVertex[i * 3]);
p2 = p2 & dgVector::m_triplexMask;
dgVector e01 (p1 - p0);
dgVector e02 (p2 - p0);
dgVector d (e01.CrossProduct(e02));
dgVector f1;
dgVector f2;
dgVector f3;
dgVector g0;
dgVector g1;
dgVector g2;
Subexpressions (p0.m_x, p1.m_x, p2.m_x, f1.m_x, f2.m_x, f3.m_x, g0.m_x, g1.m_x, g2.m_x);
Subexpressions (p0.m_y, p1.m_y, p2.m_y, f1.m_y, f2.m_y, f3.m_y, g0.m_y, g1.m_y, g2.m_y);
Subexpressions (p0.m_z, p1.m_z, p2.m_z, f1.m_z, f2.m_z, f3.m_z, g0.m_z, g1.m_z, g2.m_z);
// update integrals
intg[0] += d[0] * f1.m_x;
intg[1] += d[0] * f2.m_x;
intg[2] += d[1] * f2.m_y;
intg[3] += d[2] * f2.m_z;
intg[4] += d[0] * f3.m_x;
intg[5] += d[1] * f3.m_y;
intg[6] += d[2] * f3.m_z;
intg[7] += d[0] * (p0.m_y * g0.m_x + p1.m_y * g1.m_x + p2.m_y * g2.m_x);
intg[8] += d[1] * (p0.m_z * g0.m_y + p1.m_z * g1.m_y + p2.m_z * g2.m_y);
intg[9] += d[2] * (p0.m_x * g0.m_z + p1.m_x * g1.m_z + p2.m_x * g2.m_z);
p1 = p2;
}
}
dgFloat32 dgPolyhedraMassProperties::MassProperties (dgVector& cg, dgVector& inertia, dgVector& crossInertia)
{
for (dgInt32 i = 0; i < 10; i++) {
intg[i] *= mult[i];
}
cg.m_x = intg[1];
cg.m_y = intg[2];
cg.m_z = intg[3];
cg.m_w = dgFloat32 (0.0f);
inertia.m_x = intg[5] + intg[6];
inertia.m_y = intg[4] + intg[6];
inertia.m_z = intg[4] + intg[5];
inertia.m_w = dgFloat32 (0.0f);
crossInertia.m_x = -intg[8];
crossInertia.m_y = -intg[9];
crossInertia.m_z = -intg[7];
crossInertia.m_w = dgFloat32 (0.0f);
return intg[0];
}

41
thirdparty/src/newton/dgCore/dgPolyhedraMassProperties.h vendored Normal file
View File

@@ -0,0 +1,41 @@
/* Copyright (c) <2003-2019> <Julio Jerez, Newton Game Dynamics>
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
*
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
*
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
*
* 3. This notice may not be removed or altered from any source distribution.
*/
#ifndef __dgPolyhedraMassProperties__
#define __dgPolyhedraMassProperties__
class dgPolyhedraMassProperties
{
public:
dgPolyhedraMassProperties();
void AddCGFace (dgInt32 indexCount, const dgVector* const faceVertex);
void AddInertiaFace (dgInt32 indexCount, const dgFloat32* const faceVertex);
void AddInertiaAndCrossFace (dgInt32 indexCount, const dgFloat32* const faceVertex);
dgFloat32 MassProperties (dgVector& cg, dgVector& inertia, dgVector& crossInertia);
private:
dgFloat32 intg[10];
dgFloat32 mult[10];
};
#endif

23
thirdparty/src/newton/dgCore/dgProfiler.cpp vendored Normal file
View File

@@ -0,0 +1,23 @@
/* Copyright (c) <2003-2019> <Julio Jerez, Newton Game Dynamics>
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
*
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
*
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
*
* 3. This notice may not be removed or altered from any source distribution.
*/
#include "dgStdafx.h"
#include "dgProfiler.h"

Some files were not shown because too many files have changed in this diff Show More