diff --git a/LICENSE-Kiwi b/LICENSE-Kiwi new file mode 100644 index 000000000..c34aff71c --- /dev/null +++ b/LICENSE-Kiwi @@ -0,0 +1,71 @@ +========================= + The Kiwi licensing terms +========================= +Kiwi is licensed under the terms of the Modified BSD License (also known as +New or Revised BSD), as follows: + +Copyright (c) 2013, Nucleic Development Team + +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +Redistributions of source code must retain the above copyright notice, this +list of conditions and the following disclaimer. + +Redistributions in binary form must reproduce the above copyright notice, this +list of conditions and the following disclaimer in the documentation and/or +other materials provided with the distribution. + +Neither the name of the Nucleic Development Team nor the names of its +contributors may be used to endorse or promote products derived from this +software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +About Kiwi +---------- +Chris Colbert began the Kiwi project in December 2013 in an effort to +create a blisteringly fast UI constraint solver. Chris is still the +project lead. + +The Nucleic Development Team is the set of all contributors to the Nucleic +project and its subprojects. + +The core team that coordinates development on GitHub can be found here: +http://github.com/nucleic. The current team consists of: + +* Chris Colbert + +Our Copyright Policy +-------------------- +Nucleic uses a shared copyright model. Each contributor maintains copyright +over their contributions to Nucleic. But, it is important to note that these +contributions are typically only changes to the repositories. Thus, the Nucleic +source code, in its entirety is not the copyright of any single person or +institution. Instead, it is the collective copyright of the entire Nucleic +Development Team. If individual contributors want to maintain a record of what +changes/contributions they have specific copyright on, they should indicate +their copyright in the commit message of the change, when they commit the +change to one of the Nucleic repositories. + +With this in mind, the following banner should be used in any source code file +to indicate the copyright and license terms: + +#------------------------------------------------------------------------------ +# Copyright (c) 2013, Nucleic Development Team. +# +# Distributed under the terms of the Modified BSD License. +# +# The full license is in the file LICENSE, distributed with this software. +#------------------------------------------------------------------------------ diff --git a/appveyor.yml b/appveyor.yml index 544370634..c3c44154d 100644 --- a/appveyor.yml +++ b/appveyor.yml @@ -19,11 +19,11 @@ skip_commits: message: /\[Posix\]/ os: - - Visual Studio 2017 + - Visual Studio 2019 environment: matrix: - - TOOLSET: vs2017 + - TOOLSET: vs2019 install: - cd build && "./premake5.exe" %TOOLSET% && cd .. diff --git a/build/scripts/common.lua b/build/scripts/common.lua index 1c68dff88..2e7e60207 100644 --- a/build/scripts/common.lua +++ b/build/scripts/common.lua @@ -126,6 +126,10 @@ function NazaraBuild:Execute() end filter({}) + + if (libTable.Custom) then + libTable.Custom() + end self:PostconfigGenericProject() end @@ -186,6 +190,10 @@ function NazaraBuild:Execute() end filter({}) + + if (moduleTable.Custom) then + moduleTable.Custom() + end if (not _OPTIONS["united"]) then self:PostconfigNazaraProject() @@ -267,6 +275,10 @@ function NazaraBuild:Execute() filter({}) self:PostconfigNazaraProject() + + if (toolTable.Custom) then + toolTable.Custom() + end end group("Examples") @@ -321,6 +333,10 @@ function NazaraBuild:Execute() filter({}) + if (exampleTable.Custom) then + exampleTable.Custom() + end + self:PostconfigNazaraProject() end end diff --git a/build/scripts/modules/physics3d.lua b/build/scripts/modules/physics3d.lua index 0753a5f2b..d8b9ea352 100644 --- a/build/scripts/modules/physics3d.lua +++ b/build/scripts/modules/physics3d.lua @@ -1,10 +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.DynLib = { - "Newton" -} +MODULE.Custom = function() + vectorextensions("SSE3") + + filter({"architecture:x86_64", "system:linux"}) + defines("_POSIX_VER_64") + + filter({"architecture:x86", "system:linux"}) + defines("_POSIX_VER") +end diff --git a/include/Nazara/Math/Angle.hpp b/include/Nazara/Math/Angle.hpp index a8ec44641..7afb54dcc 100644 --- a/include/Nazara/Math/Angle.hpp +++ b/include/Nazara/Math/Angle.hpp @@ -27,8 +27,9 @@ namespace Nz public: Angle() = default; Angle(T angle); + Angle(const Angle& angle); + Angle(const Angle& angle); template explicit Angle(const Angle& Angle); - Angle(const Angle&) = default; ~Angle() = default; T GetCos() const; @@ -51,9 +52,6 @@ namespace Nz Angle ToRadianAngle() const; String ToString() const; - template> operator Angle() const { return ToDegreeAngle(); } // GCC < 8 bug - template> operator Angle() const { return ToRadianAngle(); } // GCC < 8 bug - Angle& operator=(const Angle&) = default; Angle operator+(const Angle& other) const; diff --git a/include/Nazara/Math/Angle.inl b/include/Nazara/Math/Angle.inl index 9779b0b53..6b04f12c3 100644 --- a/include/Nazara/Math/Angle.inl +++ b/include/Nazara/Math/Angle.inl @@ -155,6 +155,28 @@ namespace Nz { } + /*! + * \brief Constructs an Angle object from a angle in degrees, converting if required + * + * \param value Angle object to copy + */ + template + Angle::Angle(const Angle& angle) : + value(Detail::AngleUtils::FromDegrees(angle.value)) + { + } + + /*! + * \brief Constructs an Angle object from a angle in radians, converting if required + * + * \param value Angle object to copy + */ + template + Angle::Angle(const Angle& angle) : + value(Detail::AngleUtils::FromRadians(angle.value)) + { + } + /*! * \brief Computes the cosine of the angle * \return Cosine of angle diff --git a/include/Nazara/Network/AbstractSocket.hpp b/include/Nazara/Network/AbstractSocket.hpp index 74f5d6334..8fa10459d 100644 --- a/include/Nazara/Network/AbstractSocket.hpp +++ b/include/Nazara/Network/AbstractSocket.hpp @@ -22,7 +22,7 @@ namespace Nz AbstractSocket(AbstractSocket&& abstractSocket) noexcept; virtual ~AbstractSocket(); - void Close(); + void Close() noexcept; void EnableBlocking(bool blocking); @@ -41,7 +41,7 @@ namespace Nz void SetSendBufferSize(std::size_t size); AbstractSocket& operator=(const AbstractSocket&) = delete; - AbstractSocket& operator=(AbstractSocket&& abstractSocket); + AbstractSocket& operator=(AbstractSocket&& abstractSocket) noexcept; // Signals: NazaraSignal(OnStateChanged, const AbstractSocket* /*socket*/, SocketState /*oldState*/, SocketState /*newState*/); diff --git a/include/Nazara/Network/UdpSocket.hpp b/include/Nazara/Network/UdpSocket.hpp index 6fda01a23..e67416427 100644 --- a/include/Nazara/Network/UdpSocket.hpp +++ b/include/Nazara/Network/UdpSocket.hpp @@ -46,6 +46,9 @@ namespace Nz bool SendMultiple(const IpAddress& to, const NetBuffer* buffers, std::size_t bufferCount, std::size_t* sent); bool SendPacket(const IpAddress& to, const NetPacket& packet); + UdpSocket& operator=(const UdpSocket& udpSocket) = delete; + UdpSocket& operator=(UdpSocket && udpSocket) noexcept = default; + private: void OnClose() override; void OnOpened() override; diff --git a/include/Nazara/Physics3D/PhysWorld3D.hpp b/include/Nazara/Physics3D/PhysWorld3D.hpp index f985b0cc6..190b37aa4 100644 --- a/include/Nazara/Physics3D/PhysWorld3D.hpp +++ b/include/Nazara/Physics3D/PhysWorld3D.hpp @@ -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> m_callbacks; diff --git a/include/NazaraSDK/Components/CollisionComponent2D.hpp b/include/NazaraSDK/Components/CollisionComponent2D.hpp index d2ea33db8..d10dec8af 100644 --- a/include/NazaraSDK/Components/CollisionComponent2D.hpp +++ b/include/NazaraSDK/Components/CollisionComponent2D.hpp @@ -35,11 +35,11 @@ namespace Ndk void Recenter(const Nz::Vector2f& origin); - void SetGeom(Nz::Collider2DRef geom); + void SetGeom(Nz::Collider2DRef geom, bool recomputeMoment = true, bool recomputeMassCenter = true); void SetGeomOffset(const Nz::Vector2f& geomOffset); CollisionComponent2D& operator=(Nz::Collider2DRef geom); - CollisionComponent2D& operator=(CollisionComponent2D&& collision) = default; + CollisionComponent2D& operator=(CollisionComponent2D&& collision) = delete; static ComponentIndex componentIndex; diff --git a/include/NazaraSDK/Components/CollisionComponent3D.hpp b/include/NazaraSDK/Components/CollisionComponent3D.hpp index c8f832a50..7994232a8 100644 --- a/include/NazaraSDK/Components/CollisionComponent3D.hpp +++ b/include/NazaraSDK/Components/CollisionComponent3D.hpp @@ -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; diff --git a/src/Nazara/Network/AbstractSocket.cpp b/src/Nazara/Network/AbstractSocket.cpp index 321df9034..d61dfd4b2 100644 --- a/src/Nazara/Network/AbstractSocket.cpp +++ b/src/Nazara/Network/AbstractSocket.cpp @@ -71,7 +71,7 @@ namespace Nz * \brief Closes the socket */ - void AbstractSocket::Close() + void AbstractSocket::Close() noexcept { if (m_handle != SocketImpl::InvalidHandle) { @@ -237,7 +237,7 @@ namespace Nz * \param abstractSocket AbstractSocket to move in this */ - AbstractSocket& AbstractSocket::operator=(AbstractSocket&& abstractSocket) + AbstractSocket& AbstractSocket::operator=(AbstractSocket&& abstractSocket) noexcept { Close(); diff --git a/src/Nazara/Physics2D/RigidBody2D.cpp b/src/Nazara/Physics2D/RigidBody2D.cpp index 130194257..2b113d96b 100644 --- a/src/Nazara/Physics2D/RigidBody2D.cpp +++ b/src/Nazara/Physics2D/RigidBody2D.cpp @@ -695,6 +695,8 @@ namespace Nz cpBodySetVelocity(to, cpBodyGetVelocity(from)); cpBodySetType(to, cpBodyGetType(from)); + + to->velocity_func = from->velocity_func; } void RigidBody2D::CopyShapeData(cpShape* from, cpShape* to) diff --git a/src/Nazara/Physics3D/Collider3D.cpp b/src/Nazara/Physics3D/Collider3D.cpp index bcb048e2d..800caaa9b 100644 --- a/src/Nazara/Physics3D/Collider3D.cpp +++ b/src/Nazara/Physics3D/Collider3D.cpp @@ -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 **********************************/ diff --git a/src/Nazara/Physics3D/PhysWorld3D.cpp b/src/Nazara/Physics3D/PhysWorld3D.cpp index 94e0c0ab7..34214a96f 100644 --- a/src/Nazara/Physics3D/PhysWorld3D.cpp +++ b/src/Nazara/Physics3D/PhysWorld3D.cpp @@ -93,11 +93,6 @@ namespace Nz m_maxStepCount = maxStepCount; } - void PhysWorld3D::SetSolverModel(unsigned int model) - { - NewtonSetSolverModel(m_world, model); - } - void PhysWorld3D::SetStepSize(float stepSize) { m_stepSize = stepSize; @@ -116,7 +111,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); @@ -163,17 +159,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(NewtonBodyGetUserData(body0)); - RigidBody3D* bodyB = static_cast(NewtonBodyGetUserData(body1)); + RigidBody3D* bodyA = static_cast(NewtonBodyGetUserData(NewtonJointGetBody0(contactJoint))); + RigidBody3D* bodyB = static_cast(NewtonBodyGetUserData(NewtonJointGetBody1(contactJoint))); assert(bodyA && bodyB); - Callback* callbackData = static_cast(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 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(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) diff --git a/src/Nazara/Physics3D/RigidBody3D.cpp b/src/Nazara/Physics3D/RigidBody3D.cpp index 67ef690fd..945dad5a9 100644 --- a/src/Nazara/Physics3D/RigidBody3D.cpp +++ b/src/Nazara/Physics3D/RigidBody3D.cpp @@ -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); diff --git a/src/NazaraSDK/Components/CollisionComponent2D.cpp b/src/NazaraSDK/Components/CollisionComponent2D.cpp index c45a47118..ced3d8053 100644 --- a/src/NazaraSDK/Components/CollisionComponent2D.cpp +++ b/src/NazaraSDK/Components/CollisionComponent2D.cpp @@ -53,11 +53,11 @@ namespace Ndk * * \param geom Geometry used for collisions */ - void CollisionComponent2D::SetGeom(Nz::Collider2DRef geom) + void CollisionComponent2D::SetGeom(Nz::Collider2DRef geom, bool recomputeMoment, bool recomputeMassCenter) { m_geom = std::move(geom); - GetRigidBody()->SetGeom(m_geom); + GetRigidBody()->SetGeom(m_geom, recomputeMoment, recomputeMassCenter); } /*! diff --git a/src/NazaraSDK/Components/PhysicsComponent2D.cpp b/src/NazaraSDK/Components/PhysicsComponent2D.cpp index d932c3de7..91d610eb4 100644 --- a/src/NazaraSDK/Components/PhysicsComponent2D.cpp +++ b/src/NazaraSDK/Components/PhysicsComponent2D.cpp @@ -77,7 +77,7 @@ namespace Ndk if (IsComponent(component)) { NazaraAssert(m_object, "Invalid object"); - m_object->SetGeom(static_cast(component).GetGeom()); + m_object->SetGeom(static_cast(component).GetGeom(), false, false); } } @@ -94,7 +94,7 @@ namespace Ndk if (IsComponent(component)) { NazaraAssert(m_object, "Invalid object"); - m_object->SetGeom(Nz::NullCollider2D::New()); + m_object->SetGeom(Nz::NullCollider2D::New(), false, false); } } diff --git a/thirdparty/build/lua.lua b/thirdparty/build/lua.lua index 76281076a..df66e0127 100644 --- a/thirdparty/build/lua.lua +++ b/thirdparty/build/lua.lua @@ -9,10 +9,6 @@ LIBRARY.Files = { "../thirdparty/src/Lua/*.cpp" } -LIBRARY.OsDefines.Windows = { - "LUA_USE_WINDOWS" -} - LIBRARY.OsDefines.Posix = { "LUA_USE_LINUX" } diff --git a/thirdparty/build/newton.lua b/thirdparty/build/newton.lua new file mode 100644 index 000000000..d9f9d4d20 --- /dev/null +++ b/thirdparty/build/newton.lua @@ -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 \ No newline at end of file diff --git a/thirdparty/include/Newton/Newton.h b/thirdparty/include/Newton/Newton.h index ab85cba99..4e409b34b 100644 --- a/thirdparty/include/Newton/Newton.h +++ b/thirdparty/include/Newton/Newton.h @@ -1,4 +1,4 @@ -/* Copyright (c) <2003-2011> +/* Copyright (c) <2003-2019> * * This software is provided 'as-is', without any express or implied * warranty. In no event will the authors be held liable for any damages @@ -24,35 +24,16 @@ #define NEWTON_MAJOR_VERSION 3 -#define NEWTON_MINOR_VERSION 13 +#define NEWTON_MINOR_VERSION 14 +#include #ifdef _NEWTON_STATIC_LIB - #define NEWTON_API -#else - #ifdef _NEWTON_BUILD_DLL - #ifdef _WIN32 - #define NEWTON_API __declspec (dllexport) - #else - #define NEWTON_API __attribute__ ((visibility("default"))) - #endif - #else - #ifdef _WIN32 - #define NEWTON_API __declspec (dllimport) - #else - #define NEWTON_API - #endif - #endif -#endif - - -#ifdef __GNUC__ -# define NEWTON_DEPRECATED_API __attribute__((deprecated)) -#elif defined(_MSC_VER) -# define NEWTON_DEPRECATED_API __declspec(deprecated) + #define NEWTON_API DG_LIBRARY_STATIC +#elif defined(_NEWTON_BUILD_DLL) + #define NEWTON_API DG_LIBRARY_EXPORT #else -# warning NEWTON_DEPRECATED_API not implemented for this compiler -# define NEWTON_DEPRECATED_API + #define NEWTON_API DG_LIBRARY_IMPORT #endif @@ -68,11 +49,14 @@ #endif #endif +#ifndef dFloat32 + #define dFloat32 float +#endif + #ifndef dFloat64 #define dFloat64 double #endif - #ifdef __cplusplus extern "C" { #endif @@ -82,26 +66,25 @@ extern "C" { #define NEWTON_DYNAMIC_BODY 0 #define NEWTON_KINEMATIC_BODY 1 - #define NEWTON_DEFORMABLE_BODY 2 + #define NEWTON_DYNAMIC_ASYMETRIC_BODY 2 +// #define NEWTON_DEFORMABLE_BODY 2 #define SERIALIZE_ID_SPHERE 0 #define SERIALIZE_ID_CAPSULE 1 - #define SERIALIZE_ID_CHAMFERCYLINDER 2 - #define SERIALIZE_ID_TAPEREDCAPSULE 3 - #define SERIALIZE_ID_CYLINDER 4 - #define SERIALIZE_ID_TAPEREDCYLINDER 5 - #define SERIALIZE_ID_BOX 6 - #define SERIALIZE_ID_CONE 7 - #define SERIALIZE_ID_CONVEXHULL 8 - #define SERIALIZE_ID_NULL 9 - #define SERIALIZE_ID_COMPOUND 10 - #define SERIALIZE_ID_TREE 11 - #define SERIALIZE_ID_HEIGHTFIELD 12 - #define SERIALIZE_ID_CLOTH_PATCH 13 - #define SERIALIZE_ID_DEFORMABLE_SOLID 14 - #define SERIALIZE_ID_USERMESH 15 - #define SERIALIZE_ID_SCENE 16 - #define SERIALIZE_ID_FRACTURED_COMPOUND 17 + #define SERIALIZE_ID_CYLINDER 2 + #define SERIALIZE_ID_CHAMFERCYLINDER 3 + #define SERIALIZE_ID_BOX 4 + #define SERIALIZE_ID_CONE 5 + #define SERIALIZE_ID_CONVEXHULL 6 + #define SERIALIZE_ID_NULL 7 + #define SERIALIZE_ID_COMPOUND 8 + #define SERIALIZE_ID_TREE 9 + #define SERIALIZE_ID_HEIGHTFIELD 10 + #define SERIALIZE_ID_CLOTH_PATCH 11 + #define SERIALIZE_ID_DEFORMABLE_SOLID 12 + #define SERIALIZE_ID_USERMESH 13 + #define SERIALIZE_ID_SCENE 14 + #define SERIALIZE_ID_FRACTURED_COMPOUND 15 #ifdef __cplusplus class NewtonMesh; @@ -110,7 +93,6 @@ extern "C" { class NewtonJoint; class NewtonMaterial; class NewtonCollision; - class NewtonAcyclicArticulation; class NewtonDeformableMeshSegment; class NewtonFracturedCompoundMeshPart; #else @@ -120,11 +102,23 @@ extern "C" { typedef struct NewtonJoint{} NewtonJoint; typedef struct NewtonMaterial{} NewtonMaterial; typedef struct NewtonCollision{} NewtonCollision; - typedef struct NewtonAcyclicArticulation{} NewtonAcyclicArticulation; typedef struct NewtonDeformableMeshSegment{} NewtonDeformableMeshSegment; typedef struct NewtonFracturedCompoundMeshPart{} NewtonFracturedCompoundMeshPart; #endif + typedef union + { + void* m_ptr; + dLong m_int; + dFloat m_float; + } NewtonMaterialData; + + typedef struct NewtonCollisionMaterial + { + dLong m_userId; + NewtonMaterialData m_userData; + NewtonMaterialData m_userParam[6]; + } NewtonCollisionMaterial; typedef struct NewtonBoxParam { @@ -138,39 +132,27 @@ extern "C" { dFloat m_radio; } NewtonSphereParam; - typedef struct NewtonCylinderParam - { - dFloat m_radio; - dFloat m_height; - } NewtonCylinderParam; typedef struct NewtonCapsuleParam { - dFloat m_radio; + dFloat m_radio0; + dFloat m_radio1; dFloat m_height; } NewtonCapsuleParam; + typedef struct NewtonCylinderParam + { + dFloat m_radio0; + dFloat m_radio1; + dFloat m_height; + } NewtonCylinderParam; + typedef struct NewtonConeParam { dFloat m_radio; dFloat m_height; } NewtonConeParam; - typedef struct NewtonTaperedCapsuleParam - { - dFloat m_radio0; - dFloat m_radio1; - dFloat m_height; - } NewtonTaperedCapsuleParam; - - - typedef struct NewtonTaperedCylinderParam - { - dFloat m_radio0; - dFloat m_radio1; - dFloat m_height; - } NewtonTaperedCylinderParam; - typedef struct NewtonChamferCylinderParam { dFloat m_radio; @@ -211,9 +193,10 @@ extern "C" { int m_height; int m_gridsDiagonals; int m_elevationDataType; // 0 = 32 bit floats, 1 = unsigned 16 bit integers - dFloat m_horizonalScale; dFloat m_verticalScale; - void* m_elevation; + dFloat m_horizonalScale_x; + dFloat m_horizonalScale_z; + void* m_vertialElevation; char* m_atributes; } NewtonHeightFieldCollisionParam; @@ -225,17 +208,14 @@ extern "C" { typedef struct NewtonCollisionInfoRecord { dFloat m_offsetMatrix[4][4]; + NewtonCollisionMaterial m_collisionMaterial; int m_collisionType; // tag id to identify the collision primitive - int m_collisionUserID; - union { NewtonBoxParam m_box; NewtonConeParam m_cone; NewtonSphereParam m_sphere; NewtonCapsuleParam m_capsule; NewtonCylinderParam m_cylinder; - NewtonTaperedCapsuleParam m_taperedCapsule; - NewtonTaperedCylinderParam m_taperedCylinder; NewtonChamferCylinderParam m_chamferCylinder; NewtonConvexHullParam m_convexHull; NewtonDeformableMeshParam m_deformableMesh; @@ -317,25 +297,72 @@ extern "C" { dFloat m_timestep; } NewtonHingeSliderUpdateDesc; - typedef struct NewtonClothPatchMaterial + typedef struct NewtonUserContactPoint { - dFloat m_damper; - dFloat m_stiffness; - } NewtonClothPatchMaterial; + dFloat m_point[4]; + dFloat m_normal[4]; + dLong m_shapeId0; + dLong m_shapeId1; + dFloat m_penetration; + int m_unused[3]; + } NewtonUserContactPoint; + typedef struct NewtonImmediateModeConstraint + { + dFloat m_jacobian01[8][6]; + dFloat m_jacobian10[8][6]; + dFloat m_minFriction[8]; + dFloat m_maxFriction[8]; + dFloat m_jointAccel[8]; + dFloat m_jointStiffness[8]; + } NewtonConstraintDescriptor; + + + // data structure for interfacing with NewtonMesh + typedef struct NewtonMeshDoubleData + { + dFloat64* m_data; + int* m_indexList; + int m_strideInBytes; + } NewtonMeshDoubleData; + + typedef struct NewtonMeshFloatData + { + dFloat* m_data; + int* m_indexList; + int m_strideInBytes; + } NewtonMeshFloatData; + + typedef struct NewtonMeshVertexFormat + { + int m_faceCount; + int* m_faceIndexCount; + int* m_faceMaterial; + NewtonMeshDoubleData m_vertex; + NewtonMeshFloatData m_normal; + NewtonMeshFloatData m_binormal; + NewtonMeshFloatData m_uv0; + NewtonMeshFloatData m_uv1; + NewtonMeshFloatData m_vertexColor; + } NewtonMeshVertexFormat; + // Newton callback functions typedef void* (*NewtonAllocMemory) (int sizeInBytes); typedef void (*NewtonFreeMemory) (void* const ptr, int sizeInBytes); - typedef void (*NewtonWorldDestructorCallback) (const NewtonWorld* const world); + typedef void (*NewtonPostUpdateCallback) (const NewtonWorld* const world, dFloat timestep); + typedef void(*NewtonCreateContactCallback) (const NewtonWorld* const newtonWorld, NewtonJoint* const contact); + typedef void(*NewtonDestroyContactCallback) (const NewtonWorld* const newtonWorld, NewtonJoint* const contact); + + typedef void (*NewtonWorldListenerDebugCallback) (const NewtonWorld* const world, void* const listener, void* const debugContext); typedef void (*NewtonWorldListenerBodyDestroyCallback) (const NewtonWorld* const world, void* const listenerUserData, NewtonBody* const body); typedef void (*NewtonWorldUpdateListenerCallback) (const NewtonWorld* const world, void* const listenerUserData, dFloat timestep); typedef void (*NewtonWorldDestroyListenerCallback) (const NewtonWorld* const world, void* const listenerUserData); - typedef unsigned (*NewtonGetTicksCountCallback) (); + typedef dLong (*NewtonGetTimeInMicrosencondsCallback) (); typedef void (*NewtonSerializeCallback) (void* const serializeHandle, const void* const buffer, int size); typedef void (*NewtonDeserializeCallback) (void* const serializeHandle, void* const buffer, int size); @@ -382,11 +409,11 @@ extern "C" { typedef unsigned (*NewtonWorldRayPrefilterCallback)(const NewtonBody* const body, const NewtonCollision* const collision, void* const userData); typedef dFloat (*NewtonWorldRayFilterCallback)(const NewtonBody* const body, const NewtonCollision* const shapeHit, const dFloat* const hitContact, const dFloat* const hitNormal, dLong collisionID, void* const userData, dFloat intersectParam); - + typedef int (*NewtonOnAABBOverlap) (const NewtonJoint* const contact, dFloat timestep, int threadIndex); typedef void (*NewtonContactsProcess) (const NewtonJoint* const contact, dFloat timestep, int threadIndex); - typedef int (*NewtonOnAABBOverlap) (const NewtonMaterial* const material, const NewtonBody* const body0, const NewtonBody* const body1, int threadIndex); - typedef int (*NewtonOnCompoundSubCollisionAABBOverlap) (const NewtonMaterial* const material, const NewtonBody* const body0, const void* const collsionNode0, const NewtonBody* const body1, const void* const collsionNode1, int threadIndex); + typedef int (*NewtonOnCompoundSubCollisionAABBOverlap) (const NewtonJoint* const contact, dFloat timestep, const NewtonBody* const body0, const void* const collisionNode0, const NewtonBody* const body1, const void* const collisionNode1, int threadIndex); + typedef int (*NewtonOnContactGeneration) (const NewtonMaterial* const material, const NewtonBody* const body0, const NewtonCollision* const collision0, const NewtonBody* const body1, const NewtonCollision* const collision1, NewtonUserContactPoint* const contactBuffer, int maxCount, int threadIndex); typedef int (*NewtonBodyIterator) (const NewtonBody* const body, void* const userData); typedef void (*NewtonJointIterator) (const NewtonJoint* const joint, void* const userData); @@ -404,9 +431,7 @@ extern "C" { typedef void (*NewtonConstraintDestructor) (const NewtonJoint* const me); typedef void (*NewtonJobTask) (NewtonWorld* const world, void* const userData, int threadIndex); - - typedef bool (*NewtonReportProgress) (dFloat normalizedProgressPercent, void* const userData); - + typedef int (*NewtonReportProgress) (dFloat normalizedProgressPercent, void* const userData); // ********************************************************************************************** // @@ -423,106 +448,105 @@ extern "C" { NEWTON_API void NewtonDestroy (const NewtonWorld* const newtonWorld); NEWTON_API void NewtonDestroyAllBodies (const NewtonWorld* const newtonWorld); + NEWTON_API NewtonPostUpdateCallback NewtonGetPostUpdateCallback(const NewtonWorld* const newtonWorld); + NEWTON_API void NewtonSetPostUpdateCallback (const NewtonWorld* const newtonWorld, NewtonPostUpdateCallback callback); + NEWTON_API void* NewtonAlloc (int sizeInBytes); NEWTON_API void NewtonFree (void* const ptr); - // NEWTON_API void NewtonSetPlatformArchitecture (const NewtonWorld* const newtonWorld, int mode); - // NEWTON_API int NewtonGetPlatformArchitecture(const NewtonWorld* const newtonWorld, char* description); - - NEWTON_API int NewtonEnumrateDevices (const NewtonWorld* const newtonWorld); - NEWTON_API int NewtonGetCurrentDevice (const NewtonWorld* const newtonWorld); - NEWTON_API void NewtonSetCurrentDevice (const NewtonWorld* const newtonWorld, int deviceIndex); - NEWTON_API void NewtonGetDeviceString (const NewtonWorld* const newtonWorld, int deviceIndex, char* const vendorString, int maxSize); + NEWTON_API void NewtonLoadPlugins(const NewtonWorld* const newtonWorld, const char* const plugInPath); + NEWTON_API void NewtonUnloadPlugins(const NewtonWorld* const newtonWorld); + NEWTON_API void* NewtonCurrentPlugin(const NewtonWorld* const newtonWorld); + NEWTON_API void* NewtonGetFirstPlugin(const NewtonWorld* const newtonWorld); + NEWTON_API void* NewtonGetPreferedPlugin(const NewtonWorld* const newtonWorld); + NEWTON_API void* NewtonGetNextPlugin(const NewtonWorld* const newtonWorld, const void* const plugin); + NEWTON_API const char* NewtonGetPluginString(const NewtonWorld* const newtonWorld, const void* const plugin); + NEWTON_API void NewtonSelectPlugin(const NewtonWorld* const newtonWorld, const void* const plugin); NEWTON_API dFloat NewtonGetContactMergeTolerance (const NewtonWorld* const newtonWorld); NEWTON_API void NewtonSetContactMergeTolerance (const NewtonWorld* const newtonWorld, dFloat tolerance); NEWTON_API void NewtonInvalidateCache (const NewtonWorld* const newtonWorld); - NEWTON_API void NewtonSetSolverModel (const NewtonWorld* const newtonWorld, int model); - NEWTON_API void NewtonSetMultiThreadSolverOnSingleIsland (const NewtonWorld* const newtonWorld, int mode); - NEWTON_API int NewtonGetMultiThreadSolverOnSingleIsland (const NewtonWorld* const newtonWorld); + NEWTON_API void NewtonSetSolverIterations (const NewtonWorld* const newtonWorld, int model); + NEWTON_API int NewtonGetSolverIterations(const NewtonWorld* const newtonWorld); - //NEWTON_API void NewtonSetPerformanceClock (const NewtonWorld* const newtonWorld, NewtonGetTicksCountCallback callback); - //NEWTON_API unsigned NewtonReadPerformanceTicks (const NewtonWorld* const newtonWorld, unsigned performanceEntry); - //NEWTON_API unsigned NewtonReadThreadPerformanceTicks (const NewtonWorld* newtonWorld, unsigned threadIndex); + NEWTON_API void NewtonSetParallelSolverOnLargeIsland (const NewtonWorld* const newtonWorld, int mode); + NEWTON_API int NewtonGetParallelSolverOnLargeIsland (const NewtonWorld* const newtonWorld); NEWTON_API int NewtonGetBroadphaseAlgorithm (const NewtonWorld* const newtonWorld); NEWTON_API void NewtonSelectBroadphaseAlgorithm (const NewtonWorld* const newtonWorld, int algorithmType); + NEWTON_API void NewtonResetBroadphase(const NewtonWorld* const newtonWorld); NEWTON_API void NewtonUpdate (const NewtonWorld* const newtonWorld, dFloat timestep); NEWTON_API void NewtonUpdateAsync (const NewtonWorld* const newtonWorld, dFloat timestep); NEWTON_API void NewtonWaitForUpdateToFinish (const NewtonWorld* const newtonWorld); + NEWTON_API int NewtonGetNumberOfSubsteps (const NewtonWorld* const newtonWorld); + NEWTON_API void NewtonSetNumberOfSubsteps (const NewtonWorld* const newtonWorld, int subSteps); + NEWTON_API dFloat NewtonGetLastUpdateTime (const NewtonWorld* const newtonWorld); + NEWTON_API void NewtonSerializeToFile (const NewtonWorld* const newtonWorld, const char* const filename, NewtonOnBodySerializationCallback bodyCallback, void* const bodyUserData); NEWTON_API void NewtonDeserializeFromFile (const NewtonWorld* const newtonWorld, const char* const filename, NewtonOnBodyDeserializationCallback bodyCallback, void* const bodyUserData); + NEWTON_API void NewtonSerializeScene(const NewtonWorld* const newtonWorld, NewtonOnBodySerializationCallback bodyCallback, void* const bodyUserData, + NewtonSerializeCallback serializeCallback, void* const serializeHandle); + NEWTON_API void NewtonDeserializeScene(const NewtonWorld* const newtonWorld, NewtonOnBodyDeserializationCallback bodyCallback, void* const bodyUserData, + NewtonDeserializeCallback serializeCallback, void* const serializeHandle); + + NEWTON_API NewtonBody* NewtonFindSerializedBody(const NewtonWorld* const newtonWorld, int bodySerializedID); NEWTON_API void NewtonSetJointSerializationCallbacks (const NewtonWorld* const newtonWorld, NewtonOnJointSerializationCallback serializeJoint, NewtonOnJointDeserializationCallback deserializeJoint); NEWTON_API void NewtonGetJointSerializationCallbacks (const NewtonWorld* const newtonWorld, NewtonOnJointSerializationCallback* const serializeJoint, NewtonOnJointDeserializationCallback* const deserializeJoint); - - // multi threading interface NEWTON_API void NewtonWorldCriticalSectionLock (const NewtonWorld* const newtonWorld, int threadIndex); NEWTON_API void NewtonWorldCriticalSectionUnlock (const NewtonWorld* const newtonWorld); NEWTON_API void NewtonSetThreadsCount (const NewtonWorld* const newtonWorld, int threads); NEWTON_API int NewtonGetThreadsCount(const NewtonWorld* const newtonWorld); NEWTON_API int NewtonGetMaxThreadsCount(const NewtonWorld* const newtonWorld); - NEWTON_API void NewtonDispachThreadJob(const NewtonWorld* const newtonWorld, NewtonJobTask task, void* const usedData); + NEWTON_API void NewtonDispachThreadJob(const NewtonWorld* const newtonWorld, NewtonJobTask task, void* const usedData, const char* const functionName); NEWTON_API void NewtonSyncThreadJobs(const NewtonWorld* const newtonWorld); // atomic operations NEWTON_API int NewtonAtomicAdd (int* const ptr, int value); NEWTON_API int NewtonAtomicSwap (int* const ptr, int value); NEWTON_API void NewtonYield (); - - NEWTON_API void NewtonSetFrictionModel (const NewtonWorld* const newtonWorld, int model); - NEWTON_API void NewtonSetMinimumFrameRate (const NewtonWorld* const newtonWorld, dFloat frameRate); NEWTON_API void NewtonSetIslandUpdateEvent (const NewtonWorld* const newtonWorld, NewtonIslandUpdate islandUpdate); -// NEWTON_API void NewtonSetDestroyBodyByExeciveForce (const NewtonWorld* const newtonWorld, NewtonDestroyBodyByExeciveForce callback); -// NEWTON_API void NewtonWorldForEachBodyDo (const NewtonWorld* const newtonWorld, NewtonBodyIterator callback); NEWTON_API void NewtonWorldForEachJointDo (const NewtonWorld* const newtonWorld, NewtonJointIterator callback, void* const userData); NEWTON_API void NewtonWorldForEachBodyInAABBDo (const NewtonWorld* const newtonWorld, const dFloat* const p0, const dFloat* const p1, NewtonBodyIterator callback, void* const userData); - NEWTON_API void NewtonWorldSetUserData (const NewtonWorld* const newtonWorld, void* const userData); NEWTON_API void* NewtonWorldGetUserData (const NewtonWorld* const newtonWorld); - - - NEWTON_API void* NewtonWorldGetListenerUserData (const NewtonWorld* const newtonWorld, void* const listener); - NEWTON_API NewtonWorldListenerBodyDestroyCallback NewtonWorldListenerGetBodyDestroyCallback (const NewtonWorld* const newtonWorld, void* const listener); - NEWTON_API void NewtonWorldListenerSetBodyDestroyCallback (const NewtonWorld* const newtonWorld, void* const listener, NewtonWorldListenerBodyDestroyCallback bodyDestroyCallback); - - NEWTON_API void* NewtonWorldGetPreListener (const NewtonWorld* const newtonWorld, const char* const nameId); - NEWTON_API void* NewtonWorldAddPreListener (const NewtonWorld* const newtonWorld, const char* const nameId, void* const listenerUserData, NewtonWorldUpdateListenerCallback update, NewtonWorldDestroyListenerCallback destroy); - NEWTON_API void* NewtonWorldGetPostListener (const NewtonWorld* const newtonWorld, const char* const nameId); - NEWTON_API void* NewtonWorldAddPostListener (const NewtonWorld* const newtonWorld, const char* const nameId, void* const listenerUserData, NewtonWorldUpdateListenerCallback update, NewtonWorldDestroyListenerCallback destroy); + NEWTON_API void* NewtonWorldAddListener (const NewtonWorld* const newtonWorld, const char* const nameId, void* const listenerUserData); + NEWTON_API void* NewtonWorldGetListener (const NewtonWorld* const newtonWorld, const char* const nameId); + + NEWTON_API void NewtonWorldListenerSetDebugCallback (const NewtonWorld* const newtonWorld, void* const listener, NewtonWorldListenerDebugCallback callback); + NEWTON_API void NewtonWorldListenerSetPostStepCallback (const NewtonWorld* const newtonWorld, void* const listener, NewtonWorldUpdateListenerCallback callback); + NEWTON_API void NewtonWorldListenerSetPreUpdateCallback (const NewtonWorld* const newtonWorld, void* const listener, NewtonWorldUpdateListenerCallback callback); + NEWTON_API void NewtonWorldListenerSetPostUpdateCallback (const NewtonWorld* const newtonWorld, void* const listener, NewtonWorldUpdateListenerCallback callback); + NEWTON_API void NewtonWorldListenerSetDestructorCallback (const NewtonWorld* const newtonWorld, void* const listener, NewtonWorldDestroyListenerCallback callback); + NEWTON_API void NewtonWorldListenerSetBodyDestroyCallback(const NewtonWorld* const newtonWorld, void* const listener, NewtonWorldListenerBodyDestroyCallback callback); + NEWTON_API void NewtonWorldListenerDebug(const NewtonWorld* const newtonWorld, void* const context); + NEWTON_API void* NewtonWorldGetListenerUserData(const NewtonWorld* const newtonWorld, void* const listener); + NEWTON_API NewtonWorldListenerBodyDestroyCallback NewtonWorldListenerGetBodyDestroyCallback (const NewtonWorld* const newtonWorld, void* const listener); NEWTON_API void NewtonWorldSetDestructorCallback (const NewtonWorld* const newtonWorld, NewtonWorldDestructorCallback destructor); NEWTON_API NewtonWorldDestructorCallback NewtonWorldGetDestructorCallback (const NewtonWorld* const newtonWorld); - NEWTON_API void NewtonWorldSetCollisionConstructorDestructorCallback (const NewtonWorld* const newtonWorld, NewtonCollisionCopyConstructionCallback constructor, NewtonCollisionDestructorCallback destructor); - NEWTON_DEPRECATED_API inline void NewtonWorldSetCollisionConstructorDestuctorCallback (const NewtonWorld* const newtonWorld, NewtonCollisionCopyConstructionCallback constructor, NewtonCollisionDestructorCallback destructor) - { - NewtonWorldSetCollisionConstructorDestructorCallback(newtonWorld, constructor, destructor); - } + NEWTON_API void NewtonWorldSetCreateDestroyContactCallback(const NewtonWorld* const newtonWorld, NewtonCreateContactCallback createContact, NewtonDestroyContactCallback destroyContact); NEWTON_API void NewtonWorldRayCast (const NewtonWorld* const newtonWorld, const dFloat* const p0, const dFloat* const p1, NewtonWorldRayFilterCallback filter, void* const userData, NewtonWorldRayPrefilterCallback prefilter, int threadIndex); - NEWTON_API void NewtonWorldConvexRayCast (const NewtonWorld* const newtonWorld, const NewtonCollision* const shape, const dFloat* const matrix, const dFloat* const p1, NewtonWorldRayFilterCallback filter, void* const userData, NewtonWorldRayPrefilterCallback prefilter, int threadIndex); - - NEWTON_API int NewtonWorldCollide (const NewtonWorld* const newtonWorld, const dFloat* const matrix, const NewtonCollision* const shape, void* const userData, - NewtonWorldRayPrefilterCallback prefilter, NewtonWorldConvexCastReturnInfo* const info, int maxContactsCount, int threadIndex); - NEWTON_API int NewtonWorldConvexCast (const NewtonWorld* const newtonWorld, const dFloat* const matrix, const dFloat* const target, const NewtonCollision* const shape, dFloat* const hitParam, void* const userData, - NewtonWorldRayPrefilterCallback prefilter, NewtonWorldConvexCastReturnInfo* const info, int maxContactsCount, int threadIndex); - - + NEWTON_API int NewtonWorldConvexCast (const NewtonWorld* const newtonWorld, const dFloat* const matrix, const dFloat* const target, const NewtonCollision* const shape, dFloat* const param, void* const userData, NewtonWorldRayPrefilterCallback prefilter, NewtonWorldConvexCastReturnInfo* const info, int maxContactsCount, int threadIndex); + NEWTON_API int NewtonWorldCollide (const NewtonWorld* const newtonWorld, const dFloat* const matrix, const NewtonCollision* const shape, void* const userData, NewtonWorldRayPrefilterCallback prefilter, NewtonWorldConvexCastReturnInfo* const info, int maxContactsCount, int threadIndex); + // world utility functions NEWTON_API int NewtonWorldGetBodyCount(const NewtonWorld* const newtonWorld); NEWTON_API int NewtonWorldGetConstraintCount(const NewtonWorld* const newtonWorld); + NEWTON_API NewtonJoint* NewtonWorldFindJoint(const NewtonBody* const body0, const NewtonBody* const body1); // ********************************************************************************************** // @@ -547,14 +571,20 @@ extern "C" { // deprecated, not longer continue collision is set on the material // NEWTON_API void NewtonMaterialSetContinuousCollisionMode (const NewtonWorld* const newtonWorld, int id0, int id1, int state); - NEWTON_API void NewtonMaterialSetCollisionCallback (const NewtonWorld* const newtonWorld, int id0, int id1, void* const userData, NewtonOnAABBOverlap aabbOverlap, NewtonContactsProcess process); + + NEWTON_API void NewtonMaterialSetCallbackUserData (const NewtonWorld* const newtonWorld, int id0, int id1, void* const userData); + NEWTON_API void NewtonMaterialSetContactGenerationCallback (const NewtonWorld* const newtonWorld, int id0, int id1, NewtonOnContactGeneration contactGeneration); NEWTON_API void NewtonMaterialSetCompoundCollisionCallback(const NewtonWorld* const newtonWorld, int id0, int id1, NewtonOnCompoundSubCollisionAABBOverlap compoundAabbOverlap); + NEWTON_API void NewtonMaterialSetCollisionCallback (const NewtonWorld* const newtonWorld, int id0, int id1, NewtonOnAABBOverlap aabbOverlap, NewtonContactsProcess process); NEWTON_API void NewtonMaterialSetDefaultSoftness (const NewtonWorld* const newtonWorld, int id0, int id1, dFloat value); NEWTON_API void NewtonMaterialSetDefaultElasticity (const NewtonWorld* const newtonWorld, int id0, int id1, dFloat elasticCoef); NEWTON_API void NewtonMaterialSetDefaultCollidable (const NewtonWorld* const newtonWorld, int id0, int id1, int state); NEWTON_API void NewtonMaterialSetDefaultFriction (const NewtonWorld* const newtonWorld, int id0, int id1, dFloat staticFriction, dFloat kineticFriction); + NEWTON_API void NewtonMaterialJointResetIntraJointCollision (const NewtonWorld* const newtonWorld, int id0, int id1); + NEWTON_API void NewtonMaterialJointResetSelftJointCollision (const NewtonWorld* const newtonWorld, int id0, int id1); + NEWTON_API NewtonMaterial* NewtonWorldGetFirstMaterial (const NewtonWorld* const newtonWorld); NEWTON_API NewtonMaterial* NewtonWorldGetNextMaterial (const NewtonWorld* const newtonWorld, const NewtonMaterial* const material); @@ -575,22 +605,29 @@ extern "C" { NEWTON_API void NewtonMaterialGetContactPositionAndNormal (const NewtonMaterial* const material, const NewtonBody* const body, dFloat* const posit, dFloat* const normal); NEWTON_API void NewtonMaterialGetContactTangentDirections (const NewtonMaterial* const material, const NewtonBody* const body, dFloat* const dir0, dFloat* const dir1); NEWTON_API dFloat NewtonMaterialGetContactTangentSpeed (const NewtonMaterial* const material, int index); - NEWTON_API dFloat NewtonMaterialGetContactMaxNormalImpact (const NewtonMaterial* const material); NEWTON_API dFloat NewtonMaterialGetContactMaxTangentImpact (const NewtonMaterial* const material, int index); - + NEWTON_API dFloat NewtonMaterialGetContactPenetration (const NewtonMaterial* const material); + NEWTON_API void NewtonMaterialSetAsSoftContact (const NewtonMaterial* const material, dFloat relaxation); + NEWTON_API void NewtonMaterialSetContactSoftness (const NewtonMaterial* const material, dFloat softness); + NEWTON_API void NewtonMaterialSetContactThickness (const NewtonMaterial* const material, dFloat thickness); NEWTON_API void NewtonMaterialSetContactElasticity (const NewtonMaterial* const material, dFloat restitution); NEWTON_API void NewtonMaterialSetContactFrictionState (const NewtonMaterial* const material, int state, int index); NEWTON_API void NewtonMaterialSetContactFrictionCoef (const NewtonMaterial* const material, dFloat staticFrictionCoef, dFloat kineticFrictionCoef, int index); - + NEWTON_API void NewtonMaterialSetContactNormalAcceleration (const NewtonMaterial* const material, dFloat accel); NEWTON_API void NewtonMaterialSetContactNormalDirection (const NewtonMaterial* const material, const dFloat* const directionVector); + NEWTON_API void NewtonMaterialSetContactPosition (const NewtonMaterial* const material, const dFloat* const position); + NEWTON_API void NewtonMaterialSetContactTangentFriction (const NewtonMaterial* const material, dFloat friction, int index); NEWTON_API void NewtonMaterialSetContactTangentAcceleration (const NewtonMaterial* const material, dFloat accel, int index); NEWTON_API void NewtonMaterialContactRotateTangentDirections (const NewtonMaterial* const material, const dFloat* const directionVector); - + //NEWTON_API dFloat NewtonMaterialGetContactPruningTolerance (const NewtonBody* const body0, const NewtonBody* const body1); + //NEWTON_API void NewtonMaterialSetContactPruningTolerance (const NewtonBody* const body0, const NewtonBody* const body1, dFloat tolerance); + NEWTON_API dFloat NewtonMaterialGetContactPruningTolerance(const NewtonJoint* const contactJoint); + NEWTON_API void NewtonMaterialSetContactPruningTolerance(const NewtonJoint* const contactJoint, dFloat tolerance); // ********************************************************************************************** // @@ -601,10 +638,8 @@ extern "C" { NEWTON_API NewtonCollision* NewtonCreateSphere (const NewtonWorld* const newtonWorld, dFloat radius, int shapeID, const dFloat* const offsetMatrix); NEWTON_API NewtonCollision* NewtonCreateBox (const NewtonWorld* const newtonWorld, dFloat dx, dFloat dy, dFloat dz, int shapeID, const dFloat* const offsetMatrix); NEWTON_API NewtonCollision* NewtonCreateCone (const NewtonWorld* const newtonWorld, dFloat radius, dFloat height, int shapeID, const dFloat* const offsetMatrix); - NEWTON_API NewtonCollision* NewtonCreateCapsule (const NewtonWorld* const newtonWorld, dFloat radius, dFloat height, int shapeID, const dFloat* const offsetMatrix); - NEWTON_API NewtonCollision* NewtonCreateCylinder (const NewtonWorld* const newtonWorld, dFloat radius, dFloat height, int shapeID, const dFloat* const offsetMatrix); - NEWTON_API NewtonCollision* NewtonCreateTaperedCapsule (const NewtonWorld* const newtonWorld, dFloat radio0, dFloat radio1, dFloat height, int shapeID, const dFloat* const offsetMatrix); - NEWTON_API NewtonCollision* NewtonCreateTaperedCylinder (const NewtonWorld* const newtonWorld, dFloat radio0, dFloat radio1, dFloat height, int shapeID, const dFloat* const offsetMatrix); + NEWTON_API NewtonCollision* NewtonCreateCapsule (const NewtonWorld* const newtonWorld, dFloat radius0, dFloat radius1, dFloat height, int shapeID, const dFloat* const offsetMatrix); + NEWTON_API NewtonCollision* NewtonCreateCylinder (const NewtonWorld* const newtonWorld, dFloat radio0, dFloat radio1, dFloat height, int shapeID, const dFloat* const offsetMatrix); NEWTON_API NewtonCollision* NewtonCreateChamferCylinder (const NewtonWorld* const newtonWorld, dFloat radius, dFloat height, int shapeID, const dFloat* const offsetMatrix); NEWTON_API NewtonCollision* NewtonCreateConvexHull (const NewtonWorld* const newtonWorld, int count, const dFloat* const vertexCloud, int strideInBytes, dFloat tolerance, int shapeID, const dFloat* const offsetMatrix); NEWTON_API NewtonCollision* NewtonCreateConvexHullFromMesh (const NewtonWorld* const newtonWorld, const NewtonMesh* const mesh, dFloat tolerance, int shapeID); @@ -612,21 +647,12 @@ extern "C" { NEWTON_API int NewtonCollisionGetMode(const NewtonCollision* const convexCollision); NEWTON_API void NewtonCollisionSetMode (const NewtonCollision* const convexCollision, int mode); - - -// NEWTON_API void NewtonCollisionSetMaxBreakImpactImpulse(const NewtonCollision* const convexHullCollision, dFloat maxImpactImpulse); -// NEWTON_API dFloat NewtonCollisionGetMaxBreakImpactImpulse(const NewtonCollision* const convexHullCollision); - NEWTON_API int NewtonConvexHullGetFaceIndices (const NewtonCollision* const convexHullCollision, int face, int* const faceIndices); NEWTON_API int NewtonConvexHullGetVertexData (const NewtonCollision* const convexHullCollision, dFloat** const vertexData, int* strideInBytes); - NEWTON_DEPRECATED_API inline int NewtonConvexHullGetVetexData (const NewtonCollision* const convexHullCollision, dFloat** const vertexData, int* strideInBytes) - { - return NewtonConvexHullGetVertexData(convexHullCollision, vertexData, strideInBytes); - } NEWTON_API dFloat NewtonConvexCollisionCalculateVolume (const NewtonCollision* const convexCollision); NEWTON_API void NewtonConvexCollisionCalculateInertialMatrix (const NewtonCollision* convexCollision, dFloat* const inertia, dFloat* const origin); - NEWTON_API void NewtonConvexCollisionCalculateBuoyancyAcceleration (const NewtonCollision* const convexCollision, const dFloat* const matrix, const dFloat* const shapeOrigin, const dFloat* const gravityVector, const dFloat* const fluidPlane, dFloat fluidDensity, dFloat fluidViscosity, dFloat* const accel, dFloat* const alpha); + NEWTON_API dFloat NewtonConvexCollisionCalculateBuoyancyVolume (const NewtonCollision* const convexCollision, const dFloat* const matrix, const dFloat* const fluidPlane, dFloat* const centerOfBuoyancy); NEWTON_API const void* NewtonCollisionDataPointer (const NewtonCollision* const convexCollision); @@ -721,12 +747,7 @@ extern "C" { NewtonUserMeshCollisionGetFacesInAABB facesInAABBCallback, NewtonOnUserCollisionSerializationCallback serializeCallback, int shapeID); NEWTON_API int NewtonUserMeshCollisionContinuousOverlapTest (const NewtonUserMeshCollisionCollideDesc* const collideDescData, const void* const continueCollisionHandle, const dFloat* const minAabb, const dFloat* const maxAabb); - NEWTON_DEPRECATED_API inline int NewtonUserMeshCollisionContinueOveralapTest (const NewtonUserMeshCollisionCollideDesc* const collideDescData, const void* const continueCollisionHandle, const dFloat* const minAabb, const dFloat* const maxAabb) - { - return NewtonUserMeshCollisionContinuousOverlapTest(collideDescData, continueCollisionHandle, minAabb, maxAabb); - } - - + // *********************************************************************************************************** // // Collision serialization functions @@ -741,14 +762,11 @@ extern "C" { // Static collision shapes functions // // ********************************************************************************************** - NEWTON_API NewtonCollision* NewtonCreateHeightFieldCollision (const NewtonWorld* const newtonWorld, int width, int height, int gridsDiagonals, int elevationdatType, - const void* const elevationMap, const char* const attributeMap, dFloat verticalScale, dFloat horizontalScale, int shapeID); + NEWTON_API NewtonCollision* NewtonCreateHeightFieldCollision (const NewtonWorld* const newtonWorld, int width, int height, int gridsDiagonals, int elevationdatType, const void* const elevationMap, const char* const attributeMap, dFloat verticalScale, dFloat horizontalScale_x, dFloat horizontalScale_z, int shapeID); NEWTON_API void NewtonHeightFieldSetUserRayCastCallback (const NewtonCollision* const heightfieldCollision, NewtonHeightFieldRayCastCallback rayHitCallback); - NEWTON_API NewtonCollision* NewtonCreateTreeCollision (const NewtonWorld* const newtonWorld, int shapeID); NEWTON_API NewtonCollision* NewtonCreateTreeCollisionFromMesh (const NewtonWorld* const newtonWorld, const NewtonMesh* const mesh, int shapeID); - NEWTON_API void NewtonTreeCollisionSetUserRayCastCallback (const NewtonCollision* const treeCollision, NewtonCollisionTreeRayCastCallback rayHitCallback); NEWTON_API void NewtonTreeCollisionBeginBuild (const NewtonCollision* const treeCollision); @@ -758,15 +776,6 @@ extern "C" { NEWTON_API int NewtonTreeCollisionGetFaceAttribute (const NewtonCollision* const treeCollision, const int* const faceIndexArray, int indexCount); NEWTON_API void NewtonTreeCollisionSetFaceAttribute (const NewtonCollision* const treeCollision, const int* const faceIndexArray, int indexCount, int attribute); - NEWTON_DEPRECATED_API inline int NewtonTreeCollisionGetFaceAtribute (const NewtonCollision* const treeCollision, const int* const faceIndexArray, int indexCount) - { - return NewtonTreeCollisionGetFaceAttribute(treeCollision, faceIndexArray, indexCount); - } - NEWTON_DEPRECATED_API inline void NewtonTreeCollisionSetFaceAtribute (const NewtonCollision* const treeCollision, const int* const faceIndexArray, int indexCount, int attribute) - { - NewtonTreeCollisionSetFaceAttribute(treeCollision, faceIndexArray, indexCount, attribute); - } - NEWTON_API void NewtonTreeCollisionForEachFace (const NewtonCollision* const treeCollision, NewtonTreeCollisionFaceCallback forEachFaceCallback, void* const context); NEWTON_API int NewtonTreeCollisionGetVertexListTriangleListInAABB (const NewtonCollision* const treeCollision, const dFloat* const p0, const dFloat* const p1, const dFloat** const vertexArray, int* const vertexCount, int* const vertexStrideInBytes, const int* const indexList, int maxIndexCount, const int* const faceAttribute); @@ -778,20 +787,20 @@ extern "C" { // General purpose collision library functions // // ********************************************************************************************** - NEWTON_API NewtonCollision* NewtonCollisionCreateInstance (const NewtonCollision* const collision); NEWTON_API int NewtonCollisionGetType (const NewtonCollision* const collision); + NEWTON_API int NewtonCollisionIsConvexShape (const NewtonCollision* const collision); + NEWTON_API int NewtonCollisionIsStaticShape (const NewtonCollision* const collision); // for the end user NEWTON_API void NewtonCollisionSetUserData (const NewtonCollision* const collision, void* const userData); NEWTON_API void* NewtonCollisionGetUserData (const NewtonCollision* const collision); - // this is used data is used by the joint library - NEWTON_API void NewtonCollisionSetUserData1 (const NewtonCollision* const collision, void* const userData); - NEWTON_API void* NewtonCollisionGetUserData1 (const NewtonCollision* const collision); - - NEWTON_API void NewtonCollisionSetUserID (const NewtonCollision* const collision, unsigned id); - NEWTON_API unsigned NewtonCollisionGetUserID (const NewtonCollision* const collision); + NEWTON_API void NewtonCollisionSetUserID (const NewtonCollision* const collision, dLong id); + NEWTON_API dLong NewtonCollisionGetUserID (const NewtonCollision* const collision); + + NEWTON_API void NewtonCollisionGetMaterial (const NewtonCollision* const collision, NewtonCollisionMaterial* const userData); + NEWTON_API void NewtonCollisionSetMaterial (const NewtonCollision* const collision, const NewtonCollisionMaterial* const userData); NEWTON_API void* NewtonCollisionGetSubCollisionHandle (const NewtonCollision* const collision); NEWTON_API NewtonCollision* NewtonCollisionGetParentInstance (const NewtonCollision* const collision); @@ -804,6 +813,7 @@ extern "C" { NEWTON_API void NewtonDestroyCollision (const NewtonCollision* const collision); NEWTON_API dFloat NewtonCollisionGetSkinThickness (const NewtonCollision* const collision); + NEWTON_API void NewtonCollisionSetSkinThickness(const NewtonCollision* const collision, dFloat thickness); NEWTON_API int NewtonCollisionIntersectionTest (const NewtonWorld* const newtonWorld, const NewtonCollision* const collisionA, const dFloat* const matrixA, @@ -833,8 +843,23 @@ extern "C" { NEWTON_API dFloat NewtonCollisionRayCast (const NewtonCollision* const collision, const dFloat* const p0, const dFloat* const p1, dFloat* const normal, dLong* const attribute); NEWTON_API void NewtonCollisionCalculateAABB (const NewtonCollision* const collision, const dFloat* const matrix, dFloat* const p0, dFloat* const p1); NEWTON_API void NewtonCollisionForEachPolygonDo (const NewtonCollision* const collision, const dFloat* const matrix, NewtonCollisionIterator callback, void* const userData); - + // ********************************************************************************************** + // + // collision aggregates, are a collision node on eh broad phase the serve as the root nod for a collection of rigid bodies + // that shared the property of being in close proximity all the time, they are similar to compound collision by the group bodies instead of collision instances + // These are good for speeding calculation calculation of rag doll, Vehicles or contractions of rigid bodied lined by joints. + // also for example if you know that many the life time of a group of bodies like the object on a house of a building will be localize to the confide of the building + // then warping the bodies under an aggregate will reduce collision calculation of almost an order of magnitude. + // + // ********************************************************************************************** + NEWTON_API void* NewtonCollisionAggregateCreate (NewtonWorld* const world); + NEWTON_API void NewtonCollisionAggregateDestroy (void* const aggregate); + NEWTON_API void NewtonCollisionAggregateAddBody (void* const aggregate, const NewtonBody* const body); + NEWTON_API void NewtonCollisionAggregateRemoveBody (void* const aggregate, const NewtonBody* const body); + + NEWTON_API int NewtonCollisionAggregateGetSelfCollision (void* const aggregate); + NEWTON_API void NewtonCollisionAggregateSetSelfCollision (void* const aggregate, int state); // ********************************************************************************************** // @@ -852,12 +877,10 @@ extern "C" { // ********************************************************************************************** NEWTON_API NewtonBody* NewtonCreateDynamicBody (const NewtonWorld* const newtonWorld, const NewtonCollision* const collision, const dFloat* const matrix); NEWTON_API NewtonBody* NewtonCreateKinematicBody (const NewtonWorld* const newtonWorld, const NewtonCollision* const collision, const dFloat* const matrix); - NEWTON_API NewtonBody* NewtonCreateDeformableBody (const NewtonWorld* const newtonWorld, const NewtonCollision* const deformableMesh, const dFloat* const matrix); + NEWTON_API NewtonBody* NewtonCreateAsymetricDynamicBody(const NewtonWorld* const newtonWorld, const NewtonCollision* const collision, const dFloat* const matrix); - NEWTON_API void NewtonDestroyBody(const NewtonBody* const body); + NEWTON_API void NewtonDestroyBody(const NewtonBody* const body); - NEWTON_DEPRECATED_API void NewtonBodyEnableSimulation(const NewtonBody* const body); - NEWTON_DEPRECATED_API void NewtonBodyDisableSimulation(const NewtonBody* const body); NEWTON_API int NewtonBodyGetSimulationState(const NewtonBody* const body); NEWTON_API void NewtonBodySetSimulationState(const NewtonBody* const bodyPtr, const int state); @@ -867,20 +890,23 @@ extern "C" { NEWTON_API void NewtonBodyAddForce (const NewtonBody* const body, const dFloat* const force); NEWTON_API void NewtonBodyAddTorque (const NewtonBody* const body, const dFloat* const torque); - NEWTON_API void NewtonBodyCalculateInverseDynamicsForce (const NewtonBody* const body, dFloat timestep, const dFloat* const desiredVeloc, dFloat* const forceOut); NEWTON_API void NewtonBodySetCentreOfMass (const NewtonBody* const body, const dFloat* const com); NEWTON_API void NewtonBodySetMassMatrix (const NewtonBody* const body, dFloat mass, dFloat Ixx, dFloat Iyy, dFloat Izz); + NEWTON_API void NewtonBodySetFullMassMatrix (const NewtonBody* const body, dFloat mass, const dFloat* const inertiaMatrix); NEWTON_API void NewtonBodySetMassProperties (const NewtonBody* const body, dFloat mass, const NewtonCollision* const collision); NEWTON_API void NewtonBodySetMatrix (const NewtonBody* const body, const dFloat* const matrix); + NEWTON_API void NewtonBodySetMatrixNoSleep (const NewtonBody* const body, const dFloat* const matrix); NEWTON_API void NewtonBodySetMatrixRecursive (const NewtonBody* const body, const dFloat* const matrix); NEWTON_API void NewtonBodySetMaterialGroupID (const NewtonBody* const body, int id); NEWTON_API void NewtonBodySetContinuousCollisionMode (const NewtonBody* const body, unsigned state); NEWTON_API void NewtonBodySetJointRecursiveCollision (const NewtonBody* const body, unsigned state); NEWTON_API void NewtonBodySetOmega (const NewtonBody* const body, const dFloat* const omega); + NEWTON_API void NewtonBodySetOmegaNoSleep (const NewtonBody* const body, const dFloat* const omega); NEWTON_API void NewtonBodySetVelocity (const NewtonBody* const body, const dFloat* const velocity); + NEWTON_API void NewtonBodySetVelocityNoSleep (const NewtonBody* const body, const dFloat* const velocity); NEWTON_API void NewtonBodySetForce (const NewtonBody* const body, const dFloat* const force); NEWTON_API void NewtonBodySetTorque (const NewtonBody* const body, const dFloat* const torque); @@ -898,6 +924,9 @@ extern "C" { NEWTON_API int NewtonBodyGetFreezeState(const NewtonBody* const body); NEWTON_API void NewtonBodySetFreezeState (const NewtonBody* const body, int state); + NEWTON_API int NewtonBodyGetGyroscopicTorque(const NewtonBody* const body); + NEWTON_API void NewtonBodySetGyroscopicTorque(const NewtonBody* const body, int state); + NEWTON_API void NewtonBodySetDestructorCallback (const NewtonBody* const body, NewtonBodyDestructor callback); NEWTON_API NewtonBodyDestructor NewtonBodyGetDestructorCallback (const NewtonBody* const body); @@ -914,31 +943,31 @@ extern "C" { NEWTON_API NewtonWorld* NewtonBodyGetWorld (const NewtonBody* const body); NEWTON_API NewtonCollision* NewtonBodyGetCollision (const NewtonBody* const body); - NEWTON_API int NewtonBodyGetMaterialGroupID (const NewtonBody* const body); + NEWTON_API int NewtonBodyGetMaterialGroupID (const NewtonBody* const body); - NEWTON_API int NewtonBodyGetContinuousCollisionMode (const NewtonBody* const body); - NEWTON_API int NewtonBodyGetJointRecursiveCollision (const NewtonBody* const body); + NEWTON_API int NewtonBodyGetSerializedID(const NewtonBody* const body); + NEWTON_API int NewtonBodyGetContinuousCollisionMode (const NewtonBody* const body); + NEWTON_API int NewtonBodyGetJointRecursiveCollision (const NewtonBody* const body); NEWTON_API void NewtonBodyGetPosition(const NewtonBody* const body, dFloat* const pos); - NEWTON_API void NewtonBodyGetMatrix(const NewtonBody* const body, dFloat* const matrix); - NEWTON_API void NewtonBodyGetRotation(const NewtonBody* const body, dFloat* const rotation); - NEWTON_API void NewtonBodyGetMassMatrix (const NewtonBody* const body, dFloat* mass, dFloat* const Ixx, dFloat* const Iyy, dFloat* const Izz); - NEWTON_API void NewtonBodyGetInvMass(const NewtonBody* const body, dFloat* const invMass, dFloat* const invIxx, dFloat* const invIyy, dFloat* const invIzz); - NEWTON_API void NewtonBodyGetInertiaMatrix(const NewtonBody* const body, dFloat* const inertiaMatrix); - NEWTON_API void NewtonBodyGetInvInertiaMatrix(const NewtonBody* const body, dFloat* const invInertiaMatrix); - NEWTON_API void NewtonBodyGetOmega(const NewtonBody* const body, dFloat* const vector); - NEWTON_API void NewtonBodyGetVelocity(const NewtonBody* const body, dFloat* const vector); - NEWTON_API void NewtonBodyGetForce(const NewtonBody* const body, dFloat* const vector); - NEWTON_API void NewtonBodyGetTorque(const NewtonBody* const body, dFloat* const vector); - NEWTON_API void NewtonBodyGetForceAcc(const NewtonBody* const body, dFloat* const vector); - NEWTON_API void NewtonBodyGetTorqueAcc(const NewtonBody* const body, dFloat* const vector); - NEWTON_API void NewtonBodyGetCentreOfMass (const NewtonBody* const body, dFloat* const com); - + NEWTON_API void NewtonBodyGetMatrix(const NewtonBody* const body, dFloat* const matrix); + NEWTON_API void NewtonBodyGetRotation(const NewtonBody* const body, dFloat* const rotation); + NEWTON_API void NewtonBodyGetMass (const NewtonBody* const body, dFloat* mass, dFloat* const Ixx, dFloat* const Iyy, dFloat* const Izz); + NEWTON_API void NewtonBodyGetInvMass(const NewtonBody* const body, dFloat* const invMass, dFloat* const invIxx, dFloat* const invIyy, dFloat* const invIzz); + NEWTON_API void NewtonBodyGetInertiaMatrix(const NewtonBody* const body, dFloat* const inertiaMatrix); + NEWTON_API void NewtonBodyGetInvInertiaMatrix(const NewtonBody* const body, dFloat* const invInertiaMatrix); + NEWTON_API void NewtonBodyGetOmega(const NewtonBody* const body, dFloat* const vector); + NEWTON_API void NewtonBodyGetVelocity(const NewtonBody* const body, dFloat* const vector); + NEWTON_API void NewtonBodyGetAlpha(const NewtonBody* const body, dFloat* const vector); + NEWTON_API void NewtonBodyGetAcceleration(const NewtonBody* const body, dFloat* const vector); + NEWTON_API void NewtonBodyGetForce(const NewtonBody* const body, dFloat* const vector); + NEWTON_API void NewtonBodyGetTorque(const NewtonBody* const body, dFloat* const vector); + NEWTON_API void NewtonBodyGetCentreOfMass (const NewtonBody* const body, dFloat* const com); NEWTON_API void NewtonBodyGetPointVelocity (const NewtonBody* const body, const dFloat* const point, dFloat* const velocOut); - NEWTON_API void NewtonBodyAddImpulse (const NewtonBody* const body, const dFloat* const pointDeltaVeloc, const dFloat* const pointPosit); - NEWTON_API void NewtonBodyApplyImpulseArray (const NewtonBody* const body, int impuleCount, int strideInByte, const dFloat* const impulseArray, const dFloat* const pointArray); - NEWTON_API void NewtonBodyApplyImpulsePair (const NewtonBody* const body, dFloat* const linearImpulse, dFloat* const angularImpulse); + NEWTON_API void NewtonBodyApplyImpulsePair (const NewtonBody* const body, dFloat* const linearImpulse, dFloat* const angularImpulse, dFloat timestep); + NEWTON_API void NewtonBodyAddImpulse (const NewtonBody* const body, const dFloat* const pointDeltaVeloc, const dFloat* const pointPosit, dFloat timestep); + NEWTON_API void NewtonBodyApplyImpulseArray (const NewtonBody* const body, int impuleCount, int strideInByte, const dFloat* const impulseArray, const dFloat* const pointArray, dFloat timestep); NEWTON_API void NewtonBodyIntegrateVelocity (const NewtonBody* const body, dFloat timestep); @@ -946,19 +975,18 @@ extern "C" { NEWTON_API void NewtonBodyGetAngularDamping (const NewtonBody* const body, dFloat* const vector); NEWTON_API void NewtonBodyGetAABB (const NewtonBody* const body, dFloat* const p0, dFloat* const p1); - NEWTON_API NewtonJoint* NewtonBodyGetFirstJoint (const NewtonBody* const body); NEWTON_API NewtonJoint* NewtonBodyGetNextJoint (const NewtonBody* const body, const NewtonJoint* const joint); + NEWTON_API NewtonJoint* NewtonBodyGetFirstContactJoint (const NewtonBody* const body); NEWTON_API NewtonJoint* NewtonBodyGetNextContactJoint (const NewtonBody* const body, const NewtonJoint* const contactJoint); + NEWTON_API NewtonJoint* NewtonBodyFindContact (const NewtonBody* const body0, const NewtonBody* const body1); - // ********************************************************************************************** // // contact joints interface // // ********************************************************************************************** - NEWTON_API void* NewtonContactJointGetFirstContact (const NewtonJoint* const contactJoint); NEWTON_API void* NewtonContactJointGetNextContact (const NewtonJoint* const contactJoint, void* const contact); @@ -966,6 +994,8 @@ extern "C" { NEWTON_API void NewtonContactJointRemoveContact(const NewtonJoint* const contactJoint, void* const contact); NEWTON_API dFloat NewtonContactJointGetClosestDistance(const NewtonJoint* const contactJoint); + NEWTON_API void NewtonContactJointResetSelftJointCollision(const NewtonJoint* const contactJoint); + NEWTON_API void NewtonContactJointResetIntraJointCollision(const NewtonJoint* const contactJoint); NEWTON_API NewtonMaterial* NewtonContactGetMaterial (const void* const contact); @@ -993,26 +1023,33 @@ extern "C" { NEWTON_API dFloat NewtonJointGetStiffness (const NewtonJoint* const joint); NEWTON_API void NewtonJointSetStiffness (const NewtonJoint* const joint, dFloat state); - + NEWTON_API void NewtonDestroyJoint(const NewtonWorld* const newtonWorld, const NewtonJoint* const joint); NEWTON_API void NewtonJointSetDestructor (const NewtonJoint* const joint, NewtonConstraintDestructor destructor); NEWTON_API int NewtonJointIsActive (const NewtonJoint* const joint); - - // ********************************************************************************************** // // particle system interface (soft bodies, individual, pressure bodies and cloth) // // ********************************************************************************************** - NEWTON_API NewtonCollision* NewtonCreateClothPatch (const NewtonWorld* const newtonWorld, NewtonMesh* const mesh, int shapeID, NewtonClothPatchMaterial* const structuralMaterial, NewtonClothPatchMaterial* const bendMaterial); - NEWTON_API NewtonCollision* NewtonCreateDeformableMesh (const NewtonWorld* const newtonWorld, NewtonMesh* const mesh, int shapeID); + NEWTON_API NewtonCollision* NewtonCreateMassSpringDamperSystem (const NewtonWorld* const newtonWorld, int shapeID, + const dFloat* const points, int pointCount, int strideInBytes, const dFloat* const pointMass, + const int* const links, int linksCount, const dFloat* const linksSpring, const dFloat* const linksDamper); + NEWTON_API NewtonCollision* NewtonCreateDeformableSolid(const NewtonWorld* const newtonWorld, const NewtonMesh* const mesh, int shapeID); + + NEWTON_API int NewtonDeformableMeshGetParticleCount (const NewtonCollision* const deformableMesh); + NEWTON_API int NewtonDeformableMeshGetParticleStrideInBytes (const NewtonCollision* const deformableMesh); + NEWTON_API const dFloat* NewtonDeformableMeshGetParticleArray (const NewtonCollision* const deformableMesh); + +/* + NEWTON_API NewtonCollision* NewtonCreateClothPatch (const NewtonWorld* const newtonWorld, NewtonMesh* const mesh, int shapeID, NewtonClothPatchMaterial* const structuralMaterial, NewtonClothPatchMaterial* const bendMaterial); NEWTON_API void NewtonDeformableMeshCreateClusters (NewtonCollision* const deformableMesh, int clusterCount, dFloat overlapingWidth); NEWTON_API void NewtonDeformableMeshSetDebugCallback (NewtonCollision* const deformableMesh, NewtonCollisionIterator callback); - NEWTON_API int NewtonDeformableMeshGetParticleCount (const NewtonCollision* const deformableMesh); + NEWTON_API void NewtonDeformableMeshGetParticlePosition (NewtonCollision* const deformableMesh, int particleIndex, dFloat* const posit); NEWTON_API void NewtonDeformableMeshBeginConfiguration (const NewtonCollision* const deformableMesh); @@ -1033,17 +1070,13 @@ extern "C" { NEWTON_API int NewtonDeformableMeshSegmentGetMaterialID (const NewtonCollision* const deformableMesh, const NewtonDeformableMeshSegment* const segment); NEWTON_API int NewtonDeformableMeshSegmentGetIndexCount (const NewtonCollision* const deformableMesh, const NewtonDeformableMeshSegment* const segment); NEWTON_API const int* NewtonDeformableMeshSegmentGetIndexList (const NewtonCollision* const deformableMesh, const NewtonDeformableMeshSegment* const segment); - - - - +*/ // ********************************************************************************************** // // Ball and Socket joint functions // // ********************************************************************************************** - NEWTON_API NewtonJoint* NewtonConstraintCreateBall (const NewtonWorld* const newtonWorld, const dFloat* pivotPoint, - const NewtonBody* const childBody, const NewtonBody* const parentBody); + NEWTON_API NewtonJoint* NewtonConstraintCreateBall (const NewtonWorld* const newtonWorld, const dFloat* pivotPoint, const NewtonBody* const childBody, const NewtonBody* const parentBody); NEWTON_API void NewtonBallSetUserCallback (const NewtonJoint* const ball, NewtonBallCallback callback); NEWTON_API void NewtonBallGetJointAngle (const NewtonJoint* const ball, dFloat* angle); NEWTON_API void NewtonBallGetJointOmega (const NewtonJoint* const ball, dFloat* omega); @@ -1055,10 +1088,7 @@ extern "C" { // Hinge joint functions // // ********************************************************************************************** - NEWTON_API NewtonJoint* NewtonConstraintCreateHinge (const NewtonWorld* const newtonWorld, - const dFloat* pivotPoint, const dFloat* pinDir, - const NewtonBody* const childBody, const NewtonBody* const parentBody); - + NEWTON_API NewtonJoint* NewtonConstraintCreateHinge (const NewtonWorld* const newtonWorld, const dFloat* pivotPoint, const dFloat* pinDir, const NewtonBody* const childBody, const NewtonBody* const parentBody); NEWTON_API void NewtonHingeSetUserCallback (const NewtonJoint* const hinge, NewtonHingeCallback callback); NEWTON_API dFloat NewtonHingeGetJointAngle (const NewtonJoint* const hinge); NEWTON_API dFloat NewtonHingeGetJointOmega (const NewtonJoint* const hinge); @@ -1070,9 +1100,7 @@ extern "C" { // Slider joint functions // // ********************************************************************************************** - NEWTON_API NewtonJoint* NewtonConstraintCreateSlider (const NewtonWorld* const newtonWorld, - const dFloat* pivotPoint, const dFloat* pinDir, - const NewtonBody* const childBody, const NewtonBody* const parentBody); + NEWTON_API NewtonJoint* NewtonConstraintCreateSlider (const NewtonWorld* const newtonWorld, const dFloat* pivotPoint, const dFloat* pinDir, const NewtonBody* const childBody, const NewtonBody* const parentBody); NEWTON_API void NewtonSliderSetUserCallback (const NewtonJoint* const slider, NewtonSliderCallback callback); NEWTON_API dFloat NewtonSliderGetJointPosit (const NewtonJoint* slider); NEWTON_API dFloat NewtonSliderGetJointVeloc (const NewtonJoint* slider); @@ -1085,9 +1113,7 @@ extern "C" { // Corkscrew joint functions // // ********************************************************************************************** - NEWTON_API NewtonJoint* NewtonConstraintCreateCorkscrew (const NewtonWorld* const newtonWorld, - const dFloat* pivotPoint, const dFloat* pinDir, - const NewtonBody* const childBody, const NewtonBody* const parentBody); + NEWTON_API NewtonJoint* NewtonConstraintCreateCorkscrew (const NewtonWorld* const newtonWorld, const dFloat* pivotPoint, const dFloat* pinDir, const NewtonBody* const childBody, const NewtonBody* const parentBody); NEWTON_API void NewtonCorkscrewSetUserCallback (const NewtonJoint* const corkscrew, NewtonCorkscrewCallback callback); NEWTON_API dFloat NewtonCorkscrewGetJointPosit (const NewtonJoint* const corkscrew); NEWTON_API dFloat NewtonCorkscrewGetJointAngle (const NewtonJoint* const corkscrew); @@ -1103,9 +1129,7 @@ extern "C" { // Universal joint functions // // ********************************************************************************************** - NEWTON_API NewtonJoint* NewtonConstraintCreateUniversal (const NewtonWorld* const newtonWorld, - const dFloat* pivotPoint, const dFloat* pinDir0, const dFloat* pinDir1, - const NewtonBody* const childBody, const NewtonBody* const parentBody); + NEWTON_API NewtonJoint* NewtonConstraintCreateUniversal (const NewtonWorld* const newtonWorld, const dFloat* pivotPoint, const dFloat* pinDir0, const dFloat* pinDir1, const NewtonBody* const childBody, const NewtonBody* const parentBody); NEWTON_API void NewtonUniversalSetUserCallback (const NewtonJoint* const universal, NewtonUniversalCallback callback); NEWTON_API dFloat NewtonUniversalGetJointAngle0 (const NewtonJoint* const universal); NEWTON_API dFloat NewtonUniversalGetJointAngle1 (const NewtonJoint* const universal); @@ -1131,38 +1155,27 @@ extern "C" { // User defined bilateral Joint // // ********************************************************************************************** - NEWTON_API NewtonJoint* NewtonConstraintCreateUserJoint (const NewtonWorld* const newtonWorld, int maxDOF, - NewtonUserBilateralCallback callback, - NewtonUserBilateralGetInfoCallback getInfo, - const NewtonBody* const childBody, const NewtonBody* const parentBody) ; + NEWTON_API NewtonJoint* NewtonConstraintCreateUserJoint (const NewtonWorld* const newtonWorld, int maxDOF, NewtonUserBilateralCallback callback, const NewtonBody* const childBody, const NewtonBody* const parentBody) ; + NEWTON_API int NewtonUserJointGetSolverModel(const NewtonJoint* const joint); + NEWTON_API void NewtonUserJointSetSolverModel(const NewtonJoint* const joint, int model); + NEWTON_API void NewtonUserJointMassScale(const NewtonJoint* const joint, dFloat scaleBody0, dFloat scaleBody1); + NEWTON_API void NewtonUserJointSetFeedbackCollectorCallback (const NewtonJoint* const joint, NewtonUserBilateralCallback getFeedback); NEWTON_API void NewtonUserJointAddLinearRow (const NewtonJoint* const joint, const dFloat* const pivot0, const dFloat* const pivot1, const dFloat* const dir); NEWTON_API void NewtonUserJointAddAngularRow (const NewtonJoint* const joint, dFloat relativeAngle, const dFloat* const dir); NEWTON_API void NewtonUserJointAddGeneralRow (const NewtonJoint* const joint, const dFloat* const jacobian0, const dFloat* const jacobian1); NEWTON_API void NewtonUserJointSetRowMinimumFriction (const NewtonJoint* const joint, dFloat friction); NEWTON_API void NewtonUserJointSetRowMaximumFriction (const NewtonJoint* const joint, dFloat friction); + NEWTON_API dFloat NewtonUserJointCalculateRowZeroAcceleration (const NewtonJoint* const joint); + NEWTON_API dFloat NewtonUserJointGetRowAcceleration (const NewtonJoint* const joint); + NEWTON_API void NewtonUserJointGetRowJacobian(const NewtonJoint* const joint, dFloat* const linear0, dFloat* const angula0, dFloat* const linear1, dFloat* const angula1); NEWTON_API void NewtonUserJointSetRowAcceleration (const NewtonJoint* const joint, dFloat acceleration); - NEWTON_API void NewtonUserJointSetRowSpringDamperAcceleration (const NewtonJoint* const joint, dFloat springK, dFloat springD); + NEWTON_API void NewtonUserJointSetRowSpringDamperAcceleration (const NewtonJoint* const joint, dFloat rowStiffness, dFloat spring, dFloat damper); NEWTON_API void NewtonUserJointSetRowStiffness (const NewtonJoint* const joint, dFloat stiffness); + NEWTON_API int NewtonUserJoinRowsCount (const NewtonJoint* const joint); + NEWTON_API void NewtonUserJointGetGeneralRow (const NewtonJoint* const joint, int index, dFloat* const jacobian0, dFloat* const jacobian1); NEWTON_API dFloat NewtonUserJointGetRowForce (const NewtonJoint* const joint, int row); - NEWTON_API void NewtonUserJointSetSolver (const NewtonJoint* const joint, int solver, int maxContactJoints); - - - // ************************************************************************************************************************ - // - // Acyclic articulation offer the same level of accuracy that Feather stone reduced coordinate link chains algorithm - // these are goo to make near perfect Rag dolls, physically based Created animated bodies, simple robotic contractions with not internal loops, - // Vehicles, Ropes, etc. that all interact seamlessly and natural with the physics world. - // - // ************************************************************************************************************************ - NEWTON_API NewtonAcyclicArticulation* NewtonAcyclicArticulationCreate (NewtonBody* const rootBone); - NEWTON_API void NewtonAcyclicArticulationDelete (NewtonAcyclicArticulation* const articulation); - NEWTON_API void* NewtonAcyclicArticulationAttachBone (NewtonAcyclicArticulation* const articulation, NewtonBody* const parentBone, NewtonBody* const childBone); - NEWTON_API void NewtonAcyclicArticulationDetachBone (NewtonAcyclicArticulation* const articulation, void* const bone); - NEWTON_API void NewtonAcyclicArticulationAddJoint (NewtonAcyclicArticulation* const articulation, NewtonJoint* const joint); - - // ********************************************************************************************** // // Mesh joint functions @@ -1171,8 +1184,8 @@ extern "C" { NEWTON_API NewtonMesh* NewtonMeshCreate(const NewtonWorld* const newtonWorld); NEWTON_API NewtonMesh* NewtonMeshCreateFromMesh(const NewtonMesh* const mesh); NEWTON_API NewtonMesh* NewtonMeshCreateFromCollision(const NewtonCollision* const collision); + NEWTON_API NewtonMesh* NewtonMeshCreateTetrahedraIsoSurface(const NewtonMesh* const mesh); NEWTON_API NewtonMesh* NewtonMeshCreateConvexHull (const NewtonWorld* const newtonWorld, int pointCount, const dFloat* const vertexCloud, int strideInBytes, dFloat tolerance); - NEWTON_API NewtonMesh* NewtonMeshCreateDelaunayTetrahedralization (const NewtonWorld* const newtonWorld, int pointCount, const dFloat* const vertexCloud, int strideInBytes, int materialID, const dFloat* const textureMatrix); NEWTON_API NewtonMesh* NewtonMeshCreateVoronoiConvexDecomposition (const NewtonWorld* const newtonWorld, int pointCount, const dFloat* const vertexCloud, int strideInBytes, int materialID, const dFloat* const textureMatrix); NEWTON_API NewtonMesh* NewtonMeshCreateFromSerialization (const NewtonWorld* const newtonWorld, NewtonDeserializeCallback deserializeFunction, void* const serializeHandle); NEWTON_API void NewtonMeshDestroy(const NewtonMesh* const mesh); @@ -1180,17 +1193,24 @@ extern "C" { NEWTON_API void NewtonMeshSerialize (const NewtonMesh* const mesh, NewtonSerializeCallback serializeFunction, void* const serializeHandle); NEWTON_API void NewtonMeshSaveOFF(const NewtonMesh* const mesh, const char* const filename); NEWTON_API NewtonMesh* NewtonMeshLoadOFF(const NewtonWorld* const newtonWorld, const char* const filename); + NEWTON_API NewtonMesh* NewtonMeshLoadTetrahedraMesh(const NewtonWorld* const newtonWorld, const char* const filename); + + NEWTON_API void NewtonMeshFlipWinding(const NewtonMesh* const mesh); NEWTON_API void NewtonMeshApplyTransform (const NewtonMesh* const mesh, const dFloat* const matrix); NEWTON_API void NewtonMeshCalculateOOBB(const NewtonMesh* const mesh, dFloat* const matrix, dFloat* const x, dFloat* const y, dFloat* const z); NEWTON_API void NewtonMeshCalculateVertexNormals(const NewtonMesh* const mesh, dFloat angleInRadians); - NEWTON_API void NewtonMeshApplySphericalMapping(const NewtonMesh* const mesh, int material); - NEWTON_API void NewtonMeshApplyCylindricalMapping(const NewtonMesh* const mesh, int cylinderMaterial, int capMaterial); - NEWTON_API void NewtonMeshApplyBoxMapping(const NewtonMesh* const mesh, int frontMaterial, int sideMaterial, int topMaterial); - NEWTON_API void NewtonMeshApplyAngleBasedMapping(const NewtonMesh* const mesh, int material, NewtonReportProgress reportPrograssCallback, void* const reportPrgressUserData); - + NEWTON_API void NewtonMeshApplySphericalMapping(const NewtonMesh* const mesh, int material, const dFloat* const aligmentMatrix); + NEWTON_API void NewtonMeshApplyCylindricalMapping(const NewtonMesh* const mesh, int cylinderMaterial, int capMaterial, const dFloat* const aligmentMatrix); + NEWTON_API void NewtonMeshApplyBoxMapping(const NewtonMesh* const mesh, int frontMaterial, int sideMaterial, int topMaterial, const dFloat* const aligmentMatrix); + NEWTON_API void NewtonMeshApplyAngleBasedMapping(const NewtonMesh* const mesh, int material, NewtonReportProgress reportPrograssCallback, void* const reportPrgressUserData, dFloat* const aligmentMatrix); + + NEWTON_API void NewtonCreateTetrahedraLinearBlendSkinWeightsChannel(const NewtonMesh* const tetrahedraMesh, NewtonMesh* const skinMesh); + NEWTON_API void NewtonMeshOptimize (const NewtonMesh* const mesh); + NEWTON_API void NewtonMeshOptimizePoints (const NewtonMesh* const mesh); + NEWTON_API void NewtonMeshOptimizeVertex (const NewtonMesh* const mesh); NEWTON_API int NewtonMeshIsOpenMesh (const NewtonMesh* const mesh); NEWTON_API void NewtonMeshFixTJoints (const NewtonMesh* const mesh); @@ -1208,28 +1228,38 @@ extern "C" { NEWTON_API void NewtonRemoveUnusedVertices(const NewtonMesh* const mesh, int* const vertexRemapTable); - NEWTON_API void NewtonMeshBeginFace(const NewtonMesh* const mesh); - NEWTON_API void NewtonMeshAddFace(const NewtonMesh* const mesh, int vertexCount, const dFloat* const vertex, int strideInBytes, int materialIndex); - NEWTON_API void NewtonMeshEndFace(const NewtonMesh* const mesh); + NEWTON_API void NewtonMeshBeginBuild(const NewtonMesh* const mesh); + NEWTON_API void NewtonMeshBeginFace(const NewtonMesh* const mesh); + NEWTON_API void NewtonMeshAddPoint(const NewtonMesh* const mesh, dFloat64 x, dFloat64 y, dFloat64 z); + NEWTON_API void NewtonMeshAddLayer(const NewtonMesh* const mesh, int layerIndex); + NEWTON_API void NewtonMeshAddMaterial(const NewtonMesh* const mesh, int materialIndex); + NEWTON_API void NewtonMeshAddNormal(const NewtonMesh* const mesh, dFloat x, dFloat y, dFloat z); + NEWTON_API void NewtonMeshAddBinormal(const NewtonMesh* const mesh, dFloat x, dFloat y, dFloat z); + NEWTON_API void NewtonMeshAddUV0(const NewtonMesh* const mesh, dFloat u, dFloat v); + NEWTON_API void NewtonMeshAddUV1(const NewtonMesh* const mesh, dFloat u, dFloat v); + NEWTON_API void NewtonMeshAddVertexColor(const NewtonMesh* const mesh, dFloat32 r, dFloat32 g, dFloat32 b, dFloat32 a); + NEWTON_API void NewtonMeshEndFace(const NewtonMesh* const mesh); + NEWTON_API void NewtonMeshEndBuild(const NewtonMesh* const mesh); - NEWTON_API void NewtonMeshBuildFromVertexListIndexList(const NewtonMesh* const mesh, - int faceCount, const int* const faceIndexCount, const int* const faceMaterialIndex, - const dFloat* const vertex, int vertexStrideInBytes, const int* const vertexIndex, - const dFloat* const normal, int normalStrideInBytes, const int* const normalIndex, - const dFloat* const uv0, int uv0StrideInBytes, const int* const uv0Index, - const dFloat* const uv1, int uv1StrideInBytes, const int* const uv1Index); + NEWTON_API void NewtonMeshClearVertexFormat (NewtonMeshVertexFormat* const format); + NEWTON_API void NewtonMeshBuildFromVertexListIndexList (const NewtonMesh* const mesh, const NewtonMeshVertexFormat* const format); - NEWTON_API void NewtonMeshGetVertexStreams (const NewtonMesh* const mesh, - int vertexStrideInByte, dFloat* const vertex, - int normalStrideInByte, dFloat* const normal, - int uvStrideInByte0, dFloat* const uv0, - int uvStrideInByte1, dFloat* const uv1); + NEWTON_API int NewtonMeshGetPointCount (const NewtonMesh* const mesh); + NEWTON_API const int* NewtonMeshGetIndexToVertexMap(const NewtonMesh* const mesh); - NEWTON_API void NewtonMeshGetIndirectVertexStreams(const NewtonMesh* const mesh, - int vertexStrideInByte, dFloat* const vertex, int* const vertexIndices, int* const vertexCount, - int normalStrideInByte, dFloat* const normal, int* const normalIndices, int* const normalCount, - int uvStrideInByte0, dFloat* const uv0, int* const uvIndices0, int* const uvCount0, - int uvStrideInByte1, dFloat* const uv1, int* const uvIndices1, int* const uvCount1); + NEWTON_API void NewtonMeshGetVertexDoubleChannel (const NewtonMesh* const mesh, int vertexStrideInByte, dFloat64* const outBuffer); + NEWTON_API void NewtonMeshGetVertexChannel (const NewtonMesh* const mesh, int vertexStrideInByte, dFloat* const outBuffer); + NEWTON_API void NewtonMeshGetNormalChannel (const NewtonMesh* const mesh, int vertexStrideInByte, dFloat* const outBuffer); + NEWTON_API void NewtonMeshGetBinormalChannel (const NewtonMesh* const mesh, int vertexStrideInByte, dFloat* const outBuffer); + NEWTON_API void NewtonMeshGetUV0Channel (const NewtonMesh* const mesh, int vertexStrideInByte, dFloat* const outBuffer); + NEWTON_API void NewtonMeshGetUV1Channel (const NewtonMesh* const mesh, int vertexStrideInByte, dFloat* const outBuffer); + NEWTON_API void NewtonMeshGetVertexColorChannel (const NewtonMesh* const mesh, int vertexStrideInByte, dFloat* const outBuffer); + + NEWTON_API int NewtonMeshHasNormalChannel(const NewtonMesh* const mesh); + NEWTON_API int NewtonMeshHasBinormalChannel(const NewtonMesh* const mesh); + NEWTON_API int NewtonMeshHasUV0Channel(const NewtonMesh* const mesh); + NEWTON_API int NewtonMeshHasUV1Channel(const NewtonMesh* const mesh); + NEWTON_API int NewtonMeshHasVertexColorChannel(const NewtonMesh* const mesh); NEWTON_API void* NewtonMeshBeginHandle (const NewtonMesh* const mesh); NEWTON_API void NewtonMeshEndHandle (const NewtonMesh* const mesh, void* const handle); @@ -1246,23 +1276,16 @@ extern "C" { NEWTON_API NewtonMesh* NewtonMeshCreateFirstLayer (const NewtonMesh* const mesh); NEWTON_API NewtonMesh* NewtonMeshCreateNextLayer (const NewtonMesh* const mesh, const NewtonMesh* const segment); - NEWTON_API int NewtonMeshGetTotalFaceCount (const NewtonMesh* const mesh); NEWTON_API int NewtonMeshGetTotalIndexCount (const NewtonMesh* const mesh); NEWTON_API void NewtonMeshGetFaces (const NewtonMesh* const mesh, int* const faceIndexCount, int* const faceMaterial, void** const faceIndices); - - NEWTON_API int NewtonMeshGetPointCount (const NewtonMesh* const mesh); - NEWTON_API int NewtonMeshGetPointStrideInByte (const NewtonMesh* const mesh); - NEWTON_API dFloat64* NewtonMeshGetPointArray (const NewtonMesh* const mesh); - NEWTON_API dFloat64* NewtonMeshGetNormalArray (const NewtonMesh* const mesh); - NEWTON_API dFloat64* NewtonMeshGetUV0Array (const NewtonMesh* const mesh); - NEWTON_API dFloat64* NewtonMeshGetUV1Array (const NewtonMesh* const mesh); - NEWTON_API int NewtonMeshGetVertexCount (const NewtonMesh* const mesh); NEWTON_API int NewtonMeshGetVertexStrideInByte (const NewtonMesh* const mesh); - NEWTON_API dFloat64* NewtonMeshGetVertexArray (const NewtonMesh* const mesh); + NEWTON_API const dFloat64* NewtonMeshGetVertexArray (const NewtonMesh* const mesh); + NEWTON_API int NewtonMeshGetVertexBaseCount(const NewtonMesh* const mesh); + NEWTON_API void NewtonMeshSetVertexBaseCount(const NewtonMesh* const mesh, int baseCount); NEWTON_API void* NewtonMeshGetFirstVertex (const NewtonMesh* const mesh); NEWTON_API void* NewtonMeshGetNextVertex (const NewtonMesh* const mesh, const void* const vertex); @@ -1273,7 +1296,6 @@ extern "C" { NEWTON_API int NewtonMeshGetPointIndex (const NewtonMesh* const mesh, const void* const point); NEWTON_API int NewtonMeshGetVertexIndexFromPoint (const NewtonMesh* const mesh, const void* const point); - NEWTON_API void* NewtonMeshGetFirstEdge (const NewtonMesh* const mesh); NEWTON_API void* NewtonMeshGetNextEdge (const NewtonMesh* const mesh, const void* const edge); NEWTON_API void NewtonMeshGetEdgeIndices (const NewtonMesh* const mesh, const void* const edge, int* const v0, int* const v1); diff --git a/thirdparty/include/Newton/dgTypes.h b/thirdparty/include/Newton/dgTypes.h new file mode 100644 index 000000000..08c6ee94f --- /dev/null +++ b/thirdparty/include/Newton/dgTypes.h @@ -0,0 +1,696 @@ +/* Copyright (c) <2003-2019> +* +* 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 + #include + #include + #include + #include + #include + + #ifdef _DEBUG + #pragma warning (disable: 4127) //conditional expression is constant + #endif + + #pragma warning (push, 3) + #include + #include + //#ifndef _DURANGO + // #include + //#endif + #pragma warning (pop) +#endif + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#if (defined (__MINGW32__) || defined (__MINGW64__)) + #include + #include + #include + #include + #include + #include +#endif + +#if (defined (_WIN_32_VER) || defined (_WIN_64_VER)) + #include + #include + #include +#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 + #include + #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 + #include + #include + } + #endif +#endif + +#ifdef _MACOSX_VER + #include + #include + #include + #if (defined __i386__ || defined __x86_64__) + #include + #include + #include //sse3 + #include + #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 + #include + #include +#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 + 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 +DG_INLINE T dgMin(T A, T B) +{ + return (A < B) ? A : B; +} + +template +DG_INLINE T dgMax(T A, T B) +{ + return (A > B) ? A : B; +} + +template +DG_INLINE T dgMin(T A, T B, T C) +{ + return dgMin(dgMin (A, B), C); +} + +template +DG_INLINE T dgMax(T A, T B, T C) +{ + return dgMax(dgMax (A, B), C); +} + +template +DG_INLINE T dgClamp(T val, T min, T max) +{ + return dgMax (min, dgMin (max, val)); +} + +template +DG_INLINE void dgSwap(T& A, T& B) +{ + T tmp (A); + A = B; + B = tmp; +} + +template +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 +DG_INLINE T dgSign(T A) +{ + return (A >= T(0)) ? T(1) : T(-1); +} + +template +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 +#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 + diff --git a/thirdparty/include/kiwi/AssocVector.h b/thirdparty/include/kiwi/AssocVector.h new file mode 100644 index 000000000..2a5092416 --- /dev/null +++ b/thirdparty/include/kiwi/AssocVector.h @@ -0,0 +1,356 @@ +//////////////////////////////////////////////////////////////////////////////// +// The Loki Library +// Copyright (c) 2001 by Andrei Alexandrescu +// This code accompanies the book: +// Alexandrescu, Andrei. "Modern C++ Design: Generic Programming and Design +// Patterns Applied". Copyright (c) 2001. Addison-Wesley. +// Permission to use, copy, modify, distribute and sell this software for any +// purpose is hereby granted without fee, provided that the above copyright +// notice appear in all copies and that both that copyright notice and this +// permission notice appear in supporting documentation. +// The author or Addison-Wesley Longman make no representations about the +// suitability of this software for any purpose. It is provided "as is" +// without express or implied warranty. +//////////////////////////////////////////////////////////////////////////////// +// Updated 2019 by Matthieu Dartiailh for C++11 compliancy +//////////////////////////////////////////////////////////////////////////////// +#pragma once + +// $Id: AssocVector.h 765 2006-10-18 13:55:32Z syntheticpp $ + + +#include +#include +#include +#include + +namespace Loki +{ +//////////////////////////////////////////////////////////////////////////////// +// class template AssocVectorCompare +// Used by AssocVector +//////////////////////////////////////////////////////////////////////////////// + + namespace Private + { + template + class AssocVectorCompare : public C + { + typedef std::pair + Data; + typedef typename C::first_argument_type first_argument_type; + + public: + AssocVectorCompare() + {} + + AssocVectorCompare(const C& src) : C(src) + {} + + bool operator()(const first_argument_type& lhs, + const first_argument_type& rhs) const + { return C::operator()(lhs, rhs); } + + bool operator()(const Data& lhs, const Data& rhs) const + { return operator()(lhs.first, rhs.first); } + + bool operator()(const Data& lhs, + const first_argument_type& rhs) const + { return operator()(lhs.first, rhs); } + + bool operator()(const first_argument_type& lhs, + const Data& rhs) const + { return operator()(lhs, rhs.first); } + }; + } + +//////////////////////////////////////////////////////////////////////////////// +// class template AssocVector +// An associative vector built as a syntactic drop-in replacement for std::map +// BEWARE: AssocVector doesn't respect all map's guarantees, the most important +// being: +// * iterators are invalidated by insert and erase operations +// * the complexity of insert/erase is O(N) not O(log N) +// * value_type is std::pair not std::pair +// * iterators are random +//////////////////////////////////////////////////////////////////////////////// + + + template + < + class K, + class V, + class C = std::less, + class A = std::allocator< std::pair > + > + class AssocVector + : private std::vector< std::pair, A > + , private Private::AssocVectorCompare + { + typedef std::vector, A> Base; + typedef Private::AssocVectorCompare MyCompare; + + public: + typedef K key_type; + typedef V mapped_type; + typedef typename Base::value_type value_type; + + typedef C key_compare; + typedef A allocator_type; + typedef typename A::reference reference; + typedef typename A::const_reference const_reference; + typedef typename Base::iterator iterator; + typedef typename Base::const_iterator const_iterator; + typedef typename Base::size_type size_type; + typedef typename Base::difference_type difference_type; + typedef typename A::pointer pointer; + typedef typename A::const_pointer const_pointer; + typedef typename Base::reverse_iterator reverse_iterator; + typedef typename Base::const_reverse_iterator const_reverse_iterator; + + class value_compare + : public std::function + , private key_compare + { + friend class AssocVector; + + protected: + value_compare(key_compare pred) : key_compare(pred) + {} + + public: + bool operator()(const value_type& lhs, const value_type& rhs) const + { return key_compare::operator()(lhs.first, rhs.first); } + }; + + // 23.3.1.1 construct/copy/destroy + + explicit AssocVector(const key_compare& comp = key_compare(), + const A& alloc = A()) + : Base(alloc), MyCompare(comp) + {} + + template + AssocVector(InputIterator first, InputIterator last, + const key_compare& comp = key_compare(), + const A& alloc = A()) + : Base(first, last, alloc), MyCompare(comp) + { + MyCompare& me = *this; + std::sort(begin(), end(), me); + } + + AssocVector& operator=(const AssocVector& rhs) + { + AssocVector(rhs).swap(*this); + return *this; + } + + // iterators: + // The following are here because MWCW gets 'using' wrong + iterator begin() { return Base::begin(); } + const_iterator begin() const { return Base::begin(); } + iterator end() { return Base::end(); } + const_iterator end() const { return Base::end(); } + reverse_iterator rbegin() { return Base::rbegin(); } + const_reverse_iterator rbegin() const { return Base::rbegin(); } + reverse_iterator rend() { return Base::rend(); } + const_reverse_iterator rend() const { return Base::rend(); } + + // capacity: + bool empty() const { return Base::empty(); } + size_type size() const { return Base::size(); } + size_type max_size() { return Base::max_size(); } + + // 23.3.1.2 element access: + mapped_type& operator[](const key_type& key) + { return insert(value_type(key, mapped_type())).first->second; } + + // modifiers: + std::pair insert(const value_type& val) + { + bool found(true); + iterator i(lower_bound(val.first)); + + if (i == end() || this->operator()(val.first, i->first)) + { + i = Base::insert(i, val); + found = false; + } + return std::make_pair(i, !found); + } + //Section [23.1.2], Table 69 + //http://developer.apple.com/documentation/DeveloperTools/gcc-3.3/libstdc++/23_containers/howto.html#4 + iterator insert(iterator pos, const value_type& val) + { + if( (pos == begin() || this->operator()(*(pos-1),val)) && + (pos == end() || this->operator()(val, *pos)) ) + { + return Base::insert(pos, val); + } + return insert(val).first; + } + + template + void insert(InputIterator first, InputIterator last) + { for (; first != last; ++first) insert(*first); } + + void erase(iterator pos) + { Base::erase(pos); } + + size_type erase(const key_type& k) + { + iterator i(find(k)); + if (i == end()) return 0; + erase(i); + return 1; + } + + void erase(iterator first, iterator last) + { Base::erase(first, last); } + + void swap(AssocVector& other) + { + Base::swap(other); + MyCompare& me = *this; + MyCompare& rhs = other; + std::swap(me, rhs); + } + + void clear() + { Base::clear(); } + + // observers: + key_compare key_comp() const + { return *this; } + + value_compare value_comp() const + { + const key_compare& comp = *this; + return value_compare(comp); + } + + // 23.3.1.3 map operations: + iterator find(const key_type& k) + { + iterator i(lower_bound(k)); + if (i != end() && this->operator()(k, i->first)) + { + i = end(); + } + return i; + } + + const_iterator find(const key_type& k) const + { + const_iterator i(lower_bound(k)); + if (i != end() && this->operator()(k, i->first)) + { + i = end(); + } + return i; + } + + size_type count(const key_type& k) const + { return find(k) != end(); } + + iterator lower_bound(const key_type& k) + { + MyCompare& me = *this; + return std::lower_bound(begin(), end(), k, me); + } + + const_iterator lower_bound(const key_type& k) const + { + const MyCompare& me = *this; + return std::lower_bound(begin(), end(), k, me); + } + + iterator upper_bound(const key_type& k) + { + MyCompare& me = *this; + return std::upper_bound(begin(), end(), k, me); + } + + const_iterator upper_bound(const key_type& k) const + { + const MyCompare& me = *this; + return std::upper_bound(begin(), end(), k, me); + } + + std::pair equal_range(const key_type& k) + { + MyCompare& me = *this; + return std::equal_range(begin(), end(), k, me); + } + + std::pair equal_range( + const key_type& k) const + { + const MyCompare& me = *this; + return std::equal_range(begin(), end(), k, me); + } + + template + friend bool operator==(const AssocVector& lhs, + const AssocVector& rhs); + + bool operator<(const AssocVector& rhs) const + { + const Base& me = *this; + const Base& yo = rhs; + return me < yo; + } + + template + friend bool operator!=(const AssocVector& lhs, + const AssocVector& rhs); + + template + friend bool operator>(const AssocVector& lhs, + const AssocVector& rhs); + + template + friend bool operator>=(const AssocVector& lhs, + const AssocVector& rhs); + + template + friend bool operator<=(const AssocVector& lhs, + const AssocVector& rhs); + }; + + template + inline bool operator==(const AssocVector& lhs, + const AssocVector& rhs) + { + const std::vector, A>& me = lhs; + return me == rhs; + } + + template + inline bool operator!=(const AssocVector& lhs, + const AssocVector& rhs) + { return !(lhs == rhs); } + + template + inline bool operator>(const AssocVector& lhs, + const AssocVector& rhs) + { return rhs < lhs; } + + template + inline bool operator>=(const AssocVector& lhs, + const AssocVector& rhs) + { return !(lhs < rhs); } + + template + inline bool operator<=(const AssocVector& lhs, + const AssocVector& rhs) + { return !(rhs < lhs); } + + + // specialized algorithms: + template + void swap(AssocVector& lhs, AssocVector& rhs) + { lhs.swap(rhs); } + +} // namespace Loki diff --git a/thirdparty/include/kiwi/constraint.h b/thirdparty/include/kiwi/constraint.h new file mode 100644 index 000000000..9d8322b94 --- /dev/null +++ b/thirdparty/include/kiwi/constraint.h @@ -0,0 +1,119 @@ +/*----------------------------------------------------------------------------- +| Copyright (c) 2013-2017, Nucleic Development Team. +| +| Distributed under the terms of the Modified BSD License. +| +| The full license is in the file LICENSE, distributed with this software. +|----------------------------------------------------------------------------*/ +#pragma once +#include +#include +#include "expression.h" +#include "shareddata.h" +#include "strength.h" +#include "term.h" +#include "variable.h" + +namespace kiwi +{ + +enum RelationalOperator +{ + OP_LE, + OP_GE, + OP_EQ +}; + +class Constraint +{ + +public: + Constraint() : m_data(0) {} + + Constraint(const Expression &expr, + RelationalOperator op, + double strength = strength::required) : m_data(new ConstraintData(expr, op, strength)) {} + + Constraint(const Constraint &other, double strength) : m_data(new ConstraintData(other, strength)) {} + + ~Constraint() {} + + const Expression &expression() const + { + return m_data->m_expression; + } + + RelationalOperator op() const + { + return m_data->m_op; + } + + double strength() const + { + return m_data->m_strength; + } + + bool operator!() const + { + return !m_data; + } + +private: + static Expression reduce(const Expression &expr) + { + std::map vars; + typedef std::vector::const_iterator iter_t; + iter_t end = expr.terms().end(); + for (iter_t it = expr.terms().begin(); it != end; ++it) + vars[it->variable()] += it->coefficient(); + std::vector terms(vars.begin(), vars.end()); + return Expression(terms, expr.constant()); + } + + class ConstraintData : public SharedData + { + + public: + ConstraintData(const Expression &expr, + RelationalOperator op, + double strength) : SharedData(), + m_expression(reduce(expr)), + m_strength(strength::clip(strength)), + m_op(op) {} + + ConstraintData(const Constraint &other, double strength) : SharedData(), + m_expression(other.expression()), + m_strength(strength::clip(strength)), + m_op(other.op()) {} + + ~ConstraintData() {} + + Expression m_expression; + double m_strength; + RelationalOperator m_op; + + private: + ConstraintData(const ConstraintData &other); + + ConstraintData &operator=(const ConstraintData &other); + }; + + SharedDataPtr m_data; + + friend bool operator<(const Constraint &lhs, const Constraint &rhs) + { + return lhs.m_data < rhs.m_data; + } + + friend bool operator==(const Constraint &lhs, const Constraint &rhs) + { + return lhs.m_data == rhs.m_data; + } + + friend bool operator!=(const Constraint &lhs, const Constraint &rhs) + { + return lhs.m_data != rhs.m_data; + } +}; + +} // namespace kiwi diff --git a/thirdparty/include/kiwi/debug.h b/thirdparty/include/kiwi/debug.h new file mode 100644 index 000000000..0d86091b2 --- /dev/null +++ b/thirdparty/include/kiwi/debug.h @@ -0,0 +1,200 @@ +/*----------------------------------------------------------------------------- +| Copyright (c) 2013-2017, Nucleic Development Team. +| +| Distributed under the terms of the Modified BSD License. +| +| The full license is in the file LICENSE, distributed with this software. +|----------------------------------------------------------------------------*/ +#pragma once +#include +#include +#include +#include "constraint.h" +#include "solverimpl.h" +#include "term.h" + +namespace kiwi +{ + +namespace impl +{ + +class DebugHelper +{ + +public: + static void dump(const SolverImpl &solver, std::ostream &out) + { + out << "Objective" << std::endl; + out << "---------" << std::endl; + dump(*solver.m_objective, out); + out << std::endl; + out << "Tableau" << std::endl; + out << "-------" << std::endl; + dump(solver.m_rows, out); + out << std::endl; + out << "Infeasible" << std::endl; + out << "----------" << std::endl; + dump(solver.m_infeasible_rows, out); + out << std::endl; + out << "Variables" << std::endl; + out << "---------" << std::endl; + dump(solver.m_vars, out); + out << std::endl; + out << "Edit Variables" << std::endl; + out << "--------------" << std::endl; + dump(solver.m_edits, out); + out << std::endl; + out << "Constraints" << std::endl; + out << "-----------" << std::endl; + dump(solver.m_cns, out); + out << std::endl; + out << std::endl; + } + + static void dump(const SolverImpl::RowMap &rows, std::ostream &out) + { + typedef SolverImpl::RowMap::const_iterator iter_t; + iter_t end = rows.end(); + for (iter_t it = rows.begin(); it != end; ++it) + { + dump(it->first, out); + out << " | "; + dump(*it->second, out); + } + } + + static void dump(const std::vector &symbols, std::ostream &out) + { + typedef std::vector::const_iterator iter_t; + iter_t end = symbols.end(); + for (iter_t it = symbols.begin(); it != end; ++it) + { + dump(*it, out); + out << std::endl; + } + } + + static void dump(const SolverImpl::VarMap &vars, std::ostream &out) + { + typedef SolverImpl::VarMap::const_iterator iter_t; + iter_t end = vars.end(); + for (iter_t it = vars.begin(); it != end; ++it) + { + out << it->first.name() << " = "; + dump(it->second, out); + out << std::endl; + } + } + + static void dump(const SolverImpl::CnMap &cns, std::ostream &out) + { + typedef SolverImpl::CnMap::const_iterator iter_t; + iter_t end = cns.end(); + for (iter_t it = cns.begin(); it != end; ++it) + dump(it->first, out); + } + + static void dump(const SolverImpl::EditMap &edits, std::ostream &out) + { + typedef SolverImpl::EditMap::const_iterator iter_t; + iter_t end = edits.end(); + for (iter_t it = edits.begin(); it != end; ++it) + out << it->first.name() << std::endl; + } + + static void dump(const Row &row, std::ostream &out) + { + typedef Row::CellMap::const_iterator iter_t; + out << row.constant(); + iter_t end = row.cells().end(); + for (iter_t it = row.cells().begin(); it != end; ++it) + { + out << " + " << it->second << " * "; + dump(it->first, out); + } + out << std::endl; + } + + static void dump(const Symbol &symbol, std::ostream &out) + { + switch (symbol.type()) + { + case Symbol::Invalid: + out << "i"; + break; + case Symbol::External: + out << "v"; + break; + case Symbol::Slack: + out << "s"; + break; + case Symbol::Error: + out << "e"; + break; + case Symbol::Dummy: + out << "d"; + break; + default: + break; + } + out << symbol.id(); + } + + static void dump(const Constraint &cn, std::ostream &out) + { + typedef std::vector::const_iterator iter_t; + iter_t begin = cn.expression().terms().begin(); + iter_t end = cn.expression().terms().end(); + for (iter_t it = begin; it != end; ++it) + { + out << it->coefficient() << " * "; + out << it->variable().name() << " + "; + } + out << cn.expression().constant(); + switch (cn.op()) + { + case OP_LE: + out << " <= 0 "; + break; + case OP_GE: + out << " >= 0 "; + break; + case OP_EQ: + out << " == 0 "; + break; + default: + break; + } + out << " | strength = " << cn.strength() << std::endl; + } +}; + +} // namespace impl + +namespace debug +{ + +template +void dump(const T &value) +{ + impl::DebugHelper::dump(value, std::cout); +} + +template +void dump(const T &value, std::ostream &out) +{ + impl::DebugHelper::dump(value, out); +} + +template +std::string dumps(const T &value) +{ + std::stringstream stream; + impl::DebugHelper::dump(value, stream); + return stream.str(); +} + +} // namespace debug + +} // namespace kiwi diff --git a/thirdparty/include/kiwi/errors.h b/thirdparty/include/kiwi/errors.h new file mode 100644 index 000000000..eb54560f4 --- /dev/null +++ b/thirdparty/include/kiwi/errors.h @@ -0,0 +1,162 @@ +/*----------------------------------------------------------------------------- +| Copyright (c) 2013-2017, Nucleic Development Team. +| +| Distributed under the terms of the Modified BSD License. +| +| The full license is in the file LICENSE, distributed with this software. +|----------------------------------------------------------------------------*/ +#pragma once +#include +#include +#include "constraint.h" +#include "variable.h" + +namespace kiwi +{ + +class UnsatisfiableConstraint : public std::exception +{ + +public: + UnsatisfiableConstraint(const Constraint &constraint) : m_constraint(constraint) {} + + ~UnsatisfiableConstraint() throw() {} + + const char *what() const throw() + { + return "The constraint can not be satisfied."; + } + + const Constraint &constraint() const + { + return m_constraint; + } + +private: + Constraint m_constraint; +}; + +class UnknownConstraint : public std::exception +{ + +public: + UnknownConstraint(const Constraint &constraint) : m_constraint(constraint) {} + + ~UnknownConstraint() throw() {} + + const char *what() const throw() + { + return "The constraint has not been added to the solver."; + } + + const Constraint &constraint() const + { + return m_constraint; + } + +private: + Constraint m_constraint; +}; + +class DuplicateConstraint : public std::exception +{ + +public: + DuplicateConstraint(const Constraint &constraint) : m_constraint(constraint) {} + + ~DuplicateConstraint() throw() {} + + const char *what() const throw() + { + return "The constraint has already been added to the solver."; + } + + const Constraint &constraint() const + { + return m_constraint; + } + +private: + Constraint m_constraint; +}; + +class UnknownEditVariable : public std::exception +{ + +public: + UnknownEditVariable(const Variable &variable) : m_variable(variable) {} + + ~UnknownEditVariable() throw() {} + + const char *what() const throw() + { + return "The edit variable has not been added to the solver."; + } + + const Variable &variable() const + { + return m_variable; + } + +private: + Variable m_variable; +}; + +class DuplicateEditVariable : public std::exception +{ + +public: + DuplicateEditVariable(const Variable &variable) : m_variable(variable) {} + + ~DuplicateEditVariable() throw() {} + + const char *what() const throw() + { + return "The edit variable has already been added to the solver."; + } + + const Variable &variable() const + { + return m_variable; + } + +private: + Variable m_variable; +}; + +class BadRequiredStrength : public std::exception +{ + +public: + BadRequiredStrength() {} + + ~BadRequiredStrength() throw() {} + + const char *what() const throw() + { + return "A required strength cannot be used in this context."; + } +}; + +class InternalSolverError : public std::exception +{ + +public: + InternalSolverError() : m_msg("An internal solver error ocurred.") {} + + InternalSolverError(const char *msg) : m_msg(msg) {} + + InternalSolverError(const std::string &msg) : m_msg(msg) {} + + ~InternalSolverError() throw() {} + + const char *what() const throw() + { + return m_msg.c_str(); + } + +private: + std::string m_msg; +}; + +} // namespace kiwi diff --git a/thirdparty/include/kiwi/expression.h b/thirdparty/include/kiwi/expression.h new file mode 100644 index 000000000..e2b5ae668 --- /dev/null +++ b/thirdparty/include/kiwi/expression.h @@ -0,0 +1,52 @@ +/*----------------------------------------------------------------------------- +| Copyright (c) 2013-2017, Nucleic Development Team. +| +| Distributed under the terms of the Modified BSD License. +| +| The full license is in the file LICENSE, distributed with this software. +|----------------------------------------------------------------------------*/ +#pragma once +#include +#include "term.h" + +namespace kiwi +{ + +class Expression +{ + +public: + Expression(double constant = 0.0) : m_constant(constant) {} + + Expression(const Term &term, double constant = 0.0) : m_terms(1, term), m_constant(constant) {} + + Expression(const std::vector &terms, double constant = 0.0) : m_terms(terms), m_constant(constant) {} + + ~Expression() {} + + const std::vector &terms() const + { + return m_terms; + } + + double constant() const + { + return m_constant; + } + + double value() const + { + typedef std::vector::const_iterator iter_t; + double result = m_constant; + iter_t end = m_terms.end(); + for (iter_t it = m_terms.begin(); it != end; ++it) + result += it->value(); + return result; + } + +private: + std::vector m_terms; + double m_constant; +}; + +} // namespace kiwi diff --git a/thirdparty/include/kiwi/kiwi.h b/thirdparty/include/kiwi/kiwi.h new file mode 100644 index 000000000..77bb6a8ec --- /dev/null +++ b/thirdparty/include/kiwi/kiwi.h @@ -0,0 +1,19 @@ +/*----------------------------------------------------------------------------- +| Copyright (c) 2013-2017, Nucleic Development Team. +| +| Distributed under the terms of the Modified BSD License. +| +| The full license is in the file LICENSE, distributed with this software. +|----------------------------------------------------------------------------*/ +#pragma once +#include "constraint.h" +#include "debug.h" +#include "errors.h" +#include "expression.h" +#include "shareddata.h" +#include "solver.h" +#include "strength.h" +#include "symbolics.h" +#include "term.h" +#include "variable.h" +#include "version.h" diff --git a/thirdparty/include/kiwi/maptype.h b/thirdparty/include/kiwi/maptype.h new file mode 100644 index 000000000..9b19a2173 --- /dev/null +++ b/thirdparty/include/kiwi/maptype.h @@ -0,0 +1,37 @@ +/*----------------------------------------------------------------------------- +| Copyright (c) 2013-2019, Nucleic Development Team. +| +| Distributed under the terms of the Modified BSD License. +| +| The full license is in the file LICENSE, distributed with this software. +|----------------------------------------------------------------------------*/ +#pragma once +#include +#include +#include +#include +#include "AssocVector.h" + +namespace kiwi +{ + +namespace impl +{ + +template < + typename K, + typename V, + typename C = std::less, + typename A = std::allocator>> +using MapType = Loki::AssocVector; + +// template< +// typename K, +// typename V, +// typename C = std::less, +// typename A = std::allocator< std::pair > > +// using MapType = std::map; + +} // namespace impl + +} // namespace kiwi diff --git a/thirdparty/include/kiwi/row.h b/thirdparty/include/kiwi/row.h new file mode 100644 index 000000000..29b25135b --- /dev/null +++ b/thirdparty/include/kiwi/row.h @@ -0,0 +1,188 @@ +/*----------------------------------------------------------------------------- +| Copyright (c) 2013-2017, Nucleic Development Team. +| +| Distributed under the terms of the Modified BSD License. +| +| The full license is in the file LICENSE, distributed with this software. +|----------------------------------------------------------------------------*/ +#pragma once +#include "maptype.h" +#include "symbol.h" +#include "util.h" + +namespace kiwi +{ + +namespace impl +{ + +class Row +{ + +public: + typedef MapType CellMap; + + Row() : m_constant(0.0) {} + + Row(double constant) : m_constant(constant) {} + + Row(const Row &other) : m_cells(other.m_cells), m_constant(other.m_constant) {} + + ~Row() {} + + const CellMap &cells() const + { + return m_cells; + } + + double constant() const + { + return m_constant; + } + + /* Add a constant value to the row constant. + + The new value of the constant is returned. + + */ + double add(double value) + { + return m_constant += value; + } + + /* Insert a symbol into the row with a given coefficient. + + If the symbol already exists in the row, the coefficient will be + added to the existing coefficient. If the resulting coefficient + is zero, the symbol will be removed from the row. + + */ + void insert(const Symbol &symbol, double coefficient = 1.0) + { + if (nearZero(m_cells[symbol] += coefficient)) + m_cells.erase(symbol); + } + + /* Insert a row into this row with a given coefficient. + + The constant and the cells of the other row will be multiplied by + the coefficient and added to this row. Any cell with a resulting + coefficient of zero will be removed from the row. + + */ + void insert(const Row &other, double coefficient = 1.0) + { + typedef CellMap::const_iterator iter_t; + m_constant += other.m_constant * coefficient; + iter_t end = other.m_cells.end(); + for (iter_t it = other.m_cells.begin(); it != end; ++it) + { + double coeff = it->second * coefficient; + if (nearZero(m_cells[it->first] += coeff)) + m_cells.erase(it->first); + } + } + + /* Remove the given symbol from the row. + + */ + void remove(const Symbol &symbol) + { + CellMap::iterator it = m_cells.find(symbol); + if (it != m_cells.end()) + m_cells.erase(it); + } + + /* Reverse the sign of the constant and all cells in the row. + + */ + void reverseSign() + { + typedef CellMap::iterator iter_t; + m_constant = -m_constant; + iter_t end = m_cells.end(); + for (iter_t it = m_cells.begin(); it != end; ++it) + it->second = -it->second; + } + + /* Solve the row for the given symbol. + + This method assumes the row is of the form a * x + b * y + c = 0 + and (assuming solve for x) will modify the row to represent the + right hand side of x = -b/a * y - c / a. The target symbol will + be removed from the row, and the constant and other cells will + be multiplied by the negative inverse of the target coefficient. + + The given symbol *must* exist in the row. + + */ + void solveFor(const Symbol &symbol) + { + typedef CellMap::iterator iter_t; + double coeff = -1.0 / m_cells[symbol]; + m_cells.erase(symbol); + m_constant *= coeff; + iter_t end = m_cells.end(); + for (iter_t it = m_cells.begin(); it != end; ++it) + it->second *= coeff; + } + + /* Solve the row for the given symbols. + + This method assumes the row is of the form x = b * y + c and will + solve the row such that y = x / b - c / b. The rhs symbol will be + removed from the row, the lhs added, and the result divided by the + negative inverse of the rhs coefficient. + + The lhs symbol *must not* exist in the row, and the rhs symbol + *must* exist in the row. + + */ + void solveFor(const Symbol &lhs, const Symbol &rhs) + { + insert(lhs, -1.0); + solveFor(rhs); + } + + /* Get the coefficient for the given symbol. + + If the symbol does not exist in the row, zero will be returned. + + */ + double coefficientFor(const Symbol &symbol) const + { + CellMap::const_iterator it = m_cells.find(symbol); + if (it == m_cells.end()) + return 0.0; + return it->second; + } + + /* Substitute a symbol with the data from another row. + + Given a row of the form a * x + b and a substitution of the + form x = 3 * y + c the row will be updated to reflect the + expression 3 * a * y + a * c + b. + + If the symbol does not exist in the row, this is a no-op. + + */ + void substitute(const Symbol &symbol, const Row &row) + { + typedef CellMap::iterator iter_t; + iter_t it = m_cells.find(symbol); + if (it != m_cells.end()) + { + double coefficient = it->second; + m_cells.erase(it); + insert(row, coefficient); + } + } + +private: + CellMap m_cells; + double m_constant; +}; + +} // namespace impl + +} // namespace kiwi diff --git a/thirdparty/include/kiwi/shareddata.h b/thirdparty/include/kiwi/shareddata.h new file mode 100644 index 000000000..9b1bc2c01 --- /dev/null +++ b/thirdparty/include/kiwi/shareddata.h @@ -0,0 +1,151 @@ +/*----------------------------------------------------------------------------- +| Copyright (c) 2013-2017, Nucleic Development Team. +| +| Distributed under the terms of the Modified BSD License. +| +| The full license is in the file LICENSE, distributed with this software. +|----------------------------------------------------------------------------*/ +#pragma once + +namespace kiwi +{ + +class SharedData +{ + +public: + SharedData() : m_refcount(0) {} + + SharedData(const SharedData &other) : m_refcount(0) {} + + int m_refcount; + +private: + SharedData &operator=(const SharedData &other); +}; + +template +class SharedDataPtr +{ + +public: + typedef T Type; + + SharedDataPtr() : m_data(0) {} + + explicit SharedDataPtr(T *data) : m_data(data) + { + incref(m_data); + } + + ~SharedDataPtr() + { + decref(m_data); + } + + T *data() + { + return m_data; + } + + const T *data() const + { + return m_data; + } + + operator T *() + { + return m_data; + } + + operator const T *() const + { + return m_data; + } + + T *operator->() + { + return m_data; + } + + const T *operator->() const + { + return m_data; + } + + T &operator*() + { + return *m_data; + } + + const T &operator*() const + { + return *m_data; + } + + bool operator!() const + { + return !m_data; + } + + bool operator<(const SharedDataPtr &other) const + { + return m_data < other.m_data; + } + + bool operator==(const SharedDataPtr &other) const + { + return m_data == other.m_data; + } + + bool operator!=(const SharedDataPtr &other) const + { + return m_data != other.m_data; + } + + SharedDataPtr(const SharedDataPtr &other) : m_data(other.m_data) + { + incref(m_data); + } + + SharedDataPtr &operator=(const SharedDataPtr &other) + { + if (m_data != other.m_data) + { + T *temp = m_data; + m_data = other.m_data; + incref(m_data); + decref(temp); + } + return *this; + } + + SharedDataPtr &operator=(T *other) + { + if (m_data != other) + { + T *temp = m_data; + m_data = other; + incref(m_data); + decref(temp); + } + return *this; + } + +private: + static void incref(T *data) + { + if (data) + ++data->m_refcount; + } + + static void decref(T *data) + { + if (data && --data->m_refcount == 0) + delete data; + } + + T *m_data; +}; + +} // namespace kiwi diff --git a/thirdparty/include/kiwi/solver.h b/thirdparty/include/kiwi/solver.h new file mode 100644 index 000000000..678df8ac7 --- /dev/null +++ b/thirdparty/include/kiwi/solver.h @@ -0,0 +1,178 @@ +/*----------------------------------------------------------------------------- +| Copyright (c) 2013-2017, Nucleic Development Team. +| +| Distributed under the terms of the Modified BSD License. +| +| The full license is in the file LICENSE, distributed with this software. +|----------------------------------------------------------------------------*/ +#pragma once +#include "constraint.h" +#include "debug.h" +#include "solverimpl.h" +#include "strength.h" +#include "variable.h" + + +namespace kiwi +{ + +class Solver +{ + +public: + + Solver() {} + + ~Solver() {} + + /* Add a constraint to the solver. + + Throws + ------ + DuplicateConstraint + The given constraint has already been added to the solver. + + UnsatisfiableConstraint + The given constraint is required and cannot be satisfied. + + */ + void addConstraint( const Constraint& constraint ) + { + m_impl.addConstraint( constraint ); + } + + /* Remove a constraint from the solver. + + Throws + ------ + UnknownConstraint + The given constraint has not been added to the solver. + + */ + void removeConstraint( const Constraint& constraint ) + { + m_impl.removeConstraint( constraint ); + } + + /* Test whether a constraint has been added to the solver. + + */ + bool hasConstraint( const Constraint& constraint ) const + { + return m_impl.hasConstraint( constraint ); + } + + /* Add an edit variable to the solver. + + This method should be called before the `suggestValue` method is + used to supply a suggested value for the given edit variable. + + Throws + ------ + DuplicateEditVariable + The given edit variable has already been added to the solver. + + BadRequiredStrength + The given strength is >= required. + + */ + void addEditVariable( const Variable& variable, double strength ) + { + m_impl.addEditVariable( variable, strength ); + } + + /* Remove an edit variable from the solver. + + Throws + ------ + UnknownEditVariable + The given edit variable has not been added to the solver. + + */ + void removeEditVariable( const Variable& variable ) + { + m_impl.removeEditVariable( variable ); + } + + /* Test whether an edit variable has been added to the solver. + + */ + bool hasEditVariable( const Variable& variable ) const + { + return m_impl.hasEditVariable( variable ); + } + + /* Suggest a value for the given edit variable. + + This method should be used after an edit variable as been added to + the solver in order to suggest the value for that variable. After + all suggestions have been made, the `solve` method can be used to + update the values of all variables. + + Throws + ------ + UnknownEditVariable + The given edit variable has not been added to the solver. + + */ + void suggestValue( const Variable& variable, double value ) + { + m_impl.suggestValue( variable, value ); + } + + /* Update the values of the external solver variables. + + */ + void updateVariables() + { + m_impl.updateVariables(); + } + + /* Reset the solver to the empty starting condition. + + This method resets the internal solver state to the empty starting + condition, as if no constraints or edit variables have been added. + This can be faster than deleting the solver and creating a new one + when the entire system must change, since it can avoid unecessary + heap (de)allocations. + + */ + void reset() + { + m_impl.reset(); + } + + /* Dump a representation of the solver internals to stdout. + + */ + void dump() + { + debug::dump( m_impl ); + } + + /* Dump a representation of the solver internals to a stream. + + */ + void dump( std::ostream& out ) + { + debug::dump( m_impl, out ); + } + + /* Dump a representation of the solver internals to a string. + + */ + std::string dumps() + { + return debug::dumps( m_impl ); + } + +private: + + Solver( const Solver& ); + + Solver& operator=( const Solver& ); + + impl::SolverImpl m_impl; +}; + +} // namespace kiwi diff --git a/thirdparty/include/kiwi/solverimpl.h b/thirdparty/include/kiwi/solverimpl.h new file mode 100644 index 000000000..bb6690aeb --- /dev/null +++ b/thirdparty/include/kiwi/solverimpl.h @@ -0,0 +1,840 @@ +/*----------------------------------------------------------------------------- +| Copyright (c) 2013-2017, Nucleic Development Team. +| +| Distributed under the terms of the Modified BSD License. +| +| The full license is in the file LICENSE, distributed with this software. +|----------------------------------------------------------------------------*/ +#pragma once +#include +#include +#include +#include +#include "constraint.h" +#include "errors.h" +#include "expression.h" +#include "maptype.h" +#include "row.h" +#include "symbol.h" +#include "term.h" +#include "util.h" +#include "variable.h" + + +namespace kiwi +{ + +namespace impl +{ + +class SolverImpl +{ + friend class DebugHelper; + + struct Tag + { + Symbol marker; + Symbol other; + }; + + struct EditInfo + { + Tag tag; + Constraint constraint; + double constant; + }; + + typedef MapType VarMap; + + typedef MapType RowMap; + + typedef MapType CnMap; + + typedef MapType EditMap; + + struct DualOptimizeGuard + { + DualOptimizeGuard( SolverImpl& impl ) : m_impl( impl ) {} + ~DualOptimizeGuard() { m_impl.dualOptimize(); } + SolverImpl& m_impl; + }; + +public: + + SolverImpl() : m_objective( new Row() ), m_id_tick( 1 ) {} + + ~SolverImpl() { clearRows(); } + + /* Add a constraint to the solver. + + Throws + ------ + DuplicateConstraint + The given constraint has already been added to the solver. + + UnsatisfiableConstraint + The given constraint is required and cannot be satisfied. + + */ + void addConstraint( const Constraint& constraint ) + { + if( m_cns.find( constraint ) != m_cns.end() ) + throw DuplicateConstraint( constraint ); + + // Creating a row causes symbols to be reserved for the variables + // in the constraint. If this method exits with an exception, + // then its possible those variables will linger in the var map. + // Since its likely that those variables will be used in other + // constraints and since exceptional conditions are uncommon, + // i'm not too worried about aggressive cleanup of the var map. + Tag tag; + std::unique_ptr rowptr( createRow( constraint, tag ) ); + Symbol subject( chooseSubject( *rowptr, tag ) ); + + // If chooseSubject could not find a valid entering symbol, one + // last option is available if the entire row is composed of + // dummy variables. If the constant of the row is zero, then + // this represents redundant constraints and the new dummy + // marker can enter the basis. If the constant is non-zero, + // then it represents an unsatisfiable constraint. + if( subject.type() == Symbol::Invalid && allDummies( *rowptr ) ) + { + if( !nearZero( rowptr->constant() ) ) + throw UnsatisfiableConstraint( constraint ); + else + subject = tag.marker; + } + + // If an entering symbol still isn't found, then the row must + // be added using an artificial variable. If that fails, then + // the row represents an unsatisfiable constraint. + if( subject.type() == Symbol::Invalid ) + { + if( !addWithArtificialVariable( *rowptr ) ) + throw UnsatisfiableConstraint( constraint ); + } + else + { + rowptr->solveFor( subject ); + substitute( subject, *rowptr ); + m_rows[ subject ] = rowptr.release(); + } + + m_cns[ constraint ] = tag; + + // Optimizing after each constraint is added performs less + // aggregate work due to a smaller average system size. It + // also ensures the solver remains in a consistent state. + optimize( *m_objective ); + } + + /* Remove a constraint from the solver. + + Throws + ------ + UnknownConstraint + The given constraint has not been added to the solver. + + */ + void removeConstraint( const Constraint& constraint ) + { + CnMap::iterator cn_it = m_cns.find( constraint ); + if( cn_it == m_cns.end() ) + throw UnknownConstraint( constraint ); + + Tag tag( cn_it->second ); + m_cns.erase( cn_it ); + + // Remove the error effects from the objective function + // *before* pivoting, or substitutions into the objective + // will lead to incorrect solver results. + removeConstraintEffects( constraint, tag ); + + // If the marker is basic, simply drop the row. Otherwise, + // pivot the marker into the basis and then drop the row. + RowMap::iterator row_it = m_rows.find( tag.marker ); + if( row_it != m_rows.end() ) + { + std::unique_ptr rowptr( row_it->second ); + m_rows.erase( row_it ); + } + else + { + row_it = getMarkerLeavingRow( tag.marker ); + if( row_it == m_rows.end() ) + throw InternalSolverError( "failed to find leaving row" ); + Symbol leaving( row_it->first ); + std::unique_ptr rowptr( row_it->second ); + m_rows.erase( row_it ); + rowptr->solveFor( leaving, tag.marker ); + substitute( tag.marker, *rowptr ); + } + + // Optimizing after each constraint is removed ensures that the + // solver remains consistent. It makes the solver api easier to + // use at a small tradeoff for speed. + optimize( *m_objective ); + } + + /* Test whether a constraint has been added to the solver. + + */ + bool hasConstraint( const Constraint& constraint ) const + { + return m_cns.find( constraint ) != m_cns.end(); + } + + /* Add an edit variable to the solver. + + This method should be called before the `suggestValue` method is + used to supply a suggested value for the given edit variable. + + Throws + ------ + DuplicateEditVariable + The given edit variable has already been added to the solver. + + BadRequiredStrength + The given strength is >= required. + + */ + void addEditVariable( const Variable& variable, double strength ) + { + if( m_edits.find( variable ) != m_edits.end() ) + throw DuplicateEditVariable( variable ); + strength = strength::clip( strength ); + if( strength == strength::required ) + throw BadRequiredStrength(); + Constraint cn( Expression( variable ), OP_EQ, strength ); + addConstraint( cn ); + EditInfo info; + info.tag = m_cns[ cn ]; + info.constraint = cn; + info.constant = 0.0; + m_edits[ variable ] = info; + } + + /* Remove an edit variable from the solver. + + Throws + ------ + UnknownEditVariable + The given edit variable has not been added to the solver. + + */ + void removeEditVariable( const Variable& variable ) + { + EditMap::iterator it = m_edits.find( variable ); + if( it == m_edits.end() ) + throw UnknownEditVariable( variable ); + removeConstraint( it->second.constraint ); + m_edits.erase( it ); + } + + /* Test whether an edit variable has been added to the solver. + + */ + bool hasEditVariable( const Variable& variable ) const + { + return m_edits.find( variable ) != m_edits.end(); + } + + /* Suggest a value for the given edit variable. + + This method should be used after an edit variable as been added to + the solver in order to suggest the value for that variable. + + Throws + ------ + UnknownEditVariable + The given edit variable has not been added to the solver. + + */ + void suggestValue( const Variable& variable, double value ) + { + EditMap::iterator it = m_edits.find( variable ); + if( it == m_edits.end() ) + throw UnknownEditVariable( variable ); + + DualOptimizeGuard guard( *this ); + EditInfo& info = it->second; + double delta = value - info.constant; + info.constant = value; + + // Check first if the positive error variable is basic. + RowMap::iterator row_it = m_rows.find( info.tag.marker ); + if( row_it != m_rows.end() ) + { + if( row_it->second->add( -delta ) < 0.0 ) + m_infeasible_rows.push_back( row_it->first ); + return; + } + + // Check next if the negative error variable is basic. + row_it = m_rows.find( info.tag.other ); + if( row_it != m_rows.end() ) + { + if( row_it->second->add( delta ) < 0.0 ) + m_infeasible_rows.push_back( row_it->first ); + return; + } + + // Otherwise update each row where the error variables exist. + RowMap::iterator end = m_rows.end(); + for( row_it = m_rows.begin(); row_it != end; ++row_it ) + { + double coeff = row_it->second->coefficientFor( info.tag.marker ); + if( coeff != 0.0 && + row_it->second->add( delta * coeff ) < 0.0 && + row_it->first.type() != Symbol::External ) + m_infeasible_rows.push_back( row_it->first ); + } + } + + /* Update the values of the external solver variables. + + */ + void updateVariables() + { + typedef RowMap::iterator row_iter_t; + typedef VarMap::iterator var_iter_t; + row_iter_t row_end = m_rows.end(); + var_iter_t var_end = m_vars.end(); + for( var_iter_t var_it = m_vars.begin(); var_it != var_end; ++var_it ) + { + Variable& var( const_cast( var_it->first ) ); + row_iter_t row_it = m_rows.find( var_it->second ); + if( row_it == row_end ) + var.setValue( 0.0 ); + else + var.setValue( row_it->second->constant() ); + } + } + + /* Reset the solver to the empty starting condition. + + This method resets the internal solver state to the empty starting + condition, as if no constraints or edit variables have been added. + This can be faster than deleting the solver and creating a new one + when the entire system must change, since it can avoid unecessary + heap (de)allocations. + + */ + void reset() + { + clearRows(); + m_cns.clear(); + m_vars.clear(); + m_edits.clear(); + m_infeasible_rows.clear(); + m_objective.reset( new Row() ); + m_artificial.reset(); + m_id_tick = 1; + } + +private: + + SolverImpl( const SolverImpl& ); + + SolverImpl& operator=( const SolverImpl& ); + + struct RowDeleter + { + template + void operator()( T& pair ) { delete pair.second; } + }; + + void clearRows() + { + std::for_each( m_rows.begin(), m_rows.end(), RowDeleter() ); + m_rows.clear(); + } + + /* Get the symbol for the given variable. + + If a symbol does not exist for the variable, one will be created. + + */ + Symbol getVarSymbol( const Variable& variable ) + { + VarMap::iterator it = m_vars.find( variable ); + if( it != m_vars.end() ) + return it->second; + Symbol symbol( Symbol::External, m_id_tick++ ); + m_vars[ variable ] = symbol; + return symbol; + } + + /* Create a new Row object for the given constraint. + + The terms in the constraint will be converted to cells in the row. + Any term in the constraint with a coefficient of zero is ignored. + This method uses the `getVarSymbol` method to get the symbol for + the variables added to the row. If the symbol for a given cell + variable is basic, the cell variable will be substituted with the + basic row. + + The necessary slack and error variables will be added to the row. + If the constant for the row is negative, the sign for the row + will be inverted so the constant becomes positive. + + The tag will be updated with the marker and error symbols to use + for tracking the movement of the constraint in the tableau. + + */ + Row* createRow( const Constraint& constraint, Tag& tag ) + { + typedef std::vector::const_iterator iter_t; + const Expression& expr( constraint.expression() ); + Row* row = new Row( expr.constant() ); + + // Substitute the current basic variables into the row. + iter_t end = expr.terms().end(); + for( iter_t it = expr.terms().begin(); it != end; ++it ) + { + if( !nearZero( it->coefficient() ) ) + { + Symbol symbol( getVarSymbol( it->variable() ) ); + RowMap::const_iterator row_it = m_rows.find( symbol ); + if( row_it != m_rows.end() ) + row->insert( *row_it->second, it->coefficient() ); + else + row->insert( symbol, it->coefficient() ); + } + } + + // Add the necessary slack, error, and dummy variables. + switch( constraint.op() ) + { + case OP_LE: + case OP_GE: + { + double coeff = constraint.op() == OP_LE ? 1.0 : -1.0; + Symbol slack( Symbol::Slack, m_id_tick++ ); + tag.marker = slack; + row->insert( slack, coeff ); + if( constraint.strength() < strength::required ) + { + Symbol error( Symbol::Error, m_id_tick++ ); + tag.other = error; + row->insert( error, -coeff ); + m_objective->insert( error, constraint.strength() ); + } + break; + } + case OP_EQ: + { + if( constraint.strength() < strength::required ) + { + Symbol errplus( Symbol::Error, m_id_tick++ ); + Symbol errminus( Symbol::Error, m_id_tick++ ); + tag.marker = errplus; + tag.other = errminus; + row->insert( errplus, -1.0 ); // v = eplus - eminus + row->insert( errminus, 1.0 ); // v - eplus + eminus = 0 + m_objective->insert( errplus, constraint.strength() ); + m_objective->insert( errminus, constraint.strength() ); + } + else + { + Symbol dummy( Symbol::Dummy, m_id_tick++ ); + tag.marker = dummy; + row->insert( dummy ); + } + break; + } + } + + // Ensure the row as a positive constant. + if( row->constant() < 0.0 ) + row->reverseSign(); + + return row; + } + + /* Choose the subject for solving for the row. + + This method will choose the best subject for using as the solve + target for the row. An invalid symbol will be returned if there + is no valid target. + + The symbols are chosen according to the following precedence: + + 1) The first symbol representing an external variable. + 2) A negative slack or error tag variable. + + If a subject cannot be found, an invalid symbol will be returned. + + */ + Symbol chooseSubject( const Row& row, const Tag& tag ) + { + typedef Row::CellMap::const_iterator iter_t; + iter_t end = row.cells().end(); + for( iter_t it = row.cells().begin(); it != end; ++it ) + { + if( it->first.type() == Symbol::External ) + return it->first; + } + if( tag.marker.type() == Symbol::Slack || tag.marker.type() == Symbol::Error ) + { + if( row.coefficientFor( tag.marker ) < 0.0 ) + return tag.marker; + } + if( tag.other.type() == Symbol::Slack || tag.other.type() == Symbol::Error ) + { + if( row.coefficientFor( tag.other ) < 0.0 ) + return tag.other; + } + return Symbol(); + } + + /* Add the row to the tableau using an artificial variable. + + This will return false if the constraint cannot be satisfied. + + */ + bool addWithArtificialVariable( const Row& row ) + { + // Create and add the artificial variable to the tableau + Symbol art( Symbol::Slack, m_id_tick++ ); + m_rows[ art ] = new Row( row ); + m_artificial.reset( new Row( row ) ); + + // Optimize the artificial objective. This is successful + // only if the artificial objective is optimized to zero. + optimize( *m_artificial ); + bool success = nearZero( m_artificial->constant() ); + m_artificial.reset(); + + // If the artificial variable is not basic, pivot the row so that + // it becomes basic. If the row is constant, exit early. + RowMap::iterator it = m_rows.find( art ); + if( it != m_rows.end() ) + { + std::unique_ptr rowptr( it->second ); + m_rows.erase( it ); + if( rowptr->cells().empty() ) + return success; + Symbol entering( anyPivotableSymbol( *rowptr ) ); + if( entering.type() == Symbol::Invalid ) + return false; // unsatisfiable (will this ever happen?) + rowptr->solveFor( art, entering ); + substitute( entering, *rowptr ); + m_rows[ entering ] = rowptr.release(); + } + + // Remove the artificial variable from the tableau. + RowMap::iterator end = m_rows.end(); + for( it = m_rows.begin(); it != end; ++it ) + it->second->remove( art ); + m_objective->remove( art ); + return success; + } + + /* Substitute the parametric symbol with the given row. + + This method will substitute all instances of the parametric symbol + in the tableau and the objective function with the given row. + + */ + void substitute( const Symbol& symbol, const Row& row ) + { + typedef RowMap::iterator iter_t; + iter_t end = m_rows.end(); + for( iter_t it = m_rows.begin(); it != end; ++it ) + { + it->second->substitute( symbol, row ); + if( it->first.type() != Symbol::External && + it->second->constant() < 0.0 ) + m_infeasible_rows.push_back( it->first ); + } + m_objective->substitute( symbol, row ); + if( m_artificial.get() ) + m_artificial->substitute( symbol, row ); + } + + /* Optimize the system for the given objective function. + + This method performs iterations of Phase 2 of the simplex method + until the objective function reaches a minimum. + + Throws + ------ + InternalSolverError + The value of the objective function is unbounded. + + */ + void optimize( const Row& objective ) + { + while( true ) + { + Symbol entering( getEnteringSymbol( objective ) ); + if( entering.type() == Symbol::Invalid ) + return; + RowMap::iterator it = getLeavingRow( entering ); + if( it == m_rows.end() ) + throw InternalSolverError( "The objective is unbounded." ); + // pivot the entering symbol into the basis + Symbol leaving( it->first ); + Row* row = it->second; + m_rows.erase( it ); + row->solveFor( leaving, entering ); + substitute( entering, *row ); + m_rows[ entering ] = row; + } + } + + /* Optimize the system using the dual of the simplex method. + + The current state of the system should be such that the objective + function is optimal, but not feasible. This method will perform + an iteration of the dual simplex method to make the solution both + optimal and feasible. + + Throws + ------ + InternalSolverError + The system cannot be dual optimized. + + */ + void dualOptimize() + { + while( !m_infeasible_rows.empty() ) + { + + Symbol leaving( m_infeasible_rows.back() ); + m_infeasible_rows.pop_back(); + RowMap::iterator it = m_rows.find( leaving ); + if( it != m_rows.end() && !nearZero( it->second->constant() ) && + it->second->constant() < 0.0 ) + { + Symbol entering( getDualEnteringSymbol( *it->second ) ); + if( entering.type() == Symbol::Invalid ) + throw InternalSolverError( "Dual optimize failed." ); + // pivot the entering symbol into the basis + Row* row = it->second; + m_rows.erase( it ); + row->solveFor( leaving, entering ); + substitute( entering, *row ); + m_rows[ entering ] = row; + } + } + } + + /* Compute the entering variable for a pivot operation. + + This method will return first symbol in the objective function which + is non-dummy and has a coefficient less than zero. If no symbol meets + the criteria, it means the objective function is at a minimum, and an + invalid symbol is returned. + + */ + Symbol getEnteringSymbol( const Row& objective ) + { + typedef Row::CellMap::const_iterator iter_t; + iter_t end = objective.cells().end(); + for( iter_t it = objective.cells().begin(); it != end; ++it ) + { + if( it->first.type() != Symbol::Dummy && it->second < 0.0 ) + return it->first; + } + return Symbol(); + } + + /* Compute the entering symbol for the dual optimize operation. + + This method will return the symbol in the row which has a positive + coefficient and yields the minimum ratio for its respective symbol + in the objective function. The provided row *must* be infeasible. + If no symbol is found which meats the criteria, an invalid symbol + is returned. + + */ + Symbol getDualEnteringSymbol( const Row& row ) + { + typedef Row::CellMap::const_iterator iter_t; + Symbol entering; + double ratio = std::numeric_limits::max(); + iter_t end = row.cells().end(); + for( iter_t it = row.cells().begin(); it != end; ++it ) + { + if( it->second > 0.0 && it->first.type() != Symbol::Dummy ) + { + double coeff = m_objective->coefficientFor( it->first ); + double r = coeff / it->second; + if( r < ratio ) + { + ratio = r; + entering = it->first; + } + } + } + return entering; + } + + /* Get the first Slack or Error symbol in the row. + + If no such symbol is present, and Invalid symbol will be returned. + + */ + Symbol anyPivotableSymbol( const Row& row ) + { + typedef Row::CellMap::const_iterator iter_t; + iter_t end = row.cells().end(); + for( iter_t it = row.cells().begin(); it != end; ++it ) + { + const Symbol& sym( it->first ); + if( sym.type() == Symbol::Slack || sym.type() == Symbol::Error ) + return sym; + } + return Symbol(); + } + + /* Compute the row which holds the exit symbol for a pivot. + + This method will return an iterator to the row in the row map + which holds the exit symbol. If no appropriate exit symbol is + found, the end() iterator will be returned. This indicates that + the objective function is unbounded. + + */ + RowMap::iterator getLeavingRow( const Symbol& entering ) + { + typedef RowMap::iterator iter_t; + double ratio = std::numeric_limits::max(); + iter_t end = m_rows.end(); + iter_t found = m_rows.end(); + for( iter_t it = m_rows.begin(); it != end; ++it ) + { + if( it->first.type() != Symbol::External ) + { + double temp = it->second->coefficientFor( entering ); + if( temp < 0.0 ) + { + double temp_ratio = -it->second->constant() / temp; + if( temp_ratio < ratio ) + { + ratio = temp_ratio; + found = it; + } + } + } + } + return found; + } + + /* Compute the leaving row for a marker variable. + + This method will return an iterator to the row in the row map + which holds the given marker variable. The row will be chosen + according to the following precedence: + + 1) The row with a restricted basic varible and a negative coefficient + for the marker with the smallest ratio of -constant / coefficient. + + 2) The row with a restricted basic variable and the smallest ratio + of constant / coefficient. + + 3) The last unrestricted row which contains the marker. + + If the marker does not exist in any row, the row map end() iterator + will be returned. This indicates an internal solver error since + the marker *should* exist somewhere in the tableau. + + */ + RowMap::iterator getMarkerLeavingRow( const Symbol& marker ) + { + const double dmax = std::numeric_limits::max(); + typedef RowMap::iterator iter_t; + double r1 = dmax; + double r2 = dmax; + iter_t end = m_rows.end(); + iter_t first = end; + iter_t second = end; + iter_t third = end; + for( iter_t it = m_rows.begin(); it != end; ++it ) + { + double c = it->second->coefficientFor( marker ); + if( c == 0.0 ) + continue; + if( it->first.type() == Symbol::External ) + { + third = it; + } + else if( c < 0.0 ) + { + double r = -it->second->constant() / c; + if( r < r1 ) + { + r1 = r; + first = it; + } + } + else + { + double r = it->second->constant() / c; + if( r < r2 ) + { + r2 = r; + second = it; + } + } + } + if( first != end ) + return first; + if( second != end ) + return second; + return third; + } + + /* Remove the effects of a constraint on the objective function. + + */ + void removeConstraintEffects( const Constraint& cn, const Tag& tag ) + { + if( tag.marker.type() == Symbol::Error ) + removeMarkerEffects( tag.marker, cn.strength() ); + if( tag.other.type() == Symbol::Error ) + removeMarkerEffects( tag.other, cn.strength() ); + } + + /* Remove the effects of an error marker on the objective function. + + */ + void removeMarkerEffects( const Symbol& marker, double strength ) + { + RowMap::iterator row_it = m_rows.find( marker ); + if( row_it != m_rows.end() ) + m_objective->insert( *row_it->second, -strength ); + else + m_objective->insert( marker, -strength ); + } + + /* Test whether a row is composed of all dummy variables. + + */ + bool allDummies( const Row& row ) + { + typedef Row::CellMap::const_iterator iter_t; + iter_t end = row.cells().end(); + for( iter_t it = row.cells().begin(); it != end; ++it ) + { + if( it->first.type() != Symbol::Dummy ) + return false; + } + return true; + } + + CnMap m_cns; + RowMap m_rows; + VarMap m_vars; + EditMap m_edits; + std::vector m_infeasible_rows; + std::unique_ptr m_objective; + std::unique_ptr m_artificial; + Symbol::Id m_id_tick; +}; + +} // namespace impl + +} // namespace kiwi diff --git a/thirdparty/include/kiwi/strength.h b/thirdparty/include/kiwi/strength.h new file mode 100644 index 000000000..11732e98f --- /dev/null +++ b/thirdparty/include/kiwi/strength.h @@ -0,0 +1,44 @@ +/*----------------------------------------------------------------------------- +| Copyright (c) 2013-2017, Nucleic Development Team. +| +| Distributed under the terms of the Modified BSD License. +| +| The full license is in the file LICENSE, distributed with this software. +|----------------------------------------------------------------------------*/ +#pragma once +#include + + +namespace kiwi +{ + +namespace strength +{ + +inline double create( double a, double b, double c, double w = 1.0 ) +{ + double result = 0.0; + result += std::max( 0.0, std::min( 1000.0, a * w ) ) * 1000000.0; + result += std::max( 0.0, std::min( 1000.0, b * w ) ) * 1000.0; + result += std::max( 0.0, std::min( 1000.0, c * w ) ); + return result; +} + + +const double required = create( 1000.0, 1000.0, 1000.0 ); + +const double strong = create( 1.0, 0.0, 0.0 ); + +const double medium = create( 0.0, 1.0, 0.0 ); + +const double weak = create( 0.0, 0.0, 1.0 ); + + +inline double clip( double value ) +{ + return std::max( 0.0, std::min( required, value ) ); +} + +} // namespace strength + +} // namespace kiwi diff --git a/thirdparty/include/kiwi/symbol.h b/thirdparty/include/kiwi/symbol.h new file mode 100644 index 000000000..ec422ad12 --- /dev/null +++ b/thirdparty/include/kiwi/symbol.h @@ -0,0 +1,68 @@ +/*----------------------------------------------------------------------------- +| Copyright (c) 2013-2017, Nucleic Development Team. +| +| Distributed under the terms of the Modified BSD License. +| +| The full license is in the file LICENSE, distributed with this software. +|----------------------------------------------------------------------------*/ +#pragma once + + +namespace kiwi +{ + +namespace impl +{ + +class Symbol +{ + +public: + + typedef unsigned long long Id; + + enum Type + { + Invalid, + External, + Slack, + Error, + Dummy + }; + + Symbol() : m_id( 0 ), m_type( Invalid ) {} + + Symbol( Type type, Id id ) : m_id( id ), m_type( type ) {} + + ~Symbol() {} + + Id id() const + { + return m_id; + } + + Type type() const + { + return m_type; + } + +private: + + Id m_id; + Type m_type; + + friend bool operator<( const Symbol& lhs, const Symbol& rhs ) + { + return lhs.m_id < rhs.m_id; + } + + friend bool operator==( const Symbol& lhs, const Symbol& rhs ) + { + return lhs.m_id == rhs.m_id; + } + +}; + +} // namespace impl + +} // namespace kiwi diff --git a/thirdparty/include/kiwi/symbolics.h b/thirdparty/include/kiwi/symbolics.h new file mode 100644 index 000000000..23eed60f5 --- /dev/null +++ b/thirdparty/include/kiwi/symbolics.h @@ -0,0 +1,685 @@ +/*----------------------------------------------------------------------------- +| Copyright (c) 2013-2017, Nucleic Development Team. +| +| Distributed under the terms of the Modified BSD License. +| +| The full license is in the file LICENSE, distributed with this software. +|----------------------------------------------------------------------------*/ +#pragma once +#include +#include "constraint.h" +#include "expression.h" +#include "term.h" +#include "variable.h" + + +namespace kiwi +{ + +// Variable multiply, divide, and unary invert + +inline +Term operator*( const Variable& variable, double coefficient ) +{ + return Term( variable, coefficient ); +} + + +inline +Term operator/( const Variable& variable, double denominator ) +{ + return variable * ( 1.0 / denominator ); +} + + +inline +Term operator-( const Variable& variable ) +{ + return variable * -1.0; +} + + +// Term multiply, divide, and unary invert + +inline +Term operator*( const Term& term, double coefficient ) +{ + return Term( term.variable(), term.coefficient() * coefficient ); +} + + +inline +Term operator/( const Term& term, double denominator ) +{ + return term * ( 1.0 / denominator ); +} + + +inline +Term operator-( const Term& term ) +{ + return term * -1.0; +} + + +// Expression multiply, divide, and unary invert + +inline +Expression operator*( const Expression& expression, double coefficient ) +{ + std::vector terms; + terms.reserve( expression.terms().size() ); + typedef std::vector::const_iterator iter_t; + iter_t begin = expression.terms().begin(); + iter_t end = expression.terms().end(); + for( iter_t it = begin; it != end; ++it ) + terms.push_back( ( *it ) * coefficient ); + return Expression( terms, expression.constant() * coefficient ); +} + + +inline +Expression operator/( const Expression& expression, double denominator ) +{ + return expression * ( 1.0 / denominator ); +} + + +inline +Expression operator-( const Expression& expression ) +{ + return expression * -1.0; +} + + +// Double multiply + +inline +Expression operator*( double coefficient, const Expression& expression ) +{ + return expression * coefficient; +} + + +inline +Term operator*( double coefficient, const Term& term ) +{ + return term * coefficient; +} + + +inline +Term operator*( double coefficient, const Variable& variable ) +{ + return variable * coefficient; +} + + +// Expression add and subtract + +inline +Expression operator+( const Expression& first, const Expression& second ) +{ + std::vector terms; + terms.reserve( first.terms().size() + second.terms().size() ); + terms.insert( terms.begin(), first.terms().begin(), first.terms().end() ); + terms.insert( terms.end(), second.terms().begin(), second.terms().end() ); + return Expression( terms, first.constant() + second.constant() ); +} + + +inline +Expression operator+( const Expression& first, const Term& second ) +{ + std::vector terms; + terms.reserve( first.terms().size() + 1 ); + terms.insert( terms.begin(), first.terms().begin(), first.terms().end() ); + terms.push_back( second ); + return Expression( terms, first.constant() ); +} + + +inline +Expression operator+( const Expression& expression, const Variable& variable ) +{ + return expression + Term( variable ); +} + + +inline +Expression operator+( const Expression& expression, double constant ) +{ + return Expression( expression.terms(), expression.constant() + constant ); +} + + +inline +Expression operator-( const Expression& first, const Expression& second ) +{ + return first + -second; +} + + +inline +Expression operator-( const Expression& expression, const Term& term ) +{ + return expression + -term; +} + + +inline +Expression operator-( const Expression& expression, const Variable& variable ) +{ + return expression + -variable; +} + + +inline +Expression operator-( const Expression& expression, double constant ) +{ + return expression + -constant; +} + + +// Term add and subtract + +inline +Expression operator+( const Term& term, const Expression& expression ) +{ + return expression + term; +} + + +inline +Expression operator+( const Term& first, const Term& second ) +{ + std::vector terms; + terms.reserve( 2 ); + terms.push_back( first ); + terms.push_back( second ); + return Expression( terms ); +} + + +inline +Expression operator+( const Term& term, const Variable& variable ) +{ + return term + Term( variable ); +} + + +inline +Expression operator+( const Term& term, double constant ) +{ + return Expression( term, constant ); +} + + +inline +Expression operator-( const Term& term, const Expression& expression ) +{ + return -expression + term; +} + + +inline +Expression operator-( const Term& first, const Term& second ) +{ + return first + -second; +} + + +inline +Expression operator-( const Term& term, const Variable& variable ) +{ + return term + -variable; +} + + +inline +Expression operator-( const Term& term, double constant ) +{ + return term + -constant; +} + + +// Variable add and subtract + +inline +Expression operator+( const Variable& variable, const Expression& expression ) +{ + return expression + variable; +} + + +inline +Expression operator+( const Variable& variable, const Term& term ) +{ + return term + variable; +} + + +inline +Expression operator+( const Variable& first, const Variable& second ) +{ + return Term( first ) + second; +} + + +inline +Expression operator+( const Variable& variable, double constant ) +{ + return Term( variable ) + constant; +} + + +inline +Expression operator-( const Variable& variable, const Expression& expression ) +{ + return variable + -expression; +} + + +inline +Expression operator-( const Variable& variable, const Term& term ) +{ + return variable + -term; +} + + +inline +Expression operator-( const Variable& first, const Variable& second ) +{ + return first + -second; +} + + +inline +Expression operator-( const Variable& variable, double constant ) +{ + return variable + -constant; +} + + +// Double add and subtract + +inline +Expression operator+( double constant, const Expression& expression ) +{ + return expression + constant; +} + + +inline +Expression operator+( double constant, const Term& term ) +{ + return term + constant; +} + + +inline +Expression operator+( double constant, const Variable& variable ) +{ + return variable + constant; +} + + +inline +Expression operator-( double constant, const Expression& expression ) +{ + return -expression + constant; +} + + +inline +Expression operator-( double constant, const Term& term ) +{ + return -term + constant; +} + + +inline +Expression operator-( double constant, const Variable& variable ) +{ + return -variable + constant; +} + + +// Expression relations + +inline +Constraint operator==( const Expression& first, const Expression& second ) +{ + return Constraint( first - second, OP_EQ ); +} + + +inline +Constraint operator==( const Expression& expression, const Term& term ) +{ + return expression == Expression( term ); +} + + +inline +Constraint operator==( const Expression& expression, const Variable& variable ) +{ + return expression == Term( variable ); +} + + +inline +Constraint operator==( const Expression& expression, double constant ) +{ + return expression == Expression( constant ); +} + + +inline +Constraint operator<=( const Expression& first, const Expression& second ) +{ + return Constraint( first - second, OP_LE ); +} + + +inline +Constraint operator<=( const Expression& expression, const Term& term ) +{ + return expression <= Expression( term ); +} + + +inline +Constraint operator<=( const Expression& expression, const Variable& variable ) +{ + return expression <= Term( variable ); +} + + +inline +Constraint operator<=( const Expression& expression, double constant ) +{ + return expression <= Expression( constant ); +} + + +inline +Constraint operator>=( const Expression& first, const Expression& second ) +{ + return Constraint( first - second, OP_GE ); +} + + +inline +Constraint operator>=( const Expression& expression, const Term& term ) +{ + return expression >= Expression( term ); +} + + +inline +Constraint operator>=( const Expression& expression, const Variable& variable ) +{ + return expression >= Term( variable ); +} + + +inline +Constraint operator>=( const Expression& expression, double constant ) +{ + return expression >= Expression( constant ); +} + + +// Term relations + +inline +Constraint operator==( const Term& term, const Expression& expression ) +{ + return expression == term; +} + + +inline +Constraint operator==( const Term& first, const Term& second ) +{ + return Expression( first ) == second; +} + + +inline +Constraint operator==( const Term& term, const Variable& variable ) +{ + return Expression( term ) == variable; +} + + +inline +Constraint operator==( const Term& term, double constant ) +{ + return Expression( term ) == constant; +} + + +inline +Constraint operator<=( const Term& term, const Expression& expression ) +{ + return expression >= term; +} + + +inline +Constraint operator<=( const Term& first, const Term& second ) +{ + return Expression( first ) <= second; +} + + +inline +Constraint operator<=( const Term& term, const Variable& variable ) +{ + return Expression( term ) <= variable; +} + + +inline +Constraint operator<=( const Term& term, double constant ) +{ + return Expression( term ) <= constant; +} + + +inline +Constraint operator>=( const Term& term, const Expression& expression ) +{ + return expression <= term; +} + + +inline +Constraint operator>=( const Term& first, const Term& second ) +{ + return Expression( first ) >= second; +} + + +inline +Constraint operator>=( const Term& term, const Variable& variable ) +{ + return Expression( term ) >= variable; +} + + +inline +Constraint operator>=( const Term& term, double constant ) +{ + return Expression( term ) >= constant; +} + + +// Variable relations +inline +Constraint operator==( const Variable& variable, const Expression& expression ) +{ + return expression == variable; +} + + +inline +Constraint operator==( const Variable& variable, const Term& term ) +{ + return term == variable; +} + + +inline +Constraint operator==( const Variable& first, const Variable& second ) +{ + return Term( first ) == second; +} + + +inline +Constraint operator==( const Variable& variable, double constant ) +{ + return Term( variable ) == constant; +} + + +inline +Constraint operator<=( const Variable& variable, const Expression& expression ) +{ + return expression >= variable; +} + + +inline +Constraint operator<=( const Variable& variable, const Term& term ) +{ + return term >= variable; +} + + +inline +Constraint operator<=( const Variable& first, const Variable& second ) +{ + return Term( first ) <= second; +} + + +inline +Constraint operator<=( const Variable& variable, double constant ) +{ + return Term( variable ) <= constant; +} + + +inline +Constraint operator>=( const Variable& variable, const Expression& expression ) +{ + return expression <= variable; +} + + +inline +Constraint operator>=( const Variable& variable, const Term& term ) +{ + return term <= variable; +} + + +inline +Constraint operator>=( const Variable& first, const Variable& second ) +{ + return Term( first ) >= second; +} + + +inline +Constraint operator>=( const Variable& variable, double constant ) +{ + return Term( variable ) >= constant; +} + + +// Double relations + +inline +Constraint operator==( double constant, const Expression& expression ) +{ + return expression == constant; +} + + +inline +Constraint operator==( double constant, const Term& term ) +{ + return term == constant; +} + + +inline +Constraint operator==( double constant, const Variable& variable ) +{ + return variable == constant; +} + + +inline +Constraint operator<=( double constant, const Expression& expression ) +{ + return expression >= constant; +} + + +inline +Constraint operator<=( double constant, const Term& term ) +{ + return term >= constant; +} + + +inline +Constraint operator<=( double constant, const Variable& variable ) +{ + return variable >= constant; +} + + +inline +Constraint operator>=( double constant, const Expression& expression ) +{ + return expression <= constant; +} + + +inline +Constraint operator>=( double constant, const Term& term ) +{ + return term <= constant; +} + + +inline +Constraint operator>=( double constant, const Variable& variable ) +{ + return variable <= constant; +} + + +// Constraint strength modifier + +inline +Constraint operator|( const Constraint& constraint, double strength ) +{ + return Constraint( constraint, strength ); +} + + +inline +Constraint operator|( double strength, const Constraint& constraint ) +{ + return constraint | strength; +} + +} // namespace kiwi diff --git a/thirdparty/include/kiwi/term.h b/thirdparty/include/kiwi/term.h new file mode 100644 index 000000000..aecfdf06d --- /dev/null +++ b/thirdparty/include/kiwi/term.h @@ -0,0 +1,51 @@ +/*----------------------------------------------------------------------------- +| Copyright (c) 2013-2017, Nucleic Development Team. +| +| Distributed under the terms of the Modified BSD License. +| +| The full license is in the file LICENSE, distributed with this software. +|----------------------------------------------------------------------------*/ +#pragma once +#include +#include "variable.h" + + +namespace kiwi +{ + +class Term +{ + +public: + + Term( const Variable& variable, double coefficient = 1.0 ) : + m_variable( variable ), m_coefficient( coefficient ) {} + + // to facilitate efficient map -> vector copies + Term( const std::pair& pair ) : + m_variable( pair.first ), m_coefficient( pair.second ) {} + + ~Term() {} + + const Variable& variable() const + { + return m_variable; + } + + double coefficient() const + { + return m_coefficient; + } + + double value() const + { + return m_coefficient * m_variable.value(); + } + +private: + + Variable m_variable; + double m_coefficient; +}; + +} // namespace kiwi diff --git a/thirdparty/include/kiwi/util.h b/thirdparty/include/kiwi/util.h new file mode 100644 index 000000000..560a43a74 --- /dev/null +++ b/thirdparty/include/kiwi/util.h @@ -0,0 +1,24 @@ +/*----------------------------------------------------------------------------- +| Copyright (c) 2013-2017, Nucleic Development Team. +| +| Distributed under the terms of the Modified BSD License. +| +| The full license is in the file LICENSE, distributed with this software. +|----------------------------------------------------------------------------*/ +#pragma once + +namespace kiwi +{ + +namespace impl +{ + +inline bool nearZero(double value) +{ + const double eps = 1.0e-8; + return value < 0.0 ? -value < eps : value < eps; +} + +} // namespace impl + +} // namespace kiwi diff --git a/thirdparty/include/kiwi/variable.h b/thirdparty/include/kiwi/variable.h new file mode 100644 index 000000000..a4db77706 --- /dev/null +++ b/thirdparty/include/kiwi/variable.h @@ -0,0 +1,111 @@ +/*----------------------------------------------------------------------------- +| Copyright (c) 2013-2017, Nucleic Development Team. +| +| Distributed under the terms of the Modified BSD License. +| +| The full license is in the file LICENSE, distributed with this software. +|----------------------------------------------------------------------------*/ +#pragma once +#include +#include +#include "shareddata.h" + +namespace kiwi +{ + +class Variable +{ + +public: + class Context + { + public: + Context() {} + virtual ~Context() {} // LCOV_EXCL_LINE + }; + + Variable(Context *context = 0) : m_data(new VariableData("", context)) {} + + Variable(const std::string &name, Context *context = 0) : m_data(new VariableData(name, context)) {} + + Variable(const char *name, Context *context = 0) : m_data(new VariableData(name, context)) {} + + ~Variable() {} + + const std::string &name() const + { + return m_data->m_name; + } + + void setName(const char *name) + { + m_data->m_name = name; + } + + void setName(const std::string &name) + { + m_data->m_name = name; + } + + Context *context() const + { + return m_data->m_context.get(); + } + + void setContext(Context *context) + { + m_data->m_context.reset(context); + } + + double value() const + { + return m_data->m_value; + } + + void setValue(double value) + { + m_data->m_value = value; + } + + // operator== is used for symbolics + bool equals(const Variable &other) + { + return m_data == other.m_data; + } + +private: + class VariableData : public SharedData + { + + public: + VariableData(const std::string &name, Context *context) : SharedData(), + m_name(name), + m_context(context), + m_value(0.0) {} + + VariableData(const char *name, Context *context) : SharedData(), + m_name(name), + m_context(context), + m_value(0.0) {} + + ~VariableData() {} + + std::string m_name; + std::unique_ptr m_context; + double m_value; + + private: + VariableData(const VariableData &other); + + VariableData &operator=(const VariableData &other); + }; + + SharedDataPtr m_data; + + friend bool operator<(const Variable &lhs, const Variable &rhs) + { + return lhs.m_data < rhs.m_data; + } +}; + +} // namespace kiwi diff --git a/thirdparty/include/kiwi/version.h b/thirdparty/include/kiwi/version.h new file mode 100644 index 000000000..f6154482a --- /dev/null +++ b/thirdparty/include/kiwi/version.h @@ -0,0 +1,14 @@ +/*----------------------------------------------------------------------------- +| Copyright (c) 2013-2020, Nucleic Development Team. +| +| Distributed under the terms of the Modified BSD License. +| +| The full license is in the file LICENSE, distributed with this software. +|----------------------------------------------------------------------------*/ +#pragma once + +#define KIWI_MAJOR_VERSION 1 +#define KIWI_MINOR_VERSION 2 +#define KIWI_MICRO_VERSION 0 +#define KIWI_VERSION_HEX 0x010200 +#define KIWI_VERSION "1.2.0" diff --git a/thirdparty/lib/common/x64/Newton_d.dll.REMOVED.git-id b/thirdparty/lib/common/x64/Newton_d.dll.REMOVED.git-id deleted file mode 100644 index 23b5690f0..000000000 --- a/thirdparty/lib/common/x64/Newton_d.dll.REMOVED.git-id +++ /dev/null @@ -1 +0,0 @@ -0248a702d5e35c14dc8adfb6f0d4ba7d18d90c8b \ No newline at end of file diff --git a/thirdparty/lib/common/x86/Newton_d.dll.REMOVED.git-id b/thirdparty/lib/common/x86/Newton_d.dll.REMOVED.git-id deleted file mode 100644 index 52c39a266..000000000 --- a/thirdparty/lib/common/x86/Newton_d.dll.REMOVED.git-id +++ /dev/null @@ -1 +0,0 @@ -c1629ecc8c31a6a8f6648448bc655824e35435b0 \ No newline at end of file diff --git a/thirdparty/lib/gmake/x64/libNewton.a.REMOVED.git-id b/thirdparty/lib/gmake/x64/libNewton.a.REMOVED.git-id deleted file mode 100644 index f5d53440c..000000000 --- a/thirdparty/lib/gmake/x64/libNewton.a.REMOVED.git-id +++ /dev/null @@ -1 +0,0 @@ -f79ffcc0c10c1bffafba367583e3675a15fa9167 \ No newline at end of file diff --git a/thirdparty/lib/gmake/x86/libNewton.a.REMOVED.git-id b/thirdparty/lib/gmake/x86/libNewton.a.REMOVED.git-id deleted file mode 100644 index a34f1f96c..000000000 --- a/thirdparty/lib/gmake/x86/libNewton.a.REMOVED.git-id +++ /dev/null @@ -1 +0,0 @@ -50c86d0a541fd1757e41ef7f8a368d969395fdf6 \ No newline at end of file diff --git a/thirdparty/src/newton/dContainers/CMakeLists.txt b/thirdparty/src/newton/dContainers/CMakeLists.txt new file mode 100644 index 000000000..24b86806d --- /dev/null +++ b/thirdparty/src/newton/dContainers/CMakeLists.txt @@ -0,0 +1,61 @@ +# Copyright (c) <2014-2017> +# +# 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 $ ${PROJECT_BINARY_DIR}/applications/demosSandbox/${CMAKE_CFG_INTDIR}/$) + endif () +endif(MSVC) + +install(TARGETS ${projectName} + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin) + +install(FILES ${HEADERS} DESTINATION include/${projectName}) + diff --git a/thirdparty/src/newton/dContainers/dArray.h b/thirdparty/src/newton/dContainers/dArray.h new file mode 100644 index 000000000..007b05482 --- /dev/null +++ b/thirdparty/src/newton/dContainers/dArray.h @@ -0,0 +1,114 @@ +/* Copyright (c) <2003-2019> +* +* 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 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 +dArray::dArray() + :m_capacity(0) + ,m_data(NULL) +{ +} + +template +dArray::dArray(int size) + :dContainersAlloc() + ,m_capacity(0) + ,m_data(NULL) +{ + Resize(size); +} + + +template +dArray::~dArray() +{ + if (m_data) { + delete[] m_data; + } +} + + +template +T& dArray::operator[] (int i) +{ + dAssert(i >= 0); + while (i >= m_capacity) { + Resize(dMax (i * 2, 1)); + } + return m_data[i]; +} + +template +const T& dArray::operator[] (int i) const +{ + dAssert(i >= 0); + while (i >= m_capacity) { + Resize(dMax (i * 2, 1)); + } + return m_data[i]; +} + +template +int dArray ::GetSize() const +{ + return m_capacity; +} + +template +void dArray ::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 diff --git a/thirdparty/src/newton/dContainers/dBaseHierarchy.cpp b/thirdparty/src/newton/dContainers/dBaseHierarchy.cpp new file mode 100644 index 000000000..710500985 --- /dev/null +++ b/thirdparty/src/newton/dContainers/dBaseHierarchy.cpp @@ -0,0 +1,148 @@ +/* Copyright (c) <2003-2019> +* +* 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; +} + + + + + + + + + diff --git a/thirdparty/src/newton/dContainers/dBaseHierarchy.h b/thirdparty/src/newton/dContainers/dBaseHierarchy.h new file mode 100644 index 000000000..06e5d6a96 --- /dev/null +++ b/thirdparty/src/newton/dContainers/dBaseHierarchy.h @@ -0,0 +1,256 @@ +/* Copyright (c) <2003-2019> +* +* 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 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 +dHierarchy::dHierarchy () + :dBaseHierarchy () +{ +} + +template +dHierarchy::dHierarchy (const T &clone) + :dBaseHierarchy (clone) +{ +} + +template +dHierarchy::dHierarchy (const char* const name) + :dBaseHierarchy (name) +{ +} + +template +dHierarchy::~dHierarchy () +{ +} + + +//template +//dBaseHierarchy* dHierarchy::CreateClone () const +//{ +// return new T (*(T*)this); +//} + +template +void dHierarchy::Attach (T* const parent, bool addFirst) +{ + dBaseHierarchy::Attach(parent, addFirst); +} + +template +void dHierarchy::Detach () +{ + dBaseHierarchy::Detach (); +} + +template +T* dHierarchy::GetChild () const +{ + return (T*) dBaseHierarchy::GetChild(); +} + +template +T* dHierarchy::GetSibling () const +{ + return (T*) dBaseHierarchy::GetSibling (); +} + +template +T* dHierarchy::GetParent () const +{ + return (T*) dBaseHierarchy::GetParent (); +} + + +template +T* dHierarchy::GetRoot () const +{ + return (T*) dBaseHierarchy::GetRoot (); +} + + +template +T* dHierarchy::GetFirst() const +{ + return (T*) dBaseHierarchy::GetFirst (); +} + +template +T* dHierarchy::GetLast() const +{ + return (T*) dBaseHierarchy::GetLast (); +} + + +template +T* dHierarchy::GetNext() const +{ + return (T*) dBaseHierarchy::GetNext (); +} + +template +T* dHierarchy::GetPrev() const +{ + return (T*) dBaseHierarchy::GetPrev (); +} + + +template +T* dHierarchy::Find (dCRCTYPE nameCRC) const +{ + return (T*) dBaseHierarchy::Find (nameCRC); +} + +template +T* dHierarchy::Find (const char* const name) const +{ + return (T*) dBaseHierarchy::Find (name); +} + + +#endif + diff --git a/thirdparty/src/newton/dContainers/dBezierSpline.cpp b/thirdparty/src/newton/dContainers/dBezierSpline.cpp new file mode 100644 index 000000000..6b90aecd8 --- /dev/null +++ b/thirdparty/src/newton/dContainers/dBezierSpline.cpp @@ -0,0 +1,641 @@ +/* Copyright (c) <2003-2019> +* +* 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 ©) +{ + Clear(); + if (copy.m_knotsCount) { + CreateFromKnotVectorAndControlPoints (copy.m_degree, copy.m_knotsCount - 2 * copy.m_degree, ©.m_knotVector[copy.m_degree], ©.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& dBezierSpline::GetKnotArray() +{ + return m_knotVector; +} + +const dArray& 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& dBezierSpline::GetControlPointArray() +{ + return m_controlPoints; +} + +const dArray& 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; +} diff --git a/thirdparty/src/newton/dContainers/dBezierSpline.h b/thirdparty/src/newton/dContainers/dBezierSpline.h new file mode 100644 index 000000000..346b32248 --- /dev/null +++ b/thirdparty/src/newton/dContainers/dBezierSpline.h @@ -0,0 +1,79 @@ +/* Copyright (c) <2003-2019> +* +* 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 ©) ; + + 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& GetControlPointArray(); + DCONTAINERS_API const dArray& 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& GetKnotArray(); + DCONTAINERS_API const dArray& 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 m_knotVector; + dArray m_controlPoints; + + int m_degree; + int m_knotsCount; + int m_controlPointsCount; +}; +#endif + diff --git a/thirdparty/src/newton/dContainers/dCRC.cpp b/thirdparty/src/newton/dContainers/dCRC.cpp new file mode 100644 index 000000000..bbf3887b7 --- /dev/null +++ b/thirdparty/src/newton/dContainers/dCRC.cpp @@ -0,0 +1,135 @@ +/* Copyright (c) <2003-2019> +* +* 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(7266447313870364031ULL), static_cast(4946485549665804864ULL), static_cast(16945909448695747420ULL), static_cast(16394063075524226720ULL), + static_cast(4873882236456199058ULL), static_cast(14877448043947020171ULL), static_cast(6740343660852211943ULL), static_cast(13857871200353263164ULL), + static_cast(5249110015610582907ULL), static_cast(10205081126064480383ULL), static_cast(1235879089597390050ULL), static_cast(17320312680810499042ULL), + static_cast(16489141110565194782ULL), static_cast(8942268601720066061ULL), static_cast(13520575722002588570ULL), static_cast(14226945236717732373ULL), + + static_cast(9383926873555417063ULL), static_cast(15690281668532552105ULL), static_cast(11510704754157191257ULL), static_cast(15864264574919463609ULL), + static_cast(6489677788245343319ULL), static_cast(5112602299894754389ULL), static_cast(10828930062652518694ULL), static_cast(15942305434158995996ULL), + static_cast(15445717675088218264ULL), static_cast(4764500002345775851ULL), static_cast(14673753115101942098ULL), static_cast(236502320419669032ULL), + static_cast(13670483975188204088ULL), static_cast(14931360615268175698ULL), static_cast(8904234204977263924ULL), static_cast(12836915408046564963ULL), + + static_cast(12120302420213647524ULL), static_cast(15755110976537356441ULL), static_cast(5405758943702519480ULL), static_cast(10951858968426898805ULL), + static_cast(17251681303478610375ULL), static_cast(4144140664012008120ULL), static_cast(18286145806977825275ULL), static_cast(13075804672185204371ULL), + static_cast(10831805955733617705ULL), static_cast(6172975950399619139ULL), static_cast(12837097014497293886ULL), static_cast(12903857913610213846ULL), + static_cast(560691676108914154ULL), static_cast(1074659097419704618ULL), static_cast(14266121283820281686ULL), static_cast(11696403736022963346ULL), + + static_cast(13383246710985227247ULL), static_cast(7132746073714321322ULL), static_cast(10608108217231874211ULL), static_cast(9027884570906061560ULL), + static_cast(12893913769120703138ULL), static_cast(15675160838921962454ULL), static_cast(2511068401785704737ULL), static_cast(14483183001716371453ULL), + static_cast(3774730664208216065ULL), static_cast(5083371700846102796ULL), static_cast(9583498264570933637ULL), static_cast(17119870085051257224ULL), + static_cast(5217910858257235075ULL), static_cast(10612176809475689857ULL), static_cast(1924700483125896976ULL), static_cast(7171619684536160599ULL), + + + static_cast(10949279256701751503ULL), static_cast(15596196964072664893ULL), static_cast(14097948002655599357ULL), static_cast(615821766635933047ULL), + static_cast(5636498760852923045ULL), static_cast(17618792803942051220ULL), static_cast(580805356741162327ULL), static_cast(425267967796817241ULL), + static_cast(8381470634608387938ULL), static_cast(13212228678420887626ULL), static_cast(16993060308636741960ULL), static_cast(957923366004347591ULL), + static_cast(6210242862396777185ULL), static_cast(1012818702180800310ULL), static_cast(15299383925974515757ULL), static_cast(17501832009465945633ULL), + + static_cast(17453794942891241229ULL), static_cast(15807805462076484491ULL), static_cast(8407189590930420827ULL), static_cast(974125122787311712ULL), + static_cast(1861591264068118966ULL), static_cast(997568339582634050ULL), static_cast(18046771844467391493ULL), static_cast(17981867688435687790ULL), + static_cast(3809841506498447207ULL), static_cast(9460108917638135678ULL), static_cast(16172980638639374310ULL), static_cast(958022432077424298ULL), + static_cast(4393365126459778813ULL), static_cast(13408683141069553686ULL), static_cast(13900005529547645957ULL), static_cast(15773550354402817866ULL), + + static_cast(16475327524349230602ULL), static_cast(6260298154874769264ULL), static_cast(12224576659776460914ULL), static_cast(6405294864092763507ULL), + static_cast(7585484664713203306ULL), static_cast(5187641382818981381ULL), static_cast(12435998400285353380ULL), static_cast(13554353441017344755ULL), + static_cast(646091557254529188ULL), static_cast(11393747116974949255ULL), static_cast(16797249248413342857ULL), static_cast(15713519023537495495ULL), + static_cast(12823504709579858843ULL), static_cast(4738086532119935073ULL), static_cast(4429068783387643752ULL), static_cast(585582692562183870ULL), + + static_cast(1048280754023674130ULL), static_cast(6788940719869959076ULL), static_cast(11670856244972073775ULL), static_cast(2488756775360218862ULL), + static_cast(2061695363573180185ULL), static_cast(6884655301895085032ULL), static_cast(3566345954323888697ULL), static_cast(12784319933059041817ULL), + static_cast(4772468691551857254ULL), static_cast(6864898938209826895ULL), static_cast(7198730565322227090ULL), static_cast(2452224231472687253ULL), + static_cast(13424792606032445807ULL), static_cast(10827695224855383989ULL), static_cast(11016608897122070904ULL), static_cast(14683280565151378358ULL), + + static_cast(7077866519618824360ULL), static_cast(17487079941198422333ULL), static_cast(3956319990205097495ULL), static_cast(5804870313319323478ULL), + static_cast(8017203611194497730ULL), static_cast(3310931575584983808ULL), static_cast(5009341981771541845ULL), static_cast(11772020174577005930ULL), + static_cast(3537640779967351792ULL), static_cast(6801855569284252424ULL), static_cast(17687268231192623388ULL), static_cast(12968358613633237218ULL), + static_cast(1429775571144180123ULL), static_cast(10427377732172208413ULL), static_cast(12155566091986788996ULL), static_cast(16465954421598296115ULL), + + static_cast(12710429690464359999ULL), static_cast(9547226351541565595ULL), static_cast(12156624891403410342ULL), static_cast(2985938688676214686ULL), + static_cast(18066917785985010959ULL), static_cast(5975570403614438776ULL), static_cast(11541343163022500560ULL), static_cast(11115388652389704592ULL), + static_cast(9499328389494710074ULL), static_cast(9247163036769651820ULL), static_cast(3688303938005101774ULL), static_cast(2210483654336887556ULL), + static_cast(15458161910089693228ULL), static_cast(6558785204455557683ULL), static_cast(1288373156735958118ULL), static_cast(18433986059948829624ULL), + + static_cast(3435082195390932486ULL), static_cast(16822351800343061990ULL), static_cast(3120532877336962310ULL), static_cast(16681785111062885568ULL), + static_cast(7835551710041302304ULL), static_cast(2612798015018627203ULL), static_cast(15083279177152657491ULL), static_cast(6591467229462292195ULL), + static_cast(10592706450534565444ULL), static_cast(7438147750787157163ULL), static_cast(323186165595851698ULL), static_cast(7444710627467609883ULL), + static_cast(8473714411329896576ULL), static_cast(2782675857700189492ULL), static_cast(3383567662400128329ULL), static_cast(3200233909833521327ULL), + + static_cast(12897601280285604448ULL), static_cast(3612068790453735040ULL), static_cast(8324209243736219497ULL), static_cast(15789570356497723463ULL), + static_cast(1083312926512215996ULL), static_cast(4797349136059339390ULL), static_cast(5556729349871544986ULL), static_cast(18266943104929747076ULL), + static_cast(1620389818516182276ULL), static_cast(172225355691600141ULL), static_cast(3034352936522087096ULL), static_cast(1266779576738385285ULL), + static_cast(3906668377244742888ULL), static_cast(6961783143042492788ULL), static_cast(17159706887321247572ULL), static_cast(4676208075243319061ULL), + + static_cast(10315634697142985816ULL), static_cast(13435140047933251189ULL), static_cast(716076639492622016ULL), static_cast(13847954035438697558ULL), + static_cast(7195811275139178570ULL), static_cast(10815312636510328870ULL), static_cast(6214164734784158515ULL), static_cast(16412194511839921544ULL), + static_cast(3862249798930641332ULL), static_cast(1005482699535576005ULL), static_cast(4644542796609371301ULL), static_cast(17600091057367987283ULL), + static_cast(4209958422564632034ULL), static_cast(5419285945389823940ULL), static_cast(11453701547564354601ULL), static_cast(9951588026679380114ULL), + + static_cast(7425168333159839689ULL), static_cast(8436306210125134906ULL), static_cast(11216615872596820107ULL), static_cast(3681345096403933680ULL), + static_cast(5770016989916553752ULL), static_cast(11102855936150871733ULL), static_cast(11187980892339693935ULL), static_cast(396336430216428875ULL), + static_cast(6384853777489155236ULL), static_cast(7551613839184151117ULL), static_cast(16527062023276943109ULL), static_cast(13429850429024956898ULL), + static_cast(9901753960477271766ULL), static_cast(9731501992702612259ULL), static_cast(5217575797614661659ULL), static_cast(10311708346636548706ULL), + + static_cast(15111747519735330483ULL), static_cast(4353415295139137513ULL), static_cast(1845293119018433391ULL), static_cast(11952006873430493561ULL), + static_cast(3531972641585683893ULL), static_cast(16852246477648409827ULL), static_cast(15956854822143321380ULL), static_cast(12314609993579474774ULL), + static_cast(16763911684844598963ULL), static_cast(16392145690385382634ULL), static_cast(1545507136970403756ULL), static_cast(17771199061862790062ULL), + static_cast(12121348462972638971ULL), static_cast(12613068545148305776ULL), static_cast(954203144844315208ULL), static_cast(1257976447679270605ULL), + + static_cast(3664184785462160180ULL), static_cast(2747964788443845091ULL), static_cast(15895917007470512307ULL), static_cast(15552935765724302120ULL), + static_cast(16366915862261682626ULL), static_cast(8385468783684865323ULL), static_cast(10745343827145102946ULL), static_cast(2485742734157099909ULL), + static_cast(916246281077683950ULL), static_cast(15214206653637466707ULL), static_cast(12895483149474345798ULL), static_cast(1079510114301747843ULL), + static_cast(10718876134480663664ULL), static_cast(1259990987526807294ULL), static_cast(8326303777037206221ULL), static_cast(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; +} + + + + diff --git a/thirdparty/src/newton/dContainers/dCRC.h b/thirdparty/src/newton/dContainers/dCRC.h new file mode 100644 index 000000000..f12b59766 --- /dev/null +++ b/thirdparty/src/newton/dContainers/dCRC.h @@ -0,0 +1,25 @@ +/* Copyright (c) <2003-2019> +* +* 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 + diff --git a/thirdparty/src/newton/dContainers/dClassInfo.cpp b/thirdparty/src/newton/dContainers/dClassInfo.cpp new file mode 100644 index 000000000..588d8e6d6 --- /dev/null +++ b/thirdparty/src/newton/dContainers/dClassInfo.cpp @@ -0,0 +1,18 @@ + +/* Copyright (c) <2003-2019> +* +* 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); diff --git a/thirdparty/src/newton/dContainers/dClassInfo.h b/thirdparty/src/newton/dContainers/dClassInfo.h new file mode 100644 index 000000000..0aebf9c1e --- /dev/null +++ b/thirdparty/src/newton/dContainers/dClassInfo.h @@ -0,0 +1,32 @@ + +/* Copyright (c) <2003-2019> +* +* 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 diff --git a/thirdparty/src/newton/dContainers/dContainersAlloc.cpp b/thirdparty/src/newton/dContainers/dContainersAlloc.cpp new file mode 100644 index 000000000..fbc25eec2 --- /dev/null +++ b/thirdparty/src/newton/dContainers/dContainersAlloc.cpp @@ -0,0 +1,215 @@ +/* Copyright (c) <2003-2019> +* +* 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(); + } +} diff --git a/thirdparty/src/newton/dContainers/dContainersAlloc.h b/thirdparty/src/newton/dContainers/dContainersAlloc.h new file mode 100644 index 000000000..a0163fc56 --- /dev/null +++ b/thirdparty/src/newton/dContainers/dContainersAlloc.h @@ -0,0 +1,75 @@ +/* Copyright (c) <2003-2019> +* +* 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 diff --git a/thirdparty/src/newton/dContainers/dContainersStdAfx.cpp b/thirdparty/src/newton/dContainers/dContainersStdAfx.cpp new file mode 100644 index 000000000..7f10bf6a3 --- /dev/null +++ b/thirdparty/src/newton/dContainers/dContainersStdAfx.cpp @@ -0,0 +1,21 @@ +/* Copyright (c) <2003-2019> +* +* 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 + + diff --git a/thirdparty/src/newton/dContainers/dContainersStdAfx.h b/thirdparty/src/newton/dContainers/dContainersStdAfx.h new file mode 100644 index 000000000..0f9816eb2 --- /dev/null +++ b/thirdparty/src/newton/dContainers/dContainersStdAfx.h @@ -0,0 +1,55 @@ +/* Copyright (c) <2003-2019> +* +* 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 +#include +#include +#include +#include +#include +#include +#include + +#include + +#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 + #include +#endif + +#ifdef _MACOSX_VER + #include + #include + #include +#endif + +#include + +#endif diff --git a/thirdparty/src/newton/dContainers/dHeap.h b/thirdparty/src/newton/dContainers/dHeap.h new file mode 100644 index 000000000..c32daef17 --- /dev/null +++ b/thirdparty/src/newton/dContainers/dHeap.h @@ -0,0 +1,421 @@ +/* Copyright (c) <2003-2019> +* +* 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 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 dDownHeap: public dHeapBase +{ + public: + dDownHeap (int maxElements); + + void Pop () {Remove (0);} + void Push (OBJECT &obj, KEY key); + void Sort (); + void Remove (int Index); + bool SanityCheck(); +}; + +template +class dUpHeap: public dHeapBase +{ + public: + dUpHeap (int maxElements); + + void Pop () {Remove (0);} + void Push (OBJECT &obj, KEY key); + void Sort (); + void Remove (int Index); + bool SanityCheck(); +}; + +template +dHeapBase::dHeapBase (int maxElements) + :m_curCount(0) + ,m_maxCount(maxElements) + ,m_allocated(true) + ,m_pool(new dHeapRecord[maxElements]) +{ + Flush(); +} + + +template +dHeapBase::~dHeapBase () +{ + if (m_allocated == true) { + delete[] m_pool; + } +} + + +template +KEY dHeapBase::Value(int i) const +{ + return m_pool[i].m_key; +} + + +template +int dHeapBase::GetCount() const +{ + return m_curCount; +} + + +template +void dHeapBase::Flush () +{ + m_curCount = 0; + + #ifdef _DEBUG +// dHeapBase::m_pool[dHeapBase::m_curCount].m_key = KEY (0); + #endif +} + + +template +KEY dHeapBase::MaxValue() const +{ + return m_pool[0].m_key; +} + + +template +int dHeapBase::GetMaxCount() const +{ + return m_maxCount; +} + + +template +int dHeapBase::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 +int dHeapBase::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 +const OBJECT& dHeapBase::operator[] (int i) const +{ + dAssert (i<= m_curCount); + return m_pool[i].m_obj; +} + + +// ************************************************************************** +// +// down Heap +// +// ************************************************************************** +template +dDownHeap::dDownHeap (int maxElements) + :dHeapBase (maxElements) +{ +} + +template +void dDownHeap::Push (OBJECT &obj, KEY key) +{ + dAssert ((dHeapBase::m_curCount < dHeapBase::m_maxCount)); + dHeapBase::m_curCount ++; + + int j; + int i = dHeapBase::m_curCount; + for (; i; i = j) { + j = i >> 1; + if (!j || (dHeapBase::m_pool[j - 1].m_key > key)) { + break; + } + dHeapBase::m_pool[i - 1] = dHeapBase::m_pool[j - 1]; + } + dAssert (i); + dHeapBase::m_pool[i - 1].m_key = key; + dHeapBase::m_pool[i - 1].m_obj = obj; + dAssert (SanityCheck()); +} + +template +void dDownHeap::Remove (int index) +{ + dHeapBase::m_curCount--; + dHeapBase::m_pool[index] = dHeapBase::m_pool[dHeapBase::m_curCount]; + while (index && dHeapBase::m_pool[(index - 1) >> 1].m_key < dHeapBase::m_pool[index].m_key) { + dSwap(dHeapBase::m_pool[(index - 1) >> 1], dHeapBase::m_pool[index]); + index = (index - 1) >> 1; + } + + while ((2 * index + 1) < dHeapBase::m_curCount) { + int i0 = 2 * index + 1; + int i1 = 2 * index + 2; + if (i1 < dHeapBase::m_curCount) { + i0 = (dHeapBase::m_pool[i0].m_key > dHeapBase::m_pool[i1].m_key) ? i0 : i1; + if (dHeapBase::m_pool[i0].m_key <= dHeapBase::m_pool[index].m_key) { + break; + } + dSwap(dHeapBase::m_pool[i0], dHeapBase::m_pool[index]); + index = i0; + } else { + if (dHeapBase::m_pool[i0].m_key > dHeapBase::m_pool[index].m_key) { + dSwap(dHeapBase::m_pool[i0], dHeapBase::m_pool[index]); + } + index = i0; + } + } + dAssert(SanityCheck()); +} + +template +void dDownHeap::Sort () +{ + int count = dHeapBase::m_curCount; + for (int i = 1; i < count; i ++) { + KEY key (dHeapBase::m_pool[0].m_key); + OBJECT obj (dHeapBase::m_pool[0].m_obj); + + Pop(); + + dHeapBase::m_pool[dHeapBase::m_curCount].m_key = key; + dHeapBase::m_pool[dHeapBase::m_curCount].m_obj = obj; + } + + dHeapBase::m_curCount = count; + for (int i = 0; i < count / 2; i ++) { + KEY key (dHeapBase::m_pool[i].m_key); + OBJECT obj (dHeapBase::m_pool[i].m_obj); + + dHeapBase::m_pool[i].m_key = dHeapBase::m_pool[count - i - 1].m_key; + dHeapBase::m_pool[i].m_obj = dHeapBase::m_pool[count - i - 1].m_obj; + + dHeapBase::m_pool[count - i - 1].m_key = key; + dHeapBase::m_pool[count - i - 1].m_obj = obj; + } + dAssert (SanityCheck()); +} + +template +bool dDownHeap::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::m_pool[i].m_key < dHeapBase::m_pool[i1].m_key)) { + return false; + } + if ((i2 < this->m_curCount) && (dHeapBase::m_pool[i].m_key < dHeapBase::m_pool[i2].m_key)) { + return false; + } + } +#endif + return true; +} + + +// ************************************************************************** +// +// Up Heap +// +// ************************************************************************** +template +dUpHeap::dUpHeap (int maxElements) + :dHeapBase (maxElements) +{ +} + +template +void dUpHeap::Push (OBJECT &obj, KEY key) +{ + dAssert ((dHeapBase::m_curCount < dHeapBase::m_maxCount)); + dHeapBase::m_curCount ++; + + int j; + int i = dHeapBase::m_curCount; + for (; i; i = j) { + j = i >> 1; + if (!j || (dHeapBase::m_pool[j - 1].m_key < key)) { + break; + } + dHeapBase::m_pool[i - 1] = dHeapBase::m_pool[j - 1]; + } + dAssert (i); + dHeapBase::m_pool[i - 1].m_key = key; + dHeapBase::m_pool[i - 1].m_obj = obj; + dAssert (SanityCheck()); +} + + +template +void dUpHeap::Sort () +{ + int count = dHeapBase::m_curCount; + for (int i = 1; i < count; i ++) { + KEY key (dHeapBase::m_pool[0].m_key); + OBJECT obj (dHeapBase::m_pool[0].m_obj); + + Pop(); + + dHeapBase::m_pool[dHeapBase::m_curCount].m_key = key; + dHeapBase::m_pool[dHeapBase::m_curCount].m_obj = obj; + } + + dHeapBase::m_curCount = count; + for (int i = 0; i < count / 2; i ++) { + KEY key (dHeapBase::m_pool[i].m_key); + OBJECT obj (dHeapBase::m_pool[i].m_obj); + + dHeapBase::m_pool[i].m_key = dHeapBase::m_pool[count - i - 1].m_key; + dHeapBase::m_pool[i].m_obj = dHeapBase::m_pool[count - i - 1].m_obj; + + dHeapBase::m_pool[count - i - 1].m_key = key; + dHeapBase::m_pool[count - i - 1].m_obj = obj; + } + dAssert (SanityCheck()); +} + +template +void dUpHeap::Remove (int index) +{ + dHeapBase::m_curCount--; + dHeapBase::m_pool[index] = dHeapBase::m_pool[dHeapBase::m_curCount]; + while (index && dHeapBase::m_pool[(index - 1) >> 1].m_key > dHeapBase::m_pool[index].m_key) { + dSwap(dHeapBase::m_pool[(index - 1) >> 1], dHeapBase::m_pool[index]); + index = (index - 1) >> 1; + } + + while ((2 * index + 1) < dHeapBase::m_curCount) { + int i0 = 2 * index + 1; + int i1 = 2 * index + 2; + if (i1 < dHeapBase::m_curCount) { + i0 = (dHeapBase::m_pool[i0].m_key < dHeapBase::m_pool[i1].m_key) ? i0 : i1; + if (dHeapBase::m_pool[i0].m_key >= dHeapBase::m_pool[index].m_key) { + break; + } + dSwap(dHeapBase::m_pool[i0], dHeapBase::m_pool[index]); + index = i0; + } else { + if (dHeapBase::m_pool[i0].m_key < dHeapBase::m_pool[index].m_key) { + dSwap(dHeapBase::m_pool[i0], dHeapBase::m_pool[index]); + } + index = i0; + } + } + dAssert (SanityCheck()); +} + +template +bool dUpHeap::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::m_pool[i].m_key > dHeapBase::m_pool[i1].m_key)) { + return false; + } + if ((i2 < this->m_curCount) && (dHeapBase::m_pool[i].m_key > dHeapBase::m_pool[i2].m_key)) { + return false; + } + } +#endif + return true; +} + + +#endif + diff --git a/thirdparty/src/newton/dContainers/dList.h b/thirdparty/src/newton/dContainers/dList.h new file mode 100644 index 000000000..cb5373e9b --- /dev/null +++ b/thirdparty/src/newton/dContainers/dList.h @@ -0,0 +1,483 @@ +/* Copyright (c) <2003-2019> +* +* 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 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; + }; + + class Iterator + { + public: + Iterator (const dList &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::dListNode), poolSize); + } + return *allocator; + } + + int m_count; + dListNode* m_first; + dListNode* m_last; + friend class dListNode; +}; + + +template +dList::dList () +{ + m_count = 0; + m_first = NULL; + m_last = NULL; + GetAllocator(); +} + + +template +dList::~dList () +{ + RemoveAll (); +} + + +template +int dList::GetCount() const +{ + return m_count; +} + +template +dList::operator int() const +{ + return m_first != NULL; +} + +template +typename dList::dListNode* dList::GetFirst() const +{ + return m_first; +} + +template +typename dList::dListNode* dList::GetLast() const +{ + return m_last; +} + +template +typename dList::dListNode* dList::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 +typename dList::dListNode* dList::AppendAfter (dListNode* const node) +{ + dListNode* const ptr = Append (); + InsertAfter (node, ptr); + return ptr; +} + +template +typename dList::dListNode* dList::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 +typename dList::dListNode* dList::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 +typename dList::dListNode* dList::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 +void dList::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 +void dList::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 +void dList::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 +typename dList::dListNode* dList::Find (const T &element) const +{ + dListNode* node; + for (node = m_first; node; node = node->GetNext()) { + if (element == node->m_info) { + break; + } + } + return node; +} + + +template +typename dList::dListNode* dList::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 +void dList::Remove (const T &element) +{ + dListNode* const node = Find (element); + if (node) { + Remove (node); + } +} + +template +void dList::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 +void dList::RemoveAll () +{ + while (m_first) { + Remove(m_first); + } + dAssert (!m_count); +} + +template +void dList::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 + + diff --git a/thirdparty/src/newton/dContainers/dMap.h b/thirdparty/src/newton/dContainers/dMap.h new file mode 100644 index 000000000..4e4ef6f97 --- /dev/null +++ b/thirdparty/src/newton/dContainers/dMap.h @@ -0,0 +1,1049 @@ +/* Copyright (c) <2003-2019> +* +* 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_MAP__ +#define __D_MAP__ + +template +class dMap +{ + public: + class dTreeNode + { + enum REDBLACK_COLOR + { + RED = true, + BLACK = false + }; + + dTreeNode (const KEY &key, dTreeNode* parentNode) + :m_info() + ,m_key(key) + ,m_left(nullptr) + ,m_right(nullptr) + ,m_parent(parentNode) + ,m_color(false) + ,m_inTree(false) + { + SetColor(RED); + SetInTreeFlag(true); + } + + dTreeNode (const OBJECT &info, const KEY &key, dTreeNode* parentNode) + :m_info(info) + ,m_key(key) + ,m_left(nullptr) + ,m_right(nullptr) + ,m_parent(parentNode) + ,m_color(false) + ,m_inTree(false) + { + SetColor(RED); + SetInTreeFlag(true); + } + + virtual ~dTreeNode () + { + } + + void SetInTreeFlag(bool flag) + { + m_inTree = flag; + } + + bool GetColor() const + { + return m_color; + } + + void SetColor(bool color) + { + m_color = color; + } + + dTreeNode& operator= (dTreeNode& src) + { + dAssert (0); + return* this; + } + + dTreeNode* GetLeft () const + { + return m_left; + } + + dTreeNode* GetRight () const + { + return m_right; + } + + dTreeNode* GetParent () + { + return m_parent; + } + + void SetLeft (dTreeNode* const node) + { + m_left = node; + } + + void SetRight (dTreeNode* const node) + { + m_right = node; + } + + void SetParent (dTreeNode* const node) + { + m_parent = node; + } + + public: + const KEY& GetKey() const + { + return m_key; + } + + OBJECT& GetInfo() + { + return m_info; + } + + dTreeNode* Minimum() const + { + dTreeNode* ptr = (dTreeNode*)this; + for (; ptr->m_left; ptr = ptr->m_left); + return ptr; + } + + dTreeNode* Next() const + { + if (m_right) { + return m_right->Minimum(); + } + + dTreeNode* node = (dTreeNode*)this; + dTreeNode* ptr = m_parent; + for (; ptr && node == ptr->m_right; ptr = ptr->m_parent) { + node = ptr; + } + return ptr; + } + + void Unlink(dTreeNode** const head) + { + dTreeNode* const node = this; + node->SetInTreeFlag(false); + + if (!node->m_left || !node->m_right) { + // y has a nullptr node as a child + dTreeNode* const endNode = node; + + // x is y's only child + dTreeNode* 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 nullptr node as a child + dTreeNode* endNode = node->m_right; + while (endNode->m_left != nullptr) { + endNode = endNode->m_left; + } + + // x is y's only child + dTreeNode* const child = endNode->m_right; + + endNode->m_left = node->m_left; + node->m_left->m_parent = endNode; + + dTreeNode* 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); + } + } + } + + void RotateLeft(dTreeNode** const head) + { + dTreeNode* const me = this; + dTreeNode* const child = me->m_right; + + //dAssert(child); + me->m_right = child->m_left; + if (child->m_left != nullptr) { + child->m_left->m_parent = me; + } + + if (child != nullptr) { + 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 != nullptr) { + me->m_parent = child; + } + } + + // rotate node me to right * + void RotateRight(dTreeNode** const head) + { + dTreeNode* const me = this; + dTreeNode* const child = me->m_left; + + //dAssert(child); + me->m_left = child->m_right; + if (child->m_right != nullptr) { + child->m_right->m_parent = me; + } + + // establish child->m_parent link + if (child != nullptr) { + 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 != nullptr) { + me->m_parent = child; + } + } + + void RemoveFixup(dTreeNode* const me, dTreeNode** const head) + { + dTreeNode* ptr = this; + dTreeNode* node = me; + while ((node != *head) && (!node || node->GetColor() == BLACK)) { + if (node == ptr->m_left) { + if (!ptr) { + return; + } + dTreeNode* 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 (!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; + } + dTreeNode* 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 (!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 InsertFixup(dTreeNode** const head) + { + dTreeNode* 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) { + dTreeNode* 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 + dTreeNode* 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); + } + + + private: + OBJECT m_info; + KEY m_key; + dTreeNode* m_left; + dTreeNode* m_right; + dTreeNode* m_parent; + bool m_color; + bool m_inTree; + friend class dMap; + }; + + class Iterator + { + public: + Iterator(const dMap &me) + { + m_ptr = nullptr; + 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 != nullptr; + } + + 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: + dTreeNode* m_ptr; + const dMap* m_tree; + }; + + + // *********************************************************** + // member functions + // *********************************************************** + public: + dMap (); + virtual ~dMap (); + + 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: + 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; +}; + +template +dMap::dMap () +{ + m_count = 0; + m_head = nullptr; +} + +template +dMap::~dMap () +{ + RemoveAll(); +} + + +template +dMap::operator int() const +{ + return m_head != nullptr; +} + +template +int dMap::GetCount() const +{ + return m_count; +} + +template +typename dMap::dTreeNode* dMap::Minimum () const +{ + return m_head ? (dTreeNode*) m_head->Minimum() : nullptr; +} + +template +typename dMap::dTreeNode* dMap::Maximum () const +{ + return m_head ? (dTreeNode*) m_head->Maximum() : nullptr; +} + +template +typename dMap::dTreeNode* dMap::GetRoot () const +{ + return m_head; +} + +template +typename dMap::dTreeNode* dMap::Find (KEY key) const +{ + if (m_head == nullptr) { + return nullptr; + } + + dTreeNode* ptr = m_head; + while (ptr != nullptr) { + int val = CompareKeys (ptr->m_key, key); + if (!val) { + break; + } + if (val < 0) { + ptr = ptr->GetLeft(); + } else { + ptr = ptr->GetRight(); + } + } + return ptr; +} + +template +typename dMap::dTreeNode* dMap::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 : nullptr; +} + +template +typename dMap::dTreeNode* dMap::FindGreater (KEY key) const +{ + if (m_head == nullptr) { + return nullptr; + } + + dTreeNode* prev = nullptr; + dTreeNode* ptr = m_head; + int val = 0; + while (ptr != nullptr) { + 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 +typename dMap::dTreeNode* dMap::FindGreaterEqual (KEY key) const +{ + if (m_head == nullptr) { + return nullptr; + } + + dTreeNode* prev = nullptr; + dTreeNode* ptr = m_head; + int val = 0; + while (ptr != nullptr) { + 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 +typename dMap::dTreeNode* dMap::FindLessEqual (KEY key) const +{ + if (m_head == nullptr) { + return nullptr; + } + + dTreeNode* prev = nullptr; + dTreeNode* ptr = m_head; + int val = 0; + while (ptr != nullptr) { + 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 +typename dMap::dTreeNode* dMap::Insert (KEY key) +{ + dTreeNode* parent = nullptr; + dTreeNode* ptr = m_head; + int val = 0; + while (ptr != nullptr) { + 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 dTreeNode (key, parent); + if (!parent) { + m_head = ptr; + } else { + if (val < 0) { + parent->m_left = ptr; + } else { + parent->m_right = ptr; + } + } + ptr->InsertFixup ((dTreeNode**)&m_head); + return ptr; +} + + +template +typename dMap::dTreeNode* dMap::Insert (const OBJECT &element, KEY key, bool& elementWasInTree) +{ + dTreeNode* parent = nullptr; + dTreeNode* ptr = m_head; + int val = 0; + elementWasInTree = false; + while (ptr != nullptr) { + 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 dTreeNode (element, key, parent); + if (!parent) { + m_head = ptr; + } else { + if (val < 0) { + parent->m_left = ptr; + } else { + parent->m_right = ptr; + } + } + ptr->InsertFixup ((dTreeNode**)&m_head); + return ptr; +} + +template +typename dMap::dTreeNode* dMap::Insert (const OBJECT &element, KEY key) +{ + bool foundState; + dTreeNode* node = Insert (element, key, foundState); + if (foundState) { + node = nullptr; + } + return node; +} + +template +typename dMap::dTreeNode* dMap::Insert (typename dMap::dTreeNode* const node, KEY key) +{ + int val = 0; + dTreeNode* ptr = m_head; + dTreeNode* parent = nullptr; + while (ptr != nullptr) { + parent = ptr; + val = CompareKeys (ptr->m_key, key); + + if (val < 0) { + ptr = ptr->GetLeft(); + } else if (val > 0) { + ptr = ptr->GetRight(); + } else { + return nullptr; + } + } + + 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 ((dTreeNode**)&m_head); + return ptr; +} + +template +typename dMap::dTreeNode* dMap::Replace (OBJECT &element, KEY key) +{ + dTreeNode* parent = nullptr; + dTreeNode* ptr = m_head; + int val = 0; + while (ptr != nullptr) { + 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 dTreeNode (element, key, parent); + if (!parent) { + m_head = ptr; + } else { + if (val < 0) { + parent->m_left = ptr; + } else { + parent->m_right = ptr; + } + } + ptr->InsertFixup ((dTreeNode**)&m_head); + return ptr; +} + +template +typename dMap::dTreeNode* dMap::ReplaceKey (typename dMap::dTreeNode* node, KEY key) +{ + Unlink(node); + dTreeNode* const ptr = Insert (node, key); + + dAssert (ptr); + return ptr; +} + +template +typename dMap::dTreeNode* dMap::ReplaceKey (KEY oldKey, KEY newKey) +{ + dTreeNode* const node = Find (oldKey); + return node ? ReplaceKey (node, newKey) : nullptr; +} + +template +void dMap::Unlink (typename dMap::dTreeNode* const node) +{ + m_count --; + node->Unlink((dTreeNode** )&m_head); +} + +template +void dMap::Remove (typename dMap::dTreeNode* const node) +{ + m_count --; + node->Unlink ((dTreeNode** )&m_head); + delete node; +} + +template +void dMap::Remove (KEY key) +{ + // find node in tree + dTreeNode* const node = Find (key); + if (node) { + Remove (node); + } +} + + +template +void dMap::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); + delete root; +} + + +template +void dMap::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 = nullptr; + } +} + +template +bool dMap::SanityCheck () const +{ + return SanityCheck (m_head, 0); +} + + +template +bool dMap::SanityCheck (typename dMap::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 +int dMap::CompareKeys (const KEY &key0, const KEY &key1) const +{ + if (key1 < key0) { + return - 1; + } + if (key1 > key0) { + return 1; + } + return 0; +} + +#endif + + diff --git a/thirdparty/src/newton/dContainers/dPointer.h b/thirdparty/src/newton/dContainers/dPointer.h new file mode 100644 index 000000000..d0ef2eb1d --- /dev/null +++ b/thirdparty/src/newton/dContainers/dPointer.h @@ -0,0 +1,79 @@ +/* Copyright (c) <2003-2019> +* +* 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 +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 +dPointer::dPointer() + :m_data(NULL) +{ +} + + +template +dPointer::dPointer(T* const data) + :m_data(data) +{ +} + +template +dPointer::~dPointer() +{ + if (m_data) { + delete m_data; + } +} + +template +T& dPointer::operator* () +{ + return *m_data; +} + +template +T* dPointer::operator-> () +{ + return m_data; +} + +template +T* dPointer::GetData() const +{ + return m_data; +} + +template +void dPointer::SetData(T* const data) +{ + m_data = data; +} + +#endif \ No newline at end of file diff --git a/thirdparty/src/newton/dContainers/dRefCounter.cpp b/thirdparty/src/newton/dContainers/dRefCounter.cpp new file mode 100644 index 000000000..13977dcff --- /dev/null +++ b/thirdparty/src/newton/dContainers/dRefCounter.cpp @@ -0,0 +1,46 @@ +/* Copyright (c) <2003-2019> +* +* 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 ++; +} + diff --git a/thirdparty/src/newton/dContainers/dRefCounter.h b/thirdparty/src/newton/dContainers/dRefCounter.h new file mode 100644 index 000000000..a78270d2b --- /dev/null +++ b/thirdparty/src/newton/dContainers/dRefCounter.h @@ -0,0 +1,34 @@ +/* Copyright (c) <2003-2019> +* +* 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 \ No newline at end of file diff --git a/thirdparty/src/newton/dContainers/dRtti.h b/thirdparty/src/newton/dContainers/dRtti.h new file mode 100644 index 000000000..c3949bf5d --- /dev/null +++ b/thirdparty/src/newton/dContainers/dRtti.h @@ -0,0 +1,62 @@ +/* Copyright (c) <2003-2019> +* +* 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 diff --git a/thirdparty/src/newton/dContainers/dString.cpp b/thirdparty/src/newton/dContainers/dString.cpp new file mode 100644 index 000000000..fc96de4a8 --- /dev/null +++ b/thirdparty/src/newton/dContainers/dString.cpp @@ -0,0 +1,571 @@ +/* Copyright (c) <2003-2019> +* +* 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); + } +} diff --git a/thirdparty/src/newton/dContainers/dString.h b/thirdparty/src/newton/dContainers/dString.h new file mode 100644 index 000000000..1a4e998c2 --- /dev/null +++ b/thirdparty/src/newton/dContainers/dString.h @@ -0,0 +1,214 @@ +/* Copyright (c) <2003-2019> +* +* 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 + + diff --git a/thirdparty/src/newton/dContainers/dTree.cpp b/thirdparty/src/newton/dContainers/dTree.cpp new file mode 100644 index 000000000..04896f19f --- /dev/null +++ b/thirdparty/src/newton/dContainers/dTree.cpp @@ -0,0 +1,369 @@ +/* Copyright (c) <2003-2019> +* +* 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); + } + } +} + + diff --git a/thirdparty/src/newton/dContainers/dTree.h b/thirdparty/src/newton/dContainers/dTree.h new file mode 100644 index 000000000..1f2f2238d --- /dev/null +++ b/thirdparty/src/newton/dContainers/dTree.h @@ -0,0 +1,796 @@ +/* Copyright (c) <2003-2019> +* +* 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 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; + }; + + class Iterator + { + public: + Iterator(const dTree &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::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 +dTree::dTree () +{ + m_count = 0; + m_head = NULL; + GetAllocator(); +} + + +template +dTree::~dTree () +{ + RemoveAll(); +} + + +template +dTree::operator int() const +{ + return m_head != NULL; +} + +template +int dTree::GetCount() const +{ + return m_count; +} + +template +typename dTree::dTreeNode* dTree::Minimum () const +{ + return m_head ? (dTreeNode*) m_head->Minimum() : NULL; +} + +template +typename dTree::dTreeNode* dTree::Maximum () const +{ + return m_head ? (dTreeNode*) m_head->Maximum() : NULL; +} + +template +typename dTree::dTreeNode* dTree::GetRoot () const +{ + return m_head; +} + +template +typename dTree::dTreeNode* dTree::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 +typename dTree::dTreeNode* dTree::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 +typename dTree::dTreeNode* dTree::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 +typename dTree::dTreeNode* dTree::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 +typename dTree::dTreeNode* dTree::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 +typename dTree::dTreeNode* dTree::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 +typename dTree::dTreeNode* dTree::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 +typename dTree::dTreeNode* dTree::Insert (const OBJECT &element, KEY key) +{ + bool foundState; + dTreeNode* node = Insert (element, key, foundState); + if (foundState) { + node = NULL; + } + return node; +} + +template +typename dTree::dTreeNode* dTree::Insert (typename dTree::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 +typename dTree::dTreeNode* dTree::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 +typename dTree::dTreeNode* dTree::ReplaceKey (typename dTree::dTreeNode* node, KEY key) +{ + Unlink(node); + dTreeNode* const ptr = Insert (node, key); + + dAssert (ptr); + return ptr; +} + +template +typename dTree::dTreeNode* dTree::ReplaceKey (KEY oldKey, KEY newKey) +{ + dTreeNode* const node = Find (oldKey); + return node ? ReplaceKey (node, newKey) : NULL; +} + +template +void dTree::Unlink (typename dTree::dTreeNode* const node) +{ + m_count --; + node->Unlink((dRedBackNode** )&m_head); +} + + +template +void dTree::Remove (typename dTree::dTreeNode* const node) +{ + m_count --; + node->Unlink ((dRedBackNode** )&m_head); + node->~dTreeNode(); + GetAllocator().Free (node); +} + +template +void dTree::Remove (KEY key) +{ + // find node in tree + dTreeNode* const node = Find (key); + if (node) { + Remove (node); + } +} + + +template +void dTree::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 +void dTree::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 +bool dTree::SanityCheck () const +{ + return SanityCheck (m_head, 0); +} + + +template +bool dTree::SanityCheck (typename dTree::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 +int dTree::CompareKeys (const KEY &key0, const KEY &key1) const +{ + if (key1 < key0) { + return - 1; + } + if (key1 > key0) { + return 1; + } + return 0; +} + +#endif + + diff --git a/thirdparty/src/newton/dMath/CMakeLists.txt b/thirdparty/src/newton/dMath/CMakeLists.txt new file mode 100644 index 000000000..e9cc1fb8f --- /dev/null +++ b/thirdparty/src/newton/dMath/CMakeLists.txt @@ -0,0 +1,48 @@ +# Copyright (c) <2014-2017> +# +# 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}) diff --git a/thirdparty/src/newton/dMath/dLinearAlgebra.cpp b/thirdparty/src/newton/dMath/dLinearAlgebra.cpp new file mode 100644 index 000000000..f1c20748f --- /dev/null +++ b/thirdparty/src/newton/dMath/dLinearAlgebra.cpp @@ -0,0 +1,948 @@ +/* Copyright (c) <2003-2019> +* +* 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); + } +} + diff --git a/thirdparty/src/newton/dMath/dLinearAlgebra.h b/thirdparty/src/newton/dMath/dLinearAlgebra.h new file mode 100644 index 000000000..827d26c81 --- /dev/null +++ b/thirdparty/src/newton/dMath/dLinearAlgebra.h @@ -0,0 +1,294 @@ +/* Copyright (c) <2003-2019> +* +* 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 + diff --git a/thirdparty/src/newton/dMath/dMathDefines.cpp b/thirdparty/src/newton/dMath/dMathDefines.cpp new file mode 100644 index 000000000..5a32e6788 --- /dev/null +++ b/thirdparty/src/newton/dMath/dMathDefines.cpp @@ -0,0 +1,15 @@ +/* Copyright (c) <2003-2019> +* +* 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" + + diff --git a/thirdparty/src/newton/dMath/dMathDefines.h b/thirdparty/src/newton/dMath/dMathDefines.h new file mode 100644 index 000000000..1b6b8a693 --- /dev/null +++ b/thirdparty/src/newton/dMath/dMathDefines.h @@ -0,0 +1,925 @@ +/* Copyright (c) <2003-2019> +* +* 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 +#include +#include +#include +#include +#include + +#ifdef _MSC_VER + #include + #include + #ifdef _DEBUG + #include + 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 + #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 + + #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 +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 +T dSign (T A) +{ + return (A >= T(0)) ? T(1) : T(-1); +} + +template +void dSwap(T& A, T& B) +{ + T tmp (A); + A = B; + B = tmp; +} + +template +T dMax(T A, T B) +{ + return (A > B) ? A : B; +} + +template +T dMin(T A, T B) +{ + return (A < B) ? A : B; +} + +template +T dClamp(T val, T min, T max) +{ + return dMax (min, dMin (max, val)); +} + +template +T dMod(T val, T mod) +{ + return T (fmod (T(val), T(mod))); +} + +template +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 +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 +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 +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 +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 +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 +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 (©[i * size], &matrix[row], size * sizeof (T)); + row += stride; + } + return dCholeskyFactorization(size, size, copy); +} + +template +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 +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 +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 +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 +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 +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 +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 +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 + diff --git a/thirdparty/src/newton/dMath/dMatrix.cpp b/thirdparty/src/newton/dMath/dMatrix.cpp new file mode 100644 index 000000000..57dfb6ca4 --- /dev/null +++ b/thirdparty/src/newton/dMath/dMatrix.cpp @@ -0,0 +1,1036 @@ +/* Copyright (c) <2003-2019> +* +* 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" + +// calculate an orthonormal matrix with the front vector pointing on the +// dir direction, and the up and right are determined by using the GramSchidth procedure + +dMatrix dGetIdentityMatrix() +{ + return dMatrix (dVector (dFloat (1.0f), dFloat (0.0f), dFloat (0.0f), dFloat (0.0f)), + dVector (dFloat (0.0f), dFloat (1.0f), dFloat (0.0f), dFloat (0.0f)), + dVector (dFloat (0.0f), dFloat (0.0f), dFloat (1.0f), dFloat (0.0f)), + dVector (dFloat (0.0f), dFloat (0.0f), dFloat (0.0f), dFloat (1.0f))); +} + +dMatrix dGetZeroMatrix () +{ + return dMatrix (dVector (dFloat (0.0f), dFloat (0.0f), dFloat (0.0f), dFloat (0.0f)), + dVector (dFloat (0.0f), dFloat (0.0f), dFloat (0.0f), dFloat (0.0f)), + dVector (dFloat (0.0f), dFloat (0.0f), dFloat (0.0f), dFloat (0.0f)), + dVector (dFloat (0.0f), dFloat (0.0f), dFloat (0.0f), dFloat (0.0f))); +} + +dMatrix dGrammSchmidt(const dVector& dir) +{ + dVector up(dFloat (0.0f)); + dVector right(dFloat (0.0f)); + dVector front (dir); + + front = front.Scale(1.0f / dSqrt (front.DotProduct3(front))); + if (dAbs (front.m_z) > 0.577f) { + right = front.CrossProduct(dVector (-front.m_y, front.m_z, dFloat (0.0f))); + } else { + right = front.CrossProduct(dVector (-front.m_y, front.m_x, dFloat (0.0f))); + } + right = right.Scale (1.0f / dSqrt (right.DotProduct3(right))); + up = right.CrossProduct(front); + + front.m_w = dFloat (0.0f); + up.m_w = dFloat (0.0f); + right.m_w = dFloat (0.0f); + return dMatrix (front, up, right, dVector (dFloat(0.0f), dFloat (0.0f), dFloat (0.0f), dFloat (1.0f))); +} + +dMatrix dPitchMatrix(dFloat ang) +{ + dFloat cosAng; + dFloat sinAng; + sinAng = dSin (ang); + cosAng = dCos (ang); + return dMatrix (dVector (dFloat (1.0f), dFloat (0.0f), dFloat (0.0f), dFloat (0.0f)), + dVector (dFloat (0.0f), cosAng, sinAng, dFloat (0.0f)), + dVector (dFloat (0.0f), -sinAng, cosAng, dFloat (0.0f)), + dVector (dFloat (0.0f), dFloat (0.0f), dFloat (0.0f), dFloat (1.0f))); + +} + +dMatrix dYawMatrix(dFloat ang) +{ + dFloat cosAng; + dFloat sinAng; + sinAng = dSin (ang); + cosAng = dCos (ang); + return dMatrix (dVector (cosAng, dFloat (0.0f), -sinAng, dFloat (0.0f)), + dVector (dFloat(0.0f), 1.0f, 0.0f, dFloat (0.0f)), + dVector (sinAng, dFloat (0.0f), cosAng, dFloat (0.0f)), + dVector (dFloat(0.0f), dFloat (0.0f), dFloat (0.0f), dFloat (1.0f))); +} + +dMatrix dRollMatrix(dFloat ang) +{ + dFloat cosAng; + dFloat sinAng; + sinAng = dSin (ang); + cosAng = dCos (ang); + return dMatrix (dVector ( cosAng, sinAng, dFloat (0.0f), dFloat (0.0f)), + dVector (-sinAng, cosAng, dFloat (0.0f), dFloat (0.0f)), + dVector ((dFloat (0.0f)), dFloat (0.0f), dFloat (1.0f), dFloat (0.0f)), + dVector ((dFloat (0.0f)), dFloat (0.0f), dFloat (0.0f), dFloat (1.0f))); +} + +dMatrix::dMatrix (const dQuaternion &rotation, const dVector &position) + :m_front(dFloat (0.0f)) + ,m_up(dFloat (0.0f)) + ,m_right(dFloat (0.0f)) + ,m_posit(dFloat (0.0f)) +{ + dFloat x2 = dFloat (2.0f) * rotation.m_x * rotation.m_x; + dFloat y2 = dFloat (2.0f) * rotation.m_y * rotation.m_y; + dFloat z2 = dFloat (2.0f) * rotation.m_z * rotation.m_z; +#ifdef _DEBUG + dFloat w2 = dFloat (2.0f) * rotation.m_w * rotation.m_w; + dAssert (dAbs (w2 + x2 + y2 + z2 - dFloat(2.0f)) < dFloat (1.e-2f)); +#endif + + dFloat xy = dFloat (2.0f) * rotation.m_x * rotation.m_y; + dFloat xz = dFloat (2.0f) * rotation.m_x * rotation.m_z; + dFloat xw = dFloat (2.0f) * rotation.m_x * rotation.m_w; + dFloat yz = dFloat (2.0f) * rotation.m_y * rotation.m_z; + dFloat yw = dFloat (2.0f) * rotation.m_y * rotation.m_w; + dFloat zw = dFloat (2.0f) * rotation.m_z * rotation.m_w; + + m_front = dVector (dFloat (1.0f) - y2 - z2, xy + zw , xz - yw , dFloat (0.0f)); + m_up = dVector (xy - zw , dFloat (1.0f) - x2 - z2, yz + xw , dFloat (0.0f)); + m_right = dVector (xz + yw , yz - xw , dFloat (1.0f) - x2 - y2 , dFloat (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 = dFloat (1.0f); +} + +dMatrix::dMatrix (dFloat pitch, dFloat yaw, dFloat roll, const dVector& location) +{ + dMatrix& me = *this; + me = dPitchMatrix(pitch) * dYawMatrix(yaw) * dRollMatrix(roll); + me.m_posit = location; + me.m_posit.m_w = dFloat (1.0f); +} + +bool dMatrix::TestIdentity() const +{ + const dMatrix& matrix = *this; + const dMatrix& identity = dGetIdentityMatrix(); + + bool isIdentity = true; + for (int i = 0; isIdentity && (i < 3); i ++) { + isIdentity &= dAbs (matrix[3][i]) < 1.0e-4f; + for (int j = i; isIdentity && (j < 3); j ++) { + isIdentity &= dAbs (matrix[i][j]-identity[i][j]) < 1.0e-4f; + } + } + return isIdentity; +} + +bool dMatrix::TestOrthogonal() const +{ + dVector n (m_front.CrossProduct(m_up)); + dFloat a = m_right.DotProduct3(m_right); + dFloat b = m_up.DotProduct3(m_up); + dFloat c = m_front.DotProduct3(m_front); + dFloat d = n.DotProduct3(m_right); + + return (m_front[3] == dFloat (dFloat (0.0f))) & + (m_up[3] == dFloat (dFloat (0.0f))) & + (m_right[3] == dFloat (dFloat (0.0f))) & + (m_posit[3] == dFloat (1.0f)) & + (dAbs(a - dFloat (1.0f)) < dFloat (1.0e-4f)) & + (dAbs(b - dFloat (1.0f)) < dFloat (1.0e-4f)) & + (dAbs(c - dFloat (1.0f)) < dFloat (1.0e-4f)) & + (dAbs(d - dFloat (1.0f)) < dFloat (1.0e-4f)); +} + +void dMatrix::GetEulerAngles(dVector& euler0, dVector& euler1, dEulerAngleOrder order) const +{ + // Assuming the angles are in radians. +#ifdef _NEWTON_USE_DOUBLE + const dFloat tol = 0.99999995f; +#else + const dFloat tol = 0.99995f; +#endif + + switch (order) + { + case m_pitchYawRoll: + { + const dMatrix& matrix = *this; + if (matrix[0][2] > tol) { + dFloat picth0 = dFloat (0.0f); + dFloat yaw0 = -dPi * 0.5f; + dFloat roll0 = -dAtan2(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] < -tol) { + dFloat picth0 = dFloat (0.0f); + dFloat yaw0 = dPi * 0.5f; + dFloat roll0 = dAtan2(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 { + dFloat yaw0 = -dAsin(matrix[0][2]); + dFloat yaw1 = dPi - yaw0; + + dFloat picth0 = dAtan2( matrix[1][2], matrix[2][2]); + dFloat picth1 = dAtan2(-matrix[1][2], -matrix[2][2]); + + dFloat roll0 = dAtan2( matrix[0][1], matrix[0][0]); + dFloat roll1 = dAtan2(-matrix[0][1], -matrix[0][0]); + + if (yaw1 > dPi) { + yaw1 -= 2.0f * dPi; + } + + euler0[0] = picth0; + euler0[1] = yaw0; + euler0[2] = roll0; + + euler1[0] = picth1; + euler1[1] = yaw1; + euler1[2] = roll1; + } + +#ifdef _DEBUG + dMatrix m0(dPitchMatrix(euler0[0]) * dYawMatrix(euler0[1]) * dRollMatrix(euler0[2])); + dMatrix m1(dPitchMatrix(euler1[0]) * dYawMatrix(euler1[1]) * dRollMatrix(euler1[2])); + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + dFloat error = dAbs(m0[i][j] - matrix[i][j]); + dAssert(error < dFloat(5.0e-2f)); + error = dAbs(m1[i][j] - matrix[i][j]); + dAssert(error < dFloat(5.0e-2f)); + } + } +#endif + break; + } + + case m_pitchRollYaw: + { + const dMatrix& matrix = *this; + //dMatrix matrix(dPitchMatrix(30.0f * dDegreeToRad) * dRollMatrix(-90.0f * dDegreeToRad) * dYawMatrix(50.0f * dDegreeToRad)); + if (matrix[0][1] > tol) { + + dFloat picth0 = dFloat (0.0f); + dFloat roll0 = dFloat (dPi * 0.5f); + dFloat yaw0 = dAtan2(matrix[1][2], matrix[2][2]); + + euler0[0] = picth0; + euler0[1] = yaw0; + euler0[2] = roll0; + + euler1[0] = picth0; + euler1[1] = yaw0; + euler1[2] = roll0; + + } else if (matrix[0][1] < -tol) { + + dFloat picth0 = dFloat (0.0f); + dFloat roll0 = dFloat(-dPi * 0.5f); + dFloat yaw0 = dAtan2(-matrix[1][2], matrix[2][2]); + + euler0[0] = picth0; + euler0[1] = yaw0; + euler0[2] = roll0; + + euler1[0] = picth0; + euler1[1] = yaw0; + euler1[2] = roll0; + + } else { + dFloat roll0 = dAsin(matrix[0][1]); + dFloat roll1 = dPi - roll0; + + dFloat yaw0 = dAtan2(-matrix[0][2], matrix[0][0]); + dFloat yaw1 = dAtan2( matrix[0][2], -matrix[0][0]); + + dFloat picth0 = dAtan2(-matrix[2][1], matrix[1][1]); + dFloat picth1 = dAtan2( matrix[2][1], -matrix[1][1]); + + if (roll1 > dFloat (dPi)) { + roll1 -= dFloat (2.0f * dPi); + } + + euler0[0] = picth0; + euler0[1] = yaw0; + euler0[2] = roll0; + + euler1[0] = picth1; + euler1[1] = yaw1; + euler1[2] = roll1; + } + +#ifdef _DEBUG + dMatrix m0(dPitchMatrix(euler0[0]) * dRollMatrix(euler0[2]) * dYawMatrix(euler0[1])); + dMatrix m1(dPitchMatrix(euler1[0]) * dRollMatrix(euler1[2]) * dYawMatrix(euler1[1])); + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + dFloat error = dAbs(m0[i][j] - matrix[i][j]); + dAssert(error < dFloat(5.0e-2f)); + error = dAbs(m1[i][j] - matrix[i][j]); + dAssert(error < dFloat(5.0e-2f)); + } + } +#endif + break; + } + + default: + { + int a0 = (order >> 8) & 3; + int a1 = (order >> 4) & 3; + int a2 = (order >> 0) & 3; + const dMatrix& matrix = *this; + + // Assuming the angles are in radians. + if (matrix[a0][a2] > tol) { + dFloat picth0 = dFloat (0.0f); + dFloat yaw0 = dFloat(-3.141592f * 0.5f); + dFloat roll0 = -dAtan2(matrix[a2][a1], matrix[a1][a1]); + euler0[a0] = picth0; + euler0[a1] = yaw0; + euler0[a2] = roll0; + + euler1[a0] = picth0; + euler1[a1] = yaw0; + euler1[a2] = roll0; + + } else if (matrix[a0][a2] < -tol) { + dFloat picth0 = dFloat (0.0f); + dFloat yaw0 = dFloat(3.141592f * 0.5f); + dFloat roll0 = dAtan2(matrix[a2][a1], matrix[a1][a1]); + euler0[a0] = picth0; + euler0[a1] = yaw0; + euler0[a2] = roll0; + + euler1[a0] = picth0; + euler1[a1] = yaw0; + euler1[a2] = roll0; + } else { + //euler[a0] = -dAtan2(-matrix[a1][a2], matrix[a2][a2]); + //euler[a1] = -dAsin ( matrix[a0][a2]); + //euler[a2] = -dAtan2(-matrix[a0][a1], matrix[a0][a0]); + + dFloat yaw0 = -dAsin(matrix[a0][a2]); + dFloat yaw1 = 3.141592f - yaw0; + dFloat sign0 = dSign(dCos(yaw0)); + dFloat sign1 = dSign(dCos(yaw1)); + + dFloat picth0 = dAtan2(matrix[a1][a2] * sign0, matrix[a2][a2] * sign0); + dFloat picth1 = dAtan2(matrix[a1][a2] * sign1, matrix[a2][a2] * sign1); + + dFloat roll0 = dAtan2(matrix[a0][a1] * sign0, matrix[a0][a0] * sign0); + dFloat roll1 = dAtan2(matrix[a0][a1] * sign1, matrix[a0][a0] * sign1); + + if (yaw1 > 3.141592f) { + yaw1 -= 2.0f * 3.141592f; + } + + euler0[a0] = picth0; + euler0[a1] = yaw0; + euler0[a2] = roll0; + + euler1[a0] = picth1; + euler1[a1] = yaw1; + euler1[a2] = roll1; + } + } + } + + euler0[3] = dFloat (0.0f); + euler1[3] = dFloat (0.0f); +} + +dMatrix dMatrix::Inverse () const +{ + return dMatrix (dVector (m_front.m_x, m_up.m_x, m_right.m_x, dFloat (0.0f)), + dVector (m_front.m_y, m_up.m_y, m_right.m_y, dFloat (0.0f)), + dVector (m_front.m_z, m_up.m_z, m_right.m_z, dFloat (0.0f)), + dVector (- m_posit.DotProduct3(m_front), - m_posit.DotProduct3(m_up), - m_posit.DotProduct3(m_right), dFloat (1.0f))); +} + +dMatrix dMatrix::Transpose () const +{ + return dMatrix (dVector (m_front.m_x, m_up.m_x, m_right.m_x, dFloat (0.0f)), + dVector (m_front.m_y, m_up.m_y, m_right.m_y, dFloat (0.0f)), + dVector (m_front.m_z, m_up.m_z, m_right.m_z, dFloat (0.0f)), + dVector (dFloat(0.0f), dFloat (0.0f), dFloat (0.0f), dFloat (1.0f))); +} + +dMatrix dMatrix::Transpose4X4 () const +{ + return dMatrix (dVector (m_front.m_x, m_up.m_x, m_right.m_x, m_posit.m_x), + dVector (m_front.m_y, m_up.m_y, m_right.m_y, m_posit.m_y), + dVector (m_front.m_z, m_up.m_z, m_right.m_z, m_posit.m_z), + dVector (m_front.m_w, m_up.m_w, m_right.m_w, m_posit.m_w)); + +} + +dVector dMatrix::RotateVector (const dVector &v) const +{ + return dVector (v.m_x * m_front.m_x + v.m_y * m_up.m_x + v.m_z * m_right.m_x, + v.m_x * m_front.m_y + v.m_y * m_up.m_y + v.m_z * m_right.m_y, + v.m_x * m_front.m_z + v.m_y * m_up.m_z + v.m_z * m_right.m_z, dFloat (0.0f)); +} + +dVector dMatrix::UnrotateVector (const dVector &v) const +{ + return dVector (v.DotProduct3(m_front), v.DotProduct3(m_up), v.DotProduct3(m_right), dFloat (0.0f)); +} + +dVector dMatrix::RotateVector4x4 (const dVector &v) const +{ + return dVector (v.m_x * m_front.m_x + v.m_y * m_up.m_x + v.m_z * m_right.m_x + v.m_w * m_posit.m_x, + v.m_x * m_front.m_y + v.m_y * m_up.m_y + v.m_z * m_right.m_y + v.m_w * m_posit.m_y, + v.m_x * m_front.m_z + v.m_y * m_up.m_z + v.m_z * m_right.m_z + v.m_w * m_posit.m_z, + v.m_x * m_front.m_w + v.m_y * m_up.m_w + v.m_z * m_right.m_w + v.m_w * m_posit.m_w); +} + + +dVector dMatrix::TransformVector (const dVector &v) const +{ + return m_posit + RotateVector(v); +} + +dVector dMatrix::UntransformVector (const dVector &v) const +{ + dVector rot (UnrotateVector(v - m_posit)); + rot.m_w = dFloat (1.0f); + return rot; +} + + +void dMatrix::TransformTriplex (dFloat* const dst, int dstStrideInBytes, const dFloat* const src, int srcStrideInBytes, int count) const +{ + dstStrideInBytes /= sizeof (dFloat); + srcStrideInBytes /= sizeof (dFloat); + for (int i = 0 ; i < count; i ++ ) { + dFloat x = src[srcStrideInBytes * i + 0]; + dFloat y = src[srcStrideInBytes * i + 1]; + dFloat z = src[srcStrideInBytes * i + 2]; + dst[dstStrideInBytes * i + 0] = x * m_front.m_x + y * m_up.m_x + z * m_right.m_x + m_posit.m_x; + dst[dstStrideInBytes * i + 1] = x * m_front.m_y + y * m_up.m_y + z * m_right.m_y + m_posit.m_y; + dst[dstStrideInBytes * i + 2] = x * m_front.m_z + y * m_up.m_z + z * m_right.m_z + m_posit.m_z; + } +} + +#ifndef _NEWTON_USE_DOUBLE +void dMatrix::TransformTriplex (dFloat64* const dst, int dstStrideInBytes, const dFloat64* const src, int srcStrideInBytes, int count) const +{ + dstStrideInBytes /= sizeof (dFloat64); + srcStrideInBytes /= sizeof (dFloat64); + for (int i = 0 ; i < count; i ++ ) { + dFloat64 x = src[srcStrideInBytes * i + 0]; + dFloat64 y = src[srcStrideInBytes * i + 1]; + dFloat64 z = src[srcStrideInBytes * i + 2]; + dst[dstStrideInBytes * i + 0] = x * m_front.m_x + y * m_up.m_x + z * m_right.m_x + m_posit.m_x; + dst[dstStrideInBytes * i + 1] = x * m_front.m_y + y * m_up.m_y + z * m_right.m_y + m_posit.m_y; + dst[dstStrideInBytes * i + 2] = x * m_front.m_z + y * m_up.m_z + z * m_right.m_z + m_posit.m_z; + } +} +#endif + +dMatrix dMatrix::operator* (const dMatrix &B) const +{ + const dMatrix& A = *this; + return dMatrix (dVector (A[0][0] * B[0][0] + A[0][1] * B[1][0] + A[0][2] * B[2][0] + A[0][3] * B[3][0], + A[0][0] * B[0][1] + A[0][1] * B[1][1] + A[0][2] * B[2][1] + A[0][3] * B[3][1], + A[0][0] * B[0][2] + A[0][1] * B[1][2] + A[0][2] * B[2][2] + A[0][3] * B[3][2], + A[0][0] * B[0][3] + A[0][1] * B[1][3] + A[0][2] * B[2][3] + A[0][3] * B[3][3]), + dVector (A[1][0] * B[0][0] + A[1][1] * B[1][0] + A[1][2] * B[2][0] + A[1][3] * B[3][0], + A[1][0] * B[0][1] + A[1][1] * B[1][1] + A[1][2] * B[2][1] + A[1][3] * B[3][1], + A[1][0] * B[0][2] + A[1][1] * B[1][2] + A[1][2] * B[2][2] + A[1][3] * B[3][2], + A[1][0] * B[0][3] + A[1][1] * B[1][3] + A[1][2] * B[2][3] + A[1][3] * B[3][3]), + dVector (A[2][0] * B[0][0] + A[2][1] * B[1][0] + A[2][2] * B[2][0] + A[2][3] * B[3][0], + A[2][0] * B[0][1] + A[2][1] * B[1][1] + A[2][2] * B[2][1] + A[2][3] * B[3][1], + A[2][0] * B[0][2] + A[2][1] * B[1][2] + A[2][2] * B[2][2] + A[2][3] * B[3][2], + A[2][0] * B[0][3] + A[2][1] * B[1][3] + A[2][2] * B[2][3] + A[2][3] * B[3][3]), + dVector (A[3][0] * B[0][0] + A[3][1] * B[1][0] + A[3][2] * B[2][0] + A[3][3] * B[3][0], + A[3][0] * B[0][1] + A[3][1] * B[1][1] + A[3][2] * B[2][1] + A[3][3] * B[3][1], + A[3][0] * B[0][2] + A[3][1] * B[1][2] + A[3][2] * B[2][2] + A[3][3] * B[3][2], + A[3][0] * B[0][3] + A[3][1] * B[1][3] + A[3][2] * B[2][3] + A[3][3] * B[3][3])); +} + +dVector dMatrix::TransformPlane (const dVector &localPlane) const +{ + dVector tmp (RotateVector (localPlane)); + tmp.m_w = localPlane.m_w - (localPlane.DotProduct3(UnrotateVector (m_posit))); + return tmp; +} + +dVector dMatrix::UntransformPlane (const dVector &globalPlane) const +{ + dVector tmp (UnrotateVector (globalPlane)); + tmp.m_w = globalPlane.DotProduct3(m_posit) + globalPlane.m_w; + return tmp; +} + +bool dMatrix::SanityCheck() const +{ + dVector right (m_front.CrossProduct(m_up)); + if (dAbs (right.DotProduct3(m_right)) < 0.9999f) { + return false; + } + if (dAbs (m_right.m_w) > 0.0f) { + return false; + } + if (dAbs (m_up.m_w) > 0.0f) { + return false; + } + if (dAbs (m_right.m_w) > 0.0f) { + return false; + } + + if (dAbs (m_posit.m_w) != dFloat (1.0f)) { + return false; + } + + return true; +} + +dMatrix dMatrix::Inverse4x4 () const +{ + dMatrix tmp (*this); + dMatrix inv (dGetIdentityMatrix()); + for (int i = 0; i < 4; i ++) { + dFloat pivot = dAbs(tmp[i][i]); + if (pivot < 0.01f) { + int permute = i; + for (int j = i + 1; j < 4; j++) { + dFloat pivot1 = dAbs(tmp[j][i]); + if (pivot1 > pivot) { + permute = j; + pivot = pivot1; + } + } + dAssert(pivot > 1.0e-6f); + if (permute != i) { + for (int j = 0; j < 4; j++) { + dSwap(inv[i][j], inv[permute][j]); + dSwap(tmp[i][j], tmp[permute][j]); + } + } + } + + for (int j = i + 1; j < 4; j++) { + dFloat scale = tmp[j][i] / tmp[i][i]; + for (int k = 0; k < 4; k++) { + tmp[j][k] -= scale * tmp[i][k]; + inv[j][k] -= scale * inv[i][k]; + } + tmp[j][i] = dFloat (0.0f); + } + } + + dVector zero(dFloat (0.0f)); + for (int i = 3; i >= 0; i--) { + dVector acc (zero); + for (int j = i + 1; j < 4; j++) { + dFloat pivot = tmp[i][j]; + for (int k = 0; k < 4; k++) { + acc[k] += pivot * inv[j][k]; + } + } + dFloat den = dFloat (1.0f) / tmp[i][i]; + for (int k = 0; k < 4; k++) { + inv[i][k] = den * (inv[i][k] - acc[k]); + } + } + +#ifdef _DEBUG + tmp = *this * inv; + for (int i = 0; i < 4; i++) { + dAssert(dAbs(tmp[i][i] - dFloat (1.0f)) < dFloat(1.0e-4f)); + for (int j = i + 1; j < 4; j++) { + dAssert(dAbs(tmp[i][j]) < dFloat(1.0e-3f)); + dAssert(dAbs(tmp[j][i]) < dFloat(1.0e-3f)); + } + } +#endif + + return inv; +} + +/* +static inline void ROT(dMatrix &a, int i, int j, int k, int l, dFloat s, dFloat tau) +{ + dFloat g; + dFloat h; + g = a[i][j]; + h = a[k][l]; + a[i][j] = g - s * (h + g * tau); + a[k][l] = h + s * (g - h * tau); +} +*/ +// from numerical recipes in c +// Jacobian method for computing the eigenvectors of a symmetric matrix +dMatrix dMatrix::JacobiDiagonalization (dVector &eigenValues, const dMatrix& initialMatrix) const +{ +/* + dMatrix mat(*this); + dMatrix eigenVectors(initialMatrix); + dFloat thresh; + dFloat b[3]; + dFloat z[3]; + dFloat d[3]; + const dFloat EPSILON = 1.0e-5f; + + b[0] = mat[0][0]; + b[1] = mat[1][1]; + b[2] = mat[2][2]; + + d[0] = mat[0][0]; + d[1] = mat[1][1]; + d[2] = mat[2][2]; + + z[0] = dFloat (0.0f); + z[1] = dFloat (0.0f); + z[2] = dFloat (0.0f); + + int nrot = 0; + for (int i = 0; i < 50; i++) { + dFloat sm = dAbs(mat[0][1]) + dAbs(mat[0][2]) + dAbs(mat[1][2]); + + if (sm < (EPSILON * 1.0e-4f)) { + dAssert (dAbs((eigenVectors.m_front.DotProduct3(eigenVectors.m_front)) - 1.0f) < EPSILON); + dAssert (dAbs((eigenVectors.m_up.DotProduct3(eigenVectors.m_up)) - 1.0f) < EPSILON); + dAssert (dAbs((eigenVectors.m_right.DotProduct3(eigenVectors.m_right)) - 1.0f) < EPSILON); + eigenValues = dVector (d[0], d[1], d[2], dFloat (dFloat (0.0f))); + return eigenVectors.Inverse(); + } + + if (i < 3) { + thresh = (dFloat)(0.2f / 9.0f) * sm; + } else { + thresh = 0.0; + } + + + // First row + dFloat g = 100.0f * dAbs(mat[0][1]); + if ((i > 3) && (dAbs(d[0]) + g == dAbs(d[0])) && (dAbs(d[1]) + g == dAbs(d[1]))) { + mat[0][1] = dFloat (0.0f); + } else if (dAbs(mat[0][1]) > thresh) { + dFloat t; + dFloat h = d[1] - d[0]; + if (dAbs(h) + g == dAbs(h)) { + t = mat[0][1] / h; + } else { + dFloat theta = dFloat (0.5f) * h / mat[0][1]; + t = dFloat (1.0f) / (dAbs(theta) + dSqrt(dFloat (1.0f) + theta * theta)); + if (theta < 0.0f) { + t = -t; + } + } + dFloat c = dFloat (1.0f) / dSqrt (1.0f + t * t); + dFloat s = t * c; + dFloat tau = s / (dFloat (1.0f) + c); + h = t * mat[0][1]; + z[0] -= h; + z[1] += h; + d[0] -= h; + d[1] += h; + mat[0][1] = dFloat (0.0f); + ROT (mat, 0, 2, 1, 2, s, tau); + ROT (eigenVectors, 0, 0, 0, 1, s, tau); + ROT (eigenVectors, 1, 0, 1, 1, s, tau); + ROT (eigenVectors, 2, 0, 2, 1, s, tau); + + nrot++; + } + + + // second row + g = 100.0f * dAbs(mat[0][2]); + if ((i > 3) && (dAbs(d[0]) + g == dAbs(d[0])) && (dAbs(d[2]) + g == dAbs(d[2]))) { + mat[0][2] = dFloat (0.0f); + } else if (dAbs(mat[0][2]) > thresh) { + dFloat t; + dFloat h = d[2] - d[0]; + if (dAbs(h) + g == dAbs(h)) { + t = (mat[0][2]) / h; + } else { + dFloat theta = dFloat (0.5f) * h / mat[0][2]; + t = dFloat (1.0f) / (dAbs(theta) + dSqrt(dFloat (1.0f) + theta * theta)); + if (theta < 0.0f) { + t = -t; + } + } + dFloat c = dFloat (1.0f) / dSqrt(1 + t * t); + dFloat s = t * c; + dFloat tau = s / (dFloat (1.0f) + c); + h = t * mat[0][2]; + z[0] -= h; + z[2] += h; + d[0] -= h; + d[2] += h; + mat[0][2]=0.0; + ROT (mat, 0, 1, 1, 2, s, tau); + ROT (eigenVectors, 0, 0, 0, 2, s, tau); + ROT (eigenVectors, 1, 0, 1, 2, s, tau); + ROT (eigenVectors, 2, 0, 2, 2, s, tau); + } + + // trird row + g = 100.0f * dAbs(mat[1][2]); + if ((i > 3) && (dAbs(d[1]) + g == dAbs(d[1])) && (dAbs(d[2]) + g == dAbs(d[2]))) { + mat[1][2] = dFloat (0.0f); + } else if (dAbs(mat[1][2]) > thresh) { + dFloat t; + dFloat h = d[2] - d[1]; + if (dAbs(h) + g == dAbs(h)) { + t = mat[1][2] / h; + } else { + dFloat theta = dFloat (0.5f) * h / mat[1][2]; + t = dFloat (1.0f) / (dAbs(theta) + dSqrt(dFloat (1.0f) + theta * theta)); + if (theta < 0.0f) { + t = -t; + } + } + dFloat c = dFloat (1.0f) / dSqrt(1 + t*t); + dFloat s = t * c; + dFloat tau = s / (dFloat (1.0f) + c); + + h = t * mat[1][2]; + z[1] -= h; + z[2] += h; + d[1] -= h; + d[2] += h; + mat[1][2] = dFloat (0.0f); + ROT (mat, 0, 1, 0, 2, s, tau); + ROT (eigenVectors, 0, 1, 0, 2, s, tau); + ROT (eigenVectors, 1, 1, 1, 2, s, tau); + ROT (eigenVectors, 2, 1, 2, 2, s, tau); + nrot++; + } + + b[0] += z[0]; d[0] = b[0]; z[0] = dFloat (0.0f); + b[1] += z[1]; d[1] = b[1]; z[1] = dFloat (0.0f); + b[2] += z[2]; d[2] = b[2]; z[2] = dFloat (0.0f); + } + + dAssert (0); + eigenValues = dVector (d[0], d[1], d[2], dFloat (dFloat (0.0f))); + return dGetIdentityMatrix(); +*/ + + dMatrix mat(*this); + dMatrix eigenVectors(initialMatrix.Transpose()); + + // 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 + dVector d(mat[0][0], mat[1][1], mat[2][2], dFloat(0.0f)); + dVector b(d); + for (int i = 0; i < 50; i++) { + dFloat sm = mat[0][1] * mat[0][1] + mat[0][2] * mat[0][2] + mat[1][2] * mat[1][2]; + if (sm < dFloat(1.0e-12f)) { + // make sure the the eigen vectors are orthonormal + //dVector tmp (eigenVectors.m_front.CrossProduct(eigenVectors.m_up)); + //if (tmp.DotProduct(eigenVectors.m_right).GetScalar() < dFloat(0.0f)) { + // eigenVectors.m_right = eigenVectors.m_right * dVector::m_negOne; + //} + dAssert(eigenVectors[0].DotProduct3(eigenVectors[1].CrossProduct(eigenVectors[2])) > dFloat(0.0f)); + break; + } + + dFloat thresh = dFloat(0.0f); + if (i < 3) { + thresh = (dFloat)(0.2f / 9.0f) * sm; + } + + dVector z(0.0f); + for (int ip = 0; ip < 2; ip++) { + for (int iq = ip + 1; iq < 3; iq++) { + dFloat g = dFloat(100.0f) * dAbs(mat[ip][iq]); + if ((i > 3) && ((dAbs(d[ip]) + g) == dAbs(d[ip])) && ((dAbs(d[iq]) + g) == dAbs(d[iq]))) { + mat[ip][iq] = dFloat(0.0f); + } + else if (dAbs(mat[ip][iq]) > thresh) { + + dFloat t; + dFloat h = d[iq] - d[ip]; + if (dAbs(h) + g == dAbs(h)) { + t = mat[ip][iq] / h; + } else { + dFloat theta = dFloat(0.5f) * h / mat[ip][iq]; + t = dFloat(1.0f) / (dAbs(theta) + dSqrt(dFloat(1.0f) + theta * theta)); + if (theta < dFloat(0.0f)) { + t = -t; + } + } + dFloat c = dFloat(1.0f) / dSqrt(dFloat(1.0f) + t * t); + dFloat s = t * c; + dFloat tau = s / (dFloat(1.0f) + c); + h = t * mat[ip][iq]; + z[ip] -= h; + z[iq] += h; + d[ip] -= h; + d[iq] += h; + mat[ip][iq] = dFloat(0.0f); + + for (int j = 0; j <= ip - 1; j++) { + dFloat g0 = mat[j][ip]; + dFloat h0 = mat[j][iq]; + mat[j][ip] = g0 - s * (h0 + g0 * tau); + mat[j][iq] = h0 + s * (g0 - h0 * tau); + } + for (int j = ip + 1; j <= iq - 1; j++) { + dFloat g0 = mat[ip][j]; + dFloat h0 = mat[j][iq]; + mat[ip][j] = g0 - s * (h0 + g0 * tau); + mat[j][iq] = h0 + s * (g0 - h0 * tau); + } + for (int j = iq + 1; j < 3; j++) { + dFloat g0 = mat[ip][j]; + dFloat h0 = mat[iq][j]; + mat[ip][j] = g0 - s * (h0 + g0 * tau); + mat[iq][j] = h0 + s * (g0 - h0 * tau); + } + + dVector sv(s); + dVector tauv(tau); + dVector gv(eigenVectors[ip]); + dVector hv(eigenVectors[iq]); + eigenVectors[ip] -= sv * (hv + gv * tauv); + eigenVectors[iq] += sv * (gv - hv * tauv); + } + } + } + + b += z; + d = b; + } + + #ifdef _DEBUG + dMatrix diag(dGetIdentityMatrix()); + diag[0][0] = d[0]; + diag[1][1] = d[1]; + diag[2][2] = d[2]; + dMatrix E(eigenVectors.Transpose()); + dMatrix matrix(E * diag * E.Transpose()); + for (int j = 0; j < 3; j++) { + for (int k = 0; k < 3; k++) { + dFloat error = (*this)[j][k] - matrix[j][k]; + dAssert((error * error) < dFloat(1.0e-4f)); + } + } + #endif + + eigenValues = d; + return eigenVectors.Transpose(); +} + +//void dMatrix::PolarDecomposition (dMatrix& orthogonal, dMatrix& symetric) const +void dMatrix::PolarDecomposition (dMatrix& transformMatrix, dVector& scale, dMatrix& stretchAxis, const dMatrix& initialStretchAxis) const +{ + // a polar decomposition decompose matrix A = O * S + // where S = sqrt (transpose (L) * L) + + // calculate transpose (L) * L + dMatrix LL ((*this) * Transpose()); + + // check is this si a pure uniformScale * rotation * translation + dFloat det2 = (LL[0][0] + LL[1][1] + LL[2][2]) * (1.0f / 3.0f); + + dFloat invdet2 = dFloat (1.0f) / det2; + + dMatrix pureRotation (LL); + pureRotation[0] = pureRotation[0].Scale (invdet2); + pureRotation[1] = pureRotation[1].Scale (invdet2); + pureRotation[2] = pureRotation[2].Scale (invdet2); + + const dMatrix& me = *this; + dFloat sign = me[2].DotProduct3 (me[0].CrossProduct(me[1])) > 0.0f ? 1.0f : -1.0f; + dFloat det = pureRotation[2].DotProduct3 (pureRotation[0].CrossProduct(pureRotation[1])); + if (dAbs (det - 1.0f) < 1.e-5f){ + // this is a pure scale * rotation * translation + det = sign * dSqrt (det2); + scale[0] = det; + scale[1] = det; + scale[2] = det; + scale[3] = dFloat (1.0f); + det = dFloat (1.0f)/ det; + transformMatrix.m_front = m_front.Scale (det); + transformMatrix.m_up = m_up.Scale (det); + transformMatrix.m_right = m_right.Scale (det); + transformMatrix[0][3] = dFloat (0.0f); + transformMatrix[1][3] = dFloat (0.0f); + transformMatrix[2][3] = dFloat (0.0f); + transformMatrix.m_posit = m_posit; + stretchAxis = dGetIdentityMatrix(); + + } else { + stretchAxis = LL.JacobiDiagonalization(scale, initialStretchAxis); + + // I need to deal with buy 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 * dSqrt (scale[0]); + scale[1] = sign * dSqrt (scale[1]); + scale[2] = sign * dSqrt (scale[2]); + scale[3] = dFloat (1.0f); + + dMatrix scaledAxis; + scaledAxis[0] = stretchAxis[0].Scale (1.0f / scale[0]); + scaledAxis[1] = stretchAxis[1].Scale (1.0f / scale[1]); + scaledAxis[2] = stretchAxis[2].Scale (1.0f / scale[2]); + scaledAxis[3] = stretchAxis[3]; + dMatrix symetricInv (stretchAxis.Transpose() * scaledAxis); + + transformMatrix = symetricInv * (*this); + transformMatrix.m_posit = m_posit; + } +} + +dMatrix::dMatrix (const dMatrix& transformMatrix, const dVector& scale, const dMatrix& stretchAxis) + :m_front(dFloat (0.0f)) + ,m_up(dFloat (0.0f)) + ,m_right(dFloat (0.0f)) + ,m_posit(dFloat (0.0f)) +{ + dMatrix 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; +} + +dSpatialMatrix dSpatialMatrix::Inverse(int rows) const +{ + dSpatialMatrix tmp(*this); + dSpatialMatrix inv(dFloat (0.0f)); + for (int i = 0; i < rows; i++) { + inv[i][i] = dFloat (1.0f); + } + +#if 0 + for (int i = 0; i < rows; i++) { + dFloat val = tmp[i][i]; + dAssert(dAbs(val) > 1.0e-12f); + dFloat den = dFloat (1.0f) / val; + + tmp[i] = tmp[i].Scale(den); + tmp[i][i] = dFloat (1.0f); + inv[i] = inv[i].Scale(den); + + for (int j = 0; j < i; j++) { + dFloat pivot = -tmp[j][i]; + tmp[j] = tmp[j] + tmp[i].Scale(pivot); + inv[j] = inv[j] + inv[i].Scale(pivot); + } + + for (int j = i + 1; j < rows; j++) { + dFloat pivot = -tmp[j][i]; + tmp[j] = tmp[j] + tmp[i].Scale(pivot); + inv[j] = inv[j] + inv[i].Scale(pivot); + } + } + +#else + + for (int i = 0; i < rows; i++) { + dFloat pivot = dAbs(tmp[i][i]); + dAssert(pivot >= 0.01f); + if (pivot <= 0.01f) { + int permute = i; + for (int j = i + 1; j < rows; j++) { + dFloat pivot1 = dAbs(tmp[j][i]); + if (pivot1 > pivot) { + permute = j; + pivot = pivot1; + } + } + dAssert(pivot > dFloat(1.0e-6f)); + if (permute != i) { + for (int j = 0; j < rows; j++) { + dSwap(tmp[i][j], tmp[permute][j]); + dSwap(tmp[i][j], tmp[permute][j]); + } + } + } + + for (int j = i + 1; j < rows; j++) { + dFloat scale = tmp[j][i] / tmp[i][i]; + tmp[j][i] = dFloat (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 (int i = rows - 1; i >= 0; i--) { + dSpatialVector acc(dFloat (0.0f)); + for (int j = i + 1; j < rows; j++) { + dFloat pivot = tmp[i][j]; + for (int k = 0; k < rows; k++) { + acc[k] += pivot * inv[j][k]; + } + } + dFloat den = dFloat (1.0f) / tmp[i][i]; + for (int k = 0; k < rows; k++) { + inv[i][k] = den * (inv[i][k] - acc[k]); + } + } +#endif + +#ifdef _DEBUG + for (int i = 0; i < rows; i++) { + for (int j = 0; j < rows; j++) { + tmp[i][j] = m_rows[j][i]; + } + } + for (int i = 0; i < rows; i++) { + dSpatialVector v(inv.VectorTimeMatrix (tmp[i], rows)); + dAssert (dAbs (v[i] - dFloat (1.0f)) < dFloat(1.0e-5f)); + for (int j = 0; j < rows; j++) { + if (j != i) { + dAssert (dAbs (v[j]) < dFloat(1.0e-5f)); + } + } + } +#endif + + return inv; +} + diff --git a/thirdparty/src/newton/dMath/dMatrix.h b/thirdparty/src/newton/dMath/dMatrix.h new file mode 100644 index 000000000..f499f5014 --- /dev/null +++ b/thirdparty/src/newton/dMath/dMatrix.h @@ -0,0 +1,181 @@ +/* Copyright (c) <2003-2019> +* +* 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 + diff --git a/thirdparty/src/newton/dMath/dQuaternion.cpp b/thirdparty/src/newton/dMath/dQuaternion.cpp new file mode 100644 index 000000000..2bfca38d8 --- /dev/null +++ b/thirdparty/src/newton/dMath/dQuaternion.cpp @@ -0,0 +1,206 @@ +/* Copyright (c) <2003-2019> +* +* 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; +} diff --git a/thirdparty/src/newton/dMath/dQuaternion.h b/thirdparty/src/newton/dMath/dQuaternion.h new file mode 100644 index 000000000..4daee1f20 --- /dev/null +++ b/thirdparty/src/newton/dMath/dQuaternion.h @@ -0,0 +1,109 @@ +/* Copyright (c) <2003-2019> +* +* 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 + diff --git a/thirdparty/src/newton/dMath/dStdAfxMath.cpp b/thirdparty/src/newton/dMath/dStdAfxMath.cpp new file mode 100644 index 000000000..70c490abd --- /dev/null +++ b/thirdparty/src/newton/dMath/dStdAfxMath.cpp @@ -0,0 +1,20 @@ +/* Copyright (c) <2003-2019> +* +* 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 + diff --git a/thirdparty/src/newton/dMath/dStdAfxMath.h b/thirdparty/src/newton/dMath/dStdAfxMath.h new file mode 100644 index 000000000..eb3a39ca5 --- /dev/null +++ b/thirdparty/src/newton/dMath/dStdAfxMath.h @@ -0,0 +1,36 @@ +/* Copyright (c) <2003-2019> +* +* 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 + #include +#endif + +#if ( defined (__MINGW32__) || defined (__MINGW64__) ) + #include +#endif + +#include +#include +#endif + diff --git a/thirdparty/src/newton/dMath/dVector.cpp b/thirdparty/src/newton/dMath/dVector.cpp new file mode 100644 index 000000000..a6c0e6ecd --- /dev/null +++ b/thirdparty/src/newton/dMath/dVector.cpp @@ -0,0 +1,13 @@ +/* Copyright (c) <2003-2019> +* +* 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" + diff --git a/thirdparty/src/newton/dMath/dVector.h b/thirdparty/src/newton/dMath/dVector.h new file mode 100644 index 000000000..a7f4f2892 --- /dev/null +++ b/thirdparty/src/newton/dMath/dVector.h @@ -0,0 +1,328 @@ +/* Copyright (c) <2003-2019> +* +* 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 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 +{ + public: + dVector() + :TemplateVector() + { + } + + dVector(dFloat val) + :TemplateVector(val) + { + } + + dVector (const TemplateVector& v) + :TemplateVector(v) + { + } + + dVector (const dFloat* const ptr) + :TemplateVector(ptr) + { + } + + dVector (dFloat x, dFloat y, dFloat z, dFloat w = 1.0f) + :TemplateVector(x, y, z, w) + { + } + +#ifndef _NEWTON_USE_DOUBLE + dVector (const TemplateVector& v) + :TemplateVector(dFloat(v.m_x), dFloat(v.m_y), dFloat(v.m_z), dFloat(v.m_w)) + { + } +#endif +}; + + +class dBigVector: public TemplateVector +{ + public: + dBigVector(){}; + dBigVector(dFloat64 val) + :TemplateVector (val) + { + } + + dBigVector (const dFloat64* const ptr) + :TemplateVector (ptr) + { + } + + dBigVector (const TemplateVector& v) + :TemplateVector (v.m_x, v.m_y, v.m_z, v.m_w) + { + } + + dBigVector(const dVector& v) + :TemplateVector(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 (x, y, z, w) + { + } +}; + + +template +TemplateVector::TemplateVector() +{ +} + +template +TemplateVector::TemplateVector(const T val) + :m_x (val), m_y(val), m_z(val), m_w(val) +{ +} + +template +TemplateVector::TemplateVector(const T *ptr) + :m_x (ptr[0]), m_y(ptr[1]), m_z(ptr[2]), m_w(0.0f) +{ +} + +template +TemplateVector::TemplateVector(T x, T y, T z, T w) + :m_x (x), m_y(y), m_z(z), m_w(w) +{ +} + +template +T& TemplateVector::operator[] (int i) +{ + return (&m_x)[i]; +} + +template +const T& TemplateVector::operator[] (int i) const +{ + return (&m_x)[i]; +} + +template +TemplateVector TemplateVector::Scale (T scale) const +{ + return TemplateVector (m_x * scale, m_y * scale, m_z * scale, m_w); +} + +template +TemplateVector TemplateVector::operator+ (const TemplateVector& B) const +{ + return TemplateVector (m_x + B.m_x, m_y + B.m_y, m_z + B.m_z, m_w); +} + +template +TemplateVector TemplateVector::operator* (const TemplateVector& B) const +{ + return TemplateVector(m_x * B.m_x, m_y * B.m_y, m_z * B.m_z, m_w); +} + + +template +TemplateVector& TemplateVector::operator+= (const TemplateVector& A) +{ + m_x += A.m_x; + m_y += A.m_y; + m_z += A.m_z; + return *this; +} + +template +TemplateVector TemplateVector::operator- (const TemplateVector& A) const +{ + return TemplateVector (m_x - A.m_x, m_y - A.m_y, m_z - A.m_z, m_w); +} + +template +TemplateVector& TemplateVector::operator-= (const TemplateVector& A) +{ + m_x -= A.m_x; + m_y -= A.m_y; + m_z -= A.m_z; + return *this; +} + +template +TemplateVector& TemplateVector::operator*= (const TemplateVector& A) +{ + m_x *= A.m_x; + m_y *= A.m_y; + m_z *= A.m_z; + //m_w *= A.m_w; + return *this; +} + +template +TemplateVector TemplateVector::Abs() const +{ + return dVector(dAbs(m_x), dAbs(m_y), dAbs(m_z), dAbs(m_w)); +} + +template +TemplateVector TemplateVector::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 +TemplateVector TemplateVector::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 +T TemplateVector::DotProduct3 (const TemplateVector& A) const +{ + return m_x * A.m_x + m_y * A.m_y + m_z * A.m_z; +} + +template +TemplateVector TemplateVector::Normalize () const +{ + T mag (DotProduct3(*this)); + return Scale (1.0f / T(sqrt (mag))); +} + +template +TemplateVector TemplateVector::CrossProduct (const TemplateVector& A) const +{ + return TemplateVector (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 + diff --git a/thirdparty/src/newton/dgCore/dg.cpp b/thirdparty/src/newton/dgCore/dg.cpp new file mode 100644 index 000000000..7b95b7001 --- /dev/null +++ b/thirdparty/src/newton/dgCore/dg.cpp @@ -0,0 +1,27 @@ +/* Copyright (c) <2003-2019> +* +* 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" + + + + + diff --git a/thirdparty/src/newton/dgCore/dg.h b/thirdparty/src/newton/dgCore/dg.h new file mode 100644 index 000000000..39fc05afa --- /dev/null +++ b/thirdparty/src/newton/dgCore/dg.h @@ -0,0 +1,67 @@ +/* Copyright (c) <2003-2019> +* +* 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 diff --git a/thirdparty/src/newton/dgCore/dgAABBPolygonSoup.cpp b/thirdparty/src/newton/dgCore/dgAABBPolygonSoup.cpp new file mode 100644 index 000000000..9b5166c1c --- /dev/null +++ b/thirdparty/src/newton/dgCore/dgAABBPolygonSoup.cpp @@ -0,0 +1,1390 @@ +/* Copyright (c) <2003-2019> +* +* 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 "dgHeap.h" +#include "dgStack.h" +#include "dgList.h" +#include "dgMatrix.h" +#include "dgAABBPolygonSoup.h" +#include "dgPolygonSoupBuilder.h" + + +#define DG_STACK_DEPTH 512 + + +DG_MSC_VECTOR_ALIGNMENT +class dgAABBPolygonSoup::dgNodeBuilder: public dgAABBPolygonSoup::dgNode +{ + public: + dgNodeBuilder (const dgVector& p0, const dgVector& p1) + :dgNode() + ,m_left (NULL) + ,m_right (NULL) + ,m_parent (NULL) + ,m_indexBox0(0) + ,m_indexBox1(0) + ,m_enumeration(-1) + ,m_faceIndex(0) + ,m_indexCount(0) + ,m_faceIndices(NULL) + { + SetBox (p0, p1); + } + + dgNodeBuilder (const dgVector* const vertexArray, dgInt32 faceIndex, dgInt32 indexCount, const dgInt32* const indexArray) + :dgNode() + ,m_left (NULL) + ,m_right (NULL) + ,m_parent (NULL) + ,m_indexBox0(0) + ,m_indexBox1(0) + ,m_enumeration(-1) + ,m_faceIndex(faceIndex) + ,m_indexCount(indexCount) + ,m_faceIndices(indexArray) + { + dgVector minP ( dgFloat32 (1.0e15f)); + dgVector maxP (-dgFloat32 (1.0e15f)); + for (dgInt32 i = 0; i < indexCount; i ++) { + dgInt32 index = indexArray[i]; + const dgVector& p (vertexArray[index]); + minP = p.GetMin(minP); + maxP = p.GetMax(maxP); + } + minP -= dgVector (dgFloat32 (1.0e-3f)); + maxP += dgVector (dgFloat32 (1.0e-3f)); + minP = minP & dgVector::m_triplexMask; + maxP = maxP & dgVector::m_triplexMask; + SetBox (minP, maxP); + } + + dgNodeBuilder (dgNodeBuilder* const left, dgNodeBuilder* const right) + :dgNode() + ,m_left(left) + ,m_right(right) + ,m_parent(NULL) + ,m_indexBox0(0) + ,m_indexBox1(0) + ,m_enumeration(-1) + ,m_faceIndex(0) + ,m_indexCount(0) + ,m_faceIndices(NULL) + { + m_left->m_parent = this; + m_right->m_parent = this; + + dgVector p0 (left->m_p0.GetMin(right->m_p0)); + dgVector p1 (left->m_p1.GetMax(right->m_p1)); + SetBox(p0, p1); + } + + void SetBox (const dgVector& p0, const dgVector& p1) + { + m_p0 = p0; + m_p1 = p1; + m_size = m_p1 - m_p0; + m_origin = (m_p1 + m_p0) * dgVector::m_half; + m_area = m_size.DotProduct(m_size.ShiftTripleRight()).m_x; + } + + DG_INLINE static dgFloat32 CalculateSurfaceArea (dgNodeBuilder* const node0, dgNodeBuilder* const node1, dgVector& minBox, dgVector& maxBox) + { + minBox = node0->m_p0.GetMin(node1->m_p0); + maxBox = node0->m_p1.GetMax(node1->m_p1); + + dgVector side0 ((maxBox - minBox) * dgVector::m_half); + //dgVector side1 (side0.m_y, side0.m_z, side0.m_x, dgFloat32 (0.0f)); + dgVector side1 (side0.ShiftTripleLeft()); + return side0.DotProduct(side1).GetScalar(); + } + + dgVector m_p0; + dgVector m_p1; + dgVector m_size; + dgVector m_origin; + dgFloat32 m_area; + + dgNodeBuilder* m_left; + dgNodeBuilder* m_right; + dgNodeBuilder* m_parent; + dgInt32 m_indexBox0; + dgInt32 m_indexBox1; + dgInt32 m_enumeration; + dgInt32 m_faceIndex; + dgInt32 m_indexCount; + const dgInt32* m_faceIndices; +} DG_GCC_VECTOR_ALIGNMENT; + + + +class dgAABBPolygonSoup::dgSpliteInfo +{ + public: + dgSpliteInfo (dgNodeBuilder* const boxArray, dgInt32 boxCount) + { + dgVector minP ( dgFloat32 (1.0e15f)); + dgVector maxP (-dgFloat32 (1.0e15f)); + + if (boxCount == 2) { + m_axis = 1; + for (dgInt32 i = 0; i < boxCount; i ++) { + const dgNodeBuilder& box = boxArray[i]; + const dgVector& p0 = box.m_p0; + const dgVector& p1 = box.m_p1; + minP = minP.GetMin (p0); + maxP = maxP.GetMax (p1); + } + + } else { + dgVector median (dgFloat32 (0.0f)); + dgVector varian (dgFloat32 (0.0f)); + for (dgInt32 i = 0; i < boxCount; i ++) { + const dgNodeBuilder& box = boxArray[i]; + + const dgVector& p0 = box.m_p0; + const dgVector& p1 = box.m_p1; + + minP = minP.GetMin (p0); + maxP = maxP.GetMax (p1); + dgVector p (dgVector::m_half * (p0 + p1)); + + median += p; + varian += p * p; + } + + varian = varian.Scale (dgFloat32 (boxCount)) - median * median; + + dgInt32 index = 0; + dgFloat32 maxVarian = dgFloat32 (-1.0e10f); + for (dgInt32 i = 0; i < 3; i ++) { + if (varian[i] > maxVarian) { + index = i; + maxVarian = varian[i]; + } + } + + dgVector center = median.Scale (dgFloat32 (1.0f) / dgFloat32 (boxCount)); + dgFloat32 test = center[index]; + dgInt32 i0 = 0; + dgInt32 i1 = boxCount - 1; + do { + for (; i0 <= i1; i0 ++) { + const dgNodeBuilder& box = boxArray[i0]; + dgFloat32 val = (box.m_p0[index] + box.m_p1[index]) * dgFloat32 (0.5f); + if (val > test) { + break; + } + } + + for (; i1 >= i0; i1 --) { + const dgNodeBuilder& box = boxArray[i1]; + dgFloat32 val = (box.m_p0[index] + box.m_p1[index]) * dgFloat32 (0.5f); + if (val < test) { + break; + } + } + + if (i0 < i1) { + dgSwap(boxArray[i0], boxArray[i1]); + i0++; + i1--; + } + } while (i0 <= i1); + + if (i0 > 0){ + i0 --; + } + if ((i0 + 1) >= boxCount) { + i0 = boxCount - 2; + } + + m_axis = i0 + 1; + } + + dgAssert (maxP.m_x - minP.m_x >= dgFloat32 (0.0f)); + dgAssert (maxP.m_y - minP.m_y >= dgFloat32 (0.0f)); + dgAssert (maxP.m_z - minP.m_z >= dgFloat32 (0.0f)); + m_p0 = minP; + m_p1 = maxP; + } + + dgInt32 m_axis; + dgVector m_p0; + dgVector m_p1; +}; + + + +dgAABBPolygonSoup::dgAABBPolygonSoup () + :dgPolygonSoupDatabase() + ,m_nodesCount(0) + ,m_indexCount(0) + ,m_aabb(NULL) + ,m_indices(NULL) +{ +} + +dgAABBPolygonSoup::~dgAABBPolygonSoup () +{ + if (m_aabb) { + dgFreeStack (m_aabb); + dgFreeStack (m_indices); + } +} + + +void dgAABBPolygonSoup::ImproveNodeFitness (dgNodeBuilder* const node) const +{ + dgAssert (node->m_left); + dgAssert (node->m_right); + + if (node->m_parent) { + dgAssert(node->m_parent->m_p0.m_w == dgFloat32(0.0f)); + dgAssert(node->m_parent->m_p1.m_w == dgFloat32(0.0f)); + + if (node->m_parent->m_left == node) { + dgFloat32 cost0 = node->m_area; + + dgVector cost1P0; + dgVector cost1P1; + dgFloat32 cost1 = dgNodeBuilder::CalculateSurfaceArea (node->m_right, node->m_parent->m_right, cost1P0, cost1P1); + + dgVector cost2P0; + dgVector cost2P1; + dgFloat32 cost2 = dgNodeBuilder::CalculateSurfaceArea (node->m_left, node->m_parent->m_right, cost2P0, cost2P1); + + if ((cost1 <= cost0) && (cost1 <= cost2)) { + dgNodeBuilder* const parent = node->m_parent; + node->m_p0 = parent->m_p0; + node->m_p1 = parent->m_p1; + node->m_area = parent->m_area; + node->m_size = parent->m_size; + node->m_origin = parent->m_origin; + + if (parent->m_parent) { + if (parent->m_parent->m_left == parent) { + parent->m_parent->m_left = node; + } else { + dgAssert (parent->m_parent->m_right == parent); + parent->m_parent->m_right = node; + } + } + node->m_parent = parent->m_parent; + parent->m_parent = node; + node->m_right->m_parent = parent; + parent->m_left = node->m_right; + node->m_right = parent; + parent->m_p0 = cost1P0; + parent->m_p1 = cost1P1; + parent->m_area = cost1; + parent->m_size = (parent->m_p1 - parent->m_p0) * dgVector::m_half; + parent->m_origin = (parent->m_p1 + parent->m_p0) * dgVector::m_half; + + } else if ((cost2 <= cost0) && (cost2 <= cost1)) { + dgNodeBuilder* const parent = node->m_parent; + node->m_p0 = parent->m_p0; + node->m_p1 = parent->m_p1; + node->m_area = parent->m_area; + node->m_size = parent->m_size; + node->m_origin = parent->m_origin; + + if (parent->m_parent) { + if (parent->m_parent->m_left == parent) { + parent->m_parent->m_left = node; + } else { + dgAssert (parent->m_parent->m_right == parent); + parent->m_parent->m_right = node; + } + } + node->m_parent = parent->m_parent; + parent->m_parent = node; + node->m_left->m_parent = parent; + parent->m_left = node->m_left; + node->m_left = parent; + + parent->m_p0 = cost2P0; + parent->m_p1 = cost2P1; + parent->m_area = cost2; + parent->m_size = (parent->m_p1 - parent->m_p0) * dgVector::m_half; + parent->m_origin = (parent->m_p1 + parent->m_p0) * dgVector::m_half; + } + } else { + dgFloat32 cost0 = node->m_area; + + dgVector cost1P0; + dgVector cost1P1; + dgFloat32 cost1 = dgNodeBuilder::CalculateSurfaceArea (node->m_left, node->m_parent->m_left, cost1P0, cost1P1); + + dgVector cost2P0; + dgVector cost2P1; + dgFloat32 cost2 = dgNodeBuilder::CalculateSurfaceArea (node->m_right, node->m_parent->m_left, cost2P0, cost2P1); + + if ((cost1 <= cost0) && (cost1 <= cost2)) { + dgNodeBuilder* const parent = node->m_parent; + node->m_p0 = parent->m_p0; + node->m_p1 = parent->m_p1; + node->m_area = parent->m_area; + node->m_size = parent->m_size; + node->m_origin = parent->m_origin; + + if (parent->m_parent) { + if (parent->m_parent->m_left == parent) { + parent->m_parent->m_left = node; + } else { + dgAssert (parent->m_parent->m_right == parent); + parent->m_parent->m_right = node; + } + } + node->m_parent = parent->m_parent; + parent->m_parent = node; + node->m_left->m_parent = parent; + parent->m_right = node->m_left; + node->m_left = parent; + + parent->m_p0 = cost1P0; + parent->m_p1 = cost1P1; + parent->m_area = cost1; + parent->m_size = (parent->m_p1 - parent->m_p0) * dgVector::m_half; + parent->m_origin = (parent->m_p1 + parent->m_p0) * dgVector::m_half; + + } else if ((cost2 <= cost0) && (cost2 <= cost1)) { + dgNodeBuilder* const parent = node->m_parent; + node->m_p0 = parent->m_p0; + node->m_p1 = parent->m_p1; + node->m_area = parent->m_area; + node->m_size = parent->m_size; + node->m_origin = parent->m_origin; + + if (parent->m_parent) { + if (parent->m_parent->m_left == parent) { + parent->m_parent->m_left = node; + } else { + dgAssert (parent->m_parent->m_right == parent); + parent->m_parent->m_right = node; + } + } + node->m_parent = parent->m_parent; + parent->m_parent = node; + node->m_right->m_parent = parent; + parent->m_right = node->m_right; + node->m_right = parent; + + parent->m_p0 = cost2P0; + parent->m_p1 = cost2P1; + parent->m_area = cost2; + parent->m_size = (parent->m_p1 - parent->m_p0) * dgVector::m_half; + parent->m_origin = (parent->m_p1 + parent->m_p0) * dgVector::m_half; + } + } + } else { + // in the future I can handle this but it is too much work for little payoff + } +} + +dgFloat32 dgAABBPolygonSoup::CalculateFaceMaxSize (const dgVector* const vertex, dgInt32 indexCount, const dgInt32* const indexArray) const +{ + dgFloat32 maxSize = dgFloat32 (0.0f); + dgInt32 index = indexArray[indexCount - 1]; + dgVector p0 (vertex[index]); + for (dgInt32 i = 0; i < indexCount; i ++) { + dgInt32 index1 = indexArray[i]; + dgVector p1 (vertex[index1]); + + dgVector dir (p1 - p0); + dgAssert (dir.m_w == dgFloat32 (0.0f)); + dir = dir.Normalize(); + + dgFloat32 maxVal = dgFloat32 (-1.0e10f); + dgFloat32 minVal = dgFloat32 ( 1.0e10f); + for (dgInt32 j = 0; j < indexCount; j ++) { + dgInt32 index2 = indexArray[j]; + dgVector q (vertex[index2]); + dgFloat32 val = dir.DotProduct(q).GetScalar(); + minVal = dgMin(minVal, val); + maxVal = dgMax(maxVal, val); + } + + dgFloat32 size = maxVal - minVal; + maxSize = dgMax(maxSize, size); + p0 = p1; + } + + return dgFloor (maxSize + dgFloat32 (1.0f)); +} + +void dgAABBPolygonSoup::GetAABB (dgVector& p0, dgVector& p1) const +{ + if (m_aabb) { + GetNodeAABB (m_aabb, p0, p1); + } else { + p0 = dgVector::m_zero; + p1 = dgVector::m_zero; + } +} + +void dgAABBPolygonSoup::CalculateAdjacendy () +{ + dgVector p0; + dgVector p1; + GetAABB (p0, p1); + dgFastAABBInfo box (p0, p1); + ForAllSectors (box, dgVector (dgFloat32 (0.0f)), dgFloat32 (1.0f), CalculateAllFaceEdgeNormals, this); + + dgStack pool ((m_indexCount / 2) - 1); + const dgTriplex* const vertexArray = (dgTriplex*)GetLocalVertexPool(); + dgInt32 normalCount = 0; + for (dgInt32 i = 0; i < m_nodesCount; i ++) { + const dgNode* const node = &m_aabb[i]; + if (node->m_left.IsLeaf()) { + dgInt32 vCount = dgInt32 (node->m_left.GetCount()); + if (vCount) { + dgInt32 index = dgInt32 (node->m_left.GetIndex()); + dgInt32* const face = &m_indices[index]; + + dgInt32 j0 = 2 * (vCount + 1) - 1; + dgVector normal (&vertexArray[face[vCount + 1]].m_x); + normal = normal & dgVector::m_triplexMask; + dgAssert (dgAbs (normal.DotProduct(normal).GetScalar() - dgFloat32 (1.0f)) < dgFloat32 (1.0e-6f)); + dgVector q0 (&vertexArray[face[vCount - 1]].m_x); + q0 = q0 & dgVector::m_triplexMask; + for (dgInt32 j = 0; j < vCount; j ++) { + dgInt32 j1 = vCount + 2 + j; + dgVector q1 (&vertexArray[face[j]].m_x); + q1 = q1 & dgVector::m_triplexMask; + if (face[j0] == -1) { + dgVector e (q1 - q0); + dgVector n (e.CrossProduct(normal)); + n = n.Scale(dgFloat32 (1.0f) / dgSqrt (n.DotProduct(n).GetScalar())); + dgAssert (dgAbs (n.DotProduct(n).GetScalar() - dgFloat32 (1.0f)) < dgFloat32 (1.0e-6f)); + pool[normalCount].m_x = n.m_x; + pool[normalCount].m_y = n.m_y; + pool[normalCount].m_z = n.m_z; + face[j0] = -normalCount - 1; + normalCount ++; + } + q0 = q1; + j0 = j1; + } + } + } + + if (node->m_right.IsLeaf()) { + dgInt32 vCount = dgInt32 (node->m_right.GetCount()); + if (vCount) { + dgInt32 index = dgInt32 (node->m_right.GetIndex()); + dgInt32* const face = &m_indices[index]; + + dgInt32 j0 = 2 * (vCount + 1) - 1; + dgVector normal (&vertexArray[face[vCount + 1]].m_x); + normal = normal & dgVector::m_triplexMask; + dgAssert (dgAbs (normal.DotProduct(normal).GetScalar() - dgFloat32 (1.0f)) < dgFloat32 (1.0e-6f)); + dgVector q0 (&vertexArray[face[vCount - 1]].m_x); + q0 = q0 & dgVector::m_triplexMask; + for (dgInt32 j = 0; j < vCount; j ++) { + dgInt32 j1 = vCount + 2 + j; + dgVector q1 (&vertexArray[face[j]].m_x); + q1 = q1 & dgVector::m_triplexMask; + if (face[j0] == -1) { + dgVector e (q1 - q0); + dgVector n (e.CrossProduct(normal)); + n = n.Scale(dgFloat32 (1.0f) / dgSqrt (n.DotProduct(n).GetScalar())); + dgAssert (dgAbs (n.DotProduct(n).GetScalar() - dgFloat32 (1.0f)) < dgFloat32 (1.0e-6f)); + pool[normalCount].m_x = n.m_x; + pool[normalCount].m_y = n.m_y; + pool[normalCount].m_z = n.m_z; + face[j0] = -normalCount - 1; + normalCount ++; + } + q0 = q1; + j0 = j1; + } + } + } + } + + if (normalCount) { + dgStack indexArray (normalCount); + dgInt32 newNormalCount = dgVertexListToIndexList (&pool[0].m_x, sizeof (dgTriplex), sizeof (dgTriplex), 0, normalCount, &indexArray[0], dgFloat32 (1.0e-6f)); + + dgInt32 oldCount = GetVertexCount(); + dgTriplex* const vertexArray1 = (dgTriplex*) dgMallocStack (sizeof (dgTriplex) * (oldCount + newNormalCount)); + memcpy (vertexArray1, GetLocalVertexPool(), sizeof (dgTriplex) * oldCount); + memcpy (&vertexArray1[oldCount], &pool[0].m_x, sizeof (dgTriplex) * newNormalCount); + dgFreeStack(GetLocalVertexPool()); + + m_localVertex = &vertexArray1[0].m_x; + m_vertexCount = oldCount + newNormalCount; + + for (dgInt32 i = 0; i < m_nodesCount; i ++) { + const dgNode* const node = &m_aabb[i]; + if (node->m_left.IsLeaf()) { + dgInt32 vCount = dgInt32 (node->m_left.GetCount()); + dgInt32 index = dgInt32 (node->m_left.GetIndex()); + dgInt32* const face = &m_indices[index]; + for (dgInt32 j = 0; j < vCount; j ++) { + if (face[vCount + 2 + j] < 0) { + dgInt32 k = -1 - face[vCount + 2 + j]; + face[vCount + 2 + j] = indexArray[k] + oldCount; + } + #ifdef _DEBUG + dgVector normal (&vertexArray1[face[vCount + 2 + j]].m_x); + normal = normal & dgVector::m_triplexMask; + dgAssert (dgAbs (normal.DotProduct(normal).GetScalar() - dgFloat32 (1.0f)) < dgFloat32 (1.0e-6f)); + #endif + } + } + + if (node->m_right.IsLeaf()) { + dgInt32 vCount = dgInt32 (node->m_right.GetCount()); + dgInt32 index = dgInt32 (node->m_right.GetIndex()); + dgInt32* const face = &m_indices[index]; + for (dgInt32 j = 0; j < vCount; j ++) { + if (face[vCount + 2 + j] < 0) { + dgInt32 k = -1 - face[vCount + 2 + j]; + face[vCount + 2 + j] = indexArray[k] + oldCount; + } + + #ifdef _DEBUG + dgVector normal (&vertexArray1[face[vCount + 2 + j]].m_x); + normal = normal & dgVector::m_triplexMask; + dgAssert (dgAbs (normal.DotProduct(normal).GetScalar() - dgFloat32 (1.0f)) < dgFloat32 (1.0e-6f)); + #endif + } + } + } + } +} + +dgIntersectStatus dgAABBPolygonSoup::CalculateAllFaceEdgeNormals (void* const context, const dgFloat32* const polygon, dgInt32 strideInBytes, const dgInt32* const indexArray, dgInt32 indexCount, dgFloat32 hitDistance) +{ + dgInt32 stride = dgInt32 (strideInBytes / sizeof (dgFloat32)); + + AdjacentdFace adjacentFaces; + adjacentFaces.m_count = indexCount; + adjacentFaces.m_index = (dgInt32*) indexArray; + + dgVector n (&polygon[indexArray[indexCount + 1] * stride]); + dgVector p (&polygon[indexArray[0] * stride]); + n = n & dgVector::m_triplexMask; + p = p & dgVector::m_triplexMask; + adjacentFaces.m_normal = dgPlane (n, - n.DotProduct(p).GetScalar()); + + dgAssert (indexCount < dgInt32 (sizeof (adjacentFaces.m_edgeMap) / sizeof (adjacentFaces.m_edgeMap[0]))); + + dgInt32 edgeIndex = indexCount - 1; + dgInt32 i0 = indexArray[indexCount - 1]; + dgVector p0 ( dgFloat32 (1.0e15f), dgFloat32 (1.0e15f), dgFloat32 (1.0e15f), dgFloat32 (0.0f)); + dgVector p1 (-dgFloat32 (1.0e15f), -dgFloat32 (1.0e15f), -dgFloat32 (1.0e15f), dgFloat32 (0.0f)); + for (dgInt32 i = 0; i < indexCount; i ++) { + dgInt32 i1 = indexArray[i]; + dgInt32 index = i1 * stride; + dgVector point (&polygon[index]); + point = point & dgVector::m_triplexMask; + p0 = p0.GetMin(point); + p1 = p1.GetMax(point); + adjacentFaces.m_edgeMap[edgeIndex] = (dgInt64 (i1) << 32) + i0; + edgeIndex = i; + i0 = i1; + } + + dgFloat32 padding = dgFloat32 (1.0f/16.0f); + p0.m_x -= padding; + p0.m_y -= padding; + p0.m_z -= padding; + p1.m_x += padding; + p1.m_y += padding; + p1.m_z += padding; + + dgAABBPolygonSoup* const me = (dgAABBPolygonSoup*) context; + dgFastAABBInfo box (p0, p1); + me->ForAllSectors (box, dgVector (dgFloat32 (0.0f)), dgFloat32 (1.0f), CalculateDisjointedFaceEdgeNormals, &adjacentFaces); + return t_ContinueSearh; +} + + +dgIntersectStatus dgAABBPolygonSoup::CalculateDisjointedFaceEdgeNormals (void* const context, const dgFloat32* const polygon, dgInt32 strideInBytes, const dgInt32* const indexArray, dgInt32 indexCount, dgFloat32 hitDistance) +{ + #define DG_WELDING_TOL (1.0e-2f) + #define DG_WELDING_TOL2 (DG_WELDING_TOL * DG_WELDING_TOL) + + const AdjacentdFace& adjacentFace = *((AdjacentdFace*)context); + + if (adjacentFace.m_index != indexArray) { + dgInt32 adjacentCount = adjacentFace.m_count; + dgInt32 stride = dgInt32 (strideInBytes / sizeof (dgFloat32)); + + dgInt32 j0 = adjacentCount - 1; + dgInt32 indexJ0 = adjacentFace.m_index[adjacentCount - 1]; + for (dgInt32 j = 0; j < adjacentCount; j ++) { + dgInt32 indexJ1 = adjacentFace.m_index[j]; + dgBigVector q0 (dgVector(&polygon[indexJ1 * stride]) & dgVector::m_triplexMask); + dgBigVector q1 (dgVector(&polygon[indexJ0 * stride]) & dgVector::m_triplexMask); + dgBigVector q1q0 (q1 - q0); + dgFloat64 q1q0Mag2 = q1q0.DotProduct(q1q0).GetScalar(); + + dgInt32 indexI0 = indexArray[indexCount - 1]; + for (dgInt32 i = 0; i < indexCount; i ++) { + dgInt32 indexI1 = indexArray[i]; + dgBigVector p0 (dgVector(&polygon[indexI0 * stride]) & dgVector::m_triplexMask); + dgBigVector p1 (dgVector(&polygon[indexI1 * stride]) & dgVector::m_triplexMask); + dgBigVector p1p0 (p1 - p0); + dgFloat64 dot = p1p0.DotProduct(q1q0).GetScalar(); + if (dot > 0.0f) { + dgFloat64 q1p0Mag2 = p1p0.DotProduct(p1p0).GetScalar(); + if ((dot * dot) >= (q1p0Mag2 * q1q0Mag2 * dgFloat64(0.99995f))) { + dgFloat64 x0 = q0.DotProduct(q1q0).GetScalar(); + dgFloat64 x1 = q1.DotProduct(q1q0).GetScalar(); + dgFloat64 y0 = p0.DotProduct(q1q0).GetScalar(); + dgFloat64 y1 = p1.DotProduct(q1q0).GetScalar(); + dgAssert (x1 > x0); + dgAssert (y1 > y0); + if (!((y0 >= x1) || (y1 <= x0))) { + dgFloat64 t = q1q0.DotProduct(p0 - q0).GetScalar() / q1q0Mag2; + dgAssert (q1q0.m_w == dgFloat32 (0.0f)); + dgBigVector q (q0 + q1q0.Scale(t)); + dgBigVector dist (p0 - q); + dgAssert (dist.m_w == dgFloat32 (0.0f)); + dgFloat64 err2 = dist.DotProduct(dist).GetScalar(); + if (err2 < DG_WELDING_TOL2) { + dgFloat32 maxDist = dgFloat32 (0.0f); + for (dgInt32 k = 0; k < indexCount; k ++) { + dgVector r (&polygon[indexArray[k] * stride]); + r = r & dgVector::m_triplexMask; + dgFloat32 dist1 = adjacentFace.m_normal.Evalue(r); + if (dgAbs (dist1) > dgAbs (maxDist)) { + maxDist = dist1; + } + } + + if (adjacentFace.m_index[j0 + adjacentCount + 2] == -1) { + if (maxDist < -dgFloat32 (1.0e-3f)) { + adjacentFace.m_index[j0 + adjacentCount + 2] = indexArray[indexCount + 1]; + } else { + adjacentFace.m_index[j0 + adjacentCount + 2] = adjacentFace.m_index[adjacentCount + 1]; + } + } else { + if (maxDist < -dgFloat32 (1.0e-3f)) { + dgBigVector n0 (adjacentFace.m_normal[0], adjacentFace.m_normal[1], adjacentFace.m_normal[2], dgFloat64(0.0f)); + dgBigVector n1 (dgVector(&polygon[adjacentFace.m_index[j0 + adjacentCount + 2] * stride]) & dgVector::m_triplexMask); + dgBigVector n2 (dgVector(&polygon[indexArray[indexCount + 1] * stride]) & dgVector::m_triplexMask); + + dgBigVector tilt0 (n0.CrossProduct(n1)); + dgBigVector tilt1 (n0.CrossProduct(n2)); + dgFloat64 dist0 (q1q0.DotProduct(tilt0).GetScalar()); + dgFloat64 dist1 (q1q0.DotProduct(tilt1).GetScalar()); + if (dist0 < dist1) { + adjacentFace.m_index[j0 + adjacentCount + 2] = indexArray[indexCount + 1]; + } + } else { + adjacentFace.m_index[j0 + adjacentCount + 2] = adjacentFace.m_index[adjacentCount + 1]; + } + } + break; + } + } + } + } + indexI0 = indexI1; + } + j0 = j; + indexJ0 = indexJ1; + } + } + return t_ContinueSearh; +} + + + +dgAABBPolygonSoup::dgNodeBuilder* dgAABBPolygonSoup::BuildTopDown (dgNodeBuilder* const leafArray, dgInt32 firstBox, dgInt32 lastBox, dgNodeBuilder** const allocator) const +{ + dgAssert (firstBox >= 0); + dgAssert (lastBox >= 0); + + if (lastBox == firstBox) { + return &leafArray[firstBox]; + } else { + dgSpliteInfo info (&leafArray[firstBox], lastBox - firstBox + 1); + + dgNodeBuilder* const parent = new (*allocator) dgNodeBuilder (info.m_p0, info.m_p1); + *allocator = *allocator + 1; + + dgAssert (parent); + parent->m_right = BuildTopDown (leafArray, firstBox + info.m_axis, lastBox, allocator); + parent->m_right->m_parent = parent; + + parent->m_left = BuildTopDown (leafArray, firstBox, firstBox + info.m_axis - 1, allocator); + parent->m_left->m_parent = parent; + return parent; + } +} + +void dgAABBPolygonSoup::Create (const dgPolygonSoupDatabaseBuilder& builder, bool optimizedBuild) +{ + if (builder.m_faceCount == 0) { + return; + } + dgAssert (builder.m_faceCount >= 1); + m_strideInBytes = sizeof (dgTriplex); + m_nodesCount = ((builder.m_faceCount - 1) < 1) ? 1 : builder.m_faceCount - 1; + m_aabb = (dgNode*) dgMallocStack (sizeof (dgNode) * m_nodesCount); + m_indexCount = builder.m_indexCount * 2 + builder.m_faceCount; + if (builder.m_faceCount == 1) { + m_indexCount *= 2; + } + m_indices = (dgInt32*) dgMallocStack (sizeof (dgInt32) * m_indexCount); + dgStack tmpVertexArrayCont(builder.m_vertexCount + builder.m_normalCount + builder.m_faceCount * 2 + 4); + + dgVector* const tmpVertexArray = &tmpVertexArrayCont[0]; + for (dgInt32 i = 0; i < builder.m_vertexCount; i ++) { + tmpVertexArray[i] = builder.m_vertexPoints[i]; + } + + for (dgInt32 i = 0; i < builder.m_normalCount; i ++) { + tmpVertexArray[i + builder.m_vertexCount] = builder.m_normalPoints[i]; + } + + const dgInt32* const indices = &builder.m_vertexIndex[0]; + dgStack constructor (builder.m_faceCount * 2 + 16); + + dgInt32 polygonIndex = 0; + dgInt32 allocatorIndex = 0; + if (builder.m_faceCount == 1) { + dgInt32 indexCount = builder.m_faceVertexCount[0] - 1; + new (&constructor[allocatorIndex]) dgNodeBuilder (&tmpVertexArray[0], 0, indexCount, &indices[0]); + allocatorIndex ++; + } + for (dgInt32 i = 0; i < builder.m_faceCount; i ++) { + dgInt32 indexCount = builder.m_faceVertexCount[i] - 1; + new (&constructor[allocatorIndex]) dgNodeBuilder (&tmpVertexArray[0], i, indexCount, &indices[polygonIndex]); + allocatorIndex ++; + polygonIndex += (indexCount + 1); + } + + dgNodeBuilder* contructorAllocator = &constructor[allocatorIndex]; + dgNodeBuilder* root = BuildTopDown (&constructor[0], 0, allocatorIndex - 1, &contructorAllocator); + + dgAssert (root); + if (root->m_left) { + + dgAssert (root->m_right); + dgList list (builder.m_allocator); + dgList stack (builder.m_allocator); + stack.Append(root); + while (stack.GetCount()) { + dgList::dgListNode* const stackNode = stack.GetLast(); + dgNodeBuilder* const node = stackNode->GetInfo(); + stack.Remove(stackNode); + + if (node->m_left) { + dgAssert (node->m_right); + list.Append(node); + stack.Append(node->m_right); + stack.Append(node->m_left); + } + } + + dgFloat64 newCost = dgFloat32 (1.0e20f); + dgFloat64 prevCost = newCost; + do { + prevCost = newCost; + for (dgList::dgListNode* listNode = list.GetFirst(); listNode; listNode = listNode->GetNext()) { + dgNodeBuilder* const node = listNode->GetInfo(); + ImproveNodeFitness (node); + } + + newCost = dgFloat32 (0.0f); + for (dgList::dgListNode* listNode = list.GetFirst(); listNode; listNode = listNode->GetNext()) { + dgNodeBuilder* const node = listNode->GetInfo(); + newCost += node->m_area; + } + } while (newCost < (prevCost * dgFloat32 (0.9999f))); + + root = list.GetLast()->GetInfo(); + while (root->m_parent) { + root = root->m_parent; + } + } + + dgList list (builder.m_allocator); + + list.Append(root); + dgInt32 nodeIndex = 0; + while (list.GetCount()) { + dgNodeBuilder* const node = list.GetFirst()->GetInfo(); + list.Remove(list.GetFirst()); + + if (node->m_left) { + node->m_enumeration = nodeIndex; + nodeIndex ++; + dgAssert (node->m_right); + list.Append(node->m_left); + list.Append(node->m_right); + } + } + dgAssert(!list.GetCount()); + + dgInt32 aabbBase = builder.m_vertexCount + builder.m_normalCount; + + + dgVector* const aabbPoints = &tmpVertexArray[aabbBase]; + + + dgInt32 vertexIndex = 0; + dgInt32 aabbNodeIndex = 0; + list.Append(root); + dgInt32 indexMap = 0; + while (list.GetCount()) { + + dgNodeBuilder* const node = list.GetFirst()->GetInfo(); + list.Remove(list.GetFirst()); + + if (node->m_enumeration >= 0) { + dgAssert (node->m_left); + dgAssert (node->m_right); + dgNode& aabbNode = m_aabb[aabbNodeIndex]; + aabbNodeIndex ++; + dgAssert (aabbNodeIndex <= m_nodesCount); + + if (node->m_parent) { + if (node->m_parent->m_left == node) { + m_aabb[node->m_parent->m_enumeration].m_left = dgNode::dgLeafNodePtr (dgUnsigned32 (&m_aabb[node->m_enumeration] - m_aabb)); + } else { + dgAssert (node->m_parent->m_right == node); + m_aabb[node->m_parent->m_enumeration].m_right = dgNode::dgLeafNodePtr (dgUnsigned32 (&m_aabb[node->m_enumeration] - m_aabb)); + } + } + + aabbPoints[vertexIndex + 0] = node->m_p0; + aabbPoints[vertexIndex + 1] = node->m_p1; + + aabbNode.m_indexBox0 = aabbBase + vertexIndex; + aabbNode.m_indexBox1 = aabbBase + vertexIndex + 1; + + vertexIndex += 2; + + } else { + dgAssert (!node->m_left); + dgAssert (!node->m_right); + + if (node->m_parent) { + if (node->m_parent->m_left == node) { + m_aabb[node->m_parent->m_enumeration].m_left = dgNode::dgLeafNodePtr (dgUnsigned32(node->m_indexCount), dgUnsigned32(indexMap)); + } else { + dgAssert (node->m_parent->m_right == node); + m_aabb[node->m_parent->m_enumeration].m_right = dgNode::dgLeafNodePtr (dgUnsigned32(node->m_indexCount), dgUnsigned32(indexMap)); + } + } + + // index format i0, i1, i2, ... , id, normal, e0Normal, e1Normal, e2Normal, ..., faceSize + for (dgInt32 j = 0; j < node->m_indexCount; j ++) { + m_indices[indexMap + j] = node->m_faceIndices[j]; + m_indices[indexMap + j + node->m_indexCount + 2] = -1; + } + + // face attribute + m_indices[indexMap + node->m_indexCount] = node->m_faceIndices[node->m_indexCount]; + // face normal + m_indices[indexMap + node->m_indexCount + 1] = builder.m_vertexCount + builder.m_normalIndex[node->m_faceIndex]; + // face size + m_indices[indexMap + node->m_indexCount * 2 + 2] = dgInt32 (CalculateFaceMaxSize (&tmpVertexArray[0], node->m_indexCount, node->m_faceIndices)); + + indexMap += node->m_indexCount * 2 + 3; + } + + if (node->m_left) { + dgAssert (node->m_right); + list.Append(node->m_left); + list.Append(node->m_right); + } + } + + dgStack indexArray (vertexIndex); + dgInt32 aabbPointCount = dgVertexListToIndexList (&aabbPoints[0].m_x, sizeof (dgVector), sizeof (dgTriplex), 0, vertexIndex, &indexArray[0], dgFloat32 (1.0e-6f)); + + m_vertexCount = aabbBase + aabbPointCount; + m_localVertex = (dgFloat32*) dgMallocStack (sizeof (dgTriplex) * m_vertexCount); + + dgTriplex* const dstPoints = (dgTriplex*)m_localVertex; + for (dgInt32 i = 0; i < m_vertexCount; i ++) { + dstPoints[i].m_x = tmpVertexArray[i].m_x; + dstPoints[i].m_y = tmpVertexArray[i].m_y; + dstPoints[i].m_z = tmpVertexArray[i].m_z; + } + + for (dgInt32 i = 0; i < m_nodesCount; i ++) { + dgNode& box = m_aabb[i]; + + dgInt32 j = box.m_indexBox0 - aabbBase; + box.m_indexBox0 = indexArray[j] + aabbBase; + + j = box.m_indexBox1 - aabbBase; + box.m_indexBox1 = indexArray[j] + aabbBase; + } + + if (builder.m_faceCount == 1) { + m_aabb[0].m_right = dgNode::dgLeafNodePtr (0, 0); + } +// CalculateAdjacendy(); +} + +void dgAABBPolygonSoup::Serialize (dgSerialize callback, void* const userData) const +{ + callback (userData, &m_vertexCount, sizeof (dgInt32)); + callback (userData, &m_indexCount, sizeof (dgInt32)); + callback (userData, &m_nodesCount, sizeof (dgInt32)); + callback (userData, &m_nodesCount, sizeof (dgInt32)); + if (m_aabb) { + callback (userData, m_localVertex, dgInt32 (sizeof (dgTriplex) * m_vertexCount)); + callback (userData, m_indices, dgInt32 (sizeof (dgInt32) * m_indexCount)); + callback (userData, m_aabb, dgInt32 (sizeof (dgNode) * m_nodesCount)); + } +} + +void dgAABBPolygonSoup::Deserialize (dgDeserialize callback, void* const userData, dgInt32 revisionNumber) +{ + m_strideInBytes = sizeof (dgTriplex); + callback (userData, &m_vertexCount, sizeof (dgInt32)); + callback (userData, &m_indexCount, sizeof (dgInt32)); + callback (userData, &m_nodesCount, sizeof (dgInt32)); + callback (userData, &m_nodesCount, sizeof (dgInt32)); + + if (m_vertexCount) { + m_localVertex = (dgFloat32*) dgMallocStack (sizeof (dgTriplex) * m_vertexCount); + m_indices = (dgInt32*) dgMallocStack (sizeof (dgInt32) * m_indexCount); + m_aabb = (dgNode*) dgMallocStack (sizeof (dgNode) * m_nodesCount); + + callback (userData, m_localVertex, dgInt32 (sizeof (dgTriplex) * m_vertexCount)); + callback (userData, m_indices, dgInt32 (sizeof (dgInt32) * m_indexCount)); + callback (userData, m_aabb, dgInt32 (sizeof (dgNode) * m_nodesCount)); + } else { + m_localVertex = NULL; + m_indices = NULL; + m_aabb = NULL; + } +} + + +dgVector dgAABBPolygonSoup::ForAllSectorsSupportVectex (const dgVector& dir) const +{ + dgVector supportVertex (dgFloat32 (0.0f)); + if (m_aabb) { + dgFloat32 aabbProjection[DG_STACK_DEPTH]; + const dgNode *stackPool[DG_STACK_DEPTH]; + + dgInt32 stack = 1; + stackPool[0] = m_aabb; + aabbProjection[0] = dgFloat32 (1.0e10f); + const dgTriplex* const boxArray = (dgTriplex*)m_localVertex; + + + dgFloat32 maxProj = dgFloat32 (-1.0e20f); + dgInt32 ix = (dir[0] > dgFloat32 (0.0f)) ? 1 : 0; + dgInt32 iy = (dir[1] > dgFloat32 (0.0f)) ? 1 : 0; + dgInt32 iz = (dir[2] > dgFloat32 (0.0f)) ? 1 : 0; + + while (stack) { + dgFloat32 boxSupportValue; + + stack--; + boxSupportValue = aabbProjection[stack]; + if (boxSupportValue > maxProj) { + dgFloat32 backSupportDist = dgFloat32 (0.0f); + dgFloat32 frontSupportDist = dgFloat32 (0.0f); + const dgNode* const me = stackPool[stack]; + if (me->m_left.IsLeaf()) { + backSupportDist = dgFloat32 (-1.0e20f); + dgInt32 index = dgInt32 (me->m_left.GetIndex()); + dgInt32 vCount = dgInt32 (me->m_left.GetCount()); + dgVector vertex (dgFloat32 (0.0f)); + for (dgInt32 j = 0; j < vCount; j ++) { + dgInt32 i0 = m_indices[index + j] * dgInt32 (sizeof (dgTriplex) / sizeof (dgFloat32)); + dgVector p (&boxArray[i0].m_x); + p = p & dgVector::m_triplexMask; + dgFloat32 dist = p.DotProduct(dir).GetScalar(); + if (dist > backSupportDist) { + backSupportDist = dist; + vertex = p; + } + } + + if (backSupportDist > maxProj) { + maxProj = backSupportDist; + supportVertex = vertex; + } + + } else { + dgVector box[2]; + const dgNode* const node = me->m_left.GetNode(m_aabb); + box[0].m_x = boxArray[node->m_indexBox0].m_x; + box[0].m_y = boxArray[node->m_indexBox0].m_y; + box[0].m_z = boxArray[node->m_indexBox0].m_z; + box[1].m_x = boxArray[node->m_indexBox1].m_x; + box[1].m_y = boxArray[node->m_indexBox1].m_y; + box[1].m_z = boxArray[node->m_indexBox1].m_z; + + dgVector supportPoint (box[ix].m_x, box[iy].m_y, box[iz].m_z, dgFloat32 (0.0)); + backSupportDist = supportPoint.DotProduct(dir).GetScalar(); + } + + if (me->m_right.IsLeaf()) { + frontSupportDist = dgFloat32 (-1.0e20f); + dgInt32 index = dgInt32 (me->m_right.GetIndex()); + dgInt32 vCount = dgInt32 (me->m_right.GetCount()); + dgVector vertex (dgFloat32 (0.0f)); + for (dgInt32 j = 0; j < vCount; j ++) { + dgInt32 i0 = m_indices[index + j] * dgInt32 (sizeof (dgTriplex) / sizeof (dgFloat32)); + dgVector p (&boxArray[i0].m_x); + p = p & dgVector::m_triplexMask; + dgFloat32 dist = p.DotProduct(dir).GetScalar(); + if (dist > frontSupportDist) { + frontSupportDist = dist; + vertex = p; + } + } + if (frontSupportDist > maxProj) { + maxProj = frontSupportDist; + supportVertex = vertex; + } + + } else { + dgVector box[2]; + const dgNode* const node = me->m_right.GetNode(m_aabb); + box[0].m_x = boxArray[node->m_indexBox0].m_x; + box[0].m_y = boxArray[node->m_indexBox0].m_y; + box[0].m_z = boxArray[node->m_indexBox0].m_z; + box[1].m_x = boxArray[node->m_indexBox1].m_x; + box[1].m_y = boxArray[node->m_indexBox1].m_y; + box[1].m_z = boxArray[node->m_indexBox1].m_z; + + dgVector supportPoint (box[ix].m_x, box[iy].m_y, box[iz].m_z, dgFloat32 (0.0f)); + frontSupportDist = supportPoint.DotProduct(dir).GetScalar(); + } + + if (frontSupportDist >= backSupportDist) { + if (!me->m_left.IsLeaf()) { + aabbProjection[stack] = backSupportDist; + stackPool[stack] = me->m_left.GetNode(m_aabb); + stack++; + } + + if (!me->m_right.IsLeaf()) { + aabbProjection[stack] = frontSupportDist; + stackPool[stack] = me->m_right.GetNode(m_aabb); + stack++; + } + + } else { + + if (!me->m_right.IsLeaf()) { + aabbProjection[stack] = frontSupportDist; + stackPool[stack] = me->m_right.GetNode(m_aabb); + stack++; + } + + if (!me->m_left.IsLeaf()) { + aabbProjection[stack] = backSupportDist; + stackPool[stack] = me->m_left.GetNode(m_aabb); + stack++; + } + } + } + } + } + return supportVertex; +} + + +void dgAABBPolygonSoup::ForAllSectorsRayHit (const dgFastRayTest& raySrc, dgFloat32 maxParam, dgRayIntersectCallback callback, void* const context) const +{ + const dgNode *stackPool[DG_STACK_DEPTH]; + dgFloat32 distance[DG_STACK_DEPTH]; + dgFastRayTest ray (raySrc); + + dgInt32 stack = 1; + const dgTriplex* const vertexArray = (dgTriplex*) m_localVertex; + + stackPool[0] = m_aabb; + distance[0] = m_aabb->RayDistance(ray, vertexArray); + while (stack) { + stack --; + dgFloat32 dist = distance[stack]; + if (dist > maxParam) { + break; + } else { + const dgNode *const me = stackPool[stack]; + if (me->m_left.IsLeaf()) { + dgInt32 vCount = dgInt32 (me->m_left.GetCount()); + if (vCount > 0) { + dgInt32 index = dgInt32 (me->m_left.GetIndex()); + dgFloat32 param = callback(context, &vertexArray[0].m_x, sizeof (dgTriplex), &m_indices[index], vCount); + dgAssert (param >= dgFloat32 (0.0f)); + if (param < maxParam) { + maxParam = param; + if (maxParam == dgFloat32 (0.0f)) { + break; + } + } + } + + } else { + const dgNode* const node = me->m_left.GetNode(m_aabb); + dgFloat32 dist1 = node->RayDistance(ray, vertexArray); + if (dist1 < maxParam) { + dgInt32 j = stack; + for ( ; j && (dist1 > distance[j - 1]); j --) { + stackPool[j] = stackPool[j - 1]; + distance[j] = distance[j - 1]; + } + dgAssert (stack < DG_STACK_DEPTH); + stackPool[j] = node; + distance[j] = dist1; + stack++; + } + } + + if (me->m_right.IsLeaf()) { + dgInt32 vCount = dgInt32 (me->m_right.GetCount()); + if (vCount > 0) { + dgInt32 index = dgInt32 (me->m_right.GetIndex()); + dgFloat32 param = callback(context, &vertexArray[0].m_x, sizeof (dgTriplex), &m_indices[index], vCount); + dgAssert (param >= dgFloat32 (0.0f)); + if (param < maxParam) { + maxParam = param; + if (maxParam == dgFloat32 (0.0f)) { + break; + } + } + } + + } else { + const dgNode* const node = me->m_right.GetNode(m_aabb); + dgFloat32 dist1 = node->RayDistance(ray, vertexArray); + if (dist1 < maxParam) { + dgInt32 j = stack; + for ( ; j && (dist1 > distance[j - 1]); j --) { + stackPool[j] = stackPool[j - 1]; + distance[j] = distance[j - 1]; + } + dgAssert (stack < DG_STACK_DEPTH); + stackPool[j] = node; + distance[j] = dist1; + stack++; + } + } + } + } +} + +void dgAABBPolygonSoup::ForAllSectors (const dgFastAABBInfo& obbAabbInfo, const dgVector& boxDistanceTravel, dgFloat32 m_maxT, dgAABBIntersectCallback callback, void* const context) const +{ + dgAssert (dgAbs(dgAbs(obbAabbInfo[0][0]) - obbAabbInfo.m_absDir[0][0]) < dgFloat32 (1.0e-4f)); + dgAssert (dgAbs(dgAbs(obbAabbInfo[1][1]) - obbAabbInfo.m_absDir[1][1]) < dgFloat32 (1.0e-4f)); + dgAssert (dgAbs(dgAbs(obbAabbInfo[2][2]) - obbAabbInfo.m_absDir[2][2]) < dgFloat32 (1.0e-4f)); + + dgAssert (dgAbs(dgAbs(obbAabbInfo[0][1]) - obbAabbInfo.m_absDir[1][0]) < dgFloat32 (1.0e-4f)); + dgAssert (dgAbs(dgAbs(obbAabbInfo[0][2]) - obbAabbInfo.m_absDir[2][0]) < dgFloat32 (1.0e-4f)); + dgAssert (dgAbs(dgAbs(obbAabbInfo[1][2]) - obbAabbInfo.m_absDir[2][1]) < dgFloat32 (1.0e-4f)); + + if (m_aabb) { + dgFloat32 distance[DG_STACK_DEPTH]; + const dgNode* stackPool[DG_STACK_DEPTH]; + + const dgInt32 stride = sizeof (dgTriplex) / sizeof (dgFloat32); + const dgTriplex* const vertexArray = (dgTriplex*) m_localVertex; + + dgAssert (boxDistanceTravel.m_w == dgFloat32 (0.0f)); + if (boxDistanceTravel.DotProduct(boxDistanceTravel).GetScalar() < dgFloat32 (1.0e-8f)) { + + dgInt32 stack = 1; + stackPool[0] = m_aabb; + distance[0] = m_aabb->BoxPenetration(obbAabbInfo, vertexArray); + if (distance[0] <= dgFloat32(0.0f)) { + obbAabbInfo.m_separationDistance = dgMin(obbAabbInfo.m_separationDistance[0], -distance[0]); + } + while (stack) { + stack --; + dgFloat32 dist = distance[stack]; + if (dist > dgFloat32 (0.0f)) { + const dgNode* const me = stackPool[stack]; + if (me->m_left.IsLeaf()) { + dgInt32 index = dgInt32 (me->m_left.GetIndex()); + dgInt32 vCount = dgInt32 (me->m_left.GetCount()); + if (vCount > 0) { + const dgInt32* const indices = &m_indices[index]; + dgInt32 normalIndex = indices[vCount + 1]; + dgVector faceNormal (&vertexArray[normalIndex].m_x); + faceNormal = faceNormal & dgVector::m_triplexMask; + dgFloat32 dist1 = obbAabbInfo.PolygonBoxDistance (faceNormal, vCount, indices, stride, &vertexArray[0].m_x); + if (dist1 > dgFloat32 (0.0f)) { + obbAabbInfo.m_separationDistance = dgFloat32(0.0f); + dgAssert (vCount >= 3); + if (callback(context, &vertexArray[0].m_x, sizeof (dgTriplex), indices, vCount, dist1) == t_StopSearh) { + return; + } + } else { + obbAabbInfo.m_separationDistance = dgMin(obbAabbInfo.m_separationDistance[0], -dist1); + } + } + + } else { + const dgNode* const node = me->m_left.GetNode(m_aabb); + dgFloat32 dist1 = node->BoxPenetration(obbAabbInfo, vertexArray); + if (dist1 > dgFloat32 (0.0f)) { + dgInt32 j = stack; + for ( ; j && (dist1 > distance[j - 1]); j --) { + stackPool[j] = stackPool[j - 1]; + distance[j] = distance[j - 1]; + } + dgAssert (stack < DG_STACK_DEPTH); + stackPool[j] = node; + distance[j] = dist1; + stack++; + } else { + obbAabbInfo.m_separationDistance = dgMin(obbAabbInfo.m_separationDistance[0], -dist1); + } + } + + if (me->m_right.IsLeaf()) { + dgInt32 index = dgInt32 (me->m_right.GetIndex()); + dgInt32 vCount = dgInt32 (me->m_right.GetCount()); + if (vCount > 0) { + const dgInt32* const indices = &m_indices[index]; + dgInt32 normalIndex = indices[vCount + 1]; + dgVector faceNormal (&vertexArray[normalIndex].m_x); + faceNormal = faceNormal & dgVector::m_triplexMask; + dgFloat32 dist1 = obbAabbInfo.PolygonBoxDistance (faceNormal, vCount, indices, stride, &vertexArray[0].m_x); + if (dist1 > dgFloat32 (0.0f)) { + dgAssert (vCount >= 3); + obbAabbInfo.m_separationDistance = dgFloat32(0.0f); + if (callback(context, &vertexArray[0].m_x, sizeof (dgTriplex), indices, vCount, dist1) == t_StopSearh) { + return; + } + } else { + obbAabbInfo.m_separationDistance = dgMin(obbAabbInfo.m_separationDistance[0], -dist1); + } + } + + } else { + const dgNode* const node = me->m_right.GetNode(m_aabb); + dgFloat32 dist1 = node->BoxPenetration(obbAabbInfo, vertexArray); + if (dist1 > dgFloat32 (0.0f)) { + dgInt32 j = stack; + for ( ; j && (dist1 > distance[j - 1]); j --) { + stackPool[j] = stackPool[j - 1]; + distance[j] = distance[j - 1]; + } + dgAssert (stack < DG_STACK_DEPTH); + stackPool[j] = node; + distance[j] = dist1; + stack++; + } else { + obbAabbInfo.m_separationDistance = dgMin(obbAabbInfo.m_separationDistance[0], -dist1); + } + } + } + } + + } else { + dgFastRayTest ray (dgVector (dgFloat32 (0.0f)), boxDistanceTravel); + dgFastRayTest obbRay (dgVector (dgFloat32 (0.0f)), obbAabbInfo.UnrotateVector(boxDistanceTravel)); + dgInt32 stack = 1; + stackPool[0] = m_aabb; + distance [0] = m_aabb->BoxIntersect (ray, obbRay, obbAabbInfo, vertexArray); + + while (stack) { + stack --; + const dgFloat32 dist = distance[stack]; + const dgNode* const me = stackPool[stack]; + if (dist < dgFloat32 (1.0f)) { + + if (me->m_left.IsLeaf()) { + dgInt32 index = dgInt32 (me->m_left.GetIndex()); + dgInt32 vCount = dgInt32 (me->m_left.GetCount()); + if (vCount > 0) { + const dgInt32* const indices = &m_indices[index]; + dgInt32 normalIndex = indices[vCount + 1]; + dgVector faceNormal (&vertexArray[normalIndex].m_x); + faceNormal = faceNormal & dgVector::m_triplexMask; + dgFloat32 hitDistance = obbAabbInfo.PolygonBoxRayDistance (faceNormal, vCount, indices, stride, &vertexArray[0].m_x, ray); + if (hitDistance < dgFloat32 (1.0f)) { + dgAssert (vCount >= 3); + if (callback(context, &vertexArray[0].m_x, sizeof (dgTriplex), indices, vCount, hitDistance) == t_StopSearh) { + return; + } + } + } + + } else { + const dgNode* const node = me->m_left.GetNode(m_aabb); + dgFloat32 dist1 = node->BoxIntersect (ray, obbRay, obbAabbInfo, vertexArray); + if (dist1 < dgFloat32 (1.0f)) { + dgInt32 j = stack; + for ( ; j && (dist1 > distance[j - 1]); j --) { + stackPool[j] = stackPool[j - 1]; + distance[j] = distance[j - 1]; + } + dgAssert (stack < DG_STACK_DEPTH); + stackPool[j] = node; + distance[j] = dist1; + stack++; + } + } + + if (me->m_right.IsLeaf()) { + dgInt32 index = dgInt32 (me->m_right.GetIndex()); + dgInt32 vCount = dgInt32 (me->m_right.GetCount()); + if (vCount > 0) { + const dgInt32* const indices = &m_indices[index]; + dgInt32 normalIndex = indices[vCount + 1]; + dgVector faceNormal (&vertexArray[normalIndex].m_x); + faceNormal = faceNormal & dgVector::m_triplexMask; + dgFloat32 hitDistance = obbAabbInfo.PolygonBoxRayDistance (faceNormal, vCount, indices, stride, &vertexArray[0].m_x, ray); + if (hitDistance < dgFloat32 (1.0f)) { + dgAssert (vCount >= 3); + if (callback(context, &vertexArray[0].m_x, sizeof (dgTriplex), indices, vCount, hitDistance) == t_StopSearh) { + return; + } + } + } + + } else { + const dgNode* const node = me->m_right.GetNode(m_aabb); + dgFloat32 dist1 = node->BoxIntersect (ray, obbRay, obbAabbInfo, vertexArray); + if (dist1 < dgFloat32 (1.0f)) { + dgInt32 j = stack; + for ( ; j && (dist1 > distance[j - 1]); j --) { + stackPool[j] = stackPool[j - 1]; + distance[j] = distance[j - 1]; + } + dgAssert (stack < DG_STACK_DEPTH); + stackPool[j] = node; + distance[j] = dist1; + stack ++; + } + } + } + } + } + } +} + + diff --git a/thirdparty/src/newton/dgCore/dgAABBPolygonSoup.h b/thirdparty/src/newton/dgCore/dgAABBPolygonSoup.h new file mode 100644 index 000000000..19beaa062 --- /dev/null +++ b/thirdparty/src/newton/dgCore/dgAABBPolygonSoup.h @@ -0,0 +1,251 @@ +/* Copyright (c) <2003-2019> +* +* 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<= 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 + + diff --git a/thirdparty/src/newton/dgCore/dgArray.h b/thirdparty/src/newton/dgCore/dgArray.h new file mode 100644 index 000000000..14160c091 --- /dev/null +++ b/thirdparty/src/newton/dgCore/dgArray.h @@ -0,0 +1,227 @@ +/* Copyright (c) <2003-2019> +* +* 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 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 +dgArray::dgArray() + :m_array(NULL) + ,m_maxSize(0) + ,m_aligmentInBytes(DG_MEMORY_GRANULARITY) + ,m_allocator(NULL) +{ + m_aligmentInBytes = 1 << dgExp2(m_aligmentInBytes); +} + +template +dgArray::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 +dgArray::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 +dgArray::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::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 +dgArray::~dgArray () +{ + if (m_array) { + m_allocator->FreeLow (m_array); + } +} + +template +DG_INLINE const T& dgArray::operator[] (dgInt32 i) const +{ + dgAssert (i >= 0); + while (i >= m_maxSize) { + Resize (i * 2); + } + return m_array[i]; +} + +template +DG_INLINE T& dgArray::operator[] (dgInt32 i) +{ + dgAssert (i >= 0); + while (i >= m_maxSize) { + Resize (i * 2); + } + return m_array[i]; +} + +template +dgInt32 dgArray::GetElementSize() const +{ + return sizeof (T); +} + +template +dgInt32 dgArray::GetElementsCapacity () const +{ + return m_maxSize; +} + +template +dgInt32 dgArray::GetBytesCapacity () const +{ + return m_maxSize * GetElementSize(); +} + +template +void dgArray::Clear () const +{ + if (m_array) { + m_allocator->FreeLow (m_array); + m_array = NULL; + } + m_maxSize = 0; +} + +template +dgMemoryAllocator* dgArray::GetAllocator() const +{ + return m_allocator; +} + +template +void dgArray::SetAllocator(dgMemoryAllocator* const allocator) +{ + dgAssert (!m_allocator); + m_allocator = allocator; +} + +template +void dgArray::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 +DG_INLINE void dgArray::ResizeIfNecessary (dgInt32 size) const +{ + while (size >= m_maxSize) { + Resize (m_maxSize * 2); + } +} + +#endif + + + + diff --git a/thirdparty/src/newton/dgCore/dgCRC.cpp b/thirdparty/src/newton/dgCore/dgCRC.cpp new file mode 100644 index 000000000..a508acb00 --- /dev/null +++ b/thirdparty/src/newton/dgCore/dgCRC.cpp @@ -0,0 +1,390 @@ +/* Copyright (c) <2003-2019> +* +* 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, public dgList + { + + 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(NULL), dgList() + { + AddContainer(); + } + + ~CRCStringLookup () + { + } + + + void AddString (dgUnsigned32 crc, const char *string) + { + dgInt32 length; + + if (dgTree::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::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); +} diff --git a/thirdparty/src/newton/dgCore/dgCRC.h b/thirdparty/src/newton/dgCore/dgCRC.h new file mode 100644 index 000000000..2b6a4b409 --- /dev/null +++ b/thirdparty/src/newton/dgCore/dgCRC.h @@ -0,0 +1,35 @@ +/* Copyright (c) <2003-2019> +* +* 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 + diff --git a/thirdparty/src/newton/dgCore/dgConvexHull3d.cpp b/thirdparty/src/newton/dgCore/dgConvexHull3d.cpp new file mode 100644 index 000000000..33b5b6323 --- /dev/null +++ b/thirdparty/src/newton/dgCore/dgConvexHull3d.cpp @@ -0,0 +1,1100 @@ +/* Copyright (c) <2003-2019> +* +* 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 "dgTree.h" +#include "dgStack.h" +#include "dgGoogol.h" +#include "dgConvexHull3d.h" +#include "dgSmallDeterminant.h" + + +#define DG_CONVEXHULL_3D_VERTEX_CLUSTER_SIZE 8 + +#ifdef DG_OLD_CONVEXHULL_3D +class dgConvexHull3d::dgNormalMap +{ + public: + dgNormalMap() + :m_count(sizeof(m_normal) / sizeof(m_normal[0])) + { + dgVector p0(dgFloat32(1.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f)); + dgVector p1(dgFloat32(-1.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f)); + dgVector p2(dgFloat32(0.0f), dgFloat32(1.0f), dgFloat32(0.0f), dgFloat32(0.0f)); + dgVector p3(dgFloat32(0.0f), dgFloat32(-1.0f), dgFloat32(0.0f), dgFloat32(0.0f)); + dgVector p4(dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(1.0f), dgFloat32(0.0f)); + dgVector p5(dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(-1.0f), dgFloat32(0.0f)); + + dgInt32 count = 0; + dgInt32 subdivitions = 2; + TessellateTriangle(subdivitions, p4, p0, p2, count); + TessellateTriangle(subdivitions, p0, p5, p2, count); + TessellateTriangle(subdivitions, p5, p1, p2, count); + TessellateTriangle(subdivitions, p1, p4, p2, count); + TessellateTriangle(subdivitions, p0, p4, p3, count); + TessellateTriangle(subdivitions, p5, p0, p3, count); + TessellateTriangle(subdivitions, p1, p5, p3, count); + TessellateTriangle(subdivitions, p4, p1, p3, count); + } + + static const dgNormalMap& GetNormaMap() + { + static dgNormalMap normalMap; + return normalMap; + } + + void TessellateTriangle(dgInt32 level, const dgVector& p0, const dgVector& p1, const dgVector& p2, dgInt32& count) + { + if (level) { + dgAssert(dgAbs(p0.DotProduct(p0).GetScalar() - dgFloat32(1.0f)) < dgFloat32(1.0e-4f)); + dgAssert(dgAbs(p1.DotProduct(p1).GetScalar() - dgFloat32(1.0f)) < dgFloat32(1.0e-4f)); + dgAssert(dgAbs(p2.DotProduct(p2).GetScalar() - dgFloat32(1.0f)) < dgFloat32(1.0e-4f)); + dgVector p01(p0 + p1); + dgVector p12(p1 + p2); + dgVector p20(p2 + p0); + + p01 = p01.Scale(dgRsqrt(p01.DotProduct(p01).GetScalar())); + p12 = p12.Scale(dgRsqrt(p12.DotProduct(p12).GetScalar())); + p20 = p20.Scale(dgRsqrt(p20.DotProduct(p20).GetScalar())); + + dgAssert(dgAbs(p01.DotProduct(p01).GetScalar() - dgFloat32(1.0f)) < dgFloat32(1.0e-4f)); + dgAssert(dgAbs(p12.DotProduct(p12).GetScalar() - dgFloat32(1.0f)) < dgFloat32(1.0e-4f)); + dgAssert(dgAbs(p20.DotProduct(p20).GetScalar() - dgFloat32(1.0f)) < dgFloat32(1.0e-4f)); + + TessellateTriangle(level - 1, p0, p01, p20, count); + TessellateTriangle(level - 1, p1, p12, p01, count); + TessellateTriangle(level - 1, p2, p20, p12, count); + TessellateTriangle(level - 1, p01, p12, p20, count); + } else { + dgBigPlane n(p0, p1, p2); + n = n.Scale(dgFloat64(1.0f) / sqrt(n.DotProduct(n).GetScalar())); + n.m_w = dgFloat64(0.0f); + dgInt32 index = dgBitReversal(count, sizeof(m_normal) / sizeof(m_normal[0])); + m_normal[index] = n; + count++; + dgAssert(count <= sizeof(m_normal) / sizeof(m_normal[0])); + } + } + + dgBigVector m_normal[128]; + dgInt32 m_count; +}; +#endif + +class dgConvexHull3DVertex: public dgBigVector +{ + public: + dgInt32 m_mark; +}; + +class dgConvexHull3dAABBTreeNode +{ + public: + dgBigVector m_box[2]; + dgConvexHull3dAABBTreeNode* m_left; + dgConvexHull3dAABBTreeNode* m_right; + dgConvexHull3dAABBTreeNode* m_parent; +}; + +class dgConvexHull3dPointCluster: public dgConvexHull3dAABBTreeNode +{ + public: + dgInt32 m_count; + dgInt32 m_indices[DG_CONVEXHULL_3D_VERTEX_CLUSTER_SIZE]; +}; + + +dgConvexHull3DFace::dgConvexHull3DFace() +{ + m_mark = 0; + m_twin[0] = NULL; + m_twin[1] = NULL; + m_twin[2] = NULL; +} + +dgFloat64 dgConvexHull3DFace::Evalue (const dgBigVector* const pointArray, const dgBigVector& point) const +{ + const dgBigVector& p0 = pointArray[m_index[0]]; + const dgBigVector& p1 = pointArray[m_index[1]]; + const dgBigVector& p2 = pointArray[m_index[2]]; + + dgFloat64 matrix[3][3]; + for (dgInt32 i = 0; i < 3; i ++) { + matrix[0][i] = p2[i] - p0[i]; + matrix[1][i] = p1[i] - p0[i]; + matrix[2][i] = point[i] - p0[i]; + } + + dgFloat64 error; + dgFloat64 det = Determinant3x3 (matrix, &error); + + // the code use double, however the threshold for accuracy test is the machine precision of a float. + // by changing this to a smaller number, the code should run faster since many small test will be considered valid + // the precision must be a power of two no smaller than the machine precision of a double, (1<<48) + // float64(1<<30) can be a good value + + // dgFloat64 precision = dgFloat64 (1.0f) / dgFloat64 (1<<30); + dgFloat64 precision = dgFloat64 (1.0f) / dgFloat64 (1<<24); + dgFloat64 errbound = error * precision; + if (fabs(det) > errbound) { + return det; + } + + dgGoogol exactMatrix[3][3]; + for (dgInt32 i = 0; i < 3; i ++) { + exactMatrix[0][i] = dgGoogol(p2[i]) - dgGoogol(p0[i]); + exactMatrix[1][i] = dgGoogol(p1[i]) - dgGoogol(p0[i]); + exactMatrix[2][i] = dgGoogol(point[i]) - dgGoogol(p0[i]); + } + return Determinant3x3(exactMatrix); +} + +dgBigPlane dgConvexHull3DFace::GetPlaneEquation (const dgBigVector* const pointArray) const +{ + const dgBigVector& p0 = pointArray[m_index[0]]; + const dgBigVector& p1 = pointArray[m_index[1]]; + const dgBigVector& p2 = pointArray[m_index[2]]; + dgBigPlane plane (p0, p1, p2); + plane = plane.Scale (1.0f / sqrt (plane.DotProduct(plane & dgBigVector::m_triplexMask).GetScalar())); + return plane; +} + + +dgConvexHull3d::dgConvexHull3d (dgMemoryAllocator* const allocator) + :dgList(allocator) + ,m_count (0) + ,m_diag() + ,m_aabbP0(dgBigVector (dgFloat64 (0.0f))) + ,m_aabbP1(dgBigVector (dgFloat64 (0.0f))) + ,m_points(allocator) +{ +} + +dgConvexHull3d::dgConvexHull3d(const dgConvexHull3d& source) + :dgList(source.GetAllocator()) + ,m_count (source.m_count) + ,m_diag(source.m_diag) + ,m_aabbP0 (source.m_aabbP0) + ,m_aabbP1 (source.m_aabbP1) + ,m_points(source.GetAllocator(), source.m_count) +{ + m_points[m_count-1].m_w = dgFloat64 (0.0f); + for (int i = 0; i < m_count; i ++) { + m_points[i] = source.m_points[i]; + } + dgTree map (GetAllocator()); + for(dgListNode* sourceNode = source.GetFirst(); sourceNode; sourceNode = sourceNode->GetNext() ) { + dgListNode* const node = Append(); + map.Insert(node, sourceNode); + } + + for(dgListNode* sourceNode = source.GetFirst(); sourceNode; sourceNode = sourceNode->GetNext() ) { + dgListNode* const node = map.Find(sourceNode)->GetInfo(); + + dgConvexHull3DFace& face = node->GetInfo(); + dgConvexHull3DFace& srcFace = sourceNode->GetInfo(); + + face.m_mark = 0; + for (dgInt32 i = 0; i < 3; i ++) { + face.m_index[i] = srcFace.m_index[i]; + face.m_twin[i] = map.Find (srcFace.m_twin[i])->GetInfo(); + } + } +} + +dgConvexHull3d::dgConvexHull3d(dgMemoryAllocator* const allocator, const dgFloat64* const vertexCloud, dgInt32 strideInBytes, dgInt32 count, dgFloat64 distTol, dgInt32 maxVertexCount) + :dgList(allocator) + ,m_count (0) + ,m_diag() + ,m_aabbP0 (dgBigVector (dgFloat64 (0.0), dgFloat64 (0.0), dgFloat64 (0.0), dgFloat64 (0.0))) + ,m_aabbP1 (dgBigVector (dgFloat64 (0.0), dgFloat64 (0.0), dgFloat64 (0.0), dgFloat64 (0.0))) + ,m_points(allocator) +{ + BuildHull (vertexCloud, strideInBytes, count, distTol, maxVertexCount); +} + +dgConvexHull3d::~dgConvexHull3d(void) +{ +} + + +void dgConvexHull3d::BuildHull (const dgFloat64* const vertexCloud, dgInt32 strideInBytes, dgInt32 count, dgFloat64 distTol, dgInt32 maxVertexCount) +{ + dgSetPrecisionDouble precision; + + dgInt32 treeCount = count / (DG_CONVEXHULL_3D_VERTEX_CLUSTER_SIZE>>1); + if (treeCount < 4) { + treeCount = 4; + } + treeCount *= 2; + + dgStack points (count); + dgStack treePool (treeCount + 256); + count = InitVertexArray(&points[0], vertexCloud, strideInBytes, count, &treePool[0], treePool.GetSizeInBytes()); + +#ifdef DG_OLD_CONVEXHULL_3D + if (m_count >= 4) { + CalculateConvexHull3d (&treePool[0], &points[0], count, distTol, maxVertexCount); + } +#else + if (m_count >= 3) { + if (CheckFlatSurface(&treePool[0], &points[0], count, distTol, maxVertexCount)) { + CalculateConvexHull2d(&treePool[0], &points[0], count, distTol, maxVertexCount); + } else { + dgAssert(m_count == 4); + CalculateConvexHull3d(&treePool[0], &points[0], count, distTol, maxVertexCount); + } + } +#endif +} + +dgInt32 dgConvexHull3d::ConvexCompareVertex(const dgConvexHull3DVertex* const A, const dgConvexHull3DVertex* const B, void* const context) +{ + for (dgInt32 i = 0; i < 3; i ++) { + if ((*A)[i] < (*B)[i]) { + return -1; + } else if ((*A)[i] > (*B)[i]) { + return 1; + } + } + return 0; +} + +dgConvexHull3dAABBTreeNode* dgConvexHull3d::BuildTree (dgConvexHull3dAABBTreeNode* const parent, dgConvexHull3DVertex* const points, dgInt32 count, dgInt32 baseIndex, dgInt8** memoryPool, dgInt32& maxMemSize) const +{ + dgConvexHull3dAABBTreeNode* tree = NULL; + + dgAssert (count); + dgBigVector minP ( dgFloat32 (1.0e15f)); + dgBigVector maxP (-dgFloat32 (1.0e15f)); + if (count <= DG_CONVEXHULL_3D_VERTEX_CLUSTER_SIZE) { + + dgConvexHull3dPointCluster* const clump = new (*memoryPool) dgConvexHull3dPointCluster; + *memoryPool += sizeof (dgConvexHull3dPointCluster); + maxMemSize -= sizeof (dgConvexHull3dPointCluster); + dgAssert (maxMemSize >= 0); + + dgAssert (clump); + clump->m_count = count; + for (dgInt32 i = 0; i < count; i ++) { + clump->m_indices[i] = i + baseIndex; + + const dgBigVector& p = points[i]; + dgAssert(p.m_w == dgFloat32(0.0f)); + minP = minP.GetMin(p); + maxP = maxP.GetMax(p); + } + + clump->m_left = NULL; + clump->m_right = NULL; + tree = clump; + + } else { + dgBigVector median (dgFloat32 (0.0f)); + dgBigVector varian (dgFloat32 (0.0f)); + for (dgInt32 i = 0; i < count; i ++) { + + const dgBigVector& p = points[i]; + dgAssert(p.m_w == dgFloat32(0.0f)); + minP = minP.GetMin(p); + maxP = maxP.GetMax(p); + median += p; + varian += p * p; + } + + varian = varian.Scale(dgFloat32(count)) - median * median; + dgInt32 index = 0; + dgFloat64 maxVarian = dgFloat64 (-1.0e10f); + for (dgInt32 i = 0; i < 3; i ++) { + if (varian[i] > maxVarian) { + index = i; + maxVarian = varian[i]; + } + } + dgBigVector center (median.Scale (dgFloat64 (1.0f) / dgFloat64 (count))); + + dgFloat64 test = center[index]; + + dgInt32 i0 = 0; + dgInt32 i1 = count - 1; + do { + for (; i0 <= i1; i0 ++) { + dgFloat64 val = points[i0][index]; + if (val > test) { + break; + } + } + + for (; i1 >= i0; i1 --) { + dgFloat64 val = points[i1][index]; + if (val < test) { + break; + } + } + + if (i0 < i1) { + dgSwap(points[i0], points[i1]); + i0++; + i1--; + } + } while (i0 <= i1); + + if (i0 == 0){ + i0 = count / 2; + } + if (i0 >= (count - 1)){ + i0 = count / 2; + } + + tree = new (*memoryPool) dgConvexHull3dAABBTreeNode; + *memoryPool += sizeof (dgConvexHull3dAABBTreeNode); + maxMemSize -= sizeof (dgConvexHull3dAABBTreeNode); + dgAssert (maxMemSize >= 0); + + dgAssert (i0); + dgAssert (count - i0); + + tree->m_left = BuildTree (tree, points, i0, baseIndex, memoryPool, maxMemSize); + tree->m_right = BuildTree (tree, &points[i0], count - i0, i0 + baseIndex, memoryPool, maxMemSize); + } + + dgAssert (tree); + tree->m_parent = parent; + tree->m_box[0] = (minP - dgBigVector (dgFloat64 (1.0e-3f))) & dgBigVector::m_triplexMask; + tree->m_box[1] = (maxP + dgBigVector (dgFloat64 (1.0e-3f))) & dgBigVector::m_triplexMask; + return tree; +} + +dgInt32 dgConvexHull3d::GetUniquePoints(dgConvexHull3DVertex* const points, const dgFloat64* const vertexCloud, dgInt32 strideInBytes, dgInt32 count, void* const memoryPool, dgInt32 maxMemSize) +{ + dgInt32 stride = dgInt32(strideInBytes / sizeof(dgFloat64)); + if (stride >= 4) { + for (dgInt32 i = 0; i < count; i++) { + dgInt32 index = i * stride; + dgBigVector& vertex = points[i]; + vertex = dgBigVector(vertexCloud[index], vertexCloud[index + 1], vertexCloud[index + 2], vertexCloud[index + 3]); + dgAssert(dgCheckVector(vertex)); + points[i].m_mark = 0; + } + } else { + for (dgInt32 i = 0; i < count; i++) { + dgInt32 index = i * stride; + dgBigVector& vertex = points[i]; + vertex = dgBigVector(vertexCloud[index], vertexCloud[index + 1], vertexCloud[index + 2], dgFloat64(0.0f)); + dgAssert(dgCheckVector(vertex)); + points[i].m_mark = 0; + } + } + + dgSort(points, count, ConvexCompareVertex); + + dgInt32 indexCount = 0; + for (int i = 1; i < count; i++) { + for (; i < count; i++) { + if (ConvexCompareVertex(&points[indexCount], &points[i], NULL)) { + indexCount++; + points[indexCount] = points[i]; + break; + } + } + } + count = indexCount + 1; + return count; +} + +dgInt32 dgConvexHull3d::InitVertexArray(dgConvexHull3DVertex* const points, const dgFloat64* const vertexCloud, dgInt32 strideInBytes, dgInt32 count, void* const memoryPool, dgInt32 maxMemSize) +{ + count = GetUniquePoints(points, vertexCloud, strideInBytes, count, memoryPool, maxMemSize); + if (count < 4) { + m_count = 0; + return count; + } + dgConvexHull3dAABBTreeNode* tree = BuildTree (NULL, points, count, 0, (dgInt8**) &memoryPool, maxMemSize); + + m_aabbP0 = tree->m_box[0]; + m_aabbP1 = tree->m_box[1]; + + dgBigVector boxSize (tree->m_box[1] - tree->m_box[0]); + dgAssert (boxSize.m_w == dgFloat32 (0.0f)); + m_diag = dgFloat32 (sqrt (boxSize.DotProduct(boxSize).GetScalar())); + +#ifdef DG_OLD_CONVEXHULL_3D + const dgNormalMap& normalMap = dgNormalMap::GetNormaMap(); + + dgInt32 index0 = SupportVertex (&tree, points, normalMap.m_normal[0]); + m_points[0] = points[index0]; + points[index0].m_mark = 1; + + bool validTetrahedrum = false; + dgBigVector e1 (dgFloat64 (0.0f), dgFloat64 (0.0f), dgFloat64 (0.0f), dgFloat64 (0.0f)) ; + for (dgInt32 i = 1; i < normalMap.m_count; i ++) { + dgInt32 index = SupportVertex (&tree, points, normalMap.m_normal[i]); + dgAssert (index >= 0); + + e1 = points[index] - m_points[0]; + dgAssert (e1.m_w == dgFloat32 (0.0f)); + dgFloat64 error2 = e1.DotProduct(e1).GetScalar(); + if (error2 > (dgFloat32 (1.0e-4f) * m_diag * m_diag)) { + m_points[1] = points[index]; + points[index].m_mark = 1; + validTetrahedrum = true; + break; + } + } + if (!validTetrahedrum) { + m_count = 0; + dgAssert (0); + return count; + } + + validTetrahedrum = false; + dgBigVector e2(dgFloat32 (0.0f)); + dgBigVector normal (dgFloat32 (0.0f)); + for (dgInt32 i = 2; i < normalMap.m_count; i ++) { + dgInt32 index = SupportVertex (&tree, points, normalMap.m_normal[i]); + dgAssert (index >= 0); + e2 = points[index] - m_points[0]; + normal = e1.CrossProduct(e2); + dgAssert (e2.m_w == dgFloat32 (0.0f)); + dgAssert (normal.m_w == dgFloat32 (0.0f)); + dgFloat64 error2 = sqrt (normal.DotProduct(normal).GetScalar()); + if (error2 > (dgFloat32 (1.0e-4f) * m_diag * m_diag)) { + m_points[2] = points[index]; + points[index].m_mark = 1; + validTetrahedrum = true; + break; + } + } + + dgAssert(normal.m_w == dgFloat32(0.0f)); + if (!validTetrahedrum) { + m_count = 0; + dgAssert (0); + return count; + } + + // find the largest possible tetrahedron + validTetrahedrum = false; + dgBigVector e3(dgFloat32 (0.0f)); + + index0 = SupportVertex (&tree, points, normal); + e3 = points[index0] - m_points[0]; + dgAssert (e3.m_w == dgFloat32 (0.0f)); + dgFloat64 err2 = normal.DotProduct(e3).GetScalar(); + if (fabs (err2) > (dgFloat64 (1.0e-6f) * m_diag * m_diag)) { + // we found a valid tetrahedral, about and start build the hull by adding the rest of the points + m_points[3] = points[index0]; + points[index0].m_mark = 1; + validTetrahedrum = true; + } + if (!validTetrahedrum) { + dgVector n (normal.Scale(dgFloat64 (-1.0f))); + dgInt32 index = SupportVertex (&tree, points, n); + e3 = points[index] - m_points[0]; + dgAssert (e3.m_w == dgFloat32 (0.0f)); + dgFloat64 error2 = normal.DotProduct(e3).GetScalar(); + if (fabs (error2) > (dgFloat64 (1.0e-6f) * m_diag * m_diag)) { + // we found a valid tetrahedral, about and start build the hull by adding the rest of the points + m_points[3] = points[index]; + points[index].m_mark = 1; + validTetrahedrum = true; + } + } + if (!validTetrahedrum) { + for (dgInt32 i = 3; i < normalMap.m_count; i ++) { + dgInt32 index = SupportVertex (&tree, points, normalMap.m_normal[i]); + dgAssert (index >= 0); + + //make sure the volume of the fist tetrahedral is no negative + e3 = points[index] - m_points[0]; + dgAssert (e3.m_w == dgFloat32 (0.0f)); + dgFloat64 error2 = normal.DotProduct(e3).GetScalar(); + if (fabs (error2) > (dgFloat64 (1.0e-6f) * m_diag * m_diag)) { + // we found a valid tetrahedral, about and start build the hull by adding the rest of the points + m_points[3] = points[index]; + points[index].m_mark = 1; + validTetrahedrum = true; + break; + } + } + } + if (!validTetrahedrum) { + // the points do not form a convex hull + m_count = 0; + //dgAssert (0); + return count; + } + + + m_count = 4; + dgFloat64 volume = TetrahedrumVolume (m_points[0], m_points[1], m_points[2], m_points[3]); + if (volume > dgFloat64 (0.0f)) { + dgSwap(m_points[2], m_points[3]); + } + dgAssert (TetrahedrumVolume(m_points[0], m_points[1], m_points[2], m_points[3]) < dgFloat64(0.0f)); + + return count; +#else + + dgBigVector origin((m_aabbP1 + m_aabbP0).Scale (0.5f)); + + dgBigVector dir(m_aabbP1 - m_aabbP0); + dgAssert(dir.DotProduct3(dir) > dgFloat32(1.0e-4f)); + dir = dir.Normalize(); + dgInt32 index0 = SupportVertex(&tree, points, dir); + m_points[0] = points[index0]; + points[index0].m_mark = 1; + + dir = origin - m_points[0]; + dgAssert(dir.DotProduct3(dir) > dgFloat32(1.0e-4f)); + dir = dir.Normalize(); + dgInt32 index1 = SupportVertex(&tree, points, dir); + m_points[1] = points[index1]; + points[index1].m_mark = 1; + + dgBigVector e0(m_points[1] - m_points[0]); + dgAssert(e0.DotProduct3(e0) > dgFloat32(1.0e-4f)); + dgFloat64 t = -e0.DotProduct3(origin - m_points[0]) / e0.DotProduct3(e0); + dir = m_points[0] + e0.Scale(t) - origin; + + dgAssert(dir.DotProduct3(dir) > dgFloat32(1.0e-4f)); + dir = dir.Normalize(); + dgInt32 index2 = SupportVertex(&tree, points, dir); + m_points[2] = points[index2]; + points[index2].m_mark = 1; + + dgBigVector e1 (m_points[2] - m_points[0]); + dgBigVector normal (e1.CrossProduct(e0)); + dgFloat64 error2 = sqrt(normal.DotProduct3(normal)); + if (error2 < (dgFloat32(1.0e-4f) * m_diag * m_diag)) { + dgAssert(0); +// m_points[2] = points[index]; +// points[index].m_mark = 1; +// validTetrahedrum = true; +// break; + } + + m_count = 3; + return count; +#endif +} + +dgFloat64 dgConvexHull3d::TetrahedrumVolume (const dgBigVector& p0, const dgBigVector& p1, const dgBigVector& p2, const dgBigVector& p3) const +{ + dgBigVector p1p0 (p1 - p0); + dgBigVector p2p0 (p2 - p0); + dgBigVector p3p0 (p3 - p0); + dgAssert (p1p0.m_w == dgFloat32 (0.0f)); + dgAssert (p2p0.m_w == dgFloat32 (0.0f)); + dgAssert (p3p0.m_w == dgFloat32 (0.0f)); + return p3p0.DotProduct(p1p0.CrossProduct(p2p0)).GetScalar(); +} + +dgInt32 dgConvexHull3d::SupportVertex (dgConvexHull3dAABBTreeNode** const treePointer, const dgConvexHull3DVertex* const points, const dgBigVector& dirPlane, const bool removeEntry) const +{ + #define DG_STACK_DEPTH_3D 64 + dgFloat64 aabbProjection[DG_STACK_DEPTH_3D]; + const dgConvexHull3dAABBTreeNode *stackPool[DG_STACK_DEPTH_3D]; + + dgBigVector dir(dirPlane & dgBigPlane::m_triplexMask); + dgAssert (dir.m_w == dgFloat32 (0.0f)); + + dgInt32 index = -1; + dgInt32 stack = 1; + stackPool[0] = *treePointer; + aabbProjection[0] = dgFloat32 (1.0e20f); + dgFloat64 maxProj = dgFloat64 (-1.0e20f); + dgInt32 ix = (dir[0] > dgFloat64 (0.0f)) ? 1 : 0; + dgInt32 iy = (dir[1] > dgFloat64 (0.0f)) ? 1 : 0; + dgInt32 iz = (dir[2] > dgFloat64 (0.0f)) ? 1 : 0; + while (stack) { + stack--; + dgFloat64 boxSupportValue = aabbProjection[stack]; + if (boxSupportValue > maxProj) { + const dgConvexHull3dAABBTreeNode* const me = stackPool[stack]; + + if (me->m_left && me->m_right) { + dgBigVector leftSupportPoint (me->m_left->m_box[ix].m_x, me->m_left->m_box[iy].m_y, me->m_left->m_box[iz].m_z, dgFloat32 (0.0f)); + dgFloat64 leftSupportDist = leftSupportPoint.DotProduct(dir).GetScalar(); + + dgBigVector rightSupportPoint (me->m_right->m_box[ix].m_x, me->m_right->m_box[iy].m_y, me->m_right->m_box[iz].m_z, dgFloat32 (0.0f)); + dgFloat64 rightSupportDist = rightSupportPoint.DotProduct(dir).GetScalar(); + + if (rightSupportDist >= leftSupportDist) { + aabbProjection[stack] = leftSupportDist; + stackPool[stack] = me->m_left; + stack++; + dgAssert (stack < DG_STACK_DEPTH_3D); + aabbProjection[stack] = rightSupportDist; + stackPool[stack] = me->m_right; + stack++; + dgAssert (stack < DG_STACK_DEPTH_3D); + } else { + aabbProjection[stack] = rightSupportDist; + stackPool[stack] = me->m_right; + stack++; + dgAssert (stack < DG_STACK_DEPTH_3D); + aabbProjection[stack] = leftSupportDist; + stackPool[stack] = me->m_left; + stack++; + dgAssert (stack < DG_STACK_DEPTH_3D); + } + + } else { + dgConvexHull3dPointCluster* const cluster = (dgConvexHull3dPointCluster*) me; + for (dgInt32 i = 0; i < cluster->m_count; i ++) { + const dgConvexHull3DVertex& p = points[cluster->m_indices[i]]; + dgAssert (p.m_x >= cluster->m_box[0].m_x); + dgAssert (p.m_x <= cluster->m_box[1].m_x); + dgAssert (p.m_y >= cluster->m_box[0].m_y); + dgAssert (p.m_y <= cluster->m_box[1].m_y); + dgAssert (p.m_z >= cluster->m_box[0].m_z); + dgAssert (p.m_z <= cluster->m_box[1].m_z); + if (!p.m_mark) { + dgAssert (p.m_w == dgFloat32 (0.0f)); + dgFloat64 dist = p.DotProduct(dir).GetScalar(); + if (dist > maxProj) { + maxProj = dist; + index = cluster->m_indices[i]; + } + } else if (removeEntry) { + cluster->m_indices[i] = cluster->m_indices[cluster->m_count - 1]; + cluster->m_count = cluster->m_count - 1; + i --; + } + } + + if (cluster->m_count == 0) { + dgConvexHull3dAABBTreeNode* const parent = cluster->m_parent; + if (parent) { + dgConvexHull3dAABBTreeNode* const sibling = (parent->m_left != cluster) ? parent->m_left : parent->m_right; + dgAssert (sibling != cluster); + dgConvexHull3dAABBTreeNode* const grandParent = parent->m_parent; + if (grandParent) { + sibling->m_parent = grandParent; + if (grandParent->m_right == parent) { + grandParent->m_right = sibling; + } else { + grandParent->m_left = sibling; + } + } else { + sibling->m_parent = NULL; + *treePointer = sibling; + } + } + } + } + } + } + + dgAssert (index != -1); + return index; +} + +dgConvexHull3d::dgListNode* dgConvexHull3d::AddFace (dgInt32 i0, dgInt32 i1, dgInt32 i2) +{ + dgListNode* const node = Append(); + dgConvexHull3DFace& face = node->GetInfo(); + + face.m_index[0] = i0; + face.m_index[1] = i1; + face.m_index[2] = i2; + return node; +} + +void dgConvexHull3d::DeleteFace (dgListNode* const node) +{ + Remove (node); +} + +bool dgConvexHull3d::Sanity() const +{ +/* + for (dgListNode* node = GetFirst(); node; node = node->GetNext()) { + dgConvexHull3DFace* const face = &node->GetInfo(); + for (dgInt32 i = 0; i < 3; i ++) { + dgListNode* const twinNode = face->m_twin[i]; + if (!twinNode) { + return false; + } + + dgInt32 count = 0; + dgListNode* me = NULL; + dgConvexHull3DFace* const twinFace = &twinNode->GetInfo(); + for (dgInt32 j = 0; j < 3; j ++) { + if (twinFace->m_twin[j] == node) { + count ++; + me = twinFace->m_twin[j]; + } + } + if (count != 1) { + return false; + } + if (me != node) { + return false; + } + } + } +*/ + return true; +} + +bool dgConvexHull3d::CheckFlatSurface(dgConvexHull3dAABBTreeNode* tree, dgConvexHull3DVertex* const points, dgInt32 count, dgFloat64 distTol, dgInt32 maxVertexCount) +{ + dgBigVector e0(m_points[1] - m_points[0]); + dgBigVector e1(m_points[2] - m_points[0]); + dgAssert(e0.m_w == dgFloat32(0.0f)); + dgAssert(e1.m_w == dgFloat32(0.0f)); + dgAssert(e0.DotProduct(e0).GetScalar() > dgFloat32(1.0e-4f)); + dgAssert(e1.DotProduct(e1).GetScalar() > dgFloat32(1.0e-4f)); + dgBigVector normal(e1.CrossProduct(e0)); + dgAssert(normal.m_w == dgFloat32(0.0f)); + dgAssert(normal.DotProduct(normal).GetScalar() > dgFloat32(1.0e-6f)); + normal = normal.Normalize(); + + dgInt32 index = SupportVertex(&tree, points, normal); + m_points[3] = points[index]; + + dgFloat64 volume = TetrahedrumVolume(m_points[0], m_points[1], m_points[2], m_points[3]); + if (dgAbs(volume) < dgFloat32(1.0e-9f)) { + normal = normal.Scale(dgFloat32(-1.0f)); + index = SupportVertex(&tree, points, normal); + m_points[3] = points[index]; + volume = TetrahedrumVolume(m_points[0], m_points[1], m_points[2], m_points[3]); + if (dgAbs(volume) < dgFloat32(1.0e-9f)) { + return true; + } + } + points[index].m_mark = 1; + if (volume > dgFloat64(0.0f)) { + dgSwap(m_points[2], m_points[3]); + } + dgAssert(TetrahedrumVolume(m_points[0], m_points[1], m_points[2], m_points[3]) < dgFloat64(0.0f)); + m_count = 4; + return false; +} + + +void dgConvexHull3d::CalculateConvexHull2d(dgConvexHull3dAABBTreeNode* tree, dgConvexHull3DVertex* const points, dgInt32 count, dgFloat64 distTol, dgInt32 maxVertexCount) +{ + +} + +void dgConvexHull3d::CalculateConvexHull3d (dgConvexHull3dAABBTreeNode* vertexTree, dgConvexHull3DVertex* const points, dgInt32 count, dgFloat64 distTol, dgInt32 maxVertexCount) +{ + distTol = dgAbs (distTol) * m_diag; + dgListNode* const f0Node = AddFace (0, 1, 2); + dgListNode* const f1Node = AddFace (0, 2, 3); + dgListNode* const f2Node = AddFace (2, 1, 3); + dgListNode* const f3Node = AddFace (1, 0, 3); + + dgConvexHull3DFace* const f0 = &f0Node->GetInfo(); + dgConvexHull3DFace* const f1 = &f1Node->GetInfo(); + dgConvexHull3DFace* const f2 = &f2Node->GetInfo(); + dgConvexHull3DFace* const f3 = &f3Node->GetInfo(); + + f0->m_twin[0] = (dgList::dgListNode*)f3Node; + f0->m_twin[1] = (dgList::dgListNode*)f2Node; + f0->m_twin[2] = (dgList::dgListNode*)f1Node; + + f1->m_twin[0] = (dgList::dgListNode*)f0Node; + f1->m_twin[1] = (dgList::dgListNode*)f2Node; + f1->m_twin[2] = (dgList::dgListNode*)f3Node; + + f2->m_twin[0] = (dgList::dgListNode*)f0Node; + f2->m_twin[1] = (dgList::dgListNode*)f3Node; + f2->m_twin[2] = (dgList::dgListNode*)f1Node; + + f3->m_twin[0] = (dgList::dgListNode*)f0Node; + f3->m_twin[1] = (dgList::dgListNode*)f1Node; + f3->m_twin[2] = (dgList::dgListNode*)f2Node; + + dgList boundaryFaces(GetAllocator()); + + boundaryFaces.Append(f0Node); + boundaryFaces.Append(f1Node); + boundaryFaces.Append(f2Node); + boundaryFaces.Append(f3Node); + count -= 4; + maxVertexCount -= 4; + dgInt32 currentIndex = 4; + + + dgStack stackPool(1024 + m_count); + dgStack coneListPool(1024 + m_count); + dgStack deleteListPool(1024 + m_count); + + dgListNode** const stack = &stackPool[0]; + dgListNode** const coneList = &stackPool[0]; + dgListNode** const deleteList = &deleteListPool[0]; + + while (boundaryFaces.GetCount() && count && (maxVertexCount > 0)) { + // my definition of the optimal convex hull of a given vertex count, + // is the convex hull formed by a subset of the input vertex that minimizes the volume difference + // between the perfect hull formed from all input vertex and the hull of the sub set of vertex. + // When using a priority heap this algorithms will generate the an optimal of a fix vertex count. + // Since all Newton's tools do not have a limit on the point count of a convex hull, I can use either a stack or a queue. + // a stack maximize construction speed, a Queue tend to maximize the volume of the generated Hull approaching a perfect Hull. + // For now we use a queue. + // For general hulls it does not make a difference if we use a stack, queue, or a priority heap. + // perfect optimal hull only apply for when build hull of a limited vertex count. + // + // Also when building Hulls of a limited vertex count, this function runs in constant time. + // yes that is correct, it does not makes a difference if you build a N point hull from 100 vertex + // or from 100000 vertex input array. + + #if 0 + // using stack (faster) + dgListNode* const faceNode = boundaryFaces.GetFirst()->GetInfo(); + #else + // using a queue (some what slower by better hull when reduced vertex count is desired) + dgListNode* const faceNode = boundaryFaces.GetLast()->GetInfo(); + #endif + + dgConvexHull3DFace* const face = &faceNode->GetInfo(); + dgBigPlane planeEquation (face->GetPlaneEquation (&m_points[0])); + + dgInt32 index = SupportVertex (&vertexTree, points, planeEquation); + const dgBigVector& p = points[index]; + dgFloat64 dist = planeEquation.Evalue(p); + + if ((dist >= distTol) && (face->Evalue(&m_points[0], p) > dgFloat64(0.0f))) { + dgAssert (Sanity()); + + dgAssert (faceNode); + stack[0] = faceNode; + + dgInt32 stackIndex = 1; + dgInt32 deletedCount = 0; + + while (stackIndex) { + stackIndex --; + dgListNode* const node1 = stack[stackIndex]; + dgConvexHull3DFace* const face1 = &node1->GetInfo(); + + if (!face1->m_mark && (face1->Evalue(&m_points[0], p) > dgFloat64(0.0f))) { + #ifdef _DEBUG + for (dgInt32 i = 0; i < deletedCount; i ++) { + dgAssert (deleteList[i] != node1); + } + #endif + + deleteList[deletedCount] = node1; + deletedCount ++; + dgAssert (deletedCount < dgInt32 (deleteListPool.GetElementsCount())); + face1->m_mark = 1; + for (dgInt32 i = 0; i < 3; i ++) { + dgListNode* const twinNode = (dgListNode*)face1->m_twin[i]; + dgAssert (twinNode); + dgConvexHull3DFace* const twinFace = &twinNode->GetInfo(); + if (!twinFace->m_mark) { + stack[stackIndex] = twinNode; + stackIndex ++; + dgAssert (stackIndex < dgInt32 (stackPool.GetElementsCount())); + } + } + } + } + + m_points[currentIndex] = points[index]; + points[index].m_mark = 1; + + dgInt32 newCount = 0; + for (dgInt32 i = 0; i < deletedCount; i ++) { + dgListNode* const node1 = deleteList[i]; + dgConvexHull3DFace* const face1 = &node1->GetInfo(); + dgAssert (face1->m_mark == 1); + for (dgInt32 j0 = 0; j0 < 3; j0 ++) { + dgListNode* const twinNode = face1->m_twin[j0]; + dgConvexHull3DFace* const twinFace = &twinNode->GetInfo(); + if (!twinFace->m_mark) { + dgInt32 j1 = (j0 == 2) ? 0 : j0 + 1; + dgListNode* const newNode = AddFace (currentIndex, face1->m_index[j0], face1->m_index[j1]); + boundaryFaces.Addtop(newNode); + + dgConvexHull3DFace* const newFace = &newNode->GetInfo(); + newFace->m_twin[1] = twinNode; + for (dgInt32 k = 0; k < 3; k ++) { + if (twinFace->m_twin[k] == node1) { + twinFace->m_twin[k] = newNode; + } + } + coneList[newCount] = newNode; + newCount ++; + dgAssert (newCount < dgInt32 (coneListPool.GetElementsCount())); + } + } + } + + for (dgInt32 i = 0; i < newCount - 1; i ++) { + dgListNode* const nodeA = coneList[i]; + dgConvexHull3DFace* const faceA = &nodeA->GetInfo(); + dgAssert (faceA->m_mark == 0); + for (dgInt32 j = i + 1; j < newCount; j ++) { + dgListNode* const nodeB = coneList[j]; + dgConvexHull3DFace* const faceB = &nodeB->GetInfo(); + dgAssert (faceB->m_mark == 0); + if (faceA->m_index[2] == faceB->m_index[1]) { + faceA->m_twin[2] = nodeB; + faceB->m_twin[0] = nodeA; + break; + } + } + + for (dgInt32 j = i + 1; j < newCount; j ++) { + dgListNode* const nodeB = coneList[j]; + dgConvexHull3DFace* const faceB = &nodeB->GetInfo(); + dgAssert (faceB->m_mark == 0); + if (faceA->m_index[1] == faceB->m_index[2]) { + faceA->m_twin[0] = nodeB; + faceB->m_twin[2] = nodeA; + break; + } + } + } + + for (dgInt32 i = 0; i < deletedCount; i ++) { + dgListNode* const node = deleteList[i]; + boundaryFaces.Remove (node); + DeleteFace (node); + } + + maxVertexCount --; + currentIndex ++; + count --; + } else { + boundaryFaces.Remove (faceNode); + } + } + m_count = currentIndex; +} + + +void dgConvexHull3d::CalculateVolumeAndSurfaceArea (dgFloat64& volume, dgFloat64& surfaceArea) const +{ + dgFloat64 areaAcc = dgFloat32 (0.0f); + dgFloat64 volumeAcc = dgFloat32 (0.0f); + for (dgListNode* node = GetFirst(); node; node = node->GetNext()) { + const dgConvexHull3DFace* const face = &node->GetInfo(); + dgInt32 i0 = face->m_index[0]; + dgInt32 i1 = face->m_index[1]; + dgInt32 i2 = face->m_index[2]; + const dgBigVector& p0 = m_points[i0]; + const dgBigVector& p1 = m_points[i1]; + const dgBigVector& p2 = m_points[i2]; + dgAssert(p0.m_w == dgFloat32(0.0f)); + dgAssert(p1.m_w == dgFloat32(0.0f)); + dgAssert(p2.m_w == dgFloat32(0.0f)); + dgBigVector normal ((p1 - p0).CrossProduct(p2 - p0)); + dgAssert(normal.m_w == dgFloat32(0.0f)); + dgFloat64 area = sqrt (normal.DotProduct(normal).GetScalar()); + areaAcc += area; + volumeAcc += p2.DotProduct(p0.CrossProduct(p1)).GetScalar(); + } + dgAssert (volumeAcc >= dgFloat64 (0.0f)); + volume = volumeAcc * dgFloat64 (1.0f/6.0f); + surfaceArea = areaAcc * dgFloat64 (0.5f); +} + +// this code has linear time complexity on the number of faces +dgFloat64 dgConvexHull3d::RayCast (const dgBigVector& localP0, const dgBigVector& localP1) const +{ + dgFloat64 interset = dgFloat32 (1.2f); + dgFloat64 tE = dgFloat64 (0.0f); // for the maximum entering segment parameter; + dgFloat64 tL = dgFloat64 (1.0f); // for the minimum leaving segment parameter; + dgBigVector dS (localP1 - localP0); // is the segment direction vector; + + dgAssert(dS.m_w == dgFloat32(0.0f)); + dgInt32 hasHit = 0; + + for (dgListNode* node = GetFirst(); node; node = node->GetNext()) { + const dgConvexHull3DFace* const face = &node->GetInfo(); + + dgInt32 i0 = face->m_index[0]; + dgInt32 i1 = face->m_index[1]; + dgInt32 i2 = face->m_index[2]; + + const dgBigVector& p0 = m_points[i0]; + dgAssert(p0.m_w == dgFloat32(0.0f)); + dgBigVector normal ((m_points[i1] - p0).CrossProduct(m_points[i2] - p0)); + + dgAssert(normal.m_w == dgFloat32(0.0f)); + dgAssert(localP0.m_w == dgFloat32(0.0f)); + + //dgFloat64 N = -((localP0 - p0) % normal); + dgFloat64 D = normal.DotProduct(dS).GetScalar(); + dgFloat64 N = -normal.DotProduct(localP0 - p0).GetScalar(); + + if (fabs(D) < dgFloat64 (1.0e-12f)) { // + if (N < dgFloat64 (0.0f)) { + return dgFloat64 (1.2f); + } else { + continue; + } + } + + dgFloat64 t = N / D; + if (D < dgFloat64 (0.0f)) { + if (t > tE) { + tE = t; + hasHit = 1; + } + if (tE > tL) { + return dgFloat64 (1.2f); + } + } else { + dgAssert (D >= dgFloat64 (0.0f)); + tL = dgMin (tL, t); + if (tL < tE) { + return dgFloat64 (1.2f); + } + } + } + + if (hasHit) { + interset = tE; + } + + return interset; +} + +void dgConvexHull3d::Save (const char* const filename) const +{ + FILE* const file = fopen(filename, "wb"); + int index = 0; +// fprintf(file, "final\n"); + for (dgListNode* nodePtr = GetFirst(); nodePtr; nodePtr = nodePtr->GetNext()) { + fprintf(file, "triangle %d\n", index); + index++; + const dgConvexHull3DFace& face = nodePtr->GetInfo(); + const dgBigVector& p0 = m_points[face.m_index[0]]; + const dgBigVector& p1 = m_points[face.m_index[1]]; + const dgBigVector& p2 = m_points[face.m_index[2]]; + + fprintf(file, "p0(%f %f %f)\n", p0[0], p0[1], p0[2]); + fprintf(file, "p1(%f %f %f)\n", p1[0], p1[1], p1[2]); + fprintf(file, "p2(%f %f %f)\n", p2[0], p2[1], p2[2]); + } + fprintf(file, "\n"); + + fclose(file); +} \ No newline at end of file diff --git a/thirdparty/src/newton/dgCore/dgConvexHull3d.h b/thirdparty/src/newton/dgCore/dgConvexHull3d.h new file mode 100644 index 000000000..6c2138ed0 --- /dev/null +++ b/thirdparty/src/newton/dgCore/dgConvexHull3d.h @@ -0,0 +1,136 @@ +/* Copyright (c) <2003-2019> +* +* 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::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::dgListNode* m_twin[3]; + friend class dgConvexHull3d; +}; + +class dgConvexHull3d: public dgList +{ +#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 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 diff --git a/thirdparty/src/newton/dgCore/dgConvexHull4d.cpp b/thirdparty/src/newton/dgCore/dgConvexHull4d.cpp new file mode 100644 index 000000000..ca4a38d02 --- /dev/null +++ b/thirdparty/src/newton/dgCore/dgConvexHull4d.cpp @@ -0,0 +1,1056 @@ +/* Copyright (c) <2003-2019> +* +* 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 "dgTree.h" +#include "dgHeap.h" +#include "dgStack.h" +#include "dgGoogol.h" +#include "dgConvexHull4d.h" +#include "dgSmallDeterminant.h" + +#define DG_VERTEX_CLUMP_SIZE_4D 8 + + +dgConvexHull4d::dgNormalMap::dgNormalMap() + :m_count(sizeof (m_normal) / sizeof (m_normal[0])) +{ + dgVector p0(dgFloat32( 1.0f), dgFloat32( 0.0f), dgFloat32( 0.0f), dgFloat32(0.0f)); + dgVector p1(dgFloat32(-1.0f), dgFloat32( 0.0f), dgFloat32( 0.0f), dgFloat32(0.0f)); + dgVector p2(dgFloat32( 0.0f), dgFloat32( 1.0f), dgFloat32( 0.0f), dgFloat32(0.0f)); + dgVector p3(dgFloat32( 0.0f), dgFloat32(-1.0f), dgFloat32( 0.0f), dgFloat32(0.0f)); + dgVector p4(dgFloat32( 0.0f), dgFloat32( 0.0f), dgFloat32( 1.0f), dgFloat32(0.0f)); + dgVector p5(dgFloat32( 0.0f), dgFloat32( 0.0f), dgFloat32(-1.0f), dgFloat32(0.0f)); + + dgInt32 count = 0; + dgInt32 subdivitions = 2; + + dgBigVector tmp[128]; + TessellateTriangle(subdivitions, p4, p0, p2, tmp, count); + TessellateTriangle(subdivitions, p0, p5, p2, tmp, count); + TessellateTriangle(subdivitions, p5, p1, p2, tmp, count); + TessellateTriangle(subdivitions, p1, p4, p2, tmp, count); + TessellateTriangle(subdivitions, p0, p4, p3, tmp, count); + TessellateTriangle(subdivitions, p5, p0, p3, tmp, count); + TessellateTriangle(subdivitions, p1, p5, p3, tmp, count); + TessellateTriangle(subdivitions, p4, p1, p3, tmp, count); + + count = 0; + for (dgInt32 j = 0; j < 8; j++) { + dgFloat64 beta = (j - 4) * dgFloat64 (22.5f * dgDegreeToRad) + dgFloat64 (10.5f * dgDegreeToRad); + dgFloat64 sinBeta = sin(beta); + dgFloat64 cosBeta = cos(beta); + + dgFloat64 w = sinBeta; + for (dgInt32 i = 0; i < 128; i ++) { + dgFloat64 z = cosBeta * tmp[i].m_z; + dgFloat64 y = cosBeta * tmp[i].m_y; + dgFloat64 x = cosBeta * tmp[i].m_x; + dgInt32 index = dgBitReversal(count, sizeof (m_normal) / sizeof (m_normal[0])); + dgAssert (index < sizeof (m_normal) / sizeof (m_normal[0])); + dgAssert (count < sizeof (m_normal) / sizeof (m_normal[0])); + m_normal[index] = dgBigVector (x, y, z, w); + count ++; + } + } +} + +void dgConvexHull4d::dgNormalMap::TessellateTriangle(dgInt32 level, const dgVector& p0, const dgVector& p1, const dgVector& p2, dgBigVector* const buffer, dgInt32& count) +{ + dgAssert(p0.m_w == dgFloat32(0.0f)); + dgAssert(p1.m_w == dgFloat32(0.0f)); + dgAssert(p2.m_w == dgFloat32(0.0f)); + if (level) { + dgAssert(dgAbs(p0.DotProduct(p0).GetScalar() - dgFloat32(1.0f)) < dgFloat32(1.0e-4f)); + dgAssert(dgAbs(p1.DotProduct(p1).GetScalar() - dgFloat32(1.0f)) < dgFloat32(1.0e-4f)); + dgAssert(dgAbs(p2.DotProduct(p2).GetScalar() - dgFloat32(1.0f)) < dgFloat32(1.0e-4f)); + dgVector p01(p0 + p1); + dgVector p12(p1 + p2); + dgVector p20(p2 + p0); + + dgAssert (p01.m_w == dgFloat32 (0.0f)); + dgAssert (p12.m_w == dgFloat32 (0.0f)); + dgAssert (p20.m_w == dgFloat32 (0.0f)); + p01 = p01.Scale(dgRsqrt(p01.DotProduct(p01).GetScalar())); + p12 = p12.Scale(dgRsqrt(p12.DotProduct(p12).GetScalar())); + p20 = p20.Scale(dgRsqrt(p20.DotProduct(p20).GetScalar())); + + dgAssert(dgAbs(p01.DotProduct(p01).GetScalar() - dgFloat32(1.0f)) < dgFloat32(1.0e-4f)); + dgAssert(dgAbs(p12.DotProduct(p12).GetScalar() - dgFloat32(1.0f)) < dgFloat32(1.0e-4f)); + dgAssert(dgAbs(p20.DotProduct(p20).GetScalar() - dgFloat32(1.0f)) < dgFloat32(1.0e-4f)); + + TessellateTriangle(level - 1, p0, p01, p20, buffer, count); + TessellateTriangle(level - 1, p1, p12, p01, buffer, count); + TessellateTriangle(level - 1, p2, p20, p12, buffer, count); + TessellateTriangle(level - 1, p01, p12, p20, buffer, count); + } else { + dgBigPlane n(p0, p1, p2); + n = n.Scale(dgFloat64(1.0f) / sqrt(n.DotProduct(n).GetScalar())); + n.m_w = dgFloat64(0.0f); + dgInt32 index = dgBitReversal(count, 128); + buffer[index] = n; + dgAssert(count < 128); + count++; + } +} + + +class dgConvexHull4dAABBTreeNode +{ + public: + #ifdef _DEBUG + dgConvexHull4dAABBTreeNode() + { + static dgInt32 id = 0; + m_id = id; + id ++; + } + dgInt32 m_id; + #endif + + dgBigVector m_box[2]; + dgConvexHull4dAABBTreeNode* m_left; + dgConvexHull4dAABBTreeNode* m_right; + dgConvexHull4dAABBTreeNode* m_parent; +}; + +class dgConvexHull4dPointCluster: public dgConvexHull4dAABBTreeNode +{ + public: + dgInt32 m_count; + dgInt32 m_indices[DG_VERTEX_CLUMP_SIZE_4D]; +}; + + +dgConvexHull4dTetraherum::dgTetrahedrumPlane::dgTetrahedrumPlane (const dgBigVector& p0, const dgBigVector& p1, const dgBigVector& p2, const dgBigVector& p3) + :dgBigVector ((p1 - p0).CrossProduct (p2 - p0, p3 - p0)) +{ + dgBigVector& me = *this; + dgFloat64 invMag2 = dgFloat32 (0.0f); + dgFloat64 val = me.DotProduct(me).m_x; + if (val > dgFloat64 (1.0e-38)) { + invMag2 = dgFloat64 (1.0f) / sqrt (val); + } else { + invMag2 = dgFloat32 (0.0f); + } + + me.m_x *= invMag2; + me.m_y *= invMag2; + me.m_z *= invMag2; + me.m_w *= invMag2; + m_dist = - me.DotProduct(p0).m_x; +} + +dgFloat64 dgConvexHull4dTetraherum::dgTetrahedrumPlane::Evalue (const dgBigVector& point) const +{ + const dgBigVector& me = *this; + return me.DotProduct(point).m_x + m_dist; +} + + +dgConvexHull4dTetraherum::dgConvexHull4dTetraherum() +{ +#ifdef _DEBUG + static dgInt32 debugID; + m_debugID = debugID; + debugID ++; +#endif + static dgInt32 m_monotonicID; + + m_uniqueID = m_monotonicID; + m_monotonicID ++; +} + +void dgConvexHull4dTetraherum::Init (const dgConvexHull4dVector* const points, dgInt32 v0, dgInt32 v1, dgInt32 v2, dgInt32 v3) +{ + m_faces[0].m_index[0] = v0; + m_faces[0].m_index[1] = v1; + m_faces[0].m_index[2] = v2; + m_faces[0].m_index[3] = v3; + + m_faces[1].m_index[0] = v3; + m_faces[1].m_index[1] = v0; + m_faces[1].m_index[2] = v2; + m_faces[1].m_index[3] = v1; + + m_faces[2].m_index[0] = v3; + m_faces[2].m_index[1] = v2; + m_faces[2].m_index[2] = v1; + m_faces[2].m_index[3] = v0; + + m_faces[3].m_index[0] = v3; + m_faces[3].m_index[1] = v1; + m_faces[3].m_index[2] = v0; + m_faces[3].m_index[3] = v2; + + SetMark (0); + for (dgInt32 i = 0; i < 4; i ++) { + m_faces[i].m_twin = NULL; + } + +#ifdef _DEBUG + dgBigVector p1p0 (points[v1] - points[v0]); + dgBigVector p2p0 (points[v2] - points[v0]); + dgBigVector p3p0 (points[v3] - points[v0]); + dgBigVector normal (p1p0.CrossProduct(p2p0, p3p0)); + dgFloat64 volume = normal.DotProduct(normal).m_x; + dgAssert (volume > dgFloat64 (0.0f)); +#endif +} + + +dgFloat64 dgConvexHull4dTetraherum::Evalue (const dgConvexHull4dVector* const pointArray, const dgBigVector& point) const +{ + const dgBigVector &p0 = pointArray[m_faces[0].m_index[0]]; + const dgBigVector &p1 = pointArray[m_faces[0].m_index[1]]; + const dgBigVector &p2 = pointArray[m_faces[0].m_index[2]]; + const dgBigVector &p3 = pointArray[m_faces[0].m_index[3]]; + + dgFloat64 matrix[4][4]; + for (dgInt32 i = 0; i < 4; i ++) { + matrix[0][i] = p1[i] - p0[i]; + matrix[1][i] = p2[i] - p0[i]; + matrix[2][i] = p3[i] - p0[i]; + matrix[3][i] = point[i] - p0[i]; + } + + dgFloat64 error; + dgFloat64 det = Determinant4x4 (matrix, &error); + dgFloat64 precision = dgFloat64 (1.0f) / dgFloat64 (1<<24); + dgFloat64 errbound = error * precision; + if (fabs(det) > errbound) { + return det; + } + + dgGoogol exactMatrix[4][4]; + for (dgInt32 i = 0; i < 4; i ++) { + exactMatrix[0][i] = dgGoogol(p1[i]) - dgGoogol(p0[i]); + exactMatrix[1][i] = dgGoogol(p2[i]) - dgGoogol(p0[i]); + exactMatrix[2][i] = dgGoogol(p3[i]) - dgGoogol(p0[i]); + exactMatrix[3][i] = dgGoogol(point[i]) - dgGoogol(p0[i]); + } + return Determinant4x4(exactMatrix); +} + +dgFloat64 dgConvexHull4dTetraherum::GetTetraVolume(const dgConvexHull4dVector* const points) const +{ + const dgBigVector &p0 = points[m_faces[0].m_index[0]]; + const dgBigVector &p1 = points[m_faces[0].m_index[1]]; + const dgBigVector &p2 = points[m_faces[0].m_index[2]]; + const dgBigVector &p3 = points[m_faces[0].m_index[3]]; + + dgFloat64 matrix[3][3]; + for (dgInt32 i = 0; i < 3; i++) { + matrix[0][i] = p2[i] - p0[i]; + matrix[1][i] = p1[i] - p0[i]; + matrix[2][i] = p3[i] - p0[i]; + } + + dgFloat64 error; + dgFloat64 det = Determinant3x3(matrix, &error); + + + dgFloat64 precision = dgFloat64(1.0f) / dgFloat64(1 << 24); + dgFloat64 errbound = error * precision; + if (fabs(det) > errbound) { + return det; + } + + dgGoogol exactMatrix[3][3]; + for (dgInt32 i = 0; i < 3; i++) { + exactMatrix[0][i] = dgGoogol(p2[i]) - dgGoogol(p0[i]); + exactMatrix[1][i] = dgGoogol(p1[i]) - dgGoogol(p0[i]); + exactMatrix[2][i] = dgGoogol(p3[i]) - dgGoogol(p0[i]); + } + return Determinant3x3(exactMatrix); +} + + +dgBigVector dgConvexHull4dTetraherum::CircumSphereCenter (const dgConvexHull4dVector* const pointArray) const +{ + dgGoogol matrix[4][4]; + + dgBigVector points[4]; + points[0] = pointArray[m_faces[0].m_index[0]]; + points[1] = pointArray[m_faces[0].m_index[1]]; + points[2] = pointArray[m_faces[0].m_index[2]]; + points[3] = pointArray[m_faces[0].m_index[3]]; + + for (dgInt32 i = 0; i < 4; i ++) { + for (dgInt32 j = 0; j < 3; j ++) { + matrix[i][j] = dgGoogol (points[i][j]); + } + matrix[i][3] = dgGoogol (1.0f); + } + dgGoogol det (Determinant4x4(matrix)); + dgFloat64 invDen = dgFloat64 (1.0f) / (dgFloat64(det) * dgFloat64 (2.0f)); + + dgBigVector centerOut; + dgFloat64 sign = dgFloat64 (1.0f); + for (dgInt32 k = 0; k < 3; k ++) { + for (dgInt32 i = 0; i < 4; i ++) { + matrix[i][0] = dgGoogol (points[i][3]); + for (dgInt32 j = 0; j < 2; j ++) { + dgInt32 j1 = (j < k) ? j : j + 1; + matrix[i][j + 1] = dgGoogol (points[i][j1]); + } + matrix[i][3] = dgGoogol (1.0f); + } + dgGoogol det1 (Determinant4x4(matrix)); + dgFloat64 val = dgFloat64 (det1) * sign; + sign *= dgFloat64 (-1.0f); + centerOut[k] = val * invDen; + } + centerOut[3] = dgFloat32 (0.0f); + return centerOut; +} + +dgConvexHull4dTetraherum::dgTetrahedrumPlane dgConvexHull4dTetraherum::GetPlaneEquation (const dgConvexHull4dVector* const points) const +{ + const dgBigVector &p0 = points[m_faces[0].m_index[0]]; + const dgBigVector &p1 = points[m_faces[0].m_index[1]]; + const dgBigVector &p2 = points[m_faces[0].m_index[2]]; + const dgBigVector &p3 = points[m_faces[0].m_index[3]]; + return dgTetrahedrumPlane (p0, p1, p2, p3); +} + +dgConvexHull4d::dgConvexHull4d (dgMemoryAllocator* const allocator) + :dgList(allocator), m_mark(0), m_count (0), m_diag(), m_points(allocator) +{ +} + + +dgConvexHull4d::dgConvexHull4d (dgMemoryAllocator* const allocator, const dgFloat64* const vertexCloud, dgInt32 strideInBytes, dgInt32 count, dgFloat64 distTol) + :dgList(allocator), m_mark(0), m_count (0), m_diag(), m_points(allocator) +{ + BuildHull (allocator, vertexCloud, strideInBytes, count, distTol); +} + + +dgConvexHull4d::~dgConvexHull4d(void) +{ +} + +const dgConvexHull4d::dgNormalMap& dgConvexHull4d::GetNormaMap() +{ + static dgNormalMap normalMap; + return normalMap; +} + + +void dgConvexHull4d::Save (const char* const filename) const +{ + FILE* const file = fopen (filename, "wb"); + int index = 0; +// fprintf (file, "final\n"); + for (dgListNode* nodePtr = GetFirst(); nodePtr; nodePtr = nodePtr->GetNext()) { + fprintf (file, "tetra %d\n", index); + index ++; + const dgConvexHull4dTetraherum& face = nodePtr->GetInfo(); + const dgBigVector& p0 = m_points[face.m_faces[0].m_index[0]]; + const dgBigVector& p1 = m_points[face.m_faces[0].m_index[1]]; + const dgBigVector& p2 = m_points[face.m_faces[0].m_index[2]]; + const dgBigVector& p3 = m_points[face.m_faces[0].m_index[3]]; + fprintf (file, "p0(%f %f %f %f)\n", p0[0], p0[1], p0[2], p0[3]); + fprintf (file, "p1(%f %f %f %f)\n", p1[0], p1[1], p1[2], p1[3]); + fprintf (file, "p2(%f %f %f %f)\n", p2[0], p2[1], p2[2], p2[3]); + fprintf (file, "p3(%f %f %f %f)\n", p3[0], p3[1], p3[2], p3[3]); + } + fprintf (file, "\n"); + + fclose (file); +} + + +dgInt32 dgConvexHull4d::SupportVertex (dgConvexHull4dAABBTreeNode** const treePointer, const dgConvexHull4dVector* const points, const dgBigVector& dir, const bool removeEntry) const +{ + #define DG_STACK_DEPTH_4D 64 + dgFloat64 aabbProjection[DG_STACK_DEPTH_4D]; + const dgConvexHull4dAABBTreeNode *stackPool[DG_STACK_DEPTH_4D]; + + dgInt32 index = -1; + dgInt32 stack = 1; + stackPool[0] = *treePointer; + aabbProjection[0] = dgFloat32 (1.0e20f); + dgFloat64 maxProj = dgFloat64 (-1.0e20f); + dgInt32 ix = (dir[0] > dgFloat64 (0.0f)) ? 1 : 0; + dgInt32 iy = (dir[1] > dgFloat64 (0.0f)) ? 1 : 0; + dgInt32 iz = (dir[2] > dgFloat64 (0.0f)) ? 1 : 0; + dgInt32 iw = (dir[3] > dgFloat64 (0.0f)) ? 1 : 0; + while (stack) { + stack--; + dgFloat64 boxSupportValue = aabbProjection[stack]; + if (boxSupportValue > maxProj) { + const dgConvexHull4dAABBTreeNode* const me = stackPool[stack]; + + if (me->m_left && me->m_right) { + dgBigVector leftSupportPoint (me->m_left->m_box[ix].m_x, me->m_left->m_box[iy].m_y, me->m_left->m_box[iz].m_z, me->m_left->m_box[iw].m_w); + dgFloat64 leftSupportDist = leftSupportPoint.DotProduct(dir).m_x; + + dgBigVector rightSupportPoint (me->m_right->m_box[ix].m_x, me->m_right->m_box[iy].m_y, me->m_right->m_box[iz].m_z, me->m_right->m_box[iw].m_w); + dgFloat64 rightSupportDist = rightSupportPoint.DotProduct(dir).m_x; + + if (rightSupportDist >= leftSupportDist) { + aabbProjection[stack] = leftSupportDist; + stackPool[stack] = me->m_left; + stack++; + dgAssert (stack < DG_STACK_DEPTH_4D); + aabbProjection[stack] = rightSupportDist; + stackPool[stack] = me->m_right; + stack++; + dgAssert (stack < DG_STACK_DEPTH_4D); + } else { + aabbProjection[stack] = rightSupportDist; + stackPool[stack] = me->m_right; + stack++; + dgAssert (stack < DG_STACK_DEPTH_4D); + aabbProjection[stack] = leftSupportDist; + stackPool[stack] = me->m_left; + stack++; + dgAssert (stack < DG_STACK_DEPTH_4D); + } + + } else { + dgConvexHull4dPointCluster* const cluster = (dgConvexHull4dPointCluster*) me; + for (dgInt32 i = 0; i < cluster->m_count; i ++) { + const dgConvexHull4dVector& p = points[cluster->m_indices[i]]; + dgAssert (p.m_x >= cluster->m_box[0].m_x); + dgAssert (p.m_x <= cluster->m_box[1].m_x); + dgAssert (p.m_y >= cluster->m_box[0].m_y); + dgAssert (p.m_y <= cluster->m_box[1].m_y); + dgAssert (p.m_z >= cluster->m_box[0].m_z); + dgAssert (p.m_z <= cluster->m_box[1].m_z); + dgAssert (p.m_w >= cluster->m_box[0].m_w); + dgAssert (p.m_w <= cluster->m_box[1].m_w); + if (!p.m_mark) { + dgFloat64 dist = p.DotProduct(dir).m_x; + if (dist > maxProj) { + maxProj = dist; + index = cluster->m_indices[i]; + } + } else if (removeEntry) { + cluster->m_indices[i] = cluster->m_indices[cluster->m_count - 1]; + cluster->m_count = cluster->m_count - 1; + i --; + } + } + + if (cluster->m_count == 0) { + dgConvexHull4dAABBTreeNode* const parent = cluster->m_parent; + if (parent) { + dgConvexHull4dAABBTreeNode* const sibling = (parent->m_left != cluster) ? parent->m_left : parent->m_right; + dgAssert (sibling != cluster); + dgConvexHull4dAABBTreeNode* const grandParent = parent->m_parent; + if (grandParent) { + sibling->m_parent = grandParent; + if (grandParent->m_right == parent) { + grandParent->m_right = sibling; + } else { + grandParent->m_left = sibling; + } + } else { + sibling->m_parent = NULL; + *treePointer = sibling; + } + } + } + } + } + } + + dgAssert (index != -1); + return index; +} + + +dgInt32 dgConvexHull4d::ConvexCompareVertex(const dgConvexHull4dVector* const A, const dgConvexHull4dVector* const B, void* const context) +{ + for (dgInt32 i = 0; i < 4; i ++) { + if ((*A)[i] < (*B)[i]) { + return -1; + } else if ((*A)[i] > (*B)[i]) { + return 1; + } + } + return 0; +} + + +dgConvexHull4dAABBTreeNode* dgConvexHull4d::BuildTree (dgConvexHull4dAABBTreeNode* const parent, dgConvexHull4dVector* const points, dgInt32 count, dgInt32 baseIndex, dgInt8** memoryPool, dgInt32& maxMemSize) const +{ + dgConvexHull4dAABBTreeNode* tree = NULL; + + dgAssert (count); + dgBigVector minP ( dgFloat32 (1.0e15f)); + dgBigVector maxP (-dgFloat32 (1.0e15f)); + if (count <= DG_VERTEX_CLUMP_SIZE_4D) { + + dgConvexHull4dPointCluster* const clump = new (*memoryPool) dgConvexHull4dPointCluster; + *memoryPool += sizeof (dgConvexHull4dPointCluster); + maxMemSize -= sizeof (dgConvexHull4dPointCluster); + dgAssert (maxMemSize >= 0); + + dgAssert (clump); + clump->m_count = count; + for (dgInt32 i = 0; i < count; i ++) { + clump->m_indices[i] = i + baseIndex; + + const dgBigVector& p = points[i]; + minP.m_x = dgMin (p.m_x, minP.m_x); + minP.m_y = dgMin (p.m_y, minP.m_y); + minP.m_z = dgMin (p.m_z, minP.m_z); + minP.m_w = dgMin (p.m_w, minP.m_w); + + maxP.m_x = dgMax (p.m_x, maxP.m_x); + maxP.m_y = dgMax (p.m_y, maxP.m_y); + maxP.m_z = dgMax (p.m_z, maxP.m_z); + maxP.m_w = dgMax (p.m_w, maxP.m_w); + } + + clump->m_left = NULL; + clump->m_right = NULL; + tree = clump; + + } else { + dgBigVector median (dgFloat32 (0.0f)); + dgBigVector varian (dgFloat32 (0.0f)); + for (dgInt32 i = 0; i < count; i ++) { + + const dgBigVector& p = points[i]; + minP.m_x = dgMin (p.m_x, minP.m_x); + minP.m_y = dgMin (p.m_y, minP.m_y); + minP.m_z = dgMin (p.m_z, minP.m_z); + minP.m_w = dgMin (p.m_w, minP.m_w); + + maxP.m_x = dgMax (p.m_x, maxP.m_x); + maxP.m_y = dgMax (p.m_y, maxP.m_y); + maxP.m_z = dgMax (p.m_z, maxP.m_z); + maxP.m_w = dgMax (p.m_w, maxP.m_w); + + median = median + p; + varian = varian + p * p; + } + + varian = varian.Scale (dgFloat32 (count)) - median * median; + + dgInt32 index = 0; + dgFloat64 maxVarian = dgFloat64 (-1.0e10f); + for (dgInt32 i = 0; i < 4; i ++) { + if (varian[i] > maxVarian) { + index = i; + maxVarian = varian[i]; + } + } + dgBigVector center = median.Scale (dgFloat64 (1.0f) / dgFloat64 (count)); + + dgFloat64 test = center[index]; + + dgInt32 i0 = 0; + dgInt32 i1 = count - 1; + do { + for (; i0 <= i1; i0 ++) { + dgFloat64 val = points[i0][index]; + if (val > test) { + break; + } + } + + for (; i1 >= i0; i1 --) { + dgFloat64 val = points[i1][index]; + if (val < test) { + break; + } + } + + if (i0 < i1) { + dgSwap(points[i0], points[i1]); + i0++; + i1--; + } + } while (i0 <= i1); + + if (i0 == 0){ + i0 = count / 2; + } + if (i0 >= (count - 1)){ + i0 = count / 2; + } + + tree = new (*memoryPool) dgConvexHull4dAABBTreeNode; + *memoryPool += sizeof (dgConvexHull4dAABBTreeNode); + maxMemSize -= sizeof (dgConvexHull4dAABBTreeNode); + dgAssert (maxMemSize >= 0); + + dgAssert (i0); + dgAssert (count - i0); + + tree->m_left = BuildTree (tree, points, i0, baseIndex, memoryPool, maxMemSize); + tree->m_right = BuildTree (tree, &points[i0], count - i0, i0 + baseIndex, memoryPool, maxMemSize); + } + + dgAssert (tree); + tree->m_parent = parent; + tree->m_box[0] = minP - dgBigVector (dgFloat64 (1.0e-3f)); + tree->m_box[1] = maxP + dgBigVector (dgFloat64 (1.0e-3f)); + return tree; +} + +dgInt32 dgConvexHull4d::InitVertexArray(dgConvexHull4dVector* const points, const dgFloat64* const vertexCloud, dgInt32 strideInBytes, dgInt32 count, void* const memoryPool, dgInt32 maxMemSize) +{ + dgInt32 stride = dgInt32(strideInBytes / sizeof (dgFloat64)); + for (dgInt32 i = 0; i < count; i ++) { + points[i] = dgBigVector (vertexCloud[i * stride + 0], vertexCloud[i * stride + 1], vertexCloud[i * stride + 2], vertexCloud[i * stride + 3]); + points[i].m_index = i; + points[i].m_mark = 0; + } + + dgSort(points, count, ConvexCompareVertex); + dgInt32 indexCount = 0; + for (int i = 1; i < count; i ++) { + for (; i < count; i ++) { + if (ConvexCompareVertex (&points[indexCount], &points[i], NULL)) { + indexCount ++; + points[indexCount] = points[i]; + break; + } + } + } + count = indexCount + 1; + if (count < 4) { + m_count = 0; + return count; + } + + dgConvexHull4dAABBTreeNode* tree = BuildTree (NULL, points, count, 0, (dgInt8**) &memoryPool, maxMemSize); + + dgBigVector boxSize (tree->m_box[1] - tree->m_box[0]); + m_diag = dgFloat32 (sqrt (boxSize.DotProduct(boxSize).m_x)); + + dgInt32 marks[4]; + m_points.Resize(count); + bool validTetrahedrum = false; + dgConvexHull4dVector* const convexPoints = &m_points[0]; + const dgFloat64 testVol = dgFloat32 (1.0e-6f) * m_diag * m_diag * m_diag; + + const dgNormalMap& normalMap = GetNormaMap(); + for (dgInt32 i = 0; !validTetrahedrum && (i < normalMap.m_count); i++) { + dgInt32 index = SupportVertex(&tree, points, normalMap.m_normal[i], false); + convexPoints[0] = points[index]; + marks[0] = index; + for (dgInt32 j = i + 1; !validTetrahedrum && (j < normalMap.m_count); j++) { + dgInt32 index1 = SupportVertex(&tree, points, normalMap.m_normal[j], false); + convexPoints[1] = points[index1]; + dgBigVector p10(convexPoints[1] - convexPoints[0]); + if (p10.DotProduct(p10).GetScalar() >(dgFloat32(1.0e-3f) * m_diag)) { + marks[1] = index1; + for (dgInt32 k = j + 1; !validTetrahedrum && (k < normalMap.m_count); k++) { + dgInt32 index2 = SupportVertex(&tree, points, normalMap.m_normal[k], false); + convexPoints[2] = points[index2]; + dgBigVector p20(convexPoints[2] - convexPoints[0]); + dgBigVector p21(convexPoints[2] - convexPoints[1]); + bool test = p20.DotProduct(p20).GetScalar() > (dgFloat32(1.0e-3f) * m_diag); + test = test && (p21.DotProduct(p21).GetScalar() > (dgFloat32(1.0e-3f) * m_diag)); + if (test) { + marks[2] = index2; + for (dgInt32 l = k + 1; !validTetrahedrum && (l < normalMap.m_count); l++) { + dgInt32 index3 = SupportVertex(&tree, points, normalMap.m_normal[l], false); + convexPoints[3] = points[index3]; + dgBigVector p30(convexPoints[3] - convexPoints[0]); + dgBigVector plane(p10.CrossProduct(p20, p30)); + dgFloat64 volume = plane.DotProduct(plane).GetScalar(); + if (volume > testVol) { + validTetrahedrum = true; + marks[3] = index3; + } + } + } + } + } + } + } + + m_count = 4; + if (!validTetrahedrum) { + m_count = 0; + } + + if (validTetrahedrum) { + for (dgInt32 i = 0; i < 4; i ++) { + points[marks[i]].m_mark = 1; + } + } + + return count; +} + + +dgConvexHull4d::dgListNode* dgConvexHull4d::AddFace (dgInt32 i0, dgInt32 i1, dgInt32 i2, dgInt32 i3) +{ + dgListNode* const node = Append(); + dgConvexHull4dTetraherum& face = node->GetInfo(); + face.Init (&m_points[0], i0, i1, i2, i3); + return node; +} + + +void dgConvexHull4d::DeleteFace (dgListNode* const node) +{ + Remove (node); +} + + +bool dgConvexHull4d::Sanity() const +{ + for (dgListNode* node = GetFirst(); node; node = node->GetNext()) { + dgConvexHull4dTetraherum* const tetra = &node->GetInfo(); + + for (dgInt32 i = 0; i < 4; i ++) { + dgConvexHull4dTetraherum::dgTetrahedrumFace* const face = &tetra->m_faces[i]; + dgListNode* const twinNode = face->m_twin; + if (!twinNode) { + return false; + } + } + } + +/* + dgList tetraList(GetAllocator()); + const dgHullVector* const points = &m_points[0]; + for (dgListNode* node = GetFirst(); node; node = node->GetNext()) { + dgConvexHull4dTetraherum* const tetra = &node->GetInfo(); + const dgBigVector &p0 = points[tetra->m_faces[0].m_index[0]]; + const dgBigVector &p1 = points[tetra->m_faces[0].m_index[1]]; + const dgBigVector &p2 = points[tetra->m_faces[0].m_index[2]]; + const dgBigVector &p3 = points[tetra->m_faces[0].m_index[3]]; + + dgBigVector p1p0 (p1.Sub4(p0)); + dgBigVector p2p0 (p2.Sub4(p0)); + dgBigVector p3p0 (p3.Sub4(p0)); + dgBigVector normal (p1p0.CrossProduct (p2p0, p3p0)); + + if (normal.m_w < dgFloat64 (0.0f)) { + tetraList.Append(node); + } + } + + for (dgList::dgListNode* node0 = tetraList.GetFirst(); node0; node0 = node0->GetNext()) { + dgListNode* const tetraNode0 = node0->GetInfo(); + dgConvexHull4dTetraherum* const tetra0 = &tetraNode0->GetInfo(); + + dgInt32 index0[4]; + index0[0] = tetra0->m_faces[0].m_index[0]; + index0[1] = tetra0->m_faces[0].m_index[1]; + index0[2] = tetra0->m_faces[0].m_index[2]; + index0[3] = tetra0->m_faces[0].m_index[3]; + + const dgBigVector &p0 = points[index0[0]]; + const dgBigVector &p1 = points[index0[1]]; + const dgBigVector &p2 = points[index0[2]]; + const dgBigVector &p3 = points[index0[3]]; + for (dgList::dgListNode* node1 = node0->GetNext(); node1; node1 = node1->GetNext()) { + dgListNode* const tetraNode1 = node1->GetInfo(); + dgConvexHull4dTetraherum* const tetra1 = &tetraNode1->GetInfo(); + + dgInt32 index1[4]; + index1[0] = tetra1->m_faces[0].m_index[0]; + index1[1] = tetra1->m_faces[0].m_index[1]; + index1[2] = tetra1->m_faces[0].m_index[2]; + index1[3] = tetra1->m_faces[0].m_index[3]; + + for (dgInt32 i = 0; i < 4; i ++) { + dgInt32 count = 0; + dgInt32 k = index1[i]; + for (dgInt32 j = 0; j < 4; j ++) { + count += (k == index0[j]); + } + if (!count){ +// const dgBigVector &p = points[k]; +// dgFloat64 size = -insphere(&p0.m_x, &p1.m_x, &p2.m_x, &p3.m_x, &p.m_x); +// if (size < dgFloat64 (0.0f)) { +// return false; +// } + } + } + } + } +*/ + + return true; +} + + +void dgConvexHull4d::LinkSibling (dgListNode* node0, dgListNode* node1) const +{ + dgConvexHull4dTetraherum* const tetra0 = &node0->GetInfo(); + dgConvexHull4dTetraherum* const tetra1 = &node1->GetInfo(); + for (int i = 0; i < 4; i ++) { + dgConvexHull4dTetraherum::dgTetrahedrumFace* const face0 = &tetra0->m_faces[i]; + if (!face0->m_twin) { + dgInt32 i0 = face0->m_index[0]; + dgInt32 i1 = face0->m_index[1]; + dgInt32 i2 = face0->m_index[2]; + for (int j = 0; j < 4; j ++) { + dgConvexHull4dTetraherum::dgTetrahedrumFace* const face1 = &tetra1->m_faces[j]; + if (!face1->m_twin) { + dgInt32 j2 = face1->m_index[0]; + dgInt32 j1 = face1->m_index[1]; + dgInt32 j0 = face1->m_index[2]; + + if (((i0 == j0) && (i1 == j1) && (i2 == j2)) || ((i1 == j0) && (i2 == j1) && (i0 == j2)) || ((i2 == j0) && (i0 == j1) && (i1 == j2))) { + face0->m_twin = node1; + face1->m_twin = node0; + return; + } + } + } + } + } +} + + +void dgConvexHull4d::InsertNewVertex(dgInt32 vertexIndex, dgListNode* const frontFace, dgList& deletedFaces, dgList& newFaces) +{ + dgAssert (Sanity()); + dgList stack(GetAllocator()); + + dgInt32 mark = IncMark(); + stack.Append(frontFace); + dgConvexHull4dVector* const hullVertexArray = &m_points[0]; + const dgBigVector& p = hullVertexArray[vertexIndex]; + while (stack.GetCount()) { + dgList::dgListNode* const stackNode = stack.GetLast(); + dgListNode* const node = stackNode->GetInfo(); + stack.Remove(stackNode); + dgConvexHull4dTetraherum* const face = &node->GetInfo(); + if ((face->GetMark() != mark) && (face->Evalue(hullVertexArray, p) > dgFloat64(0.0f))) { +#ifdef _DEBUG + for (dgList::dgListNode* deleteNode = deletedFaces.GetFirst(); deleteNode; deleteNode = deleteNode->GetNext()) { + dgAssert (deleteNode->GetInfo() != node); + } +#endif + deletedFaces.Append(node); + + face->SetMark(mark); + for (dgInt32 i = 0; i < 4; i ++) { + dgListNode* const twinNode = (dgListNode*)face->m_faces[i].m_twin; + dgAssert (twinNode); + dgConvexHull4dTetraherum* const twinFace = &twinNode->GetInfo(); + + if (twinFace->GetMark() != mark) { + stack.Append(twinNode); + } + } + } + } + + dgTree perimeter(GetAllocator()); + for (dgList::dgListNode* deleteNode = deletedFaces.GetFirst(); deleteNode; deleteNode = deleteNode->GetNext()) { + dgListNode* const deleteTetraNode = deleteNode->GetInfo(); + dgConvexHull4dTetraherum* const deletedTetra = &deleteTetraNode->GetInfo(); + dgAssert (deletedTetra->GetMark() == mark); + for (dgInt32 i = 0; i < 4; i ++) { + dgListNode* const twinNode = deletedTetra->m_faces[i].m_twin; + dgConvexHull4dTetraherum* const twinTetra = &twinNode->GetInfo(); + + if (twinTetra->GetMark() != mark) { + if (!perimeter.Find(twinTetra->m_uniqueID)) { + perimeter.Insert(twinNode, twinTetra->m_uniqueID); + } + } + deletedTetra->m_faces[i].m_twin = NULL; + } + } + + dgList coneList(GetAllocator()); + dgTree::Iterator iter (perimeter); + for (iter.Begin(); iter; iter ++) { + dgListNode* const perimeterNode = iter.GetNode()->GetInfo(); + dgConvexHull4dTetraherum* const perimeterTetra = &perimeterNode->GetInfo(); + for (dgInt32 i = 0; i < 4; i ++) { + dgConvexHull4dTetraherum::dgTetrahedrumFace* const perimeterFace = &perimeterTetra->m_faces[i]; + + if (perimeterFace->m_twin->GetInfo().GetMark() == mark) { + dgListNode* const newNode = AddFace (vertexIndex, perimeterFace->m_index[0], perimeterFace->m_index[1], perimeterFace->m_index[2]); + newFaces.Addtop(newNode); + + dgConvexHull4dTetraherum* const newTetra = &newNode->GetInfo(); + newTetra->m_faces[2].m_twin = perimeterNode; + perimeterFace->m_twin = newNode; + coneList.Append (newNode); + } + } + } + + for (dgList::dgListNode* coneNode = coneList.GetFirst(); coneNode->GetNext(); coneNode = coneNode->GetNext()) { + dgListNode* const coneNodeA = coneNode->GetInfo(); + for (dgList::dgListNode* nextConeNode = coneNode->GetNext(); nextConeNode; nextConeNode = nextConeNode->GetNext()) { + dgListNode* const coneNodeB = nextConeNode->GetInfo(); + LinkSibling (coneNodeA, coneNodeB); + } + } +} + +dgConvexHull4d::dgListNode* dgConvexHull4d::FindFacingNode(const dgBigVector& vertex) +{ + const dgConvexHull4dVector* const hullVertexArray = &m_points[0]; + + dgListNode* bestNode = GetFirst(); + dgConvexHull4dTetraherum* const tetra = &bestNode->GetInfo(); + dgConvexHull4dTetraherum::dgTetrahedrumPlane plane (tetra->GetPlaneEquation (hullVertexArray)); + dgFloat64 dist = plane.Evalue(vertex); + dgInt32 mark = IncMark(); + tetra->SetMark(mark); + + dgInt8 buffer[1024 * 2 * sizeof (dgFloat64)]; + dgDownHeap heap (buffer, sizeof (buffer)); + + heap.Push(bestNode, dist); + dgInt32 maxCount = heap.GetMaxCount() - 1; + dgInt32 releafCount = maxCount >> 3; + while (heap.GetCount()) { + dgListNode* const node1 = heap[0]; + dgFloat64 dist1 = heap.Value(); + if (dist1 > dgFloat64 (1.0e-5f)) { + return node1; + } + heap.Pop(); + dgConvexHull4dTetraherum* const tetra1 = &node1->GetInfo(); + for (dgInt32 i = 0; i < 4; i ++) { + dgListNode* neigborghNode = tetra1->m_faces[i].m_twin; + dgConvexHull4dTetraherum* const neighborgh = &neigborghNode->GetInfo(); + if (neighborgh->GetMark() != mark) { + neighborgh->SetMark(mark); + if (heap.GetCount() >= maxCount) { + for (dgInt32 j = 0; j < releafCount; j ++) { + heap.Remove(heap.GetCount() - 1); + } + } + dgConvexHull4dTetraherum::dgTetrahedrumPlane plane1 (neighborgh->GetPlaneEquation (hullVertexArray)); + heap.Push(neigborghNode, plane1.Evalue(vertex)); + } + } + } + + for (dgListNode* node1 = GetFirst(); node1; node1 = node1->GetNext()) { + dgConvexHull4dTetraherum* const tetra1 = &node1->GetInfo(); + dgFloat64 dist1 = tetra1->Evalue(hullVertexArray, vertex); + if (dist1 > dgFloat64(0.0f)) { + return node1; + } + } + + + return NULL; +} + +dgInt32 dgConvexHull4d::AddVertex (const dgBigVector& vertex) +{ + dgSetPrecisionDouble precision; + dgInt32 index = -1; + dgListNode* const faceNode = FindFacingNode(vertex); + if (faceNode) { + index = m_count; + m_points[index] = vertex; + m_points[index].m_index = index; + m_count ++; + + dgList newFaces(GetAllocator()); + dgList deleteList(GetAllocator()); + + InsertNewVertex(index, faceNode, deleteList, newFaces); + for (dgList::dgListNode* deleteNode = deleteList.GetFirst(); deleteNode; deleteNode = deleteNode->GetNext()) { + dgListNode* const node = deleteNode->GetInfo(); + DeleteFace (node); + } + } + return index; +} + + +void dgConvexHull4d::BuildHull (dgMemoryAllocator* const allocator, const dgFloat64* const vertexCloud, dgInt32 strideInBytes, dgInt32 count, dgFloat64 distTol) +{ + dgInt32 treeCount = count / (DG_VERTEX_CLUMP_SIZE_4D>>1); + if (treeCount < 4) { + treeCount = 4; + } + treeCount *= 2; + + dgStack points (count); + dgStack treePool (treeCount + 256); + + count = InitVertexArray(&points[0], vertexCloud, strideInBytes, count, &treePool[0], treePool.GetSizeInBytes()); + if (m_count >= 4) { + CalculateConvexHull (&treePool[0], &points[0], count, distTol); + } +} + + +void dgConvexHull4d::CalculateConvexHull (dgConvexHull4dAABBTreeNode* vertexTree, dgConvexHull4dVector* const points, dgInt32 count, dgFloat64 distTol) +{ + distTol = fabs (distTol) * m_diag; + dgListNode* const nodes0 = AddFace (0, 1, 2, 3); + dgListNode* const nodes1 = AddFace (0, 1, 3, 2); + + dgList boundaryFaces(GetAllocator()); + boundaryFaces.Append(nodes0); + boundaryFaces.Append(nodes1); + + LinkSibling (nodes0, nodes1); + LinkSibling (nodes0, nodes1); + LinkSibling (nodes0, nodes1); + LinkSibling (nodes0, nodes1); + + IncMark(); + count -= 4; + dgInt32 currentIndex = 4; + while (boundaryFaces.GetCount() && count) { + dgConvexHull4dVector* const hullVertexArray = &m_points[0]; + dgListNode* const faceNode = boundaryFaces.GetFirst()->GetInfo(); + dgConvexHull4dTetraherum* const face = &faceNode->GetInfo(); + dgConvexHull4dTetraherum::dgTetrahedrumPlane planeEquation (face->GetPlaneEquation (hullVertexArray)); + + dgInt32 index = SupportVertex (&vertexTree, points, planeEquation); + + const dgConvexHull4dVector& p = points[index]; + dgFloat64 dist = planeEquation.Evalue(p); + if ((dist > distTol) && (face->Evalue(hullVertexArray, p) > dgFloat64(0.0f))) { + + m_points[currentIndex] = p; + points[index].m_mark = 1; + + dgList deleteList(GetAllocator()); + InsertNewVertex(currentIndex, faceNode, deleteList, boundaryFaces); + + for (dgList::dgListNode* deleteNode = deleteList.GetFirst(); deleteNode; deleteNode = deleteNode->GetNext()) { + dgListNode* const node = deleteNode->GetInfo(); + boundaryFaces.Remove (node); + DeleteFace (node); + } + + currentIndex ++; + count --; + } else { + boundaryFaces.Remove (faceNode); + } + } + m_count = currentIndex; +} diff --git a/thirdparty/src/newton/dgCore/dgConvexHull4d.h b/thirdparty/src/newton/dgCore/dgConvexHull4d.h new file mode 100644 index 000000000..e6d6b19ca --- /dev/null +++ b/thirdparty/src/newton/dgCore/dgConvexHull4d.h @@ -0,0 +1,193 @@ +/* Copyright (c) <2003-2019> +* +* 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::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 +{ + 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& deletedFaces, dgList& 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 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 diff --git a/thirdparty/src/newton/dgCore/dgDebug.cpp b/thirdparty/src/newton/dgCore/dgDebug.cpp new file mode 100644 index 000000000..17bfaadac --- /dev/null +++ b/thirdparty/src/newton/dgCore/dgDebug.cpp @@ -0,0 +1,26 @@ +/* Copyright (c) <2003-2019> +* +* 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" + + + diff --git a/thirdparty/src/newton/dgCore/dgDebug.h b/thirdparty/src/newton/dgCore/dgDebug.h new file mode 100644 index 000000000..92228e688 --- /dev/null +++ b/thirdparty/src/newton/dgCore/dgDebug.h @@ -0,0 +1,75 @@ +/* Copyright (c) <2003-2019> +* +* 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 + diff --git a/thirdparty/src/newton/dgCore/dgDelaunayTetrahedralization.cpp b/thirdparty/src/newton/dgCore/dgDelaunayTetrahedralization.cpp new file mode 100644 index 000000000..a722d5161 --- /dev/null +++ b/thirdparty/src/newton/dgCore/dgDelaunayTetrahedralization.cpp @@ -0,0 +1,178 @@ +/* Copyright (c) <2003-2019> +* +* 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 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); +} + + + + diff --git a/thirdparty/src/newton/dgCore/dgDelaunayTetrahedralization.h b/thirdparty/src/newton/dgCore/dgDelaunayTetrahedralization.h new file mode 100644 index 000000000..70e72b503 --- /dev/null +++ b/thirdparty/src/newton/dgCore/dgDelaunayTetrahedralization.h @@ -0,0 +1,45 @@ +/* Copyright (c) <2003-2019> +* +* 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 diff --git a/thirdparty/src/newton/dgCore/dgFastQueue.h b/thirdparty/src/newton/dgCore/dgFastQueue.h new file mode 100644 index 000000000..dc85b2af6 --- /dev/null +++ b/thirdparty/src/newton/dgCore/dgFastQueue.h @@ -0,0 +1,113 @@ +/* Copyright (c) <2003-2019> +* +* 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 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 +dgFastQueue::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 +dgFastQueue::~dgFastQueue () +{ + m_allocator->FreeLow(m_pool); +} + +template +bool dgFastQueue::IsEmpty() const +{ + return (m_head == m_tail); +} + +template +bool dgFastQueue::IsFull() const +{ + return (((m_tail + 1) & (sizeInPowerOfTwo - 1)) == m_head); +} + +template +const T& dgFastQueue::GetHead() const +{ + dgAssert (!IsEmpty()); + return m_pool[m_head]; +} + +template +void dgFastQueue::Pop() +{ + dgAssert (!IsEmpty()); + m_head = (m_head + 1) & (sizeInPowerOfTwo - 1); +} + +template +void dgFastQueue::Push(T& object) +{ + dgAssert (!IsFull()); + m_pool[m_tail] = object; + m_tail = (m_tail + 1) & (sizeInPowerOfTwo - 1); +} + + + +#endif + + + + diff --git a/thirdparty/src/newton/dgCore/dgGeneralMatrix.cpp b/thirdparty/src/newton/dgCore/dgGeneralMatrix.cpp new file mode 100644 index 000000000..baf5a1b90 --- /dev/null +++ b/thirdparty/src/newton/dgCore/dgGeneralMatrix.cpp @@ -0,0 +1,66 @@ +/* Copyright (c) <2003-2019> +* +* 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; +} +*/ \ No newline at end of file diff --git a/thirdparty/src/newton/dgCore/dgGeneralMatrix.h b/thirdparty/src/newton/dgCore/dgGeneralMatrix.h new file mode 100644 index 000000000..275709195 --- /dev/null +++ b/thirdparty/src/newton/dgCore/dgGeneralMatrix.h @@ -0,0 +1,1337 @@ + +/* Copyright (c) <2003-2019> +* +* 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 __dgGeneralMatrix__ +#define __dgGeneralMatrix__ + +#include "dgStdafx.h" +#include "dgDebug.h" +#include "dgVector.h" +#include "dgGeneralVector.h" + +#define DG_LCP_MAX_VALUE dgFloat32 (1.0e10f) + + +template +class dgOldSolverNetwon_1_5 +{ + public: + dgOldSolverNetwon_1_5() + { + } + + void SetSize(dgInt32 size) + { + m_size = size; + } + + dgInt32 GetSize() const + { + return m_size; + } + + dgFloat32* GetX() + { + return m_x; + } + + dgFloat32* GetB() + { + return m_b; + } + + dgFloat32* GetLow() + { + return m_low; + } + + dgFloat32* GetHigh() + { + return m_high; + } + + dgInt32* GetFrictionIndex() + { + return m_frictionIndex; + } + + dgFloat32* GetInvDiag() + { + return m_invDiag; + } + + dgFloat32* GetMatrixRow(dgInt32 i) + { + return &m_matrix[i * m_size]; + } + + dgFloat32 Solve() + { + dgInt32 stride = 0; + m_x[m_size] = dgFloat32(1.0f); + dgFloat32 accelNorm = dgFloat32(0.0f); + for (dgInt32 i = 0; i < m_size; i++) { + dgVector error(m_b[i]); + dgVector x(m_x[i]); + const dgInt32 frictionIndex = m_frictionIndex[i]; + const dgVector low(m_low[i] * m_x[frictionIndex]); + const dgVector high(m_high[i] * m_x[frictionIndex]); + error = error & (x < high) & (x > low); + accelNorm += error.GetScalar() * error.GetScalar(); + stride += m_size; + } + + const dgFloat32 tol2 = dgFloat32(1.0e-5f); + if (accelNorm > tol2) { + dgFloat32 accelNorm0 = accelNorm; + for (dgInt32 i = 0; (i < 5) && (accelNorm0 > tol2); i++) { + stride = 0; + accelNorm0 = dgFloat32(0.0f); + for (dgInt32 j = 0; j < m_size; j++) { + const dgFloat32* const row = &m_matrix[stride]; + dgFloat32 r = m_b[j]; + for (dgInt32 k = 0; k < m_size; k++) { + r = r - row[k] * m_x[k]; + } + const dgInt32 frictionIndex = m_frictionIndex[j]; + const dgVector low(m_low[j] * m_x[frictionIndex]); + const dgVector high(m_high[j] * m_x[frictionIndex]); + dgVector x((r + row[j] * m_x[j]) * m_invDiag[j]); + + dgVector a(r); + a = a & (x < high) & (x > low); + x = x.GetMax(low).GetMin(high); + m_x[j] = x.GetScalar(); + + accelNorm0 += a.GetScalar() * a.GetScalar(); + stride += m_size; + } + } + + if (accelNorm0 > tol2) { + stride = 0; + dgFloat32 mask[maxRows]; + for (dgInt32 i = 0; i < m_size; i++) { + dgFloat32 r = dgFloat32(0.0f); + const dgFloat32* const row = &m_matrix[stride]; + for (dgInt32 j = 0; j < m_size; j++) { + r += row[j] * m_x[j]; + } + m_b[i] -= r; + m_delta_x[i] = m_b[i]; + mask[i] = dgFloat32(1.0f); + const dgInt32 frictionIndex = m_frictionIndex[i]; + m_low[i] *= m_x[frictionIndex]; + m_high[i] *= m_x[frictionIndex]; + stride += m_size; + } + + dgFloat32 beta = dgFloat32(1.0f); + for (dgInt32 k = 0; (k < 20) && (beta > tol2); k++) { + stride = 0; + dgFloat32 num = dgFloat32(0.0f); + dgFloat32 den = dgFloat32(0.0f); + for (dgInt32 i = 0; i < m_size; i++) { + const dgFloat32* const row = &m_matrix[stride]; + dgFloat32 r = dgFloat32(0.0f); + for (dgInt32 j = 0; j < m_size; j++) { + r += row[j] * m_delta_x[j]; + } + stride += m_size; + m_delta_r[i] = r; + den += m_delta_x[i] * r; + num += m_b[i] * m_b[i] * mask[i]; + } + + dgInt32 index = -1; + dgFloat32 alpha = num / den; + dgAssert(alpha > dgFloat32(0.0f)); + + for (dgInt32 i = 0; (i < m_size) && (alpha > dgFloat32(0.0f)); i++) { + + if (m_delta_x[i]) { + dgFloat32 x = m_x[i] + alpha * m_delta_x[i]; + if (x < m_low[i]) { + index = i; + alpha = (m_low[i] - m_x[i]) / m_delta_x[i]; + } else if (x > m_high[i]) { + index = i; + alpha = (m_high[i] - m_x[i]) / m_delta_x[i]; + } + dgAssert(alpha >= dgFloat32(-1.0e-4f)); + if (alpha < dgFloat32(1.0e-6f)) { + alpha = dgFloat32(0.0f); + } + } + } + + beta = dgFloat32(0.0f); + for (dgInt32 i = 0; i < m_size; i++) { + m_x[i] += alpha * m_delta_x[i]; + m_b[i] -= alpha * m_delta_r[i]; + beta += m_b[i] * m_b[i] * mask[i]; + } + + if (index >= 0) { + beta = dgFloat32(0.0f); + mask[index] = dgFloat32(0.0f); + for (dgInt32 i = 0; i < m_size; i++) { + m_delta_x[i] = m_b[i] * mask[i]; + beta += m_b[i] * m_b[i] * mask[i]; + stride += m_size; + } + } else { + alpha = beta / num; + for (dgInt32 i = 0; i < m_size; i++) { + m_delta_x[i] = m_b[i] * mask[i] + alpha * m_delta_x[i]; + } + } + } + } + } + + return accelNorm; + } + + private: + dgFloat32 m_x[maxRows + 4]; + dgFloat32 m_b[maxRows]; + dgFloat32 m_low[maxRows]; + dgFloat32 m_high[maxRows]; + dgFloat32 m_invDiag[maxRows]; + dgFloat32 m_delta_x[maxRows]; + dgFloat32 m_delta_r[maxRows]; + dgFloat32 m_matrix[maxRows * maxRows]; + dgInt32 m_frictionIndex[maxRows]; + dgInt32 m_size; +}; + + +template +class dgSymmetricConjugateGradientSolver +{ + public: + dgSymmetricConjugateGradientSolver(); + dgSymmetricConjugateGradientSolver(T* const r0, T* const z0, T* const p0, T* const q0); + ~dgSymmetricConjugateGradientSolver(); + + void SetBuffers(T* const r0, T* const z0, T* const p0, T* const q0); + T Solve(dgInt32 size, T tolerance, T* const x, const T* const b); + + protected: + virtual void MatrixTimeVector(T* const out, const T* const v) const = 0; + virtual void InversePrecoditionerTimeVector(T* const out, const T* const v) const = 0; + + private: + T SolveInternal(dgInt32 size, T tolerance, T* const x, const T* const b) const; + //T DotProduct(dgInt32 size, const T* const b, const T* const c) const; + //void Sub(dgInt32 size, T* const a, const T* const b, const T* const c) const; + //void ScaleAdd(dgInt32 size, T* const a, const T* const b, T scale, const T* const c) const; + + T* m_r0; + T* m_z0; + T* m_p0; + T* m_q0; +}; + + +template +dgSymmetricConjugateGradientSolver::dgSymmetricConjugateGradientSolver() +{ + SetBuffers(NULL, NULL, NULL, NULL); +} + +template +dgSymmetricConjugateGradientSolver::dgSymmetricConjugateGradientSolver(T* const r0, T* const z0, T* const p0, T* const q0) +{ + SetBuffers(r0, z0, p0, q0); +} + +template +dgSymmetricConjugateGradientSolver::~dgSymmetricConjugateGradientSolver() +{ +} + +template +void dgSymmetricConjugateGradientSolver::SetBuffers(T* const r0, T* const z0, T* const p0, T* const q0) +{ + m_r0 = r0; + m_z0 = z0; + m_p0 = p0; + m_q0 = q0; +} + +template +T dgSymmetricConjugateGradientSolver::Solve(dgInt32 size, T tolerance, T* const x, const T* const b) +{ + if (m_r0) { + return SolveInternal(size, tolerance, x, b); + } else { + T* const r0 = dgAlloca(T, size); + T* const z0 = dgAlloca(T, size); + T* const p0 = dgAlloca(T, size); + T* const q0 = dgAlloca(T, size); + SetBuffers(r0, z0, p0, q0); + T error = SolveInternal(size, tolerance, x, b); + SetBuffers(NULL, NULL, NULL, NULL); + return error; + } +} + +template +T dgSymmetricConjugateGradientSolver::SolveInternal(dgInt32 size, T tolerance, T* const x, const T* const b) const +{ + MatrixTimeVector(m_z0, x); + dgSub(size, m_r0, b, m_z0); + InversePrecoditionerTimeVector(m_p0, m_r0); + + dgInt32 iter = 0; + T num = dgDotProduct(size, m_r0, m_p0); + T error2 = num; + for (dgInt32 j = 0; (j < size) && (error2 > tolerance); j++) { + + MatrixTimeVector(m_z0, m_p0); + T den = dgDotProduct(size, m_p0, m_z0); + + dgAssert(fabs(den) > T(0.0f)); + T alpha = num / den; + + dgMulAdd(size, x, x, m_p0, alpha); + if ((j % 50) != 49) { + dgMulAdd(size, m_r0, m_r0, m_z0, -alpha); + } else { + MatrixTimeVector(m_z0, x); + dgSub(size, m_r0, b, m_z0); + } + + InversePrecoditionerTimeVector(m_q0, m_r0); + + T num1 = dgDotProduct(size, m_r0, m_q0); + T beta = num1 / num; + dgMulAdd(size, m_p0, m_q0, m_p0, beta); + num = dgDotProduct(size, m_r0, m_q0); + iter++; + error2 = num; + if (j > 10) { + error2 = T(0.0f); + for (dgInt32 i = 0; i < size; i++) { + error2 = dgMax(error2, m_r0[i] * m_r0[i]); + } + } + } + dgAssert(iter <= size); + return num; +} + + +//************************************************************* +// +// generic linear algebra functions +// +//************************************************************* +template +void dgMatrixTimeVector(dgInt32 size, const T* const matrix, const T* const v, T* const out) +{ + dgInt32 stride = 0; + for (dgInt32 i = 0; i < size; i++) { + const T* const row = &matrix[stride]; + out[i] = dgDotProduct(size, row, v); + stride += size; + } +} + +template +void dgMatrixTimeMatrix(dgInt32 size, const T* const matrixA, const T* const matrixB, T* const out) +{ + for (dgInt32 i = 0; i < size; i++) { + const T* const rowA = &matrixA[i * size]; + T* const rowOut = &out[i * size]; + for (dgInt32 j = 0; j < size; j++) { + T acc = T(0.0f); + for (dgInt32 k = 0; k < size; k++) { + acc += rowA[k] * matrixB[k * size + j]; + } + rowOut[j] = acc; + } + } +} + +template +void dgCovarianceMatrix(dgInt32 size, T* const matrix, const T* const vectorA, const T* const vectorB) +{ + dgInt32 stride = 0; + for (dgInt32 i = 0; i < size; i++) { + T* const row = &matrix[stride]; + T scale (vectorA[i]); + for (dgInt32 j = 0; j < size; j++) { + row[j] = scale * vectorA[j]; + } + stride += size; + } +} + +template +DG_INLINE bool dgCholeskyFactorizationAddRow(dgInt32 size, dgInt32 stride, dgInt32 n, T* const matrix, T* const invDiagonalOut) +{ + T* const rowN = &matrix[stride * n]; + + dgInt32 base = 0; + for (dgInt32 j = 0; j <= n; j++) { + T s(0.0f); + T* const rowJ = &matrix[base]; + for (dgInt32 k = 0; k < j; k++) { + s += rowN[k] * rowJ[k]; + } + + if (n == j) { + T diag = rowN[n] - s; + if (diag < T(1.0e-6f)) { + return false; + } + + rowN[n] = T(sqrt(diag)); + invDiagonalOut[n] = T(1.0f) / rowN[n]; + } else { + rowJ[n] = T(0.0f); + //rowN[j] = (rowN[j] - s) / rowJ[j]; + rowN[j] = invDiagonalOut[j] * (rowN[j] - s); + } + + base += stride; + } + + return true; +} + +template +bool dgCholeskyFactorization(dgInt32 size, dgInt32 stride, T* const psdMatrix) +{ + bool state = true; + T* const invDiagonal = dgAlloca(T, size); + for (dgInt32 i = 0; (i < size) && state; i++) { + state = state && dgCholeskyFactorizationAddRow(size, stride, i, psdMatrix, invDiagonal); + } + return state; +} + +template +bool dgTestPSDmatrix(dgInt32 size, dgInt32 stride, T* const matrix) +{ + T* const copy = dgAlloca(T, size * size); + int row = 0; + for (int i = 0; i < size; i++) { + memcpy(©[i * size], &matrix[row], size * sizeof (T)); + row += stride; + } + return dgCholeskyFactorization(size, size, copy); +} + +template +void dgCholeskyApplyRegularizer (dgInt32 size, dgInt32 stride, T* const psdMatrix, T* const regularizer) +{ + bool isPsdMatrix = false; + dgFloat32* const lowerTriangule = dgAlloca(dgFloat32, stride * stride); + do { + memcpy(lowerTriangule, psdMatrix, sizeof(dgFloat32) * stride * stride); + isPsdMatrix = dgCholeskyFactorization(size, stride, lowerTriangule); + if (!isPsdMatrix) { + for (dgInt32 i = 0; i < size; i++) { + regularizer[i] *= dgFloat32(4.0f); + psdMatrix[i * stride + i] += regularizer[i]; + } + } + } while (!isPsdMatrix); +} + +template +DG_INLINE void dgSolveCholesky(dgInt32 size, dgInt32 stride, const T* const choleskyMatrix, T* const x, const T* const b) +{ + dgInt32 rowStart = 0; + for (dgInt32 i = 0; i < size; i++) { + T acc(0.0f); + const T* const row = &choleskyMatrix[rowStart]; + for (dgInt32 j = 0; j < i; j++) { + acc = acc + row[j] * x[j]; + } + x[i] = (b[i] - acc) / row[i]; + rowStart += stride; + } + + for (dgInt32 i = size - 1; i >= 0; i--) { + T acc = 0.0f; + for (dgInt32 j = i + 1; j < size; j++) { + acc = acc + choleskyMatrix[stride * j + i] * x[j]; + } + x[i] = (x[i] - acc) / choleskyMatrix[stride * i + i]; + } +} + +template +void dgSolveCholesky(dgInt32 size, T* const choleskyMatrix, T* const x) +{ + dgSolveCholesky(size, size, choleskyMatrix, x); +} + +template +bool dgSolveGaussian(dgInt32 size, T* const matrix, T* const b) +{ + for (dgInt32 i = 0; i < size - 1; i++) { + const T* const rowI = &matrix[i * size]; + dgInt32 m = i; + T maxVal (dgAbs(rowI[i])); + for (dgInt32 j = i + 1; j < size - 1; j++) { + T val (dgAbs(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 (dgInt32 j = 0; j < size; j++) { + dgSwap(rowK[j], rowJ[j]); + } + dgSwap(b[i], b[m]); + } + + T den = T(1.0f) / rowI[i]; + for (dgInt32 k = i + 1; k < size; k++) { + T* const rowK = &matrix[size * k]; + T factor(-rowK[i] * den); + for (dgInt32 j = i + 1; j < size; j++) { + rowK[j] += rowI[j] * factor; + } + rowK[i] = T(0.0f); + b[k] += b[i] * factor; + } + } + + for (dgInt32 i = size - 1; i >= 0; i--) { + T acc(0); + T* const rowI = &matrix[i * size]; + for (dgInt32 j = i + 1; j < size; j++) { + acc = acc + rowI[j] * b[j]; + } + b[i] = (b[i] - acc) / rowI[i]; + } + return true; +} + +template +void dgEigenValues(const dgInt32 size, const dgInt32 stride, const T* const symmetricMatrix, T* const eigenValues) +{ + T* const offDiag = dgAlloca(T, size); + T* const matrix = dgAlloca(T, size * stride); + + memcpy(matrix, symmetricMatrix, sizeof(T) * size * stride); + for (dgInt32 i = size - 1; i > 0; i--) { + T h(0.0f); + T* const rowI = &matrix[i * stride]; + + if (i > 1) { + T scale(0.0f); + for (dgInt32 k = 0; k < i; k++) { + scale += dgAbs(rowI[k]); + } + + if (scale == T(0.0f)) { + offDiag[i] = rowI[i - 1]; + } else { + for (dgInt32 k = 0; k < i; k++) { + rowI[k] /= scale; + h += rowI[k] * rowI[k]; + } + + T f(rowI[i - 1]); + T g((f >= T(0.0f) ? -T(sqrt(h)) : T(sqrt(h)))); + offDiag[i] = scale * g; + h -= f * g; + rowI[i - 1] = f - g; + f = T(0.0f); + + for (dgInt32 j = 0; j < i; j++) { + g = T(0.0f); + const T* const rowJ = &matrix[j * stride]; + for (dgInt32 k = 0; k <= j; k++) { + g += rowJ[k] * rowI[k]; + } + for (dgInt32 k = j + 1; k < i; k++) { + g += matrix[k * stride + j] * rowI[k]; + } + offDiag[j] = g / h; + f += offDiag[j] * rowI[j]; + } + + T hh(f / (h + h)); + for (dgInt32 j = 0; j < i; j++) { + T f1 (rowI[j]); + T g1(offDiag[j] - hh * f1); + offDiag[j] = g1; + T* const rowJ = &matrix[j * stride]; + for (dgInt32 k = 0; k <= j; k++) { + rowJ[k] -= (f1 * offDiag[k] + g1 * rowI[k]); + } + } + } + } else { + offDiag[i] = rowI[i - 1]; + } + eigenValues[i] = h; + } + + dgInt32 index = stride; + eigenValues[0] = matrix[0]; + for (dgInt32 i = 1; i < size; i++) { + eigenValues[i] = matrix[index + i]; + offDiag[i - 1] = offDiag[i]; + index += stride; + } + + for (dgInt32 i = 0; i < size; i++) { + dgInt32 j; + dgInt32 iter = 0; + do { + for (j = i; j < size - 1; j++) { + T dd(dgAbs(eigenValues[j]) + dgAbs(eigenValues[j + 1])); + if (dgAbs(offDiag[j]) <= (T(1.e-6f) * dd)) { + break; + } + } + + if (j != i) { + iter++; + if (iter == 10) { + dgAssert(0); + return; + } + + T g((eigenValues[i + 1] - eigenValues[i]) / (T(2.0f) * offDiag[i])); + T r(dgPythag(g, T(1.0f))); + g = eigenValues[j] - eigenValues[i] + offDiag[i] / (g + dgSign(r, g)); + T s(1.0f); + T c(1.0f); + T p(0.0f); + + dgInt32 k; + for (k = j - 1; k >= i; k--) { + T f(s * offDiag[k]); + T b(c * offDiag[k]); + T d(dgPythag(f, g)); + offDiag[k + 1] = d; + if (d == T(0.0f)) { + eigenValues[k + 1] -= p; + offDiag[j] = T(0.0f); + break; + } + s = f / d; + c = g / d; + g = eigenValues[k + 1] - p; + d = (eigenValues[k] - g) * s + T(2.0f) * c * b; + p = s * d; + eigenValues[k + 1] = g + p; + g = c * d - b; + } + + if (r == T(0.0f) && k >= i) { + continue; + } + eigenValues[i] -= p; + offDiag[i] = g; + offDiag[j] = T(0.0f); + } + } while (j != i); + } +} + +template +T dgConditionNumber(const dgInt32 size, const dgInt32 stride, const T* const choleskyMatrix) +{ + T* const eigenValues = dgAlloca(T, size); + dgEigenValues(size, stride, choleskyMatrix, eigenValues); + + T minVal = T(1.0e20f); + T maxVal = T(-1.0e20f); + for (int i = 0; i < size; i++) { + minVal = dgMin(minVal, eigenValues[i]); + maxVal = dgMax(maxVal, eigenValues[i]); + } + T condition = T(dgAbs(maxVal) / dgAbs(minVal)); + return condition; +} + + +// 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 +void dgGaussSeidelLcpSor(const dgInt32 size, const T* const matrix, T* const x, const T* const b, const T* const low, const T* const high, T tol2, dgInt32 maxIterCount, dgInt16* const clipped, T sor) +{ + const T* const me = matrix; + T* const invDiag1 = dgAlloca(T, size); + + dgInt32 stride = 0; + for (dgInt32 i = 0; i < size; i++) { + x[i] = dgClamp(T(0.0f), low[i], high[i]); + invDiag1[i] = T(1.0f) / me[stride + i]; + stride += size; + } + + T tolerance(tol2 * 2.0f); + const T* const invDiag = invDiag1; +#ifdef _DEBUG + dgInt32 passes = 0; +#endif + for (dgInt32 i = 0; (i < maxIterCount) && (tolerance > tol2); i++) { + dgInt32 base = 0; + tolerance = T(0.0f); +#ifdef _DEBUG + passes++; +#endif + for (dgInt32 j = 0; j < size; j++) { + const T* const row = &me[base]; + T r(b[j] - dgDotProduct(size, row, x)); + T f((r + row[j] * x[j]) * invDiag[j]); + if (f > high[j]) { + x[j] = high[j]; + clipped[j] = 1; + } else if (f < low[j]) { + x[j] = low[j]; + clipped[j] = 1; + } else { + clipped[j] = 0; + tolerance += r * r; + x[j] = x[j] + (f - x[j]) * sor; + } + base += 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 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 +void dgGaussSeidelLCP(const dgInt32 size, const T* const matrix, T* const x, const T* const b, const T* const low, const T* const high, T sor = T(1.2f)) +{ + dgInt16* const clipped = dgAlloca(dgInt16, size); + dgGaussSeidelLcpSor(size, matrix, x, b, low, high, T(1.0e-3f), size * size, clipped, sor); +} + +template +void dgPermuteRows(dgInt32 size, dgInt32 i, dgInt32 j, T* const matrix, T* const choleskyMatrix, T* const x, T* const r, T* const low, T* const high, dgInt16* const permute) +{ + if (i != j) { + T* const A = &matrix[size * i]; + T* const B = &matrix[size * j]; + T* const invA = &choleskyMatrix[size * i]; + T* const invB = &choleskyMatrix[size * j]; + for (dgInt32 k = 0; k < size; k++) { + dgSwap(A[k], B[k]); + dgSwap(invA[k], invB[k]); + } + + dgInt32 stride = 0; + for (dgInt32 k = 0; k < size; k++) { + dgSwap(matrix[stride + i], matrix[stride + j]); + stride += size; + } + + dgSwap(x[i], x[j]); + dgSwap(r[i], r[j]); + dgSwap(low[i], low[j]); + dgSwap(high[i], high[j]); + dgSwap(permute[i], permute[j]); + } +} + +template +DG_INLINE void dgCalculateDelta_x(dgInt32 size, dgInt32 n, const T* const matrix, const T* const choleskyMatrix, T* const delta_x) +{ + const T* const row = &matrix[size * n]; + for (dgInt32 i = 0; i < n; i++) { + delta_x[i] = -row[i]; + } + dgSolveCholesky(size, n, choleskyMatrix, delta_x, delta_x); + delta_x[n] = T(1.0f); +} + +// calculate delta_r = A * delta_x +template +DG_INLINE void dgCalculateDelta_r(dgInt32 size, dgInt32 n, const T* const matrix, const T* const delta_x, T* const delta_r) +{ + dgInt32 stride = n * size; + const dgInt32 size1 = n + 1; + for (dgInt32 i = n; i < size; i++) { + delta_r[i] = dgDotProduct(size1, &matrix[stride], delta_x); + stride += size; + } +} + +template +DG_INLINE void dgHouseholderReflection(dgInt32 size, dgInt32 row, dgInt32 colum, T* const choleskyMatrix, T* const tmp, T* const reflection) +{ + dgAssert(row <= colum); + if (row < colum) { + for (dgInt32 i = row; i <= colum; i++) { + T* const rowI = &choleskyMatrix[size * i]; + T mag2(0.0f); + for (dgInt32 j = i + 1; j <= colum; j++) { + mag2 += rowI[j] * rowI[j]; + reflection[j] = rowI[j]; + } + if (mag2 > T(1.0e-14f)) { + reflection[i] = rowI[i] + dgSign(rowI[i]) * T(sqrt(mag2 + rowI[i] * rowI[i])); + + const T vMag2(mag2 + reflection[i] * reflection[i]); + const T den = T(2.0f) / vMag2; + for (dgInt32 j = i; j < size; j++) { + T acc(0.0f); + T* const rowJ = &choleskyMatrix[size * j]; + for (dgInt32 k = i; k <= colum; k++) { + acc += rowJ[k] * reflection[k]; + } + tmp[j] = acc; + } + + for (dgInt32 j = i + 1; j < size; j++) { + rowI[j] = T(0.0f); + T* const rowJ = &choleskyMatrix[size * j]; + const T a = tmp[j] * den; + for (dgInt32 k = i; k <= colum; k++) { + rowJ[k] -= a * reflection[k]; + } + } + rowI[i] -= tmp[i] * reflection[i] * den; + } + + if (rowI[i] < T(0.0f)) { + for (dgInt32 k = i; k < size; k++) { + choleskyMatrix[size * k + i] = -choleskyMatrix[size * k + i]; + } + } + } + + for (dgInt32 i = row; i < size; i++) { + choleskyMatrix[size * i + i] = dgMax(choleskyMatrix[size * i + i], T(1.0e-6f)); + } + } +} + +template +void dgCholeskyUpdate(dgInt32 size, dgInt32 row, dgInt32 colum, T* const choleskyMatrix, T* const tmp, T* const reflexion, const T* const psdMatrix) +{ + const dgInt32 n0 = colum - row; + const dgInt32 n1 = n0 + 1; + const dgInt32 choleskyCost = size * size * size / 3; + const dgInt32 householdCost = n0 * (n0 + 1) / 2 + n1 * (n1 + 1) * (2 * (2 * n1 + 1) - 3 + 3 * (size - colum - 1)) / 6 - 1; + + if (householdCost < choleskyCost) { + dgHouseholderReflection(size, row, colum, choleskyMatrix, tmp, reflexion); + } else { + memcpy (choleskyMatrix, psdMatrix, sizeof (T) * size * size); + dgCholeskyFactorization(size, choleskyMatrix); + } + +//#if _DEBUG +#if 0 + T* const psdMatrixCopy = dgAlloca(T, size * size); + memcpy(psdMatrixCopy, psdMatrix, sizeof(T) * size * size); + dgCholeskyFactorization(size, psdMatrixCopy); + + for (int i = 0; i < size; i++) { + for (int j = 0; j < size; j++) { + T err = psdMatrixCopy[i*size + j] - choleskyMatrix[i*size + j]; + dgAssert(dgAbs(err) < T(1.0e-4f)); + } + } +#endif +} + +// 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 +void dgSolveDantzigLcpLow(dgInt32 size, T* const symmetricMatrixPSD, T* const x, T* const b, T* const low, T* const high) +{ + T* const x0 = dgAlloca(T, size); + T* const r0 = dgAlloca(T, size); + T* const tmp0 = dgAlloca(T, size); + T* const tmp1 = dgAlloca(T, size); + T* const delta_r = dgAlloca(T, size); + T* const delta_x = dgAlloca(T, size); + T* const lowerTriangularMatrix = dgAlloca(T, size * size); + dgInt16* const permute = dgAlloca(dgInt16, size); + + for (dgInt32 i = 0; i < size; i++) { + permute[i] = dgInt16(i); + x0[i] = T(0.0f); + x[i] = dgMax (b[i] * b[i], T (1.0f)); + } + + for (dgInt32 n = size - 1, i = size - 1; i >= 0; i--) { + if (x[i] > T(1.0)) { + dgPermuteRows(size, n, i, symmetricMatrixPSD, lowerTriangularMatrix, x, b, low, high, permute); + n --; + } + } + + for (dgInt32 i = size - 1; (i >= 0) && (x[i] > T(1.0f)) ; i--) { + dgInt32 min = i; + for (dgInt32 j = i - 1; (j >= 0) && (x[j] > T(1.0f)); j--) { + if (x[j] > x[min]) { + min = j; + } + } + if (min != i) { + dgPermuteRows(size, i, min, symmetricMatrixPSD, lowerTriangularMatrix, x, b, low, high, permute); + } + } + + dgInt32 initialGuessCount = size; + while (x[initialGuessCount - 1] >= T(16.0f)) { + initialGuessCount --; + } + + memcpy(lowerTriangularMatrix, symmetricMatrixPSD, sizeof(T) * size * size); +#ifdef _DEBUG + bool valid = dgCholeskyFactorization(size, lowerTriangularMatrix); + dgAssert(valid); +#else + dgCholeskyFactorization(size, lowerTriangularMatrix); +#endif + for (dgInt32 j = 0; (j != -1) && initialGuessCount;) { + dgSolveCholesky(size, initialGuessCount, lowerTriangularMatrix, x0, b); + + j = -1; + T alpha(1.0f); + T value(0.0f); + for (dgInt32 i = initialGuessCount - 1; i >= 0; i--) { + T x1 = alpha * x0[i]; + if (x1 < low[i]) { + j = i; + value = low[i]; + alpha = low[i] / x0[i]; + } else if (x1 > high[i]) { + j = i; + value = high[i]; + alpha = high[i] / x0[i]; + } + } + + if (j != -1) { + x0[j] = value; + initialGuessCount--; + dgPermuteRows(size, j, initialGuessCount, symmetricMatrixPSD, lowerTriangularMatrix, x0, b, low, high, permute); + dgCholeskyUpdate(size, j, initialGuessCount, lowerTriangularMatrix, tmp0, tmp1, symmetricMatrixPSD); + } + } + + if (initialGuessCount == size) { + for (dgInt32 i = 0; i < size; i++) { + dgInt32 j = permute[i]; + x[j] = x0[i]; + b[i] = T(0.0f); + } + return; + } + + dgInt32 clampedIndex = size; + dgInt32 index = initialGuessCount; + dgInt32 count = size - initialGuessCount; + dgInt32 stride = index * size; + + for (dgInt32 i = 0; i < size; i++) { + r0[i] = T(0.0f); + delta_x[i] = T(0.0f); + delta_r[i] = T(0.0f); + } + + for (dgInt32 i = index; i < size; i++) { + r0[i] = dgDotProduct(size, &symmetricMatrixPSD[stride], x0) - b[i]; + stride += size; + } + + + while (count) { + bool loop = true; + + while (loop) { + loop = false; + T clamp_x(0.0f); + dgInt32 swapIndex = -1; + + if (dgAbs(r0[index]) > T(1.0e-12f)) { + dgCalculateDelta_x(size, index, symmetricMatrixPSD, lowerTriangularMatrix, delta_x); + dgCalculateDelta_r(size, index, symmetricMatrixPSD, delta_x, delta_r); + + dgAssert(delta_r[index] != T(0.0f)); + dgAssert(dgAbs(delta_x[index]) == T(1.0f)); + delta_r[index] = (delta_r[index] == T(0.0f)) ? T(1.0e-12f) : delta_r[index]; + + T scale = -r0[index] / delta_r[index]; + dgAssert(dgAbs(scale) >= T(0.0f)); + + for (dgInt32 i = 0; i <= index; i++) { + T x1 = x0[i] + scale * delta_x[i]; + if (x1 > high[i]) { + swapIndex = i; + clamp_x = high[i]; + scale = (high[i] - x0[i]) / delta_x[i]; + } else if (x1 < low[i]) { + swapIndex = i; + clamp_x = low[i]; + scale = (low[i] - x0[i]) / delta_x[i]; + } + } + dgAssert(dgAbs(scale) >= T(0.0f)); + + for (dgInt32 i = clampedIndex; (i < size) && (scale > T(1.0e-12f)); i++) { + T r1 = r0[i] + scale * delta_r[i]; + if ((r1 * r0[i]) < T(0.0f)) { + dgAssert(dgAbs(delta_r[i]) > T(0.0f)); + T s1 = -r0[i] / delta_r[i]; + dgAssert(dgAbs(s1) >= T(0.0f)); + dgAssert(dgAbs(s1) <= dgAbs(scale)); + if (dgAbs(s1) < dgAbs(scale)) { + scale = s1; + swapIndex = i; + } + } + } + + if (dgAbs(scale) > T(1.0e-12f)) { + for (dgInt32 i = 0; i < size; i++) { + x0[i] += scale * delta_x[i]; + r0[i] += scale * delta_r[i]; + } + } + } + + if (swapIndex == -1) { + r0[index] = T(0.0f); + delta_r[index] = T(0.0f); + index++; + count--; + loop = false; + } else if (swapIndex == index) { + count--; + clampedIndex--; + x0[index] = clamp_x; + dgPermuteRows(size, index, clampedIndex, symmetricMatrixPSD, lowerTriangularMatrix, x0, r0, low, high, permute); + dgCholeskyUpdate(size, index, clampedIndex, lowerTriangularMatrix, tmp0, tmp1, symmetricMatrixPSD); + loop = count ? true : false; + } else if (swapIndex > index) { + loop = true; + r0[swapIndex] = T(0.0f); + dgAssert(swapIndex < size); + dgAssert(clampedIndex <= size); + if (swapIndex < clampedIndex) { + count--; + clampedIndex--; + dgPermuteRows(size, clampedIndex, swapIndex, symmetricMatrixPSD, lowerTriangularMatrix, x0, r0, low, high, permute); + dgCholeskyUpdate(size, swapIndex, clampedIndex, lowerTriangularMatrix, tmp0, tmp1, symmetricMatrixPSD); + dgAssert(clampedIndex >= index); + } else { + count++; + dgAssert(clampedIndex < size); + dgPermuteRows(size, clampedIndex, swapIndex, symmetricMatrixPSD, lowerTriangularMatrix, x0, r0, low, high, permute); + dgCholeskyUpdate(size, clampedIndex, swapIndex, lowerTriangularMatrix, tmp0, tmp1, symmetricMatrixPSD); + clampedIndex++; + dgAssert(clampedIndex <= size); + dgAssert(clampedIndex >= index); + } + + } else { + dgAssert(index > 0); + x0[swapIndex] = clamp_x; + delta_x[index] = T(0.0f); + + dgAssert(swapIndex < index); + dgPermuteRows(size, swapIndex, index - 1, symmetricMatrixPSD, lowerTriangularMatrix, x0, r0, low, high, permute); + dgPermuteRows(size, index - 1, index, symmetricMatrixPSD, lowerTriangularMatrix, x0, r0, low, high, permute); + dgPermuteRows(size, clampedIndex - 1, index, symmetricMatrixPSD, lowerTriangularMatrix, x0, r0, low, high, permute); + dgCholeskyUpdate (size, swapIndex, clampedIndex - 1, lowerTriangularMatrix, tmp0, tmp1, symmetricMatrixPSD); + + clampedIndex--; + index--; + loop = true; + } + } + } + + for (dgInt32 i = 0; i < size; i++) { + dgInt32 j = permute[i]; + x[j] = x0[i]; + b[j] = r0[i]; + } +} + +/* +// 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 +bool dgSolveDantzigLCP(dgInt32 size, T* const symetricMatrix, T* const x, T* const b, T* const low, T* const high) +{ + T* const choleskyMatrix = dgAlloca(T, size * size); + dgCheckAligment(choleskyMatrix); + + memcpy (choleskyMatrix, symetricMatrix, sizeof (T) * size * size); + dgCholeskyFactorization(size, choleskyMatrix); + for (dgInt32 i = 0; i < size; i ++) { + T* const row = &choleskyMatrix[i * size]; + for (dgInt32 j = i + 1; j < size; j ++) { + row[j] = T(0.0f); + } + } + return dgSolveDantzigLCP(size, symetricMatrix, choleskyMatrix, x, b, low, high); +} +*/ + +// 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 +bool dgSolvePartitionDantzigLCP(dgInt32 size, T* const symmetricMatrixPSD , T* const x, T* const b, T* const low, T* const high) +{ + dgInt16* const permute = dgAlloca(dgInt16, size); + + for (dgInt32 i = 0; i < size; i++) { + x[i] = b[i]; + permute[i] = dgInt16(i); + } + + dgInt32 unboundedSize = size; + for (dgInt32 i = 0; i < unboundedSize; i++) { + if ((low[i] <= T(-DG_LCP_MAX_VALUE)) && (high[i] >= T(DG_LCP_MAX_VALUE))) { + dgCholeskyFactorizationAddRow(size, i, symmetricMatrixPSD ); + } else { + dgInt32 j = unboundedSize - 1; + if (i != j) { + T* const A = &symmetricMatrixPSD [size * i]; + T* const B = &symmetricMatrixPSD [size * j]; + for (dgInt32 k = 0; k < size; k++) { + dgSwap(A[k], B[k]); + } + + dgInt32 stride = 0; + for (dgInt32 k = 0; k < size; k++) { + dgSwap(symmetricMatrixPSD [stride + i], symmetricMatrixPSD [stride + j]); + stride += size; + } + dgSwap(x[i], x[j]); + dgSwap(b[i], b[j]); + dgSwap(low[i], low[j]); + dgSwap(high[i], high[j]); + dgSwap(permute[i], permute[j]); + } + + i--; + unboundedSize--; + } + } + + bool ret = false; + if (unboundedSize > 0) { + dgSolveCholesky(size, unboundedSize, symmetricMatrixPSD , x); + dgInt32 base = unboundedSize * size; + for (dgInt32 i = unboundedSize; i < size; i++) { + b[i] -= dgDotProduct(unboundedSize, &symmetricMatrixPSD[base], x); + base += size; + } + + const dgInt32 boundedSize = size - unboundedSize; + T* const l = dgAlloca(T, boundedSize); + T* const h = dgAlloca(T, boundedSize); + T* const c = dgAlloca(T, boundedSize); + T* const u = dgAlloca(T, boundedSize); + T* const a11 = dgAlloca(T, boundedSize * boundedSize); + T* const a10 = dgAlloca(T, boundedSize * unboundedSize); + + for (dgInt32 i = 0; i < boundedSize; i++) { + T* const g = &a10[i * unboundedSize]; + const T* const row = &symmetricMatrixPSD [(unboundedSize + i) * size]; + for (dgInt32 j = 0; j < unboundedSize; j++) { + g[j] = -row[j]; + } + dgSolveCholesky(size, unboundedSize, symmetricMatrixPSD, g); + + T* const arow = &a11[i * boundedSize]; + const T* const row2 = &symmetricMatrixPSD[(unboundedSize + i) * size]; + arow[i] = row2[unboundedSize + i] + dgDotProduct(unboundedSize, g, row2); + for (dgInt32 j = i + 1; j < boundedSize; j++) { + const T* const row1 = &symmetricMatrixPSD [(unboundedSize + j) * size]; + T elem = row1[unboundedSize + i] + dgDotProduct(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 (dgSolveDantzigLCP(boundedSize, a11, u, c, l, h)) { + for (dgInt32 i = 0; i < boundedSize; i++) { + const T s = u[i]; + x[unboundedSize + i] = s; + const T* const g = &a10[i * unboundedSize]; + for (dgInt32 j = 0; j < unboundedSize; j++) { + x[j] += g[j] * s; + } + } + ret = true; + } + } else { + for (dgInt32 i = 0; i < size; i++) { + x[i] = T(0.0f); + } + ret = dgSolveDantzigLCP(size, symmetricMatrixPSD, x, b, low, high); + } + + for (dgInt32 i = 0; i < size; i++) { + b[i] = x[i]; + } + for (dgInt32 i = 0; i < size; i++) { + dgInt32 j = permute[i]; + x[j] = b[i]; + b[i] = T(0.0f); + } + return ret; +} + + +template +void dgSolveDantzigLCP(dgInt32 size, T* const symmetricMatrixPSD, T* const x, T* const b, T* const low, T* const high) +{ + T tol2 = T(0.25f * 0.25f); + dgInt32 passes = dgClamp(size, 12, 20); + T* const r = dgAlloca(T, size); + dgInt16* const clipped = dgAlloca(dgInt16, size); + + // find an approximation to the solution + dgGaussSeidelLcpSor(size, symmetricMatrixPSD, x, b, low, high, tol2, passes, clipped, T(1.3f)); + + T err2(0.0f); + dgInt32 stride = 0; + dgInt32 clippeCount = 0; + for (dgInt32 i = 0; i < size; i++) { + const T* const row = &symmetricMatrixPSD[stride]; + r[i] = b[i] - dgDotProduct(size, row, x); + clippeCount += clipped[i]; + err2 += clipped[i] ? T(0.0f) : r[i] * r[i]; + stride += size; + } + + if (err2 > tol2) { + // check for small lcp + if ((clippeCount < 16) && ((clippeCount < 32) && (err2 < T(16.0f)))) { + // small lcp can be solved with direct method + T* const x0 = dgAlloca(T, size); + for (dgInt32 i = 0; i < size; i++) { + low[i] -= x[i]; + high[i] -= x[i]; + } + dgSolveDantzigLcpLow(size, symmetricMatrixPSD, x0, r, low, high); + for (dgInt32 i = 0; i < size; i++) { + x[i] += x0[i]; + } + } else { + // larger lcp are too hard for direct method, see if we can get better approximation + dgGaussSeidelLcpSor(size, symmetricMatrixPSD, x, b, low, high, tol2, 20, clipped, T(1.3f)); + } + } +} + +#endif diff --git a/thirdparty/src/newton/dgCore/dgGeneralVector.cpp b/thirdparty/src/newton/dgCore/dgGeneralVector.cpp new file mode 100644 index 000000000..adfec7c57 --- /dev/null +++ b/thirdparty/src/newton/dgCore/dgGeneralVector.cpp @@ -0,0 +1,25 @@ +/* Copyright (c) <2003-2019> +* +* 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" + + diff --git a/thirdparty/src/newton/dgCore/dgGeneralVector.h b/thirdparty/src/newton/dgCore/dgGeneralVector.h new file mode 100644 index 000000000..a8d18e031 --- /dev/null +++ b/thirdparty/src/newton/dgCore/dgGeneralVector.h @@ -0,0 +1,89 @@ +/* Copyright (c) <2003-2019> +* +* 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 +DG_INLINE T dgSQRH(const T num, const T den) +{ + T r(num / den); + return T(sqrt(T(1.0f) + r * r)); +} + +template +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 +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 +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 +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 +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 +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 + + diff --git a/thirdparty/src/newton/dgCore/dgGoogol.cpp b/thirdparty/src/newton/dgCore/dgGoogol.cpp new file mode 100644 index 000000000..7b326dd44 --- /dev/null +++ b/thirdparty/src/newton/dgCore/dgGoogol.cpp @@ -0,0 +1,584 @@ +/* Copyright (c) <2003-2019> +* +* 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))); +} + + + + + + + diff --git a/thirdparty/src/newton/dgCore/dgGoogol.h b/thirdparty/src/newton/dgCore/dgGoogol.h new file mode 100644 index 000000000..95518b5e7 --- /dev/null +++ b/thirdparty/src/newton/dgCore/dgGoogol.h @@ -0,0 +1,144 @@ +/* Copyright (c) <2003-2019> +* +* 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 +{ + public: + dgHugeVector () + :dgTemplateVector() + { + } + + dgHugeVector (const dgBigVector& a) + :dgTemplateVector(dgGoogol (a.m_x), dgGoogol (a.m_y), dgGoogol (a.m_z), dgGoogol (a.m_w)) + { + } + + dgHugeVector (const dgTemplateVector& a) + :dgTemplateVector(a) + { + } + + dgHugeVector (dgFloat64 x, dgFloat64 y, dgFloat64 z, dgFloat64 w) + :dgTemplateVector(x, y, z, w) + { + } + + dgHugeVector(const dgGoogol& x, const dgGoogol& y, const dgGoogol& z, const dgGoogol& w) + :dgTemplateVector(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 diff --git a/thirdparty/src/newton/dgCore/dgGraph.h b/thirdparty/src/newton/dgCore/dgGraph.h new file mode 100644 index 000000000..db1b06e48 --- /dev/null +++ b/thirdparty/src/newton/dgCore/dgGraph.h @@ -0,0 +1,191 @@ +/* Copyright (c) <2003-2019> +* +* 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 dgGraphEdge; +template class dgGraphNode; + + +template +class dgGraph: public dgList > +{ + public: + dgGraph (dgMemoryAllocator* const allocator); + ~dgGraph (); + + typename dgGraph::dgListNode* AddNode (); + void DeleteNode (typename dgGraph::dgListNode* const node); + + typename dgGraph::dgListNode* GetNodeFromNodeData(dgNodeData* const nodeData); + + //void Trace () const; +}; + +template +class dgGraphNode: public dgList > +{ + public: + dgGraphNode (); + ~dgGraphNode (); + + typename dgGraphNode::dgListNode* AddEdge(typename dgGraph::dgListNode* const node); + void DeleteHalfEdge(typename dgGraphNode::dgListNode* const edge); + void DeleteEdge(typename dgGraphNode::dgListNode* const edge); + + //void Trace () const; + + dgNodeData m_nodeData; +}; + +template +class dgGraphEdge +{ + public: + dgGraphEdge(); + ~dgGraphEdge(); + + typename dgGraph::dgListNode* m_node; + dgEdgeData m_edgeData; +}; + + + +template +dgGraph::dgGraph (dgMemoryAllocator* const allocator) + :dgList >(allocator) +{ +} + + +template +dgGraph::~dgGraph () +{ +} + +template +typename dgGraph::dgListNode* dgGraph::AddNode () +{ + typename dgGraph::dgListNode* const node = dgGraph::Append(); + + node->GetInfo().SetAllocator(dgGraph::GetAllocator()); + + return node; +} + +template +void dgGraph::DeleteNode (typename dgGraph::dgListNode* const node) +{ + for (typename dgGraphNode::dgListNode* link = node->GetInfo().GetFirst(); link; link = link->GetNext()) { + typename dgGraph::dgListNode* const twinNode = link->GetInfo().m_node; + for (typename dgGraphNode::dgListNode* link1 = twinNode->GetInfo().GetFirst(); link1; link1 = link1->GetNext()) { + if (link1->GetInfo().m_node == node) { + twinNode->GetInfo().Remove (link1); + break; + } + } + } + dgList >::Remove (node); +} + +template +typename dgGraph::dgListNode* dgGraph::GetNodeFromNodeData(dgNodeData* const nodeData) +{ + dgInt32 size1 = sizeof (*nodeData); + dgInt32 size0 = sizeof (dgGraphNode); + void* const ptr = ((char*)nodeData) - (size0 - size1); + dgGraphNode* const nodeInfo = (dgGraphNode*) ptr; + return this->GetNodeFromInfo(*nodeInfo); +} + + +template +dgGraphNode::dgGraphNode() + :dgList >(NULL) +{ + +} + + +template +dgGraphNode::~dgGraphNode() +{ + +} + +template +typename dgGraphNode::dgListNode* dgGraphNode::AddEdge (typename dgGraph::dgListNode* const node) +{ + typename dgGraphNode::dgListNode* const edge = dgGraphNode::Append(); + + edge->GetInfo().m_node = node; + return edge; +} + +template +void dgGraphNode::DeleteHalfEdge(typename dgGraphNode::dgListNode* const edge) +{ + dgList >::Remove (edge); +} + +template +void dgGraphNode::DeleteEdge(typename dgGraphNode::dgListNode* const edge) +{ + typename dgGraph::dgListNode* const node = edge->GetInfo().m_node; + + for (typename dgGraphNode::dgListNode* twinEdge = node->GetInfo().GetFirst(); twinEdge; twinEdge = twinEdge->GetNext()) { + if (&twinEdge->GetInfo().m_node->GetInfo() == this) { + node->GetInfo().DeleteHalfEdge(twinEdge); + break; + } + } + + DeleteHalfEdge(edge); +} + + +template +dgGraphEdge::dgGraphEdge() +{ + +} + +template +dgGraphEdge::~dgGraphEdge() +{ + +} + + + +#endif + diff --git a/thirdparty/src/newton/dgCore/dgHeap.h b/thirdparty/src/newton/dgCore/dgHeap.h new file mode 100644 index 000000000..f8b91c473 --- /dev/null +++ b/thirdparty/src/newton/dgCore/dgHeap.h @@ -0,0 +1,435 @@ +/* Copyright (c) <2003-2019> +* +* 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 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 dgDownHeap: public dgHeapBase +{ + 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 dgUpHeap: public dgHeapBase +{ + 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 +dgHeapBase::dgHeapBase (dgInt32 maxElements, dgMemoryAllocator* const allocator) +{ + m_allocator = allocator; + m_pool = (RECORD *)m_allocator->Malloc (maxElements * sizeof (RECORD)); + m_maxCount = maxElements; + Flush(); +} + +template +dgHeapBase::dgHeapBase (const void * const buffer, dgInt32 sizeInBytes) +{ + m_allocator = NULL; + m_pool = (RECORD *) buffer; + m_maxCount = dgInt32 (sizeInBytes / sizeof (RECORD)); + Flush(); +} + +template +dgHeapBase::~dgHeapBase () +{ + if (m_allocator) { + m_allocator->Free (m_pool); + } +} + + +template +KEY dgHeapBase::Value(dgInt32 i) const +{ + return m_pool[i].m_key; +} + + +template +dgInt32 dgHeapBase::GetCount() const +{ + return m_curCount; +} + +template +void dgHeapBase::Flush () +{ + m_curCount = 0; + + #ifdef _DEBUG +// dgHeapBase::m_pool[dgHeapBase::m_curCount].m_key = KEY (0); + #endif +} + +template +KEY dgHeapBase::MaxValue() const +{ + return m_pool[0].m_key; +} + +template +dgInt32 dgHeapBase::GetMaxCount() const +{ + return m_maxCount; +} + + +template +dgInt32 dgHeapBase::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 +dgInt32 dgHeapBase::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 +const OBJECT& dgHeapBase::operator[] (dgInt32 i) const +{ + dgAssert (i<= m_curCount); + return m_pool[i].m_obj; +} + + +// ************************************************************************** +// +// down Heap +// +// ************************************************************************** +template +dgDownHeap::dgDownHeap (dgInt32 maxElements, dgMemoryAllocator* const allocator) + :dgHeapBase (maxElements, allocator) +{ +} + +template +dgDownHeap::dgDownHeap (const void * const buffer, dgInt32 sizeInBytes) + :dgHeapBase (buffer, sizeInBytes) +{ +} + + +template +void dgDownHeap::Push (OBJECT &obj, KEY key) +{ + dgHeapBase::m_curCount ++; + + dgInt32 j; + dgInt32 i = dgHeapBase::m_curCount; + for (; i; i = j) { + j = i >> 1; + if (!j || (dgHeapBase::m_pool[j - 1].m_key > key)) { + break; + } + dgHeapBase::m_pool[i - 1] = dgHeapBase::m_pool[j - 1]; + } + dgAssert (i); + dgHeapBase::m_pool[i - 1].m_key = key; + dgHeapBase::m_pool[i - 1].m_obj = obj; + + dgAssert (SanityCheck()); +} + +template +void dgDownHeap::Remove (dgInt32 index) +{ + dgHeapBase::m_curCount--; + dgHeapBase::m_pool[index] = dgHeapBase::m_pool[dgHeapBase::m_curCount]; + while (index && dgHeapBase::m_pool[(index - 1) >> 1].m_key < dgHeapBase::m_pool[index].m_key) { + dgSwap(dgHeapBase::m_pool[(index - 1) >> 1], dgHeapBase::m_pool[index]); + index = (index - 1) >> 1; + } + + while ((2 * index + 1) < dgHeapBase::m_curCount) { + dgInt32 i0 = 2 * index + 1; + dgInt32 i1 = 2 * index + 2; + if (i1 < dgHeapBase::m_curCount) { + i0 = (dgHeapBase::m_pool[i0].m_key > dgHeapBase::m_pool[i1].m_key) ? i0 : i1; + if (dgHeapBase::m_pool[i0].m_key <= dgHeapBase::m_pool[index].m_key) { + break; + } + dgSwap(dgHeapBase::m_pool[i0], dgHeapBase::m_pool[index]); + index = i0; + } else { + if (dgHeapBase::m_pool[i0].m_key > dgHeapBase::m_pool[index].m_key) { + dgSwap(dgHeapBase::m_pool[i0], dgHeapBase::m_pool[index]); + } + index = i0; + } + } + dgAssert (SanityCheck()); +} + +template +void dgDownHeap::Sort () +{ + dgInt32 count = dgHeapBase::m_curCount; + for (dgInt32 i = 1; i < count; i ++) { + KEY key (dgHeapBase::m_pool[0].m_key); + OBJECT obj (dgHeapBase::m_pool[0].m_obj); + + Pop(); + + dgHeapBase::m_pool[dgHeapBase::m_curCount].m_key = key; + dgHeapBase::m_pool[dgHeapBase::m_curCount].m_obj = obj; + } + + dgHeapBase::m_curCount = count; + for (dgInt32 i = 0; i < count / 2; i ++) { + KEY key (dgHeapBase::m_pool[i].m_key); + OBJECT obj (dgHeapBase::m_pool[i].m_obj); + + dgHeapBase::m_pool[i].m_key = dgHeapBase::m_pool[count - i - 1].m_key; + dgHeapBase::m_pool[i].m_obj = dgHeapBase::m_pool[count - i - 1].m_obj; + + dgHeapBase::m_pool[count - i - 1].m_key = key; + dgHeapBase::m_pool[count - i - 1].m_obj = obj; + } + dgAssert (SanityCheck()); +} + +template +bool dgDownHeap::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::m_pool[i].m_key < dgHeapBase::m_pool[i1].m_key)) { + return false; + } + if ((i2 < m_curCount) && (dgHeapBase::m_pool[i].m_key < dgHeapBase::m_pool[i2].m_key)) { + return false; + } + } +#endif + return true; +} + +// ************************************************************************** +// +// down Heap +// +// ************************************************************************** +template +dgUpHeap::dgUpHeap (dgInt32 maxElements, dgMemoryAllocator* const allocator) + :dgHeapBase (maxElements, allocator) +{ +} + +template +dgUpHeap::dgUpHeap (const void * const buffer, dgInt32 sizeInBytes) + :dgHeapBase (buffer, sizeInBytes) +{ +} + +template +bool dgUpHeap::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::m_pool[i].m_key > dgHeapBase::m_pool[i1].m_key)) { + return false; + } + if ((i2 < m_curCount) && (dgHeapBase::m_pool[i].m_key > dgHeapBase::m_pool[i2].m_key)) { + return false; + } + } +#endif + return true; +} + +template +void dgUpHeap::Push (OBJECT &obj, KEY key) +{ + dgHeapBase::m_curCount ++; + + dgInt32 j; + dgInt32 i = dgHeapBase::m_curCount; + for (; i; i = j) { + j = i >> 1; + if (!j || (dgHeapBase::m_pool[j - 1].m_key < key)) { + break; + } + dgHeapBase::m_pool[i - 1] = dgHeapBase::m_pool[j - 1]; + } + dgAssert (i); + dgHeapBase::m_pool[i - 1].m_key = key; + dgHeapBase::m_pool[i - 1].m_obj = obj; + dgAssert (SanityCheck()); +} + +template +void dgUpHeap::Sort () +{ + dgInt32 count = dgHeapBase::m_curCount; + for (dgInt32 i = 1; i < count; i ++) { + KEY key (dgHeapBase::m_pool[0].m_key); + OBJECT obj (dgHeapBase::m_pool[0].m_obj); + + Pop(); + + dgHeapBase::m_pool[dgHeapBase::m_curCount].m_key = key; + dgHeapBase::m_pool[dgHeapBase::m_curCount].m_obj = obj; + } + + dgHeapBase::m_curCount = count; + for (dgInt32 i = 0; i < count / 2; i ++) { + KEY key (dgHeapBase::m_pool[i].m_key); + OBJECT obj (dgHeapBase::m_pool[i].m_obj); + + dgHeapBase::m_pool[i].m_key = dgHeapBase::m_pool[count - i - 1].m_key; + dgHeapBase::m_pool[i].m_obj = dgHeapBase::m_pool[count - i - 1].m_obj; + + dgHeapBase::m_pool[count - i - 1].m_key = key; + dgHeapBase::m_pool[count - i - 1].m_obj = obj; + } + dgAssert (SanityCheck()); +} + +template +void dgUpHeap::Remove (dgInt32 index) +{ + dgHeapBase::m_curCount--; + dgHeapBase::m_pool[index] = dgHeapBase::m_pool[dgHeapBase::m_curCount]; + while (index && dgHeapBase::m_pool[(index - 1) >> 1].m_key > dgHeapBase::m_pool[index].m_key) { + dgSwap(dgHeapBase::m_pool[(index - 1) >> 1], dgHeapBase::m_pool[index]); + index = (index - 1) >> 1; + } + + while ((2 * index + 1) < dgHeapBase::m_curCount) { + dgInt32 i0 = 2 * index + 1; + dgInt32 i1 = 2 * index + 2; + if (i1 < dgHeapBase::m_curCount) { + i0 = (dgHeapBase::m_pool[i0].m_key < dgHeapBase::m_pool[i1].m_key) ? i0 : i1; + if (dgHeapBase::m_pool[i0].m_key >= dgHeapBase::m_pool[index].m_key) { + break; + } + dgSwap(dgHeapBase::m_pool[i0], dgHeapBase::m_pool[index]); + index = i0; + } else { + if (dgHeapBase::m_pool[i0].m_key < dgHeapBase::m_pool[index].m_key) { + dgSwap(dgHeapBase::m_pool[i0], dgHeapBase::m_pool[index]); + } + index = i0; + } + } + dgAssert (SanityCheck()); +} + + +#endif \ No newline at end of file diff --git a/thirdparty/src/newton/dgCore/dgIntersections.cpp b/thirdparty/src/newton/dgCore/dgIntersections.cpp new file mode 100644 index 000000000..43712d778 --- /dev/null +++ b/thirdparty/src/newton/dgCore/dgIntersections.cpp @@ -0,0 +1,400 @@ +/* Copyright (c) <2003-2019> +* +* 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; + +} + diff --git a/thirdparty/src/newton/dgCore/dgIntersections.h b/thirdparty/src/newton/dgCore/dgIntersections.h new file mode 100644 index 000000000..1e2d5af59 --- /dev/null +++ b/thirdparty/src/newton/dgCore/dgIntersections.h @@ -0,0 +1,393 @@ +/* Copyright (c) <2003-2019> +* +* 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 + diff --git a/thirdparty/src/newton/dgCore/dgList.h b/thirdparty/src/newton/dgCore/dgList.h new file mode 100644 index 000000000..137efc321 --- /dev/null +++ b/thirdparty/src/newton/dgCore/dgList.h @@ -0,0 +1,701 @@ +/* Copyright (c) <2003-2019> +* +* 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 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; + + }; + + class Iterator + { + public: + Iterator (const dgList &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& 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 +dgList::dgList () +{ + m_count = 0; + m_first = NULL; + m_last = NULL; + m_allocator = NULL; +} +*/ + +template +dgList::dgList (dgMemoryAllocator* const allocator) + :m_count(0) + ,m_first(NULL) + ,m_last(NULL) + ,m_allocator(allocator) +{ +} + + +template +dgList::~dgList () +{ + RemoveAll (); +} + +template +void dgList::SetAllocator (dgMemoryAllocator* const allocator) +{ + if ((m_count == 0) && (m_allocator == NULL)) { + m_allocator = allocator; + } +} + +template +dgMemoryAllocator* dgList::GetAllocator () const +{ + return m_allocator; +} + + +template +dgInt32 dgList::GetCount() const +{ + return m_count; +} + +template +dgList::operator dgInt32() const +{ + return m_first != NULL; +} + +template +typename dgList::dgListNode *dgList::GetFirst() const +{ + return m_first; +} + +template +typename dgList::dgListNode *dgList::GetLast() const +{ + return m_last; +} + +template +typename dgList::dgListNode *dgList::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 +typename dgList::dgListNode *dgList::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 +typename dgList::dgListNode *dgList::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 +typename dgList::dgListNode *dgList::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 +typename dgList::dgListNode *dgList::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 +typename dgList::dgListNode *dgList::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 +typename dgList::dgListNode *dgList::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 +void dgList::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 +void dgList::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 +void dgList::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 +void dgList::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 +typename dgList::dgListNode *dgList::Find (const T &element) const +{ + dgListNode *node; + for (node = m_first; node; node = node->GetNext()) { + if (element == node->m_info) { + break; + } + } + return node; +} + +template +typename dgList::dgListNode *dgList::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 +void dgList::Remove (const T &element) +{ + dgListNode *const node = Find (element); + if (node) { + Remove (node); + } +} + +template +void dgList::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 +void dgList::Merge (dgList& 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 +void dgList::Remove (dgListNode* const node) +{ + Unlink (node); + delete node; +} + + +template +void dgList::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 +bool dgList::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 +//void dgList::SetAllocator (dgMemoryAllocator * allocator) +//{ +// m_allocator = allocator; +//} + +//template +//dgMemoryAllocator* dgList::GetAllocator () const +//{ +// return m_allocator; +//} +//template dgInt32 dgList ::m_size = 0; +//template dgMemoryAllocator* dgList::m_staticAllocator = NULL; + + +#endif + + diff --git a/thirdparty/src/newton/dgCore/dgMatrix.cpp b/thirdparty/src/newton/dgCore/dgMatrix.cpp new file mode 100644 index 000000000..4f86fb2d1 --- /dev/null +++ b/thirdparty/src/newton/dgCore/dgMatrix.cpp @@ -0,0 +1,778 @@ +/* Copyright (c) <2003-2019> +* +* 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)) 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; +} diff --git a/thirdparty/src/newton/dgCore/dgMatrix.h b/thirdparty/src/newton/dgCore/dgMatrix.h new file mode 100644 index 000000000..38b36cff4 --- /dev/null +++ b/thirdparty/src/newton/dgCore/dgMatrix.h @@ -0,0 +1,380 @@ +/* Copyright (c) <2003-2019> +* +* 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 + +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 + diff --git a/thirdparty/src/newton/dgCore/dgMemory.cpp b/thirdparty/src/newton/dgCore/dgMemory.cpp new file mode 100644 index 000000000..4fe5be585 --- /dev/null +++ b/thirdparty/src/newton/dgCore/dgMemory.cpp @@ -0,0 +1,781 @@ +/* Copyright (c) <2003-2019> +* +* 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 +{ + public: + dgGlobalAllocator () + :dgMemoryAllocator (__malloc__, __free__), dgList (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::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(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 \ No newline at end of file diff --git a/thirdparty/src/newton/dgCore/dgMemory.h b/thirdparty/src/newton/dgCore/dgMemory.h new file mode 100644 index 000000000..62ecb9897 --- /dev/null +++ b/thirdparty/src/newton/dgCore/dgMemory.h @@ -0,0 +1,352 @@ +/* Copyright (c) <2003-2019> +* +* 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(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 + diff --git a/thirdparty/src/newton/dgCore/dgMutexThread.cpp b/thirdparty/src/newton/dgCore/dgMutexThread.cpp new file mode 100644 index 000000000..bf6a3db84 --- /dev/null +++ b/thirdparty/src/newton/dgCore/dgMutexThread.cpp @@ -0,0 +1,124 @@ +/* Copyright (c) <2003-2019> +* +* 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); + } + } +} \ No newline at end of file diff --git a/thirdparty/src/newton/dgCore/dgMutexThread.h b/thirdparty/src/newton/dgCore/dgMutexThread.h new file mode 100644 index 000000000..725f47c4f --- /dev/null +++ b/thirdparty/src/newton/dgCore/dgMutexThread.h @@ -0,0 +1,66 @@ +/* Copyright (c) <2003-2019> +* +* 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 \ No newline at end of file diff --git a/thirdparty/src/newton/dgCore/dgNode.cpp b/thirdparty/src/newton/dgCore/dgNode.cpp new file mode 100644 index 000000000..02dc635e0 --- /dev/null +++ b/thirdparty/src/newton/dgCore/dgNode.cpp @@ -0,0 +1,231 @@ +/* Copyright (c) <2003-2019> +* +* 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); +*/ +} + + + + + + diff --git a/thirdparty/src/newton/dgCore/dgNode.h b/thirdparty/src/newton/dgCore/dgNode.h new file mode 100644 index 000000000..3b3bd07b2 --- /dev/null +++ b/thirdparty/src/newton/dgCore/dgNode.h @@ -0,0 +1,262 @@ +/* Copyright (c) <2003-2019> +* +* 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 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 +dgNode::dgNode () + :dgBaseNode () +{ +} + +template +dgNode::dgNode (const T &clone) + :dgBaseNode (clone) +{ +} + +template +dgNode::dgNode (const char* const name) + :dgBaseNode (name) +{ +} + +template +dgNode::~dgNode () +{ +} + + +template +dgRef *dgNode::CreateClone () const +{ + return new T (*(T*)this); +} + +template +void dgNode::Attach (T* const parent, bool addFirst) +{ + dgBaseNode::Attach(parent, addFirst); +} + +template +void dgNode::Detach () +{ + dgBaseNode::Detach (); +} + +template +T* dgNode::GetChild () const +{ + return (T*) dgBaseNode::GetChild(); +} + +template +T* dgNode::GetSibling () const +{ + return (T*) dgBaseNode::GetSibling (); +} + +template +T* dgNode::GetParent () const +{ + return (T*) dgBaseNode::GetParent (); +} + + +template +T* dgNode::GetRoot () const +{ + return (T*) dgBaseNode::GetRoot (); +} + + +template +T* dgNode::GetFirst() const +{ + return (T*) dgBaseNode::GetFirst (); +} + +template +T* dgNode::GetLast() const +{ + return (T*) dgBaseNode::GetLast (); +} + + +template +T* dgNode::GetNext() const +{ + return (T*) dgBaseNode::GetNext (); +} + +template +T* dgNode::GetPrev() const +{ + return (T*) dgBaseNode::GetPrev (); +} + + +template +T* dgNode::Find (dgUnsigned32 nameCRC) const +{ + return (T*) dgBaseNode::Find (nameCRC); +} + +template +T* dgNode::Find (const char* const name) const +{ + return (T*) dgBaseNode::Find (name); +} + + + + +#endif + diff --git a/thirdparty/src/newton/dgCore/dgObb.cpp b/thirdparty/src/newton/dgCore/dgObb.cpp new file mode 100644 index 000000000..3b12f2201 --- /dev/null +++ b/thirdparty/src/newton/dgCore/dgObb.cpp @@ -0,0 +1,868 @@ +/* Copyright (c) <2003-2019> +* +* 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(); +} +*/ + diff --git a/thirdparty/src/newton/dgCore/dgObb.h b/thirdparty/src/newton/dgCore/dgObb.h new file mode 100644 index 000000000..4dfdeff36 --- /dev/null +++ b/thirdparty/src/newton/dgCore/dgObb.h @@ -0,0 +1,115 @@ +/* Copyright (c) <2003-2019> +* +* 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 + diff --git a/thirdparty/src/newton/dgCore/dgPathFinder.h b/thirdparty/src/newton/dgCore/dgPathFinder.h new file mode 100644 index 000000000..b5e321fe9 --- /dev/null +++ b/thirdparty/src/newton/dgCore/dgPathFinder.h @@ -0,0 +1,226 @@ +/* Copyright (c) <2003-2019> +* +* 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 dgPathFinder; + + +#define DG_MAX_PATH_ENUMERATION_CHILDREN 128 + +template +class dgPathNode +{ + friend class dgPathFinder; + 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 dgPathCloseList: public dgTree, NODEID> +{ + protected: + dgPathCloseList(): dgTree, NODEID>() {} +}; + + +template +class dgPathOpenHeap: public dgUpHeap::dgTreeNode*, COST> +{ + protected: + dgPathOpenHeap(dgInt32 maxElements) + :dgUpHeap::dgTreeNode*, COST>(maxElements) + { + } + + friend class dgPathFinder; +}; + + +template +class dgPathFinder: public dgPathCloseList +{ + dgInt32 maxChildren; + + public: + dgPathFinder(dgInt32 maxElementsInOpenList, dgInt32 nodeMaxChildren); + virtual ~dgPathFinder(); + + virtual const dgPathNode* CalCulatePath (NODEID source, NODEID goal); + + // this funtions must be overloaded by the user + virtual COST GetCostFromParent(const dgPathNode& node) const; + virtual COST GetEstimatedCostToGoal(NODEID id) const; + virtual dgInt32 EnumerateChildren(NODEID parent, NODEID array[]) const; + + dgPathOpenHeap m_openList; +}; + + +template +dgPathNode::dgPathNode () +{ +} + +template +NODEID dgPathNode::GetId () const +{ + return m_Id; +} + + +template +const dgPathNode* dgPathNode::GetNext() const +{ + return m_Next; +} + + +template +const dgPathNode* dgPathNode::GetParent() const +{ + return m_Next; +} + + +template +dgPathFinder::dgPathFinder( + dgInt32 maxElementsInOpenList, + dgInt32 nodeMaxChildren) + :dgPathCloseList(), + m_openList(maxElementsInOpenList) +{ + maxChildren = nodeMaxChildren; +} + + +template +dgPathFinder::~dgPathFinder() +{ +} + +template +const dgPathNode* dgPathFinder::CalCulatePath (NODEID source, NODEID goal) +{ + dgInt32 count; + dgInt32 heapMaxCount; + dgPathNode cell; + dgPathNode* next; + dgPathNode* prev; + dgPathNode* link; + typename dgPathCloseList::dgTreeNode* node; + NODEID idArray[DG_MAX_PATH_ENUMERATION_CHILDREN]; + + dgPathCloseList& 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& 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& 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 +COST dgPathFinder::GetCostFromParent(const dgPathNode& node) const +{ + return COST (1); +} + +template +COST dgPathFinder::GetEstimatedCostToGoal(NODEID id) const +{ + return COST (1); +} + +template +dgInt32 dgPathFinder::EnumerateChildren(NODEID parent, NODEID array[]) const +{ + return 0; +} + + + + +#endif + + + diff --git a/thirdparty/src/newton/dgCore/dgPlane.h b/thirdparty/src/newton/dgCore/dgPlane.h new file mode 100644 index 000000000..3e5883b0f --- /dev/null +++ b/thirdparty/src/newton/dgCore/dgPlane.h @@ -0,0 +1,159 @@ +/* Copyright (c) <2003-2019> +* +* 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 + + diff --git a/thirdparty/src/newton/dgCore/dgPolygonSoupBuilder.cpp b/thirdparty/src/newton/dgCore/dgPolygonSoupBuilder.cpp new file mode 100644 index 000000000..44dbca865 --- /dev/null +++ b/thirdparty/src/newton/dgCore/dgPolygonSoupBuilder.cpp @@ -0,0 +1,1090 @@ +/* Copyright (c) <2003-2019> +* +* 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 "dgStack.h" +#include "dgMatrix.h" +#include "dgMemory.h" +#include "dgPolyhedra.h" +#include "dgPolygonSoupBuilder.h" + +#define DG_POINTS_RUN (512 * 1024) + + + +class dgPolygonSoupDatabaseBuilder::dgFaceInfo +{ + public: + dgInt32 indexCount; + dgInt32 indexStart; +}; + +class dgPolygonSoupDatabaseBuilder::dgFaceBucket: public dgList +{ + public: + dgFaceBucket (dgMemoryAllocator* const allocator) + :dgList(allocator) + { + } +}; + +class dgPolygonSoupDatabaseBuilder::dgFaceMap: public dgTree +{ + public: + dgFaceMap (dgMemoryAllocator* const allocator, dgPolygonSoupDatabaseBuilder& builder) + :dgTree(allocator) + { + dgInt32 polygonIndex = 0; + dgInt32 faceCount = builder.m_faceCount; + const dgInt32* const faceVertexCounts = &builder.m_faceVertexCount[0]; + const dgInt32* const faceVertexIndex = &builder.m_vertexIndex[0]; + for (dgInt32 i = 0; i < faceCount; i ++) { + dgInt32 count = faceVertexCounts[i]; + dgInt32 attribute = faceVertexIndex[polygonIndex + count - 1]; + + dgTreeNode* node = Find(attribute); + if (!node) { + dgFaceBucket tmp (GetAllocator()); + node = Insert(tmp, attribute); + } + + dgFaceBucket& bucket = node->GetInfo(); + dgFaceInfo& face = bucket.Append()->GetInfo(); + face.indexCount = count; + face.indexStart = polygonIndex; + polygonIndex += count; + } + } +}; + +class dgPolygonSoupDatabaseBuilder::dgPolySoupFilterAllocator: public dgPolyhedra +{ + public: + dgPolySoupFilterAllocator (dgMemoryAllocator* const allocator) + :dgPolyhedra (allocator) + { + } + + ~dgPolySoupFilterAllocator () + { + } + + dgInt32 AddFilterFace (dgUnsigned32 count, dgInt32* const pool) + { + BeginFace(); + dgAssert (count); + bool reduction = true; + while (reduction && !AddFace (dgInt32 (count), pool)) { + reduction = false; + if (count >3) { + for (dgUnsigned32 i = 0; i < count; i ++) { + for (dgUnsigned32 j = i + 1; j < count; j ++) { + if (pool[j] == pool[i]) { + for (i = j; i < count - 1; i ++) { + pool[i] = pool[i + 1]; + } + count --; + i = count; + reduction = true; + break; + } + } + } + } + } + EndFace(); + return reduction ? dgInt32 (count) : 0; + } +}; + + +dgPolygonSoupDatabaseBuilder::dgPolygonSoupDatabaseBuilder (dgMemoryAllocator* const allocator) + :m_faceVertexCount(allocator) + ,m_vertexIndex(allocator) + ,m_normalIndex(allocator) + ,m_vertexPoints(allocator) + ,m_normalPoints(allocator) +{ + m_run = DG_POINTS_RUN; + m_faceCount = 0; + m_indexCount = 0; + m_vertexCount = 0; + m_normalCount = 0; + m_allocator = allocator; +} + +dgPolygonSoupDatabaseBuilder::dgPolygonSoupDatabaseBuilder (const dgPolygonSoupDatabaseBuilder& source) + :m_faceVertexCount(source.m_allocator) + ,m_vertexIndex(source.m_allocator) + ,m_normalIndex(source.m_allocator) + ,m_vertexPoints(source.m_allocator) + ,m_normalPoints(source.m_allocator) +{ + m_run = DG_POINTS_RUN; + m_faceCount = source.m_faceCount; + m_indexCount = source.m_indexCount; + m_vertexCount = source.m_vertexCount; + m_normalCount = source.m_normalCount; + m_allocator = source.m_allocator; + + m_vertexIndex[m_indexCount-1] = 0; + m_faceVertexCount[m_faceCount-1] = 0; + m_vertexPoints[m_vertexCount-1].m_w = 0; + + memcpy (&m_vertexIndex[0], &source.m_vertexIndex[0], sizeof (dgInt32) * m_indexCount); + memcpy (&m_faceVertexCount[0], &source.m_faceVertexCount[0], sizeof (dgInt32) * m_faceCount); + memcpy (&m_vertexPoints[0], &source.m_vertexPoints[0], sizeof (dgBigVector) * m_vertexCount); + + if (m_normalCount) { + m_normalIndex[m_faceCount-1] = 0; + m_normalPoints[m_normalCount - 1].m_w = 0; + + memcpy (&m_normalIndex[0], &source.m_normalIndex[0], sizeof (dgInt32) * m_faceCount); + memcpy (&m_normalPoints[0], &source.m_normalPoints[0], sizeof (dgBigVector) * m_normalCount); + } else { + m_normalIndex[0] = 0; + m_normalPoints[0].m_w = 0; + } +} + +dgPolygonSoupDatabaseBuilder::~dgPolygonSoupDatabaseBuilder () +{ +} + +void dgPolygonSoupDatabaseBuilder::Begin() +{ + m_run = DG_POINTS_RUN; + m_faceCount = 0; + m_indexCount = 0; + m_vertexCount = 0; + m_normalCount = 0; +} + +void dgPolygonSoupDatabaseBuilder::SavePLY(const char* const fileName) const +{ + FILE* const file = fopen(fileName, "wb"); + + fprintf(file, "ply\n"); + fprintf(file, "format ascii 1.0\n"); + + //dgInt32 faceCount = 0; + fprintf(file, "element vertex %d\n", m_vertexCount); + fprintf(file, "property float x\n"); + fprintf(file, "property float y\n"); + fprintf(file, "property float z\n"); + fprintf(file, "element face %d\n", m_faceCount); + fprintf(file, "property list uchar int vertex_index\n"); + fprintf(file, "end_header\n"); + + for (dgInt32 i = 0; i < m_vertexCount; i ++) { + const dgBigVector& point = m_vertexPoints[i]; + fprintf(file, "%f %f %f\n", point.m_x, point.m_y, point.m_z); + } + + dgInt32 index = 0; + for (dgInt32 i = 0; i < m_faceCount; i ++) { + dgInt32 count = m_faceVertexCount[i]; + fprintf(file, "%d", count - 1); + for (dgInt32 j = 0; j < count - 1; j++) { + fprintf(file, " %d", m_vertexIndex[index + j]); + } + index += count; + fprintf(file, "\n"); + } + fclose(file); +} + +void dgPolygonSoupDatabaseBuilder::AddMesh (const dgFloat32* const vertex, dgInt32 vertexCount, dgInt32 strideInBytes, dgInt32 faceCount, + const dgInt32* const faceArray, const dgInt32* const indexArray, const dgInt32* const faceMaterialId, const dgMatrix& worldMatrix) +{ + dgInt32 faces[256]; + dgInt32 pool[2048]; + + m_vertexPoints[m_vertexCount + vertexCount].m_x = dgFloat64 (0.0f); + dgBigVector* const vertexPool = &m_vertexPoints[m_vertexCount]; + + worldMatrix.TransformTriplex (&vertexPool[0].m_x, sizeof (dgBigVector), vertex, strideInBytes, vertexCount); + for (dgInt32 i = 0; i < vertexCount; i ++) { + vertexPool[i].m_w = dgFloat64 (0.0f); + } + + dgInt32 totalIndexCount = faceCount; + for (dgInt32 i = 0; i < faceCount; i ++) { + totalIndexCount += faceArray[i]; + } + + m_vertexIndex[m_indexCount + totalIndexCount] = 0; + m_faceVertexCount[m_faceCount + faceCount] = 0; + + dgInt32 k = 0; + for (dgInt32 i = 0; i < faceCount; i ++) { + dgInt32 count = faceArray[i]; + for (dgInt32 j = 0; j < count; j ++) { + dgInt32 index = indexArray[k]; + pool[j] = index + m_vertexCount; + k ++; + } + + dgInt32 convexFaces = 0; + if (count == 3) { + convexFaces = 1; + dgBigVector p0 (m_vertexPoints[pool[2]]); + p0 = p0 & dgBigVector::m_triplexMask; + for (dgInt32 j = 0; j < 3; j ++) { + dgBigVector p1 (m_vertexPoints[pool[j]]); + p1 = p1 & dgBigVector::m_triplexMask; + dgBigVector edge (p1 - p0); + dgFloat64 mag2 = edge.DotProduct(edge).GetScalar(); + if (mag2 < dgFloat32 (1.0e-6f)) { + convexFaces = 0; + } + p0 = p1; + } + + if (convexFaces) { + dgBigVector edge0 (m_vertexPoints[pool[2]] - m_vertexPoints[pool[0]]); + dgBigVector edge1 (m_vertexPoints[pool[1]] - m_vertexPoints[pool[0]]); + dgAssert (edge0.m_w == dgFloat32 (0.0f)); + dgAssert (edge1.m_w == dgFloat32 (0.0f)); + dgBigVector normal (edge0.CrossProduct(edge1)); + dgFloat64 mag2 = normal.DotProduct(normal).GetScalar(); + if (mag2 < dgFloat32 (1.0e-8f)) { + convexFaces = 0; + } + } + + if (convexFaces) { + faces[0] = 3; + } + + } else { + convexFaces = AddConvexFace (count, pool, faces); + } + + dgInt32 indexAcc = 0; + for (dgInt32 j = 0; j < convexFaces; j ++) { + dgInt32 count1 = faces[j]; + m_vertexIndex[m_indexCount + count1] = faceMaterialId[i]; + for (dgInt32 m = 0; m < count1; m ++) { + m_vertexIndex[m_indexCount + m] = pool[indexAcc + m]; + } + indexAcc += count1; + m_indexCount += (count1 + 1); + m_faceVertexCount[m_faceCount] = count1 + 1; + m_faceCount ++; + } + } + m_vertexCount += vertexCount; + m_run -= vertexCount; + if (m_run <= 0) { + PackArray(); + } +} + +void dgPolygonSoupDatabaseBuilder::PackArray() +{ + dgStack indexMapPool (m_vertexCount); + dgInt32* const indexMap = &indexMapPool[0]; + m_vertexCount = dgVertexListToIndexList (&m_vertexPoints[0].m_x, sizeof (dgBigVector), 3, m_vertexCount, &indexMap[0], dgFloat32 (1.0e-6f)); + + dgInt32 k = 0; + for (dgInt32 i = 0; i < m_faceCount; i ++) { + dgInt32 count = m_faceVertexCount[i] - 1; + for (dgInt32 j = 0; j < count; j ++) { + dgInt32 index = m_vertexIndex[k]; + index = indexMap[index]; + m_vertexIndex[k] = index; + k ++; + } + k ++; + } + + m_run = DG_POINTS_RUN; +} + +void dgPolygonSoupDatabaseBuilder::Finalize() +{ + if (m_faceCount) { + dgStack indexMapPool (m_indexCount + m_vertexCount); + + dgInt32* const indexMap = &indexMapPool[0]; + m_vertexCount = dgVertexListToIndexList (&m_vertexPoints[0].m_x, sizeof (dgBigVector), 3, m_vertexCount, &indexMap[0], dgFloat32 (1.0e-4f)); + + dgInt32 k = 0; + for (dgInt32 i = 0; i < m_faceCount; i ++) { + dgInt32 count = m_faceVertexCount[i] - 1; + for (dgInt32 j = 0; j < count; j ++) { + dgInt32 index = m_vertexIndex[k]; + index = indexMap[index]; + m_vertexIndex[k] = index; + k ++; + } + k ++; + } + OptimizeByIndividualFaces(); + } +} + +void dgPolygonSoupDatabaseBuilder::FinalizeAndOptimize() +{ + Finalize(); + dgPolyhedra polyhedra(m_allocator); + dgPolygonSoupDatabaseBuilder source(*this); + dgPolygonSoupDatabaseBuilder leftOver(m_allocator); + dgInt32 tmpIndexPool[1024]; + dgVector tmpVertexPool[1024]; + + Begin(); + leftOver.Begin(); + polyhedra.BeginFace (); + dgInt32 faceIndexNumber = 0; + dgInt32 attribute = m_vertexIndex[0]; + + for (dgInt32 i = 0; i < source.m_faceCount; i ++) { + dgInt32 indexCount = source.m_faceVertexCount[i]; + dgAssert (indexCount < 1024); + + dgEdge* const face = polyhedra.AddFace(indexCount - 1, &source.m_vertexIndex[faceIndexNumber]); + if (!face) { + for (dgInt32 j = 0; j < indexCount - 1; j ++) { + dgInt32 index = source.m_vertexIndex[faceIndexNumber + j]; + tmpVertexPool[j] = source.m_vertexPoints[index]; + tmpIndexPool[j] = j; + } + dgInt32 faceArray = indexCount - 1; + leftOver.AddMesh (&tmpVertexPool[0].m_x, indexCount, sizeof (tmpVertexPool[0]), 1, &faceArray, tmpIndexPool, &attribute, dgGetIdentityMatrix()); + } else { + // set the attribute + dgEdge* ptr = face; + do { + ptr->m_userData = dgUnsigned64 (attribute); + ptr = ptr->m_next; + } while (ptr != face); + } + faceIndexNumber += indexCount; + } + polyhedra.EndFace(); + + dgPolyhedra facesLeft(m_allocator); + facesLeft.BeginFace(); + polyhedra.ConvexPartition (&source.m_vertexPoints[0].m_x, source.m_vertexPoints.GetElementSize(), &facesLeft); + facesLeft.EndFace(); + + dgInt32 mark = polyhedra.IncLRU(); + dgPolyhedra::Iterator iter (polyhedra); + for (iter.Begin(); iter; iter ++) { + dgEdge* const edge = &(*iter); + if (edge->m_incidentFace < 0) { + continue; + } + if (edge->m_mark == mark) { + continue; + } + + dgEdge* ptr = edge; + dgInt32 indexCount = 0; + do { + ptr->m_mark = mark; + tmpVertexPool[indexCount] = source.m_vertexPoints[ptr->m_incidentVertex]; + tmpIndexPool[indexCount] = indexCount; + indexCount ++; + ptr = ptr->m_next; + } while (ptr != edge); + + if (indexCount >= 3) { + AddMesh (&tmpVertexPool[0].m_x, indexCount, sizeof (tmpVertexPool[0]), 1, &indexCount, tmpIndexPool, &attribute, dgGetIdentityMatrix()); + } + } + + + mark = facesLeft.IncLRU(); + dgPolyhedra::Iterator iter1 (facesLeft); + for (iter1.Begin(); iter1; iter1 ++) { + dgEdge* const edge = &(*iter1); + if (edge->m_incidentFace < 0) { + continue; + } + if (edge->m_mark == mark) { + continue; + } + + dgEdge* ptr = edge; + dgInt32 indexCount = 0; + do { + ptr->m_mark = mark; + tmpVertexPool[indexCount] = source.m_vertexPoints[ptr->m_incidentVertex]; + tmpIndexPool[indexCount] = indexCount; + indexCount ++; + ptr = ptr->m_next; + } while (ptr != edge); + if (indexCount >= 3) { + AddMesh (&tmpVertexPool[0].m_x, indexCount, sizeof (dgVector), 1, &indexCount, tmpIndexPool, &attribute, dgGetIdentityMatrix()); + } + } + + faceIndexNumber = 0; + for (dgInt32 i = 0; i < leftOver.m_faceCount; i ++) { + dgInt32 indexCount = leftOver.m_faceVertexCount[i] - 1; + for (dgInt32 j = 0; j < indexCount; j ++) { + dgInt32 index = leftOver.m_vertexIndex[faceIndexNumber + j]; + tmpVertexPool[j] = leftOver.m_vertexPoints[index]; + tmpIndexPool[j] = j; + } + dgInt32 faceArray = indexCount; + AddMesh (&tmpVertexPool[0].m_x, indexCount, sizeof (tmpVertexPool[0]), 1, &faceArray, tmpIndexPool, &attribute, dgGetIdentityMatrix()); + + faceIndexNumber += (indexCount + 1); + } + + Finalize(); +} + +void dgPolygonSoupDatabaseBuilder::OptimizeByIndividualFaces() +{ + dgInt32* const faceArray = &m_faceVertexCount[0]; + dgInt32* const indexArray = &m_vertexIndex[0]; + + dgInt32* const oldFaceArray = &m_faceVertexCount[0]; + dgInt32* const oldIndexArray = &m_vertexIndex[0]; + + dgInt32 polygonIndex = 0; + dgInt32 newFaceCount = 0; + dgInt32 newIndexCount = 0; + for (dgInt32 i = 0; i < m_faceCount; i ++) { + dgInt32 oldCount = oldFaceArray[i]; + dgInt32 count = FilterFace (oldCount - 1, &oldIndexArray[polygonIndex]); + if (count) { + faceArray[newFaceCount] = count + 1; + for (dgInt32 j = 0; j < count; j ++) { + indexArray[newIndexCount + j] = oldIndexArray[polygonIndex + j]; + } + indexArray[newIndexCount + count] = oldIndexArray[polygonIndex + oldCount - 1]; + newFaceCount ++; + newIndexCount += (count + 1); + } + polygonIndex += oldCount; + } + dgAssert (polygonIndex == m_indexCount); + m_faceCount = newFaceCount; + m_indexCount = newIndexCount; +} + + +void dgPolygonSoupDatabaseBuilder::End(bool optimize) +{ + if (optimize) { + dgPolygonSoupDatabaseBuilder copy (*this); + dgFaceMap faceMap (m_allocator, copy); + + Begin(); + dgFaceMap::Iterator iter (faceMap); + for (iter.Begin(); iter; iter ++) { + const dgFaceBucket& bucket = iter.GetNode()->GetInfo(); + Optimize(iter.GetNode()->GetKey(), bucket, copy); + } + } + Finalize(); + + // build the normal array and adjacency array + // calculate all face the normals + dgInt32 indexCount = 0; + m_normalPoints[m_faceCount].m_x = dgFloat64 (0.0f); + for (dgInt32 i = 0; i < m_faceCount; i ++) { + dgInt32 faceIndexCount = m_faceVertexCount[i]; + + const dgInt32* const ptr = &m_vertexIndex[indexCount]; + dgBigVector v0 (&m_vertexPoints[ptr[0]].m_x); + dgBigVector v1 (&m_vertexPoints[ptr[1]].m_x); + dgBigVector e0 (v1 - v0); + dgBigVector normal0 (dgBigVector::m_zero); + for (dgInt32 j = 2; j < faceIndexCount - 1; j ++) { + dgBigVector v2 (&m_vertexPoints[ptr[j]].m_x); + dgBigVector e1 (v2 - v0); + normal0 += e0.CrossProduct(e1); + e0 = e1; + } + dgBigVector normal (normal0.Normalize()); + + m_normalPoints[i].m_x = normal.m_x; + m_normalPoints[i].m_y = normal.m_y; + m_normalPoints[i].m_z = normal.m_z; + m_normalPoints[i].m_w = dgFloat32 (0.0f); + indexCount += faceIndexCount; + } + // compress normals array + m_normalIndex[m_faceCount] = 0; + m_normalCount = dgVertexListToIndexList(&m_normalPoints[0].m_x, sizeof (dgBigVector), 3, m_faceCount, &m_normalIndex[0], dgFloat32 (1.0e-6f)); +} + + +void dgPolygonSoupDatabaseBuilder::Optimize(dgInt32 faceId, const dgFaceBucket& faceBucket, const dgPolygonSoupDatabaseBuilder& source) +{ + #define DG_MESH_PARTITION_SIZE (1024 * 4) + + const dgInt32* const indexArray = &source.m_vertexIndex[0]; + const dgBigVector* const points = &source.m_vertexPoints[0]; + + dgVector face[256]; + dgInt32 faceIndex[256]; + if (faceBucket.GetCount() >= DG_MESH_PARTITION_SIZE) { + dgStack array(faceBucket.GetCount()); + dgInt32 count = 0; + for (dgFaceBucket::dgListNode* node = faceBucket.GetFirst(); node; node = node->GetNext()) { + array[count] = node; + count ++; + } + + dgInt32 stack = 1; + dgInt32 segments[32][2]; + + segments[0][0] = 0; + segments[0][1] = count; + + while (stack) { + stack --; + dgInt32 faceStart = segments[stack][0]; + dgInt32 faceCount = segments[stack][1]; + + if (faceCount <= DG_MESH_PARTITION_SIZE) { + + dgPolygonSoupDatabaseBuilder tmpBuilder (m_allocator); + for (dgInt32 i = 0; i < faceCount; i ++) { + const dgFaceInfo& faceInfo = array[faceStart + i]->GetInfo(); + + dgInt32 count1 = faceInfo.indexCount - 1; + dgInt32 start1 = faceInfo.indexStart; + dgAssert (faceId == indexArray[start1 + count1]); + for (dgInt32 j = 0; j < count1; j ++) { + dgInt32 index = indexArray[start1 + j]; + face[j] = points[index]; + faceIndex[j] = j; + } + dgInt32 faceIndexCount = count1; + tmpBuilder.AddMesh (&face[0].m_x, count1, sizeof (dgVector), 1, &faceIndexCount, &faceIndex[0], &faceId, dgGetIdentityMatrix()); + } + tmpBuilder.FinalizeAndOptimize (); + + dgInt32 faceIndexNumber = 0; + for (dgInt32 i = 0; i < tmpBuilder.m_faceCount; i ++) { + dgInt32 indexCount = tmpBuilder.m_faceVertexCount[i] - 1; + for (dgInt32 j = 0; j < indexCount; j ++) { + dgInt32 index = tmpBuilder.m_vertexIndex[faceIndexNumber + j]; + face[j] = tmpBuilder.m_vertexPoints[index]; + faceIndex[j] = j; + } + dgInt32 faceArray = indexCount; + AddMesh (&face[0].m_x, indexCount, sizeof (dgVector), 1, &faceArray, faceIndex, &faceId, dgGetIdentityMatrix()); + + faceIndexNumber += (indexCount + 1); + } + + } else { + dgBigVector median (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f)); + dgBigVector varian (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f)); + for (dgInt32 i = 0; i < faceCount; i ++) { + const dgFaceInfo& faceInfo = array[faceStart + i]->GetInfo(); + dgInt32 count1 = faceInfo.indexCount - 1; + dgInt32 start1 = faceInfo.indexStart; + dgBigVector p0 (dgFloat32 ( 1.0e10f), dgFloat32 ( 1.0e10f), dgFloat32 ( 1.0e10f), dgFloat32 (0.0f)); + dgBigVector p1 (dgFloat32 (-1.0e10f), dgFloat32 (-1.0e10f), dgFloat32 (-1.0e10f), dgFloat32 (0.0f)); + for (dgInt32 j = 0; j < count1; j ++) { + dgInt32 index = indexArray[start1 + j]; + const dgBigVector& p = points[index]; + dgAssert(p.m_w == dgFloat32(0.0f)); + p0 = p0.GetMin(p); + p1 = p1.GetMax(p); + } + dgBigVector p ((p0 + p1).Scale (0.5f)); + median += p; + varian += p * p; + } + + varian = varian.Scale (dgFloat32 (faceCount)) - median * median; + + dgInt32 axis = 0; + dgFloat32 maxVarian = dgFloat32 (-1.0e10f); + for (dgInt32 i = 0; i < 3; i ++) { + if (varian[i] > maxVarian) { + axis = i; + maxVarian = dgFloat32 (varian[i]); + } + } + dgBigVector center = median.Scale (dgFloat32 (1.0f) / dgFloat32 (faceCount)); + dgFloat64 axisVal = center[axis]; + + dgInt32 leftCount = 0; + dgInt32 lastFace = faceCount; + + for (dgInt32 i = 0; i < lastFace; i ++) { + dgInt32 side = 0; + const dgFaceInfo& faceInfo = array[faceStart + i]->GetInfo(); + + dgInt32 start1 = faceInfo.indexStart; + dgInt32 count1 = faceInfo.indexCount - 1; + for (dgInt32 j = 0; j < count1; j ++) { + dgInt32 index = indexArray[start1 + j]; + const dgBigVector& p = points[index]; + if (p[axis] > axisVal) { + side = 1; + break; + } + } + + if (side) { + dgSwap (array[faceStart + i], array[faceStart + lastFace - 1]); + lastFace --; + i --; + } else { + leftCount ++; + } + } + dgAssert (leftCount); + dgAssert (leftCount < faceCount); + + segments[stack][0] = faceStart; + segments[stack][1] = leftCount; + stack ++; + + segments[stack][0] = faceStart + leftCount; + segments[stack][1] = faceCount - leftCount; + stack ++; + } + } + + } else { + dgPolygonSoupDatabaseBuilder tmpBuilder (m_allocator); + for (dgFaceBucket::dgListNode* node = faceBucket.GetFirst(); node; node = node->GetNext()) { + const dgFaceInfo& faceInfo = node->GetInfo(); + + dgInt32 count = faceInfo.indexCount - 1; + dgInt32 start = faceInfo.indexStart; + dgAssert (faceId == indexArray[start + count]); + for (dgInt32 j = 0; j < count; j ++) { + dgInt32 index = indexArray[start + j]; + face[j] = points[index]; + faceIndex[j] = j; + } + dgInt32 faceIndexCount = count; + tmpBuilder.AddMesh (&face[0].m_x, count, sizeof (dgVector), 1, &faceIndexCount, &faceIndex[0], &faceId, dgGetIdentityMatrix()); + } + tmpBuilder.FinalizeAndOptimize (); + + dgInt32 faceIndexNumber = 0; + for (dgInt32 i = 0; i < tmpBuilder.m_faceCount; i ++) { + dgInt32 indexCount = tmpBuilder.m_faceVertexCount[i] - 1; + for (dgInt32 j = 0; j < indexCount; j ++) { + dgInt32 index = tmpBuilder.m_vertexIndex[faceIndexNumber + j]; + face[j] = tmpBuilder.m_vertexPoints[index]; + faceIndex[j] = j; + } + dgInt32 faceArray = indexCount; + AddMesh (&face[0].m_x, indexCount, sizeof (dgVector), 1, &faceArray, faceIndex, &faceId, dgGetIdentityMatrix()); + + faceIndexNumber += (indexCount + 1); + } + } +} + + +dgInt32 dgPolygonSoupDatabaseBuilder::FilterFace (dgInt32 count, dgInt32* const pool) +{ + if (count == 3) { + dgBigVector p0 (m_vertexPoints[pool[2]]); + p0 = p0 & dgBigVector::m_triplexMask; + for (dgInt32 i = 0; i < 3; i ++) { + dgBigVector p1 (m_vertexPoints[pool[i]]); + p1 = p1 & dgBigVector::m_triplexMask; + dgBigVector edge (p1 - p0); + dgFloat64 mag2 = edge.DotProduct(edge).GetScalar(); + if (mag2 < dgFloat32 (1.0e-6f)) { + count = 0; + } + p0 = p1; + } + + if (count == 3) { + dgBigVector edge0 (m_vertexPoints[pool[2]] - m_vertexPoints[pool[0]]); + dgBigVector edge1 (m_vertexPoints[pool[1]] - m_vertexPoints[pool[0]]); + dgBigVector normal (edge0.CrossProduct(edge1)); + + dgAssert(edge0.m_w == dgFloat32(0.0f)); + dgAssert(edge1.m_w == dgFloat32(0.0f)); + dgAssert (normal.m_w == dgFloat32 (0.0f)); + dgFloat64 mag2 = normal.DotProduct(normal).GetScalar(); + if (mag2 < dgFloat32 (1.0e-8f)) { + count = 0; + } + } + } else { + dgPolySoupFilterAllocator polyhedra(m_allocator); + count = polyhedra.AddFilterFace (dgUnsigned32 (count), pool); + + if (!count) { + return 0; + } + + dgEdge* edge = &polyhedra.GetRoot()->GetInfo(); + if (edge->m_incidentFace < 0) { + edge = edge->m_twin; + } + + bool flag = true; + while (flag) { + flag = false; + if (count >= 3) { + dgEdge* ptr = edge; + + dgBigVector p0 (&m_vertexPoints[ptr->m_incidentVertex].m_x); + p0 = p0 & dgBigVector::m_triplexMask; + do { + dgBigVector p1 (&m_vertexPoints[ptr->m_next->m_incidentVertex].m_x); + p1 = p1 & dgBigVector::m_triplexMask; + dgBigVector e0 (p1 - p0); + dgFloat64 mag2 = e0.DotProduct(e0).GetScalar(); + if (mag2 < dgFloat32 (1.0e-6f)) { + count --; + flag = true; + edge = ptr->m_next; + ptr->m_prev->m_next = ptr->m_next; + ptr->m_next->m_prev = ptr->m_prev; + ptr->m_twin->m_next->m_prev = ptr->m_twin->m_prev; + ptr->m_twin->m_prev->m_next = ptr->m_twin->m_next; + break; + } + p0 = p1; + ptr = ptr->m_next; + } while (ptr != edge); + } + } + if (count >= 3) { + flag = true; + dgBigVector normal (polyhedra.FaceNormal (edge, &m_vertexPoints[0].m_x, sizeof (dgBigVector))); + dgAssert (normal.m_w == dgFloat32 (0.0f)); + + dgAssert (normal.DotProduct(normal).GetScalar() > dgFloat32 (1.0e-10f)); + normal = normal.Scale (dgFloat64 (1.0f) / sqrt (normal.DotProduct(normal).GetScalar() + dgFloat32 (1.0e-24f))); + + while (flag) { + flag = false; + if (count >= 3) { + dgEdge* ptr = edge; + + dgBigVector p0 (&m_vertexPoints[ptr->m_prev->m_incidentVertex].m_x); + dgBigVector p1 (&m_vertexPoints[ptr->m_incidentVertex].m_x); + + p0 = p0 & dgBigVector::m_triplexMask; + p1 = p1 & dgBigVector::m_triplexMask; + dgBigVector e0 (p1 - p0); + e0 = e0.Scale (dgFloat64 (1.0f) / sqrt (e0.DotProduct(e0).GetScalar() + dgFloat32(1.0e-24f))); + do { + dgBigVector p2 (&m_vertexPoints[ptr->m_next->m_incidentVertex].m_x); + p2 = p2 & dgBigVector::m_triplexMask; + dgBigVector e1 (p2 - p1); + + e1 = e1.Scale (dgFloat64 (1.0f) / sqrt (e1.DotProduct(e1).GetScalar() + dgFloat32(1.0e-24f))); + dgFloat64 mag2 = e1.DotProduct(e0).GetScalar(); + if (mag2 > dgFloat32 (0.9999f)) { + count --; + flag = true; + edge = ptr->m_next; + ptr->m_prev->m_next = ptr->m_next; + ptr->m_next->m_prev = ptr->m_prev; + ptr->m_twin->m_next->m_prev = ptr->m_twin->m_prev; + ptr->m_twin->m_prev->m_next = ptr->m_twin->m_next; + break; + } + + dgBigVector n (e0.CrossProduct(e1)); + dgAssert (n.m_w == dgFloat32 (0.0f)); + mag2 = n.DotProduct(normal).GetScalar(); + if (mag2 < dgFloat32 (1.0e-5f)) { + count --; + flag = true; + edge = ptr->m_next; + ptr->m_prev->m_next = ptr->m_next; + ptr->m_next->m_prev = ptr->m_prev; + ptr->m_twin->m_next->m_prev = ptr->m_twin->m_prev; + ptr->m_twin->m_prev->m_next = ptr->m_twin->m_next; + break; + } + + e0 = e1; + p1 = p2; + ptr = ptr->m_next; + } while (ptr != edge); + } + } + } + + dgEdge* first = edge; + if (count >= 3) { + dgFloat64 best = dgFloat32 (2.0f); + dgEdge* ptr = edge; + + dgBigVector p0 (&m_vertexPoints[ptr->m_incidentVertex].m_x); + dgBigVector p1 (&m_vertexPoints[ptr->m_next->m_incidentVertex].m_x); + p0 = p0 & dgBigVector::m_triplexMask; + p1 = p1 & dgBigVector::m_triplexMask; + dgBigVector e0 (p1 - p0); + e0 = e0.Scale (dgFloat64 (1.0f) / sqrt (e0.DotProduct(e0).GetScalar() + dgFloat32(1.0e-24f))); + do { + dgBigVector p2 (&m_vertexPoints[ptr->m_next->m_next->m_incidentVertex].m_x); + p2 = p2 & dgBigVector::m_triplexMask; + dgBigVector e1 (p2 - p1); + + e1 = e1.Scale (dgFloat64 (1.0f) / sqrt (e1.DotProduct(e1).GetScalar() + dgFloat32(1.0e-24f))); + dgFloat64 mag2 = fabs (e1.DotProduct(e0).GetScalar()); + if (mag2 < best) { + best = mag2; + first = ptr; + } + + e0 = e1; + p1 = p2; + ptr = ptr->m_next; + } while (ptr != edge); + + count = 0; + ptr = first; + do { + pool[count] = ptr->m_incidentVertex; + count ++; + ptr = ptr->m_next; + } while (ptr != first); + } + + #ifdef _DEBUG + if (count >= 3) { + dgInt32 j0 = count - 2; + dgInt32 j1 = count - 1; + dgBigVector normal (polyhedra.FaceNormal (edge, &m_vertexPoints[0].m_x, sizeof (dgBigVector))); + dgAssert (normal.m_w == dgFloat32 (0.0f)); + for (dgInt32 j2 = 0; j2 < count; j2 ++) { + dgBigVector p0 (&m_vertexPoints[pool[j0]].m_x); + dgBigVector p1 (&m_vertexPoints[pool[j1]].m_x); + dgBigVector p2 (&m_vertexPoints[pool[j2]].m_x); + p0 = p0 & dgBigVector::m_triplexMask; + p1 = p1 & dgBigVector::m_triplexMask; + p2 = p2 & dgBigVector::m_triplexMask; + + dgBigVector e0 ((p0 - p1)); + dgBigVector e1 ((p2 - p1)); + + dgBigVector n (e1.CrossProduct(e0)); + dgAssert (n.DotProduct(normal).GetScalar() > dgFloat32 (0.0f)); + j0 = j1; + j1 = j2; + } + } + #endif + } + + return (count >= 3) ? count : 0; +} + + +dgInt32 dgPolygonSoupDatabaseBuilder::AddConvexFace (dgInt32 count, dgInt32* const pool, dgInt32* const facesArray) +{ + dgPolySoupFilterAllocator polyhedra(m_allocator); + + count = polyhedra.AddFilterFace(dgUnsigned32 (count), pool); + + dgEdge* edge = &polyhedra.GetRoot()->GetInfo(); + if (edge->m_incidentFace < 0) { + edge = edge->m_twin; + } + + + dgInt32 isconvex = 1; + dgInt32 facesCount = 0; + + dgInt32 flag = 1; + while (flag) { + flag = 0; + if (count >= 3) { + dgEdge* ptr = edge; + + dgBigVector p0 (&m_vertexPoints[ptr->m_incidentVertex].m_x); + do { + dgBigVector p1 (&m_vertexPoints[ptr->m_next->m_incidentVertex].m_x); + dgBigVector e0 (p1 - p0); + dgFloat64 mag2 = e0.DotProduct3(e0); + if (mag2 < dgFloat32 (1.0e-6f)) { + count --; + flag = 1; + edge = ptr->m_next; + ptr->m_prev->m_next = ptr->m_next; + ptr->m_next->m_prev = ptr->m_prev; + ptr->m_twin->m_next->m_prev = ptr->m_twin->m_prev; + ptr->m_twin->m_prev->m_next = ptr->m_twin->m_next; + break; + } + p0 = p1; + ptr = ptr->m_next; + } while (ptr != edge); + } + } + if (count >= 3) { + flag = 1; + + while (flag) { + flag = 0; + if (count >= 3) { + dgEdge* ptr = edge; + + dgBigVector p0 (&m_vertexPoints[ptr->m_prev->m_incidentVertex].m_x); + dgBigVector p1 (&m_vertexPoints[ptr->m_incidentVertex].m_x); + dgBigVector e0 (p1 - p0); + e0 = e0.Scale (dgFloat64 (1.0f) / sqrt (e0.DotProduct3(e0) + dgFloat32(1.0e-24f))); + do { + dgBigVector p2 (&m_vertexPoints[ptr->m_next->m_incidentVertex].m_x); + dgBigVector e1 (p2 - p1); + + e1 = e1.Scale (dgFloat64 (1.0f) / sqrt (e1.DotProduct3(e1) + dgFloat32(1.0e-24f))); + dgFloat64 mag2 = e1.DotProduct3(e0); + if (mag2 > dgFloat32 (0.9999f)) { + count --; + flag = 1; + edge = ptr->m_next; + ptr->m_prev->m_next = ptr->m_next; + ptr->m_next->m_prev = ptr->m_prev; + ptr->m_twin->m_next->m_prev = ptr->m_twin->m_prev; + ptr->m_twin->m_prev->m_next = ptr->m_twin->m_next; + break; + } + + e0 = e1; + p1 = p2; + ptr = ptr->m_next; + } while (ptr != edge); + } + } + + dgBigVector normal (polyhedra.FaceNormal (edge, &m_vertexPoints[0].m_x, sizeof (dgBigVector))); + dgFloat64 mag2 = normal.DotProduct3(normal); + if (mag2 < dgFloat32 (1.0e-8f)) { + return 0; + } + normal = normal.Scale (dgFloat64 (1.0f) / sqrt (mag2)); + + if (count >= 3) { + dgEdge* ptr = edge; + dgBigVector p0 (&m_vertexPoints[ptr->m_prev->m_incidentVertex].m_x); + dgBigVector p1 (&m_vertexPoints[ptr->m_incidentVertex].m_x); + dgBigVector e0 (p1 - p0); + e0 = e0.Scale (dgFloat64 (1.0f) / sqrt (e0.DotProduct3(e0) + dgFloat32(1.0e-24f))); + do { + dgBigVector p2 (&m_vertexPoints[ptr->m_next->m_incidentVertex].m_x); + dgBigVector e1 (p2 - p1); + + e1 = e1.Scale (dgFloat64 (1.0f) / sqrt (e1.DotProduct3(e1) + dgFloat32(1.0e-24f))); + + dgBigVector n (e0.CrossProduct(e1)); + dgFloat64 magnitud2 = n.DotProduct3(normal); + if (magnitud2 < dgFloat32 (1.0e-5f)) { + isconvex = 0; + break; + } + + e0 = e1; + p1 = p2; + ptr = ptr->m_next; + } while (ptr != edge); + } + } + + if (isconvex) { + dgEdge* const first = edge; + if (count >= 3) { + count = 0; + dgEdge* ptr = first; + do { + pool[count] = ptr->m_incidentVertex; + count ++; + ptr = ptr->m_next; + } while (ptr != first); + facesArray[facesCount] = count; + facesCount = 1; + } + } else { + dgPolyhedra leftOver(m_allocator); + dgPolyhedra polyhedra2(m_allocator); + dgEdge* ptr = edge; + count = 0; + do { + pool[count] = ptr->m_incidentVertex; + count ++; + ptr = ptr->m_next; + } while (ptr != edge); + + + polyhedra2.BeginFace(); + polyhedra2.AddFace (count, pool); + polyhedra2.EndFace(); + leftOver.BeginFace(); + polyhedra2.ConvexPartition (&m_vertexPoints[0].m_x, m_vertexPoints.GetElementSize(), &leftOver); + leftOver.EndFace(); + +#if _DEBUG + if (leftOver.GetCount()) { + dgTrace (("warning: %d faces with more that a one shared edge\n", leftOver.GetCount())); + dgTrace ((" this mesh is not a manifold and may lead to collision malfunctions\n")); + } +#endif + + dgInt32 mark = polyhedra2.IncLRU(); + dgInt32 index = 0; + dgPolyhedra::Iterator iter (polyhedra2); + for (iter.Begin(); iter; iter ++) { + dgEdge* const edge1 = &(*iter); + if (edge1->m_incidentFace < 0) { + continue; + } + if (edge1->m_mark == mark) { + continue; + } + + ptr = edge1; + count = 0; + do { + ptr->m_mark = mark; + pool[index] = ptr->m_incidentVertex; + index ++; + count ++; + ptr = ptr->m_next; + } while (ptr != edge1); + + facesArray[facesCount] = count; + facesCount ++; + } + } + + return facesCount; +} + + + + diff --git a/thirdparty/src/newton/dgCore/dgPolygonSoupBuilder.h b/thirdparty/src/newton/dgCore/dgPolygonSoupBuilder.h new file mode 100644 index 000000000..e3881160a --- /dev/null +++ b/thirdparty/src/newton/dgCore/dgPolygonSoupBuilder.h @@ -0,0 +1,115 @@ +/* Copyright (c) <2003-2019> +* +* 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 + { + public: + dgVertexArray(dgMemoryAllocator* const allocator) + :dgArray(allocator) + { + } + }; + + class dgIndexArray: public dgArray + { + public: + dgIndexArray(dgMemoryAllocator* const allocator) + :dgArray(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 + diff --git a/thirdparty/src/newton/dgCore/dgPolygonSoupDatabase.cpp b/thirdparty/src/newton/dgCore/dgPolygonSoupDatabase.cpp new file mode 100644 index 000000000..184c8cc6b --- /dev/null +++ b/thirdparty/src/newton/dgCore/dgPolygonSoupDatabase.cpp @@ -0,0 +1,79 @@ +/* Copyright (c) <2003-2019> +* +* 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; +} + + + diff --git a/thirdparty/src/newton/dgCore/dgPolygonSoupDatabase.h b/thirdparty/src/newton/dgCore/dgPolygonSoupDatabase.h new file mode 100644 index 000000000..9b81d5092 --- /dev/null +++ b/thirdparty/src/newton/dgCore/dgPolygonSoupDatabase.h @@ -0,0 +1,66 @@ +/* Copyright (c) <2003-2019> +* +* 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 + diff --git a/thirdparty/src/newton/dgCore/dgPolyhedra.cpp b/thirdparty/src/newton/dgCore/dgPolyhedra.cpp new file mode 100644 index 000000000..80d13a3dd --- /dev/null +++ b/thirdparty/src/newton/dgCore/dgPolyhedra.cpp @@ -0,0 +1,2864 @@ +/* Copyright (c) <2003-2019> +* +* 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 "dgHeap.h" +#include "dgDebug.h" +#include "dgStack.h" +#include "dgPolyhedra.h" +#include "dgConvexHull3d.h" +#include "dgSmallDeterminant.h" + + +#define DG_LOCAL_BUFFER_SIZE 1024 + +class dgDiagonalEdge +{ + public: + dgDiagonalEdge (dgEdge* const edge) + :m_i0(edge->m_incidentVertex), m_i1(edge->m_twin->m_incidentVertex) + { + } + dgInt32 m_i0; + dgInt32 m_i1; +}; + + +struct dgEdgeCollapseEdgeHandle +{ + dgEdgeCollapseEdgeHandle (dgEdge* const newEdge) + :m_inList(false), m_edge(newEdge) + { + } + + dgEdgeCollapseEdgeHandle (const dgEdgeCollapseEdgeHandle &dataHandle) + :m_inList(true), m_edge(dataHandle.m_edge) + { + dgEdgeCollapseEdgeHandle* const handle = (dgEdgeCollapseEdgeHandle *)IntToPointer (m_edge->m_userData); + if (handle) { + dgAssert (handle != this); + handle->m_edge = NULL; + } + m_edge->m_userData = dgUnsigned64 (PointerToInt(this)); + } + + ~dgEdgeCollapseEdgeHandle () + { + if (m_inList) { + if (m_edge) { + dgEdgeCollapseEdgeHandle* const handle = (dgEdgeCollapseEdgeHandle *)IntToPointer (m_edge->m_userData); + if (handle == this) { + m_edge->m_userData = PointerToInt (NULL); + } + } + } + m_edge = NULL; + } + + dgUnsigned32 m_inList; + dgEdge* m_edge; +}; + + +class dgVertexCollapseVertexMetric +{ + public: + dgVertexCollapseVertexMetric (const dgBigPlane &plane) + { + elem[0] = plane.m_x * plane.m_x; + elem[1] = plane.m_y * plane.m_y; + elem[2] = plane.m_z * plane.m_z; + elem[3] = plane.m_w * plane.m_w; + elem[4] = dgFloat64 (2.0) * plane.m_x * plane.m_y; + elem[5] = dgFloat64 (2.0) * plane.m_x * plane.m_z; + elem[6] = dgFloat64 (2.0) * plane.m_x * plane.m_w; + elem[7] = dgFloat64 (2.0) * plane.m_y * plane.m_z; + elem[8] = dgFloat64 (2.0) * plane.m_y * plane.m_w; + elem[9] = dgFloat64 (2.0) * plane.m_z * plane.m_w; + } + + void Clear () + { + memset (elem, 0, 10 * sizeof (dgFloat64)); + } + + void Accumulate (const dgVertexCollapseVertexMetric& p) + { + elem[0] += p.elem[0]; + elem[1] += p.elem[1]; + elem[2] += p.elem[2]; + elem[3] += p.elem[3]; + elem[4] += p.elem[4]; + elem[5] += p.elem[5]; + elem[6] += p.elem[6]; + elem[7] += p.elem[7]; + elem[8] += p.elem[8]; + elem[9] += p.elem[9]; + } + + void Accumulate (const dgBigPlane& plane) + { + elem[0] += plane.m_x * plane.m_x; + elem[1] += plane.m_y * plane.m_y; + elem[2] += plane.m_z * plane.m_z; + elem[3] += plane.m_w * plane.m_w; + + elem[4] += dgFloat64 (2.0f) * plane.m_x * plane.m_y; + elem[5] += dgFloat64 (2.0f) * plane.m_x * plane.m_z; + elem[7] += dgFloat64 (2.0f) * plane.m_y * plane.m_z; + + elem[6] += dgFloat64 (2.0f) * plane.m_x * plane.m_w; + elem[8] += dgFloat64 (2.0f) * plane.m_y * plane.m_w; + elem[9] += dgFloat64 (2.0f) * plane.m_z * plane.m_w; + } + + + dgFloat64 Evalue (const dgBigVector &p) const + { + dgFloat64 acc = elem[0] * p.m_x * p.m_x + elem[1] * p.m_y * p.m_y + elem[2] * p.m_z * p.m_z + + elem[4] * p.m_x * p.m_y + elem[5] * p.m_x * p.m_z + elem[7] * p.m_y * p.m_z + + elem[6] * p.m_x + elem[8] * p.m_y + elem[9] * p.m_z + elem[3]; + return fabs (acc); + } + + dgFloat64 elem[10]; +}; + + + +dgPolyhedra::dgPolyhedra (dgMemoryAllocator* const allocator) + :dgTree (allocator) + ,m_baseMark(0) + ,m_edgeMark(0) + ,m_faceSecuence(0) +{ +} + +dgPolyhedra::dgPolyhedra (const dgPolyhedra &polyhedra) + :dgTree (polyhedra.GetAllocator()) + ,m_baseMark(0) + ,m_edgeMark(0) + ,m_faceSecuence(0) +{ + dgStack indexPool (DG_LOCAL_BUFFER_SIZE * 16); + dgStack userPool (DG_LOCAL_BUFFER_SIZE * 16); + dgInt32* const index = &indexPool[0]; + dgUnsigned64* const user = &userPool[0]; + + BeginFace (); + Iterator iter(polyhedra); + for (iter.Begin(); iter; iter ++) { + dgEdge* const edge = &(*iter); + if (edge->m_incidentFace < 0) { + continue; + } + + if (!FindEdge(edge->m_incidentVertex, edge->m_twin->m_incidentVertex)) { + dgInt32 indexCount = 0; + dgEdge* ptr = edge; + do { + user[indexCount] = ptr->m_userData; + index[indexCount] = ptr->m_incidentVertex; + indexCount ++; + ptr = ptr->m_next; + } while (ptr != edge); + + dgEdge* const face = AddFace (indexCount, index, (dgInt64*) user); + if (face) { + ptr = face; + do { + ptr->m_incidentFace = edge->m_incidentFace; + ptr = ptr->m_next; + } while (ptr != face); + } + } + } + EndFace(); + + m_faceSecuence = polyhedra.m_faceSecuence; + +#ifdef __ENABLE_DG_CONTAINERS_SANITY_CHECK + dgAssert (SanityCheck()); +#endif +} + +dgPolyhedra::~dgPolyhedra () +{ +} + +void dgPolyhedra::SavePLY(const char* const fileName, const dgFloat64* const vertexArray, dgInt32 strideInBytes) const +{ + FILE* const file = fopen(fileName, "wb"); + + fprintf(file, "ply\n"); + fprintf(file, "format ascii 1.0\n"); + + dgPolyhedra copy(*this); + + dgInt32 faceCount = 0; + Iterator iter(copy); + int mark = copy.IncLRU(); + for (iter.Begin(); iter; iter++) { + dgEdge* const face = &iter.GetNode()->GetInfo(); + if ((face->m_mark < mark) && (face->m_incidentFace > 0)) { + faceCount++; + dgEdge* edge = face; + do { + edge->m_mark = mark; + edge = edge->m_next; + } while (edge != face); + } + } + + mark = copy.IncLRU(); + dgInt32 vertexCount = 0; + for (iter.Begin(); iter; iter++) { + dgEdge* const vertex = &iter.GetNode()->GetInfo(); + if (vertex->m_mark < mark) { + dgEdge* edge = vertex; + do { + edge->m_userData = vertexCount; + edge->m_mark = mark; + edge = edge->m_twin->m_next; + } while (edge != vertex); + vertexCount++; + } + } + + fprintf(file, "element vertex %d\n", vertexCount); + fprintf(file, "property float x\n"); + fprintf(file, "property float y\n"); + fprintf(file, "property float z\n"); + fprintf(file, "element face %d\n", faceCount); + fprintf(file, "property list uchar int vertex_index\n"); + fprintf(file, "end_header\n"); + + mark = copy.IncLRU(); + const dgInt8* const points = (dgInt8*)vertexArray; + for (iter.Begin(); iter; iter++) { + dgEdge* const vertex = &iter.GetNode()->GetInfo(); + if (vertex->m_mark < mark) { + dgEdge* edge = vertex; + do { + edge->m_mark = mark; + edge = edge->m_twin->m_next; + } while (edge != vertex); + dgInt32 index = edge->m_incidentVertex * strideInBytes; + + const dgFloat64* const p = (dgFloat64*)&points[index]; + dgBigVector point(p[0], p[1], p[2], dgFloat64(0.0f)); + fprintf(file, "%f %f %f\n", point.m_x, point.m_y, point.m_z); + } + } + + mark = copy.IncLRU(); + for (iter.Begin(); iter; iter++) { + dgInt32 indices[1024]; + dgInt32 count = 0; + dgEdge* const face = &iter.GetNode()->GetInfo(); + if ((face->m_mark < mark) && (face->m_incidentFace > 0)) { + dgEdge* edge = face; + do { + indices[count] = dgInt32 (edge->m_userData); + count++; + edge->m_mark = mark; + edge = edge->m_next; + } while (edge != face); + + fprintf(file, "%d", count); + for (dgInt32 j = 0; j < count; j++) { + fprintf(file, " %d", indices[j]); + } + fprintf(file, "\n"); + } + } + fclose(file); +} + +dgInt32 dgPolyhedra::GetFaceCount() const +{ + Iterator iter (*this); + dgInt32 count = 0; + dgInt32 mark = IncLRU(); + for (iter.Begin(); iter; iter ++) { + dgEdge* const edge = &(*iter); + if (edge->m_mark == mark) { + continue; + } + + if (edge->m_incidentFace < 0) { + continue; + } + + count ++; + dgEdge* ptr = edge; + do { + ptr->m_mark = mark; + ptr = ptr->m_next; + } while (ptr != edge); + } + return count; +} + +dgEdge* dgPolyhedra::AddFace ( dgInt32 count, const dgInt32* const index, const dgInt64* const userdata) +{ + class IntersectionFilter + { + public: + IntersectionFilter () + { + m_count = 0; + } + + bool Insert (dgInt64 value) + { + dgInt32 i = 0; + for (; i < m_count; i ++) { + if (m_array[i] == value) { + return false; + } + } + m_array[i] = value; + m_count ++; + return true; + } + + dgInt32 m_count; + dgInt64 m_array[2048]; + }; + + IntersectionFilter selfIntersectingFaceFilter; + + dgInt32 i0 = index[count-1]; + for (dgInt32 i = 0; i < count; i ++) { + dgInt32 i1 = index[i]; + + dgPairKey code0 (i0, i1); + if (!selfIntersectingFaceFilter.Insert (code0.GetVal())) { + return NULL; + } + + dgPairKey code1 (i1, i0); + if (!selfIntersectingFaceFilter.Insert (code1.GetVal())) { + return NULL; + } + + if (i0 == i1) { + return NULL; + } + if (FindEdge (i0, i1)) { + return NULL; + } + i0 = i1; + } + + m_faceSecuence ++; + + i0 = index[count-1]; + dgInt32 i1 = index[0]; + dgUnsigned64 udata0 = 0; + dgUnsigned64 udata1 = 0; + if (userdata) { + udata0 = dgUnsigned64 (userdata[count-1]); + udata1 = dgUnsigned64 (userdata[0]); + } + + bool state; + dgPairKey code (i0, i1); + dgEdge tmpEdge (i0, m_faceSecuence, udata0); + dgTreeNode* const node = Insert (tmpEdge, code.GetVal(), state); + dgAssert (!state); + dgEdge* edge0 = &node->GetInfo(); + dgEdge* const first = edge0; + + for (dgInt32 i = 1; i < count; i ++) { + i0 = i1; + i1 = index[i]; + udata0 = udata1; + udata1 = dgUnsigned64 (userdata ? userdata[i] : 0); + + dgPairKey code1 (i0, i1); + dgEdge tmpEdge1 (i0, m_faceSecuence, udata0); + dgTreeNode* const node1 = Insert (tmpEdge1, code1.GetVal(), state); + dgAssert (!state); + + dgEdge* const edge1 = &node1->GetInfo(); + edge0->m_next = edge1; + edge1->m_prev = edge0; + edge0 = edge1; + } + + first->m_prev = edge0; + edge0->m_next = first; + + return first->m_next; +} + +bool dgPolyhedra::EndFace () +{ + dgPolyhedra::Iterator iter (*this); + + // Connect all twin edge + for (iter.Begin(); iter; iter ++) { + dgEdge* const edge = &(*iter); + if (!edge->m_twin) { + edge->m_twin = FindEdge (edge->m_next->m_incidentVertex, edge->m_incidentVertex); + if (edge->m_twin) { + edge->m_twin->m_twin = edge; + } + } + } + +#ifdef __ENABLE_DG_CONTAINERS_SANITY_CHECK + dgAssert (SanityCheck()); +#endif + dgStack edgeArrayPool(GetCount() * 2 + 256); + + dgInt32 edgeCount = 0; + dgEdge** const edgeArray = &edgeArrayPool[0]; + for (iter.Begin(); iter; iter ++) { + dgEdge* const edge = &(*iter); + if (!edge->m_twin) { + bool state; + dgPolyhedra::dgPairKey code (edge->m_next->m_incidentVertex, edge->m_incidentVertex); + dgEdge tmpEdge (edge->m_next->m_incidentVertex, -1); + tmpEdge.m_incidentFace = -1; + dgPolyhedra::dgTreeNode* const node = Insert (tmpEdge, code.GetVal(), state); + dgAssert (!state); + edge->m_twin = &node->GetInfo(); + edge->m_twin->m_twin = edge; + edgeArray[edgeCount] = edge->m_twin; + edgeCount ++; + } + } + + for (dgInt32 i = 0; i < edgeCount; i ++) { + dgEdge* const edge = edgeArray[i]; + dgAssert (!edge->m_prev); + dgEdge *ptr = edge->m_twin; + for (; ptr->m_next; ptr = ptr->m_next->m_twin){} + ptr->m_next = edge; + edge->m_prev = ptr; + } + +#ifdef __ENABLE_DG_CONTAINERS_SANITY_CHECK + dgAssert (SanityCheck ()); +#endif + + return true; +} + +void dgPolyhedra::DeleteFace(dgEdge* const face) +{ + dgEdge* edgeList[DG_LOCAL_BUFFER_SIZE * 16]; + + if (face->m_incidentFace > 0) { + dgInt32 count = 0; + dgEdge* ptr = face; + do { + ptr->m_incidentFace = -1; + dgInt32 i = 0; + for (; i < count; i ++) { + if ((edgeList[i] == ptr) || (edgeList[i]->m_twin == ptr)) { + break; + } + } + if (i == count) { + edgeList[count] = ptr; + count ++; + } + ptr = ptr->m_next; + } while (ptr != face); + + + for (dgInt32 i = 0; i < count; i ++) { + dgEdge* const ptr1 = edgeList[i]; + if (ptr1->m_twin->m_incidentFace < 0) { + DeleteEdge (ptr1); + } + } + } +} + +dgBigVector dgPolyhedra::FaceNormal (const dgEdge* const face, const dgFloat64* const pool, dgInt32 strideInBytes) const +{ + dgInt32 stride = dgInt32 (strideInBytes / sizeof (dgFloat64)); + const dgEdge* edge = face; + dgBigVector p0 (&pool[edge->m_incidentVertex * stride]); + edge = edge->m_next; + dgBigVector p1 (&pool[edge->m_incidentVertex * stride]); + dgBigVector e1 (p1 - p0); + + dgBigVector normal (dgFloat32 (0.0f)); + for (edge = edge->m_next; edge != face; edge = edge->m_next) { + dgBigVector p2 (&pool[edge->m_incidentVertex * stride]); + dgBigVector e2 (p2 - p0); + normal += e1.CrossProduct(e2); + e1 = e2; + } + dgAssert (normal.m_w == dgFloat32 (0.0f)); + return normal; +} + + +dgEdge* dgPolyhedra::AddHalfEdge (dgInt32 v0, dgInt32 v1) +{ + if (v0 != v1) { + dgPairKey pairKey (v0, v1); + dgEdge tmpEdge (v0, -1); + + dgTreeNode* node = Insert (tmpEdge, pairKey.GetVal()); + return node ? &node->GetInfo() : NULL; + } else { + return NULL; + } +} + + +void dgPolyhedra::DeleteEdge (dgEdge* const edge) +{ + dgEdge *const twin = edge->m_twin; + + edge->m_prev->m_next = twin->m_next; + twin->m_next->m_prev = edge->m_prev; + edge->m_next->m_prev = twin->m_prev; + twin->m_prev->m_next = edge->m_next; + + dgTreeNode *const nodeA = GetNodeFromInfo (*edge); + dgTreeNode *const nodeB = GetNodeFromInfo (*twin); + + dgAssert (&nodeA->GetInfo() == edge); + dgAssert (&nodeB->GetInfo() == twin); + + Remove (nodeA); + Remove (nodeB); +} + + +dgEdge* dgPolyhedra::ConnectVertex (dgEdge* const e0, dgEdge* const e1) +{ + dgEdge* const edge = AddHalfEdge(e1->m_incidentVertex, e0->m_incidentVertex); + dgEdge* const twin = AddHalfEdge(e0->m_incidentVertex, e1->m_incidentVertex); + dgAssert ((edge && twin) || !(edge || twin)); + if (edge) { + edge->m_twin = twin; + twin->m_twin = edge; + + edge->m_incidentFace = e0->m_incidentFace; + twin->m_incidentFace = e1->m_incidentFace; + + edge->m_userData = e1->m_userData; + twin->m_userData = e0->m_userData; + + edge->m_next = e0; + edge->m_prev = e1->m_prev; + + twin->m_next = e1; + twin->m_prev = e0->m_prev; + + e0->m_prev->m_next = twin; + e0->m_prev = edge; + + e1->m_prev->m_next = edge; + e1->m_prev = twin; + } + + return edge; +} + +dgEdge* dgPolyhedra::SpliteEdge (dgInt32 newIndex, dgEdge* const edge) +{ + dgEdge* const edge00 = edge->m_prev; + dgEdge* const edge01 = edge->m_next; + dgEdge* const twin00 = edge->m_twin->m_next; + dgEdge* const twin01 = edge->m_twin->m_prev; + + dgInt32 i0 = edge->m_incidentVertex; + dgInt32 i1 = edge->m_twin->m_incidentVertex; + + dgInt32 f0 = edge->m_incidentFace; + dgInt32 f1 = edge->m_twin->m_incidentFace; + + DeleteEdge (edge); + + dgEdge* const edge0 = AddHalfEdge (i0, newIndex); + dgEdge* const edge1 = AddHalfEdge (newIndex, i1); + + dgEdge* const twin0 = AddHalfEdge (newIndex, i0); + dgEdge* const twin1 = AddHalfEdge (i1, newIndex); + dgAssert (edge0); + dgAssert (edge1); + dgAssert (twin0); + dgAssert (twin1); + + edge0->m_twin = twin0; + twin0->m_twin = edge0; + + edge1->m_twin = twin1; + twin1->m_twin = edge1; + + edge0->m_next = edge1; + edge1->m_prev = edge0; + + twin1->m_next = twin0; + twin0->m_prev = twin1; + + edge0->m_prev = edge00; + edge00 ->m_next = edge0; + + edge1->m_next = edge01; + edge01->m_prev = edge1; + + twin0->m_next = twin00; + twin00->m_prev = twin0; + + twin1->m_prev = twin01; + twin01->m_next = twin1; + + edge0->m_incidentFace = f0; + edge1->m_incidentFace = f0; + + twin0->m_incidentFace = f1; + twin1->m_incidentFace = f1; + +#ifdef __ENABLE_DG_CONTAINERS_SANITY_CHECK + // dgAssert (SanityCheck ()); +#endif + + return edge0; +} + +bool dgPolyhedra::FlipEdge (dgEdge* const edge) +{ + // dgTreeNode *node; + if (edge->m_next->m_next->m_next != edge) { + return false; + } + + if (edge->m_twin->m_next->m_next->m_next != edge->m_twin) { + return false; + } + + if (FindEdge(edge->m_prev->m_incidentVertex, edge->m_twin->m_prev->m_incidentVertex)) { + return false; + } + + dgEdge *const prevEdge = edge->m_prev; + dgEdge *const prevTwin = edge->m_twin->m_prev; + + dgPairKey edgeKey (prevTwin->m_incidentVertex, prevEdge->m_incidentVertex); + dgPairKey twinKey (prevEdge->m_incidentVertex, prevTwin->m_incidentVertex); + + ReplaceKey (GetNodeFromInfo (*edge), edgeKey.GetVal()); + // dgAssert (node); + + ReplaceKey (GetNodeFromInfo (*edge->m_twin), twinKey.GetVal()); + // dgAssert (node); + + edge->m_incidentVertex = prevTwin->m_incidentVertex; + edge->m_twin->m_incidentVertex = prevEdge->m_incidentVertex; + + edge->m_userData = prevTwin->m_userData; + edge->m_twin->m_userData = prevEdge->m_userData; + + prevEdge->m_next = edge->m_twin->m_next; + prevTwin->m_prev->m_prev = edge->m_prev; + + prevTwin->m_next = edge->m_next; + prevEdge->m_prev->m_prev = edge->m_twin->m_prev; + + edge->m_prev = prevTwin->m_prev; + edge->m_next = prevEdge; + + edge->m_twin->m_prev = prevEdge->m_prev; + edge->m_twin->m_next = prevTwin; + + prevTwin->m_prev->m_next = edge; + prevTwin->m_prev = edge->m_twin; + + prevEdge->m_prev->m_next = edge->m_twin; + prevEdge->m_prev = edge; + + edge->m_next->m_incidentFace = edge->m_incidentFace; + edge->m_prev->m_incidentFace = edge->m_incidentFace; + + edge->m_twin->m_next->m_incidentFace = edge->m_twin->m_incidentFace; + edge->m_twin->m_prev->m_incidentFace = edge->m_twin->m_incidentFace; + + +#ifdef __ENABLE_DG_CONTAINERS_SANITY_CHECK + dgAssert (SanityCheck ()); +#endif + + return true; +} + + + +bool dgPolyhedra::GetConectedSurface (dgPolyhedra &polyhedra) const +{ + if (!GetCount()) { + return false; + } + + dgEdge* edge = NULL; + Iterator iter(*this); + for (iter.Begin (); iter; iter ++) { + edge = &(*iter); + if ((edge->m_mark < m_baseMark) && (edge->m_incidentFace > 0)) { + break; + } + } + + if (!iter) { + return false; + } + + dgInt32 faceIndex[4096]; + dgInt64 faceDataIndex[4096]; + dgStack stackPool (GetCount()); + dgEdge** const stack = &stackPool[0]; + + dgInt32 mark = IncLRU(); + + polyhedra.BeginFace (); + stack[0] = edge; + dgInt32 index = 1; + while (index) { + index --; + dgEdge* const edge1 = stack[index]; + dgAssert (edge1); + if (edge1->m_mark == mark) { + continue; + } + + dgInt32 count = 0; + dgEdge* ptr = edge1; + do { + dgAssert (ptr); + ptr->m_mark = mark; + faceIndex[count] = ptr->m_incidentVertex; + faceDataIndex[count] = dgInt64 (ptr->m_userData); + count ++; + dgAssert (count < dgInt32 ((sizeof (faceIndex)/sizeof(faceIndex[0])))); + + if ((ptr->m_twin->m_incidentFace > 0) && (ptr->m_twin->m_mark != mark)) { + stack[index] = ptr->m_twin; + index ++; + dgAssert (index < GetCount()); + } + + ptr = ptr->m_next; + } while (ptr != edge1); + + polyhedra.AddFace (count, &faceIndex[0], &faceDataIndex[0]); + } + + polyhedra.EndFace (); + + return true; +} + + +void dgPolyhedra::ChangeEdgeIncidentVertex (dgEdge* const edge, dgInt32 newIndex) +{ + dgEdge* ptr = edge; + do { + dgTreeNode* node = GetNodeFromInfo(*ptr); + dgPairKey Key0 (newIndex, ptr->m_twin->m_incidentVertex); + ReplaceKey (node, Key0.GetVal()); + + node = GetNodeFromInfo(*ptr->m_twin); + dgPairKey Key1 (ptr->m_twin->m_incidentVertex, newIndex); + ReplaceKey (node, Key1.GetVal()); + + ptr->m_incidentVertex = newIndex; + + ptr = ptr->m_twin->m_next; + } while (ptr != edge); +} + + +void dgPolyhedra::DeleteDegenerateFaces (const dgFloat64* const pool, dgInt32 strideInBytes, dgFloat64 area) +{ + if (!GetCount()) { + return; + } + +#ifdef __ENABLE_DG_CONTAINERS_SANITY_CHECK + dgAssert (SanityCheck ()); +#endif + dgStack faceArrayPool(GetCount() / 2 + 100); + + dgInt32 count = 0; + dgPolyhedra::dgTreeNode** const faceArray = &faceArrayPool[0]; + dgInt32 mark = IncLRU(); + Iterator iter (*this); + for (iter.Begin(); iter; iter ++) { + dgEdge* const edge = &(*iter); + + if ((edge->m_mark != mark) && (edge->m_incidentFace > 0)) { + faceArray[count] = iter.GetNode(); + count ++; + dgEdge* ptr = edge; + do { + ptr->m_mark = mark; + ptr = ptr->m_next; + } while (ptr != edge); + } + } + + dgFloat64 area2 = area * area; + area2 *= dgFloat64 (4.0f); + + for (dgInt32 i = 0; i < count; i ++) { + dgPolyhedra::dgTreeNode* const faceNode = faceArray[i]; + dgEdge* const edge = &faceNode->GetInfo(); + + dgBigVector normal (FaceNormal (edge, pool, strideInBytes)); + + dgFloat64 faceArea = normal.DotProduct(normal).GetScalar(); + if (faceArea < area2) { + DeleteFace (edge); + } + } + +#ifdef __ENABLE_DG_CONTAINERS_SANITY_CHECK + mark = IncLRU(); + for (iter.Begin(); iter; iter ++) { + dgEdge* const edge = &(*iter); + if ((edge->m_mark != mark) && (edge->m_incidentFace > 0)) { + //dgAssert (edge->m_next->m_next->m_next == edge); + dgEdge* ptr = edge; + do { + ptr->m_mark = mark; + ptr = ptr->m_next; + } while (ptr != edge); + + dgBigVector normal (FaceNormal (edge, pool, strideInBytes)); + + dgFloat64 faceArea = normal.DotProduct(normal).GetScalar(); + dgAssert (faceArea >= area2); + } + } + dgAssert (SanityCheck ()); +#endif +} + + +dgBigPlane dgPolyhedra::UnboundedLoopPlane (dgInt32 i0, dgInt32 i1, dgInt32 i2, const dgBigVector* const pool) +{ + const dgBigVector p0 = pool[i0]; + const dgBigVector p1 = pool[i1]; + const dgBigVector p2 = pool[i2]; + dgBigVector E0 (p1 - p0); + dgBigVector E1 (p2 - p0); + + dgBigVector N ((E0.CrossProduct(E1)).CrossProduct(E0)); + dgFloat64 dist = - N.DotProduct3(p0); + dgBigPlane plane (N, dist); + + dgFloat64 mag = sqrt (plane.DotProduct3(plane)); + if (mag < dgFloat64 (1.0e-12f)) { + mag = dgFloat64 (1.0e-12f); + } + mag = dgFloat64 (10.0f) / mag; + + plane.m_x *= mag; + plane.m_y *= mag; + plane.m_z *= mag; + plane.m_w *= mag; + + return plane; +} + +dgEdge* dgPolyhedra::CollapseEdge(dgEdge* const edge) +{ + dgInt32 v0 = edge->m_incidentVertex; + dgInt32 v1 = edge->m_twin->m_incidentVertex; + + dgEdge* retEdge = edge->m_twin->m_prev->m_twin; + if (retEdge == edge->m_twin->m_next) { + return NULL; + } + if (retEdge == edge->m_twin) { + return NULL; + } + if (retEdge == edge->m_next) { + retEdge = edge->m_prev->m_twin; + if (retEdge == edge->m_twin->m_next) { + return NULL; + } + if (retEdge == edge->m_twin) { + return NULL; + } + } + + dgEdge* lastEdge = NULL; + dgEdge* firstEdge = NULL; + if ((edge->m_incidentFace >= 0) && (edge->m_twin->m_incidentFace >= 0)) { + lastEdge = edge->m_prev->m_twin; + firstEdge = edge->m_twin->m_next->m_twin->m_next; + } else if (edge->m_twin->m_incidentFace >= 0) { + firstEdge = edge->m_twin->m_next->m_twin->m_next; + lastEdge = edge; + } else { + lastEdge = edge->m_prev->m_twin; + firstEdge = edge->m_twin->m_next; + } + + for (dgEdge* ptr = firstEdge; ptr != lastEdge; ptr = ptr->m_twin->m_next) { + dgEdge* const badEdge = FindEdge (edge->m_twin->m_incidentVertex, ptr->m_twin->m_incidentVertex); + if (badEdge) { + return NULL; + } + } + + dgEdge* const twin = edge->m_twin; + if (twin->m_next == twin->m_prev->m_prev) { + twin->m_prev->m_twin->m_twin = twin->m_next->m_twin; + twin->m_next->m_twin->m_twin = twin->m_prev->m_twin; + + Remove (GetNodeFromInfo(*twin->m_prev)); + Remove (GetNodeFromInfo(*twin->m_next)); + } else { + twin->m_next->m_userData = twin->m_userData; + twin->m_next->m_prev = twin->m_prev; + twin->m_prev->m_next = twin->m_next; + } + + if (edge->m_next == edge->m_prev->m_prev) { + edge->m_next->m_twin->m_twin = edge->m_prev->m_twin; + edge->m_prev->m_twin->m_twin = edge->m_next->m_twin; + Remove (GetNodeFromInfo(*edge->m_next)); + Remove (GetNodeFromInfo(*edge->m_prev)); + } else { + edge->m_next->m_prev = edge->m_prev; + edge->m_prev->m_next = edge->m_next; + } + + dgAssert (twin->m_twin->m_incidentVertex == v0); + dgAssert (edge->m_twin->m_incidentVertex == v1); + Remove (GetNodeFromInfo(*twin)); + Remove (GetNodeFromInfo(*edge)); + + dgEdge* ptr = retEdge; + do { + dgPolyhedra::dgPairKey pairKey (v0, ptr->m_twin->m_incidentVertex); + + dgPolyhedra::dgTreeNode* node = Find (pairKey.GetVal()); + if (node) { + if (&node->GetInfo() == ptr) { + dgPolyhedra::dgPairKey key (v1, ptr->m_twin->m_incidentVertex); + ptr->m_incidentVertex = v1; + node = ReplaceKey (node, key.GetVal()); + dgAssert (node); + } + } + + dgPolyhedra::dgPairKey TwinKey (ptr->m_twin->m_incidentVertex, v0); + node = Find (TwinKey.GetVal()); + if (node) { + if (&node->GetInfo() == ptr->m_twin) { + dgPolyhedra::dgPairKey key (ptr->m_twin->m_incidentVertex, v1); + node = ReplaceKey (node, key.GetVal()); + dgAssert (node); + } + } + + ptr = ptr->m_twin->m_next; + } while (ptr != retEdge); + + return retEdge; +} + + + +void dgPolyhedra::RemoveHalfEdge (dgEdge* const edge) +{ + dgEdgeCollapseEdgeHandle* const handle = (dgEdgeCollapseEdgeHandle *) IntToPointer (edge->m_userData); + if (handle) { + handle->m_edge = NULL; + } + + dgPolyhedra::dgTreeNode* const node = GetNodeFromInfo(*edge); + dgAssert (node); + Remove (node); +} + + +dgEdge* dgPolyhedra::FindEarTip (dgEdge* const face, const dgFloat64* const pool, dgInt32 stride, dgDownHeap& heap, const dgBigVector &normal) const +{ + dgEdge* ptr = face; + dgBigVector p0 (&pool[ptr->m_prev->m_incidentVertex * stride]); + dgBigVector p1 (&pool[ptr->m_incidentVertex * stride]); + dgBigVector d0 (p1 - p0); + dgFloat64 val = sqrt (d0.DotProduct3(d0)); + if (val < dgFloat64 (1.0e-10f)) { + val = dgFloat64 (1.0e-10f); + } + d0 = d0.Scale (dgFloat64 (1.0f) / val); + + dgFloat64 minAngle = dgFloat32 (10.0f); + do { + dgBigVector p2 (&pool [ptr->m_next->m_incidentVertex * stride]); + dgBigVector d1 (p2 - p1); + dgFloat64 val1 = dgFloat64 (1.0f) / sqrt (d1.DotProduct3(d1)); + if (val1 < dgFloat64 (1.0e-10f)) { + val1 = dgFloat64 (1.0e-10f); + } + d1 = d1.Scale (dgFloat32 (1.0f) / val1); + dgBigVector n (d0.CrossProduct(d1)); + + dgFloat64 angle = normal.DotProduct3(n); + if (angle >= dgFloat64 (0.0f)) { + heap.Push (ptr, angle); + } + + if (angle < minAngle) { + minAngle = angle; + } + + d0 = d1; + p1 = p2; + ptr = ptr->m_next; + } while (ptr != face); + + if (minAngle > dgFloat32 (0.1f)) { + return heap[0]; + } + + dgEdge* ear = NULL; + while (heap.GetCount()) { + ear = heap[0]; + heap.Pop(); + + if (FindEdge (ear->m_prev->m_incidentVertex, ear->m_next->m_incidentVertex)) { + continue; + } + + dgBigVector q0 (&pool [ear->m_prev->m_incidentVertex * stride]); + dgBigVector q1 (&pool [ear->m_incidentVertex * stride]); + dgBigVector q2 (&pool [ear->m_next->m_incidentVertex * stride]); + + dgBigVector p10 (q1 - q0); + dgBigVector p21 (q2 - q1); + dgBigVector p02 (q0 - q2); + + for (ptr = ear->m_next->m_next; ptr != ear->m_prev; ptr = ptr->m_next) { + if (!((ptr->m_incidentVertex == ear->m_incidentVertex) || (ptr->m_incidentVertex == ear->m_prev->m_incidentVertex) || (ptr->m_incidentVertex == ear->m_next->m_incidentVertex))) { + dgBigVector p (&pool [ptr->m_incidentVertex * stride]); + + //dgFloat64 side = ((p - p0) * p10) % normal; + dgFloat64 side = normal.DotProduct3((p - q0).CrossProduct(p10)); + if (side < dgFloat64 (0.05f)) { + //side = ((p - p1) * p21) % normal; + side = normal.DotProduct3((p - q1).CrossProduct(p21)); + if (side < dgFloat64 (0.05f)) { + //side = ((p - p2) * p02) % normal; + side = normal.DotProduct3((p - q2).CrossProduct(p02)); + if (side < dgFloat32 (0.05f)) { + break; + + } + } + } + } + } + + if (ptr == ear->m_prev) { + break; + } + } + + return ear; +} + +dgEdge* dgPolyhedra::TriangulateFace (dgEdge* const faceIn, const dgFloat64* const pool, dgInt32 stride, dgDownHeap& heap, dgBigVector* const faceNormalOut) +{ + dgEdge* face = faceIn; + dgBigVector normal (FaceNormal (face, pool, dgInt32 (stride * sizeof (dgFloat64)))); + + dgFloat64 dot = normal.DotProduct3(normal); + if (dot < dgFloat64 (1.0e-12f)) { + if (faceNormalOut) { + *faceNormalOut = dgBigVector (dgFloat32 (0.0f)); + } + return face; + } + normal = normal.Scale (dgFloat64 (1.0f) / sqrt (dot)); + if (faceNormalOut) { + *faceNormalOut = normal; + } + + while (face->m_next->m_next->m_next != face) { + dgEdge* const ear = FindEarTip (face, pool, stride, heap, normal); + if (!ear) { + return face; + } + if ((face == ear) || (face == ear->m_prev)) { + face = ear->m_prev->m_prev; + } + dgEdge* const edge = AddHalfEdge (ear->m_next->m_incidentVertex, ear->m_prev->m_incidentVertex); + if (!edge) { + return face; + } + dgEdge* const twin = AddHalfEdge (ear->m_prev->m_incidentVertex, ear->m_next->m_incidentVertex); + if (!twin) { + return face; + } + dgAssert (twin); + + + edge->m_mark = ear->m_mark; + edge->m_userData = ear->m_next->m_userData; + edge->m_incidentFace = ear->m_incidentFace; + + twin->m_mark = ear->m_mark; + twin->m_userData = ear->m_prev->m_userData; + twin->m_incidentFace = ear->m_incidentFace; + + edge->m_twin = twin; + twin->m_twin = edge; + + twin->m_prev = ear->m_prev->m_prev; + twin->m_next = ear->m_next; + ear->m_prev->m_prev->m_next = twin; + ear->m_next->m_prev = twin; + + edge->m_next = ear->m_prev; + edge->m_prev = ear; + ear->m_prev->m_prev = edge; + ear->m_next = edge; + + heap.Flush (); + } + return NULL; +} + + +void dgPolyhedra::MarkAdjacentCoplanarFaces (dgPolyhedra& polyhedraOut, dgEdge* const face, const dgFloat64* const pool, dgInt32 strideInBytes) +{ + const dgFloat64 normalDeviation = dgFloat64 (0.9999f); + const dgFloat64 distanceFromPlane = dgFloat64 (1.0f / 128.0f); + + dgInt32 faceIndex[DG_LOCAL_BUFFER_SIZE * 8]; + dgInt64 userIndex[DG_LOCAL_BUFFER_SIZE * 8]; + dgEdge* stack[DG_LOCAL_BUFFER_SIZE * 8]; + dgEdge* deleteEdge[DG_LOCAL_BUFFER_SIZE * 32]; + + dgInt32 deleteCount = 1; + deleteEdge[0] = face; + dgInt32 stride = dgInt32 (strideInBytes / sizeof (dgFloat64)); + + dgAssert (face->m_incidentFace > 0); + + dgBigVector normalAverage (FaceNormal (face, pool, strideInBytes)); + dgAssert (normalAverage.m_w == dgFloat32 (0.0f)); + dgFloat64 dot = normalAverage.DotProduct(normalAverage).GetScalar(); + if (dot > dgFloat64 (1.0e-12f)) { + dgInt32 testPointsCount = 1; + dot = dgFloat64 (1.0f) / sqrt (dot); + dgBigVector normal (normalAverage.Scale (dot)); + + dgBigVector averageTestPoint (&pool[face->m_incidentVertex * stride]); + dgBigPlane testPlane(normal, - normal.DotProduct(averageTestPoint & dgBigVector::m_triplexMask).GetScalar()); + + polyhedraOut.BeginFace(); + + IncLRU(); + dgInt32 faceMark = IncLRU(); + + dgInt32 faceIndexCount = 0; + dgEdge* ptr = face; + do { + ptr->m_mark = faceMark; + faceIndex[faceIndexCount] = ptr->m_incidentVertex; + userIndex[faceIndexCount] = dgInt64 (ptr->m_userData); + faceIndexCount ++; + dgAssert (faceIndexCount < dgInt32 (sizeof (faceIndex) / sizeof (faceIndex[0]))); + ptr = ptr ->m_next; + } while (ptr != face); + polyhedraOut.AddFace(faceIndexCount, faceIndex, userIndex); + + dgInt32 index = 1; + deleteCount = 0; + stack[0] = face; + while (index) { + index --; + dgEdge* const face1 = stack[index]; + deleteEdge[deleteCount] = face1; + deleteCount ++; + dgAssert (deleteCount < dgInt32 (sizeof (deleteEdge) / sizeof (deleteEdge[0]))); + dgAssert (face1->m_next->m_next->m_next == face1); + + dgEdge* edge = face1; + do { + dgEdge* const ptr1 = edge->m_twin; + if (ptr1->m_incidentFace > 0) { + if (ptr1->m_mark != faceMark) { + dgEdge* ptr2 = ptr1; + faceIndexCount = 0; + do { + ptr2->m_mark = faceMark; + faceIndex[faceIndexCount] = ptr2->m_incidentVertex; + userIndex[faceIndexCount] = dgInt64 (ptr2->m_userData); + dgAssert (faceIndexCount < dgInt32 (sizeof (faceIndex) / sizeof (faceIndex[0]))); + faceIndexCount ++; + ptr2 = ptr2 ->m_next; + } while (ptr2 != ptr1); + + dgBigVector normal1 (FaceNormal (ptr1, pool, strideInBytes)); + dot = normal1.DotProduct(normal1).GetScalar(); + if (dot < dgFloat64 (1.0e-12f)) { + deleteEdge[deleteCount] = ptr1; + deleteCount ++; + dgAssert (deleteCount < dgInt32 (sizeof (deleteEdge) / sizeof (deleteEdge[0]))); + } else { + dgBigVector testNormal (normal1.Scale (dgFloat64 (1.0f) / sqrt (dot))); + dgAssert (testNormal.m_w == dgFloat32 (0.0f)); + dot = normal.DotProduct(testNormal).GetScalar(); + if (dot >= normalDeviation) { + dgBigVector testPoint (&pool[ptr1->m_prev->m_incidentVertex * stride]); + dgFloat64 dist = fabs (testPlane.Evalue (testPoint)); + if (dist < distanceFromPlane) { + testPointsCount ++; + + averageTestPoint += testPoint; + testPoint = averageTestPoint.Scale (dgFloat64 (1.0f) / dgFloat64(testPointsCount)); + + normalAverage += normal1; + dgAssert (normalAverage.m_w == dgFloat32 (0.0f)); + testNormal = normalAverage.Scale (dgFloat64 (1.0f) / sqrt (normalAverage.DotProduct(normalAverage).GetScalar())); + testPlane = dgBigPlane (testNormal, - testPoint.DotProduct (testNormal).GetScalar()); + + polyhedraOut.AddFace(faceIndexCount, faceIndex, userIndex); + stack[index] = ptr1; + index ++; + dgAssert (index < dgInt32 (sizeof (stack) / sizeof (stack[0]))); + } + } + } + } + } + + edge = edge->m_next; + } while (edge != face1); + } + polyhedraOut.EndFace(); + } + + for (dgInt32 index = 0; index < deleteCount; index ++) { + DeleteFace (deleteEdge[index]); + } +} + +void dgPolyhedra::RefineTriangulation (const dgFloat64* const vertex, dgInt32 stride, const dgBigVector& normal, dgInt32 perimeterCount, dgEdge** const perimeter) +{ + dgList dignonals(GetAllocator()); + + for (dgInt32 i = 1; i <= perimeterCount; i ++) { + dgEdge* const last = perimeter[i - 1]; + for (dgEdge* ptr = perimeter[i]->m_prev; ptr != last; ptr = ptr->m_twin->m_prev) { + dgList::dgListNode* node = dignonals.GetFirst(); + for (; node; node = node->GetNext()) { + const dgDiagonalEdge& key = node->GetInfo(); + if (((key.m_i0 == ptr->m_incidentVertex) && (key.m_i1 == ptr->m_twin->m_incidentVertex)) || + ((key.m_i1 == ptr->m_incidentVertex) && (key.m_i0 == ptr->m_twin->m_incidentVertex))) { + break; + } + } + if (!node) { + dgDiagonalEdge key (ptr); + dignonals.Append(key); + } + } + } + + dgEdge* const face = perimeter[0]; + dgInt32 i0 = face->m_incidentVertex * stride; + dgInt32 i1 = face->m_next->m_incidentVertex * stride; + dgBigVector p0 (vertex[i0], vertex[i0 + 1], vertex[i0 + 2], dgFloat32 (0.0f)); + dgBigVector p1 (vertex[i1], vertex[i1 + 1], vertex[i1 + 2], dgFloat32 (0.0f)); + + dgBigVector p1p0 (p1 - p0); + dgFloat64 mag2 = p1p0.DotProduct(p1p0).GetScalar(); + for (dgEdge* ptr = face->m_next->m_next; mag2 < dgFloat32 (1.0e-12f); ptr = ptr->m_next) { + dgInt32 i2 = ptr->m_incidentVertex * stride; + dgBigVector p2 (vertex[i2], vertex[i2 + 1], vertex[i2 + 2], dgFloat32 (0.0f)); + p1p0 = p2 - p0; + mag2 = p1p0.DotProduct(p1p0).GetScalar(); + } + + dgAssert (p1p0.m_w == dgFloat32 (0.0f)); + dgMatrix matrix (dgGetIdentityMatrix()); + matrix.m_posit = p0; + matrix.m_front = dgVector (p1p0.Scale (dgFloat64 (1.0f) / sqrt (mag2))); + matrix.m_right = dgVector (normal.Scale (dgFloat64 (1.0f) / sqrt (normal.DotProduct(normal).GetScalar()))); + matrix.m_up = matrix.m_right.CrossProduct(matrix.m_front); + matrix = matrix.Inverse(); + dgAssert (matrix.m_posit.m_w == dgFloat32 (1.0f)); +// matrix.m_posit.m_w = dgFloat32 (1.0f); + + dgInt32 maxCount = dignonals.GetCount() * dignonals.GetCount(); + while (dignonals.GetCount() && maxCount) { + maxCount --; + dgList::dgListNode* const node = dignonals.GetFirst(); + dgDiagonalEdge key (node->GetInfo()); + dignonals.Remove(node); + dgEdge* const edge = FindEdge(key.m_i0, key.m_i1); + if (edge) { + dgInt32 k0 = edge->m_incidentVertex * stride; + dgInt32 k1 = edge->m_next->m_incidentVertex * stride; + dgInt32 k2 = edge->m_next->m_next->m_incidentVertex * stride; + dgInt32 k3 = edge->m_twin->m_prev->m_incidentVertex * stride; + + dgBigVector q0 (vertex[k0], vertex[k0 + 1], vertex[k0 + 2], dgFloat64 (1.0f)); + dgBigVector q1 (vertex[k1], vertex[k1 + 1], vertex[k1 + 2], dgFloat64 (1.0f)); + dgBigVector q2 (vertex[k2], vertex[k2 + 1], vertex[k2 + 2], dgFloat64 (1.0f)); + dgBigVector q3 (vertex[k3], vertex[k3 + 1], vertex[k3 + 2], dgFloat64 (1.0f)); + + q0 = matrix.TransformVector(q0); + q1 = matrix.TransformVector(q1); + q2 = matrix.TransformVector(q2); + q3 = matrix.TransformVector(q3); + + dgFloat64 circleTest[3][3]; + circleTest[0][0] = q0[0] - q3[0]; + circleTest[0][1] = q0[1] - q3[1]; + circleTest[0][2] = circleTest[0][0] * circleTest[0][0] + circleTest[0][1] * circleTest[0][1]; + + circleTest[1][0] = q1[0] - q3[0]; + circleTest[1][1] = q1[1] - q3[1]; + circleTest[1][2] = circleTest[1][0] * circleTest[1][0] + circleTest[1][1] * circleTest[1][1]; + + circleTest[2][0] = q2[0] - q3[0]; + circleTest[2][1] = q2[1] - q3[1]; + circleTest[2][2] = circleTest[2][0] * circleTest[2][0] + circleTest[2][1] * circleTest[2][1]; + + dgFloat64 error; + dgFloat64 det = Determinant3x3 (circleTest, &error); + if (det < dgFloat32 (0.0f)) { + dgEdge* frontFace0 = edge->m_prev; + dgEdge* backFace0 = edge->m_twin->m_prev; + + FlipEdge(edge); + + if (perimeterCount > 4) { + dgEdge* backFace1 = backFace0->m_next; + dgEdge* frontFace1 = frontFace0->m_next; + for (dgInt32 i = 0; i < perimeterCount; i ++) { + if (frontFace0 == perimeter[i]) { + frontFace0 = NULL; + } + if (frontFace1 == perimeter[i]) { + frontFace1 = NULL; + } + + if (backFace0 == perimeter[i]) { + backFace0 = NULL; + } + if (backFace1 == perimeter[i]) { + backFace1 = NULL; + } + } + + if (backFace0 && (backFace0->m_incidentFace > 0) && (backFace0->m_twin->m_incidentFace > 0)) { + dgDiagonalEdge key0 (backFace0); + dignonals.Append(key0); + } + if (backFace1 && (backFace1->m_incidentFace > 0) && (backFace1->m_twin->m_incidentFace > 0)) { + dgDiagonalEdge key1 (backFace1); + dignonals.Append(key1); + } + + if (frontFace0 && (frontFace0->m_incidentFace > 0) && (frontFace0->m_twin->m_incidentFace > 0)) { + dgDiagonalEdge key0 (frontFace0); + dignonals.Append(key0); + } + + if (frontFace1 && (frontFace1->m_incidentFace > 0) && (frontFace1->m_twin->m_incidentFace > 0)) { + dgDiagonalEdge key1 (frontFace1); + dignonals.Append(key1); + } + } + } + } + } +} + +void dgPolyhedra::RefineTriangulation (const dgFloat64* const vertex, dgInt32 stride) +{ + if (GetCount() <= 6) { + return; + } + + dgInt32 mark = IncLRU(); + dgInt32 loopCount = 0; + + dgPolyhedra::Iterator iter (*this); + dgEdge* edgePerimeters[DG_LOCAL_BUFFER_SIZE * 16]; + dgInt32 perimeterCount = 0; + dgTree filter (GetAllocator()); + for (iter.Begin(); iter && (loopCount <= 1) ; iter ++) { + dgEdge* const edge = &(*iter); + if ((edge->m_incidentFace < 0) && (edge->m_mark != mark)){ + loopCount ++; + dgEdge* ptr = edge; + do { + ptr->m_mark = mark; + if (!filter.Insert(ptr, ptr->m_incidentVertex)) { + loopCount = 2; + break; + } + edgePerimeters[perimeterCount] = ptr->m_twin; + perimeterCount ++; + dgAssert (perimeterCount < dgInt32 (sizeof (edgePerimeters) / sizeof (edgePerimeters[0]))); + ptr = ptr->m_prev; + } while (ptr != edge); + } + } + + if (loopCount == 1) { + #ifdef _DEBUG + for (dgInt32 i = 0; i < perimeterCount; i ++) { + for (dgInt32 j = i + 1; j < perimeterCount; j ++) { + dgAssert (edgePerimeters[i]->m_incidentVertex != edgePerimeters[j]->m_incidentVertex); + } + } + #endif + + dgAssert (perimeterCount); + dgAssert (perimeterCount < dgInt32 (sizeof (edgePerimeters) / sizeof (edgePerimeters[0]))); + edgePerimeters[perimeterCount] = edgePerimeters[0]; + + dgBigVector normal (FaceNormal(edgePerimeters[0], vertex, dgInt32 (stride * sizeof (dgFloat64)))); + if (normal.DotProduct(normal).GetScalar() > dgFloat32 (1.0e-12f)) { + RefineTriangulation (vertex, stride, normal, perimeterCount, edgePerimeters); + } + } +} + + +void dgPolyhedra::OptimizeTriangulation (const dgFloat64* const vertex, dgInt32 strideInBytes) +{ + dgInt32 polygon[DG_LOCAL_BUFFER_SIZE * 8]; + dgInt64 userData[DG_LOCAL_BUFFER_SIZE * 8]; + dgInt32 stride = dgInt32 (strideInBytes / sizeof (dgFloat64)); + + dgPolyhedra leftOver(GetAllocator()); + dgPolyhedra buildConvex(GetAllocator()); + + buildConvex.BeginFace(); + dgPolyhedra::Iterator iter (*this); + + for (iter.Begin(); iter; ) { + dgEdge* const edge = &(*iter); + iter++; + + if (edge->m_incidentFace > 0) { + dgPolyhedra flatFace(GetAllocator()); + MarkAdjacentCoplanarFaces (flatFace, edge, vertex, strideInBytes); + //dgAssert (flatFace.GetCount()); + + if (flatFace.GetCount()) { + flatFace.RefineTriangulation (vertex, stride); + + dgInt32 mark = flatFace.IncLRU(); + dgPolyhedra::Iterator iter1 (flatFace); + for (iter1.Begin(); iter1; iter1 ++) { + dgEdge* const edge1 = &(*iter1); + if (edge1->m_mark != mark) { + if (edge1->m_incidentFace > 0) { + dgEdge* ptr = edge1; + dgInt32 vertexCount = 0; + do { + polygon[vertexCount] = ptr->m_incidentVertex; + userData[vertexCount] = dgInt64 (ptr->m_userData); + vertexCount ++; + dgAssert (vertexCount < dgInt32 (sizeof (polygon) / sizeof (polygon[0]))); + ptr->m_mark = mark; + ptr = ptr->m_next; + } while (ptr != edge1); + if (vertexCount >= 3) { + buildConvex.AddFace (vertexCount, polygon, userData); + } + } + } + } + } + iter.Begin(); + } + } + buildConvex.EndFace(); + dgAssert (GetCount() == 0); + SwapInfo(buildConvex); +} + +void dgPolyhedra::Triangulate (const dgFloat64* const vertex, dgInt32 strideInBytes, dgPolyhedra* const leftOver) +{ + dgInt32 stride = dgInt32 (strideInBytes / sizeof (dgFloat64)); + + dgInt32 count = GetCount() / 2; + dgStack memPool (dgInt32 ((count + 512) * (2 * sizeof (dgFloat64)))); + dgDownHeap heap(&memPool[0], memPool.GetSizeInBytes()); + + dgInt32 mark = IncLRU(); + Iterator iter (*this); + for (iter.Begin(); iter; ) { + dgEdge* const thisEdge = &(*iter); + iter ++; + + if (thisEdge->m_mark == mark) { + continue; + } + if (thisEdge->m_incidentFace < 0) { + continue; + } + + count = 0; + dgEdge* ptr = thisEdge; + do { + count ++; + ptr->m_mark = mark; + ptr = ptr->m_next; + } while (ptr != thisEdge); + + if (count > 3) { + dgEdge* const edge = TriangulateFace (thisEdge, vertex, stride, heap, NULL); + heap.Flush (); + + if (edge) { + dgAssert (edge->m_incidentFace > 0); + + if (leftOver) { + dgInt32* const index = (dgInt32 *) &heap[0]; + dgInt64* const data = (dgInt64 *)&index[count]; + dgInt32 i = 0; + dgEdge* ptr1 = edge; + do { + index[i] = ptr1->m_incidentVertex; + data[i] = dgInt64 (ptr1->m_userData); + i ++; + ptr1 = ptr1->m_next; + } while (ptr1 != edge); + leftOver->AddFace(i, index, data); + + } else { + dgTrace (("Deleting face:")); + ptr = edge; + do { + dgTrace (("%d ", ptr->m_incidentVertex)); + } while (ptr != edge); + dgTrace (("\n")); + } + + DeleteFace (edge); + iter.Begin(); + } + } + } + + OptimizeTriangulation (vertex, strideInBytes); + + mark = IncLRU(); + m_faceSecuence = 1; + for (iter.Begin(); iter; iter ++) { + dgEdge* edge = &(*iter); + if (edge->m_mark == mark) { + continue; + } + if (edge->m_incidentFace < 0) { + continue; + } + dgAssert (edge == edge->m_next->m_next->m_next); + + for (dgInt32 i = 0; i < 3; i ++) { + edge->m_incidentFace = m_faceSecuence; + edge->m_mark = mark; + edge = edge->m_next; + } + m_faceSecuence ++; + } +} + +bool dgPolyhedra::IsFaceConvex(dgEdge* const face, const dgFloat64* const vertex, dgInt32 strideInBytes) const +{ + if (face->m_next->m_next->m_next == face) { + return true; + } + dgBigVector normal(FaceNormal(face, vertex, strideInBytes)); + normal.m_w = dgFloat32(0.0f); + + dgInt32 stride = strideInBytes / sizeof(dgFloat64); + dgEdge* ptr = face; + do { + dgBigVector p0(&vertex[ptr->m_incidentVertex * stride]); + dgBigVector p1(&vertex[ptr->m_prev->m_incidentVertex * stride]); + dgBigVector p2(&vertex[ptr->m_next->m_incidentVertex * stride]); + dgBigVector e0(p1 - p0); + dgBigVector e1(p2 - p1); + dgBigVector cornerNormal(e1.CrossProduct(e0)); + dgFloat64 project(normal.DotProduct(cornerNormal).GetScalar()); + if (project < dgFloat32(0.0f)) { + return false; + } + + ptr = ptr->m_next; + } while (ptr != face); + + return true; +} + +void dgPolyhedra::RemoveOuterColinearEdges (dgPolyhedra& flatFace, const dgFloat64* const vertex, dgInt32 stride) +{ + dgEdge* edgePerimeters[DG_LOCAL_BUFFER_SIZE]; + + dgInt32 perimeterCount = 0; + dgInt32 mark = flatFace.IncLRU(); + dgPolyhedra::Iterator iter (flatFace); + for (iter.Begin(); iter; iter ++) { + dgEdge* const edge = &(*iter); + if ((edge->m_incidentFace < 0) && (edge->m_mark != mark)) { + dgEdge* ptr = edge; + do { + ptr->m_mark = mark; + ptr = ptr->m_next; + } while (ptr != edge); + + edgePerimeters[perimeterCount] = edge; + perimeterCount++; + dgAssert(perimeterCount < dgInt32(sizeof(edgePerimeters) / sizeof(edgePerimeters[0]))); + } + } + + dgInt8 buffer[2048 * sizeof (dgFloat64)]; + dgDownHeap heap(&buffer[0], sizeof (buffer)); + for (dgInt32 i = 0; i < perimeterCount; i ++) { + dgEdge* edge = edgePerimeters[i]; + dgEdge* ptr = edge; + dgBigVector p0 (&vertex[ptr->m_incidentVertex * stride]); + dgBigVector p1 (&vertex[ptr->m_next->m_incidentVertex * stride]); + dgBigVector e0 (p1 - p0) ; + e0 = e0.Scale (dgRsqrt (e0.DotProduct3(e0) + dgFloat32 (1.0e-12f))); + dgInt32 ignoreTest = 1; + do { + ignoreTest = 0; + dgBigVector p2 (&vertex[ptr->m_next->m_next->m_incidentVertex * stride]); + dgBigVector e1 (p2 - p1); + e1 = e1.Scale (dgRsqrt (e1.DotProduct3(e1) + dgFloat32 (1.0e-12f))); + dgFloat64 dot = e1.DotProduct3(e0); + if (dot > dgFloat32 (dgFloat32 (0.9999f))) { + + for (dgEdge* interiorEdge = ptr->m_next->m_twin->m_next; interiorEdge != ptr->m_twin; interiorEdge = ptr->m_next->m_twin->m_next) { + dgAssert((interiorEdge->m_incidentFace > 0) && (interiorEdge->m_twin->m_incidentFace > 0)); + if ((interiorEdge->m_incidentFace > 0) && (interiorEdge->m_twin->m_incidentFace > 0)) { + flatFace.DeleteEdge(interiorEdge); + } else { + return; + } + } + + if (ptr->m_twin->m_next->m_next->m_next == ptr->m_twin) { + dgAssert (ptr->m_twin->m_next->m_incidentFace > 0); + flatFace.DeleteEdge (ptr->m_twin->m_next); + } + + dgAssert (ptr->m_next->m_twin->m_next->m_twin == ptr); + edge = ptr->m_next; + + if (!flatFace.FindEdge (ptr->m_incidentVertex, edge->m_twin->m_incidentVertex) && + !flatFace.FindEdge (edge->m_twin->m_incidentVertex, ptr->m_incidentVertex)) { + ptr->m_twin->m_prev = edge->m_twin->m_prev; + edge->m_twin->m_prev->m_next = ptr->m_twin; + + edge->m_next->m_prev = ptr; + ptr->m_next = edge->m_next; + + edge->m_next = edge->m_twin; + edge->m_prev = edge->m_twin; + edge->m_twin->m_next = edge; + edge->m_twin->m_prev = edge; + flatFace.DeleteEdge (edge); + flatFace.ChangeEdgeIncidentVertex (ptr->m_twin, ptr->m_next->m_incidentVertex); + + if (!flatFace.IsFaceConvex(ptr->m_twin, vertex, stride * sizeof(dgFloat64))) { + heap.Flush(); + flatFace.TriangulateFace(ptr->m_twin, vertex, stride, heap, NULL); + } + + e1 = e0; + p1 = p2; + edge = ptr; + ignoreTest = 1; + continue; + } + } + + e0 = e1; + p1 = p2; + ptr = ptr->m_next; + } while ((ptr != edge) || ignoreTest); + } +} + +void dgPolyhedra::RemoveInteriorColinearEdges(dgPolyhedra& flatFace, const dgFloat64* const vertex, dgInt32 stride) +{ + bool foundEdge = true; + while (foundEdge) { + foundEdge = false; + dgPolyhedra::Iterator iter(flatFace); + for (iter.Begin(); iter; iter++) { + dgEdge* const edge = &(*iter); + if ((edge->m_incidentFace > 0) && (edge->m_twin->m_incidentFace > 0)) { + if (edge->m_twin->m_next->m_twin->m_next == edge) { + dgBigVector p0(&vertex[edge->m_prev->m_incidentVertex * stride]); + dgBigVector p1(&vertex[edge->m_incidentVertex * stride]); + dgBigVector p2(&vertex[edge->m_next->m_incidentVertex * stride]); + + dgBigVector e0(p1 - p0); + dgBigVector e1(p2 - p1); + e0 = e0.Scale(dgRsqrt(e0.DotProduct3(e0) + dgFloat32(1.0e-12f))); + e1 = e1.Scale(dgRsqrt(e1.DotProduct3(e1) + dgFloat32(1.0e-12f))); + dgFloat64 dot = e1.DotProduct3(e0); + if (dot > dgFloat32(0.9999f)) { + dgInt32 v = edge->m_twin->m_incidentVertex; + dgEdge* const nextEdge = edge->m_twin->m_next; + edge->m_next->m_prev = edge->m_prev; + edge->m_prev->m_next = edge->m_next; + edge->m_twin->m_next->m_prev = edge->m_twin->m_prev; + edge->m_twin->m_prev->m_next = edge->m_twin->m_next; + + edge->m_next = edge->m_twin; + edge->m_prev = edge->m_twin; + edge->m_twin->m_next = edge; + edge->m_twin->m_prev = edge; + flatFace.DeleteEdge(edge); + flatFace.ChangeEdgeIncidentVertex(nextEdge, v); + foundEdge = true; + break; + } + } + } + } + } +} + + +dgInt32 dgPolyhedra::GetInteriorDiagonals (dgPolyhedra& polyhedra, dgEdge** const diagonals, dgInt32 maxCount) +{ + dgInt32 count = 0; + dgInt32 mark = polyhedra.IncLRU(); + dgPolyhedra::Iterator iter (polyhedra); + for (iter.Begin(); iter; iter++) { + dgEdge* const edge = &(*iter); + if (edge->m_mark != mark) { + if (edge->m_incidentFace > 0) { + if (edge->m_twin->m_incidentFace > 0) { + edge->m_twin->m_mark = mark; + if (count < maxCount){ + diagonals[count] = edge; + count ++; + } + dgAssert (count <= maxCount); + } + } + } + edge->m_mark = mark; + } + + return count; +} + +bool dgPolyhedra::IsEssensialPointDiagonal (dgEdge* const diagonal, const dgBigVector& normal, const dgFloat64* const pool, dgInt32 stride) +{ + if (diagonal->m_twin->m_next->m_twin->m_next != diagonal) { + dgBigVector p0 (&pool[diagonal->m_incidentVertex * stride]); + dgBigVector p1 (&pool[diagonal->m_twin->m_next->m_twin->m_incidentVertex * stride]); + dgBigVector p2 (&pool[diagonal->m_prev->m_incidentVertex * stride]); + + dgBigVector e1 (p1 - p0); + dgFloat64 dot = e1.DotProduct3(e1); + if (dot < dgFloat64 (1.0e-12f)) { + return false; + } + e1 = e1.Scale (dgFloat64 (1.0f) / sqrt(dot)); + + dgBigVector e2 (p2 - p0); + dot = e2.DotProduct3(e2); + if (dot < dgFloat64 (1.0e-12f)) { + return false; + } + e2 = e2.Scale (dgFloat64 (1.0f) / sqrt(dot)); + + dgBigVector n1 (e1.CrossProduct(e2)); + + dot = normal.DotProduct3(n1); + if (dot >= dgFloat64 (0.0f)) { + return false; + } + } + return true; +} + + +bool dgPolyhedra::IsEssensialDiagonal (dgEdge* const diagonal, const dgBigVector& normal, const dgFloat64* const pool, dgInt32 stride) +{ + return IsEssensialPointDiagonal (diagonal, normal, pool, stride) || IsEssensialPointDiagonal (diagonal->m_twin, normal, pool, stride); +} + +dgObb dgPolyhedra::CalculateSphere (const dgFloat64* const vertex, dgInt32 strideInBytes, const dgMatrix* const basis) const +{ + dgInt32 stride = dgInt32 (strideInBytes / sizeof (dgFloat64)); + + dgInt32 vertexCount = 0; + dgInt32 mark = IncLRU(); + dgPolyhedra::Iterator iter(*this); + for (iter.Begin(); iter; iter ++) { + dgEdge* const edge = &(*iter); + if (edge->m_mark != mark) { + dgEdge* ptr = edge; + do { + ptr->m_mark = mark; + ptr = ptr->m_twin->m_next; + } while (ptr != edge); + vertexCount ++; + } + } + dgAssert (vertexCount); + + mark = IncLRU(); + dgInt32 vertexCountIndex = 0; + dgStack pool (vertexCount); + for (iter.Begin(); iter; iter ++) { + dgEdge* const edge = &(*iter); + if (edge->m_mark != mark) { + dgEdge* ptr = edge; + do { + ptr->m_mark = mark; + ptr = ptr->m_twin->m_next; + } while (ptr != edge); + dgInt32 incidentVertex = edge->m_incidentVertex * stride; + pool[vertexCountIndex] = dgBigVector (vertex[incidentVertex + 0], vertex[incidentVertex + 1], vertex[incidentVertex + 2], dgFloat32 (0.0f)); + vertexCountIndex ++; + } + } + dgAssert (vertexCountIndex <= vertexCount); + + dgMatrix axis (dgGetIdentityMatrix()); + dgObb sphere (axis); + dgConvexHull3d convexHull (GetAllocator(), &pool[0].m_x, sizeof (dgBigVector), vertexCountIndex, 0.0f); + if (convexHull.GetCount()) { + dgStack triangleList (convexHull.GetCount() * 3); + dgInt32 trianglesCount = 0; + for (dgConvexHull3d::dgListNode* node = convexHull.GetFirst(); node; node = node->GetNext()) { + dgConvexHull3DFace* const face = &node->GetInfo(); + triangleList[trianglesCount * 3 + 0] = face->m_index[0]; + triangleList[trianglesCount * 3 + 1] = face->m_index[1]; + triangleList[trianglesCount * 3 + 2] = face->m_index[2]; + trianglesCount ++; + dgAssert ((trianglesCount * 3) <= triangleList.GetElementsCount()); + } + + dgVector* const dst = (dgVector*) &pool[0].m_x; + for (dgInt32 i = 0; i < convexHull.GetVertexCount(); i ++) { + dst[i] = convexHull.GetVertex(i); + } + sphere.SetDimensions (&dst[0].m_x, sizeof (dgVector), &triangleList[0], trianglesCount * 3, NULL); + + } else if (vertexCountIndex >= 3) { + dgStack triangleList (GetCount() * 3 * 2); + dgInt32 mark1 = IncLRU(); + dgInt32 trianglesCount = 0; + for (iter.Begin(); iter; iter ++) { + dgEdge* const edge = &(*iter); + if (edge->m_mark != mark1) { + dgEdge* ptr = edge; + do { + ptr->m_mark = mark1; + ptr = ptr->m_twin->m_next; + } while (ptr != edge); + + ptr = edge->m_next->m_next; + do { + triangleList[trianglesCount * 3 + 0] = edge->m_incidentVertex; + triangleList[trianglesCount * 3 + 1] = ptr->m_prev->m_incidentVertex; + triangleList[trianglesCount * 3 + 2] = ptr->m_incidentVertex; + trianglesCount ++; + dgAssert ((trianglesCount * 3) <= triangleList.GetElementsCount()); + ptr = ptr->m_twin->m_next; + } while (ptr != edge); + + dgVector* const dst = (dgVector*) &pool[0].m_x; + for (dgInt32 i = 0; i < vertexCountIndex; i ++) { + dst[i] = pool[i]; + } + sphere.SetDimensions (&dst[0].m_x, sizeof (dgVector), &triangleList[0], trianglesCount * 3, NULL); + } + } + } + return sphere; +} + +dgBigPlane dgPolyhedra::EdgePlane (dgInt32 i0, dgInt32 i1, dgInt32 i2, const dgBigVector* const pool) const +{ + const dgBigVector& p0 = pool[i0]; + const dgBigVector& p1 = pool[i1]; + const dgBigVector& p2 = pool[i2]; + + dgBigPlane plane (p0, p1, p2); + dgFloat64 mag = sqrt (plane.DotProduct(plane & dgBigPlane::m_triplexMask).GetScalar()); + if (mag < dgFloat64 (1.0e-12f)) { + mag = dgFloat64 (1.0e-12f); + } + mag = dgFloat64 (1.0f) / mag; + + plane.m_x *= mag; + plane.m_y *= mag; + plane.m_z *= mag; + plane.m_w *= mag; + + return plane; +} + + +void dgPolyhedra::CalculateVertexMetrics (dgVertexCollapseVertexMetric* const table, const dgBigVector* const pool, dgEdge* const edge) const +{ + dgInt32 i0 = edge->m_incidentVertex; + + table[i0].Clear (); + dgEdge* ptr = edge; + do { + + if (ptr->m_incidentFace > 0) { + dgInt32 i1 = ptr->m_next->m_incidentVertex; + dgInt32 i2 = ptr->m_prev->m_incidentVertex; + dgBigPlane constrainPlane (EdgePlane (i0, i1, i2, pool)); + table[i0].Accumulate (constrainPlane); + + } else { + dgInt32 i1 = ptr->m_twin->m_incidentVertex; + dgInt32 i2 = ptr->m_twin->m_prev->m_incidentVertex; + dgBigPlane constrainPlane (UnboundedLoopPlane (i0, i1, i2, pool)); + table[i0].Accumulate (constrainPlane); + + i1 = ptr->m_prev->m_incidentVertex; + i2 = ptr->m_prev->m_twin->m_prev->m_incidentVertex; + constrainPlane = UnboundedLoopPlane (i0, i1, i2, pool); + table[i0].Accumulate (constrainPlane); + } + + ptr = ptr->m_twin->m_next; + } while (ptr != edge); +} + + +void dgPolyhedra::CalculateAllMetrics (dgVertexCollapseVertexMetric* const table, const dgBigVector* const pool) const +{ + dgInt32 edgeMark = IncLRU(); + dgPolyhedra::Iterator iter (*this); + for (iter.Begin(); iter; iter ++) { + dgEdge* const edge = &(*iter); + + dgAssert (edge); + if (edge->m_mark != edgeMark) { + + if (edge->m_incidentFace > 0) { + dgInt32 i0 = edge->m_incidentVertex; + dgInt32 i1 = edge->m_next->m_incidentVertex; + dgInt32 i2 = edge->m_prev->m_incidentVertex; + + dgBigPlane constrainPlane (EdgePlane (i0, i1, i2, pool)); + dgVertexCollapseVertexMetric tmp (constrainPlane); + + dgEdge* ptr = edge; + do { + ptr->m_mark = edgeMark; + i0 = ptr->m_incidentVertex; + table[i0].Accumulate(tmp); + + ptr = ptr->m_next; + } while (ptr != edge); + + } else { + dgAssert (edge->m_twin->m_incidentFace > 0); + dgInt32 i0 = edge->m_twin->m_incidentVertex; + dgInt32 i1 = edge->m_twin->m_next->m_incidentVertex; + dgInt32 i2 = edge->m_twin->m_prev->m_incidentVertex; + + edge->m_mark = edgeMark; + dgBigPlane constrainPlane (UnboundedLoopPlane (i0, i1, i2, pool)); + dgVertexCollapseVertexMetric tmp (constrainPlane); + + i0 = edge->m_incidentVertex; + table[i0].Accumulate(tmp); + + i0 = edge->m_twin->m_incidentVertex; + table[i0].Accumulate(tmp); + } + } + } +} + +bool dgPolyhedra::IsOkToCollapse (const dgBigVector* const pool, dgEdge* const edge) const +{ + const dgBigVector& q = pool[edge->m_incidentVertex]; + const dgBigVector& p = pool[edge->m_twin->m_incidentVertex]; + for (dgEdge* triangle = edge->m_prev->m_twin; triangle != edge->m_twin->m_next; triangle = triangle->m_prev->m_twin) { + if (triangle->m_incidentFace > 0) { + dgAssert ((edge->m_incidentFace < 0) || (edge->m_incidentVertex == edge->m_next->m_next->m_next->m_incidentVertex)); + + dgBigVector originalArea ((pool[triangle->m_next->m_incidentVertex] - q).CrossProduct(pool[triangle->m_prev->m_incidentVertex] - q)); + dgBigVector newArea ((pool[triangle->m_next->m_incidentVertex] - p).CrossProduct(pool[triangle->m_prev->m_incidentVertex] - p)); + + dgFloat64 projectedArea = newArea.DotProduct3(originalArea); + if (projectedArea <= dgFloat64 (0.0f)) { + return false; + } + + dgFloat64 mag20 = newArea.DotProduct3(newArea); + dgFloat64 mag21 = originalArea.DotProduct3(originalArea); + if ((projectedArea * projectedArea) < (mag20 * mag21 * dgFloat64 (1.0e-10f))) { + return false; + } + } + } + + return true; +} + + +dgEdge* dgPolyhedra::OptimizeCollapseEdge (dgEdge* const edge) +{ + dgInt32 v0 = edge->m_incidentVertex; + dgInt32 v1 = edge->m_twin->m_incidentVertex; + +#ifdef _DEBUG + dgPolyhedra::dgPairKey TwinKey (v1, v0); + dgPolyhedra::dgTreeNode* const node = Find (TwinKey.GetVal()); + dgEdge* const twin1 = node ? &node->GetInfo() : NULL; + dgAssert (twin1); + dgAssert (edge->m_twin == twin1); + dgAssert (twin1->m_twin == edge); + dgAssert (edge->m_incidentFace != 0); + dgAssert (twin1->m_incidentFace != 0); + dgAssert ((edge->m_incidentFace < 0) || (edge->m_incidentVertex == edge->m_next->m_next->m_next->m_incidentVertex)); + dgAssert ((edge->m_twin->m_incidentFace < 0) || (edge->m_twin->m_incidentVertex == edge->m_twin->m_next->m_next->m_next->m_incidentVertex)); +#endif + + dgEdge* retEdge = edge->m_twin->m_prev->m_twin; + if (retEdge == edge->m_twin->m_next) { + return NULL; + } + if (retEdge == edge->m_twin) { + return NULL; + } + if (retEdge == edge->m_next) { + retEdge = edge->m_prev->m_twin; + if (retEdge == edge->m_twin->m_next) { + return NULL; + } + if (retEdge == edge->m_twin) { + return NULL; + } + } + + dgEdge* lastEdge = NULL; + dgEdge* firstEdge = NULL; + if ((edge->m_incidentFace >= 0) && (edge->m_twin->m_incidentFace >= 0)) { + lastEdge = edge->m_prev->m_twin; + firstEdge = edge->m_twin->m_next->m_twin->m_next; + } else if (edge->m_twin->m_incidentFace >= 0) { + firstEdge = edge->m_twin->m_next->m_twin->m_next; + lastEdge = edge; + } else { + lastEdge = edge->m_prev->m_twin; + firstEdge = edge->m_twin->m_next; + } + + for (dgEdge* ptr = firstEdge; ptr != lastEdge; ptr = ptr->m_twin->m_next) { + dgEdge* badEdge = FindEdge (edge->m_twin->m_incidentVertex, ptr->m_twin->m_incidentVertex); + if (badEdge) { + return NULL; + } + } + + dgEdge* const twin = edge->m_twin; + if (twin->m_next == twin->m_prev->m_prev) { + twin->m_prev->m_twin->m_twin = twin->m_next->m_twin; + twin->m_next->m_twin->m_twin = twin->m_prev->m_twin; + + RemoveHalfEdge (twin->m_prev); + RemoveHalfEdge (twin->m_next); + } else { + twin->m_next->m_prev = twin->m_prev; + twin->m_prev->m_next = twin->m_next; + } + + if (edge->m_next == edge->m_prev->m_prev) { + edge->m_next->m_twin->m_twin = edge->m_prev->m_twin; + edge->m_prev->m_twin->m_twin = edge->m_next->m_twin; + RemoveHalfEdge (edge->m_next); + RemoveHalfEdge (edge->m_prev); + } else { + edge->m_next->m_prev = edge->m_prev; + edge->m_prev->m_next = edge->m_next; + } + + dgAssert (twin->m_twin->m_incidentVertex == v0); + dgAssert (edge->m_twin->m_incidentVertex == v1); + RemoveHalfEdge (twin); + RemoveHalfEdge (edge); + + dgEdge* remapPtr = retEdge; + do { + dgPolyhedra::dgPairKey pairKey (v0, remapPtr->m_twin->m_incidentVertex); + dgPolyhedra::dgTreeNode* const pairEdgeNode = Find (pairKey.GetVal()); + if (pairEdgeNode) { + if (&pairEdgeNode->GetInfo() == remapPtr) { + dgPolyhedra::dgPairKey key (v1, remapPtr->m_twin->m_incidentVertex); + remapPtr->m_incidentVertex = v1; + ReplaceKey (pairEdgeNode, key.GetVal()); + } + } + + dgPolyhedra::dgPairKey twinKey1 (remapPtr->m_twin->m_incidentVertex, v0); + dgPolyhedra::dgTreeNode* const pairTwinNode = Find (twinKey1.GetVal()); + if (pairTwinNode) { + if (&pairTwinNode->GetInfo() == remapPtr->m_twin) { + dgPolyhedra::dgPairKey key (remapPtr->m_twin->m_incidentVertex, v1); + ReplaceKey (pairTwinNode, key.GetVal()); + } + } + + remapPtr = remapPtr->m_twin->m_next; + } while (remapPtr != retEdge); + + return retEdge; +} + + +dgFloat64 dgPolyhedra::EdgePenalty (const dgBigVector* const pool, dgEdge* const edge, dgFloat64 dist) const +{ + dgInt32 i0 = edge->m_incidentVertex; + dgInt32 i1 = edge->m_next->m_incidentVertex; + + dgFloat32 maxPenalty = dgFloat32 (1.0e14f); + + const dgBigVector& p0 = pool[i0]; + const dgBigVector& p1 = pool[i1]; + dgBigVector dp (p1 - p0); + + dgFloat64 dot = dp.DotProduct3(dp); + if (dot < dgFloat64(1.0e-6f)) { + return dist * maxPenalty; + } + + if ((edge->m_incidentFace > 0) && (edge->m_twin->m_incidentFace > 0)) { + dgBigVector edgeNormal (FaceNormal (edge, &pool[0].m_x, sizeof (dgBigVector))); + dgBigVector twinNormal (FaceNormal (edge->m_twin, &pool[0].m_x, sizeof (dgBigVector))); + + dgFloat64 mag0 = edgeNormal.DotProduct3(edgeNormal); + dgFloat64 mag1 = twinNormal.DotProduct3(twinNormal); + if ((mag0 < dgFloat64 (1.0e-24f)) || (mag1 < dgFloat64 (1.0e-24f))) { + return dist * maxPenalty; + } + + edgeNormal = edgeNormal.Scale (dgFloat64 (1.0f) / sqrt(mag0)); + twinNormal = twinNormal.Scale (dgFloat64 (1.0f) / sqrt(mag1)); + + dot = edgeNormal.DotProduct3(twinNormal); + if (dot < dgFloat64 (-0.9f)) { + return dist * maxPenalty; + } + + dgEdge* ptr = edge; + do { + if ((ptr->m_incidentFace <= 0) || (ptr->m_twin->m_incidentFace <= 0)){ + dgEdge* const adj = edge->m_twin; + ptr = edge; + do { + if ((ptr->m_incidentFace <= 0) || (ptr->m_twin->m_incidentFace <= 0)){ + return dist * maxPenalty; + } + ptr = ptr->m_twin->m_next; + } while (ptr != adj); + } + ptr = ptr->m_twin->m_next; + } while (ptr != edge); + } + + dgInt32 faceA = edge->m_incidentFace; + dgInt32 faceB = edge->m_twin->m_incidentFace; + + i0 = edge->m_twin->m_incidentVertex; + dgBigVector p (pool[i0].m_x, pool[i0].m_y, pool[i0].m_z, dgFloat32 (0.0f)); + + bool penalty = false; + dgEdge* ptr = edge; + do { + dgEdge* const adj = ptr->m_twin; + + dgInt32 face = adj->m_incidentFace; + if ((face != faceB) && (face != faceA) && (face >= 0) && (adj->m_next->m_incidentFace == face) && (adj->m_prev->m_incidentFace == face)){ + + dgInt32 k0 = adj->m_next->m_incidentVertex; + const dgBigVector& q0 = pool[k0]; + + dgInt32 k1 = adj->m_incidentVertex; + const dgBigVector& q1 = pool[k1]; + + dgInt32 k2 = adj->m_prev->m_incidentVertex; + const dgBigVector& q2 = pool[k2]; + + dgBigVector n0 ((q1 - q0).CrossProduct(q2 - q0)); + dgBigVector n1 ((q1 - p).CrossProduct(q2 - p)); + dgFloat64 project = n0.DotProduct3(n1); + if (project < dgFloat64 (0.0f)) { + penalty = true; + break; + } + } + + ptr = ptr->m_twin->m_next; + } while (ptr != edge); + + dgFloat64 aspect = dgFloat32 (0.0f); + if (!penalty) { + dgInt32 k0 = edge->m_twin->m_incidentVertex; + dgBigVector q0 (pool[k0]); + + aspect = dgFloat32 (1.0f); + for (dgEdge* ptr1 = edge->m_twin->m_next->m_twin->m_next; ptr1 != edge; ptr1 = ptr1->m_twin->m_next) { + if (ptr1->m_incidentFace > 0) { + dgInt32 k1 = ptr1->m_next->m_incidentVertex; + const dgBigVector& q1 = pool[k1]; + + dgInt32 k2 = ptr1->m_prev->m_incidentVertex; + const dgBigVector& q2 = pool[k2]; + + dgBigVector e0 (q1 - q0); + dgBigVector e1 (q2 - q1); + dgBigVector e2 (q0 - q2); + + dgFloat64 mag0 = e0.DotProduct3(e0); + dgFloat64 mag1 = e1.DotProduct3(e1); + dgFloat64 mag2 = e2.DotProduct3(e2); + dgFloat64 maxMag = dgMax (mag0, mag1, mag2); + dgFloat64 minMag = dgMin (mag0, mag1, mag2); + dgFloat64 ratio = minMag / maxMag; + + if (ratio < aspect) { + aspect = ratio; + } + } + } + aspect = dgFloat32 (1.0f) - aspect; + } + return aspect * aspect * dist; +} + + +bool dgPolyhedra::Optimize (const dgFloat64* const array, dgInt32 strideInBytes, dgReportProgress normalizedProgress, void* const reportProgressUserData, dgFloat64 tol, dgInt32 maxFaceCount) +{ + dgInt32 stride = dgInt32 (strideInBytes / sizeof (dgFloat64)); + +#ifdef __ENABLE_DG_CONTAINERS_SANITY_CHECK + dgAssert (SanityCheck ()); +#endif + + dgFloat32 progressDen = dgFloat32 (1.0f / GetEdgeCount()); + dgInt32 edgeCount = GetEdgeCount() * 4 + DG_LOCAL_BUFFER_SIZE * 16; + dgInt32 maxVertexIndex = GetLastVertexIndex(); + + dgStack vertexPool (maxVertexIndex); + dgStack vertexMetrics (maxVertexIndex + 512); + + dgList edgeHandleList(GetAllocator()); + dgStack heapPool (2 * edgeCount * dgInt32 (sizeof (dgFloat64) + sizeof (dgEdgeCollapseEdgeHandle*) + sizeof (dgInt32))); + dgUpHeap::dgListNode* , dgFloat64> bigHeapArray(&heapPool[0], heapPool.GetSizeInBytes()); + + for (dgInt32 i = 0; i < maxVertexIndex; i ++) { + vertexPool[i].m_x = array[i * stride + 0]; + vertexPool[i].m_y = array[i * stride + 1]; + vertexPool[i].m_z = array[i * stride + 2]; + vertexPool[i].m_w= dgFloat64 (0.0f); + } + + memset (&vertexMetrics[0], 0, maxVertexIndex * sizeof (dgVertexCollapseVertexMetric)); + CalculateAllMetrics (&vertexMetrics[0], &vertexPool[0]); + + const dgFloat64 maxCost = dgFloat32 (1.0e-3f); + dgFloat64 tol2 = tol * tol; + dgFloat64 distTol = dgMax (tol2, dgFloat64 (1.0e-12f)); + Iterator iter (*this); + for (iter.Begin(); iter; iter ++) { + dgEdge* const edge = &(*iter); + + edge->m_userData = 0; + dgInt32 index0 = edge->m_incidentVertex; + dgInt32 index1 = edge->m_twin->m_incidentVertex; + + dgVertexCollapseVertexMetric &metric = vertexMetrics[index0]; + const dgBigVector& p = vertexPool[index1]; + dgFloat64 faceCost = metric.Evalue (p); + dgFloat64 edgePenalty = EdgePenalty (&vertexPool[0], edge, distTol); + dgAssert (edgePenalty >= dgFloat32 (0.0f)); + dgEdgeCollapseEdgeHandle handle (edge); + dgList ::dgListNode* handleNodePtr = edgeHandleList.Addtop (handle); + bigHeapArray.Push (handleNodePtr, faceCost + edgePenalty); + } + + bool progress = true; + dgInt32 interPasses = 0; + dgInt32 faceCount = GetFaceCount(); + while (bigHeapArray.GetCount() && (bigHeapArray.Value() < maxCost) && ((bigHeapArray.Value() < tol2) || (faceCount > maxFaceCount)) && progress ) { + dgList ::dgListNode* const handleNodePtr = bigHeapArray[0]; + + dgEdge* edge = handleNodePtr->GetInfo().m_edge; + bigHeapArray.Pop(); + edgeHandleList.Remove (handleNodePtr); + + if (edge) { + if (IsOkToCollapse (&vertexPool[0], edge)) { + + if (normalizedProgress) { + interPasses ++; + faceCount -= 2; + if (interPasses >= 400) { + interPasses = 0; + faceCount = GetFaceCount(); + progress = normalizedProgress(dgFloat32 (1.0f) - GetEdgeCount() * progressDen, reportProgressUserData); + } + } + + if (bigHeapArray.GetCount() > (bigHeapArray.GetMaxCount() - 100)) { + for(dgInt32 i = bigHeapArray.GetCount() - 1; i >= 0; i --) { + dgList ::dgListNode* const emptyHandle = bigHeapArray[i]; + if (!emptyHandle->GetInfo().m_edge) { + bigHeapArray.Remove(i); + edgeHandleList.Remove (emptyHandle); + } + } + } + +#ifdef __ENABLE_DG_CONTAINERS_SANITY_CHECK + dgAssert (SanityCheck ()); +#endif + + edge = OptimizeCollapseEdge(edge); + +#ifdef __ENABLE_DG_CONTAINERS_SANITY_CHECK + dgAssert (SanityCheck ()); +#endif + if (edge) { + // Update vertex metrics + CalculateVertexMetrics (&vertexMetrics[0], &vertexPool[0], edge); + + // Update metrics for all surrounding vertex + dgEdge* ptr = edge; + do { + CalculateVertexMetrics (&vertexMetrics[0], &vertexPool[0], ptr->m_twin); + ptr = ptr->m_twin->m_next; + } while (ptr != edge); + + // calculate edge cost of all incident edges + dgInt32 mark = IncLRU(); + ptr = edge; + do { + dgAssert (ptr->m_mark != mark); + ptr->m_mark = mark; + + dgInt32 index0 = ptr->m_incidentVertex; + dgInt32 index1 = ptr->m_twin->m_incidentVertex; + + dgVertexCollapseVertexMetric &metric = vertexMetrics[index0]; + const dgBigVector& p = vertexPool[index1]; + + dgFloat64 faceCost = metric.Evalue (p); + dgFloat64 edgePenalty = EdgePenalty (&vertexPool[0], ptr, distTol); + dgAssert (edgePenalty >= dgFloat32 (0.0f)); + dgEdgeCollapseEdgeHandle handle (ptr); + dgList ::dgListNode* handleNodePtr1 = edgeHandleList.Addtop (handle); + bigHeapArray.Push (handleNodePtr1, faceCost + edgePenalty); + + ptr = ptr->m_twin->m_next; + } while (ptr != edge); + + + // calculate edge cost of all incident edges to a surrounding vertex + ptr = edge; + do { + dgEdge* const incidentEdge = ptr->m_twin; + + dgEdge* ptr1 = incidentEdge; + do { + dgInt32 index0 = ptr1->m_incidentVertex; + dgInt32 index1 = ptr1->m_twin->m_incidentVertex; + + if (ptr1->m_mark != mark) { + ptr1->m_mark = mark; + dgVertexCollapseVertexMetric &metric = vertexMetrics[index0]; + const dgBigVector& p = vertexPool[index1]; + + dgFloat64 faceCost = metric.Evalue (p); + dgFloat64 edgePenalty = EdgePenalty (&vertexPool[0], ptr1, distTol); + dgAssert (edgePenalty >= dgFloat32 (0.0f)); + dgEdgeCollapseEdgeHandle handle (ptr1); + dgList ::dgListNode* handleNodePtr1 = edgeHandleList.Addtop (handle); + bigHeapArray.Push (handleNodePtr1, faceCost + edgePenalty); + } + + if (ptr1->m_twin->m_mark != mark) { + ptr1->m_twin->m_mark = mark; + dgVertexCollapseVertexMetric &metric = vertexMetrics[index1]; + const dgBigVector& p = vertexPool[index0]; + dgFloat64 faceCost = metric.Evalue (p); + dgFloat64 edgePenalty = EdgePenalty (&vertexPool[0], ptr1->m_twin, distTol); + dgAssert (edgePenalty >= dgFloat32 (0.0f)); + dgEdgeCollapseEdgeHandle handle (ptr1->m_twin); + dgList ::dgListNode* handleNodePtr1 = edgeHandleList.Addtop (handle); + bigHeapArray.Push (handleNodePtr1, faceCost + edgePenalty); + } + + ptr1 = ptr1->m_twin->m_next; + } while (ptr1 != incidentEdge); + + ptr = ptr->m_twin->m_next; + } while (ptr != edge); + } + } + } + } + + if (normalizedProgress && progress) { + progress = normalizedProgress(dgFloat32 (1.0f), reportProgressUserData); + } + return progress; +} + +bool dgPolyhedra::TriangulateFace(dgEdge* const face, const dgFloat64* const pool, dgInt32 strideInBytes) +{ + if (face->m_next->m_next->m_next != face) { + dgInt32 mark = IncLRU(); + dgEdge* ptr = face; + do { + ptr->m_mark = mark; + ptr = ptr->m_next; + } while (ptr != face); + char memPool[DG_LOCAL_BUFFER_SIZE * (sizeof (dgEdge*)+sizeof (dgFloat64))]; + dgDownHeap heap(&memPool[0], sizeof (memPool)); + + dgInt32 stride = dgInt32(strideInBytes / sizeof (dgFloat64)); + dgEdge* const edge = TriangulateFace(face, pool, stride, heap, NULL); + dgAssert(!edge); + return !edge; + } + return true; +} + + +dgEdge* dgPolyhedra::BestEdgePolygonizeFace(const dgBigVector& normal, dgEdge* const edge, const dgFloat64* const pool, dgInt32 stride, const dgBigVector& point) const +{ + dgBigVector p0(&pool[edge->m_incidentVertex * stride]); + dgBigVector r(point - p0); + dgEdge* e0 = edge; + do { + dgBigVector p1(&pool[e0->m_twin->m_incidentVertex * stride]); + dgBigVector p2(&pool[e0->m_prev->m_incidentVertex * stride]); + //dgFloat64 test0 = (normal * (p1 - p0)) % r; + //dgFloat64 test1 = ((p2 - p0) * normal) % r; + dgFloat64 test0 = r.DotProduct3(normal.CrossProduct(p1 - p0)); + dgFloat64 test1 = r.DotProduct3((p2 - p0).CrossProduct(normal)); + + if ((test0 > 0.0f) && (test1 > 0.0f)) { + break; + } + e0 = e0->m_prev->m_twin; + } while (e0 != edge); + return e0; +} + +bool dgPolyhedra::PolygonizeFace(dgEdge* const face, const dgFloat64* const pool, dgInt32 strideInBytes) +{ + dgPolyhedra flatFace(GetAllocator()); + dgEdge* array[DG_LOCAL_BUFFER_SIZE]; + + dgInt32 count = 0; + dgEdge* edge = face; + do { + dgEdge* const perimeter = flatFace.AddHalfEdge (edge->m_incidentVertex, edge->m_twin->m_incidentVertex); + dgAssert (perimeter); + perimeter->m_userData = edge->m_userData; + perimeter->m_incidentFace = 1; + perimeter->m_twin = NULL; + perimeter->m_prev = NULL; + perimeter->m_next = NULL; + + array[count] = perimeter; + count++; + dgAssert(count <= DG_LOCAL_BUFFER_SIZE); + edge = edge->m_next; + } while (edge != face); + + dgInt32 i0 = count - 1; + for(dgInt32 i = 0; i < count; i ++) { + dgEdge* const edge1 = array[i]; + dgEdge* const prev1 = array[i0]; + + edge1->m_prev = prev1; + prev1->m_next = edge1; + i0 = i; + } + + for(dgInt32 i = 0; i < count; i ++) { + dgEdge* const edge1 = array[i]; + dgEdge* const twin1 = flatFace.FindEdge (edge1->m_next->m_incidentVertex, edge1->m_incidentVertex); + if (twin1) { + twin1->m_twin = edge1; + edge1->m_twin = twin1; + } else { + dgEdge* const perimeter = flatFace.AddHalfEdge (edge1->m_next->m_incidentVertex, edge1->m_incidentVertex); + perimeter->m_twin = edge1; + edge1->m_twin = perimeter; + perimeter->m_incidentFace = -1; + perimeter->m_prev = NULL; + perimeter->m_next = NULL; + } + } + + for (dgInt32 i = 0; i < count; i++) { + dgEdge* const edge1 = array[i]; + dgEdge* const twin1 = edge1->m_twin; + if (!twin1->m_next) { + dgEdge* next = edge1->m_prev->m_twin; + while (next->m_prev) { + next = next->m_prev->m_twin; + } + twin1->m_next = next; + next->m_prev = next; + } + } + + dgBigVector normal (flatFace.FaceNormal(array[0], pool, strideInBytes)); + if (flatFace.TriangulateFace(array[0], pool, strideInBytes)) { + dgInt32 stride = dgInt32(strideInBytes / sizeof (dgFloat64)); + flatFace.RefineTriangulation(pool, stride); + + //RemoveOuterColinearEdges(*this, vertex, stride); + dgInt32 polygon[DG_LOCAL_BUFFER_SIZE]; + dgEdge* diagonalsPool[DG_LOCAL_BUFFER_SIZE]; + + dgInt32 diagonalCount = GetInteriorDiagonals(flatFace, diagonalsPool, sizeof (diagonalsPool) / sizeof (diagonalsPool[0])); + + if (diagonalCount) { + dgEdge* edge1 = &flatFace.GetRoot()->GetInfo(); + if (edge1->m_incidentFace < 0) { + edge1 = edge1->m_twin; + } + + dgAssert(edge1->m_incidentFace > 0); + + dgBigVector normal1(flatFace.FaceNormal(edge1, pool, strideInBytes)); + normal1 = normal1.Scale(dgFloat64(1.0f) / sqrt(normal1.DotProduct3(normal1))); + + edge1 = NULL; + dgPolyhedra::Iterator iter0(flatFace); + for (iter0.Begin(); iter0; iter0++) { + edge1 = &(*iter0); + if (edge1->m_incidentFace < 0) { + break; + } + } + dgAssert(edge1); + + dgInt32 isConvex = 1; + dgEdge* ptr = edge1; + dgInt32 mark = flatFace.IncLRU(); + + dgBigVector normal2(normal1); + dgBigVector p0(&pool[ptr->m_prev->m_incidentVertex * stride]); + dgBigVector p1(&pool[ptr->m_incidentVertex * stride]); + dgBigVector e0(p1 - p0); + e0 = e0.Scale(dgFloat64(1.0f) / sqrt(e0.DotProduct3(e0) + dgFloat64(1.0e-24f))); + do { + dgBigVector p2(&pool[ptr->m_next->m_incidentVertex * stride]); + dgBigVector e1(p2 - p1); + e1 = e1.Scale(dgFloat64(1.0f) / sqrt(e1.DotProduct3(e1) + dgFloat32(1.0e-24f))); + dgFloat64 dot = normal2.DotProduct3(e0.CrossProduct(e1)); + + if (dot > dgFloat32(5.0e-3f)) { + isConvex = 0; + break; + } + ptr->m_mark = mark; + e0 = e1; + p1 = p2; + ptr = ptr->m_next; + } while (ptr != edge1); + + if (isConvex) { + dgPolyhedra::Iterator iter(flatFace); + for (iter.Begin(); iter; iter++) { + ptr = &(*iter); + if (ptr->m_incidentFace < 0) { + if (ptr->m_mark < mark) { + isConvex = 0; + break; + } + } + } + } + + if (isConvex) { + if (diagonalCount > 2) { + dgInt32 count1 = 0; + ptr = edge1; + do { + polygon[count1] = ptr->m_incidentVertex; + count1++; + dgAssert(count1 < dgInt32(sizeof (polygon) / sizeof (polygon[0]))); + ptr = ptr->m_next; + } while (ptr != edge1); + + for (dgInt32 i = 0; i < count1 - 1; i++) { + for (dgInt32 j = i + 1; j < count1; j++) { + if (polygon[i] == polygon[j]) { + i = count1; + isConvex = 0; + break; + } + } + } + } + } + + if (isConvex) { + for (dgInt32 j = 0; j < diagonalCount; j++) { + dgEdge* const diagonal = diagonalsPool[j]; + flatFace.DeleteEdge(diagonal); + } + } else { + for (dgInt32 j = 0; j < diagonalCount; j++) { + dgEdge* const diagonal = diagonalsPool[j]; + if (!IsEssensialDiagonal(diagonal, normal1, pool, stride)) { + flatFace.DeleteEdge(diagonal); + } + } + } + } + + dgInt32 mark = flatFace.IncLRU(); + dgPolyhedra::Iterator iter0(flatFace); + for (iter0.Begin(); iter0; iter0++) { + dgEdge* const edge1 = &(*iter0); + if ((edge1->m_mark != mark) && (edge1->m_incidentFace > 0)) { + edge1->m_mark = mark; + edge1->m_twin->m_mark = mark; + if (!FindEdge(edge1->m_incidentVertex, edge1->m_twin->m_incidentVertex)) { + dgPairKey key0 (edge1->m_incidentVertex, 0); + dgPairKey key1 (edge1->m_twin->m_incidentVertex, 0); + dgTreeNode* const node0 = FindGreater (key0.GetVal()); + dgTreeNode* const node1 = FindGreater (key1.GetVal()); + dgAssert (node0); + dgAssert (node1); + dgEdge* e0 = &node0->GetInfo(); + dgEdge* e1 = &node1->GetInfo(); + + dgBigVector p0 (&pool[e0->m_incidentVertex * stride]); + dgBigVector p1 (&pool[e1->m_incidentVertex * stride]); + e0 = BestEdgePolygonizeFace (normal, e0, pool, stride, p1); + e1 = BestEdgePolygonizeFace (normal, e1, pool, stride, p0); + ConnectVertex (e0, e1); + } + } + } + } + + return true; +} + +void dgPolyhedra::RemoveInteriorEdges (dgPolyhedra& buildConvex, const dgFloat64* const vertex, dgInt32 strideInBytes) +{ + dgInt32 polygon[DG_LOCAL_BUFFER_SIZE * 8]; + dgEdge* diagonalsPool[DG_LOCAL_BUFFER_SIZE * 8]; + + dgInt32 stride = dgInt32 (strideInBytes / sizeof (dgFloat64)); + + buildConvex.BeginFace(); + dgPolyhedra::Iterator iter(*this); + for (iter.Begin(); iter;) { + dgEdge* edge = &(*iter); + iter++; + if (edge->m_incidentFace > 0) { + + dgPolyhedra flatFace(GetAllocator()); + MarkAdjacentCoplanarFaces(flatFace, edge, vertex, strideInBytes); + if (flatFace.GetCount()) { + //flatFace.RefineTriangulation(vertex, stride); + RemoveOuterColinearEdges(flatFace, vertex, stride); + RemoveInteriorColinearEdges(flatFace, vertex, stride); + flatFace.RefineTriangulation(vertex, stride); + + dgInt32 diagonalCount = GetInteriorDiagonals(flatFace, diagonalsPool, sizeof(diagonalsPool) / sizeof(diagonalsPool[0])); + if (diagonalCount) { + edge = &flatFace.GetRoot()->GetInfo(); + if (edge->m_incidentFace < 0) { + edge = edge->m_twin; + } + dgAssert(edge->m_incidentFace > 0); + + dgBigVector normal(FaceNormal(edge, vertex, strideInBytes)); + normal = normal.Scale(dgFloat64(1.0f) / sqrt(normal.DotProduct3(normal))); + + edge = NULL; + dgPolyhedra::Iterator iter1(flatFace); + for (iter1.Begin(); iter1; iter1++) { + edge = &(*iter1); + if (edge->m_incidentFace < 0) { + break; + } + } + dgAssert(edge); + + dgInt32 isConvex = 1; + dgEdge* ptr = edge; + dgInt32 mark = flatFace.IncLRU(); + + dgBigVector normal2(normal); + dgBigVector p0(&vertex[ptr->m_prev->m_incidentVertex * stride]); + dgBigVector p1(&vertex[ptr->m_incidentVertex * stride]); + dgBigVector e0(p1 - p0); + e0 = e0.Scale(dgFloat64(1.0f) / sqrt(e0.DotProduct3(e0) + dgFloat64(1.0e-24f))); + do { + dgBigVector p2(&vertex[ptr->m_next->m_incidentVertex * stride]); + dgBigVector e1(p2 - p1); + e1 = e1.Scale(dgFloat64(1.0f) / sqrt(e1.DotProduct3(e1) + dgFloat32(1.0e-24f))); + dgFloat64 dot = normal2.DotProduct3(e0.CrossProduct(e1)); + + if (dot > dgFloat32(5.0e-3f)) { + isConvex = 0; + break; + } + ptr->m_mark = mark; + e0 = e1; + p1 = p2; + ptr = ptr->m_next; + } while (ptr != edge); + + if (isConvex) { + dgPolyhedra::Iterator iter2(flatFace); + for (iter2.Begin(); iter2; iter2++) { + ptr = &(*iter2); + if (ptr->m_incidentFace < 0) { + if (ptr->m_mark < mark) { + isConvex = 0; + break; + } + } + } + } + + if (isConvex) { + if (diagonalCount > 2) { + dgInt32 count = 0; + ptr = edge; + do { + polygon[count] = ptr->m_incidentVertex; + count++; + dgAssert(count < dgInt32(sizeof(polygon) / sizeof(polygon[0]))); + ptr = ptr->m_next; + } while (ptr != edge); + + for (dgInt32 i = 0; i < count - 1; i++) { + for (dgInt32 j = i + 1; j < count; j++) { + if (polygon[i] == polygon[j]) { + i = count; + isConvex = 0; + break; + } + } + } + } + } + + if (isConvex) { + for (dgInt32 j = 0; j < diagonalCount; j++) { + dgEdge* const diagonal = diagonalsPool[j]; + flatFace.DeleteEdge(diagonal); + } + } else { + for (dgInt32 j = 0; j < diagonalCount; j++) { + dgEdge* const diagonal = diagonalsPool[j]; + if (!IsEssensialDiagonal(diagonal, normal, vertex, stride)) { + flatFace.DeleteEdge(diagonal); + } + } + } + } + + dgInt32 mark = flatFace.IncLRU(); + dgPolyhedra::Iterator iter1(flatFace); + for (iter1.Begin(); iter1; iter1++) { + dgEdge* const edge1 = &(*iter1); + if (edge1->m_mark != mark) { + if (edge1->m_incidentFace > 0) { + dgEdge* ptr = edge1; + dgInt32 diagonalCount1 = 0; + do { + polygon[diagonalCount1] = ptr->m_incidentVertex; + diagonalCount1++; + dgAssert(diagonalCount1 < dgInt32(sizeof(polygon) / sizeof(polygon[0]))); + ptr->m_mark = mark; + ptr = ptr->m_next; + } while (ptr != edge1); + if (diagonalCount1 >= 3) { + buildConvex.AddFace(diagonalCount1, polygon); + } + } + } + } + } + + iter.Begin(); + } + } + + buildConvex.EndFace(); + dgAssert(GetCount() == 0); +} + +void dgPolyhedra::ConvexPartition (const dgFloat64* const vertex, dgInt32 strideInBytes, dgPolyhedra* const leftOversOut) +{ + if (GetCount()) { + Triangulate (vertex, strideInBytes, leftOversOut); + DeleteDegenerateFaces (vertex, strideInBytes, dgFloat32 (1.0e-5f)); + Optimize (vertex, strideInBytes, NULL, NULL, dgFloat32 (1.0e-3f)); + DeleteDegenerateFaces (vertex, strideInBytes, dgFloat32 (1.0e-5f)); + + if (GetCount()) { + dgPolyhedra buildConvex(GetAllocator()); + RemoveInteriorEdges (buildConvex, vertex, strideInBytes); + SwapInfo(buildConvex); + } + } +} diff --git a/thirdparty/src/newton/dgCore/dgPolyhedra.h b/thirdparty/src/newton/dgCore/dgPolyhedra.h new file mode 100644 index 000000000..03b594088 --- /dev/null +++ b/thirdparty/src/newton/dgCore/dgPolyhedra.h @@ -0,0 +1,325 @@ +/* Copyright (c) <2003-2019> +* +* 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 +{ + 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& heap, const dgBigVector &normal) const; + dgEdge* TriangulateFace (dgEdge* const face, const dgFloat64* const pool, dgInt32 stride, dgDownHeap& 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 + diff --git a/thirdparty/src/newton/dgCore/dgPolyhedraMassProperties.cpp b/thirdparty/src/newton/dgCore/dgPolyhedraMassProperties.cpp new file mode 100644 index 000000000..a26ea96ac --- /dev/null +++ b/thirdparty/src/newton/dgCore/dgPolyhedraMassProperties.cpp @@ -0,0 +1,573 @@ +/* Copyright (c) <2003-2019> +* +* 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]; +} + + + diff --git a/thirdparty/src/newton/dgCore/dgPolyhedraMassProperties.h b/thirdparty/src/newton/dgCore/dgPolyhedraMassProperties.h new file mode 100644 index 000000000..751745de7 --- /dev/null +++ b/thirdparty/src/newton/dgCore/dgPolyhedraMassProperties.h @@ -0,0 +1,41 @@ +/* Copyright (c) <2003-2019> +* +* 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 diff --git a/thirdparty/src/newton/dgCore/dgProfiler.cpp b/thirdparty/src/newton/dgCore/dgProfiler.cpp new file mode 100644 index 000000000..c0bf7165d --- /dev/null +++ b/thirdparty/src/newton/dgCore/dgProfiler.cpp @@ -0,0 +1,23 @@ +/* Copyright (c) <2003-2019> +* +* 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" diff --git a/thirdparty/src/newton/dgCore/dgProfiler.h b/thirdparty/src/newton/dgCore/dgProfiler.h new file mode 100644 index 000000000..9b6adfb8d --- /dev/null +++ b/thirdparty/src/newton/dgCore/dgProfiler.h @@ -0,0 +1,39 @@ +/* Copyright (c) <2003-2019> +* +* 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_PROFILER_H__ +#define __DG_PROFILER_H__ + +// to make a profile build use Use CMAKE to create a profile configuration +// or make a configuration that define macro D_PROFILER + +#ifdef D_PROFILER + #include + #define D_TRACKTIME() dProfilerZoneScoped(__FUNCTION__) + #define D_SET_TRACK_NAME(trackName) dProfilerSetTrackName(trackName) + #define DG_TRACKTIME() D_TRACKTIME() +#else + #define D_TRACKTIME() + #define D_SET_TRACK_NAME(trackName) + #define DG_TRACKTIME() +#endif + +#endif diff --git a/thirdparty/src/newton/dgCore/dgQuaternion.cpp b/thirdparty/src/newton/dgCore/dgQuaternion.cpp new file mode 100644 index 000000000..4190c26e1 --- /dev/null +++ b/thirdparty/src/newton/dgCore/dgQuaternion.cpp @@ -0,0 +1,172 @@ +/* Copyright (c) <2003-2019> +* +* 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 "dgMatrix.h" +#include "dgQuaternion.h" + +enum QUAT_INDEX +{ + X_INDEX = 0, + Y_INDEX = 1, + Z_INDEX = 2 +}; +static QUAT_INDEX QIndex[] = { Y_INDEX, Z_INDEX, X_INDEX }; + +dgQuaternion::dgQuaternion (const dgMatrix& matrix) +{ + dgFloat32 trace = matrix[0][0] + matrix[1][1] + matrix[2][2]; + if (trace > dgFloat32(0.0f)) { + trace = dgSqrt (trace + dgFloat32(1.0f)); + m_w = dgFloat32 (0.5f) * trace; + trace = dgFloat32 (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 = dgFloat32(1.0f) + matrix[i][i] - matrix[j][j] - matrix[k][k]; + trace = dgSqrt (trace); + + dgFloat32* const ptr = &m_x; + ptr[i] = dgFloat32 (0.5f) * trace; + trace = dgFloat32 (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; + } + +#ifdef _DEBUG + dgMatrix tmp (*this, matrix.m_posit); + dgMatrix unitMatrix (tmp * matrix.Inverse()); + for (dgInt32 i = 0; i < 4; i ++) { + dgFloat32 err = dgAbs (unitMatrix[i][i] - dgFloat32(1.0f)); + dgAssert (err < dgFloat32 (1.0e-2f)); + } + + dgFloat32 err = dgAbs (DotProduct(*this) - dgFloat32(1.0f)); + dgAssert (err < dgFloat32(dgEpsilon * 100.0f)); +#endif +} + +dgQuaternion::dgQuaternion (const dgVector &unitAxis, dgFloat32 angle) +{ + angle *= dgFloat32 (0.5f); + m_w = dgCos (angle); + dgFloat32 sinAng = dgSin (angle); + +#ifdef _DEBUG + if (dgAbs (angle) > dgFloat32(dgEpsilon / 10.0f)) { + dgAssert (dgAbs (dgFloat32(1.0f) - unitAxis.DotProduct(unitAxis & dgVector::m_triplexMask).GetScalar()) < dgFloat32(dgEpsilon * 10.0f)); + } +#endif + m_x = unitAxis.m_x * sinAng; + m_y = unitAxis.m_y * sinAng; + m_z = unitAxis.m_z * sinAng; + +} + +dgVector dgQuaternion::CalcAverageOmega (const dgQuaternion &q1, dgFloat32 invdt) const +{ + dgQuaternion q0 (*this); + if (q0.DotProduct (q1) < 0.0f) { + q0.Scale(-1.0f); + } + dgQuaternion dq (q0.Inverse() * q1); + dgVector omegaDir (dq.m_x, dq.m_y, dq.m_z, dgFloat32 (0.0f)); + + dgFloat32 dirMag2 = omegaDir.DotProduct(omegaDir).GetScalar(); + if (dirMag2 < dgFloat32(dgFloat32 (1.0e-5f) * dgFloat32 (1.0e-5f))) { + return dgVector (dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f)); + } + + dgFloat32 dirMagInv = dgRsqrt (dirMag2); + dgFloat32 dirMag = dirMag2 * dirMagInv; + + dgFloat32 omegaMag = dgFloat32(2.0f) * dgAtan2 (dirMag, dq.m_w) * invdt; + return omegaDir.Scale (dirMagInv * omegaMag); +} + +dgQuaternion dgQuaternion::Slerp (const dgQuaternion &q1, dgFloat32 t) const +{ + dgQuaternion q0; + + dgFloat32 dot = DotProduct (q1); + if ((dot + dgFloat32(1.0f)) > dgEpsilon) { + dgFloat32 Sclp; + dgFloat32 Sclq; + if (dot < (dgFloat32(1.0f) - dgEpsilon) ) { + dgFloat32 ang = dgAcos (dot); + + dgFloat32 sinAng = dgSin (ang); + dgFloat32 den = dgFloat32(1.0f) / sinAng; + + Sclp = dgSin ((dgFloat32(1.0f) - t ) * ang) * den; + Sclq = dgSin (t * ang) * den; + } else { + Sclp = dgFloat32(1.0f) - t; + Sclq = t; + } + + q0.m_w = m_w * Sclp + q1.m_w * Sclq; + q0.m_x = m_x * Sclp + q1.m_x * Sclq; + q0.m_y = m_y * Sclp + q1.m_y * Sclq; + q0.m_z = m_z * Sclp + q1.m_z * Sclq; + + } else { + q0.m_w = m_z; + q0.m_x = -m_y; + q0.m_y = m_x; + q0.m_z = m_w; + + dgFloat32 Sclp = dgSin ((dgFloat32(1.0f) - t) * dgPi * dgFloat32 (0.5f)); + dgFloat32 Sclq = dgSin (t * dgPi * dgFloat32 (0.5f)); + + q0.m_w = m_w * Sclp + q0.m_w * Sclq; + q0.m_x = m_x * Sclp + q0.m_x * Sclq; + q0.m_y = m_y * Sclp + q0.m_y * Sclq; + q0.m_z = m_z * Sclp + q0.m_z * Sclq; + } + + dot = q0.DotProduct (q0); + if ((dot) < dgFloat32(1.0f - dgEpsilon * 10.0f) ) { + //dot = dgFloat32(1.0f) / dgSqrt (dot); + dot = dgRsqrt (dot); + q0.m_w *= dot; + q0.m_x *= dot; + q0.m_y *= dot; + q0.m_z *= dot; + } + return q0; +} + + diff --git a/thirdparty/src/newton/dgCore/dgQuaternion.h b/thirdparty/src/newton/dgCore/dgQuaternion.h new file mode 100644 index 000000000..91ddae957 --- /dev/null +++ b/thirdparty/src/newton/dgCore/dgQuaternion.h @@ -0,0 +1,138 @@ +/* Copyright (c) <2003-2019> +* +* 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 __dgQuaternion__ +#define __dgQuaternion__ + +#include "dgStdafx.h" + +class dgVector; +class dgMatrix; + +DG_MSC_VECTOR_ALIGNMENT +class dgQuaternion +{ + public: + dgQuaternion (); + dgQuaternion (const dgMatrix& matrix); + dgQuaternion (dgFloat32 q0, dgFloat32 q1, dgFloat32 q2, dgFloat32 q3); + dgQuaternion (const dgVector &unit_Axis, dgFloat32 angle = dgFloat32 (0.0f)); + +// dgFloat32& operator[] (dgInt32 i); +// const dgFloat32& operator[] (dgInt32 i) const; + + void Scale (dgFloat32 scale); + void Normalize (); + dgQuaternion Inverse () const; + dgQuaternion Slerp (const dgQuaternion &q1, dgFloat32 t) const; + + dgFloat32 DotProduct (const dgQuaternion &QB) const; + dgVector CalcAverageOmega (const dgQuaternion &q1, dgFloat32 invdt) const; + + dgQuaternion operator* (const dgQuaternion &B) const; + dgQuaternion operator+ (const dgQuaternion &B) const; + dgQuaternion operator- (const dgQuaternion &B) const; + + dgFloat32 m_x; + dgFloat32 m_y; + dgFloat32 m_z; + dgFloat32 m_w; +} DG_GCC_VECTOR_ALIGNMENT; + + +DG_INLINE dgQuaternion::dgQuaternion() + :m_x(dgFloat32(0.0f)) + ,m_y(dgFloat32(0.0f)) + ,m_z(dgFloat32(0.0f)) + ,m_w(dgFloat32(1.0f)) +{ +} + +DG_INLINE dgQuaternion::dgQuaternion(dgFloat32 Q0, dgFloat32 Q1, dgFloat32 Q2, dgFloat32 Q3) + :m_x(Q1) + ,m_y(Q2) + ,m_z(Q3) + ,m_w(Q0) +{ +// dgAssert (dgAbs (DotProduct (*this) -dgFloat32 (1.0f)) < dgFloat32(1.0e-4f)); +} + +/* +DG_INLINE dgFloat32& dgQuaternion::operator[] (dgInt32 i) +{ + dgAssert(i < 4); + dgAssert(i >= 0); + return (&m_w)[i]; +} + +DG_INLINE const dgFloat32& dgQuaternion::operator[] (dgInt32 i) const +{ + dgAssert(i < 4); + dgAssert(i >= 0); + return (&m_w)[i]; +} +*/ + +DG_INLINE void dgQuaternion::Scale (dgFloat32 scale) +{ + m_w *= scale; + m_x *= scale; + m_y *= scale; + m_z *= scale; +} + +DG_INLINE void dgQuaternion::Normalize () +{ + Scale (dgRsqrt (DotProduct (*this))); +} + +DG_INLINE dgFloat32 dgQuaternion::DotProduct (const dgQuaternion &q1) const +{ + return m_w * q1.m_w + m_x * q1.m_x + m_y * q1.m_y + m_z * q1.m_z; +} + +DG_INLINE dgQuaternion dgQuaternion::Inverse () const +{ + return dgQuaternion (m_w, -m_x, -m_y, -m_z); +} + +DG_INLINE dgQuaternion dgQuaternion::operator+ (const dgQuaternion &q) const +{ + return dgQuaternion (m_w + q.m_w, m_x + q.m_x, m_y + q.m_y, m_z + q.m_z); +} + +DG_INLINE dgQuaternion dgQuaternion::operator- (const dgQuaternion &q) const +{ + return dgQuaternion (m_w - q.m_w, m_x - q.m_x, m_y - q.m_y, m_z - q.m_z); +} + +DG_INLINE dgQuaternion dgQuaternion::operator* (const dgQuaternion &q) const +{ + return dgQuaternion (q.m_w * m_w - q.m_x * m_x - q.m_y * m_y - q.m_z * m_z, + 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); +} + + + +#endif + diff --git a/thirdparty/src/newton/dgCore/dgRandom.cpp b/thirdparty/src/newton/dgCore/dgRandom.cpp new file mode 100644 index 000000000..e7e16ec74 --- /dev/null +++ b/thirdparty/src/newton/dgCore/dgRandom.cpp @@ -0,0 +1,39 @@ +/* Copyright (c) <2003-2019> +* +* 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 "dgRandom.h" + +#define RAND_MUL 31415821u +static dgUnsigned32 randSeed = RAND_MUL; + +void dgApi dgRandomize (dgUnsigned32 Seed) +{ + randSeed = Seed; +} + +dgUnsigned32 dgApi dgRandom() +{ + randSeed = RAND_MUL * randSeed + 1; + return randSeed; +} + + diff --git a/thirdparty/src/newton/dgCore/dgRandom.h b/thirdparty/src/newton/dgCore/dgRandom.h new file mode 100644 index 000000000..76dc313df --- /dev/null +++ b/thirdparty/src/newton/dgCore/dgRandom.h @@ -0,0 +1,41 @@ +/* Copyright (c) <2003-2019> +* +* 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 __dgRandom__ +#define __dgRandom__ + +#include "dgStdafx.h" + +const dgFloat64 fRandom = (dgFloat64 (1.0) / dgFloat64 ((dgUnsigned32)(0xffffffff))); + +// return a random number between 0 and 0xffffffff; +dgUnsigned32 dgApi dgRandom(); + +inline dgFloat32 dgfRandom() +{ + return (dgFloat32) (dgRandom() * fRandom); +} + + +void dgApi dgRandomize (dgUnsigned32 Seed); + +#endif + diff --git a/thirdparty/src/newton/dgCore/dgRef.cpp b/thirdparty/src/newton/dgCore/dgRef.cpp new file mode 100644 index 000000000..a34299a2a --- /dev/null +++ b/thirdparty/src/newton/dgCore/dgRef.cpp @@ -0,0 +1,198 @@ +/* Copyright (c) <2003-2019> +* +* 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 "dgRef.h" +#include "dgList.h" +#include "dgTree.h" + + +dgRtti dgRef::m_rtti ("dgRef"); + +dgRefFlags::dgRefFlags() +{ + *this = 0; + m_alive = true; + m_ref = 1; +} + +dgInt32 dgRefFlags::operator = (dgInt32 val) +{ + dgInt32* ptr; + ptr = &(*(dgInt32*)this); + *ptr = val; + return val; +} + + + +dgRef::dgRef() +{ + m_id = 0; +} + +dgRef::dgRef(const char *name) +{ + SetName(name); +} + +dgRef::dgRef(dgUnsigned32 idArg) +{ + SetNameID(idArg); +} + +dgRef::dgRef(const dgRef &Clone) +{ + m_id = Clone.m_id; +} + +dgRef::~dgRef() +{ +} + +dgRef *dgRef::AddRef() +{ + m_ref++; + dgAssert(m_ref < ((1 << 24) - 1)); + return this; +} + +dgInt32 dgRef::Release() +{ + m_ref--; + if (m_ref) { + return dgInt32(m_ref); + } + delete this; + return 0; +} + +dgRef *dgRef::CreateClone() const +{ + dgAssert(0); + return NULL; +} + + +dgUnsigned32 dgRef::GetTypeId() const +{ + return m_rtti.GetTypeId(); +} + +bool dgRef::IsType(dgUnsigned32 typeId) const +{ + return m_rtti.IsTypeID(typeId); +} + +dgUnsigned32 dgRef::GetRttiType() +{ + return m_rtti.GetTypeId(); +} + + +bool dgRef::GetUserFlag0() const +{ + return m_userFlag0 ? true : false; +} + +bool dgRef::GetUserFlag1() const +{ + return m_userFlag1 ? true : false; +} + + +void dgRef::SetUserFlag0(bool flags) +{ + m_userFlag0 = dgUnsigned8(flags); +} + +void dgRef::SetUserFlag1(bool flags) +{ + m_userFlag1 = dgUnsigned8(flags); +} + + +bool dgRef::IsAlive() const +{ + return m_alive ? true : false; +} + +void dgRef::Kill() +{ + m_alive = false; +} + +void dgRef::Unkill() +{ + m_alive = true; +} + +void dgRef::SetNameID(dgUnsigned32 newID) +{ + m_id = newID; +} + +dgUnsigned32 dgRef::GetNameID() const +{ + return m_id; +} + + +const char* dgRef::GetName() const +{ + return dgInverseCRC(GetNameID()); +} + +dgInt32 dgRef::GetRefCount() const +{ + return dgInt32(m_ref); +} + + +void dgRef::SetName(const char *name) +{ + SetNameID(0); + if (name) { + SetNameID(dgCRC(name)); + } +} + +bool dgRef::IsTypeByName(const char *typeName) const +{ + return IsType(dgCRC(typeName, (dgInt32)strlen(typeName))); +} + + +void dgRef::AttachRef ( + dgRef **oldObj, + dgRef *newObj) +{ + if (*oldObj) { + (*oldObj)->Release(); + } + *oldObj = newObj; + if (newObj) { + newObj->AddRef(); + } +} + + + diff --git a/thirdparty/src/newton/dgCore/dgRef.h b/thirdparty/src/newton/dgCore/dgRef.h new file mode 100644 index 000000000..2f252afee --- /dev/null +++ b/thirdparty/src/newton/dgCore/dgRef.h @@ -0,0 +1,97 @@ +/* Copyright (c) <2003-2019> +* +* 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 __dgRef__ +#define __dgRef__ + +#include "dgStdafx.h" +#include "dgRtti.h" +#include "dgDebug.h" +#include "dgMemory.h" + + +struct dgRefFlags +{ + dgRefFlags (); + inline dgInt32 operator = (dgInt32 val); + + dgUnsigned8 m_alive; + dgUnsigned8 m_userFlag0; + dgUnsigned8 m_userFlag1; + dgUnsigned8 m_userFlag2; +// dgUnsigned32 m_userFlag3 : 1; +// dgUnsigned32 m_userFlag4 : 1; +// dgUnsigned32 m_userFlag5 : 1; +// dgUnsigned32 m_userFlag6 : 1; + + dgUnsigned32 m_ref; +}; + + +class dgRef: public dgRefFlags +{ + public: + dgRef (); + dgRef (const char *name); + dgRef (dgUnsigned32 idArg); + dgRef(const dgRef &Clone); + dgRef *AddRef () ; + dgInt32 Release (); + dgInt32 GetRefCount() const; + + DG_CLASS_ALLOCATOR(allocator) + + virtual dgRef *CreateClone () const; + virtual dgUnsigned32 GetTypeId () const; + virtual bool IsType (dgUnsigned32 typeId) const; + + bool GetUserFlag0 () const; + bool GetUserFlag1 () const; + void SetUserFlag0 (bool flags); + void SetUserFlag1 (bool flags); + + bool IsAlive() const; + virtual void Kill(); + virtual void Unkill(); + + const char* GetName () const; + dgUnsigned32 GetNameID () const; + inline void SetNameID (dgUnsigned32 newID); + virtual void SetName (const char *name); + + void AttachRef (dgRef **oldRef, dgRef *newRef); + + + bool IsTypeByName (const char *typeName) const; + static dgUnsigned32 GetRttiType(); + + protected: + virtual ~dgRef (); + + private: + dgUnsigned32 m_id; + static dgRtti m_rtti; +}; + + + +#endif + diff --git a/thirdparty/src/newton/dgCore/dgRefCounter.cpp b/thirdparty/src/newton/dgCore/dgRefCounter.cpp new file mode 100644 index 000000000..05f87f718 --- /dev/null +++ b/thirdparty/src/newton/dgCore/dgRefCounter.cpp @@ -0,0 +1,57 @@ +/* Copyright (c) <2003-2019> +* +* 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 "dgRefCounter.h" + +/* +dgRefCounter::dgRefCounter(void) +{ + m_refCount = 1; +} + +dgRefCounter::~dgRefCounter(void) +{ +} + + +int dgRefCounter::GetRef() const +{ + return m_refCount; +} + +int dgRefCounter::Release() +{ + m_refCount --; + if (!m_refCount) { + delete this; + return 0; + } + return m_refCount; +} + +void dgRefCounter::AddRef() +{ + m_refCount ++; +} +*/ + diff --git a/thirdparty/src/newton/dgCore/dgRefCounter.h b/thirdparty/src/newton/dgCore/dgRefCounter.h new file mode 100644 index 000000000..5f349d58f --- /dev/null +++ b/thirdparty/src/newton/dgCore/dgRefCounter.h @@ -0,0 +1,72 @@ +/* Copyright (c) <2003-2019> +* +* 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 __DGREF_COUNTER__ +#define __DGREF_COUNTER__ + +class dgRefCounter +{ + public: + dgRefCounter(void); + int GetRef() const; + int Release(); + void AddRef(); + + protected: + virtual ~dgRefCounter(void); + + private: + int m_refCount; +}; + + +inline dgRefCounter::dgRefCounter(void) +{ + m_refCount = 1; +} + +inline dgRefCounter::~dgRefCounter(void) +{ + dgAssert (m_refCount <= 1); +} + + +inline int dgRefCounter::GetRef() const +{ + return m_refCount; +} + +inline int dgRefCounter::Release() +{ + m_refCount --; + if (!m_refCount) { + delete this; + return 0; + } + return m_refCount; +} + +inline void dgRefCounter::AddRef() +{ + m_refCount ++; +} + +#endif diff --git a/thirdparty/src/newton/dgCore/dgRtti.h b/thirdparty/src/newton/dgCore/dgRtti.h new file mode 100644 index 000000000..fbac83383 --- /dev/null +++ b/thirdparty/src/newton/dgCore/dgRtti.h @@ -0,0 +1,81 @@ +/* Copyright (c) <2003-2019> +* +* 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 __dgRtti__ +#define __dgRtti__ + +#include "dgStdafx.h" +#include "dgCRC.h" + +class dgRtti +{ + public: + dgRtti(const char* typeName); + dgUnsigned32 GetTypeId() const; + bool IsTypeID(dgUnsigned32 id) const; + + private: + dgUnsigned32 m_TypeId; +}; + +inline dgRtti::dgRtti(const char* typeName) +{ + m_TypeId = dgCRC (typeName, (dgInt32) strlen (typeName)); +} + +inline dgUnsigned32 dgRtti::GetTypeId() const +{ + return m_TypeId; +} + +inline bool dgRtti::IsTypeID (dgUnsigned32 id) const +{ + return m_TypeId == id; +} + + + +#define dgAddRtti(baseClass) \ + private: \ + static dgRtti rtti; \ + public: \ + virtual bool IsType (dgUnsigned32 typeId) const \ + { \ + if (rtti.IsTypeID (typeId)) { \ + return true; \ + } \ + return baseClass::IsType (typeId); \ + } \ + virtual dgUnsigned32 GetTypeId () const \ + { \ + return rtti.GetTypeId (); \ + } \ + static dgUnsigned32 GetRttiType() \ + { \ + return rtti.GetTypeId(); \ + } + + +#define dgInitRtti(className) \ + dgRtti className::rtti (#className) + +#endif + diff --git a/thirdparty/src/newton/dgCore/dgSmallDeterminant.cpp b/thirdparty/src/newton/dgCore/dgSmallDeterminant.cpp new file mode 100644 index 000000000..644d25c1f --- /dev/null +++ b/thirdparty/src/newton/dgCore/dgSmallDeterminant.cpp @@ -0,0 +1,153 @@ +/* Copyright (c) <2003-2019> +* +* 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" +#include "dgSmallDeterminant.h" + +#define Absolute(a) ((a) >= 0.0 ? (a) : -(a)) + +dgFloat64 Determinant2x2 (const dgFloat64 matrix[2][2], dgFloat64* const error) +{ + dgFloat64 a00xa11 = matrix[0][0] * matrix[1][1]; + dgFloat64 a01xa10 = matrix[0][1] * matrix[1][0]; + *error = Absolute(a00xa11) + Absolute(a01xa10); + return a00xa11 - a01xa10; +} + +dgGoogol Determinant2x2 (const dgGoogol matrix[2][2]) +{ + dgGoogol a00xa11 (matrix[0][0] * matrix[1][1]); + dgGoogol a01xa10 (matrix[0][1] * matrix[1][0]); + return a00xa11 - a01xa10; +} + + + +dgFloat64 Determinant3x3 (const dgFloat64 matrix[3][3], dgFloat64* const error) +{ + dgFloat64 sign = dgFloat64 (-1.0f); + dgFloat64 det = dgFloat64 (0.0f); + dgFloat64 accError = dgFloat64 (0.0f); + for (dgInt32 i = 0; i < 3; i ++) { + dgFloat64 cofactor[2][2]; + for (dgInt32 j = 0; j < 2; j ++) { + dgInt32 k0 = 0; + for (dgInt32 k = 0; k < 3; k ++) { + if (k != i) { + cofactor[j][k0] = matrix[j][k]; + k0 ++; + } + } + } + + dgFloat64 parcialError; + dgFloat64 minorDet = Determinant2x2 (cofactor, &parcialError); + accError += parcialError * Absolute (matrix[2][i]); + det += sign * minorDet * matrix[2][i]; + sign *= dgFloat64 (-1.0f); + } + + *error = accError; + return det; +} + +dgGoogol Determinant3x3 (const dgGoogol matrix[3][3]) +{ + dgGoogol negOne (dgFloat64 (-1.0f)); + dgGoogol sign (dgFloat64 (-1.0f)); + dgGoogol det = dgFloat64 (0.0f); + for (dgInt32 i = 0; i < 3; i ++) { + dgGoogol cofactor[2][2]; + + for (dgInt32 j = 0; j < 2; j ++) { + dgInt32 k0 = 0; + for (dgInt32 k = 0; k < 3; k ++) { + if (k != i) { + cofactor[j][k0] = matrix[j][k]; + k0 ++; + } + } + } + + dgGoogol minorDet (Determinant2x2 (cofactor)); + det = det + sign * minorDet * matrix[2][i]; + sign = sign * negOne; + } + return det; +} + + +dgFloat64 Determinant4x4 (const dgFloat64 matrix[4][4], dgFloat64* const error) +{ + dgFloat64 sign = dgFloat64 (1.0f); + dgFloat64 det = dgFloat64 (0.0f); + dgFloat64 accError = dgFloat64 (0.0f); + for (dgInt32 i = 0; i < 4; i ++) { + dgFloat64 cofactor[3][3]; + for (dgInt32 j = 0; j < 3; j ++) { + dgInt32 k0 = 0; + for (dgInt32 k = 0; k < 4; k ++) { + if (k != i) { + cofactor[j][k0] = matrix[j][k]; + k0 ++; + } + } + } + + dgFloat64 parcialError; + dgFloat64 minorDet = Determinant3x3 (cofactor, &parcialError); + accError += parcialError * Absolute (matrix[3][i]); + det += sign * minorDet * matrix[3][i]; + sign *= dgFloat64 (-1.0f); + } + + *error = accError; + return det; +} + + +dgGoogol Determinant4x4 (const dgGoogol matrix[4][4]) +{ + dgGoogol sign = dgFloat64 (1.0f); + dgGoogol det = dgFloat64 (0.0f); + dgGoogol negOne (dgFloat64 (-1.0f)); + //dgGoogol accError = dgFloat64 (0.0f); + for (dgInt32 i = 0; i < 4; i ++) { + dgGoogol cofactor[3][3]; + for (dgInt32 j = 0; j < 3; j ++) { + dgInt32 k0 = 0; + for (dgInt32 k = 0; k < 4; k ++) { + if (k != i) { + cofactor[j][k0] = matrix[j][k]; + k0 ++; + } + } + } + + dgGoogol minorDet = Determinant3x3 (cofactor); + det = det + sign * minorDet * matrix[3][i]; + sign = sign * negOne; + } + return det; +} + + diff --git a/thirdparty/src/newton/dgCore/dgSmallDeterminant.h b/thirdparty/src/newton/dgCore/dgSmallDeterminant.h new file mode 100644 index 000000000..bbc9d4d7c --- /dev/null +++ b/thirdparty/src/newton/dgCore/dgSmallDeterminant.h @@ -0,0 +1,37 @@ +/* Copyright (c) <2003-2019> +* +* 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 __dgSMALLDETERMINANT__ +#define __dgSMALLDETERMINANT__ + +#include "dgStdafx.h" + +class dgGoogol; +dgFloat64 Determinant2x2 (const dgFloat64 matrix[2][2], dgFloat64* const error); +dgFloat64 Determinant3x3 (const dgFloat64 matrix[3][3], dgFloat64* const error); +dgFloat64 Determinant4x4 (const dgFloat64 matrix[4][4], dgFloat64* const error); + + +dgGoogol Determinant2x2 (const dgGoogol matrix[2][2]); +dgGoogol Determinant3x3 (const dgGoogol matrix[3][3]); +dgGoogol Determinant4x4 (const dgGoogol matrix[4][4]); + +#endif diff --git a/thirdparty/src/newton/dgCore/dgSort.h b/thirdparty/src/newton/dgCore/dgSort.h new file mode 100644 index 000000000..63650c4b9 --- /dev/null +++ b/thirdparty/src/newton/dgCore/dgSort.h @@ -0,0 +1,439 @@ +/* Copyright (c) <2003-2019> +* +* 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_SORT_H__ +#define __DG_SORT_H__ +#include "dgStdafx.h" +#include "dgHeap.h" +#include "dgProfiler.h" +#include "dgThreadHive.h" + +#define DG_PARALLET_SORT_BATCH_SIZE 1024 + +template +dgInt32 dgBinarySearch(T const* array, dgInt32 elements, const T& entry, dgInt32(*compare) (const T* const A, const T* const B, void* const context), void* const context = NULL) +{ + dgInt32 index0 = 0; + dgInt32 index2 = elements - 1; + + while ((index2 - index0) > 4) { + dgInt32 index1 = (index0 + index2) >> 1; + dgInt32 test = compare(&array[index1], &entry, context); + if (test < 0) { + index0 = index1; + } + else { + index2 = index1; + } + } + + index0 = (index0 > 0) ? index0 - 1 : 0; + index2 = ((index2 + 1) < elements) ? index2 + 1 : elements; + dgInt32 index = index0 - 1; + for (dgInt32 i = index0; i < index2; i++) { + dgInt32 test = compare(&array[i], &entry, context); + if (!test) { + return i; + } + else if (test > 0) { + break; + } + index = i; + } + return index; +} + +template +dgInt32 dgBinarySearchIndirect(T** const array, dgInt32 elements, const T& entry, dgInt32(*compare) (const T* const A, const T* const B, void* const context), void* const context = NULL) +{ + dgInt32 index0 = 0; + dgInt32 index2 = elements - 1; + + while ((index2 - index0) > 4) { + dgInt32 index1 = (index0 + index2) >> 1; + dgInt32 test = compare(array[index1], &entry, context); + if (test < 0) { + index0 = index1; + } + else { + index2 = index1; + } + } + + index0 = (index0 > 0) ? index0 - 1 : 0; + index2 = ((index2 + 1) < elements) ? index2 + 1 : elements; + dgInt32 index = index0 - 1; + for (dgInt32 i = index0; i < index2; i++) { + dgInt32 test = compare(array[i], &entry, context); + if (!test) { + return i; + } + else if (test > 0) { + break; + } + index = i; + } + return index; +} + +template +void dgRadixSort(T* const array, T* const tmpArray, dgInt32 elements, dgInt32 radixPass, dgInt32(*getRadixKey) (const T* const A, void* const context), void* const context = NULL) +{ + dgInt32 scanCount[256]; + dgInt32 histogram[256][4]; + + dgAssert(radixPass >= 1); + dgAssert(radixPass <= 4); + + memset(histogram, 0, sizeof(histogram)); + for (dgInt32 i = 0; i < elements; i++) { + dgInt32 key = getRadixKey(&array[i], context); + for (dgInt32 j = 0; j < radixPass; j++) { + dgInt32 radix = (key >> (j << 3)) & 0xff; + histogram[radix][j] = histogram[radix][j] + 1; + } + } + + for (dgInt32 radix = 0; radix < radixPass; radix += 2) { + scanCount[0] = 0; + for (dgInt32 i = 1; i < 256; i++) { + scanCount[i] = scanCount[i - 1] + histogram[i - 1][radix]; + } + dgInt32 radixShift = radix << 3; + for (dgInt32 i = 0; i < elements; i++) { + dgInt32 key = (getRadixKey(&array[i], context) >> radixShift) & 0xff; + dgInt32 index = scanCount[key]; + tmpArray[index] = array[i]; + scanCount[key] = index + 1; + } + + if ((radix + 1) < radixPass) { + scanCount[0] = 0; + for (dgInt32 i = 1; i < 256; i++) { + scanCount[i] = scanCount[i - 1] + histogram[i - 1][radix + 1]; + } + + dgInt32 radixShift = (radix + 1) << 3; + for (dgInt32 i = 0; i < elements; i++) { + dgInt32 key = (getRadixKey(&array[i], context) >> radixShift) & 0xff; + dgInt32 index = scanCount[key]; + array[index] = tmpArray[i]; + scanCount[key] = index + 1; + } + } + else { + memcpy(array, tmpArray, elements * sizeof(T)); + } + } + +#ifdef _DEBUG + for (dgInt32 i = 0; i < (elements - 1); i++) { + dgAssert(getRadixKey(&array[i], context) <= getRadixKey(&array[i + 1], context)); + } +#endif +} + +template +void dgSort(T* const array, dgInt32 elements, dgInt32(*compare) (const T* const A, const T* const B, void* const context), void* const context = NULL) +{ + //DG_TRACKTIME(); + const dgInt32 batchSize = 8; + dgInt32 stack[1024][2]; + + stack[0][0] = 0; + stack[0][1] = elements - 1; + dgInt32 stackIndex = 1; + while (stackIndex) { + stackIndex--; + dgInt32 lo = stack[stackIndex][0]; + dgInt32 hi = stack[stackIndex][1]; + if ((hi - lo) > batchSize) { + dgInt32 mid = (lo + hi) >> 1; + if (compare(&array[lo], &array[mid], context) > 0) { + dgSwap(array[lo], array[mid]); + } + if (compare(&array[mid], &array[hi], context) > 0) { + dgSwap(array[mid], array[hi]); + } + if (compare(&array[lo], &array[mid], context) > 0) { + dgSwap(array[lo], array[mid]); + } + dgInt32 i = lo + 1; + dgInt32 j = hi - 1; + T pivot(array[mid]); + do { + while (compare(&array[i], &pivot, context) < 0) i++; + while (compare(&array[j], &pivot, context) > 0) j--; + + if (i <= j) { + dgSwap(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++; + } + dgAssert(stackIndex < dgInt32(sizeof(stack) / (2 * sizeof(stack[0][0])))); + } + } + + dgInt32 stride = batchSize + 1; + if (elements < stride) { + stride = elements; + } + for (dgInt32 i = 1; i < stride; i++) { + if (compare(&array[0], &array[i], context) > 0) { + dgSwap(array[0], array[i]); + } + } + + for (dgInt32 i = 1; i < elements; i++) { + dgInt32 j = i; + T tmp(array[i]); + for (; compare(&array[j - 1], &tmp, context) > 0; j--) { + dgAssert(j > 0); + array[j] = array[j - 1]; + } + array[j] = tmp; + } + +#ifdef _DEBUG + for (dgInt32 i = 0; i < (elements - 1); i++) { + dgAssert(compare(&array[i], &array[i + 1], context) <= 0); + } +#endif +} + +template +void dgSortIndirect(T** const array, dgInt32 elements, dgInt32(*compare) (const T* const A, const T* const B, void* const context), void* const context = NULL) +{ + //DG_TRACKTIME(); + const dgInt32 batchSize = 8; + dgInt32 stack[1024][2]; + + stack[0][0] = 0; + stack[0][1] = elements - 1; + dgInt32 stackIndex = 1; + while (stackIndex) { + stackIndex--; + dgInt32 lo = stack[stackIndex][0]; + dgInt32 hi = stack[stackIndex][1]; + if ((hi - lo) > batchSize) { + dgInt32 mid = (lo + hi) >> 1; + if (compare(array[lo], array[mid], context) > 0) { + dgSwap(array[lo], array[mid]); + } + if (compare(array[mid], array[hi], context) > 0) { + dgSwap(array[mid], array[hi]); + } + if (compare(array[lo], array[mid], context) > 0) { + dgSwap(array[lo], array[mid]); + } + dgInt32 i = lo + 1; + dgInt32 j = hi - 1; + T* val(array[mid]); + do { + while (compare(array[i], val, context) < 0) i++; + while (compare(array[j], val, context) > 0) j--; + + if (i <= j) { + dgSwap(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++; + } + dgAssert(stackIndex < dgInt32(sizeof(stack) / (2 * sizeof(stack[0][0])))); + } + } + + dgInt32 stride = batchSize + 1; + if (elements < stride) { + stride = elements; + } + for (dgInt32 i = 1; i < stride; i++) { + if (compare(array[0], array[i], context) > 0) { + dgSwap(array[0], array[i]); + } + } + + for (dgInt32 i = 1; i < elements; i++) { + dgInt32 j = i; + T* tmp(array[i]); + for (; compare(array[j - 1], tmp, context) > 0; j--) { + dgAssert(j > 0); + array[j] = array[j - 1]; + } + array[j] = tmp; + } + +#ifdef _DEBUG + for (dgInt32 i = 0; i < (elements - 1); i++) { + dgAssert(compare(array[i], array[i + 1], context) <= 0); + } +#endif +} + + +class dgParallelSortRange +{ + public: + dgParallelSortRange() {} + dgParallelSortRange(dgInt32 i0, dgInt32 i1) + :m_i0(i0) + , m_i1(i1) + { + } + dgInt32 m_i0; + dgInt32 m_i1; +}; + +template +class dgParallelSourtDesc +{ + public: + typedef dgInt32(*CompareFunction) (const T* const A, const T* const B, void* const context); + + dgParallelSourtDesc(dgThreadHive& threadPool, T* const array, dgInt32 elements, CompareFunction compareFunct, void* const context) + :m_data(array) + ,m_callback(compareFunct) + ,m_context(context) + ,m_threadCount(threadPool.GetThreadCount()) + { + dgDownHeap rangeMerge(m_buffer, sizeof(m_buffer)); + + dgParallelSortRange range(0, elements - 1); + rangeMerge.Push(range, elements); + + const dgInt32 batchSize = DG_PARALLET_SORT_BATCH_SIZE; + const dgInt32 rangesCount = m_threadCount; + + while ((rangeMerge.GetCount() < rangesCount) && (rangeMerge.Value() > batchSize)) { + dgParallelSortRange splitRange(rangeMerge[0]); + rangeMerge.Pop(); + + const dgInt32 lo = splitRange.m_i0; + const dgInt32 hi = splitRange.m_i1; + const dgInt32 mid = (lo + hi) >> 1; + if (m_callback(&array[lo], &array[mid], context) > 0) { + dgSwap(array[lo], array[mid]); + } + if (m_callback(&array[mid], &array[hi], context) > 0) { + dgSwap(array[mid], array[hi]); + } + if (m_callback(&array[lo], &array[mid], context) > 0) { + dgSwap(array[lo], array[mid]); + } + dgInt32 i = lo; + dgInt32 j = hi; + T pivot(array[mid]); + for (;;) { + do { + i++; + } while (m_callback(&array[i], &pivot, context) < 0); + do { + j--; + } while (m_callback(&array[j], &pivot, context) > 0); + + if (i >= j) { + break; + } + dgSwap(array[i], array[j]); + } + + dgParallelSortRange newRange0(lo, j); + dgParallelSortRange newRange1(j + 1, hi); + rangeMerge.Push(newRange0, j - lo + 1); + rangeMerge.Push(newRange1, hi - j); + } + + m_rangeMerge = &rangeMerge; + for (dgInt32 i = 0; i < m_threadCount; i++) { + threadPool.QueueJob(dgParallelKernel, this, NULL, __FUNCTION__); + } + threadPool.SynchronizationBarrier(); + + #ifdef _DEBUG + for (dgInt32 i = 0; i < (elements - 1); i++) { + dgAssert(m_callback(&m_data[i], &m_data[i + 1], context) <= 0); + } + #endif + } + + static void dgParallelKernel(void* const context, void* const worldContext, dgInt32 threadID) + { + DG_TRACKTIME(); + dgParallelSourtDesc* const me = (dgParallelSourtDesc*) context; + me->dgParallelKernel(threadID); + } + + void dgParallelKernel(dgInt32 threadID) + { + dgDownHeap& rangeMerge = *((dgDownHeap*)m_rangeMerge); + const dgInt32 count = rangeMerge.GetCount(); + for (dgInt32 i = threadID; i < count; i += m_threadCount) { + dgParallelSortRange range(rangeMerge[i]); + T* const data = &m_data[range.m_i0]; + dgSort(data, range.m_i1 - range.m_i0 + 1, m_callback, m_context); + } + } + + T* m_data; + void* m_rangeMerge; + CompareFunction m_callback; + void* m_context; + int m_threadCount; + dgInt8 m_buffer[256 * sizeof (dgParallelSortRange)]; +}; + +template +void dgParallelSort(dgThreadHive& threadPool, T* const array, dgInt32 elements, dgInt32(*compare) (const T* const A, const T* const B, void* const context), void* const context = NULL) +{ + //DG_TRACKTIME(); + if ((threadPool.GetThreadCount() <= 1) || (elements < DG_PARALLET_SORT_BATCH_SIZE)) { +// if (1) { + dgSort(array, elements, compare, context); + } else { + dgParallelSourtDesc sort(threadPool, array, elements, compare, context); + } +} + +#endif diff --git a/thirdparty/src/newton/dgCore/dgStack.h b/thirdparty/src/newton/dgCore/dgStack.h new file mode 100644 index 000000000..f0eaacbb5 --- /dev/null +++ b/thirdparty/src/newton/dgCore/dgStack.h @@ -0,0 +1,111 @@ +/* Copyright (c) <2003-2019> +* +* 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 __dgStack__ +#define __dgStack__ + +#include "dgStdafx.h" +#include "dgDebug.h" +#include "dgMemory.h" + +class dgStackBase +{ + protected: + dgStackBase (dgInt32 size); + ~dgStackBase (); + + const void *m_ptr; +}; + +inline dgStackBase::dgStackBase (dgInt32 size) + :m_ptr (dgMallocStack (size_t (size))) +{ +} + +inline dgStackBase::~dgStackBase () +{ + dgFreeStack ((void*)m_ptr); +} + +template +class dgStack: public dgStackBase +{ + public: + dgStack (dgInt32 size); + ~dgStack (); + dgInt32 GetSizeInBytes() const; + dgInt32 GetElementsCount() const; + + DG_INLINE T& operator[] (dgInt32 entry); + DG_INLINE const T& operator[] (dgInt32 entry) const; + + private: + dgInt32 m_size; +}; + +template +dgStack::dgStack (dgInt32 size) + :dgStackBase (dgInt32 (size * sizeof(T))) +{ + m_size = size; +} + +template +dgStack::~dgStack () +{ +} + +template +dgInt32 dgStack::GetElementsCount() const +{ + return m_size; +} + +template +dgInt32 dgStack::GetSizeInBytes() const +{ + return dgInt32 (m_size * sizeof(T)); +} + + +template +DG_INLINE T& dgStack::operator[] (dgInt32 entry) +{ + dgAssert (entry >= 0); + dgAssert ((entry < m_size) || ((m_size == 0) && (entry == 0))); + + T* const mem = (T*) m_ptr; + return mem[entry]; +} + +template +DG_INLINE const T& dgStack::operator[] (dgInt32 entry) const +{ + dgAssert (entry >= 0); + dgAssert ((entry < m_size) || ((m_size == 0) && (entry == 0))); + + const T* const mem = (T*) m_ptr; + return mem[entry]; +} + + +#endif + diff --git a/thirdparty/src/newton/dgCore/dgStdafx.h b/thirdparty/src/newton/dgCore/dgStdafx.h new file mode 100644 index 000000000..fc9f647dc --- /dev/null +++ b/thirdparty/src/newton/dgCore/dgStdafx.h @@ -0,0 +1,28 @@ +/* Copyright (c) <2003-2019> +* +* 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_TYPES_H_ +#define _DG_TYPES_H_ + +#include "dgTypes.h" + +#endif + diff --git a/thirdparty/src/newton/dgCore/dgThread.cpp b/thirdparty/src/newton/dgCore/dgThread.cpp new file mode 100644 index 000000000..9ca91bdc8 --- /dev/null +++ b/thirdparty/src/newton/dgCore/dgThread.cpp @@ -0,0 +1,211 @@ +/* Copyright (c) <2003-2019> +* +* 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 "dgProfiler.h" + +dgThread::dgThread () + :m_id(0) + ,m_terminate(0) + ,m_threadRunning(0) +{ + m_name[0] = 0; +} + +dgThread::dgThread (const char* const name, dgInt32 id) + :m_id(id) + ,m_terminate(0) + ,m_threadRunning(0) +{ + strncpy (m_name, name, sizeof (m_name) - 1); +} + +void dgThread::Init (const char* const name, dgInt32 id) +{ + m_id = id; + strncpy (m_name, name, sizeof (m_name) - 1); + Init (); +} + + +bool dgThread::IsThreadActive() const +{ + return m_threadRunning ? true : false; +} + +#ifdef DG_USE_THREAD_EMULATION + +dgThread::dgSemaphore::dgSemaphore () +{ + m_sem = 0; +} + +dgThread::dgSemaphore::~dgSemaphore () +{ +} + +void dgThread::dgSemaphore::Release () +{ +} + +void dgThread::dgSemaphore::Wait() +{ +} + +dgThread::~dgThread () +{ +} + +void dgThread::Init () +{ +} + +void dgThread::Close () +{ +} + +//void dgThread::Wait (dgSemaphore& mutex) +//{ +//} + +void dgThread::Wait (dgInt32 count, dgSemaphore* const semArray) +{ +} + +void* dgThread::dgThreadSystemCallback(void* threadData) +{ + return 0; +} + +#else + +dgThread::dgSemaphore::dgSemaphore () + :m_count(0) +{ +} + +dgThread::dgSemaphore::~dgSemaphore () +{ +} + +void dgThread::dgSemaphore::Release () +{ + std::unique_lock lck(m_mutex); + m_count ++; + m_sem.notify_one(); +} + +void dgThread::dgSemaphore::Wait() +{ + std::unique_lock lck(m_mutex); + dgAssert (m_count >= 0); + while (m_count == 0) + { + m_sem.wait(lck); + } + m_count --; +} + +dgThread::~dgThread () +{ +} + + +void dgThread::SetName() +{ +#if defined(_MSC_VER) + // a hideous way to set the thread name, bu this is how Microsoft does it + const DWORD MS_VC_EXCEPTION = 0x406D1388; + #pragma pack(push,8) + struct THREADNAME_INFO + { + DWORD dwType; // Must be 0x1000. + LPCSTR szName; // Pointer to name (in user addr space). + DWORD dwThreadID; // Thread ID (-1=caller thread). + DWORD dwFlags; // Reserved for future use, must be zero. + }; + #pragma pack(pop) + + + THREADNAME_INFO info; + info.dwType = 0x1000; + info.szName = m_name; + info.dwThreadID = GetThreadId(m_handle.native_handle()); + info.dwFlags = 0; + __try { + RaiseException(MS_VC_EXCEPTION, 0, sizeof(info) / sizeof(ULONG_PTR), (ULONG_PTR*)&info); + } + __except (EXCEPTION_EXECUTE_HANDLER) + { + } +#endif +} + + +void dgThread::Init () +{ + // This must be set now because otherwise if this thread is + // immediately closed the Terminate method won't detect that the + // thread is running + dgInterlockedExchange(&m_threadRunning, 0); + m_handle = std::thread(dgThreadSystemCallback, this); + // wait until the thread procedure is called. + while (!m_threadRunning) { + dgThreadYield(); + } + + SetName(); +} + +void dgThread::Close () +{ + m_handle.join(); +} + +void dgThread::Wait (dgInt32 count, dgSemaphore* const semArray) +{ + for (dgInt32 i = 0; i < count; i ++) { + semArray[i].Wait(); + } +} + + +void* dgThread::dgThreadSystemCallback(void* threadData) +{ + dgFloatExceptions exception; + dgSetPrecisionDouble precision; + + dgThread* const me = (dgThread*) threadData; + + D_SET_TRACK_NAME(me->m_name); + dgInterlockedExchange(&me->m_threadRunning, 1); + me->Execute(me->m_id); + dgInterlockedExchange(&me->m_threadRunning, 0); + + return 0; +} + + +#endif + + + diff --git a/thirdparty/src/newton/dgCore/dgThread.h b/thirdparty/src/newton/dgCore/dgThread.h new file mode 100644 index 000000000..a0900cd13 --- /dev/null +++ b/thirdparty/src/newton/dgCore/dgThread.h @@ -0,0 +1,86 @@ +/* Copyright (c) <2003-2019> +* +* 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_THREAD_API_H__ +#define __DG_THREAD_API_H__ + +// by default newton is run on a separate thread, 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 multi threading is not stable +//#define DG_USE_THREAD_EMULATION + + +class dgThread +{ + public: + class dgSemaphore + { + public: + dgSemaphore (); + ~dgSemaphore (); + void Wait(); + void Release(); + + dgInt32 GetCount() const + { + #ifdef DG_USE_THREAD_EMULATION + return 0; + #else + return m_count; + #endif + } + + private: + #ifdef DG_USE_THREAD_EMULATION + dgInt32 m_sem; + #else + std::condition_variable m_sem; + std::mutex m_mutex; + dgInt32 m_count; + #endif + }; + + dgThread (); + dgThread (const char* const name, dgInt32 id); + virtual ~dgThread (); + + virtual void Execute (dgInt32 threadId) = 0; + + bool IsThreadActive() const; + void Wait (dgInt32 count, dgSemaphore* const mutexes); + + protected: + void Init (); + void Init (const char* const name, dgInt32 id); + void Close (); + void SetName (); + static void* dgThreadSystemCallback(void* threadData); + + #ifndef DG_USE_THREAD_EMULATION + std::thread m_handle; + #endif + dgInt32 m_id; + dgInt32 m_terminate; + dgInt32 m_threadRunning; + + char m_name[32]; +}; + +#endif diff --git a/thirdparty/src/newton/dgCore/dgThreadHive.cpp b/thirdparty/src/newton/dgCore/dgThreadHive.cpp new file mode 100644 index 000000000..50bcdbdbc --- /dev/null +++ b/thirdparty/src/newton/dgCore/dgThreadHive.cpp @@ -0,0 +1,386 @@ +/* Copyright (c) <2003-2019> +* +* 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 "dgTypes.h" +#include "dgMemory.h" +#include "dgProfiler.h" +#include "dgThreadHive.h" + +#ifdef USE_UNIX_THREAD_POOL +dgThreadHive::dgWorkerThread::dgWorkerThread() + :dgThread() + ,m_hive(NULL) + ,m_allocator(NULL) + ,m_isBusy(0) + ,m_jobsCount(0) + ,m_workerSemaphore() +{ +} + +dgThreadHive::dgWorkerThread::~dgWorkerThread() +{ + while (IsBusy()); + + dgInterlockedExchange(&m_terminate, 1); + m_workerSemaphore.Release(); + Close(); +} + +void dgThreadHive::dgWorkerThread::SetUp(dgMemoryAllocator* const allocator, const char* const name, dgInt32 id, dgThreadHive* const hive) +{ + m_hive = hive; + m_allocator = allocator; + Init (name, id); + + #ifndef DG_USE_THREAD_EMULATION + #if (defined (_WIN_32_VER) || defined (_WIN_64_VER)) + SetThreadPriority(m_handle.native_handle(), THREAD_PRIORITY_ABOVE_NORMAL); + #endif + #endif +} + +bool dgThreadHive::dgWorkerThread::IsBusy() const +{ + return m_isBusy ? true : false; +} + +void dgThreadHive::dgWorkerThread::Execute (dgInt32 threadId) +{ + m_hive->OnBeginWorkerThread (threadId); + + while (!m_terminate) { + dgInterlockedExchange(&m_isBusy, 0); + m_workerSemaphore.Wait(); + dgInterlockedExchange(&m_isBusy, 1); + if (!m_terminate) { + RunNextJobInQueue(threadId); + m_hive->m_beginSectionSemaphores[threadId].Release(); + } + } + + dgInterlockedExchange(&m_isBusy, 0); + + m_hive->OnEndWorkerThread (threadId); +} + +dgInt32 dgThreadHive::dgWorkerThread::PushJob(const dgThreadJob& job) +{ + dgAssert (m_jobsCount < sizeof (m_jobPool)/ sizeof (m_jobPool[0])); + m_jobPool[m_jobsCount] = job; + m_jobsCount ++; + return m_jobsCount; +} + +void dgThreadHive::dgWorkerThread::RunNextJobInQueue(dgInt32 threadId) +{ + for (dgInt32 i = 0; i < m_jobsCount; i ++) { + const dgThreadJob& job = m_jobPool[i]; + job.m_callback (job.m_context0, job.m_context1, m_id); + } + m_jobsCount = 0; +} + +dgThreadHive::dgThreadHive(dgMemoryAllocator* const allocator) + :m_parentThread(NULL) + ,m_workerThreads(NULL) + ,m_allocator(allocator) + ,m_jobsCount(0) + ,m_workerThreadsCount(0) + ,m_globalCriticalSection(0) +{ +} + +dgThreadHive::~dgThreadHive() +{ + DestroyThreads(); +} + +void dgThreadHive::SetParentThread (dgThread* const parentThread) +{ + m_parentThread = parentThread; +} + +void dgThreadHive::DestroyThreads() +{ + if (m_workerThreadsCount) { + delete[] m_workerThreads; + m_workerThreads = NULL; + m_workerThreadsCount = 0; + } +} + +void dgThreadHive::SetThreadsCount (dgInt32 threads) +{ + DestroyThreads(); + + m_workerThreadsCount = dgMin (threads, DG_MAX_THREADS_HIVE_COUNT); + if (m_workerThreadsCount == 1) { + m_workerThreadsCount = 0; + } + + if (m_workerThreadsCount) { + m_workerThreads = new (m_allocator) dgWorkerThread[dgUnsigned32 (m_workerThreadsCount)]; + + for (dgInt32 i = 0; i < m_workerThreadsCount; i ++) { + char name[256]; + sprintf (name, "dgWorkerThread%d", i); + m_workerThreads[i].SetUp(m_allocator, name, i, this); + } + } +} + +void dgThreadHive::QueueJob (dgWorkerThreadTaskCallback callback, void* const context0, void* const context1, const char* const functionName) +{ + if (!m_workerThreadsCount) { + //DG_TRACKTIME(functionName); + callback (context0, context1, 0); + } else { + dgInt32 workerTreadEntry = m_jobsCount % m_workerThreadsCount; + #ifdef DG_USE_THREAD_EMULATION + //DG_TRACKTIME(functionName); + callback (context0, context1, workerTreadEntry); + #else + dgInt32 index = m_workerThreads[workerTreadEntry].PushJob(dgThreadJob(context0, context1, callback, functionName)); + if (index >= DG_THREAD_POOL_JOB_SIZE) { + dgAssert (0); + SynchronizationBarrier (); + } + #endif + } + + m_jobsCount ++; +} + +void dgThreadHive::OnBeginWorkerThread (dgInt32 threadId) +{ +} + +void dgThreadHive::OnEndWorkerThread (dgInt32 threadId) +{ +} + +void dgThreadHive::SynchronizationBarrier () +{ + if (m_workerThreadsCount) { + //DG_TRACKTIME(); + for (dgInt32 i = 0; i < m_workerThreadsCount; i ++) { + m_workerThreads[i].m_workerSemaphore.Release(); + } + m_parentThread->Wait(m_workerThreadsCount, m_beginSectionSemaphores); + } + m_jobsCount = 0; +} + +#else + +dgThreadHive::dgWorkerThread::dgWorkerThread() + :dgThread() + ,m_workerSemaphore() + ,m_hive(NULL) + ,m_allocator(NULL) + ,m_concurrentWork(0) + ,m_pendingWork(0) + ,m_jobsCount(0) +{ +} + +dgThreadHive::dgWorkerThread::~dgWorkerThread() +{ + dgInterlockedExchange(&m_terminate, 1); + m_workerSemaphore.Release(); + Close(); +} + +void dgThreadHive::dgWorkerThread::SetUp(dgMemoryAllocator* const allocator, const char* const name, dgInt32 id, dgThreadHive* const hive) +{ + m_hive = hive; + m_allocator = allocator; + Init(name, id); +} + +void dgThreadHive::dgWorkerThread::RunNextJobInQueue(dgInt32 threadId) +{ + for (dgInt32 i = 0; i < m_jobsCount; i++) { + const dgThreadJob& job = m_jobPool[i]; + job.m_callback(job.m_context0, job.m_context1, m_id); + } +} + +dgInt32 dgThreadHive::dgWorkerThread::PushJob(const dgThreadJob& job) +{ + dgAssert(m_jobsCount < sizeof (m_jobPool) / sizeof (m_jobPool[0])); + m_jobPool[m_jobsCount] = job; + m_jobsCount++; + return m_jobsCount; +} + + +void dgThreadHive::dgWorkerThread::ConcurrentWork(dgInt32 threadId) +{ + while (dgInterlockedTest(&m_concurrentWork, 1)) { + if (dgInterlockedExchange(&m_pendingWork, 0)) { + //DG_TRACKTIME(); + RunNextJobInQueue(threadId); + m_jobsCount = 0; + dgAtomicExchangeAndAdd(&m_hive->m_syncLock, -1); + } + dgThreadYield(); + } +} + +void dgThreadHive::dgWorkerThread::Execute(dgInt32 threadId) +{ + m_hive->OnBeginWorkerThread(threadId); + + while (!m_terminate) { + m_workerSemaphore.Wait(); + if (!m_terminate) { + m_concurrentWork = 1; + m_hive->m_beginSectionSemaphores[threadId].Release(); + ConcurrentWork(threadId); + m_hive->m_endSectionSemaphores[threadId].Release(); + } + } + + m_hive->OnEndWorkerThread(threadId); +} + +dgThreadHive::dgThreadHive(dgMemoryAllocator* const allocator) + :m_parentThread(NULL) + ,m_workerThreads(NULL) + ,m_allocator(allocator) + ,m_syncLock(0) + ,m_jobsCount(0) + ,m_workerThreadsCount(0) + ,m_globalCriticalSection(0) +{ +} + +dgThreadHive::~dgThreadHive() +{ + DestroyThreads(); +} + +void dgThreadHive::SetParentThread(dgThread* const mastertThread) +{ + m_parentThread = mastertThread; +} + +void dgThreadHive::OnBeginWorkerThread(dgInt32 threadId) +{ +} + +void dgThreadHive::OnEndWorkerThread(dgInt32 threadId) +{ +} + +void dgThreadHive::BeginSection() +{ + if (m_workerThreadsCount) { + //DG_TRACKTIME(); + for (dgInt32 i = 0; i < m_workerThreadsCount; i++) { + m_workerThreads[i].m_workerSemaphore.Release(); + } + m_parentThread->Wait(m_workerThreadsCount, m_beginSectionSemaphores); + } +} + +void dgThreadHive::EndSection() +{ + if (m_workerThreadsCount) { + //DG_TRACKTIME(); + for (dgInt32 i = 0; i < m_workerThreadsCount; i++) { + dgInterlockedExchange(&m_workerThreads[i].m_concurrentWork, 0); + } + m_parentThread->Wait(m_workerThreadsCount, m_endSectionSemaphores); + } +} + +void dgThreadHive::QueueJob(dgWorkerThreadTaskCallback callback, void* const context0, void* const context1, const char* const functionName) +{ + if (!m_workerThreadsCount) { + //DG_TRACKTIME(functionName); + callback(context0, context1, 0); + } else { + dgInt32 workerTreadEntry = m_jobsCount % m_workerThreadsCount; + #ifdef DG_USE_THREAD_EMULATION + //DG_TRACKTIME(functionName); + callback(context0, context1, workerTreadEntry); + #else + dgInt32 index = m_workerThreads[workerTreadEntry].PushJob(dgThreadJob(context0, context1, callback, functionName)); + if (index >= DG_THREAD_POOL_JOB_SIZE) { + dgAssert(0); + SynchronizationBarrier(); + } + #endif + } + m_jobsCount++; +} + +void dgThreadHive::SetThreadsCount(dgInt32 threads) +{ + DestroyThreads(); + + m_workerThreadsCount = dgMin(threads, DG_MAX_THREADS_HIVE_COUNT); + if (m_workerThreadsCount == 1) { + m_workerThreadsCount = 0; + } + + if (m_workerThreadsCount) { + m_workerThreads = new (m_allocator) dgWorkerThread[dgUnsigned32(m_workerThreadsCount)]; + + for (dgInt32 i = 0; i < m_workerThreadsCount; i++) { + char name[256]; + sprintf(name, "dgWorkerThread%d", i); + m_workerThreads[i].SetUp(m_allocator, name, i, this); + } + } +} + +void dgThreadHive::SynchronizationBarrier() +{ + if (m_workerThreadsCount) { + //DG_TRACKTIME(); + + #ifndef DG_USE_THREAD_EMULATION + m_syncLock = m_workerThreadsCount; + for (dgInt32 i = 0; i < m_workerThreadsCount; i++) { + dgInterlockedExchange(&m_workerThreads[i].m_pendingWork, 1); + } + while (dgInterlockedTest(&m_syncLock, 0)) { + dgThreadYield(); + } + #endif + } + m_jobsCount = 0; +} + +void dgThreadHive::DestroyThreads() +{ + if (m_workerThreadsCount) { + delete[] m_workerThreads; + m_workerThreads = NULL; + m_workerThreadsCount = 0; + } +} + +#endif \ No newline at end of file diff --git a/thirdparty/src/newton/dgCore/dgThreadHive.h b/thirdparty/src/newton/dgCore/dgThreadHive.h new file mode 100644 index 000000000..d5fc39e42 --- /dev/null +++ b/thirdparty/src/newton/dgCore/dgThreadHive.h @@ -0,0 +1,283 @@ +/* Copyright (c) <2003-2019> +* +* 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_THREAD_HIVE_H__ +#define __DG_THREAD_HIVE_H__ + +#include "dgThread.h" +#include "dgMemory.h" +#include "dgFastQueue.h" + +#define DG_THREAD_POOL_JOB_SIZE (256) +typedef void (*dgWorkerThreadTaskCallback) (void* const context0, void* const context1, dgInt32 threadID); + +#ifndef WIN32 +#define USE_UNIX_THREAD_POOL +#endif + +#ifdef USE_UNIX_THREAD_POOL + class dgThreadHive + { + public: + class dgThreadJob + { + public: + dgThreadJob() + { + } + + dgThreadJob (void* const context0, void* const context1, dgWorkerThreadTaskCallback callback, const char* const jobName) + :m_context0(context0) + ,m_context1(context1) + ,m_callback(callback) + ,m_jobName(jobName) + { + } + + void* m_context0; + void* m_context1; + dgWorkerThreadTaskCallback m_callback; + const char* m_jobName; + }; + + class dgWorkerThread: public dgThread + { + public: + DG_CLASS_ALLOCATOR(allocator) + + dgWorkerThread(); + ~dgWorkerThread(); + + bool IsBusy() const; + void SetUp(dgMemoryAllocator* const allocator, const char* const name, dgInt32 id, dgThreadHive* const hive); + virtual void Execute (dgInt32 threadId); + + dgInt32 PushJob(const dgThreadJob& job); + void RunNextJobInQueue(dgInt32 threadId); + + dgThreadHive* m_hive; + dgMemoryAllocator* m_allocator; + dgInt32 m_isBusy; + dgInt32 m_jobsCount; + dgSemaphore m_workerSemaphore; + dgThreadJob m_jobPool[DG_THREAD_POOL_JOB_SIZE]; + }; + + dgThreadHive(dgMemoryAllocator* const allocator); + virtual ~dgThreadHive(); + + virtual void OnBeginWorkerThread (dgInt32 threadId); + virtual void OnEndWorkerThread (dgInt32 threadId); + + void BeginSection() {} + void EndSection() {} + + void SetParentThread (dgThread* const mastertThread); + + void GlobalLock() const; + void GlobalUnlock() const; + + void GetIndirectLock (dgInt32* const criticalSectionLock) const; + void ReleaseIndirectLock (dgInt32* const criticalSectionLock) const; + + dgInt32 GetThreadCount() const; + dgInt32 GetMaxThreadCount() const; + void SetThreadsCount (dgInt32 count); + + virtual void QueueJob (dgWorkerThreadTaskCallback callback, void* const context0, void* const context1, const char* const functionName); + virtual void SynchronizationBarrier (); + + private: + void DestroyThreads(); + + dgThread* m_parentThread; + dgWorkerThread* m_workerThreads; + dgMemoryAllocator* m_allocator; + dgInt32 m_jobsCount; + dgInt32 m_workerThreadsCount; + mutable dgInt32 m_globalCriticalSection; + dgThread::dgSemaphore m_beginSectionSemaphores[DG_MAX_THREADS_HIVE_COUNT]; + }; + + DG_INLINE dgInt32 dgThreadHive::GetThreadCount() const + { + return m_workerThreadsCount ? m_workerThreadsCount : 1; + } + + DG_INLINE dgInt32 dgThreadHive::GetMaxThreadCount() const + { + return DG_MAX_THREADS_HIVE_COUNT; + } + + + DG_INLINE void dgThreadHive::GlobalLock() const + { + GetIndirectLock(&m_globalCriticalSection); + } + + DG_INLINE void dgThreadHive::GlobalUnlock() const + { + ReleaseIndirectLock(&m_globalCriticalSection); + } + + DG_INLINE void dgThreadHive::GetIndirectLock (dgInt32* const criticalSectionLock) const + { + if (m_workerThreadsCount) { + dgSpinLock(criticalSectionLock); + } + } + + DG_INLINE void dgThreadHive::ReleaseIndirectLock (dgInt32* const criticalSectionLock) const + { + if (m_workerThreadsCount) { + dgSpinUnlock(criticalSectionLock); + } + } + +#else + class dgThreadHive + { + + class dgThreadJob + { + public: + dgThreadJob() + { + } + + dgThreadJob(void* const context0, void* const context1, dgWorkerThreadTaskCallback callback, const char* const jobName) + :m_context0(context0) + ,m_context1(context1) + ,m_callback(callback) + ,m_jobName(jobName) + { + } + + void* m_context0; + void* m_context1; + const char* m_jobName; + dgWorkerThreadTaskCallback m_callback; + }; + + + class dgWorkerThread: public dgThread + { + public: + DG_CLASS_ALLOCATOR(allocator) + + dgWorkerThread(); + ~dgWorkerThread(); + + void SetUp(dgMemoryAllocator* const allocator, const char* const name, dgInt32 id, dgThreadHive* const hive); + virtual void Execute(dgInt32 threadId); + + dgInt32 PushJob(const dgThreadJob& job); + void RunNextJobInQueue(dgInt32 threadId); + void ConcurrentWork(dgInt32 threadId); + + // bool IsBusy() const; + // dgInt32 m_isBusy; + + dgSemaphore m_workerSemaphore; + dgThreadHive* m_hive; + dgMemoryAllocator* m_allocator; + dgInt32 m_concurrentWork; + dgInt32 m_pendingWork; + dgInt32 m_jobsCount; + dgThreadJob m_jobPool[DG_THREAD_POOL_JOB_SIZE]; + }; + + public: + dgThreadHive(dgMemoryAllocator* const allocator); + virtual ~dgThreadHive(); + + virtual void OnBeginWorkerThread(dgInt32 threadId); + virtual void OnEndWorkerThread(dgInt32 threadId); + + void BeginSection(); + void EndSection(); + + void SetParentThread(dgThread* const mastertThread); + + void GlobalLock() const; + void GlobalUnlock() const; + + void GetIndirectLock(dgInt32* const criticalSectionLock) const; + void ReleaseIndirectLock(dgInt32* const criticalSectionLock) const; + + dgInt32 GetThreadCount() const; + dgInt32 GetMaxThreadCount() const; + void SetThreadsCount(dgInt32 count); + + virtual void QueueJob(dgWorkerThreadTaskCallback callback, void* const context0, void* const context1, const char* const functionName); + virtual void SynchronizationBarrier(); + + private: + void DestroyThreads(); + + dgThread* m_parentThread; + dgWorkerThread* m_workerThreads; + dgMemoryAllocator* m_allocator; + dgInt32 m_syncLock; + dgInt32 m_jobsCount; + dgInt32 m_workerThreadsCount; + mutable dgInt32 m_globalCriticalSection; + dgThread::dgSemaphore m_endSectionSemaphores[DG_MAX_THREADS_HIVE_COUNT]; + dgThread::dgSemaphore m_beginSectionSemaphores[DG_MAX_THREADS_HIVE_COUNT]; + }; + + DG_INLINE dgInt32 dgThreadHive::GetThreadCount() const + { + return m_workerThreadsCount ? m_workerThreadsCount : 1; + } + + DG_INLINE dgInt32 dgThreadHive::GetMaxThreadCount() const + { + return DG_MAX_THREADS_HIVE_COUNT; + } + + DG_INLINE void dgThreadHive::GlobalLock() const + { + GetIndirectLock(&m_globalCriticalSection); + } + + DG_INLINE void dgThreadHive::GlobalUnlock() const + { + ReleaseIndirectLock(&m_globalCriticalSection); + } + + DG_INLINE void dgThreadHive::GetIndirectLock(dgInt32* const criticalSectionLock) const + { + if (m_workerThreadsCount) { + dgSpinLock(criticalSectionLock); + } + } + + DG_INLINE void dgThreadHive::ReleaseIndirectLock(dgInt32* const criticalSectionLock) const + { + if (m_workerThreadsCount) { + dgSpinUnlock(criticalSectionLock); + } + } +#endif + +#endif diff --git a/thirdparty/src/newton/dgCore/dgTree.cpp b/thirdparty/src/newton/dgCore/dgTree.cpp new file mode 100644 index 000000000..5ddcdada4 --- /dev/null +++ b/thirdparty/src/newton/dgCore/dgTree.cpp @@ -0,0 +1,423 @@ +/* Copyright (c) <2003-2019> +* +* 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 "dgTree.h" + + +dgRedBackNode *dgRedBackNode::Minimum () const +{ + dgRedBackNode* ptr = (dgRedBackNode *)this; + for (; ptr->m_left; ptr = ptr->m_left){} + return ptr; +} + +dgRedBackNode *dgRedBackNode::Maximum () const +{ + dgRedBackNode* ptr = (dgRedBackNode *)this; + for (; ptr->m_right; ptr = ptr->m_right){} + return ptr; +} + + +dgRedBackNode *dgRedBackNode::Prev () const +{ + if (m_left) { + return m_left->Maximum (); + } + + dgRedBackNode* node = (dgRedBackNode *)this; + dgRedBackNode* ptr = m_parent; + for (; ptr && node == ptr->m_left; ptr = ptr->m_parent) { + node = ptr; + } + return ptr; + +} + +dgRedBackNode *dgRedBackNode::Next () const +{ + + if (m_right) { + return m_right->Minimum (); + } + + dgRedBackNode* node = (dgRedBackNode *)this; + dgRedBackNode* ptr = m_parent; + for (; ptr && node == ptr->m_right; ptr = ptr->m_parent) { + node = ptr; + } + return ptr; +} + +// rotate node me to left +void dgRedBackNode::RotateLeft(dgRedBackNode** const head) +{ + dgRedBackNode* const me = this; + dgRedBackNode* const child = me->m_right; + + //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 + dgAssert (child); + child->m_left = me; + if (me != NULL) { + me->m_parent = child; + } +} + + +// rotate node me to right * +void dgRedBackNode::RotateRight(dgRedBackNode ** const head) +{ + dgRedBackNode* const me = this; + dgRedBackNode* const child = me->m_left; + + // 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 + dgAssert (child); + child->m_right = me; + if (me != NULL) { + me->m_parent = child; + } +} + + +// maintain Red-Black tree balance after inserting node ptr +void dgRedBackNode::InsertFixup(dgRedBackNode ** const head) +{ + dgRedBackNode* ptr = this; + // check Red-Black properties + while ((ptr != *head) && (ptr->m_parent->GetColor() == RED)) { + // we have a violation + dgAssert (ptr->m_parent); + dgAssert (ptr->m_parent->m_parent); + if (ptr->m_parent == ptr->m_parent->m_parent->m_left) { + dgRedBackNode* 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 { + dgAssert (ptr->m_parent == ptr->m_parent->m_parent->m_right); + // mirror image of above code + dgRedBackNode* 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 dgRedBackNode::RemoveFixup (dgRedBackNode* const thisNode, dgRedBackNode ** const head) +{ + dgRedBackNode* ptr = this; + dgRedBackNode* node = thisNode; + while ((node != *head) && (!node || node->GetColor() == BLACK)) { + if (node == ptr->m_left) { + if (!ptr) { + return; + } + dgRedBackNode* 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; + } + dgRedBackNode* 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 dgRedBackNode::Unlink (dgRedBackNode ** const head) +{ +// dgRedBackNode *child; +// dgRedBackNode *endNode; +// dgRedBackNode *endNodeParent; +// dgRedBackNode::REDBLACK_COLOR oldColor; + + dgRedBackNode* const node = this; +// node->Kill(); + node->SetInTreeFlag(false); + + if (!node->m_left || !node->m_right) { + // y has a NULL node as a child + dgRedBackNode* const endNode = node; + + // x is y's only child + dgRedBackNode* 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); + } +// endNode->Release(); +// delete endNode; + + } else { + + // find tree successor with a NULL node as a child + dgRedBackNode* endNode = node->m_right; + while (endNode->m_left != NULL) { + endNode = endNode->m_left; + } + + dgAssert (endNode); + dgAssert (endNode->m_parent); + dgAssert (!endNode->m_left); + + // x is y's only child + dgRedBackNode* const child = endNode->m_right; + + dgAssert ((endNode != node->m_right) || !child || (child->m_parent == endNode)); + + endNode->m_left = node->m_left; + node->m_left->m_parent = endNode; + + dgRedBackNode* 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; + + dgRedBackNode::REDBLACK_COLOR oldColor = endNode->GetColor(); + endNode->SetColor (node->GetColor()); + node->SetColor (oldColor); + + if (oldColor == BLACK) { + endNodeParent->RemoveFixup (child, head); + } +// node->Release(); +// delete node; + } +} + +void dgRedBackNode::Remove (dgRedBackNode ** const head) +{ + Unlink (head); + delete this; +} + +void dgRedBackNode::RemoveAllLow () +{ + if (m_left) { + m_left->RemoveAllLow(); + } + if (m_right) { + m_right->RemoveAllLow (); + } + SetInTreeFlag(false); +// Kill(); +// Release(); + delete this; +} + +void dgRedBackNode::RemoveAll () +{ + dgRedBackNode* root = this; + for (; root->m_parent; root = root->m_parent); + root->RemoveAllLow(); +} + + diff --git a/thirdparty/src/newton/dgCore/dgTree.h b/thirdparty/src/newton/dgCore/dgTree.h new file mode 100644 index 000000000..a89361d27 --- /dev/null +++ b/thirdparty/src/newton/dgCore/dgTree.h @@ -0,0 +1,848 @@ +/* Copyright (c) <2003-2019> +* +* 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 __dgTree__ +#define __dgTree__ + +#include "dgStdafx.h" +#include "dgRef.h" +#include "dgDebug.h" +#include "dgMemory.h" + + + +// Note: this is a low level class for dgTree use only +// unpredictable result will happen if you attempt to manipulate +// any member of this class +class dgRedBackNode +{ + public: + enum REDBLACK_COLOR + { + RED = true, + BLACK = false + }; + + DG_CLASS_ALLOCATOR(allocator) + + dgRedBackNode() + { + } + + virtual ~dgRedBackNode () + { + } + + void RemoveAllLow (); + void RotateLeft(dgRedBackNode** const head); + void RotateRight(dgRedBackNode** const head); + void RemoveFixup (dgRedBackNode* const node, dgRedBackNode* * const head); + + dgRedBackNode* GetLeft() const; + dgRedBackNode* GetRight() const; + dgRedBackNode* GetParent() const; + + dgRedBackNode (dgRedBackNode* const parent); + inline void Initdata (dgRedBackNode* const parent); + inline void SetColor (REDBLACK_COLOR color); + REDBLACK_COLOR GetColor () const; + dgUnsigned32 IsInTree () const; + inline void SetInTreeFlag (dgUnsigned32 flag); + + void RemoveAll (); + dgRedBackNode* Prev() const; + dgRedBackNode* Next() const; + dgRedBackNode* Minimum() const; + dgRedBackNode* Maximum() const; + void Remove (dgRedBackNode** const head); + void Unlink (dgRedBackNode** const head); + void InsertFixup(dgRedBackNode** const head); + + dgRedBackNode* m_left; + dgRedBackNode* m_right; + dgRedBackNode* m_parent; + dgUnsigned32 m_color : 1; + dgUnsigned32 m_inTree : 1; +// dgUnsigned32 m_pad[2]; +}; + +template +class dgTree +{ + public: + class dgTreeNode: public dgRedBackNode + { + dgTreeNode ( + const OBJECT &info, + const KEY &key, + dgTreeNode* parentNode) + :dgRedBackNode(parentNode), m_info (info), m_key (key) + { +// dgAssert ((dgUnsigned64 (&m_info) & 0x0f) == 0); + } + + ~dgTreeNode () + { + } + + dgTreeNode* GetLeft () const + { + return (dgTreeNode* )dgRedBackNode::m_left; + } + + dgTreeNode* GetRight () const + { + return (dgTreeNode* )dgRedBackNode::m_right; + } + + dgTreeNode* GetParent () + { + return (dgTreeNode* )dgRedBackNode::m_parent; + } + + void SetLeft (dgTreeNode* const node) + { + dgRedBackNode::m_left = node; + } + + void SetRight (dgTreeNode* const node) + { + dgRedBackNode::m_right = node; + } + + void SetParent (dgTreeNode* const node) + { + dgRedBackNode::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 dgTree; + + }; + + class Iterator + { + + public: + Iterator(const dgTree &me) + { + m_ptr = NULL; + m_tree = &me; + } + + ~Iterator() + { + } + + void Begin() + { + m_ptr = m_tree->Minimum(); + } + + void End() + { + m_ptr = m_tree->Maximum(); + } + + void Set (dgTreeNode* const node) + { + m_ptr = node; + } + + operator dgInt32() const + { + return m_ptr != NULL; + } + + void operator++ () + { + dgAssert (m_ptr); + m_ptr = m_ptr->Next(); + } + + void operator++ (dgInt32) + { + dgAssert (m_ptr); + m_ptr = m_ptr->Next(); + } + + void operator-- () + { + dgAssert (m_ptr); + m_ptr = m_ptr->Prev(); + } + + void operator-- (dgInt32) + { + dgAssert (m_ptr); + m_ptr = m_ptr->Prev(); + } + + OBJECT &operator* () const + { + return ((dgTreeNode*)m_ptr)->GetInfo(); + } + + dgTreeNode* GetNode() const + { + return (dgTreeNode*)m_ptr; + } + + KEY GetKey () const + { + dgTreeNode* const tmp = (dgTreeNode*)m_ptr; + return tmp ? tmp->GetKey() : KEY(0); + } + + private: + dgRedBackNode* m_ptr; + const dgTree* m_tree; + + }; + + + // *********************************************************** + // member functions + // *********************************************************** + public: + DG_CLASS_ALLOCATOR(allocator) + +// dgTree (); + dgTree (dgMemoryAllocator* const allocator); + virtual ~dgTree (); + + dgMemoryAllocator* GetAllocator () const; + void SetAllocator (dgMemoryAllocator* const allocator); + + + operator dgInt32() const; + dgInt32 GetCount() const; + + dgTreeNode* GetRoot () const; + dgTreeNode* Minimum () const; + dgTreeNode* Maximum () const; + + dgTreeNode* Find (KEY key) const; + dgTreeNode* FindGreater (KEY key) const; + dgTreeNode* FindGreaterEqual (KEY key) const; + dgTreeNode* FindLessEqual (KEY key) const; + + dgTreeNode* GetNodeFromInfo (OBJECT &info) const; + + dgTreeNode* Insert (const OBJECT &element, KEY key, bool& elementWasInTree); + dgTreeNode* Insert (const OBJECT &element, KEY key); + dgTreeNode* Insert (dgTreeNode* const node, KEY key); + + dgTreeNode* Replace (OBJECT &element, KEY key); + dgTreeNode* ReplaceKey (KEY oldKey, KEY newKey); + dgTreeNode* ReplaceKey (dgTreeNode* const node, KEY key); + + void Remove (KEY key); + void Remove (dgTreeNode* const node); + void RemoveAll (); + + void Unlink (dgTreeNode* const node); + void SwapInfo (dgTree& tree); + + bool SanityCheck () const; + + // *********************************************************** + // member variables + // *********************************************************** + private: + dgInt32 m_count; + dgTreeNode* m_head; + dgMemoryAllocator* m_allocator; + + dgInt32 CompareKeys (const KEY &key0, const KEY &key1) const; + bool SanityCheck (dgTreeNode* const ptr, dgInt32 height) const; + + friend class dgTreeNode; +}; + + +inline dgRedBackNode::dgRedBackNode (dgRedBackNode* const parent) +{ + Initdata (parent); +} + +inline void dgRedBackNode::Initdata (dgRedBackNode* const parent) +{ + SetColor (RED); + SetInTreeFlag (true); + m_left = NULL; + m_right = NULL; + m_parent = parent; +} + +inline void dgRedBackNode::SetColor (dgRedBackNode::REDBLACK_COLOR color) +{ + m_color = color; +} + + + +inline dgRedBackNode::REDBLACK_COLOR dgRedBackNode::GetColor () const +{ + return REDBLACK_COLOR (m_color); +} + + +inline void dgRedBackNode::SetInTreeFlag (dgUnsigned32 flag) +{ + m_inTree = flag; +} + +inline dgUnsigned32 dgRedBackNode::IsInTree () const +{ + return m_inTree; +} + +/* +template +dgTree::dgTree () +{ + m_count = 0; + m_head = NULL; + m_allocator = NULL; +} +*/ + +template +dgTree::dgTree (dgMemoryAllocator* const allocator) + :m_count(0) + ,m_head(NULL) + ,m_allocator(allocator) +{ +} + + +template +dgTree::~dgTree () +{ + RemoveAll(); +} + +template +dgMemoryAllocator* dgTree::GetAllocator () const +{ + return m_allocator; +} + +template +void dgTree::SetAllocator (dgMemoryAllocator* const allocator) +{ + if ((m_count == 0) && (m_allocator == NULL)) { + m_allocator = allocator; + } +} + + +template +dgTree::operator dgInt32() const +{ + return m_head != NULL; +} + +template +dgInt32 dgTree::GetCount() const +{ + return m_count; +} + +template +typename dgTree::dgTreeNode* dgTree::Minimum () const +{ + return m_head ? (dgTreeNode* )m_head->Minimum() : NULL; +} + +template +typename dgTree::dgTreeNode* dgTree::Maximum () const +{ + return m_head ? (dgTreeNode* )m_head->Maximum() : NULL; +} + +template +typename dgTree::dgTreeNode* dgTree::GetRoot () const +{ + return m_head; +} + +template +typename dgTree::dgTreeNode* dgTree::Find (KEY key) const +{ + if (m_head == NULL) { + return NULL; + } + + dgTreeNode* ptr = m_head; + while (ptr != NULL) { + if (key < ptr->m_key) { + dgAssert (CompareKeys (ptr->m_key, key) == -1) ; + ptr = ptr->GetLeft(); + } else if (key > ptr->m_key) { + dgAssert (CompareKeys (ptr->m_key, key) == 1) ; + ptr = ptr->GetRight(); + } else { + dgAssert (CompareKeys (ptr->m_key, key) == 0) ; + break; + } + } + return ptr; +} + +template +typename dgTree::dgTreeNode* dgTree::GetNodeFromInfo (OBJECT &info) const +{ + dgTreeNode* const node = (dgTreeNode* ) &info; + dgInt64 offset = ((char*) &node->m_info) - ((char *) node); + dgTreeNode* const retnode = (dgTreeNode* ) (((char *) node) - offset); + + dgAssert (retnode->IsInTree ()); + dgAssert (&retnode->GetInfo () == &info); + return (retnode->IsInTree ()) ? retnode : NULL; + +} + +template +typename dgTree::dgTreeNode* dgTree::FindGreater (KEY key) const +{ + if (m_head == NULL) { + return NULL; + } + + dgTreeNode* prev = NULL; + dgTreeNode* ptr = m_head; + + while (ptr != NULL) { + if (key < ptr->m_key) { + prev = ptr; + ptr = ptr->GetLeft(); + } else { + ptr = ptr->GetRight(); + } + } + +#ifdef __ENABLE_DG_CONTAINERS_SANITY_CHECK + if (prev) { + Iterator iter (*this); + for (iter.Begin(); iter.GetNode() != prev; iter ++) { + KEY key1 = iter.GetKey(); + dgAssert (key1 <= key); + } + for (; iter.GetNode(); iter ++) { + KEY key1 = iter.GetKey(); + dgAssert (key1 > key); + } + } +#endif + + return (dgTreeNode* )prev; +} + +template +typename dgTree::dgTreeNode* dgTree::FindGreaterEqual (KEY key) const +{ + if (m_head == NULL) { + return NULL; + } + + dgTreeNode* prev = NULL; + dgTreeNode* ptr = m_head; + + while (ptr != NULL) { + if (key == ptr->m_key) { + return ptr; + } + if (key < ptr->m_key) { + prev = ptr; + ptr = ptr->GetLeft(); + } else { + ptr = ptr->GetRight(); + } + } + +#ifdef __ENABLE_DG_CONTAINERS_SANITY_CHECK + if (prev) { + Iterator iter (*this); + for (iter.Begin(); iter.GetNode() != prev; iter ++) { + KEY key1 = iter.GetKey(); + dgAssert (key1 <= key); + } + for (; iter.GetNode(); iter ++) { + KEY key1 = iter.GetKey(); + dgAssert (key1 >= key); + } + } +#endif + + return (dgTreeNode* )prev; +} + +template +typename dgTree::dgTreeNode* dgTree::FindLessEqual (KEY key) const +{ + if (m_head == NULL) { + return NULL; + } + + dgTreeNode* prev = NULL; + dgTreeNode* ptr = m_head; + + while (ptr != NULL) { + if (key == ptr->m_key) { + return ptr; + } + + if (key < ptr->m_key) { + ptr = ptr->GetLeft(); + } else { + prev = ptr; + ptr = ptr->GetRight(); + } + + } + +#ifdef __ENABLE_DG_CONTAINERS_SANITY_CHECK + if (prev) { + Iterator iter (*this); + for (iter.End(); iter.GetNode() != prev; iter --) { + KEY key1 = iter.GetKey(); + dgAssert (key1 >= key); + } + for (; iter.GetNode(); iter --) { + KEY key1 = iter.GetKey(); + dgAssert (key1 < key); + } + } +#endif + + return (dgTreeNode* )prev; +} + +template +typename dgTree::dgTreeNode* dgTree::Insert (const OBJECT &element, KEY key, bool& elementWasInTree) +{ + dgTreeNode* parent = NULL; + dgTreeNode* ptr = m_head; + dgInt32 val = 0; + elementWasInTree = false; + while (ptr != NULL) { + parent = ptr; + + if (key < ptr->m_key) { + dgAssert (CompareKeys (ptr->m_key, key) == -1) ; + val = -1; + ptr = ptr->GetLeft(); + } else if (key > ptr->m_key) { + dgAssert (CompareKeys (ptr->m_key, key) == 1) ; + val = 1; + ptr = ptr->GetRight(); + } else { + dgAssert (CompareKeys (ptr->m_key, key) == 0) ; + elementWasInTree = true; + return ptr; + } + } + + m_count ++; + dgAssert (m_allocator); + ptr = new (m_allocator) dgTreeNode (element, key, parent); + if (!parent) { + m_head = ptr; + } else { + if (val < 0) { + parent->m_left = ptr; + } else { + parent->m_right = ptr; + } + } + + dgTreeNode** const headPtr = (dgTreeNode**) &m_head; +// ptr->InsertFixup ((dgRedBackNode**)&m_head); + ptr->InsertFixup ((dgRedBackNode**)headPtr); + return ptr; +} + +template +typename dgTree::dgTreeNode* dgTree::Insert (const OBJECT &element, KEY key) +{ + bool foundState; + + dgTreeNode* const node = Insert (element, key, foundState); + if (foundState) { + return NULL; + } + return node; +} + +template +typename dgTree::dgTreeNode* dgTree::Insert (typename dgTree::dgTreeNode* const node, KEY key) +{ + dgInt32 val = 0; + dgTreeNode* ptr = m_head; + dgTreeNode* 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; +// } + + if (key < ptr->m_key) { + dgAssert (CompareKeys (ptr->m_key, key) == -1) ; + val = -1; + ptr = ptr->GetLeft(); + } else if (key > ptr->m_key) { + dgAssert (CompareKeys (ptr->m_key, key) == 1) ; + val = 1; + ptr = ptr->GetRight(); + } else { + dgAssert (CompareKeys (ptr->m_key, key) == 0) ; + 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; + } + } + + dgTreeNode** const headPtr = (dgTreeNode**) &m_head; +// ptr->InsertFixup ((dgRedBackNode**)&m_head); + ptr->InsertFixup ((dgRedBackNode**)headPtr); + return ptr; +} + + +template +typename dgTree::dgTreeNode* dgTree::Replace (OBJECT &element, KEY key) +{ + dgTreeNode* parent = NULL; + dgTreeNode* ptr = m_head; + dgInt32 val = 0; + while (ptr != NULL) { + parent = ptr; + + dgAssert (0); + 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(); + } + } + + dgAssert (m_allocator); + ptr = new (m_allocator) dgTreeNode (element, key, parent); + if (!parent) { + m_head = ptr; + } else { + if (val < 0) { + parent->m_left = ptr; + } else { + parent->m_right = ptr; + } + } + + dgTreeNode** const headPtr = (dgTreeNode**) &m_head; +// ptr->InsertFixup ((dgRedBackNode**)&m_head); + ptr->InsertFixup ((dgRedBackNode**)headPtr ); + return ptr; +} + + +template +typename dgTree::dgTreeNode* dgTree::ReplaceKey (typename dgTree::dgTreeNode* const node, KEY key) +{ + Unlink (node); + dgTreeNode* const ptr = Insert (node, key); + dgAssert (ptr); + return ptr; +} + +template +typename dgTree::dgTreeNode* dgTree::ReplaceKey (KEY oldKey, KEY newKey) +{ + dgTreeNode* const node = Find (oldKey); + return node ? ReplaceKey (node, newKey) : NULL; +} + +template +void dgTree::Unlink (typename dgTree::dgTreeNode* const node) +{ + m_count --; + + dgTreeNode** const headPtr = (dgTreeNode**) &m_head; + node->Unlink ((dgRedBackNode**)headPtr); + dgAssert (!Find (node->GetKey())); +} + + +template +void dgTree::Remove (typename dgTree::dgTreeNode* const node) +{ + m_count --; + dgTreeNode** const headPtr = (dgTreeNode**) &m_head; + node->Remove ((dgRedBackNode**)headPtr); +} + +template +void dgTree::Remove (KEY key) +{ + dgTreeNode* node; + + // find node in tree + node = Find (key); + if (node == NULL) { + return; + } + Remove (node); +} + +template +void dgTree::RemoveAll () +{ + if (m_head) { + m_count = 0; + m_head->RemoveAll (); + m_head = NULL; + } +} + +template +bool dgTree::SanityCheck () const +{ + return SanityCheck (m_head, 0); +} + + +template +bool dgTree::SanityCheck (typename dgTree::dgTreeNode* const ptr, dgInt32 height) const +{ + if (!ptr) { + return true; + } + + if (!ptr->IsInTree()) { + return false; + } + + 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() == dgTreeNode::BLACK) { + height ++; + } else if (!((!ptr->m_left || (ptr->m_left->GetColor() == dgTreeNode::BLACK)) && + (!ptr->m_right || (ptr->m_right->GetColor() == dgTreeNode::BLACK)))) { + return false; + } + + if (!ptr->m_left && !ptr->m_right) { + dgInt32 bh = 0; + for (dgTreeNode* x = ptr; x; x = x->GetParent()) { + if (x->GetColor() == dgTreeNode::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 +dgInt32 dgTree::CompareKeys (const KEY &key0, const KEY &key1) const +{ + if (key1 < key0) { + return - 1; + } + if (key1 > key0) { + return 1; + } + return 0; +} + +template +void dgTree::SwapInfo (dgTree& tree) +{ + dgSwap (m_head, tree.m_head); + dgSwap (m_count, tree.m_count); +} + +//template dgInt32 dgTree::m_size = 0; +//template dgMemoryAllocator* dgTree::m_staticAllocator = NULL; + + +#endif + + diff --git a/thirdparty/src/newton/dgCore/dgTypes.cpp b/thirdparty/src/newton/dgCore/dgTypes.cpp new file mode 100644 index 000000000..5843e2053 --- /dev/null +++ b/thirdparty/src/newton/dgCore/dgTypes.cpp @@ -0,0 +1,548 @@ +/* Copyright (c) <2003-2019> +* +* 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 "dgTypes.h" +#include "dgDebug.h" +#include "dgVector.h" +#include "dgMemory.h" +#include "dgStack.h" + + +dgUnsigned64 dgGetTimeInMicrosenconds() +{ +#if (defined (_MSC_VER) || defined (_MINGW_32_VER) || defined (_MINGW_64_VER)) + static LARGE_INTEGER frequency; + static LARGE_INTEGER baseCount; + if (!frequency.QuadPart) { + QueryPerformanceFrequency(&frequency); + QueryPerformanceCounter (&baseCount); + } + + LARGE_INTEGER count; + QueryPerformanceCounter (&count); + count.QuadPart -= baseCount.QuadPart; + dgUnsigned64 ticks = dgUnsigned64 (count.QuadPart * LONGLONG (1000000) / frequency.QuadPart); + return ticks; + +#elif (defined (_POSIX_VER) || defined (_POSIX_VER_64) || defined (ANDROID)) + + timespec ts; + static dgUnsigned64 baseCount = 0; + if (!baseCount) { + clock_gettime(CLOCK_REALTIME, &ts); + baseCount = dgUnsigned64 (ts.tv_sec) * 1000000 + ts.tv_nsec / 1000; + } + clock_gettime(CLOCK_REALTIME, &ts); // Works on Linux + return dgUnsigned64 (ts.tv_sec) * 1000000 + ts.tv_nsec / 1000 - baseCount; + +#elif defined (_MACOSX_VER) + timeval tp; + static dgUnsigned64 baseCount = 0; + if (!baseCount) { + gettimeofday(&tp, NULL); + baseCount = dgUnsigned64 (tp.tv_sec) * 1000000 + tp.tv_usec; + } + + gettimeofday(&tp, NULL); + dgUnsigned64 microsecunds = dgUnsigned64 (tp.tv_sec) * 1000000 + tp.tv_usec; + return microsecunds - baseCount; +#else + #error "dgGetTimeInMicrosenconds implementation required" + return 0; +#endif +} + + +dgFloat64 dgRoundToFloat(dgFloat64 val) +{ + dgInt32 exp; + dgFloat64 mantissa = frexp(val, &exp); + + const dgFloat64 power = 1 << 23; + const dgFloat64 invPower = dgFloat64(1.0f) / power; + mantissa = floor(mantissa * power) * invPower; + + dgFloat64 val1 = ldexp(mantissa, exp); + return val1; +} + + +void dgGetMinMax (dgBigVector &minOut, dgBigVector &maxOut, const dgFloat64* const vertexArray, dgInt32 vCount, dgInt32 strideInBytes) +{ + dgInt32 stride = dgInt32 (strideInBytes / sizeof (dgFloat64)); + const dgFloat64* vArray = vertexArray + stride; + + dgAssert (stride >= 3); + minOut = dgBigVector (vertexArray[0], vertexArray[1], vertexArray[2], dgFloat64 (0.0f)); + maxOut = dgBigVector (vertexArray[0], vertexArray[1], vertexArray[2], dgFloat64 (0.0f)); + + for (dgInt32 i = 1; i < vCount; i ++) { + minOut.m_x = dgMin (minOut.m_x, vArray[0]); + minOut.m_y = dgMin (minOut.m_y, vArray[1]); + minOut.m_z = dgMin (minOut.m_z, vArray[2]); + + maxOut.m_x = dgMax (maxOut.m_x, vArray[0]); + maxOut.m_y = dgMax (maxOut.m_y, vArray[1]); + maxOut.m_z = dgMax (maxOut.m_z, vArray[2]); + + vArray += stride; + } +} + + + + +static inline dgInt32 cmp_vertex (const dgFloat64* const v1, const dgFloat64* const v2, dgInt32 firstSortAxis) +{ + if (v1[firstSortAxis] < v2[firstSortAxis]) { + return -1; + } + + if (v1[firstSortAxis] > v2[firstSortAxis]){ + return 1; + } + + return 0; +} + +static dgInt32 SortVertices (dgFloat64* const vertexList, dgInt32 stride, dgInt32 compareCount, dgInt32 vertexCount, dgFloat64 tolerance) +{ + dgBigVector xc(dgFloat64(0.0f)); + dgBigVector x2c(dgFloat64(0.0f)); + dgBigVector minP (dgFloat64 (1.0e10f), dgFloat64 (1.0e10f), dgFloat64 (1.0e10f), dgFloat64 (0.0f)); + dgBigVector maxP (dgFloat64 (-1.0e10f), dgFloat64 (-1.0e10f), dgFloat64 (-1.0e10f), dgFloat64 (0.0f)); + + for (dgInt32 k = 0, i = 0; i < vertexCount; i ++) { + + dgBigVector x(vertexList[k + 2], vertexList[k + 3], vertexList[k + 4], dgFloat64(0.0f)); + xc += x; + x2c += x * x; + minP = minP.GetMin(x); + maxP = maxP.GetMax(x); + k += stride; + } + + dgBigVector del (maxP - minP); + dgFloat64 minDist = dgMin (del.m_x, del.m_y, del.m_z); + if (minDist < dgFloat64 (1.0e-3f)) { + minDist = dgFloat64 (1.0e-3f); + } + + dgFloat64 tol = tolerance * minDist + dgFloat64 (1.0e-12f); + dgFloat64 sweptWindow = dgFloat64 (2.0f) * tol; + sweptWindow += dgFloat64 (1.0e-4f); + + x2c = x2c.Scale (dgFloat32 (vertexCount)) - xc * xc; + + dgInt32 firstSortAxis = 2; + if ((x2c.m_y >= x2c.m_x) && (x2c.m_y >= x2c.m_z)) { + firstSortAxis = 3; + } else if ((x2c.m_z >= x2c.m_x) && (x2c.m_z >= x2c.m_y)) { + firstSortAxis = 4; + } + + dgInt32 stack[1024][2]; + stack[0][0] = 0; + stack[0][1] = vertexCount - 1; + dgInt32 stackIndex = 1; + while (stackIndex) { + stackIndex --; + dgInt32 lo = stack[stackIndex][0]; + dgInt32 hi = stack[stackIndex][1]; + if ((hi - lo) > 8) { + dgInt32 i = lo; + dgInt32 j = hi; + dgFloat64 val[64]; + memcpy (val, &vertexList[((lo + hi) >> 1) * stride], stride * sizeof (dgFloat64)); + do { + while (cmp_vertex (&vertexList[i * stride], val, firstSortAxis) < 0) i ++; + while (cmp_vertex (&vertexList[j * stride], val, firstSortAxis) > 0) j --; + + if (i <= j) { + if (i < j) { + dgFloat64 tmp[64]; + memcpy (tmp, &vertexList[i * stride], stride * sizeof (dgFloat64)); + memcpy (&vertexList[i * stride], &vertexList[j * stride], stride * sizeof (dgFloat64)); + memcpy (&vertexList[j * stride], tmp, stride * sizeof (dgFloat64)); + } + 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 ++; + } + dgAssert (stackIndex < dgInt32 (sizeof (stack) / (2 * sizeof (stack[0][0])))); + } else { + for (dgInt32 i = lo + 1; i <= hi ; i++) { + dgFloat64 tmp[64]; + memcpy (tmp, &vertexList[i * stride], stride * sizeof (dgFloat64)); + + dgInt32 j = i; + for (; j && (cmp_vertex (&vertexList[(j - 1) * stride], tmp, firstSortAxis) > 0); j --) { + memcpy (&vertexList[j * stride], &vertexList[(j - 1)* stride], stride * sizeof (dgFloat64)); + } + memcpy (&vertexList[j * stride], tmp, stride * sizeof (dgFloat64)); + } + } + } + + +#ifdef _DEBUG + for (dgInt32 i = 0; i < (vertexCount - 1); i ++) { + dgAssert (cmp_vertex (&vertexList[i * stride], &vertexList[(i + 1) * stride], firstSortAxis) <= 0); + } +#endif + + dgInt32 count = 0; + for (dgInt32 i = 0; i < vertexCount; i ++) { + dgInt32 m = i * stride; + dgInt32 index = dgInt32 (vertexList[m + 0]); + if (index == dgInt32 (0xffffffff)) { + dgFloat64 swept = vertexList[m + firstSortAxis] + sweptWindow; + dgInt32 k = i * stride + stride; + for (dgInt32 i1 = i + 1; i1 < vertexCount; i1 ++) { + + index = dgInt32 (vertexList[k + 0]); + if (index == dgInt32 (0xffffffff)) { + dgFloat64 val = vertexList[k + firstSortAxis]; + if (val >= swept) { + break; + } + bool test = true; + for (dgInt32 t = 0; test && (t < compareCount); t ++) { + val = fabs (vertexList[m + t + 2] - vertexList[k + t + 2]); + test = test && (val <= tol); + } + if (test) { + vertexList[k + 0] = dgFloat64 (count); + } + } + k += stride; + } + + dgAssert (&vertexList[count * stride + 2] <= &vertexList[m + 2]); + if (&vertexList[count * stride + 2] < &vertexList[m + 2]) { + memcpy (&vertexList[count * stride + 2], &vertexList[m + 2], (stride - 2) * sizeof (dgFloat64)); + } + vertexList[m + 0] = dgFloat64 (count); + count ++; + } + } + + return count; +} + + +static dgInt32 QuickSortVertices (dgFloat64* const vertList, dgInt32 stride, dgInt32 compareCount, dgInt32 vertexCount, dgFloat64 tolerance) +{ + dgInt32 count = 0; + if (vertexCount > (1024 * 256)) { + dgFloat64 x = dgFloat32 (0.0f); + dgFloat64 y = dgFloat32 (0.0f); + dgFloat64 z = dgFloat32 (0.0f); + dgFloat64 xd = dgFloat32 (0.0f); + dgFloat64 yd = dgFloat32 (0.0f); + dgFloat64 zd = dgFloat32 (0.0f); + + for (dgInt32 i = 0; i < vertexCount; i ++) { + dgFloat64 x0 = vertList[i * stride + 2]; + dgFloat64 y0 = vertList[i * stride + 3]; + dgFloat64 z0 = vertList[i * stride + 4]; + x += x0; + y += y0; + z += z0; + xd += x0 * x0; + yd += y0 * y0; + zd += z0 * z0; + } + + xd = vertexCount * xd - x * x; + yd = vertexCount * yd - y * y; + zd = vertexCount * zd - z * z; + + dgInt32 axis = 2; + dgFloat64 axisVal = x / vertexCount; + if ((yd > xd) && (yd > zd)) { + axis = 3; + axisVal = y / vertexCount; + } + if ((zd > xd) && (zd > yd)) { + axis = 4; + axisVal = z / vertexCount; + } + + dgInt32 i0 = 0; + dgInt32 i1 = vertexCount - 1; + do { + for ( ;vertList[i0 * stride + axis] < axisVal; i0 ++); + for ( ;vertList[i1 * stride + axis] > axisVal; i1 --); + if (i0 <= i1) { + for (dgInt32 i = 0; i < stride; i ++) { + dgSwap (vertList[i0 * stride + i], vertList[i1 * stride + i]); + } + i0 ++; + i1 --; + } + } while (i0 <= i1); + dgAssert (i0 < vertexCount); + + dgInt32 count0 = QuickSortVertices (&vertList[ 0 * stride], stride, compareCount, i0, tolerance); + dgInt32 count1 = QuickSortVertices (&vertList[i0 * stride], stride, compareCount, vertexCount - i0, tolerance); + + count = count0 + count1; + + for (dgInt32 i = 0; i < count1; i ++) { + memcpy (&vertList[(count0 + i) * stride + 2], &vertList[(i0 + i) * stride + 2], (stride - 2) * sizeof (dgFloat64)); + } + + + // dgFloat64* const indexPtr = (dgInt64*)vertList; + for (dgInt32 i = i0; i < vertexCount; i ++) { + // indexPtr[i * stride] += count0; + vertList[i * stride] += dgFloat64 (count0); + } + + } else { + count = SortVertices (vertList, stride, compareCount, vertexCount, tolerance); + } + + return count; +} + + +dgInt32 dgVertexListToIndexList (dgFloat64* const vertList, dgInt32 strideInBytes, dgInt32 compareCount, dgInt32 vertexCount, dgInt32* const indexListOut, dgFloat64 tolerance) +{ + dgSetPrecisionDouble precision; + + if (strideInBytes < 3 * dgInt32 (sizeof (dgFloat64))) { + return 0; + } + if (compareCount < 3) { + return 0; + } + dgAssert (compareCount <= dgInt32 (strideInBytes / sizeof (dgFloat64))); + dgAssert (strideInBytes == dgInt32 (sizeof (dgFloat64) * (strideInBytes / sizeof (dgFloat64)))); + + dgInt32 stride = strideInBytes / dgInt32 (sizeof (dgFloat64)); + dgInt32 stride2 = stride + 2; + + dgStackpool (stride2 * vertexCount); + dgFloat64* const tmpVertexList = &pool[0]; + + dgInt32 k = 0; + dgInt32 m = 0; + for (dgInt32 i = 0; i < vertexCount; i ++) { + memcpy (&tmpVertexList[m + 2], &vertList[k], stride * sizeof (dgFloat64)); + tmpVertexList[m + 0] = dgFloat64 (- 1.0f); + tmpVertexList[m + 1] = dgFloat64 (i); + k += stride; + m += stride2; + } + + dgInt32 count = QuickSortVertices (tmpVertexList, stride2, compareCount, vertexCount, tolerance); + + k = 0; + m = 0; + for (dgInt32 i = 0; i < count; i ++) { + k = i * stride; + m = i * stride2; + memcpy (&vertList[k], &tmpVertexList[m + 2], stride * sizeof (dgFloat64)); + k += stride; + m += stride2; + } + + m = 0; + for (dgInt32 i = 0; i < vertexCount; i ++) { + dgInt32 i1 = dgInt32 (tmpVertexList [m + 1]); + dgInt32 index = dgInt32 (tmpVertexList [m + 0]); + indexListOut[i1] = index; + m += stride2; + } + + return count; +} + +dgInt32 dgVertexListToIndexList (dgFloat32* const vertList, dgInt32 strideInBytes, dgInt32 floatSizeInBytes, dgInt32 unsignedSizeInBytes, dgInt32 vertexCount, dgInt32* const indexList, dgFloat32 tolerance) +{ + dgInt32 stride = dgInt32 (strideInBytes / sizeof (dgFloat32)); + + dgAssert (!unsignedSizeInBytes); + dgStack pool(vertexCount * stride); + + dgInt32 floatCount = dgInt32 (floatSizeInBytes / sizeof (dgFloat32)); + + dgFloat64* const data = &pool[0]; + for (dgInt32 i = 0; i < vertexCount; i ++) { + + dgFloat64* const dst = &data[i * stride]; + dgFloat32* const src = &vertList[i * stride]; + for (dgInt32 j = 0; j < stride; j ++) { + dst[j] = src[j]; + } + } + + dgInt32 count = dgVertexListToIndexList (data, dgInt32 (stride * sizeof (dgFloat64)), floatCount, vertexCount, indexList, dgFloat64 (tolerance)); + for (dgInt32 i = 0; i < count; i ++) { + dgFloat64* const src = &data[i * stride]; + dgFloat32* const dst = &vertList[i * stride]; + for (dgInt32 j = 0; j < stride; j ++) { + dst[j] = dgFloat32 (src[j]); + } + } + + return count; +} + + +//#define SERIALIZE_END 'dne ' +#define SERIALIZE_END 0x646e6520 + +struct dgSerializeMarkerData +{ + dgSerializeMarkerData() + :m_maker0(SERIALIZE_END) + ,m_maker1(SERIALIZE_END) + ,m_maker2(SERIALIZE_END) + ,m_revision(m_currentRevision) + { + } + dgInt32 m_maker0; + dgInt32 m_maker1; + dgInt32 m_maker2; + dgInt32 m_revision; +}; + +void dgSerializeMarker(dgSerialize serializeCallback, void* const userData) +{ + dgSerializeMarkerData marker; + serializeCallback (userData, &marker, sizeof (dgSerializeMarkerData)); +} + +dgInt32 dgDeserializeMarker(dgDeserialize serializeCallback, void* const userData) +{ + dgInt32 state = 0; + while (state != 3) { + dgInt32 marker; + serializeCallback (userData, &marker, sizeof (marker)); + switch (state) + { + case 0: + { + if (marker == SERIALIZE_END) { + state = 1; + break; + } else { + state = 0; + } + break; + } + + case 1: + { + if (marker == SERIALIZE_END) { + state = 2; + break; + } else { + state = 0; + } + break; + } + + case 2: + { + if (marker == SERIALIZE_END) { + state = 3; + break; + } else { + state = 0; + } + break; + } + } + } + + dgInt32 revision; + serializeCallback (userData, &revision, sizeof (revision)); + return revision; +} + + +dgSetPrecisionDouble::dgSetPrecisionDouble() +{ + #if (defined (_MSC_VER) && defined (_WIN_32_VER)) + dgClearFP(); + m_mask = dgInt32 (dgControlFP(0, 0)); + dgControlFP (_PC_53, _MCW_PC); + #endif +} + +dgSetPrecisionDouble::~dgSetPrecisionDouble() +{ + #if (defined (_MSC_VER) && defined (_WIN_32_VER)) + dgClearFP(); + dgControlFP (dgUnsigned32(m_mask), _MCW_PC); + #endif +} + +dgFloatExceptions::dgFloatExceptions(dgUnsigned32 mask) +{ + #if (defined (_MSC_VER) && defined (_WIN_32_VER)) + dgClearFP(); + m_mask = dgControlFP(0, 0); + dgControlFP (m_mask & ~mask, _MCW_EM); + #endif + + #ifdef _MACOSX_VER + #ifndef IOS + fesetenv(FE_DFL_DISABLE_SSE_DENORMS_ENV); + #endif + #elif (defined (_WIN_32_VER) || defined (_WIN_64_VER)) + _MM_SET_FLUSH_ZERO_MODE(_MM_FLUSH_ZERO_ON); + #elif defined (_ARM_VER) + //_MM_SET_FLUSH_ZERO_MODE(_MM_FLUSH_ZERO_ON); + #pragma message ("warning!!! do not forget to set flush to zero for arm cpus") + #endif + +// float a (1.0f); +// float b (0.1f); +// while (a != 0.0f) +// a = a * b; +} + +dgFloatExceptions::~dgFloatExceptions() +{ + #if (defined (_MSC_VER) && defined (_WIN_32_VER)) + dgClearFP(); + dgControlFP(m_mask, _MCW_EM); + #endif +} + diff --git a/thirdparty/src/newton/dgCore/dgTypes.h b/thirdparty/src/newton/dgCore/dgTypes.h new file mode 100644 index 000000000..08c6ee94f --- /dev/null +++ b/thirdparty/src/newton/dgCore/dgTypes.h @@ -0,0 +1,696 @@ +/* Copyright (c) <2003-2019> +* +* 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 + #include + #include + #include + #include + #include + + #ifdef _DEBUG + #pragma warning (disable: 4127) //conditional expression is constant + #endif + + #pragma warning (push, 3) + #include + #include + //#ifndef _DURANGO + // #include + //#endif + #pragma warning (pop) +#endif + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#if (defined (__MINGW32__) || defined (__MINGW64__)) + #include + #include + #include + #include + #include + #include +#endif + +#if (defined (_WIN_32_VER) || defined (_WIN_64_VER)) + #include + #include + #include +#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 + #include + #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 + #include + #include + } + #endif +#endif + +#ifdef _MACOSX_VER + #include + #include + #include + #if (defined __i386__ || defined __x86_64__) + #include + #include + #include //sse3 + #include + #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 + #include + #include +#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 + 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 +DG_INLINE T dgMin(T A, T B) +{ + return (A < B) ? A : B; +} + +template +DG_INLINE T dgMax(T A, T B) +{ + return (A > B) ? A : B; +} + +template +DG_INLINE T dgMin(T A, T B, T C) +{ + return dgMin(dgMin (A, B), C); +} + +template +DG_INLINE T dgMax(T A, T B, T C) +{ + return dgMax(dgMax (A, B), C); +} + +template +DG_INLINE T dgClamp(T val, T min, T max) +{ + return dgMax (min, dgMin (max, val)); +} + +template +DG_INLINE void dgSwap(T& A, T& B) +{ + T tmp (A); + A = B; + B = tmp; +} + +template +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 +DG_INLINE T dgSign(T A) +{ + return (A >= T(0)) ? T(1) : T(-1); +} + +template +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 +#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 + diff --git a/thirdparty/src/newton/dgCore/dgVector.h b/thirdparty/src/newton/dgCore/dgVector.h new file mode 100644 index 000000000..a23339c57 --- /dev/null +++ b/thirdparty/src/newton/dgCore/dgVector.h @@ -0,0 +1,228 @@ +/* Copyright (c) <2003-2019> +* +* 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 __dgVector__ +#define __dgVector__ + +#include "dgStdafx.h" +#include "dgTypes.h" +#include "dgDebug.h" +#include "dgMemory.h" + +#define dgCheckVector(x) (dgCheckFloat(x[0]) && dgCheckFloat(x[1]) && dgCheckFloat(x[2]) && dgCheckFloat(x[3])) + +#ifdef DG_SCALAR_VECTOR_CLASS + #include "dgVectorScalar.h" +#elif defined(__ANDROID__) || defined (_ARM_VER) + #include "dgVectorArmNeon.h" +#else + #include "dgVectorSimd.h" +#endif + +template +class dgTemplateVector +{ + public: + DG_CLASS_ALLOCATOR(allocator) + + DG_INLINE dgTemplateVector () + { + } + + DG_INLINE dgTemplateVector (const T* const ptr) + :m_x(ptr[0]), m_y(ptr[1]), m_z(ptr[2]), m_w (ptr[3]) + { + //dgAssert (dgCheckVector ((*this))); + } + + DG_INLINE dgTemplateVector (const dgTemplateVector& copy) + :m_x(copy.m_x), m_y(copy.m_y), m_z(copy.m_z), m_w (copy.m_w) + { + // dgAssert (dgCheckVector ((*this))); + } + + DG_INLINE dgTemplateVector (T x, T y, T z, T w) + :m_x(x), m_y(y), m_z(z), m_w (w) + { + } + + DG_INLINE T& operator[] (dgInt32 i) + { + dgAssert (i < 4); + dgAssert (i >= 0); + return (&m_x)[i]; + } + + DG_INLINE const T& operator[] (dgInt32 i) const + { + dgAssert (i < 4); + dgAssert (i >= 0); + return (&m_x)[i]; + } + + DG_INLINE T GetScalar() const + { + return m_x; + } + + DG_INLINE dgTemplateVector Scale (T scale) const + { + return dgTemplateVector (m_x * scale, m_y * scale, m_z * scale, m_w * scale); + } + + DG_INLINE dgTemplateVector operator+ (const dgTemplateVector& B) const + { + return dgTemplateVector (m_x + B.m_x, m_y + B.m_y, m_z + B.m_z, m_w + B.m_w); + } + + DG_INLINE dgTemplateVector& operator+= (const dgTemplateVector& A) + { + return (*this = dgTemplateVector (m_x + A.m_x, m_y + A.m_y, m_z + A.m_z, m_w + A.m_w)); + } + + DG_INLINE dgTemplateVector operator- (const dgTemplateVector& A) const + { + return dgTemplateVector (m_x - A.m_x, m_y - A.m_y, m_z - A.m_z, m_w - A.m_w); + } + + DG_INLINE dgTemplateVector& operator-= (const dgTemplateVector& A) + { + return (*this = dgTemplateVector (m_x - A.m_x, m_y - A.m_y, m_z - A.m_z, m_w - A.m_w)); + } + + DG_INLINE dgTemplateVector operator* (const dgTemplateVector& B) const + { + return dgTemplateVector(m_x * B.m_x, m_y * B.m_y, m_z * B.m_z, m_w * B.m_w); + } + + DG_INLINE dgTemplateVector operator*= (const dgTemplateVector& B) const + { + return (*this = dgTemplateVector(m_x * B.m_x, m_y * B.m_y, m_z * B.m_z, m_w * B.m_w)); + } + + DG_INLINE dgTemplateVector AddHorizontal() const + { + T val(m_x + m_y + m_z + m_w); + return dgTemplateVector(val, val, val, val); + } + + DG_INLINE dgTemplateVector MulAdd(const dgTemplateVector& A, const dgTemplateVector& B) const + { + return *this + A * B; + } + + DG_INLINE dgTemplateVector MulSub(const dgTemplateVector& A, const dgTemplateVector& B) const + { + return *this - A * B; + } + + // return dot product + DG_INLINE T DotProduct3 (const dgTemplateVector& A) const + { + return m_x * A.m_x + m_y * A.m_y + m_z * A.m_z; + } + + // return cross product + DG_INLINE dgTemplateVector CrossProduct (const dgTemplateVector& B) const + { + return dgTemplateVector (m_y * B.m_z - m_z * B.m_y, + m_z * B.m_x - m_x * B.m_z, + m_x * B.m_y - m_y * B.m_x, m_w); + } + + DG_INLINE dgTemplateVector CrossProduct(const dgTemplateVector &A, const dgTemplateVector &B) const + { + T cofactor[3][3]; + T array[4][4]; + + const dgTemplateVector& me = *this; + for (dgInt32 i = 0; i < 4; i++) { + array[0][i] = me[i]; + array[1][i] = A[i]; + array[2][i] = B[i]; + array[3][i] = T(1.0f); + } + + dgTemplateVector normal; + T sign = T(-1.0f); + for (dgInt32 i = 0; i < 4; i++) { + + for (dgInt32 j = 0; j < 3; j++) { + dgInt32 k0 = 0; + for (dgInt32 k = 0; k < 4; k++) { + if (k != i) { + cofactor[j][k0] = array[j][k]; + k0++; + } + } + } + T x = cofactor[0][0] * (cofactor[1][1] * cofactor[2][2] - cofactor[1][2] * cofactor[2][1]); + T y = cofactor[0][1] * (cofactor[1][2] * cofactor[2][0] - cofactor[1][0] * cofactor[2][2]); + T z = cofactor[0][2] * (cofactor[1][0] * cofactor[2][1] - cofactor[1][1] * cofactor[2][0]); + T det = x + y + z; + + normal[i] = sign * det; + sign *= T(-1.0f); + } + + return normal; + } + + // return dot 4d dot product + DG_INLINE dgTemplateVector DotProduct (const dgTemplateVector &A) const + { + T val (m_x * A.m_x + m_y * A.m_y + m_z * A.m_z + m_w * A.m_w); + return dgTemplateVector (val, val, val, val); + } + + + T GetMax () const + { + return dgMax(dgMax(m_x, m_y), dgMax(m_z, m_w)); + } + + DG_INLINE dgTemplateVector GetMax(const dgTemplateVector& data) const + { + return dgTemplateVector((m_x > data.m_x) ? m_x : data.m_x, (m_y > data.m_y) ? m_y : data.m_y, (m_z > data.m_z) ? m_z : data.m_z, (m_w > data.m_w) ? m_w : data.m_w); + } + + DG_INLINE dgTemplateVector GetMin(const dgTemplateVector& data) const + { + return dgTemplateVector((m_x < data.m_x) ? m_x : data.m_x, (m_y < data.m_y) ? m_y : data.m_y, (m_z < data.m_z) ? m_z : data.m_z, (m_w < data.m_w) ? m_w : data.m_w); + } + + // check validity of floats +#ifdef _DEBUG + void Trace (char* const name) const + { + dgTrace (("%s %f %f %f %f\n", name, m_x, m_y, m_z, m_w)); + } +#endif + + T m_x; + T m_y; + T m_z; + T m_w; +}; + + + +#endif \ No newline at end of file diff --git a/thirdparty/src/newton/dgCore/dgVectorArmNeon.h b/thirdparty/src/newton/dgCore/dgVectorArmNeon.h new file mode 100644 index 000000000..883897181 --- /dev/null +++ b/thirdparty/src/newton/dgCore/dgVectorArmNeon.h @@ -0,0 +1,2049 @@ +/* Copyright (c) <2003-2016> +* +* 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 __dgVectorArmNeon__ +#define __dgVectorArmNeon__ + +#if 0 +// ***************************************************************************************** +// +// 4 x 1 single precision vector class declaration +// +// ***************************************************************************************** +#ifdef _NEWTON_USE_DOUBLE +#define dgVector dgBigVector +#else + + +#if DG_ARCH >= DG_ARCH_NEON_64 + +A R M 6 4 + +#define vec_minv vminvq_u32 +#define vec_maxv maxvq_u32 +#define vec_hadd4 vaddvq_f32 +#define vec_floor vrndq_f32 + +#else + + +#define DG_INLINE_FORCEINLINE(type) static inline type + +DG_INLINE_FORCEINLINE(int) vec_minv(uint32x4_t v) +{ + uint32x2_t tmp = vpmin_u32(vget_low_u32(v), vget_high_u32(v)); + tmp = vpmin_u32(tmp, tmp); + return tmp[0] != 0; +} + +DG_INLINE_FORCEINLINE(int) vec_maxv(uint32x4_t v) +{ + uint32x2_t tmp = vpmax_u32(vget_low_u32(v), vget_high_u32(v)); + tmp = vpmax_u32(tmp, tmp); + return tmp[0] != 0; +} + + +DG_INLINE_FORCEINLINE(float)vec_hadd4(float32x4_t v) +{ + float32x4_t tmp = vaddq_f32(v, vrev64q_f32(v)); + tmp = vaddq_f32(tmp, vcombine_f32(vget_high_f32(tmp), vget_low_f32(tmp))); + return tmp[0]; +} + + + + +#endif + +DG_INLINE_FORCEINLINE(float) vec_hadd3(float32x4_t v) +{ + float32x2_t temp = vpadd_f32(vget_low_f32(v), vget_low_f32(v)); + temp = vadd_f32(temp, vget_high_f32(v)); + return vget_lane_f32(temp, 0); +} + + +#define vec_mul vmulq_f32 +#define vec_add vaddq_f32 +#define vec_sub vsubq_f32 +#define vec_max vmaxq_f32 +#define vec_min vminq_f32 +#define vec_splat vdupq_n_f32 +#define vec_div vdivq_f32 +#define vec_rcp vrecpeq_f32 +#define vec_store vst1q_f32 +#define vec_load vld1q_f32 +#define vec_abs vabsq_f32 +#define vec_cvt vcvtq_s32_f32 +#define vec_sqrt vrsqrtsq_f32 +#define vec_recp vrecpsq_f32 +#define vec_rsqrt rsqrteq_f32 +#define vec_cmpne vceqq_f32 +#define vec_cmpgt vcgtq_f32 +#define vec_cmpge vcgeq_f32 +#define vec_cmpeq vceqq_f32 +#define vec_cmplt vcltq_f32 +#define vec_cmple vcleq_f32 +#define vec_xor veorq_u32 +#define vec_or vorrq_u32 +#define vec_and vandq_u32 +#define vec_not vmvnq_u32 +#define vec_andnot vbicq_u32 + +#if defined __ARM_FEATURE_FMA +// a * b + c (no rounding, better results) +#define vec_madd vfmaq_f32 +#define vec_msub vmlsq_f32 +#else +#define vec_madd vmlaq_f32 +#define vec_msub vmlsq_f32 +#endif + +static inline float32x4_t vec_set(const float w, const float z, const float y, const float x) +{ + float ptr[] = { x, y, z, w }; + return vec_load(ptr); +} + + +class dgBigVector; +DG_MSC_VECTOR_ALIGMENT +class dgVector { +public: + DG_INLINE dgVector() + { + } + + DG_INLINE dgVector(const float32x4_t type) + : m_type(type) { + } + + DG_INLINE dgVector(const uint32x4_t type) + : m_typeInt(type) { + } + + + DG_INLINE dgVector(dgFloat32 val) + : m_type(vmovq_n_f32(val)) + { + } + + DG_INLINE dgVector (const dgVector& v) + : m_type( v.m_type ) + { + //dgAssert (dgCheckVector ((*this))); + } + + DG_INLINE dgVector (const dgFloat32* const ptr) + :m_x(ptr[0]), m_y(ptr[1]), m_z(ptr[2]), m_w (dgFloat32 (0.0f)) + { + dgAssert (dgCheckVector ((*this))); + } + +#ifndef _NEWTON_USE_DOUBLE + DG_INLINE dgVector(const dgFloat64* const ptr) + :m_x(dgFloat32(ptr[0])) + ,m_y(dgFloat32(ptr[1])) + ,m_z(dgFloat32(ptr[2])) + ,m_w(dgFloat32(0.0f)) + { + } +#endif + + + DG_INLINE dgVector (dgFloat32 x, dgFloat32 y, dgFloat32 z, dgFloat32 w) + :m_x(x), m_y(y), m_z(z), m_w(w) + { + dgAssert (dgCheckVector ((*this))); + } + + DG_INLINE dgVector (dgInt32 ix, dgInt32 iy, dgInt32 iz, dgInt32 iw) + :m_x(*((dgFloat32*)&ix)), m_y(*((dgFloat32*)&iy)), m_z(*((dgFloat32*)&iz)), m_w(*((dgFloat32*)&iw)) + { + } + +#ifndef _NEWTON_USE_DOUBLE + DG_INLINE dgVector (const dgBigVector& copy) + :m_x(dgFloat32 (((dgFloat64*)©)[0])) + ,m_y(dgFloat32 (((dgFloat64*)©)[1])) + ,m_z(dgFloat32 (((dgFloat64*)©)[2])) + ,m_w(dgFloat32 (((dgFloat64*)©)[3])) + { + dgAssert (dgCheckVector ((*this))); + } +#endif + + DG_INLINE dgFloat32 GetScalar () const + { + return m_x; + } + + DG_INLINE void Store (dgFloat32* const dst) const + { + vec_store(dst, m_type); + } + + DG_INLINE dgVector BroadcastX () const + { + return dgVector (m_x); + } + + DG_INLINE dgVector BroadcastY () const + { + return dgVector (m_y); + } + + DG_INLINE dgVector BroadcastZ () const + { + return dgVector (m_z); + } + + DG_INLINE dgVector BroadcastW () const + { + return dgVector (m_w); + } + + + DG_INLINE dgFloat32& operator[] (dgInt32 i) + { + dgAssert (i < 4); + dgAssert (i >= 0); + return (&m_x)[i]; + } + + DG_INLINE const dgFloat32& operator[] (dgInt32 i) const + { + dgAssert (i < 4); + dgAssert (i >= 0); + return (&m_x)[i]; + } + + DG_INLINE dgVector operator+ (const dgVector& A) const + { + return vec_add(m_type, A.m_type); + } + + DG_INLINE dgVector operator- (const dgVector& A) const + { + return vec_sub(m_type, A.m_type); + } + + DG_INLINE dgVector operator* (const dgVector& A) const + { + return vec_mul(m_type, A.m_type); + } + + DG_INLINE dgVector& operator+= (const dgVector& A) + { + m_type = vec_add(m_type, A.m_type); + return *this; + } + + DG_INLINE dgVector& operator-= (const dgVector& A) + { + m_type = vec_sub(m_type, A.m_type); + return *this; + } + + DG_INLINE dgVector& operator*= (const dgVector& A) + { + m_type = vec_mul(m_type, A.m_type); + return *this; + } + + + + DG_INLINE dgVector AddHorizontal () const + { + return vec_hadd3(m_type); // dgVector (m_x + m_y + m_z + m_w); + } + + DG_INLINE dgVector Scale3 (dgFloat32 scale) const + { + return dgVector (m_x * scale, m_y * scale, m_z * scale, m_w); + } + + DG_INLINE dgVector Scale (dgFloat32 scale) const + { + return vec_mul(vmovq_n_f32(scale), m_type); + } + + // component wise multiplication + DG_INLINE dgVector CompProduct3 (const dgVector& A) const + { + return dgVector (m_x * A.m_x, m_y * A.m_y, m_z * A.m_z, A.m_w); + } + + // return dot product + DG_INLINE dgFloat32 DotProduct3 (const dgVector& A) const + { + return vec_hadd3(vec_mul(A.m_type, m_type)); // m_x * A.m_x + m_y * A.m_y + m_z * A.m_z; + } + + // return cross product + DG_INLINE dgVector CrossProduct (const dgVector& B) const + { + /* + float32x4_t v1 = m_type; + float32x4_t v2 = B.m_type; + float32x4x2_t v_1203 = vzipq_f32(vcombine_f32(vrev64_f32(vget_low_f32(v1)), vrev64_f32(vget_low_f32(v2))), vcombine_f32(vget_high_f32(v1), vget_high_f32(v2))); + float32x4x2_t v_2013 = vzipq_f32(vcombine_f32(vrev64_f32(vget_low_f32(v_1203.val[0])), vrev64_f32(vget_low_f32(v_1203.val[1]))), vcombine_f32(vget_high_f32(v_1203.val[0]), vget_high_f32(v_1203.val[1]))); + inVec->vec = vmlsq_f32(vmulq_f32(v_1203.val[0], v_2013.val[1]), v_1203.val[1], v_2013.val[0]); + */ + return dgVector (m_y * B.m_z - m_z * B.m_y, + m_z * B.m_x - m_x * B.m_z, + m_x * B.m_y - m_y * B.m_x, m_w); + } + + DG_INLINE dgVector CrossProduct (const dgVector& A, const dgVector& B) const + { + dgFloat32 cofactor[3][3]; + dgFloat32 array[4][4]; + + const dgVector& me = *this; + for (dgInt32 i = 0; i < 4; i ++) { + array[0][i] = me[i]; + array[1][i] = A[i]; + array[2][i] = B[i]; + array[3][i] = dgFloat32 (1.0f); + } + + dgVector normal; + dgFloat32 sign = dgFloat32 (-1.0f); + for (dgInt32 i = 0; i < 4; i ++) { + + for (dgInt32 j = 0; j < 3; j ++) { + dgInt32 k0 = 0; + for (dgInt32 k = 0; k < 4; k ++) { + if (k != i) { + cofactor[j][k0] = array[j][k]; + k0 ++; + } + } + } + dgFloat32 x = cofactor[0][0] * (cofactor[1][1] * cofactor[2][2] - cofactor[1][2] * cofactor[2][1]); + dgFloat32 y = cofactor[0][1] * (cofactor[1][2] * cofactor[2][0] - cofactor[1][0] * cofactor[2][2]); + dgFloat32 z = cofactor[0][2] * (cofactor[1][0] * cofactor[2][1] - cofactor[1][1] * cofactor[2][0]); + dgFloat32 det = x + y + z; + + normal[i] = sign * det; + sign *= dgFloat32 (-1.0f); + } + + return normal; + } + + DG_INLINE dgVector GetInt () const + { + return vcvtq_u32_f32(m_type); + } + + DG_INLINE dgVector GetFloat () const + { + return vcvtq_f32_u32(m_type); + } + + DG_INLINE dgVector TestZero() const + { + return m_negOne & (*this == m_zero); + } + + DG_INLINE dgVector Floor () const + { +#if DG_ARCH >= DG_ARCH_NEON_64 + // #if __ARM_ARCH >= 8 && defined(__ARM_FEATURE_DIRECTED_ROUNDING) + return vec_floor(m_type); +#else + return dgVector (dgFloor (m_x), dgFloor (m_y), dgFloor (m_z), dgFloor (m_w)); + +#endif + } + + DG_INLINE dgVector DotProduct (const dgVector &A) const + { + auto tmp = vec_mul(m_type, A.m_type); + return vec_hadd4(tmp); + } + + + + DG_INLINE dgVector InvMagSqrt() const + { + return dgVector(dgRsqrt(DotProduct(*this).m_x)); + } + + DG_INLINE dgVector Reciproc() const + { + float32x4_t reciprocal = vrecpeq_f32(m_type); + reciprocal = vrecpsq_f32(m_type, reciprocal) * reciprocal; + return reciprocal; + + } + + + DG_INLINE dgVector MulAdd(const dgVector& A, const dgVector& B) const + { + // a * b + this + return vec_madd(A.m_type, B.m_type, m_type); + } + + DG_INLINE dgVector MulSub(const dgVector& A, const dgVector& B) const + { + // a * b - this + return vec_msub(A.m_type, B.m_type, m_type); + } + + DG_INLINE dgVector InvSqrt() const + { + float32x4_t sqrt_reciprocal = vrsqrteq_f32(m_type); + return vrsqrtsq_f32(m_type * sqrt_reciprocal, sqrt_reciprocal) * sqrt_reciprocal; + } + + DG_INLINE dgVector Sqrt() const + { + float32x4_t sqrt_reciprocal = vrsqrteq_f32(m_type); + float32x4_t tmp = vrsqrtsq_f32(m_type * sqrt_reciprocal, sqrt_reciprocal) * sqrt_reciprocal; + return vec_mul(m_type, tmp); + } + + DG_INLINE dgVector Normalize () const + { + dgAssert (m_w == dgFloat32 (0.0f)); + const dgVector& me = *this; + return me * InvMagSqrt(); + } + + dgVector Abs () const + { + return vec_abs(m_type); + } + + dgFloat32 GetMax () const + { + return dgMax(dgMax(m_x, m_y), dgMax(m_z, m_w)); + } + + dgVector GetMax (const dgVector& data) const + { + return vec_max(m_type, data.m_type); + } + + dgVector GetMin (const dgVector& data) const + { + return vec_min(m_type, data.m_type); + } + + // relational operators + DG_INLINE dgVector operator== (const dgVector& data) const + { + return vec_cmpeq(m_typeInt, data.m_typeInt); + } + + DG_INLINE dgVector operator!= (const dgVector& data) const + { + return vec_cmpne(m_typeInt, data.m_typeInt); + } + + DG_INLINE dgVector operator> (const dgVector& data) const + { + return vec_cmpgt(m_typeInt, data.m_typeInt); + } + + DG_INLINE dgVector operator< (const dgVector& data) const + { + return vec_cmplt(m_typeInt, data.m_typeInt); + } + + DG_INLINE dgVector operator>= (const dgVector& data) const + { + return vec_cmpge(m_typeInt, data.m_typeInt); + } + + DG_INLINE dgVector operator<= (const dgVector& data) const + { + return vec_cmple(m_typeInt, data.m_typeInt); + } + + // logical operations + DG_INLINE dgVector operator& (const dgVector& data) const + { + return vec_and(m_typeInt, data.m_typeInt); + } + + DG_INLINE dgVector operator| (const dgVector& data) const + { + return vec_or(m_typeInt, data.m_typeInt); + } + + DG_INLINE dgVector operator^ (const dgVector& data) const + { + return vec_xor(m_typeInt, data.m_typeInt); + } + + DG_INLINE dgVector AndNot (const dgVector& data) const + { + return vec_andnot(m_typeInt, data.m_typeInt); + } + + DG_INLINE dgInt32 GetSignMask() const + { + const dgInt32* const a = (dgInt32*)&m_x; + return (((a[0] & 0x80000000) ? 1 : 0) | ((a[1] & 0x80000000) ? 2 : 0) | ((a[2] & 0x80000000) ? 4 : 0) | ((a[3] & 0x80000000) ? 8 : 0)); + } + + DG_INLINE dgVector ShiftTripleRight () const + { + return dgVector (m_z, m_x, m_y, m_w); + } + + DG_INLINE dgVector ShiftTripleLeft () const + { + return dgVector (m_y, m_z, m_x, m_w); + } + + DG_INLINE dgVector ShiftRightLogical (int bits) const + { + return dgVector (dgInt32 (dgUnsigned32 (m_ix) >> bits), dgInt32 (dgUnsigned32 (m_iy) >> bits), dgInt32 (dgUnsigned32 (m_iz) >> bits), dgInt32 (dgUnsigned32 (m_iw) >> bits)); + } + + DG_INLINE static void Transpose4x4 (dgVector& dst0, dgVector& dst1, dgVector& dst2, dgVector& dst3, const dgVector& src0, const dgVector& src1, const dgVector& src2, const dgVector& src3) + { + float32x4x2_t vtrn1 = vzipq_f32(src0.m_type, src2.m_type); + float32x4x2_t vtrn2 = vzipq_f32(src1.m_type, src3.m_type); + float32x4x2_t res1 = vzipq_f32(vtrn1.val[0], vtrn2.val[0]); + float32x4x2_t res2 = vzipq_f32(vtrn1.val[1], vtrn2.val[1]); + dst0.m_type = res1.val[0]; + dst1.m_type = res1.val[1]; + dst2.m_type = res2.val[0]; + dst3.m_type = res2.val[1]; + + } + + DG_CLASS_ALLOCATOR(allocator) + + union { + float32x4_t m_type; + uint32x4_t m_typeInt; + dgInt32 m_i[4]; + struct { + dgFloat32 m_x; + dgFloat32 m_y; + dgFloat32 m_z; + dgFloat32 m_w; + }; + struct { + dgInt32 m_ix; + dgInt32 m_iy; + dgInt32 m_iz; + dgInt32 m_iw; + }; + }; + + static dgVector m_zero; + static dgVector m_one; + static dgVector m_wOne; + static dgVector m_half; + static dgVector m_two; + static dgVector m_three; + static dgVector m_negOne; + static dgVector m_xMask; + static dgVector m_yMask; + static dgVector m_zMask; + static dgVector m_wMask; + static dgVector m_signMask; + static dgVector m_triplexMask; +} DG_GCC_VECTOR_ALIGMENT; + +#endif + +DG_MSC_VECTOR_ALIGMENT +class dgBigVector +{ +public: + DG_INLINE dgBigVector() + { + } + + DG_INLINE dgBigVector(dgFloat64 val) + :m_x(val), m_y(val), m_z(val), m_w(val) + { + } + + DG_INLINE dgBigVector(const dgBigVector& v) + : m_x(v.m_x), m_y(v.m_y), m_z(v.m_z), m_w(v.m_w) + { + } + +#ifndef _NEWTON_USE_DOUBLE + DG_INLINE dgBigVector(const dgVector& v) + : m_x(v.m_x), m_y(v.m_y), m_z(v.m_z), m_w(v.m_w) + { + } + + DG_INLINE dgBigVector(const dgFloat32* const ptr) + : m_x(ptr[0]), m_y(ptr[1]), m_z(ptr[2]), m_w(dgFloat32(0.0f)) + { + dgAssert(dgCheckVector((*this))); + } +#endif + + DG_INLINE dgBigVector(const dgFloat64* const ptr) + :m_x(ptr[0]), m_y(ptr[1]), m_z(ptr[2]), m_w(dgFloat32(0.0f)) + { + dgAssert(dgCheckVector((*this))); + } + + DG_INLINE dgBigVector(dgFloat64 x, dgFloat64 y, dgFloat64 z, dgFloat64 w) + : m_x(x), m_y(y), m_z(z), m_w(w) + { + dgAssert(dgCheckVector((*this))); + } + + DG_INLINE dgBigVector(dgInt32 ix, dgInt32 iy, dgInt32 iz, dgInt32 iw) + : m_ix(ix), m_iy(iy), m_iz(iz), m_iw(iw) + { + } + + DG_INLINE dgBigVector(dgInt64 ix, dgInt64 iy, dgInt64 iz, dgInt64 iw) + : m_ix(ix), m_iy(iy), m_iz(iz), m_iw(iw) + { + } + + DG_INLINE dgFloat64 GetScalar() const + { + return m_x; + } + + DG_INLINE void Store(dgFloat64* const dst) const + { + dst[0] = m_x; + dst[1] = m_y; + dst[2] = m_z; + dst[3] = m_w; + } + + DG_INLINE dgBigVector BroadcastX() const + { + return dgBigVector(m_x); + } + + DG_INLINE dgBigVector BroadcastY() const + { + return dgBigVector(m_y); + } + + DG_INLINE dgBigVector BroadcastZ() const + { + return dgBigVector(m_z); + } + + DG_INLINE dgBigVector BroadcastW() const + { + return dgBigVector(m_w); + } + + + DG_INLINE dgFloat64& operator[] (dgInt32 i) + { + dgAssert(i < 4); + dgAssert(i >= 0); + return (&m_x)[i]; + } + + DG_INLINE const dgFloat64& operator[] (dgInt32 i) const + { + dgAssert(i < 4); + dgAssert(i >= 0); + return (&m_x)[i]; + } + + DG_INLINE dgBigVector operator+ (const dgBigVector& A) const + { + return dgBigVector(m_x + A.m_x, m_y + A.m_y, m_z + A.m_z, m_w + A.m_w); + } + + DG_INLINE dgBigVector operator- (const dgBigVector& A) const + { + return dgBigVector(m_x - A.m_x, m_y - A.m_y, m_z - A.m_z, m_w - A.m_w); + } + + DG_INLINE dgBigVector operator* (const dgBigVector& A) const + { + return dgBigVector(m_x * A.m_x, m_y * A.m_y, m_z * A.m_z, m_w * A.m_w); + } + + DG_INLINE dgBigVector& operator+= (const dgBigVector& A) + { + return (*this = dgBigVector(m_x + A.m_x, m_y + A.m_y, m_z + A.m_z, m_w + A.m_w)); + } + + DG_INLINE dgBigVector& operator-= (const dgBigVector& A) + { + return (*this = dgBigVector(m_x - A.m_x, m_y - A.m_y, m_z - A.m_z, m_w - A.m_w)); + } + + DG_INLINE dgBigVector& operator*= (const dgBigVector& A) + { + return (*this = dgBigVector(m_x * A.m_x, m_y * A.m_y, m_z * A.m_z, m_w * A.m_w)); + } + + DG_INLINE dgBigVector AddHorizontal() const + { + return dgBigVector(m_x + m_y + m_z + m_w); + } + + DG_INLINE dgBigVector Scale3(dgFloat64 scale) const + { + return dgBigVector(m_x * scale, m_y * scale, m_z * scale, m_w); + } + + DG_INLINE dgBigVector Scale(dgFloat64 scale) const + { + return dgBigVector(m_x * scale, m_y * scale, m_z * scale, m_w * scale); + } + + // component wise multiplication + DG_INLINE dgBigVector CompProduct3(const dgBigVector& A) const + { + return dgBigVector(m_x * A.m_x, m_y * A.m_y, m_z * A.m_z, A.m_w); + } + + // return dot product + DG_INLINE dgFloat64 DotProduct3(const dgBigVector& A) const + { + return m_x * A.m_x + m_y * A.m_y + m_z * A.m_z; + } + + // return cross product + DG_INLINE dgBigVector CrossProduct(const dgBigVector& B) const + { + return dgBigVector(m_y * B.m_z - m_z * B.m_y, m_z * B.m_x - m_x * B.m_z, m_x * B.m_y - m_y * B.m_x, m_w); + } + + DG_INLINE dgBigVector CrossProduct(const dgBigVector& A, const dgBigVector& B) const + { + dgFloat64 cofactor[3][3]; + dgFloat64 array[4][4]; + + const dgBigVector& me = *this; + for (dgInt32 i = 0; i < 4; i++) { + array[0][i] = me[i]; + array[1][i] = A[i]; + array[2][i] = B[i]; + array[3][i] = dgFloat32(1.0f); + } + + dgBigVector normal; + dgFloat64 sign = dgFloat64(-1.0f); + for (dgInt32 i = 0; i < 4; i++) { + + for (dgInt32 j = 0; j < 3; j++) { + dgInt32 k0 = 0; + for (dgInt32 k = 0; k < 4; k++) { + if (k != i) { + cofactor[j][k0] = array[j][k]; + k0++; + } + } + } + dgFloat64 x = cofactor[0][0] * (cofactor[1][1] * cofactor[2][2] - cofactor[1][2] * cofactor[2][1]); + dgFloat64 y = cofactor[0][1] * (cofactor[1][2] * cofactor[2][0] - cofactor[1][0] * cofactor[2][2]); + dgFloat64 z = cofactor[0][2] * (cofactor[1][0] * cofactor[2][1] - cofactor[1][1] * cofactor[2][0]); + dgFloat64 det = x + y + z; + + normal[i] = sign * det; + sign *= dgFloat64(-1.0f); + } + + return normal; + } + + DG_INLINE dgBigVector GetInt() const + { + return dgBigVector(dgInt64(floor(m_x)), dgInt64(floor(m_y)), dgInt64(floor(m_z)), dgInt64(floor(m_w))); + } + + DG_INLINE dgBigVector TestZero() const + { + const dgInt64* const a = (dgInt64*)&m_x; + return dgBigVector((a[0] == 0) ? dgFloat64(-1.0f) : dgFloat64(1.0f), + (a[1] == 0) ? dgFloat64(-1.0f) : dgFloat64(1.0f), + (a[2] == 0) ? dgFloat64(-1.0f) : dgFloat64(1.0f), + (a[3] == 0) ? dgFloat64(-1.0f) : dgFloat64(1.0f)); + } + + + DG_INLINE dgBigVector Floor() const + { + return dgBigVector(floor(m_x), floor(m_y), floor(m_z), floor(m_w)); + } + + DG_INLINE dgBigVector DotProduct(const dgBigVector &A) const + { + return dgBigVector(m_x * A.m_x + m_y * A.m_y + m_z * A.m_z + m_w * A.m_w); + } + + DG_INLINE dgBigVector Reciproc() const + { + return dgBigVector(dgFloat64(1.0f) / m_x, dgFloat64(1.0f) / m_y, dgFloat64(1.0f) / m_z, dgFloat64(1.0f) / m_w); + } + + DG_INLINE dgBigVector Sqrt() const + { + return dgBigVector(sqrt(m_x), sqrt(m_y), sqrt(m_z), sqrt(m_w)); + } + + DG_INLINE dgBigVector InvSqrt() const + { + return dgBigVector(dgFloat64(1.0f) / sqrt(m_x), dgFloat64(1.0f) / sqrt(m_y), dgFloat64(1.0f) / sqrt(m_z), dgFloat64(1.0f) / sqrt(m_w)); + } + + DG_INLINE dgBigVector InvMagSqrt() const + { + return dgBigVector(dgFloat64(1.0f) / sqrt(DotProduct(*this).m_x)); + } + + DG_INLINE dgBigVector Normalize() const + { + dgAssert(m_w == dgFloat64(0.0f)); + //const dgBigVector& me = *this; + //return *this * dgBigVector (dgRsqrt(DotProduct(*this).m_x)); + return *this * InvMagSqrt(); + } + + dgBigVector Abs() const + { + return dgBigVector((m_x > dgFloat64(0.0f)) ? m_x : -m_x, + (m_y > dgFloat64(0.0f)) ? m_y : -m_y, + (m_z > dgFloat64(0.0f)) ? m_z : -m_z, + (m_w > dgFloat64(0.0f)) ? m_w : -m_w); + } + + dgFloat64 GetMax() const + { + return dgMax(dgMax(m_x, m_y), dgMax(m_z, m_w)); + } + + dgBigVector GetMax(const dgBigVector& data) const + { + return dgBigVector((m_x > data.m_x) ? m_x : data.m_x, + (m_y > data.m_y) ? m_y : data.m_y, + (m_z > data.m_z) ? m_z : data.m_z, + (m_w > data.m_w) ? m_w : data.m_w); + } + + dgBigVector GetMin(const dgBigVector& data) const + { + return dgBigVector((m_x < data.m_x) ? m_x : data.m_x, + (m_y < data.m_y) ? m_y : data.m_y, + (m_z < data.m_z) ? m_z : data.m_z, + (m_w < data.m_w) ? m_w : data.m_w); + } + + // relational operators + DG_INLINE dgBigVector operator== (const dgBigVector& data) const + { + return dgBigVector((m_x == data.m_x) ? dgInt64(-1) : dgInt64(0), + (m_y == data.m_y) ? dgInt64(-1) : dgInt64(0), + (m_z == data.m_z) ? dgInt64(-1) : dgInt64(0), + (m_w == data.m_w) ? dgInt64(-1) : dgInt64(0)); + } + + DG_INLINE dgBigVector operator> (const dgBigVector& data) const + { + return dgBigVector((m_x > data.m_x) ? dgInt64(-1) : dgInt64(0), + (m_y > data.m_y) ? dgInt64(-1) : dgInt64(0), + (m_z > data.m_z) ? dgInt64(-1) : dgInt64(0), + (m_w > data.m_w) ? dgInt64(-1) : dgInt64(0)); + } + + DG_INLINE dgBigVector operator< (const dgBigVector& data) const + { + return dgBigVector((m_x < data.m_x) ? dgInt64(-1) : dgInt64(0), + (m_y < data.m_y) ? dgInt64(-1) : dgInt64(0), + (m_z < data.m_z) ? dgInt64(-1) : dgInt64(0), + (m_w < data.m_w) ? dgInt64(-1) : dgInt64(0)); + } + + DG_INLINE dgBigVector operator>= (const dgBigVector& data) const + { + return dgBigVector((m_x >= data.m_x) ? dgInt64(-1) : dgInt64(0), + (m_y >= data.m_y) ? dgInt64(-1) : dgInt64(0), + (m_z >= data.m_z) ? dgInt64(-1) : dgInt64(0), + (m_w >= data.m_w) ? dgInt64(-1) : dgInt64(0)); + } + + DG_INLINE dgBigVector operator<= (const dgBigVector& data) const + { + return dgBigVector((m_x <= data.m_x) ? dgInt64(-1) : dgInt64(0), + (m_y <= data.m_y) ? dgInt64(-1) : dgInt64(0), + (m_z <= data.m_z) ? dgInt64(-1) : dgInt64(0), + (m_w <= data.m_w) ? dgInt64(-1) : dgInt64(0)); + } + + + // logical operations + DG_INLINE dgBigVector operator& (const dgBigVector& data) const + { + const dgInt64* const a = (dgInt64*)&m_x; + const dgInt64* const b = (dgInt64*)&data.m_x; + return dgBigVector(a[0] & b[0], a[1] & b[1], a[2] & b[2], a[3] & b[3]); + } + + DG_INLINE dgBigVector operator| (const dgBigVector& data) const + { + const dgInt64* const a = (dgInt64*)&m_x; + const dgInt64* const b = (dgInt64*)&data.m_x; + return dgBigVector(a[0] | b[0], a[1] | b[1], a[2] | b[2], a[3] | b[3]); + } + + DG_INLINE dgBigVector operator^ (const dgBigVector& data) const + { + const dgInt64* const a = (dgInt64*)&m_x; + const dgInt64* const b = (dgInt64*)&data.m_x; + return dgBigVector(a[0] ^ b[0], a[1] ^ b[1], a[2] ^ b[2], a[3] ^ b[3]); + } + + DG_INLINE dgBigVector AndNot(const dgBigVector& data) const + { + const dgInt64* const a = (dgInt64*)&m_x; + const dgInt64* const b = (dgInt64*)&data.m_x; + return dgBigVector(a[0] & ~b[0], a[1] & ~b[1], a[2] & ~b[2], a[3] & ~b[3]); + } + + DG_INLINE dgInt32 GetSignMask() const + { + const dgInt64* const a = (dgInt64*)&m_x; + return (((a[0] >> 63) ? 1 : 0) | ((a[1] >> 63) ? 2 : 0) | ((a[2] >> 63) ? 4 : 0) | ((a[3] >> 63) ? 8 : 0)); + } + + DG_INLINE dgBigVector ShiftTripleRight() const + { + return dgBigVector(m_z, m_x, m_y, m_w); + } + + DG_INLINE dgBigVector ShiftTripleLeft() const + { + return dgBigVector(m_y, m_z, m_x, m_w); + } + + DG_INLINE dgBigVector ShiftRightLogical(int bits) const + { + return dgBigVector(dgInt64(dgUnsigned64(m_ix) >> bits), dgInt64(dgUnsigned64(m_iy) >> bits), dgInt64(dgUnsigned64(m_iz) >> bits), dgInt64(dgUnsigned64(m_iw) >> bits)); + } + + DG_INLINE static void Transpose4x4(dgBigVector& dst0, dgBigVector& dst1, dgBigVector& dst2, dgBigVector& dst3, const dgBigVector& src0, const dgBigVector& src1, const dgBigVector& src2, const dgBigVector& src3) + { + dgBigVector tmp0(src0); + dgBigVector tmp1(src1); + dgBigVector tmp2(src2); + dgBigVector tmp3(src3); + + dst0 = dgBigVector(tmp0.m_x, tmp1.m_x, tmp2.m_x, tmp3.m_x); + dst1 = dgBigVector(tmp0.m_y, tmp1.m_y, tmp2.m_y, tmp3.m_y); + dst2 = dgBigVector(tmp0.m_z, tmp1.m_z, tmp2.m_z, tmp3.m_z); + dst3 = dgBigVector(tmp0.m_w, tmp1.m_w, tmp2.m_w, tmp3.m_w); + } + + DG_CLASS_ALLOCATOR(allocator) + + union + { +#if DG_ARCH >= DG_ARCH_NEON_64 + struct { + float64x2_t m_xy; + float64x2_t m_zw; + }; + struct { + int64x2_t m_ixy; + int64x2_t m_izw; + }; +#endif + dgInt64 m_i[4]; + struct + { + dgFloat64 m_x; + dgFloat64 m_y; + dgFloat64 m_z; + dgFloat64 m_w; + }; + struct + { + dgInt64 m_ix; + dgInt64 m_iy; + dgInt64 m_iz; + dgInt64 m_iw; + }; + }; + + static dgBigVector m_zero; + static dgBigVector m_one; + static dgBigVector m_wOne; + static dgBigVector m_half; + static dgBigVector m_two; + static dgBigVector m_three; + static dgBigVector m_negOne; + static dgBigVector m_xMask; + static dgBigVector m_yMask; + static dgBigVector m_zMask; + static dgBigVector m_wMask; + static dgBigVector m_signMask; + static dgBigVector m_triplexMask; +} DG_GCC_VECTOR_ALIGMENT; + + +DG_MSC_VECTOR_ALIGMENT +class dgSpatialVector +{ +public: + DG_INLINE dgSpatialVector() + { + } + + DG_INLINE dgSpatialVector(const dgFloat32 a) + { + for (dgInt32 i = 0; i < 6; i++) { + m_d[i] = a; + } + } + + DG_INLINE dgSpatialVector(const dgVector& low, const dgVector& high) + { + for (dgInt32 i = 0; i < 3; i++) { + m_d[i] = low[i]; + m_d[i + 3] = high[i]; + } + } + + DG_INLINE dgSpatialVector(const dgSpatialVector& src) + { + for (dgInt32 i = 0; i < 6; i++) { + m_d[i] = src[i]; + } + } + + DG_INLINE dgFloat64& operator[] (dgInt32 i) + { + dgAssert(i < 6); + dgAssert(i >= 0); + return m_d[i]; + } + + DG_INLINE const dgFloat64& operator[] (dgInt32 i) const + { + dgAssert(i < 6); + dgAssert(i >= 0); + return m_d[i]; + } + + DG_INLINE dgSpatialVector operator+ (const dgSpatialVector& A) const + { + dgSpatialVector tmp; + for (dgInt32 i = 0; i < 6; i++) { + tmp[i] = m_d[i] + A.m_d[i]; + } + return tmp; + } + + DG_INLINE dgSpatialVector operator* (const dgSpatialVector& A) const + { + dgSpatialVector tmp; + for (dgInt32 i = 0; i < 6; i++) { + tmp[i] = m_d[i] * A.m_d[i]; + } + return tmp; + } + + DG_INLINE dgFloat64 DotProduct(const dgSpatialVector& v) const + { + dgFloat64 ret = dgFloat64(0.0f); + for (dgInt32 i = 0; i < 6; i++) { + ret += m_d[i] * v.m_d[i]; + } + return ret; + } + + DG_INLINE dgSpatialVector Scale(dgFloat64 s) const + { + dgSpatialVector tmp; + for (dgInt32 i = 0; i < 6; i++) { + tmp[i] = m_d[i] * s; + } + return tmp; + } + + dgFloat64 m_d[6]; + static dgSpatialVector m_zero; +} DG_GCC_VECTOR_ALIGMENT; + +#endif + + +// ***************************************************************************************** +// +// 4 x 1 single precision vector class declaration +// +// ***************************************************************************************** +#ifdef _NEWTON_USE_DOUBLE +#define dgVector dgBigVector +#else + +class dgBigVector; +DG_MSC_VECTOR_ALIGNMENT +class dgVector +{ +public: + DG_INLINE dgVector() + { + } + + DG_INLINE dgVector(dgFloat32 val) + :m_type(vmovq_n_f32(val)) + { + } + + DG_INLINE dgVector(const dgVector& v) + :m_type(v.m_type) + { + } + + DG_INLINE dgVector(const float32x4_t type) + :m_type(type) + { + } + + DG_INLINE dgVector(const dgFloat32* const ptr) + //: m_x(ptr[0]), m_y(ptr[1]), m_z(ptr[2]), m_w(ptr[3]) + :m_type(vld1q_f32 (ptr)) + { + dgAssert(dgCheckVector((*this))); + } + +#ifndef _NEWTON_USE_DOUBLE + DG_INLINE dgVector(const dgFloat64* const ptr) + :m_x(dgFloat32(ptr[0])) + ,m_y(dgFloat32(ptr[1])) + ,m_z(dgFloat32(ptr[2])) + ,m_w(dgFloat32(ptr[3])) + { + } +#endif + + DG_INLINE dgVector(dgFloat32 x, dgFloat32 y, dgFloat32 z, dgFloat32 w) + :m_x(x), m_y(y), m_z(z), m_w(w) + { + dgAssert(dgCheckVector((*this))); + } + + DG_INLINE dgVector(dgInt32 ix, dgInt32 iy, dgInt32 iz, dgInt32 iw) + : m_x(*((dgFloat32*)&ix)), m_y(*((dgFloat32*)&iy)), m_z(*((dgFloat32*)&iz)), m_w(*((dgFloat32*)&iw)) + { + } + +#ifndef _NEWTON_USE_DOUBLE + DG_INLINE dgVector(const dgBigVector& copy) + :m_x(dgFloat32(((dgFloat64*)©)[0])) + ,m_y(dgFloat32(((dgFloat64*)©)[1])) + ,m_z(dgFloat32(((dgFloat64*)©)[2])) + ,m_w(dgFloat32(((dgFloat64*)©)[3])) + { + dgAssert(dgCheckVector((*this))); + } +#endif + + DG_INLINE dgFloat32 GetScalar() const + { + return m_x; + } + + DG_INLINE void Store(dgFloat32* const dst) const + { + vst1q_f32(dst, m_type); + } + + DG_INLINE dgVector BroadcastX() const + { + return dgVector(m_x); + } + + DG_INLINE dgVector BroadcastY() const + { + return dgVector(m_y); + } + + DG_INLINE dgVector BroadcastZ() const + { + return dgVector(m_z); + } + + DG_INLINE dgVector BroadcastW() const + { + return dgVector(m_w); + } + + + DG_INLINE dgFloat32& operator[] (dgInt32 i) + { + dgAssert(i < 4); + dgAssert(i >= 0); + return (&m_x)[i]; + } + + DG_INLINE const dgFloat32& operator[] (dgInt32 i) const + { + dgAssert(i < 4); + dgAssert(i >= 0); + return (&m_x)[i]; + } + + DG_INLINE dgVector operator+ (const dgVector& A) const + { + return vaddq_f32(m_type, A.m_type); + } + + DG_INLINE dgVector operator- (const dgVector& A) const + { + return vsubq_f32(m_type, A.m_type); + } + + DG_INLINE dgVector operator* (const dgVector& A) const + { + return vmulq_f32(m_type, A.m_type); + } + + DG_INLINE dgVector& operator+= (const dgVector& A) + { + return (*this = vsubq_f32(m_type, A.m_type)); + } + + DG_INLINE dgVector& operator-= (const dgVector& A) + { + return (*this = vsubq_f32(m_type, A.m_type)); + } + + DG_INLINE dgVector& operator*= (const dgVector& A) + { + return (*this = vmulq_f32(m_type, A.m_type)); + } + + DG_INLINE dgVector MulAdd(const dgVector& A, const dgVector& B) const + { + //return *this + A * B; + //return vfmaq_f32(A.m_type, B.m_type, m_type); + return vmlaq_f32(m_type, A.m_type, B.m_type); + } + + DG_INLINE dgVector MulSub(const dgVector& A, const dgVector& B) const + { + //return *this - A * B; + return vmlsq_f32(m_type, A.m_type, B.m_type); + } + + DG_INLINE dgVector AddHorizontal() const + { + return dgVector(m_x + m_y + m_z + m_w); + //float32x2_t temp = vpadd_f32(vget_low_f32(m_type), vget_low_f32(m_type)); + //temp = vadd_f32(temp, vget_high_f32(m_type)); + //return vget_lane_f32(temp, 0); + } + + DG_INLINE dgVector Scale(dgFloat32 scale) const + { + return dgVector(m_x * scale, m_y * scale, m_z * scale, m_w * scale); + } + + // return dot product + DG_INLINE dgFloat32 DotProduct3(const dgVector& A) const + { + return m_x * A.m_x + m_y * A.m_y + m_z * A.m_z; + } + + // return cross product + DG_INLINE dgVector CrossProduct(const dgVector& B) const + { + return dgVector(m_y * B.m_z - m_z * B.m_y, + m_z * B.m_x - m_x * B.m_z, + m_x * B.m_y - m_y * B.m_x, m_w); + } + + DG_INLINE dgVector CrossProduct(const dgVector& A, const dgVector& B) const + { + dgFloat32 cofactor[3][3]; + dgFloat32 array[4][4]; + + const dgVector& me = *this; + for (dgInt32 i = 0; i < 4; i++) { + array[0][i] = me[i]; + array[1][i] = A[i]; + array[2][i] = B[i]; + array[3][i] = dgFloat32(1.0f); + } + + dgVector normal; + dgFloat32 sign = dgFloat32(-1.0f); + for (dgInt32 i = 0; i < 4; i++) { + + for (dgInt32 j = 0; j < 3; j++) { + dgInt32 k0 = 0; + for (dgInt32 k = 0; k < 4; k++) { + if (k != i) { + cofactor[j][k0] = array[j][k]; + k0++; + } + } + } + dgFloat32 x = cofactor[0][0] * (cofactor[1][1] * cofactor[2][2] - cofactor[1][2] * cofactor[2][1]); + dgFloat32 y = cofactor[0][1] * (cofactor[1][2] * cofactor[2][0] - cofactor[1][0] * cofactor[2][2]); + dgFloat32 z = cofactor[0][2] * (cofactor[1][0] * cofactor[2][1] - cofactor[1][1] * cofactor[2][0]); + dgFloat32 det = x + y + z; + + normal[i] = sign * det; + sign *= dgFloat32(-1.0f); + } + + return normal; + } + + DG_INLINE dgVector GetInt() const + { + return dgVector(dgInt32(dgFloor(m_x)), dgInt32(dgFloor(m_y)), dgInt32(dgFloor(m_z)), dgInt32(dgFloor(m_w))); + } + + DG_INLINE dgVector TestZero() const + { + const dgInt32* const a = (dgInt32*)&m_x; + return dgVector((a[0] == 0) ? dgFloat32(-1.0f) : dgFloat32(1.0f), + (a[1] == 0) ? dgFloat32(-1.0f) : dgFloat32(1.0f), + (a[2] == 0) ? dgFloat32(-1.0f) : dgFloat32(1.0f), + (a[3] == 0) ? dgFloat32(-1.0f) : dgFloat32(1.0f)); + } + + + DG_INLINE dgVector Floor() const + { + return dgVector(dgFloor(m_x), dgFloor(m_y), dgFloor(m_z), dgFloor(m_w)); + } + + DG_INLINE dgVector DotProduct(const dgVector &A) const + { + return dgVector(m_x * A.m_x + m_y * A.m_y + m_z * A.m_z + m_w * A.m_w); + } + + DG_INLINE dgVector Reciproc() const + { + return dgVector(dgFloat32(1.0f) / m_x, dgFloat32(1.0f) / m_y, dgFloat32(1.0f) / m_z, dgFloat32(1.0f) / m_w); + } + + DG_INLINE dgVector Sqrt() const + { + return dgVector(dgSqrt(m_x), dgSqrt(m_y), dgSqrt(m_z), dgSqrt(m_w)); + } + + DG_INLINE dgVector InvSqrt() const + { + return dgVector(dgRsqrt(m_x), dgRsqrt(m_y), dgRsqrt(m_z), dgRsqrt(m_w)); + } + + DG_INLINE dgVector InvMagSqrt() const + { + return dgVector(dgRsqrt(DotProduct(*this).m_x)); + } + + DG_INLINE dgVector Normalize() const + { + dgAssert(m_w == dgFloat32(0.0f)); + const dgVector& me = *this; + return me * InvMagSqrt(); + } + + dgVector Abs() const + { + return vabsq_f32(m_type); + } + + dgFloat32 GetMax() const + { + return dgMax(dgMax(m_x, m_y), dgMax(m_z, m_w)); + } + + dgVector GetMax(const dgVector& data) const + { + return vmaxq_f32(m_type, data.m_type); + } + + dgVector GetMin(const dgVector& data) const + { + return vminq_f32(m_type, data.m_type); + } + + // relational operators + DG_INLINE dgVector operator== (const dgVector& data) const + { + return vceqq_f32(m_typeInt, data.m_typeInt); + } + + DG_INLINE dgVector operator> (const dgVector& data) const + { + return vcgtq_f32(m_typeInt, data.m_typeInt); + } + + DG_INLINE dgVector operator< (const dgVector& data) const + { + return vcltq_f32(m_typeInt, data.m_typeInt); + } + + DG_INLINE dgVector operator>= (const dgVector& data) const + { + return vcgeq_f32(m_typeInt, data.m_typeInt); + } + + DG_INLINE dgVector operator<= (const dgVector& data) const + { + return vcleq_f32(m_typeInt, data.m_typeInt); + } + + // logical operations + DG_INLINE dgVector operator& (const dgVector& data) const + { + return vandq_u32(m_typeInt, data.m_typeInt); + } + + DG_INLINE dgVector operator| (const dgVector& data) const + { + return vorrq_u32(m_typeInt, data.m_typeInt); + } + + DG_INLINE dgVector operator^ (const dgVector& data) const + { + return veorq_u32(m_typeInt, data.m_typeInt); + } + + DG_INLINE dgVector AndNot(const dgVector& data) const + { + return vbicq_u32(m_typeInt, data.m_typeInt); + } + + DG_INLINE dgVector Select(const dgVector& data, const dgVector& mask) const + { + // (((b ^ a) & mask)^a) + return (*this) ^ (mask & (data ^ (*this))); + } + + DG_INLINE dgInt32 GetSignMask() const + { + const dgInt32* const a = (dgInt32*)&m_x; + return (((a[0] & 0x80000000) ? 1 : 0) | ((a[1] & 0x80000000) ? 2 : 0) | ((a[2] & 0x80000000) ? 4 : 0) | ((a[3] & 0x80000000) ? 8 : 0)); + } + + DG_INLINE dgVector ShiftRight() const + { + return dgVector(m_w, m_x, m_y, m_z); + } + + DG_INLINE dgVector ShiftTripleRight() const + { + return dgVector(m_z, m_x, m_y, m_w); + } + + DG_INLINE dgVector ShiftTripleLeft() const + { + return dgVector(m_y, m_z, m_x, m_w); + } + + DG_INLINE dgVector ShiftRightLogical(int bits) const + { + return dgVector(dgInt32(dgUnsigned32(m_ix) >> bits), dgInt32(dgUnsigned32(m_iy) >> bits), dgInt32(dgUnsigned32(m_iz) >> bits), dgInt32(dgUnsigned32(m_iw) >> bits)); + } + + DG_INLINE static void Transpose4x4(dgVector& dst0, dgVector& dst1, dgVector& dst2, dgVector& dst3, const dgVector& src0, const dgVector& src1, const dgVector& src2, const dgVector& src3) + { + float32x4x2_t vtrn1 = vzipq_f32(src0.m_type, src2.m_type); + float32x4x2_t vtrn2 = vzipq_f32(src1.m_type, src3.m_type); + float32x4x2_t res1 = vzipq_f32(vtrn1.val[0], vtrn2.val[0]); + float32x4x2_t res2 = vzipq_f32(vtrn1.val[1], vtrn2.val[1]); + dst0.m_type = res1.val[0]; + dst1.m_type = res1.val[1]; + dst2.m_type = res2.val[0]; + dst3.m_type = res2.val[1]; + } + + DG_CLASS_ALLOCATOR(allocator) + + union { + dgFloat32 m_f[4]; + dgInt32 m_i[4]; + float32x4_t m_type; + uint32x4_t m_typeInt; + struct { + dgFloat32 m_x; + dgFloat32 m_y; + dgFloat32 m_z; + dgFloat32 m_w; + }; + struct { + dgInt32 m_ix; + dgInt32 m_iy; + dgInt32 m_iz; + dgInt32 m_iw; + }; + }; + + static dgVector m_zero; + static dgVector m_one; + static dgVector m_wOne; + static dgVector m_half; + static dgVector m_two; + static dgVector m_three; + static dgVector m_negOne; + static dgVector m_xMask; + static dgVector m_yMask; + static dgVector m_zMask; + static dgVector m_wMask; + static dgVector m_epsilon; + static dgVector m_signMask; + static dgVector m_triplexMask; +} DG_GCC_VECTOR_ALIGNMENT; + +#endif + +DG_MSC_VECTOR_ALIGNMENT +class dgBigVector +{ +public: + DG_INLINE dgBigVector() + { + } + + DG_INLINE dgBigVector(dgFloat64 val) + :m_x(val), m_y(val), m_z(val), m_w(val) + { + } + + DG_INLINE dgBigVector(const dgBigVector& v) + : m_x(v.m_x), m_y(v.m_y), m_z(v.m_z), m_w(v.m_w) + { + } + +#ifndef _NEWTON_USE_DOUBLE + DG_INLINE dgBigVector(const dgVector& v) + : m_x(v.m_x), m_y(v.m_y), m_z(v.m_z), m_w(v.m_w) + { + } + + DG_INLINE dgBigVector(const dgFloat32* const ptr) + : m_x(ptr[0]), m_y(ptr[1]), m_z(ptr[2]), m_w(dgFloat32(0.0f)) + { + dgAssert(dgCheckVector((*this))); + } +#endif + + DG_INLINE dgBigVector(const dgFloat64* const ptr) + :m_x(ptr[0]), m_y(ptr[1]), m_z(ptr[2]), m_w(ptr[3]) + { + dgAssert(dgCheckVector((*this))); + } + + DG_INLINE dgBigVector(dgFloat64 x, dgFloat64 y, dgFloat64 z, dgFloat64 w) + : m_x(x), m_y(y), m_z(z), m_w(w) + { + dgAssert(dgCheckVector((*this))); + } + + DG_INLINE dgBigVector(dgInt32 ix, dgInt32 iy, dgInt32 iz, dgInt32 iw) + : m_ix(ix), m_iy(iy), m_iz(iz), m_iw(iw) + { + } + + DG_INLINE dgBigVector(dgInt64 ix, dgInt64 iy, dgInt64 iz, dgInt64 iw) + : m_ix(ix), m_iy(iy), m_iz(iz), m_iw(iw) + { + } + + DG_INLINE dgFloat64 GetScalar() const + { + return m_x; + } + + DG_INLINE void Store(dgFloat64* const dst) const + { + dst[0] = m_x; + dst[1] = m_y; + dst[2] = m_z; + dst[3] = m_w; + } + + DG_INLINE dgBigVector BroadcastX() const + { + return dgBigVector(m_x); + } + + DG_INLINE dgBigVector BroadcastY() const + { + return dgBigVector(m_y); + } + + DG_INLINE dgBigVector BroadcastZ() const + { + return dgBigVector(m_z); + } + + DG_INLINE dgBigVector BroadcastW() const + { + return dgBigVector(m_w); + } + + DG_INLINE dgFloat64& operator[] (dgInt32 i) + { + dgAssert(i < 4); + dgAssert(i >= 0); + return (&m_x)[i]; + } + + DG_INLINE const dgFloat64& operator[] (dgInt32 i) const + { + dgAssert(i < 4); + dgAssert(i >= 0); + return (&m_x)[i]; + } + + DG_INLINE dgBigVector operator+ (const dgBigVector& A) const + { + return dgBigVector(m_x + A.m_x, m_y + A.m_y, m_z + A.m_z, m_w + A.m_w); + } + + DG_INLINE dgBigVector operator- (const dgBigVector& A) const + { + return dgBigVector(m_x - A.m_x, m_y - A.m_y, m_z - A.m_z, m_w - A.m_w); + } + + DG_INLINE dgBigVector operator* (const dgBigVector& A) const + { + return dgBigVector(m_x * A.m_x, m_y * A.m_y, m_z * A.m_z, m_w * A.m_w); + } + + DG_INLINE dgBigVector& operator+= (const dgBigVector& A) + { + return (*this = dgBigVector(m_x + A.m_x, m_y + A.m_y, m_z + A.m_z, m_w + A.m_w)); + } + + DG_INLINE dgBigVector& operator-= (const dgBigVector& A) + { + return (*this = dgBigVector(m_x - A.m_x, m_y - A.m_y, m_z - A.m_z, m_w - A.m_w)); + } + + DG_INLINE dgBigVector& operator*= (const dgBigVector& A) + { + return (*this = dgBigVector(m_x * A.m_x, m_y * A.m_y, m_z * A.m_z, m_w * A.m_w)); + } + + DG_INLINE dgBigVector MulAdd(const dgBigVector& A, const dgBigVector& B) const + { + return *this + A * B; + } + + DG_INLINE dgBigVector MulSub(const dgVector& A, const dgBigVector& B) const + { + return *this - A * B; + } + + + DG_INLINE dgBigVector AddHorizontal() const + { + return dgBigVector(m_x + m_y + m_z + m_w); + } + + DG_INLINE dgBigVector Scale(dgFloat64 scale) const + { + return dgBigVector(m_x * scale, m_y * scale, m_z * scale, m_w * scale); + } + + // return dot product + DG_INLINE dgFloat64 DotProduct3(const dgBigVector& A) const + { + return m_x * A.m_x + m_y * A.m_y + m_z * A.m_z; + } + + // return cross product + DG_INLINE dgBigVector CrossProduct(const dgBigVector& B) const + { + return dgBigVector(m_y * B.m_z - m_z * B.m_y, m_z * B.m_x - m_x * B.m_z, m_x * B.m_y - m_y * B.m_x, m_w); + } + + DG_INLINE dgBigVector CrossProduct(const dgBigVector& A, const dgBigVector& B) const + { + dgFloat64 cofactor[3][3]; + dgFloat64 array[4][4]; + + const dgBigVector& me = *this; + for (dgInt32 i = 0; i < 4; i++) { + array[0][i] = me[i]; + array[1][i] = A[i]; + array[2][i] = B[i]; + array[3][i] = dgFloat32(1.0f); + } + + dgBigVector normal; + dgFloat64 sign = dgFloat64(-1.0f); + for (dgInt32 i = 0; i < 4; i++) { + + for (dgInt32 j = 0; j < 3; j++) { + dgInt32 k0 = 0; + for (dgInt32 k = 0; k < 4; k++) { + if (k != i) { + cofactor[j][k0] = array[j][k]; + k0++; + } + } + } + dgFloat64 x = cofactor[0][0] * (cofactor[1][1] * cofactor[2][2] - cofactor[1][2] * cofactor[2][1]); + dgFloat64 y = cofactor[0][1] * (cofactor[1][2] * cofactor[2][0] - cofactor[1][0] * cofactor[2][2]); + dgFloat64 z = cofactor[0][2] * (cofactor[1][0] * cofactor[2][1] - cofactor[1][1] * cofactor[2][0]); + dgFloat64 det = x + y + z; + + normal[i] = sign * det; + sign *= dgFloat64(-1.0f); + } + + return normal; + } + + DG_INLINE dgBigVector GetInt() const + { + return dgBigVector(dgInt64(floor(m_x)), dgInt64(floor(m_y)), dgInt64(floor(m_z)), dgInt64(floor(m_w))); + } + + DG_INLINE dgBigVector TestZero() const + { + const dgInt64* const a = (dgInt64*)&m_x; + return dgBigVector((a[0] == 0) ? dgFloat64(-1.0f) : dgFloat64(1.0f), + (a[1] == 0) ? dgFloat64(-1.0f) : dgFloat64(1.0f), + (a[2] == 0) ? dgFloat64(-1.0f) : dgFloat64(1.0f), + (a[3] == 0) ? dgFloat64(-1.0f) : dgFloat64(1.0f)); + } + + + DG_INLINE dgBigVector Floor() const + { + return dgBigVector(floor(m_x), floor(m_y), floor(m_z), floor(m_w)); + } + + DG_INLINE dgBigVector DotProduct(const dgBigVector &A) const + { + return dgBigVector(m_x * A.m_x + m_y * A.m_y + m_z * A.m_z + m_w * A.m_w); + } + + DG_INLINE dgBigVector Reciproc() const + { + return dgBigVector(dgFloat64(1.0f) / m_x, dgFloat64(1.0f) / m_y, dgFloat64(1.0f) / m_z, dgFloat64(1.0f) / m_w); + } + + DG_INLINE dgBigVector Sqrt() const + { + return dgBigVector(sqrt(m_x), sqrt(m_y), sqrt(m_z), sqrt(m_w)); + } + + DG_INLINE dgBigVector InvSqrt() const + { + return dgBigVector(dgFloat64(1.0f) / sqrt(m_x), dgFloat64(1.0f) / sqrt(m_y), dgFloat64(1.0f) / sqrt(m_z), dgFloat64(1.0f) / sqrt(m_w)); + } + + DG_INLINE dgBigVector InvMagSqrt() const + { + return dgBigVector(dgFloat64(1.0f) / sqrt(DotProduct(*this).m_x)); + } + + DG_INLINE dgBigVector Normalize() const + { + dgAssert(m_w == dgFloat64(0.0f)); + //const dgBigVector& me = *this; + //return *this * dgBigVector (dgRsqrt(DotProduct(*this).m_x)); + return *this * InvMagSqrt(); + } + + dgBigVector Abs() const + { + return dgBigVector((m_x > dgFloat64(0.0f)) ? m_x : -m_x, + (m_y > dgFloat64(0.0f)) ? m_y : -m_y, + (m_z > dgFloat64(0.0f)) ? m_z : -m_z, + (m_w > dgFloat64(0.0f)) ? m_w : -m_w); + } + + dgFloat64 GetMax() const + { + return dgMax(dgMax(m_x, m_y), dgMax(m_z, m_w)); + } + + dgBigVector GetMax(const dgBigVector& data) const + { + return dgBigVector((m_x > data.m_x) ? m_x : data.m_x, + (m_y > data.m_y) ? m_y : data.m_y, + (m_z > data.m_z) ? m_z : data.m_z, + (m_w > data.m_w) ? m_w : data.m_w); + } + + dgBigVector GetMin(const dgBigVector& data) const + { + return dgBigVector((m_x < data.m_x) ? m_x : data.m_x, + (m_y < data.m_y) ? m_y : data.m_y, + (m_z < data.m_z) ? m_z : data.m_z, + (m_w < data.m_w) ? m_w : data.m_w); + } + + // relational operators + DG_INLINE dgBigVector operator== (const dgBigVector& data) const + { + return dgBigVector((m_x == data.m_x) ? dgInt64(-1) : dgInt64(0), + (m_y == data.m_y) ? dgInt64(-1) : dgInt64(0), + (m_z == data.m_z) ? dgInt64(-1) : dgInt64(0), + (m_w == data.m_w) ? dgInt64(-1) : dgInt64(0)); + } + + DG_INLINE dgBigVector operator> (const dgBigVector& data) const + { + return dgBigVector((m_x > data.m_x) ? dgInt64(-1) : dgInt64(0), + (m_y > data.m_y) ? dgInt64(-1) : dgInt64(0), + (m_z > data.m_z) ? dgInt64(-1) : dgInt64(0), + (m_w > data.m_w) ? dgInt64(-1) : dgInt64(0)); + } + + DG_INLINE dgBigVector operator< (const dgBigVector& data) const + { + return dgBigVector((m_x < data.m_x) ? dgInt64(-1) : dgInt64(0), + (m_y < data.m_y) ? dgInt64(-1) : dgInt64(0), + (m_z < data.m_z) ? dgInt64(-1) : dgInt64(0), + (m_w < data.m_w) ? dgInt64(-1) : dgInt64(0)); + } + + DG_INLINE dgBigVector operator>= (const dgBigVector& data) const + { + return dgBigVector((m_x >= data.m_x) ? dgInt64(-1) : dgInt64(0), + (m_y >= data.m_y) ? dgInt64(-1) : dgInt64(0), + (m_z >= data.m_z) ? dgInt64(-1) : dgInt64(0), + (m_w >= data.m_w) ? dgInt64(-1) : dgInt64(0)); + } + + DG_INLINE dgBigVector operator<= (const dgBigVector& data) const + { + return dgBigVector((m_x <= data.m_x) ? dgInt64(-1) : dgInt64(0), + (m_y <= data.m_y) ? dgInt64(-1) : dgInt64(0), + (m_z <= data.m_z) ? dgInt64(-1) : dgInt64(0), + (m_w <= data.m_w) ? dgInt64(-1) : dgInt64(0)); + } + + + // logical operations + DG_INLINE dgBigVector operator& (const dgBigVector& data) const + { + const dgInt64* const a = (dgInt64*)&m_x; + const dgInt64* const b = (dgInt64*)&data.m_x; + return dgBigVector(a[0] & b[0], a[1] & b[1], a[2] & b[2], a[3] & b[3]); + } + + DG_INLINE dgBigVector operator| (const dgBigVector& data) const + { + const dgInt64* const a = (dgInt64*)&m_x; + const dgInt64* const b = (dgInt64*)&data.m_x; + return dgBigVector(a[0] | b[0], a[1] | b[1], a[2] | b[2], a[3] | b[3]); + } + + DG_INLINE dgBigVector operator^ (const dgBigVector& data) const + { + const dgInt64* const a = (dgInt64*)&m_x; + const dgInt64* const b = (dgInt64*)&data.m_x; + return dgBigVector(a[0] ^ b[0], a[1] ^ b[1], a[2] ^ b[2], a[3] ^ b[3]); + } + + DG_INLINE dgBigVector AndNot(const dgBigVector& data) const + { + const dgInt64* const a = (dgInt64*)&m_x; + const dgInt64* const b = (dgInt64*)&data.m_x; + return dgBigVector(a[0] & ~b[0], a[1] & ~b[1], a[2] & ~b[2], a[3] & ~b[3]); + } + + DG_INLINE dgBigVector Select(const dgBigVector& data, const dgBigVector& mask) const + { + // (((b ^ a) & mask)^a) + return (*this) ^ (mask & (data ^ (*this))); + } + + DG_INLINE dgInt32 GetSignMask() const + { + const dgInt64* const a = (dgInt64*)&m_x; + return (((a[0] >> 63) ? 1 : 0) | ((a[1] >> 63) ? 2 : 0) | ((a[2] >> 63) ? 4 : 0) | ((a[3] >> 63) ? 8 : 0)); + } + + DG_INLINE dgVector ShiftRight() const + { + return dgBigVector(m_w, m_x, m_y, m_z); + } + + DG_INLINE dgBigVector ShiftTripleRight() const + { + return dgBigVector(m_z, m_x, m_y, m_w); + } + + DG_INLINE dgBigVector ShiftTripleLeft() const + { + return dgBigVector(m_y, m_z, m_x, m_w); + } + + DG_INLINE dgBigVector ShiftRightLogical(int bits) const + { + return dgBigVector(dgInt64(dgUnsigned64(m_ix) >> bits), dgInt64(dgUnsigned64(m_iy) >> bits), dgInt64(dgUnsigned64(m_iz) >> bits), dgInt64(dgUnsigned64(m_iw) >> bits)); + } + + DG_INLINE static void Transpose4x4(dgBigVector& dst0, dgBigVector& dst1, dgBigVector& dst2, dgBigVector& dst3, const dgBigVector& src0, const dgBigVector& src1, const dgBigVector& src2, const dgBigVector& src3) + { + dgBigVector tmp0(src0); + dgBigVector tmp1(src1); + dgBigVector tmp2(src2); + dgBigVector tmp3(src3); + + dst0 = dgBigVector(tmp0.m_x, tmp1.m_x, tmp2.m_x, tmp3.m_x); + dst1 = dgBigVector(tmp0.m_y, tmp1.m_y, tmp2.m_y, tmp3.m_y); + dst2 = dgBigVector(tmp0.m_z, tmp1.m_z, tmp2.m_z, tmp3.m_z); + dst3 = dgBigVector(tmp0.m_w, tmp1.m_w, tmp2.m_w, tmp3.m_w); + } + + DG_CLASS_ALLOCATOR(allocator) + + union + { + dgInt64 m_i[4]; + struct + { + dgFloat64 m_x; + dgFloat64 m_y; + dgFloat64 m_z; + dgFloat64 m_w; + }; + struct + { + dgInt64 m_ix; + dgInt64 m_iy; + dgInt64 m_iz; + dgInt64 m_iw; + }; + }; + + static dgBigVector m_zero; + static dgBigVector m_one; + static dgBigVector m_wOne; + static dgBigVector m_half; + static dgBigVector m_two; + static dgBigVector m_three; + static dgBigVector m_negOne; + static dgBigVector m_xMask; + static dgBigVector m_yMask; + static dgBigVector m_zMask; + static dgBigVector m_wMask; + static dgBigVector m_epsilon; + static dgBigVector m_signMask; + static dgBigVector m_triplexMask; +} DG_GCC_VECTOR_ALIGNMENT; + + +DG_MSC_VECTOR_ALIGNMENT +class dgSpatialVector +{ + public: + DG_INLINE dgSpatialVector() + { + } + + DG_INLINE dgSpatialVector(const dgFloat32 a) + { + for (dgInt32 i = 0; i < 6; i++) { + m_d[i] = a; + } + } + + DG_INLINE dgSpatialVector(const dgVector& low, const dgVector& high) + { + for (dgInt32 i = 0; i < 3; i++) { + m_d[i] = low[i]; + m_d[i + 3] = high[i]; + } + } + + DG_INLINE dgSpatialVector(const dgSpatialVector& src) + { + for (dgInt32 i = 0; i < 6; i++) { + m_d[i] = src[i]; + } + } + + DG_INLINE dgFloat64& operator[] (dgInt32 i) + { + dgAssert(i < 6); + dgAssert(i >= 0); + return m_d[i]; + } + + DG_INLINE const dgFloat64& operator[] (dgInt32 i) const + { + dgAssert(i < 6); + dgAssert(i >= 0); + return m_d[i]; + } + + DG_INLINE dgSpatialVector operator+ (const dgSpatialVector& A) const + { + dgSpatialVector tmp; + for (dgInt32 i = 0; i < 6; i++) { + tmp[i] = m_d[i] + A.m_d[i]; + } + return tmp; + } + + DG_INLINE dgSpatialVector operator* (const dgSpatialVector& A) const + { + dgSpatialVector tmp; + for (dgInt32 i = 0; i < 6; i++) { + tmp[i] = m_d[i] * A.m_d[i]; + } + return tmp; + } + + DG_INLINE dgFloat64 DotProduct(const dgSpatialVector& v) const + { + dgFloat64 ret = dgFloat64(0.0f); + for (dgInt32 i = 0; i < 6; i++) { + ret += m_d[i] * v.m_d[i]; + } + return ret; + } + + DG_INLINE dgSpatialVector Scale(dgFloat64 s) const + { + dgSpatialVector tmp; + for (dgInt32 i = 0; i < 6; i++) { + tmp[i] = m_d[i] * s; + } + return tmp; + } + + dgFloat64 m_d[6]; + static dgSpatialVector m_zero; +} DG_GCC_VECTOR_ALIGNMENT; + +#endif \ No newline at end of file diff --git a/thirdparty/src/newton/dgCore/dgVectorScalar.h b/thirdparty/src/newton/dgCore/dgVectorScalar.h new file mode 100644 index 000000000..c7b3f756a --- /dev/null +++ b/thirdparty/src/newton/dgCore/dgVectorScalar.h @@ -0,0 +1,1002 @@ +/* Copyright (c) <2003-2019> +* +* 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 __dgVectorScalar__ +#define __dgVectorScalar__ + +// ***************************************************************************************** +// +// 4 x 1 single precision vector class declaration +// +// ***************************************************************************************** +#ifdef _NEWTON_USE_DOUBLE + #define dgVector dgBigVector +#else + +class dgBigVector; +DG_MSC_VECTOR_ALIGNMENT +class dgVector +{ + public: + DG_INLINE dgVector() + { + } + + DG_INLINE dgVector(dgFloat32 val) + :m_x(val), m_y(val), m_z(val), m_w(val) + { + } + + DG_INLINE dgVector (const dgVector& v) + :m_x(v.m_x), m_y(v.m_y), m_z(v.m_z), m_w(v.m_w) + { + //dgAssert (dgCheckVector ((*this))); + } + + DG_INLINE dgVector (const dgFloat32* const ptr) + :m_x(ptr[0]), m_y(ptr[1]), m_z(ptr[2]), m_w (ptr[3]) + { + dgAssert (dgCheckVector ((*this))); + } + +#ifndef _NEWTON_USE_DOUBLE + DG_INLINE dgVector(const dgFloat64* const ptr) + :m_x(dgFloat32(ptr[0])) + ,m_y(dgFloat32(ptr[1])) + ,m_z(dgFloat32(ptr[2])) + ,m_w(dgFloat32(ptr[3])) + { + } +#endif + + + DG_INLINE dgVector (dgFloat32 x, dgFloat32 y, dgFloat32 z, dgFloat32 w) + :m_x(x), m_y(y), m_z(z), m_w(w) + { + dgAssert (dgCheckVector ((*this))); + } + + DG_INLINE dgVector (dgInt32 ix, dgInt32 iy, dgInt32 iz, dgInt32 iw) + :m_x(*((dgFloat32*)&ix)), m_y(*((dgFloat32*)&iy)), m_z(*((dgFloat32*)&iz)), m_w(*((dgFloat32*)&iw)) + { + } + +#ifndef _NEWTON_USE_DOUBLE + DG_INLINE dgVector (const dgBigVector& copy) + :m_x(dgFloat32 (((dgFloat64*)©)[0])) + ,m_y(dgFloat32 (((dgFloat64*)©)[1])) + ,m_z(dgFloat32 (((dgFloat64*)©)[2])) + ,m_w(dgFloat32 (((dgFloat64*)©)[3])) + { + dgAssert (dgCheckVector ((*this))); + } +#endif + + DG_INLINE dgFloat32 GetScalar () const + { + return m_x; + } + + DG_INLINE void Store (dgFloat32* const dst) const + { + dst[0] = m_x; + dst[1] = m_y; + dst[2] = m_z; + dst[3] = m_w; + } + + DG_INLINE dgVector BroadcastX () const + { + return dgVector (m_x); + } + + DG_INLINE dgVector BroadcastY () const + { + return dgVector (m_y); + } + + DG_INLINE dgVector BroadcastZ () const + { + return dgVector (m_z); + } + + DG_INLINE dgVector BroadcastW () const + { + return dgVector (m_w); + } + + + DG_INLINE dgFloat32& operator[] (dgInt32 i) + { + dgAssert (i < 4); + dgAssert (i >= 0); + return (&m_x)[i]; + } + + DG_INLINE const dgFloat32& operator[] (dgInt32 i) const + { + dgAssert (i < 4); + dgAssert (i >= 0); + return (&m_x)[i]; + } + + DG_INLINE dgVector operator+ (const dgVector& A) const + { + return dgVector (m_x + A.m_x, m_y + A.m_y, m_z + A.m_z, m_w + A.m_w); + } + + DG_INLINE dgVector operator- (const dgVector& A) const + { + return dgVector (m_x - A.m_x, m_y - A.m_y, m_z - A.m_z, m_w - A.m_w); + } + + DG_INLINE dgVector operator* (const dgVector& A) const + { + return dgVector(m_x * A.m_x, m_y * A.m_y, m_z * A.m_z, m_w * A.m_w); + } + + DG_INLINE dgVector& operator+= (const dgVector& A) + { + return (*this = dgVector (m_x + A.m_x, m_y + A.m_y, m_z + A.m_z, m_w + A.m_w)); + } + + DG_INLINE dgVector& operator-= (const dgVector& A) + { + return (*this = dgVector (m_x - A.m_x, m_y - A.m_y, m_z - A.m_z, m_w - A.m_w)); + } + + DG_INLINE dgVector& operator*= (const dgVector& A) + { + return (*this = dgVector(m_x * A.m_x, m_y * A.m_y, m_z * A.m_z, m_w * A.m_w)); + } + + DG_INLINE dgVector MulAdd(const dgVector& A, const dgVector& B) const + { + return *this + A * B; + } + + DG_INLINE dgVector MulSub(const dgVector& A, const dgVector& B) const + { + return *this - A * B; + } + + DG_INLINE dgVector AddHorizontal () const + { + return dgVector (m_x + m_y + m_z + m_w); + } + + DG_INLINE dgVector Scale (dgFloat32 scale) const + { + return dgVector (m_x * scale, m_y * scale, m_z * scale, m_w * scale); + } + + // return dot product + DG_INLINE dgFloat32 DotProduct3 (const dgVector& A) const + { + return m_x * A.m_x + m_y * A.m_y + m_z * A.m_z; + } + + // return cross product + DG_INLINE dgVector CrossProduct (const dgVector& B) const + { + return dgVector (m_y * B.m_z - m_z * B.m_y, + m_z * B.m_x - m_x * B.m_z, + m_x * B.m_y - m_y * B.m_x, m_w); + } + + DG_INLINE dgVector CrossProduct (const dgVector& A, const dgVector& B) const + { + dgFloat32 cofactor[3][3]; + dgFloat32 array[4][4]; + + const dgVector& me = *this; + for (dgInt32 i = 0; i < 4; i ++) { + array[0][i] = me[i]; + array[1][i] = A[i]; + array[2][i] = B[i]; + array[3][i] = dgFloat32 (1.0f); + } + + dgVector normal; + dgFloat32 sign = dgFloat32 (-1.0f); + for (dgInt32 i = 0; i < 4; i ++) { + + for (dgInt32 j = 0; j < 3; j ++) { + dgInt32 k0 = 0; + for (dgInt32 k = 0; k < 4; k ++) { + if (k != i) { + cofactor[j][k0] = array[j][k]; + k0 ++; + } + } + } + dgFloat32 x = cofactor[0][0] * (cofactor[1][1] * cofactor[2][2] - cofactor[1][2] * cofactor[2][1]); + dgFloat32 y = cofactor[0][1] * (cofactor[1][2] * cofactor[2][0] - cofactor[1][0] * cofactor[2][2]); + dgFloat32 z = cofactor[0][2] * (cofactor[1][0] * cofactor[2][1] - cofactor[1][1] * cofactor[2][0]); + dgFloat32 det = x + y + z; + + normal[i] = sign * det; + sign *= dgFloat32 (-1.0f); + } + + return normal; + } + + DG_INLINE dgVector GetInt () const + { + return dgVector (dgInt32 (dgFloor (m_x)), dgInt32(dgFloor (m_y)), dgInt32(dgFloor (m_z)), dgInt32 (dgFloor (m_w))); + } + + DG_INLINE dgVector TestZero() const + { + const dgInt32* const a = (dgInt32*)&m_x; + return dgVector ((a[0] == 0) ? dgFloat32 (-1.0f) : dgFloat32 (1.0f), + (a[1] == 0) ? dgFloat32 (-1.0f) : dgFloat32 (1.0f), + (a[2] == 0) ? dgFloat32 (-1.0f) : dgFloat32 (1.0f), + (a[3] == 0) ? dgFloat32 (-1.0f) : dgFloat32 (1.0f)); + } + + + DG_INLINE dgVector Floor () const + { + return dgVector (dgFloor (m_x), dgFloor (m_y), dgFloor (m_z), dgFloor (m_w)); + } + + DG_INLINE dgVector DotProduct (const dgVector &A) const + { + return dgVector (m_x * A.m_x + m_y * A.m_y + m_z * A.m_z + m_w * A.m_w); + } + + DG_INLINE dgVector Reciproc () const + { + return dgVector (dgFloat32 (1.0f) / m_x, dgFloat32 (1.0f) / m_y, dgFloat32 (1.0f) / m_z, dgFloat32 (1.0f) / m_w); + } + + DG_INLINE dgVector Sqrt () const + { + return dgVector (dgSqrt (m_x), dgSqrt (m_y), dgSqrt (m_z), dgSqrt (m_w)); + } + + DG_INLINE dgVector InvSqrt () const + { + return dgVector (dgRsqrt (m_x), dgRsqrt (m_y), dgRsqrt (m_z), dgRsqrt (m_w)); + } + + DG_INLINE dgVector InvMagSqrt () const + { + return dgVector (dgRsqrt (DotProduct(*this).m_x)); + } + + DG_INLINE dgVector Normalize () const + { + dgAssert (m_w == dgFloat32 (0.0f)); + //return *this * dgVector (dgRsqrt (DotProduct(*this).m_x)); + //return Scale (dgRsqrt (DotProduct(*this).GetScalar())); + const dgVector& me = *this; + return me * InvMagSqrt(); + } + + dgVector Abs () const + { + return dgVector ((m_x > dgFloat32 (0.0f)) ? m_x : -m_x, + (m_y > dgFloat32 (0.0f)) ? m_y : -m_y, + (m_z > dgFloat32 (0.0f)) ? m_z : -m_z, + (m_w > dgFloat32 (0.0f)) ? m_w : -m_w); + } + + dgFloat32 GetMax () const + { + return dgMax(dgMax(m_x, m_y), dgMax(m_z, m_w)); + } + + dgVector GetMax (const dgVector& data) const + { + return dgVector ((m_x > data.m_x) ? m_x : data.m_x, + (m_y > data.m_y) ? m_y : data.m_y, + (m_z > data.m_z) ? m_z : data.m_z, + (m_w > data.m_w) ? m_w : data.m_w); + } + + dgVector GetMin (const dgVector& data) const + { + return dgVector ((m_x < data.m_x) ? m_x : data.m_x, + (m_y < data.m_y) ? m_y : data.m_y, + (m_z < data.m_z) ? m_z : data.m_z, + (m_w < data.m_w) ? m_w : data.m_w); + } + + + // relational operators + DG_INLINE dgVector operator== (const dgVector& data) const + { + return dgVector ((m_x == data.m_x) ? dgInt32 (0xffffffff) : 0, + (m_y == data.m_y) ? dgInt32 (0xffffffff) : 0, + (m_z == data.m_z) ? dgInt32 (0xffffffff) : 0, + (m_w == data.m_w) ? dgInt32 (0xffffffff) : 0); + } + + DG_INLINE dgVector operator> (const dgVector& data) const + { + return dgVector ((m_x > data.m_x) ? dgInt32 (0xffffffff) : 0, + (m_y > data.m_y) ? dgInt32 (0xffffffff) : 0, + (m_z > data.m_z) ? dgInt32 (0xffffffff) : 0, + (m_w > data.m_w) ? dgInt32 (0xffffffff) : 0); + } + + DG_INLINE dgVector operator< (const dgVector& data) const + { + return dgVector ((m_x < data.m_x) ? dgInt32 (0xffffffff) : 0, + (m_y < data.m_y) ? dgInt32 (0xffffffff) : 0, + (m_z < data.m_z) ? dgInt32 (0xffffffff) : 0, + (m_w < data.m_w) ? dgInt32 (0xffffffff) : 0); + } + + DG_INLINE dgVector operator>= (const dgVector& data) const + { + return dgVector ((m_x >= data.m_x) ? dgInt32 (0xffffffff) : 0, + (m_y >= data.m_y) ? dgInt32 (0xffffffff) : 0, + (m_z >= data.m_z) ? dgInt32 (0xffffffff) : 0, + (m_w >= data.m_w) ? dgInt32 (0xffffffff) : 0); + } + + DG_INLINE dgVector operator<= (const dgVector& data) const + { + return dgVector ((m_x <= data.m_x) ? dgInt32 (0xffffffff) : 0, + (m_y <= data.m_y) ? dgInt32 (0xffffffff) : 0, + (m_z <= data.m_z) ? dgInt32 (0xffffffff) : 0, + (m_w <= data.m_w) ? dgInt32 (0xffffffff) : 0); + } + + + // logical operations + DG_INLINE dgVector operator& (const dgVector& data) const + { + const dgInt32* const a = (dgInt32*)&m_x; + const dgInt32* const b = (dgInt32*)&data.m_x; + return dgVector (a[0] & b[0], a[1] & b[1], a[2] & b[2], a[3] & b[3]); + } + + DG_INLINE dgVector operator| (const dgVector& data) const + { + const dgInt32* const a = (dgInt32*)&m_x; + const dgInt32* const b = (dgInt32*)&data.m_x; + return dgVector (a[0] | b[0], a[1] | b[1], a[2] | b[2], a[3] | b[3]); + } + + DG_INLINE dgVector operator^ (const dgVector& data) const + { + const dgInt32* const a = (dgInt32*)&m_x; + const dgInt32* const b = (dgInt32*)&data.m_x; + return dgVector (a[0] ^ b[0], a[1] ^ b[1], a[2] ^ b[2], a[3] ^ b[3]); + } + + DG_INLINE dgVector AndNot (const dgVector& data) const + { + const dgInt32* const a = (dgInt32*)&m_x; + const dgInt32* const b = (dgInt32*)&data.m_x; + return dgVector (a[0] & ~b[0], a[1] & ~b[1], a[2] & ~b[2], a[3] & ~b[3]); + } + + DG_INLINE dgVector Select (const dgVector& data, const dgVector& mask) const + { + // (((b ^ a) & mask)^a) + return (*this) ^ (mask & (data ^ (*this))); + } + + DG_INLINE dgInt32 GetSignMask() const + { + const dgInt32* const a = (dgInt32*)&m_x; + return (((a[0] & 0x80000000) ? 1 : 0) | ((a[1] & 0x80000000) ? 2 : 0) | ((a[2] & 0x80000000) ? 4 : 0) | ((a[3] & 0x80000000) ? 8 : 0)); + } + + DG_INLINE dgVector ShiftRight() const + { + return dgVector (m_w, m_x, m_y, m_z); + } + + DG_INLINE dgVector ShiftTripleRight () const + { + return dgVector (m_z, m_x, m_y, m_w); + } + + DG_INLINE dgVector ShiftTripleLeft () const + { + return dgVector (m_y, m_z, m_x, m_w); + } + + DG_INLINE dgVector ShiftRightLogical (int bits) const + { + return dgVector (dgInt32 (dgUnsigned32 (m_ix) >> bits), dgInt32 (dgUnsigned32 (m_iy) >> bits), dgInt32 (dgUnsigned32 (m_iz) >> bits), dgInt32 (dgUnsigned32 (m_iw) >> bits)); + } + + DG_INLINE static void Transpose4x4 (dgVector& dst0, dgVector& dst1, dgVector& dst2, dgVector& dst3, const dgVector& src0, const dgVector& src1, const dgVector& src2, const dgVector& src3) + { + dgVector tmp0 (src0); + dgVector tmp1 (src1); + dgVector tmp2 (src2); + dgVector tmp3 (src3); + + dst0 = dgVector (tmp0.m_x, tmp1.m_x, tmp2.m_x, tmp3.m_x); + dst1 = dgVector (tmp0.m_y, tmp1.m_y, tmp2.m_y, tmp3.m_y); + dst2 = dgVector (tmp0.m_z, tmp1.m_z, tmp2.m_z, tmp3.m_z); + dst3 = dgVector (tmp0.m_w, tmp1.m_w, tmp2.m_w, tmp3.m_w); + } + + DG_CLASS_ALLOCATOR(allocator) + + union { + dgInt32 m_i[4]; + struct { + dgFloat32 m_x; + dgFloat32 m_y; + dgFloat32 m_z; + dgFloat32 m_w; + }; + struct { + dgInt32 m_ix; + dgInt32 m_iy; + dgInt32 m_iz; + dgInt32 m_iw; + }; + }; + + static dgVector m_zero; + static dgVector m_one; + static dgVector m_wOne; + static dgVector m_half; + static dgVector m_two; + static dgVector m_three; + static dgVector m_negOne; + static dgVector m_xMask; + static dgVector m_yMask; + static dgVector m_zMask; + static dgVector m_wMask; + static dgVector m_epsilon; + static dgVector m_signMask; + static dgVector m_triplexMask; +} DG_GCC_VECTOR_ALIGNMENT; + +#endif + +DG_MSC_VECTOR_ALIGNMENT +class dgBigVector +{ + public: + DG_INLINE dgBigVector() + { + } + + DG_INLINE dgBigVector(dgFloat64 val) + :m_x(val), m_y(val), m_z(val), m_w(val) + { + } + + DG_INLINE dgBigVector (const dgBigVector& v) + :m_x(v.m_x), m_y(v.m_y), m_z(v.m_z), m_w(v.m_w) + { + } + +#ifndef _NEWTON_USE_DOUBLE + DG_INLINE dgBigVector (const dgVector& v) + :m_x(v.m_x), m_y(v.m_y), m_z(v.m_z), m_w(v.m_w) + { + } + + DG_INLINE dgBigVector (const dgFloat32* const ptr) + :m_x(ptr[0]), m_y(ptr[1]), m_z(ptr[2]), m_w (dgFloat32 (0.0f)) + { + dgAssert (dgCheckVector ((*this))); + } +#endif + + DG_INLINE dgBigVector (const dgFloat64* const ptr) + :m_x(ptr[0]), m_y(ptr[1]), m_z(ptr[2]), m_w (ptr[3]) + { + dgAssert (dgCheckVector ((*this))); + } + + DG_INLINE dgBigVector (dgFloat64 x, dgFloat64 y, dgFloat64 z, dgFloat64 w) + :m_x(x), m_y(y), m_z(z), m_w(w) + { + dgAssert (dgCheckVector ((*this))); + } + + DG_INLINE dgBigVector (dgInt32 ix, dgInt32 iy, dgInt32 iz, dgInt32 iw) + :m_ix(ix), m_iy(iy), m_iz(iz), m_iw(iw) + { + } + + DG_INLINE dgBigVector (dgInt64 ix, dgInt64 iy, dgInt64 iz, dgInt64 iw) + :m_ix(ix), m_iy(iy), m_iz(iz), m_iw(iw) + { + } + + DG_INLINE dgFloat64 GetScalar () const + { + return m_x; + } + + DG_INLINE void Store (dgFloat64* const dst) const + { + dst[0] = m_x; + dst[1] = m_y; + dst[2] = m_z; + dst[3] = m_w; + } + + DG_INLINE dgBigVector BroadcastX () const + { + return dgBigVector (m_x); + } + + DG_INLINE dgBigVector BroadcastY () const + { + return dgBigVector (m_y); + } + + DG_INLINE dgBigVector BroadcastZ () const + { + return dgBigVector (m_z); + } + + DG_INLINE dgBigVector BroadcastW () const + { + return dgBigVector (m_w); + } + + + DG_INLINE dgFloat64& operator[] (dgInt32 i) + { + dgAssert (i < 4); + dgAssert (i >= 0); + return (&m_x)[i]; + } + + DG_INLINE const dgFloat64& operator[] (dgInt32 i) const + { + dgAssert (i < 4); + dgAssert (i >= 0); + return (&m_x)[i]; + } + + DG_INLINE dgBigVector operator+ (const dgBigVector& A) const + { + return dgBigVector (m_x + A.m_x, m_y + A.m_y, m_z + A.m_z, m_w + A.m_w); + } + + DG_INLINE dgBigVector operator- (const dgBigVector& A) const + { + return dgBigVector (m_x - A.m_x, m_y - A.m_y, m_z - A.m_z, m_w - A.m_w); + } + + DG_INLINE dgBigVector operator* (const dgBigVector& A) const + { + return dgBigVector(m_x * A.m_x, m_y * A.m_y, m_z * A.m_z, m_w * A.m_w); + } + + DG_INLINE dgBigVector& operator+= (const dgBigVector& A) + { + return (*this = dgBigVector (m_x + A.m_x, m_y + A.m_y, m_z + A.m_z, m_w + A.m_w)); + } + + DG_INLINE dgBigVector& operator-= (const dgBigVector& A) + { + return (*this = dgBigVector (m_x - A.m_x, m_y - A.m_y, m_z - A.m_z, m_w - A.m_w)); + } + + DG_INLINE dgBigVector& operator*= (const dgBigVector& A) + { + return (*this = dgBigVector(m_x * A.m_x, m_y * A.m_y, m_z * A.m_z, m_w * A.m_w)); + } + + DG_INLINE dgBigVector MulAdd(const dgBigVector& A, const dgBigVector& B) const + { + return *this + A * B; + } + + DG_INLINE dgBigVector MulSub(const dgVector& A, const dgBigVector& B) const + { + return *this - A * B; + } + + + DG_INLINE dgBigVector AddHorizontal () const + { + return dgBigVector (m_x + m_y + m_z + m_w); + } + + DG_INLINE dgBigVector Scale (dgFloat64 scale) const + { + return dgBigVector (m_x * scale, m_y * scale, m_z * scale, m_w * scale); + } + + // return dot product + DG_INLINE dgFloat64 DotProduct3 (const dgBigVector& A) const + { + return m_x * A.m_x + m_y * A.m_y + m_z * A.m_z; + } + + // return cross product + DG_INLINE dgBigVector CrossProduct (const dgBigVector& B) const + { + return dgBigVector (m_y * B.m_z - m_z * B.m_y, m_z * B.m_x - m_x * B.m_z, m_x * B.m_y - m_y * B.m_x, m_w); + } + + DG_INLINE dgBigVector CrossProduct (const dgBigVector& A, const dgBigVector& B) const + { + dgFloat64 cofactor[3][3]; + dgFloat64 array[4][4]; + + const dgBigVector& me = *this; + for (dgInt32 i = 0; i < 4; i ++) { + array[0][i] = me[i]; + array[1][i] = A[i]; + array[2][i] = B[i]; + array[3][i] = dgFloat32 (1.0f); + } + + dgBigVector normal; + dgFloat64 sign = dgFloat64 (-1.0f); + for (dgInt32 i = 0; i < 4; i ++) { + + for (dgInt32 j = 0; j < 3; j ++) { + dgInt32 k0 = 0; + for (dgInt32 k = 0; k < 4; k ++) { + if (k != i) { + cofactor[j][k0] = array[j][k]; + k0 ++; + } + } + } + dgFloat64 x = cofactor[0][0] * (cofactor[1][1] * cofactor[2][2] - cofactor[1][2] * cofactor[2][1]); + dgFloat64 y = cofactor[0][1] * (cofactor[1][2] * cofactor[2][0] - cofactor[1][0] * cofactor[2][2]); + dgFloat64 z = cofactor[0][2] * (cofactor[1][0] * cofactor[2][1] - cofactor[1][1] * cofactor[2][0]); + dgFloat64 det = x + y + z; + + normal[i] = sign * det; + sign *= dgFloat64 (-1.0f); + } + + return normal; + } + + DG_INLINE dgBigVector GetInt () const + { + return dgBigVector (dgInt64 (floor (m_x)), dgInt64(floor (m_y)), dgInt64(floor (m_z)), dgInt64 (floor (m_w))); + } + + DG_INLINE dgBigVector TestZero() const + { + const dgInt64* const a = (dgInt64*)&m_x; + return dgBigVector ((a[0] == 0) ? dgFloat64 (-1.0f) : dgFloat64 (1.0f), + (a[1] == 0) ? dgFloat64 (-1.0f) : dgFloat64 (1.0f), + (a[2] == 0) ? dgFloat64 (-1.0f) : dgFloat64 (1.0f), + (a[3] == 0) ? dgFloat64 (-1.0f) : dgFloat64 (1.0f)); + } + + + DG_INLINE dgBigVector Floor () const + { + return dgBigVector (floor (m_x), floor (m_y), floor (m_z), floor (m_w)); + } + + DG_INLINE dgBigVector DotProduct (const dgBigVector &A) const + { + return dgBigVector (m_x * A.m_x + m_y * A.m_y + m_z * A.m_z + m_w * A.m_w); + } + + DG_INLINE dgBigVector Reciproc () const + { + return dgBigVector (dgFloat64 (1.0f) / m_x, dgFloat64 (1.0f) / m_y, dgFloat64 (1.0f) / m_z, dgFloat64 (1.0f) / m_w); + } + + DG_INLINE dgBigVector Sqrt () const + { + return dgBigVector (sqrt (m_x), sqrt (m_y), sqrt (m_z), sqrt (m_w)); + } + + DG_INLINE dgBigVector InvSqrt () const + { + return dgBigVector (dgFloat64 (1.0f) / sqrt (m_x), dgFloat64 (1.0f) / sqrt (m_y), dgFloat64 (1.0f) / sqrt (m_z), dgFloat64 (1.0f) / sqrt (m_w)); + } + + DG_INLINE dgBigVector InvMagSqrt () const + { + return dgBigVector (dgFloat64 (1.0f) / sqrt (DotProduct(*this).m_x)); + } + + DG_INLINE dgBigVector Normalize() const + { + dgAssert (m_w == dgFloat64 (0.0f)); + //const dgBigVector& me = *this; + //return *this * dgBigVector (dgRsqrt(DotProduct(*this).m_x)); + return *this * InvMagSqrt(); + } + + dgBigVector Abs () const + { + return dgBigVector ((m_x > dgFloat64 (0.0f)) ? m_x : -m_x, + (m_y > dgFloat64 (0.0f)) ? m_y : -m_y, + (m_z > dgFloat64 (0.0f)) ? m_z : -m_z, + (m_w > dgFloat64 (0.0f)) ? m_w : -m_w); + } + + dgFloat64 GetMax () const + { + return dgMax(dgMax(m_x, m_y), dgMax(m_z, m_w)); + } + + dgBigVector GetMax (const dgBigVector& data) const + { + return dgBigVector ((m_x > data.m_x) ? m_x : data.m_x, + (m_y > data.m_y) ? m_y : data.m_y, + (m_z > data.m_z) ? m_z : data.m_z, + (m_w > data.m_w) ? m_w : data.m_w); + } + + dgBigVector GetMin (const dgBigVector& data) const + { + return dgBigVector ((m_x < data.m_x) ? m_x : data.m_x, + (m_y < data.m_y) ? m_y : data.m_y, + (m_z < data.m_z) ? m_z : data.m_z, + (m_w < data.m_w) ? m_w : data.m_w); + } + + // relational operators + DG_INLINE dgBigVector operator== (const dgBigVector& data) const + { + return dgBigVector ((m_x == data.m_x) ? dgInt64 (-1) : dgInt64 (0), + (m_y == data.m_y) ? dgInt64 (-1) : dgInt64 (0), + (m_z == data.m_z) ? dgInt64 (-1) : dgInt64 (0), + (m_w == data.m_w) ? dgInt64 (-1) : dgInt64 (0)); + } + + DG_INLINE dgBigVector operator> (const dgBigVector& data) const + { + return dgBigVector ((m_x > data.m_x) ? dgInt64 (-1) : dgInt64 (0), + (m_y > data.m_y) ? dgInt64 (-1) : dgInt64 (0), + (m_z > data.m_z) ? dgInt64 (-1) : dgInt64 (0), + (m_w > data.m_w) ? dgInt64 (-1) : dgInt64 (0)); + } + + DG_INLINE dgBigVector operator< (const dgBigVector& data) const + { + return dgBigVector ((m_x < data.m_x) ? dgInt64 (-1) : dgInt64 (0), + (m_y < data.m_y) ? dgInt64 (-1) : dgInt64 (0), + (m_z < data.m_z) ? dgInt64 (-1) : dgInt64 (0), + (m_w < data.m_w) ? dgInt64 (-1) : dgInt64 (0)); + } + + DG_INLINE dgBigVector operator>= (const dgBigVector& data) const + { + return dgBigVector ((m_x >= data.m_x) ? dgInt64 (-1) : dgInt64 (0), + (m_y >= data.m_y) ? dgInt64 (-1) : dgInt64 (0), + (m_z >= data.m_z) ? dgInt64 (-1) : dgInt64 (0), + (m_w >= data.m_w) ? dgInt64 (-1) : dgInt64 (0)); + } + + DG_INLINE dgBigVector operator<= (const dgBigVector& data) const + { + return dgBigVector ((m_x <= data.m_x) ? dgInt64 (-1) : dgInt64 (0), + (m_y <= data.m_y) ? dgInt64 (-1) : dgInt64 (0), + (m_z <= data.m_z) ? dgInt64 (-1) : dgInt64 (0), + (m_w <= data.m_w) ? dgInt64 (-1) : dgInt64 (0)); + } + + + // logical operations + DG_INLINE dgBigVector operator& (const dgBigVector& data) const + { + const dgInt64* const a = (dgInt64*)&m_x; + const dgInt64* const b = (dgInt64*)&data.m_x; + return dgBigVector (a[0] & b[0], a[1] & b[1], a[2] & b[2], a[3] & b[3]); + } + + DG_INLINE dgBigVector operator| (const dgBigVector& data) const + { + const dgInt64* const a = (dgInt64*)&m_x; + const dgInt64* const b = (dgInt64*)&data.m_x; + return dgBigVector (a[0] | b[0], a[1] | b[1], a[2] | b[2], a[3] | b[3]); + } + + DG_INLINE dgBigVector operator^ (const dgBigVector& data) const + { + const dgInt64* const a = (dgInt64*)&m_x; + const dgInt64* const b = (dgInt64*)&data.m_x; + return dgBigVector (a[0] ^ b[0], a[1] ^ b[1], a[2] ^ b[2], a[3] ^ b[3]); + } + + DG_INLINE dgBigVector AndNot (const dgBigVector& data) const + { + const dgInt64* const a = (dgInt64*)&m_x; + const dgInt64* const b = (dgInt64*)&data.m_x; + return dgBigVector (a[0] & ~b[0], a[1] & ~b[1], a[2] & ~b[2], a[3] & ~b[3]); + } + + DG_INLINE dgBigVector Select(const dgBigVector& data, const dgBigVector& mask) const + { + // (((b ^ a) & mask)^a) + return (*this) ^ (mask & (data ^ (*this))); + } + + DG_INLINE dgInt32 GetSignMask() const + { + const dgInt64* const a = (dgInt64*)&m_x; + return (((a[0]>>63) ? 1 : 0) | ((a[1]>>63) ? 2 : 0) | ((a[2]>>63) ? 4 : 0) | ((a[3]>>63) ? 8 : 0)); + } + + DG_INLINE dgVector ShiftRight() const + { + return dgBigVector (m_w, m_x, m_y, m_z); + } + + DG_INLINE dgBigVector ShiftTripleRight () const + { + return dgBigVector (m_z, m_x, m_y, m_w); + } + + DG_INLINE dgBigVector ShiftTripleLeft () const + { + return dgBigVector (m_y, m_z, m_x, m_w); + } + + DG_INLINE dgBigVector ShiftRightLogical (int bits) const + { + return dgBigVector (dgInt64 (dgUnsigned64 (m_ix) >> bits), dgInt64 (dgUnsigned64 (m_iy) >> bits), dgInt64 (dgUnsigned64 (m_iz) >> bits), dgInt64 (dgUnsigned64 (m_iw) >> bits)); + } + + DG_INLINE static void Transpose4x4 (dgBigVector& dst0, dgBigVector& dst1, dgBigVector& dst2, dgBigVector& dst3, const dgBigVector& src0, const dgBigVector& src1, const dgBigVector& src2, const dgBigVector& src3) + { + dgBigVector tmp0 (src0); + dgBigVector tmp1 (src1); + dgBigVector tmp2 (src2); + dgBigVector tmp3 (src3); + + dst0 = dgBigVector (tmp0.m_x, tmp1.m_x, tmp2.m_x, tmp3.m_x); + dst1 = dgBigVector (tmp0.m_y, tmp1.m_y, tmp2.m_y, tmp3.m_y); + dst2 = dgBigVector (tmp0.m_z, tmp1.m_z, tmp2.m_z, tmp3.m_z); + dst3 = dgBigVector (tmp0.m_w, tmp1.m_w, tmp2.m_w, tmp3.m_w); + } + + DG_CLASS_ALLOCATOR(allocator) + + union + { + dgInt64 m_i[4]; + struct + { + dgFloat64 m_x; + dgFloat64 m_y; + dgFloat64 m_z; + dgFloat64 m_w; + }; + struct + { + dgInt64 m_ix; + dgInt64 m_iy; + dgInt64 m_iz; + dgInt64 m_iw; + }; + }; + + static dgBigVector m_zero; + static dgBigVector m_one; + static dgBigVector m_wOne; + static dgBigVector m_half; + static dgBigVector m_two; + static dgBigVector m_three; + static dgBigVector m_negOne; + static dgBigVector m_xMask; + static dgBigVector m_yMask; + static dgBigVector m_zMask; + static dgBigVector m_wMask; + static dgBigVector m_epsilon; + static dgBigVector m_signMask; + static dgBigVector m_triplexMask; +} DG_GCC_VECTOR_ALIGNMENT; + + +DG_MSC_VECTOR_ALIGNMENT +class dgSpatialVector +{ + public: + DG_INLINE dgSpatialVector() + { + } + + DG_INLINE dgSpatialVector(const dgFloat32 a) + { + for (dgInt32 i = 0; i < 6; i ++) { + m_d[i] = a; + } + } + + DG_INLINE dgSpatialVector(const dgVector& low, const dgVector& high) + { + for (dgInt32 i = 0; i < 3; i ++) { + m_d[i] = low[i]; + m_d[i + 3] = high[i]; + } + } + + DG_INLINE dgSpatialVector(const dgSpatialVector& src) + { + for (dgInt32 i = 0; i < 6; i++) { + m_d[i] = src[i]; + } + } + + DG_INLINE dgFloat64& operator[] (dgInt32 i) + { + dgAssert(i < 6); + dgAssert(i >= 0); + return m_d[i]; + } + + DG_INLINE const dgFloat64& operator[] (dgInt32 i) const + { + dgAssert(i < 6); + dgAssert(i >= 0); + return m_d[i]; + } + + DG_INLINE dgSpatialVector operator+ (const dgSpatialVector& A) const + { + dgSpatialVector tmp; + for (dgInt32 i = 0; i < 6; i++) { + tmp[i] = m_d[i] + A.m_d[i]; + } + return tmp; + } + + DG_INLINE dgSpatialVector operator* (const dgSpatialVector& A) const + { + dgSpatialVector tmp; + for (dgInt32 i = 0; i < 6; i++) { + tmp[i] = m_d[i] * A.m_d[i]; + } + return tmp; + } + + DG_INLINE dgFloat64 DotProduct(const dgSpatialVector& v) const + { + dgFloat64 ret = dgFloat64 (0.0f); + for (dgInt32 i = 0; i < 6; i++) { + ret += m_d[i] * v.m_d[i]; + } + return ret; + } + + DG_INLINE dgSpatialVector Scale(dgFloat64 s) const + { + dgSpatialVector tmp; + for (dgInt32 i = 0; i < 6; i++) { + tmp[i] = m_d[i] * s; + } + return tmp; + } + + dgFloat64 m_d[6]; + static dgSpatialVector m_zero; +} DG_GCC_VECTOR_ALIGNMENT; + + +#endif diff --git a/thirdparty/src/newton/dgCore/dgVectorSimd.h b/thirdparty/src/newton/dgCore/dgVectorSimd.h new file mode 100644 index 000000000..274720631 --- /dev/null +++ b/thirdparty/src/newton/dgCore/dgVectorSimd.h @@ -0,0 +1,983 @@ +/* Copyright (c) <2003-2019> +* +* 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 __dgVectorSimd__ +#define __dgVectorSimd__ + +#ifndef DG_SCALAR_VECTOR_CLASS + +#ifdef _NEWTON_USE_DOUBLE + #define dgVector dgBigVector +#else + +class dgBigVector; +// ***************************************************************************************** +// +// 4 x 1 single precision SSE vector class declaration +// +// ***************************************************************************************** +DG_MSC_VECTOR_ALIGNMENT +class dgVector +{ + #define PERMUTE_MASK(w, z, y, x) _MM_SHUFFLE (w, z, y, x) + public: + DG_INLINE dgVector() + { + } + + DG_INLINE dgVector(const __m128 type) + :m_type (type) + { + } + + DG_INLINE dgVector(const __m128i type) + :m_typeInt (type) + { + } + + DG_INLINE dgVector (const dgFloat32 a) + :m_type(_mm_set_ps1(a)) + { + } + + DG_INLINE dgVector (const dgFloat32* const ptr) + :m_type(_mm_loadu_ps (ptr)) + { + } + +#ifndef _NEWTON_USE_DOUBLE + DG_INLINE dgVector(const dgFloat64* const ptr) + :m_type(_mm_set_ps(dgFloat32(ptr[3]), dgFloat32(ptr[2]), dgFloat32(ptr[1]), dgFloat32(ptr[0]))) + { + } +#endif + + DG_INLINE dgVector (const dgVector& copy) + :m_type(copy.m_type) + { + } + + DG_INLINE dgVector (const dgBigVector& copy) + :m_type(_mm_shuffle_ps (_mm_cvtpd_ps (((__m128d*)©)[0]), _mm_cvtpd_ps (((__m128d*)©)[1]), PERMUTE_MASK(1, 0, 1, 0))) + { + dgAssert (dgCheckVector ((*this))); + } + + DG_INLINE dgVector (dgFloat32 x, dgFloat32 y, dgFloat32 z, dgFloat32 w) + :m_type(_mm_set_ps(w, z, y, x)) + { + } + + DG_INLINE dgVector (dgInt32 ix, dgInt32 iy, dgInt32 iz, dgInt32 iw) + :m_type(_mm_set_ps(*(dgFloat32*)&iw, *(dgFloat32*)&iz, *(dgFloat32*)&iy, *(dgFloat32*)&ix)) + { + } + + DG_INLINE dgFloat32 GetScalar () const + { + //return m_x; + return _mm_cvtss_f32 (m_type); + } + + DG_INLINE void Store (dgFloat32* const dst) const + { + _mm_storeu_ps(dst, m_type); + } + + DG_INLINE dgVector BroadcastX () const + { + return _mm_shuffle_ps (m_type, m_type, PERMUTE_MASK(0, 0, 0, 0)); + } + + DG_INLINE dgVector BroadcastY () const + { + return _mm_shuffle_ps (m_type, m_type, PERMUTE_MASK(1, 1, 1, 1)); + } + + DG_INLINE dgVector BroadcastZ () const + { + return _mm_shuffle_ps (m_type, m_type, PERMUTE_MASK(2, 2, 2, 2)); + } + + DG_INLINE dgVector BroadcastW () const + { + return _mm_shuffle_ps (m_type, m_type, PERMUTE_MASK(3, 3, 3, 3)); + } + + DG_INLINE dgVector Scale (dgFloat32 s) const + { + return _mm_mul_ps (m_type, _mm_set_ps1(s)); + } + + DG_INLINE dgFloat32& operator[] (dgInt32 i) + { + dgAssert (i < 4); + dgAssert (i >= 0); + return m_f[i]; + } + + DG_INLINE const dgFloat32& operator[] (dgInt32 i) const + { + dgAssert (i < 4); + dgAssert (i >= 0); + return m_f[i]; + } + + DG_INLINE dgVector operator+ (const dgVector& A) const + { + return _mm_add_ps (m_type, A.m_type); + } + + DG_INLINE dgVector operator- (const dgVector& A) const + { + return _mm_sub_ps (m_type, A.m_type); + } + + DG_INLINE dgVector operator* (const dgVector& A) const + { + return _mm_mul_ps(m_type, A.m_type); + } + + DG_INLINE dgVector& operator+= (const dgVector& A) + { + return (*this = _mm_add_ps (m_type, A.m_type)); + } + + DG_INLINE dgVector& operator-= (const dgVector& A) + { + return (*this = _mm_sub_ps (m_type, A.m_type)); + } + + DG_INLINE dgVector& operator*= (const dgVector& A) + { + return (*this = _mm_mul_ps(m_type, A.m_type)); + } + + // return cross product + DG_INLINE dgVector CrossProduct (const dgVector& B) const + { + return _mm_sub_ps (_mm_mul_ps (_mm_shuffle_ps (m_type, m_type, PERMUTE_MASK(3, 0, 2, 1)), _mm_shuffle_ps (B.m_type, B.m_type, PERMUTE_MASK(3, 1, 0, 2))), + _mm_mul_ps (_mm_shuffle_ps (m_type, m_type, PERMUTE_MASK(3, 1, 0, 2)), _mm_shuffle_ps (B.m_type, B.m_type, PERMUTE_MASK(3, 0, 2, 1)))); + } + + DG_INLINE dgVector DotProduct(const dgVector& A) const + { + return (*this * A).AddHorizontal(); + } + + DG_INLINE dgVector CrossProduct (const dgVector& A, const dgVector& B) const + { + dgFloat32 cofactor[3][3]; + dgFloat32 array[4][4]; + + const dgVector& me = *this; + for (dgInt32 i = 0; i < 4; i ++) { + array[0][i] = me[i]; + array[1][i] = A[i]; + array[2][i] = B[i]; + array[3][i] = dgFloat32 (1.0f); + } + + dgVector normal; + dgFloat32 sign = dgFloat32 (-1.0f); + for (dgInt32 i = 0; i < 4; i ++) { + + for (dgInt32 j = 0; j < 3; j ++) { + dgInt32 k0 = 0; + for (dgInt32 k = 0; k < 4; k ++) { + if (k != i) { + cofactor[j][k0] = array[j][k]; + k0 ++; + } + } + } + dgFloat32 x = cofactor[0][0] * (cofactor[1][1] * cofactor[2][2] - cofactor[1][2] * cofactor[2][1]); + dgFloat32 y = cofactor[0][1] * (cofactor[1][2] * cofactor[2][0] - cofactor[1][0] * cofactor[2][2]); + dgFloat32 z = cofactor[0][2] * (cofactor[1][0] * cofactor[2][1] - cofactor[1][1] * cofactor[2][0]); + dgFloat32 det = x + y + z; + + normal[i] = sign * det; + sign *= dgFloat32 (-1.0f); + } + + return normal; + } + + DG_INLINE dgVector Reciproc () const + { + return _mm_div_ps (m_one.m_type, m_type); + } + + DG_INLINE dgVector MulAdd(const dgVector& A, const dgVector& B) const + { + return _mm_add_ps(m_type, _mm_mul_ps(A.m_type, B.m_type)); + } + + DG_INLINE dgVector MulSub(const dgVector& A, const dgVector& B) const + { + return _mm_sub_ps(m_type, _mm_mul_ps(A.m_type, B.m_type)); + } + + DG_INLINE dgVector AddHorizontal () const + { + __m128 tmp (_mm_hadd_ps (m_type, m_type)); + return _mm_hadd_ps (tmp, tmp); + } + + DG_INLINE dgVector Abs () const + { + return _mm_and_ps (m_type, m_signMask.m_type); + } + + dgFloat32 GetMax () const + { + __m128 tmp (_mm_max_ps (m_type, _mm_shuffle_ps (m_type, m_type, PERMUTE_MASK(3, 2, 3, 2)))); + //return dgVector (_mm_max_ps (tmp, _mm_shuffle_ps (tmp, tmp, PERMUTE_MASK(3, 2, 0, 1)))).GetScalar(); + return _mm_cvtss_f32(_mm_max_ss (tmp, _mm_shuffle_ps(tmp, tmp, PERMUTE_MASK(3, 2, 0, 1)))); + } + + dgVector GetMax (const dgVector& data) const + { + return _mm_max_ps (m_type, data.m_type); + } + + dgVector GetMin (const dgVector& data) const + { + return _mm_min_ps (m_type, data.m_type); + } + + DG_INLINE dgVector GetInt () const + { + return dgVector(_mm_cvtps_epi32(Floor().m_type)); + } + + DG_INLINE dgVector TestZero() const + { + //return dgVector (_mm_cmpeq_epi32 (m_typeInt, m_zero.m_typeInt)) & m_negOne; + return m_negOne & (*this == m_zero); + } + + DG_INLINE dgVector Floor () const + { + dgVector truncated (_mm_cvtepi32_ps (_mm_cvttps_epi32 (m_type))); + dgVector ret (truncated - (dgVector::m_one & (*this < truncated))); + dgAssert (ret.m_f[0] == dgFloor(m_f[0])); + dgAssert (ret.m_f[1] == dgFloor(m_f[1])); + dgAssert (ret.m_f[2] == dgFloor(m_f[2])); + dgAssert (ret.m_f[3] == dgFloor(m_f[3])); + return ret; + } + + DG_INLINE dgVector Sqrt () const + { + return _mm_sqrt_ps(m_type); + } + + DG_INLINE dgVector InvSqrt () const + { + dgVector tmp0 (_mm_rsqrt_ps(m_type)); + return m_half * tmp0 * (m_three - *this * tmp0 * tmp0); + } + + DG_INLINE dgVector InvMagSqrt () const + { + return DotProduct(*this).InvSqrt(); + } + + DG_INLINE dgVector Normalize () const + { + dgAssert (m_w == dgFloat32 (0.0f)); + // somehow this function changes the behavior with 3.13 + //return Scale(dgFloat32 (1.0f) / dgSqrt(DotProduct(*this).GetScalar())); + return *this * InvMagSqrt (); + } + + // relational operators + DG_INLINE dgVector operator> (const dgVector& data) const + { + return _mm_cmpgt_ps (m_type, data.m_type); + } + + DG_INLINE dgVector operator== (const dgVector& data) const + { + return _mm_cmpeq_ps (m_type, data.m_type); + } + + DG_INLINE dgVector operator< (const dgVector& data) const + { + return _mm_cmplt_ps (m_type, data.m_type); + } + + DG_INLINE dgVector operator>= (const dgVector& data) const + { + return _mm_cmpge_ps (m_type, data.m_type); + } + + DG_INLINE dgVector operator<= (const dgVector& data) const + { + return _mm_cmple_ps (m_type, data.m_type); + } + + // logical operations + DG_INLINE dgVector operator& (const dgVector& data) const + { + return _mm_and_ps (m_type, data.m_type); + } + + DG_INLINE dgVector operator| (const dgVector& data) const + { + return _mm_or_ps (m_type, data.m_type); + } + + DG_INLINE dgVector operator^ (const dgVector& data) const + { + return _mm_xor_ps (m_type, data.m_type); + } + + DG_INLINE dgVector AndNot(const dgVector& data) const + { + return _mm_andnot_ps(data.m_type, m_type); + } + + DG_INLINE dgVector Select(const dgVector& data, const dgVector& mask) const + { + // (((b ^ a) & mask)^a) + //return _mm_or_ps (_mm_and_ps (mask.m_type, data.m_type), _mm_andnot_ps(mask.m_type, m_type)); + return _mm_xor_ps(m_type, _mm_and_ps (mask.m_type, _mm_xor_ps(m_type, data.m_type))); + } + + DG_INLINE dgInt32 GetSignMask() const + { + return _mm_movemask_ps(m_type); + } + + DG_INLINE dgVector ShiftRight() const + { + return _mm_shuffle_ps(m_type, m_type, PERMUTE_MASK(2, 1, 0, 3)); + } + + DG_INLINE dgVector ShiftTripleRight () const + { + return _mm_shuffle_ps(m_type, m_type, PERMUTE_MASK(3, 1, 0, 2)); + } + + DG_INLINE dgVector ShiftTripleLeft () const + { + return _mm_shuffle_ps (m_type, m_type, PERMUTE_MASK(3, 0, 2, 1)); + } + + DG_INLINE dgVector ShiftRightLogical (int bits) const + { + return dgVector (_mm_srli_epi32(m_typeInt, bits)); + } + + DG_INLINE static void Transpose4x4 (dgVector& dst0, dgVector& dst1, dgVector& dst2, dgVector& dst3, const dgVector& src0, const dgVector& src1, const dgVector& src2, const dgVector& src3) + { + __m128 tmp0 (_mm_unpacklo_ps (src0.m_type, src1.m_type)); + __m128 tmp1 (_mm_unpacklo_ps (src2.m_type, src3.m_type)); + __m128 tmp2 (_mm_unpackhi_ps (src0.m_type, src1.m_type)); + __m128 tmp3 (_mm_unpackhi_ps (src2.m_type, src3.m_type)); + + dst0 = dgVector (_mm_movelh_ps (tmp0, tmp1)); + dst1 = dgVector (_mm_movehl_ps (tmp1, tmp0)); + dst2 = dgVector (_mm_movelh_ps (tmp2, tmp3)); + dst3 = dgVector (_mm_movehl_ps (tmp3, tmp2)); + } + +#ifdef _DEBUG + DG_INLINE void Trace(char* const name) const + { + dgTrace(("%s %f %f %f %f\n", name, m_x, m_y, m_z, m_w)); + } +#else + DG_INLINE void Trace(char* const name) const {} +#endif + + DG_CLASS_ALLOCATOR(allocator) + + union { + dgFloat32 m_f[4]; + dgInt32 m_i[4]; + __m128 m_type; + __m128i m_typeInt; + struct { + dgFloat32 m_x; + dgFloat32 m_y; + dgFloat32 m_z; + dgFloat32 m_w; + }; + struct { + dgInt32 m_ix; + dgInt32 m_iy; + dgInt32 m_iz; + dgInt32 m_iw; + }; + }; + + static dgVector m_zero; + static dgVector m_one; + static dgVector m_wOne; + static dgVector m_two; + static dgVector m_half; + static dgVector m_three; + static dgVector m_negOne; + static dgVector m_xMask; + static dgVector m_yMask; + static dgVector m_zMask; + static dgVector m_wMask; + static dgVector m_epsilon; + static dgVector m_signMask; + static dgVector m_triplexMask; +} DG_GCC_VECTOR_ALIGNMENT; +#endif + + +// ***************************************************************************************** +// +// 4 x 1 double precision SSE2 vector class declaration +// +// ***************************************************************************************** +DG_MSC_VECTOR_ALIGNMENT +class dgBigVector +{ + #define PERMUT_MASK_DOUBLE(y, x) _MM_SHUFFLE2 (y, x) + + public: + DG_INLINE dgBigVector() + { + } + + DG_INLINE dgBigVector(const dgBigVector& copy) + :m_typeLow(copy.m_typeLow) + ,m_typeHigh(copy.m_typeHigh) + { + } + + DG_INLINE dgBigVector(const __m128d typeLow, const __m128d typeHigh) + :m_typeLow(typeLow) + ,m_typeHigh(typeHigh) + { + } + + DG_INLINE dgBigVector(const __m128i typeLow, const __m128i typeHigh) + :m_typeIntLow(typeLow) + ,m_typeIntHigh(typeHigh) + { + } + + DG_INLINE dgBigVector(const dgFloat64 a) + :m_typeLow(_mm_set1_pd(a)) + ,m_typeHigh(_mm_set1_pd(a)) + { + } + +#ifdef _NEWTON_USE_DOUBLE + DG_INLINE dgBigVector (const dgFloat32* const ptr) + :m_typeLow(_mm_loadu_pd(ptr)) + ,m_typeHigh(_mm_loadu_pd(&ptr[2])) + { + } +#else + + DG_INLINE dgBigVector(const dgVector& v) + :m_typeLow(_mm_cvtps_pd (v.m_type)) + ,m_typeHigh(_mm_cvtps_pd (_mm_shuffle_ps (v.m_type, v.m_type, PERMUTE_MASK(3, 2, 3, 2)))) + { + dgAssert(dgCheckVector((*this))); + } + + DG_INLINE dgBigVector(const dgFloat64* const ptr) + :m_typeLow(_mm_loadu_pd(ptr)) + ,m_typeHigh(_mm_loadu_pd(&ptr[2])) + { + } +#endif + + + DG_INLINE dgBigVector(dgFloat64 x, dgFloat64 y, dgFloat64 z, dgFloat64 w) + :m_typeLow(_mm_set_pd(y, x)) + ,m_typeHigh(_mm_set_pd(w, z)) + { + } + + DG_INLINE dgBigVector(dgInt32 ix, dgInt32 iy, dgInt32 iz, dgInt32 iw) + :m_ix(dgInt64(ix)), m_iy(dgInt64(iy)), m_iz(dgInt64(iz)), m_iw(dgInt64(iw)) + { + } + + DG_INLINE dgBigVector(dgInt64 ix, dgInt64 iy, dgInt64 iz, dgInt64 iw) + :m_ix(ix), m_iy(iy), m_iz(iz), m_iw(iw) + { + } + + + DG_INLINE dgFloat64& operator[] (dgInt32 i) + { + dgAssert(i < 4); + dgAssert(i >= 0); + return m_f[i]; + } + + DG_INLINE const dgFloat64& operator[] (dgInt32 i) const + { + dgAssert(i < 4); + dgAssert(i >= 0); + return m_f[i]; + } + + DG_INLINE dgFloat64 GetScalar() const + { + //return m_x; + return _mm_cvtsd_f64(m_typeLow); + } + + DG_INLINE dgBigVector operator+ (const dgBigVector& A) const + { + return dgBigVector(_mm_add_pd(m_typeLow, A.m_typeLow), _mm_add_pd(m_typeHigh, A.m_typeHigh)); + } + + DG_INLINE dgBigVector operator- (const dgBigVector& A) const + { + return dgBigVector(_mm_sub_pd(m_typeLow, A.m_typeLow), _mm_sub_pd(m_typeHigh, A.m_typeHigh)); + } + + DG_INLINE dgBigVector operator* (const dgBigVector& A) const + { + return dgBigVector(_mm_mul_pd(m_typeLow, A.m_typeLow), _mm_mul_pd(m_typeHigh, A.m_typeHigh)); + } + + DG_INLINE dgBigVector& operator+= (const dgBigVector& A) + { + m_typeLow = _mm_add_pd(m_typeLow, A.m_typeLow); + m_typeHigh = _mm_add_pd(m_typeHigh, A.m_typeHigh); + return *this; + } + + DG_INLINE dgBigVector& operator-= (const dgBigVector& A) + { + m_typeLow = _mm_sub_pd(m_typeLow, A.m_typeLow); + m_typeHigh = _mm_sub_pd(m_typeHigh, A.m_typeHigh); + return *this; + } + + DG_INLINE dgBigVector& operator*= (const dgBigVector& A) + { + m_typeLow = _mm_mul_pd(m_typeLow, A.m_typeLow); + m_typeHigh = _mm_mul_pd(m_typeHigh, A.m_typeHigh); + return *this; + } + + DG_INLINE dgBigVector MulAdd(const dgBigVector& A, const dgBigVector& B) const + { + return *this + A * B; + } + + DG_INLINE dgBigVector MulSub(const dgBigVector& A, const dgBigVector& B) const + { + return *this - A * B; + } + + // return cross product + DG_INLINE dgBigVector CrossProduct(const dgBigVector& B) const + { + return dgBigVector(m_y * B.m_z - m_z * B.m_y, m_z * B.m_x - m_x * B.m_z, m_x * B.m_y - m_y * B.m_x, m_w); + } + + DG_INLINE dgBigVector AddHorizontal() const + { + __m128d tmp0(_mm_add_pd(m_typeHigh, m_typeLow)); + __m128d tmp1(_mm_hadd_pd(tmp0, tmp0)); + return dgBigVector(tmp1, tmp1); + } + + DG_INLINE dgBigVector BroadcastX() const + { + return dgBigVector(m_x); + } + + DG_INLINE dgBigVector BroadcastY() const + { + return dgBigVector(m_y); + } + + DG_INLINE dgBigVector BroadcastZ() const + { + return dgBigVector(m_z); + } + + DG_INLINE dgBigVector BroadcastW() const + { + return dgBigVector(m_w); + } + + DG_INLINE dgBigVector Scale(dgFloat64 s) const + { + __m128d tmp0(_mm_set1_pd(s)); + return dgBigVector(_mm_mul_pd(m_typeLow, tmp0), _mm_mul_pd(m_typeHigh, tmp0)); + } + + DG_INLINE dgBigVector Abs() const + { + return dgBigVector(_mm_and_pd(m_typeLow, m_signMask.m_typeLow), _mm_and_pd(m_typeHigh, m_signMask.m_typeLow)); + } + + DG_INLINE dgBigVector Reciproc() const + { + return dgBigVector(_mm_div_pd(m_one.m_typeLow, m_typeLow), _mm_div_pd(m_one.m_typeHigh, m_typeHigh)); + } + + DG_INLINE dgBigVector Sqrt() const + { + return dgBigVector(_mm_sqrt_pd(m_typeLow), _mm_sqrt_pd(m_typeHigh)); + } + + DG_INLINE dgBigVector InvSqrt() const + { + return Sqrt().Reciproc(); + } + + DG_INLINE dgBigVector Normalize() const + { + dgAssert (m_w == dgFloat32 (0.0f)); + dgFloat64 mag2 = DotProduct(*this).GetScalar(); + return Scale(dgFloat64 (1.0f) / sqrt (mag2)); + } + + dgFloat64 GetMax() const + { + __m128d tmp(_mm_max_pd(m_typeLow, m_typeHigh)); + return dgBigVector(_mm_max_pd(tmp, _mm_shuffle_pd(tmp, tmp, PERMUT_MASK_DOUBLE(0, 1))), tmp).GetScalar(); + } + + dgBigVector GetMax(const dgBigVector& data) const + { + return dgBigVector(_mm_max_pd(m_typeLow, data.m_typeLow), _mm_max_pd(m_typeHigh, data.m_typeHigh)); + } + + dgBigVector GetMin(const dgBigVector& data) const + { + return dgBigVector(_mm_min_pd(m_typeLow, data.m_typeLow), _mm_min_pd(m_typeHigh, data.m_typeHigh)); + } + + DG_INLINE dgBigVector GetInt() const + { + dgBigVector temp(Floor()); + dgInt64 x = _mm_cvtsd_si32(temp.m_typeLow); + dgInt64 y = _mm_cvtsd_si32(_mm_shuffle_pd(temp.m_typeLow, temp.m_typeLow, PERMUT_MASK_DOUBLE(1, 1))); + dgInt64 z = _mm_cvtsd_si32(temp.m_typeHigh); + dgInt64 w = _mm_cvtsd_si32(_mm_shuffle_pd(temp.m_typeHigh, temp.m_typeHigh, PERMUT_MASK_DOUBLE(1, 1))); + return dgBigVector(_mm_set_pd(*(dgFloat32*)&y, *(dgFloat32*)&x), _mm_set_pd(*(dgFloat32*)&w, *(dgFloat32*)&z)); + } + + // relational operators + DG_INLINE dgBigVector operator> (const dgBigVector& data) const + { + return dgBigVector(_mm_cmpgt_pd(m_typeLow, data.m_typeLow), _mm_cmpgt_pd(m_typeHigh, data.m_typeHigh)); + } + + DG_INLINE dgBigVector operator== (const dgBigVector& data) const + { + return dgBigVector(_mm_cmpeq_pd(m_typeLow, data.m_typeLow), _mm_cmpeq_pd(m_typeHigh, data.m_typeHigh)); + } + + DG_INLINE dgBigVector operator< (const dgBigVector& data) const + { + return dgBigVector(_mm_cmplt_pd(m_typeLow, data.m_typeLow), _mm_cmplt_pd(m_typeHigh, data.m_typeHigh)); + } + + DG_INLINE dgBigVector operator>= (const dgBigVector& data) const + { + return dgBigVector(_mm_cmpge_pd(m_typeLow, data.m_typeLow), _mm_cmpge_pd(m_typeHigh, data.m_typeHigh)); + } + + DG_INLINE dgBigVector operator<= (const dgBigVector& data) const + { + return dgBigVector(_mm_cmple_pd(m_typeLow, data.m_typeLow), _mm_cmple_pd(m_typeHigh, data.m_typeHigh)); + } + + // logical operations + DG_INLINE dgBigVector operator& (const dgBigVector& data) const + { + return dgBigVector(_mm_and_pd(m_typeLow, data.m_typeLow), _mm_and_pd(m_typeHigh, data.m_typeHigh)); + } + + DG_INLINE dgBigVector operator| (const dgBigVector& data) const + { + return dgBigVector(_mm_or_pd(m_typeLow, data.m_typeLow), _mm_or_pd(m_typeHigh, data.m_typeHigh)); + } + + DG_INLINE dgBigVector operator^ (const dgBigVector& data) const + { + return dgBigVector(_mm_xor_pd(m_typeLow, data.m_typeLow), _mm_xor_pd(m_typeHigh, data.m_typeHigh)); + } + + DG_INLINE dgBigVector AndNot(const dgBigVector& data) const + { + return dgBigVector(_mm_andnot_pd(data.m_typeLow, m_typeLow), _mm_andnot_pd(data.m_typeHigh, m_typeHigh)); + } + + DG_INLINE dgBigVector Select(const dgBigVector& data, const dgBigVector& mask) const + { + // (((b ^ a) & mask)^a) + return dgBigVector(_mm_xor_pd(m_typeLow, _mm_and_pd(mask.m_typeLow, _mm_xor_pd(m_typeLow, data.m_typeLow))), + _mm_xor_pd(m_typeHigh, _mm_and_pd(mask.m_typeHigh, _mm_xor_pd(m_typeHigh, data.m_typeHigh)))); + } + + DG_INLINE dgBigVector ShiftRight() const + { + //return dgBigVector (m_w, m_x, m_y, m_z); + return dgBigVector(_mm_shuffle_pd(m_typeHigh, m_typeLow, PERMUT_MASK_DOUBLE(0, 1)), _mm_shuffle_pd(m_typeLow, m_typeHigh, PERMUT_MASK_DOUBLE(0, 1))); + } + + DG_INLINE dgBigVector ShiftTripleRight() const + { + return dgBigVector(_mm_shuffle_pd(m_typeHigh, m_typeLow, PERMUT_MASK_DOUBLE(0, 0)), _mm_shuffle_pd(m_typeLow, m_typeHigh, PERMUT_MASK_DOUBLE(1, 1))); + } + + DG_INLINE dgBigVector ShiftTripleLeft() const + { + return dgBigVector(_mm_shuffle_pd(m_typeLow, m_typeHigh, PERMUT_MASK_DOUBLE(0, 1)), _mm_shuffle_pd(m_typeLow, m_typeHigh, PERMUT_MASK_DOUBLE(1, 0))); + } + + DG_INLINE dgBigVector ShiftRightLogical(int bits) const + { + //return dgBigVector(dgInt64(dgUnsigned64(m_ix) >> bits), dgInt64(dgUnsigned64(m_iy) >> bits), dgInt64(dgUnsigned64(m_iz) >> bits), dgInt64(dgUnsigned64(m_iw) >> bits)); + return dgBigVector(_mm_srli_epi64(m_typeIntLow, bits), _mm_srli_epi64(m_typeIntHigh, bits)); + } + + DG_INLINE dgInt32 GetSignMask() const + { + return _mm_movemask_pd(m_typeLow) | (_mm_movemask_pd(m_typeHigh) << 2); + } + + DG_INLINE dgBigVector Floor() const + { + return dgBigVector(floor(m_x), floor(m_y), floor(m_z), floor(m_w)); + } + + DG_INLINE dgBigVector TestZero() const + { + return m_negOne & (*this == m_zero); + } + + DG_INLINE static void Transpose4x4(dgBigVector& dst0, dgBigVector& dst1, dgBigVector& dst2, dgBigVector& dst3, + const dgBigVector& src0, const dgBigVector& src1, const dgBigVector& src2, const dgBigVector& src3) + { + dgBigVector tmp0(src0); + dgBigVector tmp1(src1); + dgBigVector tmp2(src2); + dgBigVector tmp3(src3); + + dst0 = dgBigVector(tmp0.m_x, tmp1.m_x, tmp2.m_x, tmp3.m_x); + dst1 = dgBigVector(tmp0.m_y, tmp1.m_y, tmp2.m_y, tmp3.m_y); + dst2 = dgBigVector(tmp0.m_z, tmp1.m_z, tmp2.m_z, tmp3.m_z); + dst3 = dgBigVector(tmp0.m_w, tmp1.m_w, tmp2.m_w, tmp3.m_w); + } + + DG_INLINE dgFloat64 DotProduct3(const dgBigVector& A) const + { + return m_x * A.m_x + m_y * A.m_y + m_z * A.m_z; + } + + // return dot 4d dot product + DG_INLINE dgBigVector DotProduct(const dgBigVector &A) const + { + return (*this * A).AddHorizontal(); + } + + DG_INLINE dgBigVector CrossProduct(const dgBigVector& A, const dgBigVector& B) const + { + dgFloat64 cofactor[3][3]; + dgFloat64 array[4][4]; + + const dgBigVector& me = *this; + for (dgInt32 i = 0; i < 4; i++) { + array[0][i] = me[i]; + array[1][i] = A[i]; + array[2][i] = B[i]; + array[3][i] = dgFloat64(1.0f); + } + + dgBigVector normal; + dgFloat64 sign = dgFloat64(-1.0f); + for (dgInt32 i = 0; i < 4; i++) { + + for (dgInt32 j = 0; j < 3; j++) { + dgInt32 k0 = 0; + for (dgInt32 k = 0; k < 4; k++) { + if (k != i) { + cofactor[j][k0] = array[j][k]; + k0++; + } + } + } + dgFloat64 x = cofactor[0][0] * (cofactor[1][1] * cofactor[2][2] - cofactor[1][2] * cofactor[2][1]); + dgFloat64 y = cofactor[0][1] * (cofactor[1][2] * cofactor[2][0] - cofactor[1][0] * cofactor[2][2]); + dgFloat64 z = cofactor[0][2] * (cofactor[1][0] * cofactor[2][1] - cofactor[1][1] * cofactor[2][0]); + dgFloat64 det = x + y + z; + + normal[i] = sign * det; + sign *= dgFloat64(-1.0f); + } + + return normal; + } + + DG_CLASS_ALLOCATOR(allocator) + + union + { + dgFloat64 m_f[4]; + dgInt64 m_i[4]; + struct + { + __m128d m_typeLow; + __m128d m_typeHigh; + }; + struct + { + __m128i m_typeIntLow; + __m128i m_typeIntHigh; + }; + struct + { + dgFloat64 m_x; + dgFloat64 m_y; + dgFloat64 m_z; + dgFloat64 m_w; + }; + struct + { + dgInt64 m_ix; + dgInt64 m_iy; + dgInt64 m_iz; + dgInt64 m_iw; + }; + }; + + static dgBigVector m_zero; + static dgBigVector m_one; + static dgBigVector m_wOne; + static dgBigVector m_two; + static dgBigVector m_half; + static dgBigVector m_three; + static dgBigVector m_negOne; + static dgBigVector m_xMask; + static dgBigVector m_yMask; + static dgBigVector m_zMask; + static dgBigVector m_wMask; + static dgBigVector m_epsilon; + static dgBigVector m_signMask; + static dgBigVector m_triplexMask; +} DG_GCC_VECTOR_ALIGNMENT; + + +DG_MSC_VECTOR_ALIGNMENT +class dgSpatialVector +{ + public: + DG_INLINE dgSpatialVector() + { + } + + DG_INLINE dgSpatialVector(const dgFloat32 a) + :m_d0(_mm_set1_pd(a)) + ,m_d1(_mm_set1_pd(a)) + ,m_d2(_mm_set1_pd(a)) + { + } + +#ifdef _NEWTON_USE_DOUBLE +#define PURMUT_MASK2(y, x) _MM_SHUFFLE2(x, y) + DG_INLINE dgSpatialVector(const dgVector& low, const dgVector& high) + :m_d0(low.m_typeLow) + ,m_d1(_mm_shuffle_pd(low.m_typeHigh, high.m_typeLow, PURMUT_MASK2(0, 0))) + ,m_d2(_mm_shuffle_pd(high.m_typeLow, high.m_typeHigh, PURMUT_MASK2(1, 0))) + { + } +#else + DG_INLINE dgSpatialVector(const dgVector& low, const dgVector& high) + :m_d0(_mm_cvtps_pd(low.m_type)) + ,m_d1(_mm_cvtps_pd(_mm_unpackhi_ps(low.m_type, _mm_shuffle_ps(low.m_type, high.m_type, PERMUTE_MASK(0, 0, 0, 2))))) + ,m_d2(_mm_cvtps_pd(_mm_shuffle_ps(high.m_type, high.m_type, PERMUTE_MASK(3, 3, 2, 1)))) + { + } +#endif + + DG_INLINE dgSpatialVector(const dgSpatialVector& copy) + :m_d0(copy.m_d0) + ,m_d1(copy.m_d1) + ,m_d2(copy.m_d2) + { + } + + DG_INLINE dgSpatialVector(const __m128d d0, const __m128d d1, const __m128d d2) + :m_d0(d0) + ,m_d1(d1) + ,m_d2(d2) + { + } + + DG_INLINE dgFloat64& operator[] (dgInt32 i) + { + dgAssert(i < 6); + dgAssert(i >= 0); + return ((dgFloat64*)&m_d0)[i]; + } + + DG_INLINE const dgFloat64& operator[] (dgInt32 i) const + { + dgAssert(i < 6); + dgAssert(i >= 0); + return ((dgFloat64*)&m_d0)[i]; + } + + DG_INLINE dgSpatialVector operator+ (const dgSpatialVector& A) const + { + return dgSpatialVector(_mm_add_pd(m_d0, A.m_d0), _mm_add_pd(m_d1, A.m_d1), _mm_add_pd(m_d2, A.m_d2)); + } + + DG_INLINE dgSpatialVector operator*(const dgSpatialVector& A) const + { + return dgSpatialVector(_mm_mul_pd(m_d0, A.m_d0), _mm_mul_pd(m_d1, A.m_d1), _mm_mul_pd(m_d2, A.m_d2)); + } + + DG_INLINE dgFloat64 DotProduct(const dgSpatialVector& v) const + { + dgSpatialVector tmp(*this * v); + __m128d tmp2(_mm_add_pd(tmp.m_d0, _mm_add_pd(tmp.m_d1, tmp.m_d2))); + return _mm_cvtsd_f64(_mm_hadd_pd(tmp2, tmp2)); + } + + DG_INLINE dgSpatialVector Scale(dgFloat64 s) const + { + __m128d tmp(_mm_set1_pd(s)); + return dgSpatialVector(_mm_mul_pd(m_d0, tmp), _mm_mul_pd(m_d1, tmp), _mm_mul_pd(m_d2, tmp)); + } + + __m128d m_d0; + __m128d m_d1; + __m128d m_d2; + static dgSpatialVector m_zero; +} DG_GCC_VECTOR_ALIGNMENT; + +#endif +#endif diff --git a/thirdparty/src/newton/dgMeshUtil/dgMeshEffect.h b/thirdparty/src/newton/dgMeshUtil/dgMeshEffect.h new file mode 100644 index 000000000..d79e95360 --- /dev/null +++ b/thirdparty/src/newton/dgMeshUtil/dgMeshEffect.h @@ -0,0 +1,596 @@ +/* Copyright (c) <2003-2019> +* +* 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 __dgMeshEffect_H__ +#define __dgMeshEffect_H__ + +#include + +class dgWorld; +class dgMeshEffect; +class dgCollisionInstance; + +#define DG_MESH_EFFECT_PRECISION_BITS 48 +#define DG_MESH_EFFECT_PRECISION_SCALE dgFloat64(dgInt64(1)< + class dgChannel: public dgArray + { + public: + dgChannel(dgMemoryAllocator* const allocator) + :dgArray(allocator) + ,m_count(0) + ,m_type(type) + { + } + + dgChannel(const dgChannel& source) + :dgArray(source, source.m_count) + ,m_count(source.m_count) + ,m_type(source.m_type) + { + } + + ~dgChannel() + { + } + + void CopyFrom (const dgChannel& source) + { + dgArray& me = *this; + dgChannel& src = *((dgChannel*)&source); + + Clear(); + m_count = src.m_count; + dgAssert (m_type == src.m_type); + for (dgInt32 i = 0; i < m_count; i++) { + me[i] = src[i]; + } + } + + void Clear() + { + m_count = 0; + dgArray::Clear (); + } + + void Reserve (dgInt32 size) + { + dgArray::Resize(size); + m_count = size; + } + + void PushBack (const T& element) + { + T tmp (element); + dgArray& me = *this; + me[m_count] = tmp; + m_count ++; + } + + void SetCount (dgInt32 count) + { + if (m_count) { + dgAssert (count >= 0); + dgAssert (m_count >= count); + m_count = count; + } + } + + dgInt32 m_count; + dgChannelType m_type; + }; + + class dgFormat + { + public: + class dgSortKey + { + public: + dgInt32 m_mask; + dgInt32 m_ordinal; + dgInt32 m_vertexIndex; + dgInt32 m_attibuteIndex; + }; + class VertexSortData + { + public: + const dgChannel* m_points; + dgInt32 m_vertexSortIndex; + }; + + dgInt32 GetSortIndex (const dgChannel& points, dgFloat64& dist) const; + static dgInt32 CompareVertex(const dgSortKey* const ptr0, const dgSortKey* const ptr1, void* const context); + }; + + class dgPointFormat: public dgFormat + { + public: + dgPointFormat(dgMemoryAllocator* const allocator); + dgPointFormat(const dgPointFormat& source); + ~dgPointFormat(); + + void Clear(); + void SetCount (dgInt32 count); + void CompressData(dgInt32* const indexList); + + dgChannel m_layers; + dgChannel m_vertex; + }; + + class dgAttibutFormat: public dgFormat + { + public: + class dgUV + { + public: + dgFloat32 m_u; + dgFloat32 m_v; + }; + + dgAttibutFormat(dgMemoryAllocator* const allocator); + dgAttibutFormat(const dgAttibutFormat& source); + ~dgAttibutFormat(); + + void Clear(); + void SetCount (dgInt32 count); + void CopyFrom (const dgAttibutFormat& source); + void CopyEntryFrom (dgInt32 index, const dgAttibutFormat& source, dgInt32 sourceIndex); + void CompressData (const dgPointFormat& points, dgInt32* const indexList); + + dgChannel m_pointChannel; + dgChannel m_materialChannel; + dgChannel m_normalChannel; + dgChannel m_binormalChannel; + dgChannel m_colorChannel; + dgChannel m_uv0Channel; + dgChannel m_uv1Channel; + }; + + class dgIndexArray + { + public: + dgInt32 m_materialCount; + dgInt32 m_indexCount; + dgInt32 m_materials[256]; + dgInt32 m_materialsIndexCount[256]; + dgInt32* m_indexList; + }; + + class dgMeshBVH + { + public: + class dgMeshBVHNode + { + public: + dgMeshBVHNode (const dgMeshEffect* const mesh, dgEdge* const face, void* const userData); + dgMeshBVHNode (dgMeshBVHNode* const left, dgMeshBVHNode* const right); + ~dgMeshBVHNode (); + void SetBox (const dgVector& p0, const dgVector& p1); + + DG_CLASS_ALLOCATOR(allocator) + dgVector m_p0; + dgVector m_p1; + + dgFloat32 m_area; + dgEdge* m_face; + void* m_userData; + dgMeshBVHNode* m_left; + dgMeshBVHNode* m_right; + dgMeshBVHNode* m_parent; + }; + + class dgFitnessList: public dgTree + { + public: + dgFitnessList (dgMemoryAllocator* const allocator); + dgFloat64 TotalCost () const; + }; + + + dgMeshBVH (const dgMeshEffect* const mesh); + virtual ~dgMeshBVH(); + + virtual void Build (); + virtual void Cleanup (); + + void FaceRayCast (const dgBigVector& l0, const dgBigVector& l1, void* const userData) const; + void GetOverlapNodes (dgList& overlapNodes, const dgBigVector& p0, const dgBigVector& p1) const; + + protected: + virtual dgMeshBVHNode* CreateLeafNode (dgEdge* const face, void* const userData) = 0; + + dgMeshBVHNode* AddFaceNode (dgEdge* const face, void* const userData); + void RemoveNode (dgMeshBVHNode* const treeNode); + void ImproveNodeFitness (); + void ImproveNodeFitness (dgMeshBVHNode* const node); + dgFloat32 CalculateSurfaceArea (dgMeshBVHNode* const node0, dgMeshBVHNode* const node1, dgVector& minBox, dgVector& maxBox) const; + virtual bool SanityCheck() const; + + virtual dgFloat64 RayFaceIntersect (const dgMeshBVHNode* const face, const dgBigVector& p0, const dgBigVector& p1, void* const userData) const; +// virtual dgFloat64 VertexRayCast (const dgBigVector& l0, const dgBigVector& l1) const; +// virtual bool RayRayIntersect (dgEdge* const edge, const dgMeshEffect* const otherMesh, dgEdge* const otherEdge, dgFloat64& param, dgFloat64& otherParam) const; + + const dgMeshEffect* m_mesh; + dgMeshBVHNode* m_rootNode; + dgFitnessList m_fitness; + friend class dgMeshEffect; + }; + + dgMeshEffect (); + dgMeshEffect(dgMemoryAllocator* const allocator); + dgMeshEffect(dgCollisionInstance* const collision); + dgMeshEffect(const dgMeshEffect& source); + dgMeshEffect(dgPolyhedra& mesh, const dgMeshEffect& source); + dgMeshEffect (dgMemoryAllocator* const allocator, dgDeserialize deserialization, void* const userData); + + // create from OFF or PLY file format + dgMeshEffect(dgMemoryAllocator* const allocator, const char* const fileName); + + // Create a convex hull Mesh form point cloud + dgMeshEffect (dgMemoryAllocator* const allocator, const dgFloat64* const vertexCloud, dgInt32 count, dgInt32 strideInByte, dgFloat64 distTol); + + // create a planar Mesh + dgMeshEffect(dgMemoryAllocator* const allocator, const dgMatrix& planeMatrix, dgFloat32 witdth, dgFloat32 breadth, dgInt32 material, const dgMatrix& textureMatrix0, const dgMatrix& textureMatrix1); + virtual ~dgMeshEffect(void); + + void Init(); + + void Trace () const; + + void ApplyTransform (const dgMatrix& matrix); + dgMatrix CalculateOOBB (dgBigVector& size) const; + void CalculateAABB (dgBigVector& min, dgBigVector& max) const; + + void FlipWinding(); + void UniformBoxMapping (dgInt32 material, const dgMatrix& textureMatrix); + void CalculateNormals (dgFloat64 angleInRadians); + void SphericalMapping (dgInt32 material, const dgMatrix& uvAligment); + void BoxMapping (dgInt32 front, dgInt32 side, dgInt32 top, const dgMatrix& uvAligment); + void CylindricalMapping (dgInt32 cylinderMaterial, dgInt32 capMaterial, const dgMatrix& uvAligment); + void AngleBaseFlatteningMapping (dgInt32 cylinderMaterial, dgReportProgress progressReportCallback, void* const userData); + + dgEdge* InsertEdgeVertex (dgEdge* const edge, dgFloat64 param); + + dgMeshEffect* Union (const dgMatrix& matrix, const dgMeshEffect* const clipper) const; + dgMeshEffect* Difference (const dgMatrix& matrix, const dgMeshEffect* const clipper) const; + dgMeshEffect* Intersection (const dgMatrix& matrix, const dgMeshEffect* const clipper) const; + void ClipMesh (const dgMatrix& matrix, const dgMeshEffect* const clipper, dgMeshEffect** const top, dgMeshEffect** const bottom) const; + + //bool PlaneClip (const dgBigPlane& plane); + + dgMeshEffect* ConvexMeshIntersection (const dgMeshEffect* const convexMesh) const; + + dgMeshEffect* GetFirstLayer (); + dgMeshEffect* GetNextLayer (dgMeshEffect* const layer); + + void Triangulate (); + void ConvertToPolygons (); + void RemoveUnusedVertices(dgInt32* const vertexRemapTable); + + void BeginBuild (); + void BeginBuildFace (); + void AddPoint (dgFloat64 x, dgFloat64 y, dgFloat64 z); + void AddLayer (dgInt32 layer); + void AddMaterial (dgInt32 materialIndex); + void AddNormal (dgFloat32 x, dgFloat32 y, dgFloat32 z); + void AddBinormal (dgFloat32 x, dgFloat32 y, dgFloat32 z); + void AddVertexColor (dgFloat32 x, dgFloat32 y, dgFloat32 z, dgFloat32 w); + void AddUV0 (dgFloat32 u, dgFloat32 v); + void AddUV1 (dgFloat32 u, dgFloat32 v); + void EndBuildFace (); + void EndBuild (dgFloat64 tol, bool fixTjoint = true); + + dgInt32 GetVertexCount() const; + dgInt32 GetVertexStrideInByte() const; + const dgFloat64* GetVertexPool () const; + + dgInt32 GetVertexBaseCount() const; + void SetVertexBaseCount(dgInt32 count); + + dgEdge* SpliteFace (dgInt32 v0, dgInt32 v1); + + dgInt32 GetTotalFaceCount() const; + dgInt32 GetTotalIndexCount() const; + void GetFaces (dgInt32* const faceCount, dgInt32* const materials, void** const faceNodeList) const; + + void RepairTJoints (); + bool SeparateDuplicateLoops (dgEdge* const face); + + bool HasOpenEdges () const; + + dgFloat64 CalculateVolume () const; + + void OptimizePoints(); + void OptimizeAttibutes(); + void BuildFromIndexList(const dgMeshVertexFormat* const format); + + dgInt32 GetPropertiesCount() const; + const dgInt32* GetIndexToVertexMap() const; + + bool HasLayersChannel() const; + bool HasNormalChannel() const; + bool HasBinormalChannel() const; + bool HasUV0Channel() const; + bool HasUV1Channel() const; + bool HasVertexColorChannel() const; + + void GetVertexChannel64(dgInt32 strideInByte, dgFloat64* const bufferOut) const; + void GetVertexChannel(dgInt32 strideInByte, dgFloat32* const bufferOut) const; + void GetNormalChannel(dgInt32 strideInByte, dgFloat32* const bufferOut) const; + void GetBinormalChannel(dgInt32 strideInByte, dgFloat32* const bufferOut) const; + void GetUV0Channel(dgInt32 strideInByte, dgFloat32* const bufferOut) const; + void GetUV1Channel(dgInt32 strideInByte, dgFloat32* const bufferOut) const; + void GetVertexColorChannel(dgInt32 strideInByte, dgFloat32* const bufferOut) const; +// void GetWeightBlendChannel(dgInt32 strideInByte, dgFloat32* const bufferOut) const; +// void GetWeightIndexChannel(dgInt32 strideInByte, dgInt32* const bufferOut) const; + + dgIndexArray* MaterialGeometryBegin(); + void MaterialGeomteryEnd(dgIndexArray* const handle); + dgInt32 GetFirstMaterial (dgIndexArray* const handle) const; + dgInt32 GetNextMaterial (dgIndexArray* const handle, dgInt32 materialHandle) const; + dgInt32 GetMaterialID (dgIndexArray* const handle, dgInt32 materialHandle) const; + dgInt32 GetMaterialIndexCount (dgIndexArray* const handle, dgInt32 materialHandle) const; + void GetMaterialGetIndexStream (dgIndexArray* const handle, dgInt32 materialHandle, dgInt32* const index) const; + void GetMaterialGetIndexStreamShort (dgIndexArray* const handle, dgInt32 materialHandle, dgInt16* const index) const; + + dgCollisionInstance* CreateCollisionTree(dgWorld* const world, dgInt32 shapeID) const; + dgCollisionInstance* CreateConvexCollision(dgWorld* const world, dgFloat64 tolerance, dgInt32 shapeID, const dgMatrix& matrix = dgGetIdentityMatrix()) const; + + dgMeshEffect* CreateSimplification (dgInt32 maxVertexCount, dgReportProgress reportProgressCallback, void* const userData) const; + dgMeshEffect* CreateConvexApproximation (dgFloat32 maxConcavity, dgFloat32 backFaceDistanceFactor, dgInt32 maxHullOuputCount, dgInt32 maxVertexPerHull, dgReportProgress reportProgressCallback, void* const userData) const; + + dgMeshEffect* CreateTetrahedraIsoSurface() const; + void CreateTetrahedraLinearBlendSkinWeightsChannel (const dgMeshEffect* const tetrahedraMesh); + + static dgMeshEffect* CreateVoronoiConvexDecomposition (dgMemoryAllocator* const allocator, dgInt32 pointCount, dgInt32 pointStrideInBytes, const dgFloat32* const pointCloud, dgInt32 materialId, const dgMatrix& textureProjectionMatrix); + static dgMeshEffect* CreateFromSerialization (dgMemoryAllocator* const allocator, dgDeserialize deserialization, void* const userData); + + void LoadOffMesh (const char* const filename); + void LoadTetraMesh (const char* const filename); + void Serialize (dgSerialize callback, void* const userData) const; + + dgBigVector GetVertex (dgInt32 index) const; + dgInt32 GetVertexLayer (dgInt32 index) const; + + void TransformMesh (const dgMatrix& matrix); + + void* GetFirstVertex () const; + void* GetNextVertex (const void* const vertex) const; + int GetVertexIndex (const void* const vertex) const; + + void* GetFirstPoint () const; + void* GetNextPoint (const void* const point) const; + dgInt32 GetPointIndex (const void* const point) const; + dgInt32 GetVertexIndexFromPoint (const void* const point) const; + + void* GetFirstEdge () const; + void* GetNextEdge (const void* const edge) const; +// void* FindEdge (dgInt32 v0, dgInt32 v1) const; + void GetEdgeIndex (const void* const edge, dgInt32& v0, dgInt32& v1) const; +// void GetEdgeAttributeIndex (const void* edge, dgInt32& v0, dgInt32& v1) const; + + const dgEdge* GetPolyhedraEdgeFromNode(const void* const edge) const; + + void* GetFirstFace () const; + void* GetNextFace (const void* const face) const; + dgInt32 IsFaceOpen (const void* const face) const; + dgInt32 GetFaceMaterial (const void* const face) const; + dgInt32 GetFaceIndexCount (const void* const face) const; + void GetFaceIndex (const void* const face, dgInt32* const indices) const; + void GetFaceAttributeIndex (const void* const face, dgInt32* const indices) const; + dgBigVector CalculateFaceNormal (const void* const face) const; + + void SetFaceMaterial (const void* const face, int materialID); + void AddInterpolatedEdgeAttribute (dgEdge* const edge, dgFloat64 param); + dgInt32 AddInterpolatedHalfAttribute(dgEdge* const edge, dgInt32 midPoint); + dgInt32 InterpolateVertex (const dgBigVector& point, const dgEdge* const face) const; + + bool Sanity () const; + + protected: + virtual void BeginFace(); + virtual bool EndFace (); + + dgBigVector GetOrigin ()const; + dgInt32 CalculateMaxAttributes () const; + dgFloat64 QuantizeCordinade(dgFloat64 val) const; + + void MergeFaces (const dgMeshEffect* const source); +// void ReverseMergeFaces (dgMeshEffect* const source); + + bool PlaneClip (const dgMeshEffect& convexMesh, const dgEdge* const face); + + dgMeshEffect* GetNextLayer (dgInt32 mark); + dgMeshEffect* CreateVoronoiConvex (const dgBigVector* const conevexPointCloud, dgInt32 count, dgInt32 materialId, const dgMatrix& textureProjectionMatrix, dgFloat32 normalAngleInRadians) const; + + void PackAttibuteData (); + void UnpackAttibuteData (); + + void PackPoints (dgFloat64 tol); + void UnpackPoints(); + + dgPointFormat m_points; + dgAttibutFormat m_attrib; + dgInt32 m_vertexBaseCount; + dgInt32 m_constructionIndex; + + friend class dgConvexHull3d; + friend class dgConvexHull4d; + friend class dgBooleanMeshBVH; + friend class dgHACDClusterGraph; + friend class dgTriangleAnglesToUV; + friend class dgTetraIsoSufaceStuffing; + friend class dgCollisionCompoundFractured; +}; + +DG_INLINE dgInt32 dgMeshEffect::GetVertexCount() const +{ + return m_points.m_vertex.m_count; +} + +DG_INLINE dgInt32 dgMeshEffect::GetVertexBaseCount() const +{ + return m_vertexBaseCount; +} + +DG_INLINE void dgMeshEffect::SetVertexBaseCount(dgInt32 count) +{ + m_vertexBaseCount = count; +} + + +DG_INLINE dgInt32 dgMeshEffect::GetPropertiesCount() const +{ + return m_attrib.m_pointChannel.m_count; +} + +DG_INLINE const dgInt32* dgMeshEffect::GetIndexToVertexMap() const +{ + return &m_attrib.m_pointChannel[0]; +} + +DG_INLINE dgInt32 dgMeshEffect::GetMaterialID (dgIndexArray* const handle, dgInt32 materialHandle) const +{ + return handle->m_materials[materialHandle]; +} + +DG_INLINE dgInt32 dgMeshEffect::GetMaterialIndexCount (dgIndexArray* const handle, dgInt32 materialHandle) const +{ + return handle->m_materialsIndexCount[materialHandle]; +} + +DG_INLINE dgBigVector dgMeshEffect::GetVertex (dgInt32 index) const +{ + dgAssert(index >= 0); + dgAssert(index < m_points.m_vertex.m_count); + return m_points.m_vertex[index]; +} + +DG_INLINE bool dgMeshEffect::HasLayersChannel() const +{ + return m_points.m_layers.m_count != 0; +} + +DG_INLINE dgInt32 dgMeshEffect::GetVertexLayer(dgInt32 index) const +{ + dgAssert(index >= 0); + dgAssert(index < m_points.m_vertex.m_count); + return (m_points.m_layers.m_count) ? m_points.m_layers[index] : 0; +} + + +DG_INLINE dgInt32 dgMeshEffect::GetVertexStrideInByte() const +{ + return sizeof (dgBigVector); +} + +DG_INLINE const dgFloat64* dgMeshEffect::GetVertexPool () const +{ + return &m_points.m_vertex[0].m_x; +} + +DG_INLINE dgMeshEffect* dgMeshEffect::GetFirstLayer () +{ + return GetNextLayer (IncLRU()); +} + +DG_INLINE dgMeshEffect* dgMeshEffect::GetNextLayer (dgMeshEffect* const layerSegment) +{ + if (!layerSegment) { + return NULL; + } + return GetNextLayer (layerSegment->IncLRU() - 1); +} + + +DG_INLINE dgFloat64 dgMeshEffect::QuantizeCordinade(dgFloat64 x) const +{ + dgInt32 exp; + dgFloat64 mantissa = frexp(x, &exp); + mantissa = DG_MESH_EFFECT_PRECISION_SCALE_INV * floor (mantissa * DG_MESH_EFFECT_PRECISION_SCALE); + + dgFloat64 x1 = ldexp(mantissa, exp); + return x1; +} + +#endif diff --git a/thirdparty/src/newton/dgMeshUtil/dgMeshEffect1.cpp b/thirdparty/src/newton/dgMeshUtil/dgMeshEffect1.cpp new file mode 100644 index 000000000..2e2729ca8 --- /dev/null +++ b/thirdparty/src/newton/dgMeshUtil/dgMeshEffect1.cpp @@ -0,0 +1,4016 @@ +/* Copyright (c) <2003-2019> +* +* 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 "dgPhysicsStdafx.h" +#include "dgBody.h" +#include "dgWorld.h" +#include "dgMeshEffect.h" +#include "dgCollisionBVH.h" +#include "dgCollisionCompound.h" +#include "dgCollisionConvexHull.h" + + +dgMeshEffect::dgPointFormat::dgPointFormat(dgMemoryAllocator* const allocator) + :m_layers(allocator) + ,m_vertex(allocator) +{ +} + +dgMeshEffect::dgPointFormat::dgPointFormat(const dgPointFormat& source) + :m_layers(source.m_layers) + ,m_vertex(source.m_vertex) +{ +} + +dgMeshEffect::dgPointFormat::~dgPointFormat() +{ +} + +void dgMeshEffect::dgPointFormat::Clear() +{ + m_layers.Clear(); + m_vertex.Clear(); +} + +void dgMeshEffect::dgPointFormat::SetCount(dgInt32 count) +{ + m_layers.SetCount(count); + m_vertex.SetCount(count); +} + +dgInt32 dgMeshEffect::dgFormat::GetSortIndex (const dgChannel& points, dgFloat64& dist) const +{ + dgBigVector xc(dgFloat64(0.0f)); + dgBigVector x2c(dgFloat64(0.0f)); + dgBigVector minP(dgFloat64(1.0e10f)); + dgBigVector maxP(dgFloat64(-1.0e10f)); + for (dgInt32 i = 0; i < points.m_count; i++) { + dgBigVector x(points[i]); + xc += x; + x2c += x * x; + minP = minP.GetMin(x); + maxP = maxP.GetMax(x); + } + + dgBigVector del(maxP - minP); + dgFloat64 minDist = dgMin(del.m_x, del.m_y, del.m_z); + if (minDist < dgFloat64(1.0e-3f)) { + minDist = dgFloat64(1.0e-3f); + } + + dgInt32 firstSortAxis = 0; + x2c = x2c.Scale(points.m_count) - xc * xc; + if ((x2c.m_y >= x2c.m_x) && (x2c.m_y >= x2c.m_z)) { + firstSortAxis = 1; + } + else if ((x2c.m_z >= x2c.m_x) && (x2c.m_z >= x2c.m_y)) { + firstSortAxis = 2; + } + dist = minDist; + return firstSortAxis; +} + +dgInt32 dgMeshEffect::dgFormat::CompareVertex(const dgSortKey* const ptr0, const dgSortKey* const ptr1, void* const context) +{ + const VertexSortData* const sortContext = (VertexSortData*)context; + const dgInt32 compIndex = sortContext->m_vertexSortIndex; + const dgChannel& points = *sortContext->m_points; + const dgFloat64 x0 = points[ptr0->m_vertexIndex][compIndex]; + const dgFloat64 x1 = points[ptr1->m_vertexIndex][compIndex]; + + if (x0 < x1) { + return -1; + } else if (x0 > x1) { + return 1; + } + return 0; +} + +void dgMeshEffect::dgPointFormat::CompressData(dgInt32* const indexList) +{ + dgFloat64 minDist; + const dgInt32 firstSortAxis = GetSortIndex(m_vertex, minDist); + + dgStack indirectListBuffer(m_vertex.m_count); + dgFormat::dgSortKey* indirectList = &indirectListBuffer[0]; + for (dgInt32 i = 0; i < m_vertex.m_count; i++) { + indirectList[i].m_mask = -1; + indirectList[i].m_ordinal = i; + indirectList[i].m_vertexIndex = i; + indirectList[i].m_attibuteIndex = -1; + } + + dgPointFormat tmpFormat(*this); + VertexSortData sortContext; + sortContext.m_points = &tmpFormat.m_vertex; + //sortContext.m_points = &tmpFormat; + sortContext.m_vertexSortIndex = firstSortAxis; + dgSort(indirectList, m_vertex.m_count, dgFormat::CompareVertex, &sortContext); + + const dgFloat64 tolerance = dgMin (minDist, dgFloat64 (1.0e-12f)); + const dgFloat64 sweptWindow = dgFloat64(2.0f) * tolerance + dgFloat64 (1.0e-10f); + + dgInt32 newCount = 0; + for (dgInt32 i = 0; i < tmpFormat.m_vertex.m_count; i++) { + const dgInt32 ii = indirectList[i].m_mask; + if (ii == -1) { + const dgInt32 i0 = indirectList[i].m_ordinal; + const dgInt32 iii = indirectList[i].m_vertexIndex; + const dgBigVector& p = tmpFormat.m_vertex[iii]; + //const dgFloat64 swept = tmpFormat.m_vertex[iii][firstSortAxis] + sweptWindow; + const dgFloat64 swept = p[firstSortAxis] + sweptWindow; + for (dgInt32 j = i + 1; j < tmpFormat.m_vertex.m_count; j++) { + + const dgInt32 jj = indirectList[j].m_mask; + if (jj == -1) { + const dgInt32 j0 = indirectList[j].m_ordinal; + const dgInt32 jjj = indirectList[j].m_vertexIndex; + const dgBigVector& q = tmpFormat.m_vertex[jjj]; + //dgFloat64 val = tmpFormat.m_vertex[jjj][firstSortAxis]; + dgFloat64 val = q[firstSortAxis]; + if (val >= swept) { + break; + } + + bool test = true; + if (iii != jjj) { + //dgBigVector dp(tmpFormat.m_vertex[iii] - tmpFormat.m_vertex[jjj]); + dgBigVector dp(p - q); + for (dgInt32 k = 0; k < 3; k++) { + test &= (fabs(dp[k]) <= tolerance); + } + } + if (test && tmpFormat.m_layers.m_count) { + test &= (tmpFormat.m_layers[i0] == tmpFormat.m_layers[j0]); + } + // note, is ok weight duplicate to be ignored. + + if (test) { + indirectList[j].m_mask = newCount; + } + } + } + + //indirectList[newCount].m_attibuteIndex = indirectList[i].m_attibuteIndex; + indirectList[newCount].m_vertexIndex = indirectList[i].m_vertexIndex; + indirectList[i].m_mask = newCount; + newCount++; + } + } + + Clear(); + for (dgInt32 i = 0; i < newCount; i++) { + dgAssert (indirectList[i].m_attibuteIndex == -1); + m_vertex.PushBack(tmpFormat.m_vertex[indirectList[i].m_vertexIndex]); + } + + if (tmpFormat.m_layers.m_count) { + for (dgInt32 i = 0; i < newCount; i++) { + m_layers.PushBack(tmpFormat.m_layers[indirectList[i].m_vertexIndex]); + } + } + + for (dgInt32 i = 0; i < tmpFormat.m_vertex.m_count; i++) { + dgInt32 i1 = indirectList[i].m_ordinal; + dgInt32 index = indirectList[i].m_mask; + indexList[i1] = index; + } +} + +dgMeshEffect::dgAttibutFormat::dgAttibutFormat(dgMemoryAllocator* const allocator) + :m_pointChannel(allocator) + ,m_materialChannel(allocator) + ,m_normalChannel(allocator) + ,m_binormalChannel(allocator) + ,m_colorChannel(allocator) + ,m_uv0Channel(allocator) + ,m_uv1Channel(allocator) +{ +} + +dgMeshEffect::dgAttibutFormat::dgAttibutFormat(const dgAttibutFormat& source) + :m_pointChannel(source.m_pointChannel) + ,m_materialChannel(source.m_materialChannel) + ,m_normalChannel(source.m_normalChannel) + ,m_binormalChannel(source.m_binormalChannel) + ,m_colorChannel(source.m_colorChannel) + ,m_uv0Channel(source.m_uv0Channel) + ,m_uv1Channel(source.m_uv1Channel) +{ +} + +dgMeshEffect::dgAttibutFormat::~dgAttibutFormat() +{ +} + + +void dgMeshEffect::dgAttibutFormat::Clear() +{ + m_pointChannel.Clear(); + m_materialChannel.Clear(); + m_normalChannel.Clear(); + m_binormalChannel.Clear(); + m_colorChannel.Clear(); + m_uv0Channel.Clear(); + m_uv1Channel.Clear(); +} + +void dgMeshEffect::dgAttibutFormat::SetCount (dgInt32 count) +{ + m_pointChannel.SetCount(count); + m_materialChannel.SetCount(count); + m_normalChannel.SetCount(count); + m_binormalChannel.SetCount(count); + m_colorChannel.SetCount(count); + m_uv0Channel.SetCount(count); + m_uv1Channel.SetCount(count); +} + +void dgMeshEffect::dgAttibutFormat::CopyFrom (const dgAttibutFormat& source) +{ + m_pointChannel.CopyFrom(source.m_pointChannel); + m_materialChannel.CopyFrom(source.m_materialChannel); + m_normalChannel.CopyFrom(source.m_normalChannel); + m_binormalChannel.CopyFrom(source.m_binormalChannel); + m_colorChannel.CopyFrom(source.m_colorChannel); + m_uv0Channel.CopyFrom(source.m_uv0Channel); + m_uv1Channel.CopyFrom(source.m_uv1Channel); +} + +void dgMeshEffect::dgAttibutFormat::CompressData (const dgPointFormat& points, dgInt32* const indexList) +{ + dgFloat64 minDist; + const dgInt32 firstSortAxis = GetSortIndex(points.m_vertex, minDist); + + dgStack indirectListBuffer(m_pointChannel.m_count); + dgFormat::dgSortKey* indirectList = &indirectListBuffer[0]; + for (dgInt32 i = 0; i < m_pointChannel.m_count; i++) { + indirectList[i].m_mask = -1; + indirectList[i].m_ordinal = i; + indirectList[i].m_attibuteIndex = i; + indirectList[i].m_vertexIndex = m_pointChannel[i]; + } + + VertexSortData sortContext; + sortContext.m_points = &points.m_vertex; + sortContext.m_vertexSortIndex = firstSortAxis; + dgSort (indirectList, m_pointChannel.m_count, dgFormat::CompareVertex, &sortContext); + dgAttibutFormat tmpFormat (*this); + Clear(); + + const dgFloat64 tolerance = dgMin(minDist, dgFloat64(1.0e-12f)); + const dgFloat64 sweptWindow = dgFloat64(2.0f) * tolerance + dgFloat64(1.0e-10f); + + dgInt32 newCount = 0; + for (dgInt32 i = 0; i < tmpFormat.m_pointChannel.m_count; i ++) { + const dgInt32 ii = indirectList[i].m_mask; + if (ii == -1) { + const dgInt32 i0 = indirectList[i].m_ordinal; + const dgInt32 iii = indirectList[i].m_vertexIndex; + const dgFloat64 swept = points.m_vertex[iii][firstSortAxis] + sweptWindow; + for (dgInt32 j = i + 1; j < tmpFormat.m_pointChannel.m_count; j++) { + const dgInt32 jj = indirectList[j].m_mask; + if (jj == -1) { + const dgInt32 j0 = indirectList[j].m_ordinal; + const dgInt32 jjj = indirectList[j].m_vertexIndex;; + dgFloat64 val = points.m_vertex[jjj][firstSortAxis]; + if (val >= swept) { + break; + } + + bool test = true; + if (iii != jjj) { + dgBigVector dp (points.m_vertex[iii] - points.m_vertex[jjj]); + for (dgInt32 k = 0; k < 3; k ++) { + test &= (fabs (dp[k]) <= tolerance); + } + } + if (test && points.m_layers.m_count) { + test &= (points.m_layers[iii] == points.m_layers[jjj]); + } + + if (test && tmpFormat.m_normalChannel.m_count) { + dgVector n0(tmpFormat.m_normalChannel[i0].m_x, tmpFormat.m_normalChannel[i0].m_y, tmpFormat.m_normalChannel[i0].m_z, dgFloat32 (0.0f)); + dgVector n1(tmpFormat.m_normalChannel[j0].m_x, tmpFormat.m_normalChannel[j0].m_y, tmpFormat.m_normalChannel[j0].m_z, dgFloat32 (0.0f)); + dgVector dp (n1 - n0); + for (dgInt32 k = 0; k < 3; k++) { + test &= (fabs(dp[k]) <= tolerance); + } + } + + if (test && tmpFormat.m_binormalChannel.m_count) { + dgVector n0(tmpFormat.m_binormalChannel[i0].m_x, tmpFormat.m_binormalChannel[i0].m_y, tmpFormat.m_binormalChannel[i0].m_z, dgFloat32(0.0f)); + dgVector n1(tmpFormat.m_binormalChannel[j0].m_x, tmpFormat.m_binormalChannel[j0].m_y, tmpFormat.m_binormalChannel[j0].m_z, dgFloat32(0.0f)); + dgVector dp(n1 - n0); + for (dgInt32 k = 0; k < 3; k++) { + test &= (fabs(dp[k]) <= tolerance); + } + } + + if (test && tmpFormat.m_uv0Channel.m_count) { + dgVector n0(tmpFormat.m_uv0Channel[i0].m_u, tmpFormat.m_uv0Channel[i0].m_v, dgFloat32(0.0f), dgFloat32(0.0f)); + dgVector n1(tmpFormat.m_uv0Channel[j0].m_u, tmpFormat.m_uv0Channel[j0].m_v, dgFloat32(0.0f), dgFloat32(0.0f)); + dgVector dp(n1 - n0); + for (dgInt32 k = 0; k < 2; k++) { + test &= (fabs(dp[k]) <= tolerance); + } + } + + if (test && tmpFormat.m_uv1Channel.m_count) { + dgVector n0(tmpFormat.m_uv1Channel[i0].m_u, tmpFormat.m_uv1Channel[i0].m_v, dgFloat32(0.0f), dgFloat32(0.0f)); + dgVector n1(tmpFormat.m_uv1Channel[j0].m_u, tmpFormat.m_uv1Channel[j0].m_v, dgFloat32(0.0f), dgFloat32(0.0f)); + dgVector dp(n1 - n0); + for (dgInt32 k = 0; k < 2; k++) { + test &= (fabs(dp[k]) <= tolerance); + } + } + + if (test && tmpFormat.m_colorChannel.m_count) { + dgVector dp(m_colorChannel[i0] - m_colorChannel[j0]); + for (dgInt32 k = 0; k < 3; k++) { + test &= (fabs(dp[k]) <= tolerance); + } + } + + if (test && tmpFormat.m_materialChannel.m_count) { + test &= (tmpFormat.m_materialChannel[i0] == tmpFormat.m_materialChannel[j0]); + } + + if (test) { + indirectList[j].m_mask = newCount; + } + } + } + + indirectList[newCount].m_attibuteIndex = indirectList[i].m_attibuteIndex; + indirectList[newCount].m_vertexIndex = indirectList[i].m_vertexIndex; + indirectList[i].m_mask = newCount; + newCount++; + } + } + + for (dgInt32 i = 0; i < newCount; i ++) { + m_pointChannel.PushBack(indirectList[i].m_vertexIndex); + } + + if (tmpFormat.m_normalChannel.m_count) { + for (dgInt32 i = 0; i < newCount; i++) { + m_normalChannel.PushBack(tmpFormat.m_normalChannel[indirectList[i].m_attibuteIndex]); + } + } + + if (tmpFormat.m_binormalChannel.m_count) { + for (dgInt32 i = 0; i < newCount; i++) { + m_binormalChannel.PushBack(tmpFormat.m_binormalChannel[indirectList[i].m_attibuteIndex]); + } + } + + if (tmpFormat.m_uv0Channel.m_count) { + for (dgInt32 i = 0; i < newCount; i++) { + m_uv0Channel.PushBack(tmpFormat.m_uv0Channel[indirectList[i].m_attibuteIndex]); + } + } + + if (tmpFormat.m_uv1Channel.m_count) { + for (dgInt32 i = 0; i < newCount; i++) { + m_uv1Channel.PushBack(tmpFormat.m_uv1Channel[indirectList[i].m_attibuteIndex]); + } + } + + if (tmpFormat.m_colorChannel.m_count) { + for (dgInt32 i = 0; i < newCount; i++) { + m_colorChannel.PushBack(tmpFormat.m_colorChannel[indirectList[i].m_attibuteIndex]); + } + } + + if (tmpFormat.m_materialChannel.m_count) { + for (dgInt32 i = 0; i < newCount; i++) { + m_materialChannel.PushBack(tmpFormat.m_materialChannel[indirectList[i].m_attibuteIndex]); + } + } + + for (dgInt32 i = 0; i < tmpFormat.m_pointChannel.m_count; i ++) { + dgInt32 i1 = indirectList[i].m_ordinal; + dgInt32 index = indirectList[i].m_mask; + indexList[i1] = index; + } +} + +dgMeshEffect::dgMeshBVH::dgFitnessList::dgFitnessList (dgMemoryAllocator* const allocator) + :dgTree (allocator) +{ +} + +dgFloat64 dgMeshEffect::dgMeshBVH::dgFitnessList::TotalCost () const +{ + dgFloat64 cost = dgFloat32 (0.0f); + Iterator iter (*this); + for (iter.Begin(); iter; iter ++) { + dgTreeNode* const node = iter.GetNode(); + dgMeshBVHNode* const box = node->GetInfo(); + cost += box->m_area; + } + return cost; +} + + +dgMeshEffect::dgMeshBVH::dgMeshBVHNode::dgMeshBVHNode (const dgMeshEffect* const mesh, dgEdge* const face, void* const userData) + :m_area(dgFloat32 (0.0f)) + ,m_face (face) + ,m_userData(userData) + ,m_left (NULL) + ,m_right(NULL) + ,m_parent(NULL) +{ + dgBigVector p0(dgFloat32 ( 1.0e30f)); + dgBigVector p1(dgFloat32 (-1.0e30f)); + + const dgBigVector* const points = (dgBigVector*) mesh->GetVertexPool(); + + dgEdge* ptr = m_face; + do { + dgInt32 i = ptr->m_incidentVertex; + const dgBigVector& p = points[i]; + p0 = p.GetMin(p0); + p1 = p.GetMax(p1); + + ptr = ptr->m_next; + } while (ptr != face); + + dgVector padding (dgFloat32 (1.0f / 32.0f)); + SetBox (p0 - padding, p1 + padding); +} + +dgMeshEffect::dgMeshBVH::dgMeshBVHNode::dgMeshBVHNode (dgMeshBVHNode* const left, dgMeshBVHNode* const right) + :m_area(dgFloat32 (0.0f)) + ,m_face (NULL) + ,m_userData(NULL) + ,m_left (left) + ,m_right(right) + ,m_parent(NULL) +{ + m_left->m_parent = this; + m_right->m_parent = this; + +// dgVector p0 (dgMin (left->m_p0.m_x, right->m_p0.m_x), dgMin (left->m_p0.m_y, right->m_p0.m_y), dgMin (left->m_p0.m_z, right->m_p0.m_z), dgFloat32 (0.0f)); +// dgVector p1 (dgMax (left->m_p1.m_x, right->m_p1.m_x), dgMax (left->m_p1.m_y, right->m_p1.m_y), dgMax (left->m_p1.m_z, right->m_p1.m_z), dgFloat32 (0.0f)); + dgVector p0 (left->m_p0.GetMin(right->m_p0)); + dgVector p1 (left->m_p1.GetMax(right->m_p1)); + SetBox(p0, p1); +} + + +dgMeshEffect::dgMeshBVH::dgMeshBVHNode::~dgMeshBVHNode () +{ + if (m_left) { + delete m_left; + } + if (m_right) { + delete m_right; + } +} + +void dgMeshEffect::dgMeshBVH::dgMeshBVHNode::SetBox (const dgVector& p0, const dgVector& p1) +{ + m_p0 = p0 & dgVector::m_triplexMask; + m_p1 = p1 & dgVector::m_triplexMask; + dgVector size (dgVector::m_half * (m_p1 - m_p0)); + dgVector size1(size.ShiftTripleLeft()); + dgAssert (size1.m_w == dgFloat32 (0.0f)); + m_area = size.DotProduct(size1).GetScalar(); +} + +dgMeshEffect::dgMeshBVH::dgMeshBVH (const dgMeshEffect* const mesh) + :m_mesh(mesh) + ,m_rootNode(NULL) + ,m_fitness(m_mesh->GetAllocator()) +{ +} + + +dgMeshEffect::dgMeshBVH::~dgMeshBVH() +{ + Cleanup (); +} + +dgMeshEffect* dgMeshEffect::CreateFromSerialization (dgMemoryAllocator* const allocator, dgDeserialize deserialization, void* const userData) +{ + return new (allocator) dgMeshEffect(allocator, deserialization, userData); +} + +void dgMeshEffect::Serialize (dgSerialize callback, void* const userData) const +{ + dgAssert(0); +/* + dgInt32 faceCount = 0; + dgTreefilter(GetAllocator()); + Iterator iter (*this); + for (iter.Begin(); iter; iter ++) { + dgEdge* const face = &iter.GetNode()->GetInfo(); + if (!filter.Find(face) && (face->m_incidentFace > 0)) { + faceCount ++; + dgEdge* edge = face; + do { + filter.Insert(edge, edge); + edge = edge->m_next; + } while (edge != face); + } + } + + callback (userData, &faceCount, sizeof (dgInt32)); + callback (userData, &m_pointCount, sizeof (dgInt32)); + callback (userData, &m_atribCount, sizeof (dgInt32)); + callback (userData, &m_atribCount, sizeof (dgInt32)); + + callback (userData, m_points, m_pointCount * sizeof (dgBigVector)); + callback (userData, m_attrib, m_atribCount * sizeof (dgVertexAtribute)); + + filter.RemoveAll(); + for (iter.Begin(); iter; iter ++) { + dgEdge* const face = &iter.GetNode()->GetInfo(); + if (!filter.Find(face) && (face->m_incidentFace > 0)) { + dgInt32 indices[1024]; + dgInt64 attibuteIndex[1024]; + dgInt32 vertexCount = 0; + dgEdge* edge = face; + do { + indices[vertexCount] = edge->m_incidentVertex; + attibuteIndex[vertexCount] = edge->m_userData; + vertexCount ++; + filter.Insert(edge, edge); + edge = edge->m_next; + } while (edge != face); + + callback (userData, &vertexCount, sizeof (dgInt32)); + callback (userData, indices, vertexCount * sizeof (dgInt32)); + callback (userData, attibuteIndex, vertexCount * sizeof (dgInt64)); + } + } +*/ +} + +void dgMeshEffect::dgMeshBVH::Build () +{ + dgInt32 lru = m_mesh->IncLRU(); +/* + for (void* faceNode = m_mesh->GetFirstFace (); faceNode; faceNode = m_mesh->GetNextFace(faceNode)) { + if (!m_mesh->IsFaceOpen(faceNode)) { + dgEdge* const face = &((dgTreeNode*)faceNode)->GetInfo(); + if (face->m_mark != mark) { + AddFaceNode(face, NULL); + } + } + } +*/ + dgMeshEffect::Iterator iter(*m_mesh); + for (iter.Begin(); iter; iter++) { + dgEdge* const face = &iter.GetNode()->GetInfo(); + if (face->m_mark != lru) { + AddFaceNode(face, NULL); + } + } + ImproveNodeFitness (); +} + +void dgMeshEffect::dgMeshBVH::Cleanup () +{ + if (m_rootNode) { + delete m_rootNode; + } +} + +dgFloat32 dgMeshEffect::dgMeshBVH::CalculateSurfaceArea (dgMeshBVHNode* const node0, dgMeshBVHNode* const node1, dgVector& minBox, dgVector& maxBox) const +{ + minBox = dgVector (dgMin (node0->m_p0.m_x, node1->m_p0.m_x), dgMin (node0->m_p0.m_y, node1->m_p0.m_y), dgMin (node0->m_p0.m_z, node1->m_p0.m_z), dgFloat32 (0.0f)); + maxBox = dgVector (dgMax (node0->m_p1.m_x, node1->m_p1.m_x), dgMax (node0->m_p1.m_y, node1->m_p1.m_y), dgMax (node0->m_p1.m_z, node1->m_p1.m_z), dgFloat32 (0.0f)); + dgVector side0 ((maxBox - minBox) * dgVector::m_half); + dgVector side1 (side0.ShiftTripleLeft()); + dgAssert (side1.m_w == dgFloat32 (0.0f)); + return side0.DotProduct(side1).GetScalar(); +} + +void dgMeshEffect::dgMeshBVH::ImproveNodeFitness (dgMeshBVHNode* const node) +{ + dgAssert (node->m_left); + dgAssert (node->m_right); + + if (node->m_parent) { + if (node->m_parent->m_left == node) { + dgFloat32 cost0 = node->m_area; + + dgVector cost1P0; + dgVector cost1P1; + dgFloat32 cost1 = CalculateSurfaceArea (node->m_right, node->m_parent->m_right, cost1P0, cost1P1); + + dgVector cost2P0; + dgVector cost2P1; + dgFloat32 cost2 = CalculateSurfaceArea (node->m_left, node->m_parent->m_right, cost2P0, cost2P1); + + if ((cost1 <= cost0) && (cost1 <= cost2)) { + dgMeshBVHNode* const parent = node->m_parent; + node->m_p0 = parent->m_p0; + node->m_p1 = parent->m_p1; + node->m_area = parent->m_area; + + if (parent->m_parent) { + if (parent->m_parent->m_left == parent) { + parent->m_parent->m_left = node; + } else { + dgAssert (parent->m_parent->m_right == parent); + parent->m_parent->m_right = node; + } + } else { + m_rootNode = node; + } + node->m_parent = parent->m_parent; + parent->m_parent = node; + node->m_right->m_parent = parent; + parent->m_left = node->m_right; + node->m_right = parent; + parent->m_p0 = cost1P0; + parent->m_p1 = cost1P1; + parent->m_area = cost1; + + + } else if ((cost2 <= cost0) && (cost2 <= cost1)) { + dgMeshBVHNode* const parent = node->m_parent; + node->m_p0 = parent->m_p0; + node->m_p1 = parent->m_p1; + node->m_area = parent->m_area; + + if (parent->m_parent) { + if (parent->m_parent->m_left == parent) { + parent->m_parent->m_left = node; + } else { + dgAssert (parent->m_parent->m_right == parent); + parent->m_parent->m_right = node; + } + } else { + m_rootNode = node; + } + node->m_parent = parent->m_parent; + parent->m_parent = node; + node->m_left->m_parent = parent; + parent->m_left = node->m_left; + node->m_left = parent; + + parent->m_p0 = cost2P0; + parent->m_p1 = cost2P1; + parent->m_area = cost2; + } + } else { + dgFloat32 cost0 = node->m_area; + + dgVector cost1P0; + dgVector cost1P1; + dgFloat32 cost1 = CalculateSurfaceArea (node->m_left, node->m_parent->m_left, cost1P0, cost1P1); + + dgVector cost2P0; + dgVector cost2P1; + dgFloat32 cost2 = CalculateSurfaceArea (node->m_right, node->m_parent->m_left, cost2P0, cost2P1); + + if ((cost1 <= cost0) && (cost1 <= cost2)) { + dgMeshBVHNode* const parent = node->m_parent; + node->m_p0 = parent->m_p0; + node->m_p1 = parent->m_p1; + node->m_area = parent->m_area; + + if (parent->m_parent) { + if (parent->m_parent->m_left == parent) { + parent->m_parent->m_left = node; + } else { + dgAssert (parent->m_parent->m_right == parent); + parent->m_parent->m_right = node; + } + } else { + m_rootNode = node; + } + node->m_parent = parent->m_parent; + parent->m_parent = node; + node->m_left->m_parent = parent; + parent->m_right = node->m_left; + node->m_left = parent; + + parent->m_p0 = cost1P0; + parent->m_p1 = cost1P1; + parent->m_area = cost1; + + } else if ((cost2 <= cost0) && (cost2 <= cost1)) { + dgMeshBVHNode* const parent = node->m_parent; + node->m_p0 = parent->m_p0; + node->m_p1 = parent->m_p1; + node->m_area = parent->m_area; + + if (parent->m_parent) { + if (parent->m_parent->m_left == parent) { + parent->m_parent->m_left = node; + } else { + dgAssert (parent->m_parent->m_right == parent); + parent->m_parent->m_right = node; + } + } else { + m_rootNode = node; + } + node->m_parent = parent->m_parent; + parent->m_parent = node; + node->m_right->m_parent = parent; + parent->m_right = node->m_right; + node->m_right = parent; + + parent->m_p0 = cost2P0; + parent->m_p1 = cost2P1; + parent->m_area = cost2; + } + } + } + // dgAssert (SanityCheck()); +} + + +void dgMeshEffect::dgMeshBVH::ImproveNodeFitness () +{ + dgFloat64 cost0 = m_fitness.TotalCost (); + dgFloat64 cost1 = cost0; + do { + cost0 = cost1; + dgFitnessList::Iterator iter (m_fitness); + for (iter.Begin(); iter; iter ++) { + dgFitnessList::dgTreeNode* const node = iter.GetNode(); + ImproveNodeFitness (node->GetInfo()); + } + cost1 = m_fitness.TotalCost (); + } while (cost1 < (dgFloat32 (0.95f)) * cost0); +} + +/* +dgMeshEffect::dgMeshBVH::dgMeshBVHNode* dgMeshEffect::dgMeshBVH::CreateLeafNode (dgEdge* const face, void* const userData) +{ + dgMemoryAllocator* const allocator = m_mesh->GetAllocator(); + return new (allocator) dgMeshBVHNode (m_mesh, face, userData); +} +*/ + +dgMeshEffect::dgMeshBVH::dgMeshBVHNode* dgMeshEffect::dgMeshBVH::AddFaceNode (dgEdge* const face, void* const userData) +{ + dgMemoryAllocator* const allocator = m_mesh->GetAllocator(); + + dgMeshBVHNode* const newNode = CreateLeafNode (face, userData); + if (!m_rootNode) { + m_rootNode = newNode; + } else { + + dgVector p0; + dgVector p1; + dgMeshBVHNode* sibling = m_rootNode; + + dgFloat32 surfaceArea = dgMeshBVH::CalculateSurfaceArea (newNode, sibling, p0, p1); + while(sibling->m_left && sibling->m_right) { + + if (surfaceArea > sibling->m_area) { + break; + } + + sibling->SetBox (p0, p1); + + dgVector leftP0; + dgVector leftP1; + dgFloat32 leftSurfaceArea = CalculateSurfaceArea (newNode, sibling->m_left, leftP0, leftP1); + + dgVector rightP0; + dgVector rightP1; + dgFloat32 rightSurfaceArea = CalculateSurfaceArea (newNode, sibling->m_right, rightP0, rightP1); + + if (leftSurfaceArea < rightSurfaceArea) { + sibling = sibling->m_left; + p0 = leftP0; + p1 = leftP1; + surfaceArea = leftSurfaceArea; + } else { + sibling = sibling->m_right; + p0 = rightP0; + p1 = rightP1; + surfaceArea = rightSurfaceArea; + } + } + + if (!sibling->m_parent) { + m_rootNode = new (allocator) dgMeshBVHNode (sibling, newNode); + m_fitness.Insert(m_rootNode, m_rootNode); + } else { + dgMeshBVHNode* const parent = sibling->m_parent; + if (parent->m_left == sibling) { + dgMeshBVHNode* const node = new (allocator) dgMeshBVHNode (sibling, newNode); + m_fitness.Insert(node, node); + parent->m_left = node; + node->m_parent = parent; + } else { + dgAssert (parent->m_right == sibling); + dgMeshBVHNode* const node = new (allocator) dgMeshBVHNode (sibling, newNode); + m_fitness.Insert(node, node); + parent->m_right = node; + node->m_parent = parent; + } + } + } + + return newNode; +} + + +void dgMeshEffect::dgMeshBVH::RemoveNode (dgMeshBVHNode* const treeNode) +{ + if (!treeNode->m_parent) { + delete (m_rootNode); + m_rootNode = NULL; + } else if (!treeNode->m_parent->m_parent) { + dgMeshBVHNode* const root = m_rootNode; + if (treeNode->m_parent->m_left == treeNode) { + m_rootNode = treeNode->m_parent->m_right; + treeNode->m_parent->m_right = NULL; + } else { + dgAssert (treeNode->m_parent->m_right == treeNode); + m_rootNode = treeNode->m_parent->m_left; + treeNode->m_parent->m_left= NULL; + } + m_rootNode->m_parent = NULL; + dgAssert (m_fitness.Find(root)); + m_fitness.Remove(root); + delete (root); + + } else { + dgMeshBVHNode* const root = treeNode->m_parent->m_parent; + if (treeNode->m_parent == root->m_left) { + if (treeNode->m_parent->m_right == treeNode) { + root->m_left = treeNode->m_parent->m_left; + treeNode->m_parent->m_left = NULL; + } else { + dgAssert (treeNode->m_parent->m_left == treeNode); + root->m_left = treeNode->m_parent->m_right; + treeNode->m_parent->m_right = NULL; + } + root->m_left->m_parent = root; + } else { + if (treeNode->m_parent->m_right == treeNode) { + root->m_right = treeNode->m_parent->m_left; + treeNode->m_parent->m_left = NULL; + } else { + dgAssert (treeNode->m_parent->m_left == treeNode); + root->m_right = treeNode->m_parent->m_right; + treeNode->m_parent->m_right = NULL; + } + root->m_right->m_parent = root; + } + + dgAssert (m_fitness.Find(treeNode->m_parent)); + m_fitness.Remove(treeNode->m_parent); + delete (treeNode->m_parent); + } + + //dgAssert (SanityCheck()); +} + +bool dgMeshEffect::dgMeshBVH::SanityCheck() const +{ + #ifdef _DEBUG + dgAssert (m_mesh->Sanity ()); + + if (!m_rootNode) { + return false; + } + + if ((!m_rootNode->m_left && m_rootNode->m_right) || (m_rootNode->m_left && !m_rootNode->m_right)) { + return false; + } + + if (m_rootNode->m_left && m_rootNode->m_right) { + if (m_rootNode->m_left->m_parent != m_rootNode) { + return false; + } + if (m_rootNode->m_right->m_parent != m_rootNode) { + return false; + } + + dgMeshBVHNode* stackPool[DG_MESH_EFFECT_BVH_STACK_DEPTH]; + + dgInt32 stack = 2; + stackPool[0] = m_rootNode->m_left; + stackPool[1] = m_rootNode->m_right; + while (stack) { + stack --; + dgMeshBVHNode* const node = stackPool[stack]; + + if ((node->m_parent->m_left != node) && (node->m_parent->m_right != node)){ + return false; + } + + if (node->m_left) { + dgAssert (node->m_right); + stackPool[stack] = node->m_left; + stack++; + dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgMeshBVHNode*))); + + stackPool[stack] = node->m_right; + stack++; + dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgMeshBVHNode*))); + } + } + } + #endif + return true; +} + +void dgMeshEffect::dgMeshBVH::GetOverlapNodes (dgList& overlapNodes, const dgBigVector& p0, const dgBigVector& p1) const +{ + dgMeshBVHNode* stackPool[DG_MESH_EFFECT_BVH_STACK_DEPTH]; + + dgInt32 stack = 1; + stackPool[0] = m_rootNode; + + dgVector l0(p0); + dgVector l1(p1); + + while (stack) { + stack --; + dgMeshBVHNode* const me = stackPool[stack]; + + if (me && dgOverlapTest (me->m_p0, me->m_p1, l0, l1)) { + + if (!me->m_left) { + dgAssert (!me->m_right); + overlapNodes.Append(me); + } else { + dgAssert (me->m_left); + dgAssert (me->m_right); + stackPool[stack] = me->m_left; + stack++; + dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgMeshBVHNode*))); + + stackPool[stack] = me->m_right; + stack++; + dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgMeshBVHNode*))); + } + } + } +} + +/* +dgFloat64 dgMeshEffect::dgMeshBVH::VertexRayCast (const dgBigVector& p0, const dgBigVector& p1) const +{ + dgAssert (0); + + dgMeshBVHNode* stackPool[DG_MESH_EFFECT_BVH_STACK_DEPTH]; + + dgInt32 stack = 1; + stackPool[0] = m_rootNode; + + dgVector l0(p0); + dgVector l1(p1); + dgBigVector p1p0 (p1 - p0); + dgFloat64 den = p1p0 % p1p0; + + const dgBigVector* const points = (dgBigVector*) m_mesh->GetVertexPool(); + while (stack) { + stack --; + dgMeshBVHNode* const me = stackPool[stack]; + + if (me && dgOverlapTest (me->m_p0, me->m_p1, l0, l1)) { + if (!me->m_left) { + dgAssert (!me->m_right); + + dgEdge* ptr = me->m_face; + do { + dgInt32 index = ptr->m_incidentVertex; + const dgBigVector& q0 = points[index]; + dgBigVector q0p0 (q0 - p0); + dgFloat64 alpha = q0p0 % p1p0; + if ((alpha > (DG_BOOLEAN_ZERO_TOLERANCE * den)) && (alpha < (den - DG_BOOLEAN_ZERO_TOLERANCE))) { + dgBigVector dist (p0 + p1p0.Scale (alpha / den) - q0); + dgFloat64 dist2 = dist % dist; + if (dist2 < (DG_BOOLEAN_ZERO_TOLERANCE * DG_BOOLEAN_ZERO_TOLERANCE)) { + return alpha / den; + } + } + + ptr = ptr->m_next; + } while (ptr != me->m_face); + } else { + dgAssert (me->m_left); + dgAssert (me->m_right); + stackPool[stack] = me->m_left; + stack++; + dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgMeshBVHNode*))); + + stackPool[stack] = me->m_right; + stack++; + dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgMeshBVHNode*))); + } + } + } + return 1.2f; +} + + +bool dgMeshEffect::dgMeshBVH::RayRayIntersect (dgEdge* const edge, const dgMeshEffect* const otherMesh, dgEdge* const otherEdge, dgFloat64& param, dgFloat64& otherParam) const +{ + dgAssert (0); + + dgBigVector ray_p0 (m_mesh->m_points[edge->m_incidentVertex]); + dgBigVector ray_p1 (m_mesh->m_points[edge->m_twin->m_incidentVertex]); + + dgBigVector ray_q0 (otherMesh->m_points[otherEdge->m_incidentVertex]); + dgBigVector ray_q1 (otherMesh->m_points[otherEdge->m_twin->m_incidentVertex]); + + dgBigVector p1p0 (ray_p1 - ray_p0); + dgBigVector q1q0 (ray_q1 - ray_q0); + dgBigVector p0q0 (ray_p0 - ray_q0); + + dgFloat64 a = p1p0 % p1p0; // always >= 0 + dgFloat64 c = q1q0 % q1q0; // always >= 0 + dgFloat64 b = p1p0 % q1q0; + + dgFloat64 d = (p1p0 % p0q0); + dgFloat64 e = (q1q0 % p0q0); + dgFloat64 den = a * c - b * b; // always >= 0 + // compute the line parameters of the two closest points + if (den < DG_BOOLEAN_ZERO_TOLERANCE) { + // the lines are almost parallel + return false; + } else { + // get the closest points on the infinite lines + dgFloat64 t = b * e - c * d; + dgFloat64 s = a * e - b * d; + + if (t < (DG_BOOLEAN_ZERO_TOLERANCE * den) || (s < (DG_BOOLEAN_ZERO_TOLERANCE * den)) || (t > (den - DG_BOOLEAN_ZERO_TOLERANCE)) || (s > (den - DG_BOOLEAN_ZERO_TOLERANCE))) { + return false; + } + //dgBigVector normal (p1p0 * q1q0); + dgBigVector r0 (ray_p0 + p1p0.Scale (t / den)); + dgBigVector r1 (ray_q0 + q1q0.Scale (s / den)); + dgBigVector r1r0 (r1 - r0); + dgFloat64 dist2 = r1r0 % r1r0; + if (dist2 > (DG_BOOLEAN_ZERO_TOLERANCE * DG_BOOLEAN_ZERO_TOLERANCE)) { + return false; + } + + param = t / den; + otherParam = s / den; + } + return true; +} +*/ + + +dgFloat64 dgMeshEffect::dgMeshBVH::RayFaceIntersect (const dgMeshBVHNode* const faceNode, const dgBigVector& p0, const dgBigVector& p1, void* const userData) const +{ + dgAssert (0); + return 0; +/* + dgBigVector normal (m_mesh->FaceNormal(faceNode->m_face, m_mesh->GetVertexPool(), sizeof(dgBigVector))); + + dgBigVector diff (p1 - p0); + + dgFloat64 tOut = 2.0f; + const dgBigVector* const points = (dgBigVector*) m_mesh->GetVertexPool(); + dgFloat64 dir = normal.DotProduct3(diff); + if (dir < 0.0f) { + dgEdge* ptr = faceNode->m_face; + do { + dgInt32 index0 = ptr->m_incidentVertex; + dgInt32 index1 = ptr->m_next->m_incidentVertex; + dgBigVector p0v0 (points[index0] - p0); + dgBigVector p0v1 (points[index1] - p0); + dgFloat64 alpha = p0v0.DotProduct3(diff.CrossProduct(p0v1)); + if (alpha <= 0.0f) { + return 1.2f; + } + + ptr = ptr->m_next; + } while (ptr != faceNode->m_face); + + dgInt32 index0 = ptr->m_incidentVertex; + dgBigVector p0v0 (points[index0] - p0); + tOut = normal.DotProduct3(p0v0); + dgFloat64 dist = normal.DotProduct3(diff); + tOut = tOut / dist; + + } else if (doubleSidedFaces && (dir > 0.0f)) { + dgEdge* ptr = faceNode->m_face; + do { + dgInt32 index0 = ptr->m_incidentVertex; + dgInt32 index1 = ptr->m_prev->m_incidentVertex; + dgBigVector p0v0 (points[index0] - p0); + dgBigVector p0v1 (points[index1] - p0); + dgFloat64 alpha = p0v0.DotProduct3(diff.CrossProduct(p0v1)); + if (alpha <= 0.0f) { + return 1.2f; + } + + ptr = ptr->m_prev; + } while (ptr != faceNode->m_face); + + dgInt32 index0 = ptr->m_incidentVertex; + dgBigVector p0v0 (points[index0] - p0); + tOut = normal.DotProduct3(p0v0); + dgFloat64 dist = normal.DotProduct3(diff); + tOut = tOut / dist; + } + + if (tOut < 1.e-12f) { + tOut = 2.0f; + } else if (tOut > (1.0 - 1.e-12f)) { + tOut = 2.0f; + } + return tOut; +*/ +} + + +void dgMeshEffect::dgMeshBVH::FaceRayCast (const dgBigVector& p0, const dgBigVector& p1, void* const userData) const +{ + dgMeshBVHNode* stackPool[DG_MESH_EFFECT_BVH_STACK_DEPTH]; + + dgInt32 stack = 1; + dgMeshBVHNode* node = NULL; + + stackPool[0] = m_rootNode; + dgFloat64 maxParam = dgFloat32 (1.2f); + + dgVector l0(p0); + dgVector l1(p1); + l0 = l0 & dgVector::m_triplexMask; + l1 = l1 & dgVector::m_triplexMask; + dgFastRayTest ray (l0, l1); + while (stack) { + stack --; + dgMeshBVHNode* const me = stackPool[stack]; + if (me && ray.BoxTest (me->m_p0, me->m_p1)) { + if (!me->m_left) { + dgAssert (!me->m_right); + dgFloat64 param = RayFaceIntersect (me, p0, p1, userData); + if (param < dgFloat64 (0.0f)) { + break; + } + if (param < maxParam) { + node = me; + maxParam = param; + } + } else { + dgAssert (me->m_left); + dgAssert (me->m_right); + stackPool[stack] = me->m_left; + stack++; + dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgMeshBVHNode*))); + + stackPool[stack] = me->m_right; + stack++; + dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgMeshBVHNode*))); + } + } + } +} + + +dgMeshEffect::dgMeshEffect () + :dgPolyhedra(NULL) + ,m_points(NULL) + ,m_attrib(NULL) + ,m_vertexBaseCount(-1) + ,m_constructionIndex(0) +{ + Init(); +} + +dgMeshEffect::dgMeshEffect(dgMemoryAllocator* const allocator) + :dgPolyhedra(allocator) + ,m_points(allocator) + ,m_attrib(allocator) + ,m_vertexBaseCount(-1) + ,m_constructionIndex(0) +{ + Init(); +} + +dgMeshEffect::dgMeshEffect (dgMemoryAllocator* const allocator, const dgMatrix& planeMatrix, dgFloat32 witdth, dgFloat32 breadth, dgInt32 material, const dgMatrix& textureMatrix0, const dgMatrix& textureMatrix1) + :dgPolyhedra(allocator) + ,m_points(allocator) + ,m_attrib(allocator) + ,m_vertexBaseCount(-1) + ,m_constructionIndex(0) +{ + dgAssert (0); + Init(); +/* + dgInt32 index[4]; + dgInt64 attrIndex[4]; + dgBigVector face[4]; + +// Init(); + + face[0] = dgBigVector (dgFloat32 (0.0f), -witdth, -breadth, dgFloat32 (0.0f)); + face[1] = dgBigVector (dgFloat32 (0.0f), witdth, -breadth, dgFloat32 (0.0f)); + face[2] = dgBigVector (dgFloat32 (0.0f), witdth, breadth, dgFloat32 (0.0f)); + face[3] = dgBigVector (dgFloat32 (0.0f), -witdth, breadth, dgFloat32 (0.0f)); + + for (dgInt32 i = 0; i < 4; i ++) { + dgBigVector uv0 (textureMatrix0.TransformVector(face[i])); + dgBigVector uv1 (textureMatrix1.TransformVector(face[i])); + + m_points[i] = planeMatrix.TransformVector(face[i]); + + m_attrib[i].m_vertex.m_x = m_points[i].m_x; + m_attrib[i].m_vertex.m_y = m_points[i].m_y; + m_attrib[i].m_vertex.m_z = m_points[i].m_z; + m_attrib[i].m_vertex.m_w = dgFloat64 (0.0f); + + m_attrib[i].m_normal_x = planeMatrix.m_front.m_x; + m_attrib[i].m_normal_y = planeMatrix.m_front.m_y; + m_attrib[i].m_normal_z = planeMatrix.m_front.m_z; + + m_attrib[i].m_u0 = uv0.m_y; + m_attrib[i].m_v0 = uv0.m_z; + + m_attrib[i].m_u1 = uv1.m_y; + m_attrib[i].m_v1 = uv1.m_z; + + m_attrib[i].m_material = material; + + index[i] = i; + attrIndex[i] = i; + } + + m_pointCount = 4; + m_atribCount = 4; + BeginFace(); + AddFace (4, index, attrIndex); + EndFace(); +*/ +} + + +dgMeshEffect::dgMeshEffect(dgPolyhedra& mesh, const dgMeshEffect& source) + :dgPolyhedra (mesh) + ,m_points(source.m_points) + ,m_attrib(source.m_attrib) + ,m_vertexBaseCount(-1) + ,m_constructionIndex(0) +{ + Init(); +} + +dgMeshEffect::dgMeshEffect(const dgMeshEffect& source) + :dgPolyhedra (source) + ,m_points(source.m_points) + ,m_attrib(source.m_attrib) + ,m_vertexBaseCount(-1) + ,m_constructionIndex(0) +{ + Init(); +} + +dgMeshEffect::dgMeshEffect(dgCollisionInstance* const collision) + :dgPolyhedra (collision->GetAllocator()) + ,m_points(collision->GetAllocator()) + ,m_attrib(collision->GetAllocator()) + ,m_vertexBaseCount(-1) + ,m_constructionIndex(0) +{ + class dgMeshEffectBuilder + { + public: + dgMeshEffectBuilder (dgMemoryAllocator* const allocator) + :m_vertex(allocator) + ,m_faceIndexCount(allocator) + ,m_brush(0) + ,m_faceCount(0) + ,m_vertexCount(0) + { + } + + static void GetShapeFromCollision (void* userData, dgInt32 vertexCount, const dgFloat32* faceVertex, dgInt32 id) + { + dgInt32 vertexIndex; + dgMeshEffectBuilder& builder = *((dgMeshEffectBuilder*)userData); + + builder.m_faceIndexCount[builder.m_faceCount] = vertexCount; + builder.m_faceCount = builder.m_faceCount + 1; + + vertexIndex = builder.m_vertexCount; + dgFloat64 brush = dgFloat64 (builder.m_brush); + for (dgInt32 i = 0; i < vertexCount; i ++) { + builder.m_vertex[vertexIndex] = dgBigVector (faceVertex[i * 3 + 0], faceVertex[i * 3 + 1], faceVertex[i * 3 + 2], brush); + vertexIndex ++; + } + + builder.m_vertexCount = vertexIndex; + } + + dgArray m_vertex; + dgArray m_faceIndexCount; + dgInt32 m_brush; + dgInt32 m_faceCount; + dgInt32 m_vertexCount; + }; + dgMeshEffectBuilder builder(GetAllocator()); + + Init(); + if (collision->IsType (dgCollision::dgCollisionCompound_RTTI)) { + dgCollisionInfo collisionInfo; + collision->GetCollisionInfo (&collisionInfo); + + dgInt32 brush = 0; + dgMatrix matrix (collisionInfo.m_offsetMatrix); + dgCollisionCompound* const compoundCollision = (dgCollisionCompound*) collision->GetChildShape(); + for (dgTree::dgTreeNode* node = compoundCollision->GetFirstNode(); node; node = compoundCollision->GetNextNode(node)) { + builder.m_brush = brush; + brush ++; + dgCollisionInstance* const childShape = compoundCollision->GetCollisionFromNode(node); + childShape->DebugCollision (matrix, (dgCollision::OnDebugCollisionMeshCallback) dgMeshEffectBuilder::GetShapeFromCollision, &builder); + } + + } else { + dgMatrix matrix (dgGetIdentityMatrix()); + collision->DebugCollision (matrix, (dgCollision::OnDebugCollisionMeshCallback) dgMeshEffectBuilder::GetShapeFromCollision, &builder); + } + + dgStackindexListBuffer (builder.m_vertexCount); + dgInt32* const indexList = &indexListBuffer[0]; + dgVertexListToIndexList (&builder.m_vertex[0].m_x, sizeof (dgBigVector), 4, builder.m_vertexCount, &indexList[0], DG_VERTEXLIST_INDEXLIST_TOL); + + dgMeshVertexFormat vertexFormat; + + vertexFormat.m_faceCount = builder.m_faceCount; + vertexFormat.m_faceIndexCount = &builder.m_faceIndexCount[0]; +// vertexFormat.m_faceMaterial = materialIndex; + + vertexFormat.m_vertex.m_data = &builder.m_vertex[0].m_x; + vertexFormat.m_vertex.m_strideInBytes = sizeof (dgBigVector); + vertexFormat.m_vertex.m_indexList = &indexList[0]; + + BuildFromIndexList (&vertexFormat); + + RepairTJoints(); + CalculateNormals(dgFloat32 (45.0f * dgDegreeToRad)); +} + +// create a convex hull +dgMeshEffect::dgMeshEffect(dgMemoryAllocator* const allocator, const dgFloat64* const vertexCloud, dgInt32 count, dgInt32 strideInByte, dgFloat64 distTol) + :dgPolyhedra(allocator) + ,m_points(allocator) + ,m_attrib(allocator) +{ + Init(); + if (count >= 4) { + dgConvexHull3d convexHull(allocator, vertexCloud, strideInByte, count, distTol); + if (convexHull.GetCount()) { + dgStack faceCountPool(convexHull.GetCount()); + dgStack vertexIndexListPool(convexHull.GetCount() * 3); + + dgInt32 index = 0; + dgMeshVertexFormat format; + format.m_faceCount = convexHull.GetCount(); + format.m_faceIndexCount = &faceCountPool[0]; + format.m_vertex.m_indexList = &vertexIndexListPool[0]; + format.m_vertex.m_data = &convexHull.GetVertexPool()[0].m_x; + format.m_vertex.m_strideInBytes = sizeof (dgBigVector); + for (dgConvexHull3d::dgListNode* faceNode = convexHull.GetFirst(); faceNode; faceNode = faceNode->GetNext()) { + dgConvexHull3DFace& face = faceNode->GetInfo(); + faceCountPool[index] = 3; + vertexIndexListPool[index * 3 + 0] = face.m_index[0]; + vertexIndexListPool[index * 3 + 1] = face.m_index[1]; + vertexIndexListPool[index * 3 + 2] = face.m_index[2]; + index++; + } + BuildFromIndexList(&format); + RepairTJoints(); + } + } +} + + +dgMeshEffect::dgMeshEffect (dgMemoryAllocator* const allocator, dgDeserialize deserialization, void* const userData) + :dgPolyhedra (allocator) + ,m_points(allocator) + ,m_attrib(allocator) + ,m_vertexBaseCount(-1) + ,m_constructionIndex(0) +{ + Init(); +dgAssert (0); +/* + dgInt32 faceCount; + deserialization (userData, &faceCount, sizeof (dgInt32)); + deserialization (userData, &m_pointCount, sizeof (dgInt32)); + deserialization (userData, &m_atribCount, sizeof (dgInt32)); + deserialization (userData, &m_atribCount, sizeof (dgInt32)); + + m_maxPointCount = m_pointCount; + m_maxAtribCount = m_atribCount; + + m_points = (dgBigVector*) GetAllocator()->MallocLow(dgInt32 (m_pointCount * sizeof(dgBigVector))); + m_attrib = (dgVertexAtribute*) GetAllocator()->MallocLow(dgInt32 (m_atribCount * sizeof(dgVertexAtribute))); + + deserialization (userData, m_points, m_pointCount * sizeof (dgBigVector)); + deserialization (userData, m_attrib, m_atribCount * sizeof (dgVertexAtribute)); + + BeginFace(); + for (dgInt32 i = 0; i < faceCount; i ++) { + dgInt32 vertexCount; + dgInt32 face[1024]; + dgInt64 attrib[1024]; + deserialization (userData, &vertexCount, sizeof (dgInt32)); + deserialization (userData, face, vertexCount * sizeof (dgInt32)); + deserialization (userData, attrib, vertexCount * sizeof (dgInt64)); + AddFace (vertexCount, face, attrib); + } + EndFace(); +*/ +} + + +dgMeshEffect::~dgMeshEffect(void) +{ +} + +void dgMeshEffect::Init() +{ +// dgAssert (); +} + +void dgMeshEffect::BeginFace() +{ + dgPolyhedra::BeginFace(); +} + +bool dgMeshEffect::EndFace () +{ + dgPolyhedra::EndFace(); + bool state = false; + for (bool hasVertexCollision = true; hasVertexCollision;) { + hasVertexCollision = false; + const dgInt32 currentCount = m_points.m_vertex.m_count; + dgStack verterCollisionBuffer (currentCount); + dgInt8* const verterCollision = &verterCollisionBuffer[0]; + memset (&verterCollision[0], 0, verterCollisionBuffer.GetSizeInBytes()); + + Iterator iter (*this); + dgInt32 mark = IncLRU(); + dgList collisionFound(GetAllocator()); + for (iter.Begin(); iter; iter ++) { + dgEdge* const edge = &iter.GetNode()->GetInfo(); + if (edge->m_mark != mark) { + if ((edge->m_incidentVertex < currentCount) && (verterCollision[edge->m_incidentVertex] == 0)) { + verterCollision[edge->m_incidentVertex] = 1; + } else { + hasVertexCollision = true; + collisionFound.Append(iter.GetNode()); + } + dgEdge* ptr = edge; + do { + ptr->m_mark = mark; + ptr = ptr->m_twin->m_next; + } while (ptr != edge); + } + } + + dgAssert (!collisionFound.GetFirst() || Sanity()); + for (dgList::dgListNode* node = collisionFound.GetFirst(); node; node = node->GetNext()) { + state = true; + dgEdge* const edge = &node->GetInfo()->GetInfo(); + + // this is a vertex collision + m_points.m_vertex.PushBack (m_points.m_vertex[edge->m_incidentVertex]); + if (m_points.m_layers.m_count) { + m_points.m_layers.PushBack (m_points.m_layers[edge->m_incidentVertex]); + } + + dgEdge* ptr = edge; + do { + if (ptr->m_incidentFace > 0) { + //m_attrib.m_pointChannel[dgInt32 (ptr->m_userData)] = m_points.m_vertex.m_count - 1; + dgInt32 index = dgInt32 (ptr->m_userData); + m_attrib.m_pointChannel.PushBack(m_points.m_vertex.m_count - 1); + if (m_attrib.m_materialChannel.m_count) { + m_attrib.m_materialChannel.PushBack(m_attrib.m_materialChannel[index]); + } + if (m_attrib.m_normalChannel.m_count) { + m_attrib.m_normalChannel.PushBack(m_attrib.m_normalChannel[index]); + } + if (m_attrib.m_binormalChannel.m_count) { + m_attrib.m_binormalChannel.PushBack(m_attrib.m_binormalChannel[index]); + } + if (m_attrib.m_colorChannel.m_count) { + m_attrib.m_colorChannel.PushBack(m_attrib.m_colorChannel[index]); + } + if (m_attrib.m_uv0Channel.m_count) { + m_attrib.m_uv0Channel.PushBack(m_attrib.m_uv0Channel[index]); + } + if (m_attrib.m_uv1Channel.m_count) { + m_attrib.m_uv1Channel.PushBack(m_attrib.m_uv1Channel[index]); + } + ptr->m_userData = m_attrib.m_pointChannel.m_count - 1; + } + + dgTreeNode* const edgeNode = GetNodeFromInfo (*ptr); + dgPairKey edgeKey (ptr->m_incidentVertex, ptr->m_twin->m_incidentVertex); + ReplaceKey (edgeNode, edgeKey.GetVal()); + + dgTreeNode* const twinNode = GetNodeFromInfo (*(ptr->m_twin)); + dgPairKey twinKey (ptr->m_twin->m_incidentVertex, ptr->m_incidentVertex); + ReplaceKey (twinNode, twinKey.GetVal()); + + ptr->m_incidentVertex = m_points.m_vertex.m_count - 1; + + ptr = ptr->m_twin->m_next; + } while (ptr != edge); + } + dgAssert (!collisionFound.GetFirst() || Sanity()); + } + + return !state; +} + + +void dgMeshEffect::Trace () const +{ +dgAssert (0); +/* + for (dgInt32 i = 0; i < m_pointCount; i ++ ) { + dgTrace (("%d-> %f %f %f\n", i, m_points[i].m_x, m_points[i].m_y, m_points[i].m_z)); + } + + + dgTreefilter(GetAllocator()); + Iterator iter (*this); + for (iter.Begin(); iter; iter ++) { + dgEdge* const edge = &iter.GetNode()->GetInfo(); + if (!filter.Find(edge)) { + dgEdge* ptr = edge; + do { + filter.Insert(edge, ptr); + dgTrace (("%d ", ptr->m_incidentVertex)); + ptr = ptr->m_next; + } while (ptr != edge); + if (edge->m_incidentFace <= 0) { + dgTrace (("open")); + } + dgTrace (("\n")); + } + } + dgTrace (("\n")); +*/ +}; + +void dgMeshEffect::FlipWinding() +{ + dgInt32 index[DG_MESH_EFFECT_POINT_SPLITED]; + dgInt64 userData[DG_MESH_EFFECT_POINT_SPLITED]; + + dgPolyhedra polyhedra(*this); + RemoveAll(); + + dgPolyhedra::BeginFace(); + dgInt32 mark = polyhedra.IncLRU(); + dgPolyhedra::Iterator iter(polyhedra); + for (iter.Begin(); iter; iter++) { + dgEdge* const face = &iter.GetNode()->GetInfo(); + if ((face->m_mark != mark) && (face->m_incidentFace > 0)) { + + dgEdge* ptr = face; + dgInt32 indexCount = 0; + do { + index[indexCount] = ptr->m_incidentVertex; + userData[indexCount] = ptr->m_userData; + ptr->m_mark = mark; + indexCount++; + ptr = ptr->m_prev; + } while (ptr != face); + AddFace(indexCount, index, userData); + } + } + dgPolyhedra::EndFace(); +} + + +void dgMeshEffect::Triangulate () +{ +/* + dgInt32 index[DG_MESH_EFFECT_POINT_SPLITED]; + dgInt64 userData[DG_MESH_EFFECT_POINT_SPLITED]; + dgPolyhedra polygon(GetAllocator()); + polygon.BeginFace(); + dgInt32 mark = IncLRU(); + dgPolyhedra::Iterator iter1 (*this); + for (iter1.Begin(); iter1; iter1 ++) { + dgEdge* const face = &iter1.GetNode()->GetInfo(); + if ((face->m_mark != mark) && (face->m_incidentFace > 0)) { + dgEdge* ptr = face; + dgInt32 indexCount = 0; + do { + index[indexCount] = dgInt32 (ptr->m_userData); + userData[indexCount] = ptr->m_incidentVertex; + ptr->m_mark = mark; + indexCount ++; + ptr = ptr->m_next; + } while (ptr != face); + polygon.AddFace(indexCount, index, userData); + } + } + polygon.EndFace(); + + dgPolyhedra leftOversOut(GetAllocator()); + polygon.Triangulate(&m_points.m_vertex[0].m_x, sizeof (dgBigVector), &leftOversOut); + dgAssert (leftOversOut.GetCount() == 0); + + SetLRU (0); + RemoveAll(); + BeginFace(); + mark = polygon.IncLRU(); + dgPolyhedra::Iterator iter (polygon); + for (iter.Begin(); iter; iter ++){ + dgEdge* const face = &iter.GetNode()->GetInfo(); + if ((face->m_mark != mark) && (face->m_incidentFace > 0)) { + dgEdge* ptr = face; + dgInt32 indexCount = 0; + do { + ptr->m_mark = mark; + index[indexCount] = dgInt32 (ptr->m_userData); + userData[indexCount] = ptr->m_incidentVertex; + indexCount ++; + ptr = ptr->m_next; + } while (ptr != face); + AddFace(indexCount, index, userData); + } + } + EndFace(); +*/ + UnpackPoints(); + dgPolyhedra leftOversOut(GetAllocator()); + dgPolyhedra::Triangulate(&m_points.m_vertex[0].m_x, sizeof (dgBigVector), &leftOversOut); + dgAssert(leftOversOut.GetCount() == 0); + + dgPolyhedra::Iterator iter(*this); + for (iter.Begin(); iter; iter++) { + dgEdge* const edge = &iter.GetNode()->GetInfo(); + edge->m_userData = (edge->m_incidentFace) > 0 ? edge->m_incidentVertex : 0; + } + PackPoints(dgFloat32(1.0e-24f)); + + RepairTJoints (); + dgAssert (Sanity ()); +} + +void dgMeshEffect::ConvertToPolygons () +{ + UnpackPoints(); + dgPolyhedra leftOversOut(GetAllocator()); + dgPolyhedra::ConvexPartition(&m_points.m_vertex[0].m_x, sizeof (dgBigVector), &leftOversOut); + dgAssert(leftOversOut.GetCount() == 0); + + dgPolyhedra::Iterator iter(*this); + for (iter.Begin(); iter; iter++) { + dgEdge* const edge = &iter.GetNode()->GetInfo(); + edge->m_userData = (edge->m_incidentFace) > 0 ? edge->m_incidentVertex : 0; + } + PackPoints(dgFloat32 (1.0e-24f)); + + RepairTJoints (); + dgAssert (Sanity ()); +} + +void dgMeshEffect::RemoveUnusedVertices(dgInt32* const vertexMapResult) +{ + dgAssert (!vertexMapResult); + UnpackAttibuteData(); + PackAttibuteData(); + UnpackPoints(); + PackPoints(dgFloat32 (1.0e-24f)); +} + + +void dgMeshEffect::ApplyTransform (const dgMatrix& matrix) +{ + matrix.TransformTriplex(&m_points.m_vertex[0].m_x, sizeof (dgBigVector), &m_points.m_vertex[0].m_x, sizeof (dgBigVector), m_points.m_vertex.m_count); + + dgMatrix invMatix(matrix.Inverse4x4()); + invMatix.m_posit = dgVector::m_wOne; + dgMatrix rotation (invMatix.Transpose4X4()); + for (dgInt32 i = 0; i < m_attrib.m_normalChannel.m_count; i ++) { + dgVector n (dgFloat32 (m_attrib.m_normalChannel[i].m_x), dgFloat32 (m_attrib.m_normalChannel[i].m_y), dgFloat32 (m_attrib.m_normalChannel[i].m_z), dgFloat32 (0.0f)); + n = rotation.RotateVector(n); + dgAssert(n.m_w == dgFloat32(0.0f)); + dgAssert (n.DotProduct(n).GetScalar() > dgFloat32 (0.0f)); + n = n.Normalize(); + m_attrib.m_normalChannel[i].m_x = n.m_x; + m_attrib.m_normalChannel[i].m_y = n.m_y; + m_attrib.m_normalChannel[i].m_z = n.m_z; + } + + for (dgInt32 i = 0; i < m_attrib.m_binormalChannel.m_count; i++) { + dgVector n(dgFloat32(m_attrib.m_binormalChannel[i].m_x), dgFloat32(m_attrib.m_binormalChannel[i].m_y), dgFloat32(m_attrib.m_binormalChannel[i].m_z), dgFloat32(0.0f)); + n = rotation.RotateVector(n); + dgAssert(n.m_w == dgFloat32(0.0f)); + dgAssert(n.DotProduct(n).GetScalar() > dgFloat32(0.0f)); + n = n.Normalize(); + m_attrib.m_binormalChannel[i].m_x = n.m_x; + m_attrib.m_binormalChannel[i].m_y = n.m_y; + m_attrib.m_binormalChannel[i].m_z = n.m_z; + } +} + +dgMatrix dgMeshEffect::CalculateOOBB (dgBigVector& size) const +{ + dgObb sphere (CalculateSphere (&m_points.m_vertex[0].m_x, sizeof (dgBigVector), NULL)); + size = sphere.m_size; + size.m_w = 0.0f; + +// dgMatrix permuation (dgGetIdentityMatrix()); +// permuation[0][0] = dgFloat32 (0.0f); +// permuation[0][1] = dgFloat32 (1.0f); +// permuation[1][1] = dgFloat32 (0.0f); +// permuation[1][2] = dgFloat32 (1.0f); +// permuation[2][2] = dgFloat32 (0.0f); +// permuation[2][0] = dgFloat32 (1.0f); +// while ((size.m_x < size.m_y) || (size.m_x < size.m_z)) { +// sphere = permuation * sphere; +// size = permuation.UnrotateVector(size); +// } + + return sphere; +} + +void dgMeshEffect::CalculateAABB (dgBigVector& minBox, dgBigVector& maxBox) const +{ + dgBigVector minP ( dgFloat64 (1.0e15f), dgFloat64 (1.0e15f), dgFloat64 (1.0e15f), dgFloat64 (0.0f)); + dgBigVector maxP (-dgFloat64 (1.0e15f), -dgFloat64 (1.0e15f), -dgFloat64 (1.0e15f), dgFloat64 (0.0f)); + + dgPolyhedra::Iterator iter (*this); + const dgBigVector* const points = &m_points.m_vertex[0]; + for (iter.Begin(); iter; iter ++){ + dgEdge* const edge = &(*iter); + const dgBigVector& p (points[edge->m_incidentVertex]); + + minP.m_x = dgMin (p.m_x, minP.m_x); + minP.m_y = dgMin (p.m_y, minP.m_y); + minP.m_z = dgMin (p.m_z, minP.m_z); + + maxP.m_x = dgMax (p.m_x, maxP.m_x); + maxP.m_y = dgMax (p.m_y, maxP.m_y); + maxP.m_z = dgMax (p.m_z, maxP.m_z); + } + + minBox = minP; + maxBox = maxP; +} + + +void dgMeshEffect::BeginBuild () +{ + m_points.Clear(); + m_attrib.Clear(); + RemoveAll(); + BeginFace(); + m_vertexBaseCount = -1; + m_constructionIndex = 0; +} + + +void dgMeshEffect::BeginBuildFace () +{ + m_constructionIndex = m_points.m_vertex.m_count; +} + +void dgMeshEffect::AddPoint (dgFloat64 x, dgFloat64 y, dgFloat64 z) +{ + m_attrib.m_pointChannel.PushBack(m_points.m_vertex.m_count); + m_points.m_vertex.PushBack(dgBigVector (QuantizeCordinade(x), QuantizeCordinade(y), QuantizeCordinade(z), dgFloat64(0.0f))); +} + +void dgMeshEffect::AddLayer(dgInt32 layer) +{ + m_points.m_layers.PushBack(layer); +} + +void dgMeshEffect::AddVertexColor(dgFloat32 x, dgFloat32 y, dgFloat32 z, dgFloat32 w) +{ + m_attrib.m_colorChannel.PushBack(dgVector (x, y, z, w)); +} + +void dgMeshEffect::AddNormal(dgFloat32 x, dgFloat32 y, dgFloat32 z) +{ + dgTriplex n; + n.m_x = x; + n.m_y = y; + n.m_z = z; + m_attrib.m_normalChannel.PushBack(n); +} + +void dgMeshEffect::AddBinormal(dgFloat32 x, dgFloat32 y, dgFloat32 z) +{ + dgTriplex n; + n.m_x = x; + n.m_y = y; + n.m_z = z; + m_attrib.m_binormalChannel.PushBack(n); +} + +void dgMeshEffect::AddUV0(dgFloat32 u, dgFloat32 v) +{ + dgAttibutFormat::dgUV uv; + uv.m_u = u; + uv.m_v = v; + m_attrib.m_uv0Channel.PushBack(uv); +} + +void dgMeshEffect::AddUV1(dgFloat32 u, dgFloat32 v) +{ + dgAttibutFormat::dgUV uv; + uv.m_u = u; + uv.m_v = v; + m_attrib.m_uv1Channel.PushBack(uv); +} + +void dgMeshEffect::AddMaterial (dgInt32 materialIndex) +{ + m_attrib.m_materialChannel.PushBack(materialIndex); +} + + +void dgMeshEffect::EndBuildFace () +{ + dgInt32 count = m_points.m_vertex.m_count - m_constructionIndex; + if (count > 3) { + dgInt32 indexList[256]; + + dgAssert(count < dgInt32(sizeof (indexList) / sizeof(indexList[0]))); + dgPolyhedra polygon(GetAllocator()); + + dgPointFormat points(GetAllocator()); + dgAttibutFormat attibutes(GetAllocator()); + for (dgInt32 i = 0; i < count; i++) { + indexList[i] = i; + + points.m_vertex.PushBack(m_points.m_vertex[m_constructionIndex + i]); + if (m_points.m_layers.m_count) { + points.m_layers.PushBack(m_points.m_layers[m_constructionIndex + i]); + } + + if (m_attrib.m_materialChannel.m_count) { + attibutes.m_materialChannel.PushBack(m_attrib.m_materialChannel[m_constructionIndex + i]); + } + + if (m_attrib.m_normalChannel.m_count) { + attibutes.m_normalChannel.PushBack(m_attrib.m_normalChannel[m_constructionIndex + i]); + } + + if (m_attrib.m_binormalChannel.m_count) { + attibutes.m_binormalChannel.PushBack(m_attrib.m_binormalChannel[m_constructionIndex + i]); + } + + if (m_attrib.m_binormalChannel.m_count) { + attibutes.m_colorChannel.PushBack(m_attrib.m_colorChannel[m_constructionIndex + i]); + } + + if (m_attrib.m_uv0Channel.m_count) { + attibutes.m_uv0Channel.PushBack(m_attrib.m_uv0Channel[m_constructionIndex + i]); + } + + if (attibutes.m_uv1Channel.m_count) { + attibutes.m_uv1Channel.PushBack(attibutes.m_uv1Channel[m_constructionIndex + i]); + } + } + + polygon.BeginFace(); + polygon.AddFace(count, indexList, NULL); + polygon.EndFace(); + polygon.Triangulate(&points.m_vertex[0].m_x, sizeof (dgBigVector), NULL); + + m_points.SetCount (m_constructionIndex); + m_attrib.SetCount (m_constructionIndex); + dgInt32 mark = polygon.IncLRU(); + dgPolyhedra::Iterator iter(polygon); + for (iter.Begin(); iter; iter++) { + dgEdge* const edge = &iter.GetNode()->GetInfo(); + if ((edge->m_incidentFace > 0) && (edge->m_mark < mark)) { + dgInt32 i0 = edge->m_incidentVertex; + dgInt32 i1 = edge->m_next->m_incidentVertex; + dgInt32 i2 = edge->m_next->m_next->m_incidentVertex; + edge->m_mark = mark; + edge->m_next->m_mark = mark; + edge->m_next->m_next->m_mark = mark; + + const dgBigVector& p0 = points.m_vertex[i0]; + const dgBigVector& p1 = points.m_vertex[i1]; + const dgBigVector& p2 = points.m_vertex[i2]; + + dgBigVector e1(p1 - p0); + dgBigVector e2(p2 - p0); + dgBigVector n(e1.CrossProduct(e2)); + + dgAssert (e1.m_w == dgFloat32 (0.0f)); + dgAssert (e2.m_w == dgFloat32 (0.0f)); + dgAssert (n.m_w == dgFloat32 (0.0f)); + + dgFloat64 mag3 = n.DotProduct(n).GetScalar(); + if (mag3 >= dgFloat64(DG_MESH_EFFECT_PRECISION_SCALE_INV * DG_MESH_EFFECT_PRECISION_SCALE_INV)) { + dgInt32 index[] = {i0, i1, i2}; + for (dgInt32 i = 0; i < 3; i ++) { + dgInt32 j = index[i]; + AddPoint(points.m_vertex[j].m_x, points.m_vertex[j].m_y, points.m_vertex[j].m_z); + if (points.m_layers.m_count) { + AddLayer(points.m_layers[j]); + } + + if (attibutes.m_materialChannel.m_count) { + AddMaterial(attibutes.m_materialChannel[j]); + } + + if (attibutes.m_normalChannel.m_count) { + AddNormal(attibutes.m_normalChannel[j].m_x, attibutes.m_normalChannel[j].m_y, attibutes.m_normalChannel[j].m_z); + } + + if (attibutes.m_binormalChannel.m_count) { + AddBinormal(attibutes.m_binormalChannel[j].m_x, attibutes.m_binormalChannel[j].m_y, attibutes.m_binormalChannel[j].m_z); + } + + if (attibutes.m_colorChannel.m_count) { + AddVertexColor(attibutes.m_colorChannel[j].m_x, attibutes.m_colorChannel[j].m_y, attibutes.m_colorChannel[j].m_z, attibutes.m_colorChannel[j].m_w); + } + + if (attibutes.m_uv0Channel.m_count) { + AddUV0(attibutes.m_uv0Channel[j].m_u, attibutes.m_uv0Channel[j].m_v); + } + + if (attibutes.m_uv1Channel.m_count) { + AddUV1(attibutes.m_uv1Channel[j].m_u, attibutes.m_uv1Channel[j].m_v); + } + } + } + } + } + + } else { + const dgBigVector& p0 = m_points.m_vertex[m_constructionIndex + 0]; + const dgBigVector& p1 = m_points.m_vertex[m_constructionIndex + 1]; + const dgBigVector& p2 = m_points.m_vertex[m_constructionIndex + 2]; + + dgBigVector e1(p1 - p0); + dgBigVector e2(p2 - p0); + dgBigVector n(e1.CrossProduct(e2)); + dgAssert(e1.m_w == dgFloat32(0.0f)); + dgAssert(e2.m_w == dgFloat32(0.0f)); + dgAssert(n.m_w == dgFloat32(0.0f)); + + dgFloat64 mag3 = n.DotProduct(n).GetScalar(); + if (mag3 < dgFloat64(DG_MESH_EFFECT_PRECISION_SCALE_INV * DG_MESH_EFFECT_PRECISION_SCALE_INV)) { + m_attrib.SetCount (m_constructionIndex); + m_points.SetCount (m_constructionIndex); + } + } +} + + +void dgMeshEffect::UnpackAttibuteData() +{ + dgAttibutFormat attibutes(m_attrib); + m_attrib.Clear(); + + Iterator iter(*this); + dgInt32 attributeCount = 0; + const dgInt32 lru = IncLRU(); + for (iter.Begin(); iter; iter++) { + dgEdge* const edge = &iter.GetNode()->GetInfo(); + if ((edge->m_incidentFace > 0) && (edge->m_mark != lru)) { + dgEdge* ptr = edge; + + ptr = edge; + do { + ptr->m_mark = lru; + m_attrib.m_pointChannel.PushBack(ptr->m_incidentVertex); + + if (attibutes.m_materialChannel.m_count) { + m_attrib.m_materialChannel.PushBack(attibutes.m_materialChannel[dgInt32(ptr->m_userData)]); + } + + if (attibutes.m_normalChannel.m_count) { + m_attrib.m_normalChannel.PushBack(attibutes.m_normalChannel[dgInt32(ptr->m_userData)]); + } + + if (attibutes.m_binormalChannel.m_count) { + m_attrib.m_binormalChannel.PushBack(attibutes.m_binormalChannel[dgInt32(ptr->m_userData)]); + } + + if (attibutes.m_colorChannel.m_count) { + m_attrib.m_colorChannel.PushBack(attibutes.m_colorChannel[dgInt32(ptr->m_userData)]); + } + + if (attibutes.m_uv0Channel.m_count) { + m_attrib.m_uv0Channel.PushBack(attibutes.m_uv0Channel[dgInt32(ptr->m_userData)]); + } + + if (attibutes.m_uv1Channel.m_count) { + m_attrib.m_uv1Channel.PushBack(attibutes.m_uv1Channel[dgInt32(ptr->m_userData)]); + } + + ptr->m_userData = attributeCount; + attributeCount++; + ptr = ptr->m_next; + } while (ptr != edge); + } + } + dgAssert(m_attrib.m_pointChannel.m_count == attributeCount); +} + +void dgMeshEffect::PackAttibuteData() +{ + dgStackattrIndexBuffer(m_attrib.m_pointChannel.m_count); + dgInt32* const attrIndexMap = &attrIndexBuffer[0]; + m_attrib.CompressData(m_points, &attrIndexMap[0]); + + Iterator iter(*this); + for (iter.Begin(); iter; iter++) { + dgEdge* const edge = &(*iter); + if (edge->m_incidentFace > 0) { + edge->m_userData = attrIndexMap[edge->m_userData]; + } + } + + memset (attrIndexMap, -1, sizeof (dgInt32) * m_attrib.m_pointChannel.m_count); + dgAttibutFormat tmpFormat (m_attrib); + m_attrib.Clear(); + + dgInt32 remapIndex = 0; + for (iter.Begin(); iter; iter++) { + dgEdge* const edge = &(*iter); + if (edge->m_incidentFace > 0) { + dgInt32 index = dgInt32(edge->m_userData); + if (attrIndexMap[edge->m_userData] == -1) { + attrIndexMap[index] = remapIndex; + remapIndex ++; + + m_attrib.m_pointChannel.PushBack(tmpFormat.m_pointChannel[index]); + if (tmpFormat.m_materialChannel.m_count) { + m_attrib.m_materialChannel.PushBack(tmpFormat.m_materialChannel[index]); + } + if (tmpFormat.m_normalChannel.m_count) { + m_attrib.m_normalChannel.PushBack(tmpFormat.m_normalChannel[index]); + } + if (tmpFormat.m_binormalChannel.m_count) { + m_attrib.m_binormalChannel.PushBack(tmpFormat.m_binormalChannel[index]); + } + if (tmpFormat.m_uv0Channel.m_count) { + m_attrib.m_uv0Channel.PushBack(tmpFormat.m_uv0Channel[index]); + } + if (tmpFormat.m_uv1Channel.m_count) { + m_attrib.m_uv1Channel.PushBack(tmpFormat.m_uv1Channel[index]); + } + if (tmpFormat.m_colorChannel.m_count) { + m_attrib.m_colorChannel.PushBack(tmpFormat.m_colorChannel[index]); + } + } + edge->m_userData = attrIndexMap[index]; + } + } +} + +void dgMeshEffect::PackPoints (dgFloat64 tol) +{ + dgStackvertexIndexMapBuffer(m_points.m_vertex.m_count); + dgInt32* const vertexIndexMap = &vertexIndexMapBuffer[0]; + m_points.CompressData(&vertexIndexMap[0]); + + dgInt32 index[DG_MESH_EFFECT_POINT_SPLITED]; + dgInt64 userData[DG_MESH_EFFECT_POINT_SPLITED]; + dgPolyhedra polygon(GetAllocator()); + SwapInfo(polygon); + dgAssert(GetCount() == 0); + + BeginFace(); + const dgInt32 mark = IncLRU(); + dgPolyhedra::Iterator iter(polygon); + for (iter.Begin(); iter; iter++) { + dgEdge* const edge = &(*iter); + if ((edge->m_mark != mark) && (edge->m_incidentFace > 0)) { + dgEdge* ptr = edge; + dgInt32 indexCount = 0; + do { + ptr->m_mark = mark; + index[indexCount] = vertexIndexMap[ptr->m_incidentVertex]; + m_attrib.m_pointChannel[dgInt32 (ptr->m_userData)] = vertexIndexMap[ptr->m_incidentVertex]; + userData[indexCount] = ptr->m_userData; + + indexCount++; + ptr = ptr->m_next; + } while (ptr != edge); + dgEdge* const face = AddFace(indexCount, index, userData); + if (!face) { + dgTrace (("skiping degeneraded face\n")); + //dgAssert (0); + } + } + } + EndFace(); +} + +void dgMeshEffect::UnpackPoints() +{ + do { + dgPointFormat points(m_points); + m_points.Clear(); + for (dgInt32 i = 0; i < m_attrib.m_pointChannel.m_count; i++) { + dgInt32 index = m_attrib.m_pointChannel[i]; + m_points.m_vertex.PushBack(points.m_vertex[index]); + if (points.m_layers.m_count) { + m_points.m_layers.PushBack(points.m_layers[index]); + } + + m_attrib.m_pointChannel[i] = i; + } + + dgInt32 index[DG_MESH_EFFECT_POINT_SPLITED]; + dgInt64 userData[DG_MESH_EFFECT_POINT_SPLITED]; + dgPolyhedra polygon(GetAllocator()); + SwapInfo (polygon); + dgAssert (GetCount() == 0); + BeginFace(); + const dgInt32 mark = IncLRU(); + dgPolyhedra::Iterator iter(polygon); + for (iter.Begin(); iter; iter++) { + dgEdge* const face = &(*iter); + if ((face->m_mark != mark) && (face->m_incidentFace > 0)) { + dgEdge* ptr = face; + dgInt32 indexCount = 0; + do { + ptr->m_mark = mark; + index[indexCount] = dgInt32(ptr->m_userData); + userData[indexCount] = ptr->m_userData; + indexCount++; + ptr = ptr->m_next; + } while (ptr != face); + AddFace(indexCount, index, userData); + } + } + } while (!EndFace()); + + dgAssert(m_points.m_vertex.m_count == m_attrib.m_pointChannel.m_count); +#ifdef _DEBUG + for (dgInt32 i = 0; i < m_attrib.m_pointChannel.m_count; i++) { + dgAssert(m_attrib.m_pointChannel[i] == i); + } +#endif + +} + + +void dgMeshEffect::EndBuild (dgFloat64 tol, bool fixTjoint) +{ +#ifdef _DEBUG + for (dgInt32 i = 0; i < m_points.m_vertex.m_count; i += 3) { + dgBigVector p0 (m_points.m_vertex[i + 0]); + dgBigVector p1 (m_points.m_vertex[i + 1]); + dgBigVector p2 (m_points.m_vertex[i + 2]); + dgBigVector e1 (p1 - p0); + dgBigVector e2 (p2 - p0); + dgBigVector n (e1.CrossProduct(e2)); + + dgAssert(e1.m_w == dgFloat32(0.0f)); + dgAssert(e2.m_w == dgFloat32(0.0f)); + dgAssert(n.m_w == dgFloat32(0.0f)); + dgFloat64 mag2 = n.DotProduct(n).GetScalar(); + dgAssert (mag2 > dgFloat32 (0.0f)); + } +#endif + + dgInt32 triangCount = m_points.m_vertex.m_count / 3; + const dgInt32* const indexList = &m_attrib.m_pointChannel[0]; + for (dgInt32 i = 0; i < triangCount; i ++) { + dgInt32 index[3]; + dgInt64 userdata[3]; + + index[0] = indexList[i * 3 + 0]; + index[1] = indexList[i * 3 + 1]; + index[2] = indexList[i * 3 + 2]; + + dgBigVector e1 (m_points.m_vertex[index[1]] - m_points.m_vertex[index[0]]); + dgBigVector e2 (m_points.m_vertex[index[2]] - m_points.m_vertex[index[0]]); + dgBigVector n (e1.CrossProduct(e2)); + + dgAssert(e1.m_w == dgFloat32(0.0f)); + dgAssert(e2.m_w == dgFloat32(0.0f)); + dgAssert(n.m_w == dgFloat32(0.0f)); + dgFloat64 mag2 = n.DotProduct(n).GetScalar(); + if (mag2 > dgFloat64 (1.0e-12f)) { + userdata[0] = i * 3 + 0; + userdata[1] = i * 3 + 1; + userdata[2] = i * 3 + 2; + dgEdge* const edge = AddFace (3, index, userdata); + if (!edge) { + dgAssert (0); +/* + //dgAssert ((m_pointCount + 3) <= m_maxPointCount); + m_points[m_pointCount + 0] = m_points[index[0]]; + m_points[m_pointCount + 1] = m_points[index[1]]; + m_points[m_pointCount + 2] = m_points[index[2]]; + + index[0] = m_pointCount + 0; + index[1] = m_pointCount + 1; + index[2] = m_pointCount + 2; + + m_pointCount += 3; + + #ifdef _DEBUG + dgEdge* test = AddFace (3, index, userdata); + dgAssert (test); + #else + AddFace (3, index, userdata); + #endif +*/ + } + } + } + EndFace(); + + PackAttibuteData (); + PackPoints (tol); + + if (fixTjoint) { + RepairTJoints (); + } + +#ifdef _DEBUG + dgPolyhedra::Iterator iter (*this); + for (iter.Begin(); iter; iter ++){ + dgEdge* const face = &(*iter); + if (face->m_incidentFace > 0) { + dgBigVector p0 (m_points.m_vertex[face->m_incidentVertex]); + dgBigVector p1 (m_points.m_vertex[face->m_next->m_incidentVertex]); + dgBigVector p2 (m_points.m_vertex[face->m_next->m_next->m_incidentVertex]); + dgBigVector e1 (p1 - p0); + dgBigVector e2 (p2 - p0); + dgBigVector n (e1.CrossProduct(e2)); + + dgAssert(e1.m_w == dgFloat32(0.0f)); + dgAssert(e2.m_w == dgFloat32(0.0f)); + dgAssert(n.m_w == dgFloat32(0.0f)); + dgFloat64 mag2 = n.DotProduct(n).GetScalar(); + dgAssert (mag2 >= dgFloat32 (0.0f)); + } + } +#endif +} + + +void dgMeshEffect::OptimizePoints() +{ + +} + +void dgMeshEffect::OptimizeAttibutes() +{ + UnpackAttibuteData (); + PackAttibuteData(); +} + +void dgMeshEffect::BuildFromIndexList(const dgMeshVertexFormat* const format) +{ + BeginBuild(); + dgAssert (format->m_vertex.m_data); + dgAssert (format->m_vertex.m_indexList); + dgAssert (format->m_vertex.m_strideInBytes); + + // calculate vertex Count + dgInt32 vertexCount = 0; + dgInt32 maxAttribCount = 0; + for (dgInt32 j = 0; j < format->m_faceCount; j++) { + dgInt32 count = format->m_faceIndexCount[j]; + for (dgInt32 i = 0; i < count; i++) { + vertexCount = dgMax(vertexCount, format->m_vertex.m_indexList[maxAttribCount + i] + 1); + } + maxAttribCount += count; + } + m_vertexBaseCount = vertexCount; + + dgInt32 layerIndex = 0; + dgInt32 vertexStride = dgInt32(format->m_vertex.m_strideInBytes / sizeof (dgFloat64)); + const dgFloat64* const vertex = format->m_vertex.m_data; + for (dgInt32 i = 0; i < vertexCount; i++) { + dgInt32 index = i * vertexStride; + m_points.m_layers.PushBack(layerIndex); + dgBigVector p(vertex[index + 0], vertex[index + 1], vertex[index + 2], dgFloat64(0.0f)); + m_points.m_vertex.PushBack(p); + } + + bool pendingFaces = true; + dgInt32 layerBase = 0; + dgInt32 attributeCount = 0; + + dgInt32 normalStride = dgInt32(format->m_normal.m_strideInBytes / sizeof (dgFloat32)); + dgInt32 binormalStride = dgInt32(format->m_binormal.m_strideInBytes / sizeof (dgFloat32)); + dgInt32 uv0Stride = dgInt32(format->m_uv0.m_strideInBytes / sizeof (dgFloat32)); + dgInt32 uv1Stride = dgInt32(format->m_uv1.m_strideInBytes / sizeof (dgFloat32)); + dgInt32 vertexColorStride = dgInt32(format->m_vertexColor.m_strideInBytes / sizeof (dgFloat32)); + + dgStack faceMark(format->m_faceCount); + memset(&faceMark[0], 0, faceMark.GetSizeInBytes()); + const dgInt32* const vertexIndex = format->m_vertex.m_indexList; + + while (pendingFaces) { + dgInt32 acc = 0; + pendingFaces = false; + dgInt32 vertexBank = layerIndex * vertexCount; + for (dgInt32 j = 0; j < format->m_faceCount; j++) { + dgInt32 indexCount = format->m_faceIndexCount[j]; + + if (indexCount > 0) { + dgInt32 index[256]; + dgInt64 userdata[256]; + dgAssert(indexCount >= 3); + dgAssert(indexCount < dgInt32(sizeof (index) / sizeof (index[0]))); + + if (!faceMark[j]) { + for (int i = 0; i < indexCount; i++) { + dgInt32 k = attributeCount + i; + userdata[i] = k; + index[i] = vertexIndex[acc + i] + vertexBank; + } + + + dgEdge* const edge = AddFace(indexCount, index, userdata); + if (edge) { + faceMark[j] = 1; + for (int i = 0; i < indexCount; i++) { + m_attrib.m_pointChannel.PushBack(index[i]); + } + + if (format->m_faceMaterial) { + dgInt32 materialIndex = format->m_faceMaterial[j]; + for (int i = 0; i < indexCount; i++) { + m_attrib.m_materialChannel.PushBack(materialIndex); + } + } + + if (format->m_normal.m_data) { + dgTriplex normal; + for (int i = 0; i < indexCount; i++) { + dgInt32 k = attributeCount + i; + dgInt32 m = format->m_normal.m_indexList[k] * normalStride; + normal.m_x = format->m_normal.m_data[m + 0]; + normal.m_y = format->m_normal.m_data[m + 1]; + normal.m_z = format->m_normal.m_data[m + 2]; + m_attrib.m_normalChannel.PushBack(normal); + } + } + + if (format->m_binormal.m_data) { + dgTriplex normal; + for (int i = 0; i < indexCount; i++) { + dgInt32 k = attributeCount + i; + dgInt32 m = format->m_binormal.m_indexList[k] * binormalStride; + normal.m_x = format->m_binormal.m_data[m + 0]; + normal.m_y = format->m_binormal.m_data[m + 1]; + normal.m_z = format->m_binormal.m_data[m + 2]; + m_attrib.m_binormalChannel.PushBack(normal); + } + } + + if (format->m_vertexColor.m_data) { + for (int i = 0; i < indexCount; i++) { + dgInt32 k = attributeCount + i; + dgInt32 m = format->m_vertexColor.m_indexList[k] * vertexColorStride; + dgVector color(format->m_vertexColor.m_data[m + 0], format->m_vertexColor.m_data[m + 1], format->m_vertexColor.m_data[m + 2], format->m_vertexColor.m_data[m + 3]); + m_attrib.m_colorChannel.PushBack(color); + } + } + + if (format->m_uv0.m_data) { + dgAttibutFormat::dgUV uv; + for (int i = 0; i < indexCount; i++) { + dgInt32 k = attributeCount + i; + dgInt32 m = format->m_uv0.m_indexList[k] * uv0Stride; + uv.m_u = format->m_uv0.m_data[m + 0]; + uv.m_v = format->m_uv0.m_data[m + 1]; + m_attrib.m_uv0Channel.PushBack(uv); + } + } + + if (format->m_uv1.m_data) { + dgAttibutFormat::dgUV uv; + for (int i = 0; i < indexCount; i++) { + dgInt32 k = attributeCount + i; + dgInt32 m = format->m_uv1.m_indexList[k] * uv1Stride; + uv.m_u = format->m_uv1.m_data[m + 0]; + uv.m_v = format->m_uv1.m_data[m + 1]; + m_attrib.m_uv1Channel.PushBack(uv); + } + } + attributeCount += indexCount; + + } else { + // check if the face is not degenerated + bool degeneratedFace = false; + for (int i = 0; i < indexCount - 1; i++) { + for (int k = i + 1; k < indexCount; k++) { + if (index[i] == index[k]) { + degeneratedFace = true; + } + } + } + if (degeneratedFace) { + faceMark[j] = 1; + } else { + pendingFaces = true; + } + } + } + acc += indexCount; + } + } + + if (pendingFaces) { + //dgAssert (0); + layerIndex++; + layerBase += vertexCount; + for (dgInt32 i = 0; i < vertexCount; i++) { + m_points.m_layers.PushBack(layerIndex); + dgInt32 index = i * vertexStride; + dgBigVector p (vertex[index + 0], vertex[index + 1], vertex[index + 2], vertex[index + 3]); + m_points.m_vertex.PushBack(p); + } + } + } + + dgAssert (m_points.m_vertex.m_count == vertexCount * (layerIndex + 1)); + dgAssert (m_attrib.m_pointChannel.m_count == attributeCount); + + EndFace(); + PackAttibuteData(); +} + +dgInt32 dgMeshEffect::GetTotalFaceCount() const +{ + return GetFaceCount(); +} + +dgInt32 dgMeshEffect::GetTotalIndexCount() const +{ + Iterator iter (*this); + dgInt32 count = 0; + dgInt32 mark = IncLRU(); + for (iter.Begin(); iter; iter ++) { + dgEdge* const edge = &(*iter); + if (edge->m_mark == mark) { + continue; + } + + if (edge->m_incidentFace < 0) { + continue; + } + + dgEdge* ptr = edge; + do { + count ++; + ptr->m_mark = mark; + ptr = ptr->m_next; + } while (ptr != edge); + } + return count; +} + +void dgMeshEffect::GetFaces (dgInt32* const facesIndex, dgInt32* const materials, void** const faceNodeList) const +{ + Iterator iter (*this); + + dgInt32 faces = 0; + dgInt32 indexCount = 0; + dgInt32 mark = IncLRU(); + for (iter.Begin(); iter; iter ++) { + dgEdge* const edge = &(*iter); + if (edge->m_mark == mark) { + continue; + } + + if (edge->m_incidentFace < 0) { + continue; + } + + dgInt32 faceCount = 0; + dgEdge* ptr = edge; + do { +// indexList[indexCount] = dgInt32 (ptr->m_userData); + faceNodeList[indexCount] = GetNodeFromInfo (*ptr); + indexCount ++; + faceCount ++; + ptr->m_mark = mark; + ptr = ptr->m_next; + } while (ptr != edge); + + facesIndex[faces] = faceCount; + //materials[faces] = dgFastInt(m_attrib[dgInt32 (edge->m_userData)].m_material); + materials[faces] = m_attrib.m_materialChannel.m_count ? m_attrib.m_materialChannel[dgInt32(edge->m_userData)] : 0; + faces ++; + } +} + +void* dgMeshEffect::GetFirstVertex () const +{ + Iterator iter (*this); + iter.Begin(); + + dgTreeNode* node = NULL; + if (iter) { + dgInt32 mark = IncLRU(); + node = iter.GetNode(); + + dgEdge* const edge = &node->GetInfo(); + dgEdge* ptr = edge; + do { + ptr->m_mark = mark; + ptr = ptr->m_twin->m_next; + } while (ptr != edge); + } + return node; +} + +void* dgMeshEffect::GetNextVertex (const void* const vertex) const +{ + dgTreeNode* const node0 = (dgTreeNode*) vertex; + dgInt32 mark = node0->GetInfo().m_mark; + + Iterator iter (*this); + iter.Set (node0); + for (iter ++; iter; iter ++) { + dgTreeNode* node = iter.GetNode(); + if (node->GetInfo().m_mark != mark) { + dgEdge* const edge = &node->GetInfo(); + dgEdge* ptr = edge; + do { + ptr->m_mark = mark; + ptr = ptr->m_twin->m_next; + } while (ptr != edge); + return node; + } + } + return NULL; +} + +dgInt32 dgMeshEffect::GetVertexIndex (const void* const vertex) const +{ + dgTreeNode* const node = (dgTreeNode*) vertex; + dgEdge* const edge = &node->GetInfo(); + return edge->m_incidentVertex; +} + +void* dgMeshEffect::GetFirstPoint () const +{ + Iterator iter (*this); + for (iter.Begin(); iter; iter ++) { + dgTreeNode* const node = iter.GetNode(); + dgEdge* const edge = &node->GetInfo(); + if (edge->m_incidentFace > 0) { + return node; + } + } + return NULL; +} + +void* dgMeshEffect::GetNextPoint (const void* const point) const +{ + Iterator iter (*this); + iter.Set ((dgTreeNode*) point); + for (iter ++; iter; iter ++) { + dgTreeNode* const node = iter.GetNode(); + dgEdge* const edge = &node->GetInfo(); + if (edge->m_incidentFace > 0) { + return node; + } + } + return NULL; +} + +dgInt32 dgMeshEffect::GetPointIndex (const void* const point) const +{ + dgTreeNode* const node = (dgTreeNode*) point; + dgEdge* const edge = &node->GetInfo(); + return int (edge->m_userData); +} + + +dgInt32 dgMeshEffect::GetVertexIndexFromPoint (const void* const point) const +{ + return GetVertexIndex (point); +} + +dgEdge* dgMeshEffect::SpliteFace (dgInt32 v0, dgInt32 v1) +{ + if (!FindEdge(v0, v1)) { + dgPolyhedra::dgPairKey key (v0, 0); + dgTreeNode* const node = FindGreaterEqual(key.GetVal()); + if (node) { + dgEdge* const edge = &node->GetInfo(); + dgEdge* edge0 = edge; + do { + if (edge0->m_incidentFace > 0) { + for (dgEdge* edge1 = edge0->m_next->m_next; edge1 != edge0->m_prev; edge1 = edge1->m_next) { + if (edge1->m_incidentVertex == v1) { + return ConnectVertex (edge0, edge1); + } + }; + } + edge0 = edge0->m_twin->m_next; + } while (edge0 != edge); + } + } + return NULL; +} + +const dgEdge* dgMeshEffect::GetPolyhedraEdgeFromNode(const void* const edge) const +{ + dgTreeNode* const node = (dgTreeNode*)edge; + return &node->GetInfo(); +} + +void* dgMeshEffect::GetFirstEdge () const +{ + Iterator iter (*this); + iter.Begin(); + + dgTreeNode* node = NULL; + if (iter) { + dgInt32 mark = IncLRU(); + + node = iter.GetNode(); + + dgEdge* const edge = &node->GetInfo(); + edge->m_mark = mark; + edge->m_twin->m_mark = mark; + } + return node; +} + +void* dgMeshEffect::GetNextEdge (const void* const edge) const +{ + dgTreeNode* const node0 = (dgTreeNode*) edge; + dgInt32 mark = node0->GetInfo().m_mark; + + Iterator iter (*this); + iter.Set (node0); + for (iter ++; iter; iter ++) { + dgTreeNode* const node = iter.GetNode(); + if (node->GetInfo().m_mark != mark) { + node->GetInfo().m_mark = mark; + node->GetInfo().m_twin->m_mark = mark; + return node; + } + } + return NULL; +} + + +void dgMeshEffect::GetEdgeIndex (const void* const edge, dgInt32& v0, dgInt32& v1) const +{ + dgTreeNode* const node = (dgTreeNode*) edge; + v0 = node->GetInfo().m_incidentVertex; + v1 = node->GetInfo().m_twin->m_incidentVertex; +} + +//void* dgMeshEffect::FindEdge (dgInt32 v0, dgInt32 v1) const +//{ +// return FindEdgeNode(v0, v1); +//} + +//void dgMeshEffect::GetEdgeAttributeIndex (const void* edge, dgInt32& v0, dgInt32& v1) const +//{ +// dgTreeNode* node = (dgTreeNode*) edge; +// v0 = int (node->GetInfo().m_userData); +// v1 = int (node->GetInfo().m_twin->m_userData); +//} + + +void* dgMeshEffect::GetFirstFace () const +{ + Iterator iter (*this); + iter.Begin(); + + dgTreeNode* node = NULL; + if (iter) { + dgInt32 mark = IncLRU(); + node = iter.GetNode(); + + dgEdge* const edge = &node->GetInfo(); + dgEdge* ptr = edge; + do { + ptr->m_mark = mark; + ptr = ptr->m_next; + } while (ptr != edge); + } + + return node; +} + +void* dgMeshEffect::GetNextFace (const void* const face) const +{ + dgTreeNode* const node0 = (dgTreeNode*) face; + dgInt32 mark = node0->GetInfo().m_mark; + + Iterator iter (*this); + iter.Set (node0); + for (iter ++; iter; iter ++) { + dgTreeNode* node = iter.GetNode(); + if (node->GetInfo().m_mark != mark) { + dgEdge* const edge = &node->GetInfo(); + dgEdge* ptr = edge; + do { + ptr->m_mark = mark; + ptr = ptr->m_next; + } while (ptr != edge); + return node; + } + } + return NULL; +} + + +dgInt32 dgMeshEffect::IsFaceOpen (const void* const face) const +{ + dgTreeNode* const node = (dgTreeNode*) face; + dgEdge* const edge = &node->GetInfo(); + return (edge->m_incidentFace > 0) ? 0 : 1; +} + +dgInt32 dgMeshEffect::GetFaceMaterial (const void* const face) const +{ + dgTreeNode* const node = (dgTreeNode*) face; + dgEdge* const edge = &node->GetInfo(); + return dgInt32 (m_attrib.m_materialChannel.m_count ? m_attrib.m_materialChannel[dgInt32 (edge->m_userData)] : 0); +} + +void dgMeshEffect::SetFaceMaterial (const void* const face, int mateialID) +{ + if (m_attrib.m_materialChannel.m_count) { + dgTreeNode* const node = (dgTreeNode*) face; + dgEdge* const edge = &node->GetInfo(); + if (edge->m_incidentFace > 0) { + dgEdge* ptr = edge; + do { + //dgVertexAtribute* const attrib = &m_attrib[ptr->m_userData]; + //attrib->m_material = dgFloat64 (mateialID); + m_attrib.m_materialChannel[dgInt32 (edge->m_userData)] = mateialID; + ptr = ptr->m_next; + } while (ptr != edge) ; + } + } +} + +dgInt32 dgMeshEffect::GetFaceIndexCount (const void* const face) const +{ + int count = 0; + dgTreeNode* node = (dgTreeNode*) face; + dgEdge* const edge = &node->GetInfo(); + dgEdge* ptr = edge; + do { + count ++; + ptr = ptr->m_next; + } while (ptr != edge); + return count; +} + +void dgMeshEffect::GetFaceIndex (const void* const face, dgInt32* const indices) const +{ + int count = 0; + dgTreeNode* node = (dgTreeNode*) face; + dgEdge* const edge = &node->GetInfo(); + dgEdge* ptr = edge; + do { + indices[count] = ptr->m_incidentVertex; + count ++; + ptr = ptr->m_next; + } while (ptr != edge); +} + +void dgMeshEffect::GetFaceAttributeIndex (const void* const face, dgInt32* const indices) const +{ + int count = 0; + dgTreeNode* node = (dgTreeNode*) face; + dgEdge* const edge = &node->GetInfo(); + dgEdge* ptr = edge; + do { + indices[count] = int (ptr->m_userData); + count ++; + ptr = ptr->m_next; + } while (ptr != edge); +} + + +dgBigVector dgMeshEffect::CalculateFaceNormal (const void* const face) const +{ + dgTreeNode* const node = (dgTreeNode*) face; + dgEdge* const faceEdge = &node->GetInfo(); + dgBigVector normal (FaceNormal (faceEdge, &m_points.m_vertex[0].m_x, sizeof (dgBigVector))); + dgAssert (normal.m_w == dgFloat32 (0.0f)); + //normal = normal.Scale (1.0f / sqrt (normal.DotProduct3(normal))); + normal = normal.Normalize(); + return normal; +} + +/* +dgInt32 GetTotalFaceCount() const; +{ + dgInt32 mark; + dgInt32 count; + dgInt32 materialCount; + dgInt32 materials[256]; + dgInt32 streamIndexMap[256]; + dgIndexArray* array; + + count = 0; + materialCount = 0; + + array = (dgIndexArray*) GetAllocator()->MallocLow (4 * sizeof (dgInt32) * GetCount() + sizeof (dgIndexArray) + 2048); + array->m_indexList = (dgInt32*)&array[1]; + + mark = IncLRU(); + dgPolyhedra::Iterator iter (*this); + memset(streamIndexMap, 0, sizeof (streamIndexMap)); + for(iter.Begin(); iter; iter ++){ + + dgEdge* const edge; + edge = &(*iter); + if ((edge->m_incidentFace >= 0) && (edge->m_mark != mark)) { + dgEdge* ptr; + dgInt32 hashValue; + dgInt32 index0; + dgInt32 index1; + + ptr = edge; + ptr->m_mark = mark; + index0 = dgInt32 (ptr->m_userData); + + ptr = ptr->m_next; + ptr->m_mark = mark; + index1 = dgInt32 (ptr->m_userData); + + ptr = ptr->m_next; + do { + ptr->m_mark = mark; + + array->m_indexList[count * 4 + 0] = index0; + array->m_indexList[count * 4 + 1] = index1; + array->m_indexList[count * 4 + 2] = dgInt32 (ptr->m_userData); + array->m_indexList[count * 4 + 3] = m_attrib[dgInt32 (edge->m_userData)].m_material; + index1 = dgInt32 (ptr->m_userData); + + hashValue = array->m_indexList[count * 4 + 3] & 0xff; + streamIndexMap[hashValue] ++; + materials[hashValue] = array->m_indexList[count * 4 + 3]; + count ++; + + ptr = ptr->m_next; + } while (ptr != edge); + } + } +} +*/ + +bool dgMeshEffect::HasNormalChannel() const +{ + return m_attrib.m_normalChannel.m_count != 0; +} + +bool dgMeshEffect::HasBinormalChannel() const +{ + return m_attrib.m_binormalChannel.m_count != 0; +} + +bool dgMeshEffect::HasUV0Channel() const +{ + return m_attrib.m_uv0Channel.m_count != 0; +} + +bool dgMeshEffect::HasUV1Channel() const +{ + return m_attrib.m_uv1Channel.m_count != 0; +} + +bool dgMeshEffect::HasVertexColorChannel() const +{ + return m_attrib.m_colorChannel.m_count != 0; +} + +void dgMeshEffect::GetVertexChannel64(dgInt32 strideInByte, dgFloat64* const bufferOut) const +{ + dgInt32 stride = strideInByte / sizeof (dgFloat64); + for (dgInt32 i = 0; i < m_attrib.m_pointChannel.m_count; i ++) { + const dgInt32 j = i * stride; + const dgInt32 index = m_attrib.m_pointChannel[i]; + bufferOut[j + 0] = m_points.m_vertex[index].m_x; + bufferOut[j + 1] = m_points.m_vertex[index].m_y; + bufferOut[j + 2] = m_points.m_vertex[index].m_z; + } +} + +void dgMeshEffect::GetVertexChannel(dgInt32 strideInByte, dgFloat32* const bufferOut) const +{ + dgInt32 stride = strideInByte / sizeof (dgFloat32); + for (dgInt32 i = 0; i < m_attrib.m_pointChannel.m_count; i++) { + const dgInt32 j = i * stride; + const dgInt32 index = m_attrib.m_pointChannel[i]; + const dgBigVector& p = m_points.m_vertex[index]; + bufferOut[j + 0] = dgFloat32(p.m_x); + bufferOut[j + 1] = dgFloat32(p.m_y); + bufferOut[j + 2] = dgFloat32(p.m_z); + } +} + +/* +void dgMeshEffect::GetWeightBlendChannel(dgInt32 strideInByte, dgFloat32* const bufferOut) const +{ + dgInt8* const buffer = (dgInt8*)bufferOut; + for (dgInt32 i = 0; i < m_attrib.m_pointChannel.m_count; i++) { + const dgInt32 j = i * strideInByte; + dgFloat32* const ptr = (dgFloat32*)&buffer[j]; + + const dgInt32 index = m_attrib.m_pointChannel[i]; + const dgFloat32* const p = &m_points.m_weights[index].m_weightBlends[0]; + ptr[0] = dgFloat32(p[0]); + ptr[1] = dgFloat32(p[1]); + ptr[2] = dgFloat32(p[2]); + ptr[3] = dgFloat32(p[3]); + } +} + +void dgMeshEffect::GetWeightIndexChannel(dgInt32 strideInByte, dgInt32* const bufferOut) const +{ + dgInt8* const buffer = (dgInt8*)bufferOut; + for (dgInt32 i = 0; i < m_attrib.m_pointChannel.m_count; i++) { + const dgInt32 j = i * strideInByte; + dgInt32* const ptr = (dgInt32*)&buffer[j]; + const dgInt32 index = m_attrib.m_pointChannel[i]; + const dgInt32* const p = &m_points.m_weights[index].m_controlIndex[0]; + ptr[0] = p[0]; + ptr[1] = p[1]; + ptr[2] = p[2]; + ptr[3] = p[3]; + } +} +*/ + +void dgMeshEffect::GetNormalChannel(dgInt32 strideInByte, dgFloat32* const bufferOut) const +{ + dgInt32 stride = strideInByte / sizeof (dgFloat32); + for (dgInt32 i = 0; i < m_attrib.m_normalChannel.m_count; i++) { + const dgInt32 j = i * stride; + bufferOut[j + 0] = dgFloat32(m_attrib.m_normalChannel[i].m_x); + bufferOut[j + 1] = dgFloat32(m_attrib.m_normalChannel[i].m_y); + bufferOut[j + 2] = dgFloat32(m_attrib.m_normalChannel[i].m_z); + } +} + +void dgMeshEffect::GetBinormalChannel(dgInt32 strideInByte, dgFloat32* const bufferOut) const +{ + dgInt32 stride = strideInByte / sizeof (dgFloat32); + for (dgInt32 i = 0; i < m_attrib.m_binormalChannel.m_count; i++) { + const dgInt32 j = i * stride; + bufferOut[j + 0] = dgFloat32(m_attrib.m_binormalChannel[i].m_x); + bufferOut[j + 1] = dgFloat32(m_attrib.m_binormalChannel[i].m_y); + bufferOut[j + 2] = dgFloat32(m_attrib.m_binormalChannel[i].m_z); + } +} + +void dgMeshEffect::GetUV0Channel(dgInt32 strideInByte, dgFloat32* const bufferOut) const +{ + dgInt32 stride = strideInByte / sizeof (dgFloat32); + for (dgInt32 i = 0; i < m_attrib.m_uv0Channel.m_count; i++) { + const dgInt32 j = i * stride; + bufferOut[j + 0] = dgFloat32(m_attrib.m_uv0Channel[i].m_u); + bufferOut[j + 1] = dgFloat32(m_attrib.m_uv0Channel[i].m_v); + } +} + +void dgMeshEffect::GetUV1Channel(dgInt32 strideInByte, dgFloat32* const bufferOut) const +{ + dgInt32 stride = strideInByte / sizeof (dgFloat32); + for (dgInt32 i = 0; i < m_attrib.m_uv1Channel.m_count; i++) { + const dgInt32 j = i * stride; + bufferOut[j + 0] = dgFloat32(m_attrib.m_uv1Channel[i].m_u); + bufferOut[j + 1] = dgFloat32(m_attrib.m_uv1Channel[i].m_v); + } +} + +void dgMeshEffect::GetVertexColorChannel(dgInt32 strideInByte, dgFloat32* const bufferOut) const +{ + dgInt32 stride = strideInByte / sizeof (dgFloat32); + for (dgInt32 i = 0; i < m_attrib.m_colorChannel.m_count; i++) { + const dgInt32 j = i * stride; + bufferOut[j + 0] = dgFloat32(m_attrib.m_colorChannel[i].m_x); + bufferOut[j + 1] = dgFloat32(m_attrib.m_colorChannel[i].m_y); + bufferOut[j + 2] = dgFloat32(m_attrib.m_colorChannel[i].m_z); + bufferOut[j + 3] = dgFloat32(m_attrib.m_colorChannel[i].m_w); + } +} + +dgMeshEffect::dgIndexArray* dgMeshEffect::MaterialGeometryBegin() +{ + dgInt32 materials[256]; + dgInt32 streamIndexMap[256]; + + dgInt32 count = 0; + dgInt32 materialCount = 0; + + dgIndexArray* const array = (dgIndexArray*) GetAllocator()->MallocLow (dgInt32 (4 * sizeof (dgInt32) * GetCount() + sizeof (dgIndexArray) + 2048)); + array->m_indexList = (dgInt32*)&array[1]; + + dgInt32 mark = IncLRU(); + dgPolyhedra::Iterator iter (*this); + memset(streamIndexMap, 0, sizeof (streamIndexMap)); + for(iter.Begin(); iter; iter ++){ + dgEdge* const edge = &(*iter); + if ((edge->m_incidentFace >= 0) && (edge->m_mark != mark)) { + dgEdge* ptr = edge; + ptr->m_mark = mark; + dgInt32 index0 = dgInt32 (ptr->m_userData); + + ptr = ptr->m_next; + ptr->m_mark = mark; + dgInt32 index1 = dgInt32 (ptr->m_userData); + + ptr = ptr->m_next; + do { + ptr->m_mark = mark; + + array->m_indexList[count * 4 + 0] = index0; + array->m_indexList[count * 4 + 1] = index1; + array->m_indexList[count * 4 + 2] = dgInt32 (ptr->m_userData); + array->m_indexList[count * 4 + 3] = m_attrib.m_materialChannel.m_count ? dgInt32 (m_attrib.m_materialChannel[dgInt32 (edge->m_userData)]) : 0; + index1 = dgInt32 (ptr->m_userData); + + dgInt32 hashValue = array->m_indexList[count * 4 + 3] & 0xff; + streamIndexMap[hashValue] ++; + materials[hashValue] = array->m_indexList[count * 4 + 3]; + count ++; + + ptr = ptr->m_next; + } while (ptr != edge); + } + } + + array->m_indexCount = count; + array->m_materialCount = materialCount; + + count = 0; + for (dgInt32 i = 0; i < 256;i ++) { + if (streamIndexMap[i]) { + array->m_materials[count] = materials[i]; + array->m_materialsIndexCount[count] = streamIndexMap[i] * 3; + count ++; + } + } + + array->m_materialCount = count; + + return array; +} + +void dgMeshEffect::MaterialGeomteryEnd(dgIndexArray* const handle) +{ + GetAllocator()->FreeLow (handle); +} + + +dgInt32 dgMeshEffect::GetFirstMaterial (dgIndexArray* const handle) const +{ + return GetNextMaterial (handle, -1); +} + +dgInt32 dgMeshEffect::GetNextMaterial (dgIndexArray* const handle, dgInt32 materialId) const +{ + materialId ++; + if(materialId >= handle->m_materialCount) { + materialId = -1; + } + return materialId; +} + +void dgMeshEffect::GetMaterialGetIndexStream (dgIndexArray* const handle, dgInt32 materialHandle, dgInt32* const indexArray) const +{ + + dgInt32 index = 0; + dgInt32 textureID = handle->m_materials[materialHandle]; + for (dgInt32 j = 0; j < handle->m_indexCount; j ++) { + if (handle->m_indexList[j * 4 + 3] == textureID) { + indexArray[index + 0] = handle->m_indexList[j * 4 + 0]; + indexArray[index + 1] = handle->m_indexList[j * 4 + 1]; + indexArray[index + 2] = handle->m_indexList[j * 4 + 2]; + + index += 3; + } + } +} + +void dgMeshEffect::GetMaterialGetIndexStreamShort (dgIndexArray* const handle, dgInt32 materialHandle, dgInt16* const indexArray) const +{ + dgInt32 index = 0; + dgInt32 textureID = handle->m_materials[materialHandle]; + for (dgInt32 j = 0; j < handle->m_indexCount; j ++) { + if (handle->m_indexList[j * 4 + 3] == textureID) { + indexArray[index + 0] = (dgInt16)handle->m_indexList[j * 4 + 0]; + indexArray[index + 1] = (dgInt16)handle->m_indexList[j * 4 + 1]; + indexArray[index + 2] = (dgInt16)handle->m_indexList[j * 4 + 2]; + index += 3; + } + } +} + +dgCollisionInstance* dgMeshEffect::CreateCollisionTree(dgWorld* const world, dgInt32 shapeID) const +{ + dgCollisionBVH* const collision = new (GetAllocator()) dgCollisionBVH (world); + + collision->BeginBuild(); + + dgInt32 mark = IncLRU(); + dgPolyhedra::Iterator iter (*this); + for (iter.Begin(); iter; iter ++){ + dgTreeNode* const faceNode = iter.GetNode(); + //dgEdge* const face = &(*iter); + dgEdge* const face = &faceNode->GetInfo(); + if ((face->m_mark != mark) && (face->m_incidentFace > 0)) { + dgInt32 count = 0; + dgVector polygon[256]; + dgEdge* ptr = face; + do { + //polygon[count] = dgVector (m_points[ptr->m_incidentVertex]); + polygon[count] = GetVertex(ptr->m_incidentVertex); + count ++; + ptr->m_mark = mark; + ptr = ptr->m_next; + } while (ptr != face); + //collision->AddFace(count, &polygon[0].m_x, sizeof (dgVector), dgInt32 (m_attrib[face->m_userData].m_material)); + collision->AddFace(count, &polygon[0].m_x, sizeof (dgVector), GetFaceMaterial(faceNode)); + } + } + collision->EndBuild(0); + + dgCollisionInstance* const instance = world->CreateInstance(collision, shapeID, dgGetIdentityMatrix()); + collision->Release(); + return instance; +} + +dgCollisionInstance* dgMeshEffect::CreateConvexCollision(dgWorld* const world, dgFloat64 tolerance, dgInt32 shapeID, const dgMatrix& srcMatrix) const +{ + dgStack poolPtr (m_points.m_vertex.m_count * 2); + dgVector* const pool = &poolPtr[0]; + + dgBigVector minBox; + dgBigVector maxBox; + CalculateAABB (minBox, maxBox); + //dgVector com ((minBox + maxBox).Scale (dgFloat32 (0.5f))); + dgVector com ((minBox + maxBox) * dgVector::m_half); + + dgInt32 count = 0; + dgInt32 mark = IncLRU(); + dgPolyhedra::Iterator iter (*this); + for (iter.Begin(); iter; iter ++){ + dgEdge* const vertex = &(*iter); + if (vertex->m_mark != mark) { + dgEdge* ptr = vertex; + do { + ptr->m_mark = mark; + ptr = ptr->m_twin->m_next; + } while (ptr != vertex); + + if (count < dgInt32 (poolPtr.GetElementsCount())) { + const dgBigVector p (m_points.m_vertex[vertex->m_incidentVertex]); + pool[count] = dgVector (p) - com; + count ++; + } + } + } + + dgMatrix matrix (srcMatrix); + matrix.m_posit += matrix.RotateVector(com); + matrix.m_posit.m_w = dgFloat32 (1.0f); + + dgUnsigned32 crc = dgCollisionConvexHull::CalculateSignature (count, &pool[0].m_x, sizeof (dgVector)); + dgCollisionConvexHull* const collision = new (GetAllocator()) dgCollisionConvexHull (GetAllocator(), crc, count, sizeof (dgVector), dgFloat32 (tolerance), &pool[0].m_x); + if (!collision->GetConvexVertexCount()) { + collision->Release(); + return NULL; + } + dgCollisionInstance* const instance = world->CreateInstance(collision, shapeID, matrix); + collision->Release(); + return instance; +} + + +void dgMeshEffect::TransformMesh (const dgMatrix& matrix) +{ + dgAssert(0); + /* + dgMatrix normalMatrix (matrix); + normalMatrix.m_posit = dgVector (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (1.0f)); + + matrix.TransformTriplex (&m_points->m_x, sizeof (dgBigVector), &m_points->m_x, sizeof (dgBigVector), m_pointCount); + matrix.TransformTriplex (&m_attrib[0].m_vertex.m_x, sizeof (dgVertexAtribute), &m_attrib[0].m_vertex.m_x, sizeof (dgVertexAtribute), m_atribCount); + normalMatrix.TransformTriplex (&m_attrib[0].m_normal_x, sizeof (dgVertexAtribute), &m_attrib[0].m_normal_x, sizeof (dgVertexAtribute), m_atribCount); +*/ +} + +dgInt32 dgMeshEffect::AddInterpolatedHalfAttribute(dgEdge* const edge, dgInt32 midPoint) +{ + dgBigVector p0(m_points.m_vertex[edge->m_incidentVertex]); + dgBigVector p2(m_points.m_vertex[edge->m_next->m_incidentVertex]); + dgBigVector p1(m_points.m_vertex[midPoint]); + dgBigVector p2p0(p2 - p0); + + dgAssert(p2p0.m_w == dgFloat32(0.0f)); + + dgFloat64 den = p2p0.DotProduct(p2p0).GetScalar(); + dgFloat64 param = p2p0.DotProduct(p1 - p0).GetScalar() / den; + dgFloat64 t1 = param; + dgFloat64 t0 = dgFloat64(1.0f) - t1; + dgAssert(t1 >= dgFloat64(0.0f)); + dgAssert(t1 <= dgFloat64(1.0f)); + + m_attrib.m_pointChannel.PushBack(midPoint); + + if (m_attrib.m_materialChannel.m_count) { + m_attrib.m_materialChannel.PushBack(m_attrib.m_materialChannel[dgInt32(edge->m_userData)]); + } + if (m_attrib.m_normalChannel.m_count) { + dgTriplex edgeNormal; + dgTriplex edgeNormal0(m_attrib.m_normalChannel[dgInt32(edge->m_userData)]); + dgTriplex edgeNormal1(m_attrib.m_normalChannel[dgInt32(edge->m_next->m_userData)]); + edgeNormal.m_x = edgeNormal0.m_x * dgFloat32(t0) + edgeNormal1.m_x * dgFloat32(t1); + edgeNormal.m_y = edgeNormal0.m_y * dgFloat32(t0) + edgeNormal1.m_y * dgFloat32(t1); + edgeNormal.m_z = edgeNormal0.m_z * dgFloat32(t0) + edgeNormal1.m_z * dgFloat32(t1); + m_attrib.m_normalChannel.PushBack(edgeNormal); + } + if (m_attrib.m_binormalChannel.m_count) { + dgAssert(0); + } + + if (m_attrib.m_uv0Channel.m_count) { + dgAttibutFormat::dgUV edgeUV; + dgAttibutFormat::dgUV edgeUV0(m_attrib.m_uv0Channel[dgInt32(edge->m_userData)]); + dgAttibutFormat::dgUV edgeUV1(m_attrib.m_uv0Channel[dgInt32(edge->m_next->m_userData)]); + edgeUV.m_u = edgeUV0.m_u * dgFloat32(t0) + edgeUV1.m_u * dgFloat32(t1); + edgeUV.m_v = edgeUV0.m_v * dgFloat32(t0) + edgeUV1.m_v * dgFloat32(t1); + m_attrib.m_uv0Channel.PushBack(edgeUV); + } + + if (m_attrib.m_uv1Channel.m_count) { + dgAssert(0); + } + + if (m_attrib.m_colorChannel.m_count) { + dgAssert(0); + } + return m_attrib.m_pointChannel.m_count - 1; +} + +void dgMeshEffect::AddInterpolatedEdgeAttribute (dgEdge* const edge, dgFloat64 param) +{ + dgFloat64 t1 = param; + dgFloat64 t0 = dgFloat64 (1.0f) - t1; + dgAssert (t1 >= dgFloat64(0.0f)); + dgAssert (t1 <= dgFloat64(1.0f)); + + const dgInt32 vertexIndex = m_points.m_vertex.m_count; + m_points.m_vertex.PushBack(m_points.m_vertex[edge->m_incidentVertex].Scale(t0) + m_points.m_vertex[edge->m_next->m_incidentVertex].Scale(t1)); + if (m_points.m_layers.m_count) { + m_points.m_layers.PushBack(m_points.m_layers[edge->m_incidentVertex]); + } + + m_attrib.m_pointChannel.PushBack(vertexIndex); + m_attrib.m_pointChannel.PushBack(vertexIndex); + + if (m_attrib.m_materialChannel.m_count) { + m_attrib.m_materialChannel.PushBack(m_attrib.m_materialChannel[dgInt32(edge->m_userData)]); + m_attrib.m_materialChannel.PushBack(m_attrib.m_materialChannel[dgInt32(edge->m_twin->m_userData)]); + } + if (m_attrib.m_normalChannel.m_count) { + dgTriplex edgeNormal; + dgTriplex edgeNormal0(m_attrib.m_normalChannel[dgInt32(edge->m_userData)]); + dgTriplex edgeNormal1(m_attrib.m_normalChannel[dgInt32(edge->m_next->m_userData)]); + edgeNormal.m_x = edgeNormal0.m_x * dgFloat32(t0) + edgeNormal1.m_x * dgFloat32(t1); + edgeNormal.m_y = edgeNormal0.m_y * dgFloat32(t0) + edgeNormal1.m_y * dgFloat32(t1); + edgeNormal.m_z = edgeNormal0.m_z * dgFloat32(t0) + edgeNormal1.m_z * dgFloat32(t1); + m_attrib.m_normalChannel.PushBack(edgeNormal); + + dgTriplex twinNormal; + dgTriplex twinNormal0(m_attrib.m_normalChannel[dgInt32(edge->m_twin->m_next->m_userData)]); + dgTriplex twinNormal1(m_attrib.m_normalChannel[dgInt32(edge->m_twin->m_userData)]); + twinNormal.m_x = twinNormal0.m_x * dgFloat32(t0) + twinNormal1.m_x * dgFloat32(t1); + twinNormal.m_y = twinNormal0.m_y * dgFloat32(t0) + twinNormal1.m_y * dgFloat32(t1); + twinNormal.m_z = twinNormal0.m_z * dgFloat32(t0) + twinNormal1.m_z * dgFloat32(t1); + m_attrib.m_normalChannel.PushBack(twinNormal); + } + if (m_attrib.m_binormalChannel.m_count) { + dgAssert(0); + } + + if (m_attrib.m_uv0Channel.m_count) { + dgAttibutFormat::dgUV edgeUV; + dgAttibutFormat::dgUV edgeUV0(m_attrib.m_uv0Channel[dgInt32(edge->m_userData)]); + dgAttibutFormat::dgUV edgeUV1(m_attrib.m_uv0Channel[dgInt32(edge->m_next->m_userData)]); + edgeUV.m_u = edgeUV0.m_u * dgFloat32(t0) + edgeUV1.m_u * dgFloat32(t1); + edgeUV.m_v = edgeUV0.m_v * dgFloat32(t0) + edgeUV1.m_v * dgFloat32(t1); + m_attrib.m_uv0Channel.PushBack(edgeUV); + + dgAttibutFormat::dgUV twinUV; + dgAttibutFormat::dgUV twinUV0(m_attrib.m_uv0Channel[dgInt32(edge->m_twin->m_next->m_userData)]); + dgAttibutFormat::dgUV twinUV1(m_attrib.m_uv0Channel[dgInt32(edge->m_twin->m_userData)]); + twinUV.m_u = twinUV0.m_u * dgFloat32(t0) + twinUV1.m_u * dgFloat32(t1); + twinUV.m_v = twinUV0.m_v * dgFloat32(t0) + twinUV1.m_v * dgFloat32(t1); + m_attrib.m_uv0Channel.PushBack(twinUV); + } + + if (m_attrib.m_uv1Channel.m_count) { + dgAssert(0); + } + + if (m_attrib.m_colorChannel.m_count) { + dgAssert(0); + } +} + +bool dgMeshEffect::Sanity () const +{ + #ifdef _DEBUG + dgMeshEffect::Iterator iter (*this); + for (iter.Begin(); iter; iter ++) { + dgEdge* const edge = &iter.GetNode()->GetInfo(); + dgAssert(edge->m_twin); + dgAssert(edge->m_next); + dgAssert(edge->m_prev); + dgAssert (edge->m_twin->m_twin == edge); + dgAssert (edge->m_next->m_incidentVertex == edge->m_twin->m_incidentVertex); + dgAssert (edge->m_incidentVertex == edge->m_twin->m_next->m_incidentVertex); + } + #endif + return true; +} + + +dgEdge* dgMeshEffect::InsertEdgeVertex (dgEdge* const edge, dgFloat64 param) +{ + dgEdge* const twin = edge->m_twin; + AddInterpolatedEdgeAttribute(edge, param); + + dgInt32 edgeAttrV0 = dgInt32 (edge->m_userData); + dgInt32 twinAttrV0 = dgInt32 (twin->m_userData); + + dgEdge* const faceA0 = edge->m_next; + dgEdge* const faceA1 = edge->m_prev; + dgEdge* const faceB0 = twin->m_next; + dgEdge* const faceB1 = twin->m_prev; + SpliteEdge (m_points.m_vertex.m_count - 1, edge); + + faceA0->m_prev->m_userData = dgUnsigned64 (m_attrib.m_pointChannel.m_count - 2); + faceA1->m_next->m_userData = dgUnsigned64 (edgeAttrV0); + + faceB0->m_prev->m_userData = dgUnsigned64 (m_attrib.m_pointChannel.m_count - 1); + faceB1->m_next->m_userData = dgUnsigned64 (twinAttrV0); + return faceA1->m_next; +} + + + +//dgMeshEffect::dgVertexAtribute dgMeshEffect::InterpolateVertex (const dgBigVector& srcPoint, const dgEdge* const face) const +dgInt32 dgMeshEffect::InterpolateVertex (const dgBigVector& srcPoint, const dgEdge* const face) const +{ +dgAssert(0); +return 0; +/* + const dgBigVector point (srcPoint); + + dgVertexAtribute attribute; + memset (&attribute, 0, sizeof (attribute)); + +// dgBigVector normal (FaceNormal(face, &m_points[0].m_x, sizeof(dgBigVector))); +// normal = normal.Scale (dgFloat64 (1.0f) / sqrt (normal % normal)); +// attribute.m_vertex = srcPoint; +// attribute.m_normal_x = normal.m_x; +// attribute.m_normal_y = normal.m_y; +// attribute.m_normal_z = normal.m_z; + + dgFloat64 tol = dgFloat32 (1.0e-4f); + for (dgInt32 i = 0; i < 4; i ++) { + const dgEdge* ptr = face; + const dgEdge* const edge0 = ptr; + dgBigVector q0 (m_points[ptr->m_incidentVertex]); + + ptr = ptr->m_next; + const dgEdge* edge1 = ptr; + dgBigVector q1 (m_points[ptr->m_incidentVertex]); + + ptr = ptr->m_next; + const dgEdge* edge2 = ptr; + do { + const dgBigVector q2 (m_points[ptr->m_incidentVertex]); + + dgBigVector p10 (q1 - q0); + dgBigVector p20 (q2 - q0); + + dgFloat64 dot = p20.DotProduct3(p10); + dgFloat64 mag1 = p10.DotProduct3(p10); + dgFloat64 mag2 = p20.DotProduct3(p20); + dgFloat64 collinear = dot * dot - mag2 * mag1; + if (fabs (collinear) > dgFloat64 (1.0e-8f)) { + dgBigVector p_p0 (point - q0); + dgBigVector p_p1 (point - q1); + dgBigVector p_p2 (point - q2); + + dgFloat64 alpha1 = p10.DotProduct3(p_p0); + dgFloat64 alpha2 = p20.DotProduct3(p_p0); + dgFloat64 alpha3 = p10.DotProduct3(p_p1); + dgFloat64 alpha4 = p20.DotProduct3(p_p1); + dgFloat64 alpha5 = p10.DotProduct3(p_p2); + dgFloat64 alpha6 = p20.DotProduct3(p_p2); + + dgFloat64 vc = alpha1 * alpha4 - alpha3 * alpha2; + dgFloat64 vb = alpha5 * alpha2 - alpha1 * alpha6; + dgFloat64 va = alpha3 * alpha6 - alpha5 * alpha4; + dgFloat64 den = va + vb + vc; + dgFloat64 minError = den * (-tol); + dgFloat64 maxError = den * (dgFloat32 (1.0f) + tol); + if ((va > minError) && (vb > minError) && (vc > minError) && (va < maxError) && (vb < maxError) && (vc < maxError)) { + edge2 = ptr; + + den = dgFloat64 (1.0f) / (va + vb + vc); + + dgFloat64 alpha0 = dgFloat32 (va * den); + dgFloat64 alpha1 = dgFloat32 (vb * den); + dgFloat64 alpha2 = dgFloat32 (vc * den); + + const dgVertexAtribute& attr0 = m_attrib[edge0->m_userData]; + const dgVertexAtribute& attr1 = m_attrib[edge1->m_userData]; + const dgVertexAtribute& attr2 = m_attrib[edge2->m_userData]; + dgBigVector normal (attr0.m_normal_x * alpha0 + attr1.m_normal_x * alpha1 + attr2.m_normal_x * alpha2, + attr0.m_normal_y * alpha0 + attr1.m_normal_y * alpha1 + attr2.m_normal_y * alpha2, + attr0.m_normal_z * alpha0 + attr1.m_normal_z * alpha1 + attr2.m_normal_z * alpha2, dgFloat32 (0.0f)); + //normal = normal.Scale (dgFloat64 (1.0f) / sqrt (normal.DotProduct3(normal))); + normal = normal.Normalize(); + + #ifdef _DEBUG + dgBigVector testPoint (attr0.m_vertex.m_x * alpha0 + attr1.m_vertex.m_x * alpha1 + attr2.m_vertex.m_x * alpha2, + attr0.m_vertex.m_y * alpha0 + attr1.m_vertex.m_y * alpha1 + attr2.m_vertex.m_y * alpha2, + attr0.m_vertex.m_z * alpha0 + attr1.m_vertex.m_z * alpha1 + attr2.m_vertex.m_z * alpha2, dgFloat32 (0.0f)); + dgAssert (fabs (testPoint.m_x - point.m_x) < dgFloat32 (1.0e-2f)); + dgAssert (fabs (testPoint.m_y - point.m_y) < dgFloat32 (1.0e-2f)); + dgAssert (fabs (testPoint.m_z - point.m_z) < dgFloat32 (1.0e-2f)); + #endif + + + attribute.m_vertex.m_x = point.m_x; + attribute.m_vertex.m_y = point.m_y; + attribute.m_vertex.m_z = point.m_z; + attribute.m_vertex.m_w = point.m_w; + attribute.m_normal_x = normal.m_x; + attribute.m_normal_y = normal.m_y; + attribute.m_normal_z = normal.m_z; + attribute.m_u0 = attr0.m_u0 * alpha0 + attr1.m_u0 * alpha1 + attr2.m_u0 * alpha2; + attribute.m_v0 = attr0.m_v0 * alpha0 + attr1.m_v0 * alpha1 + attr2.m_v0 * alpha2; + attribute.m_u1 = attr0.m_u1 * alpha0 + attr1.m_u1 * alpha1 + attr2.m_u1 * alpha2; + attribute.m_v1 = attr0.m_v1 * alpha0 + attr1.m_v1 * alpha1 + attr2.m_v1 * alpha2; + + attribute.m_material = attr0.m_material; + dgAssert (attr0.m_material == attr1.m_material); + dgAssert (attr0.m_material == attr2.m_material); + return attribute; + } + } + + q1 = q2; + edge1 = ptr; + + ptr = ptr->m_next; + } while (ptr != face); + tol *= dgFloat64 (2.0f); + } + // this should never happens + dgAssert (0); + return attribute; +*/ +} + +bool dgMeshEffect::HasOpenEdges () const +{ + dgPolyhedra::Iterator iter (*this); + for (iter.Begin(); iter; iter ++){ + dgEdge* const face = &(*iter); + if (face->m_incidentFace < 0){ + return true; + } + } + return false; +} + +dgFloat64 dgMeshEffect::CalculateVolume () const +{ + dgAssert (0); + return 0; + /* + + dgPolyhedraMassProperties localData; + + dgInt32 mark = IncLRU(); + dgPolyhedra::Iterator iter (*this); + for (iter.Begin(); iter; iter ++){ + dgInt32 count; + dgEdge* ptr; + dgEdge* face; + dgVector points[256]; + + face = &(*iter); + if ((face->m_incidentFace > 0) && (face->m_mark != mark)) { + count = 0; + ptr = face; + do { + points[count] = m_points[ptr->m_incidentVertex]; + count ++; + ptr->m_mark = mark; + ptr = ptr->m_next; + } while (ptr != face); + localData.AddCGFace (count, points); + } + } + + dgFloat32 volume; + dgVector p0; + dgVector p1; + dgVector com; + dgVector inertia; + dgVector crossInertia; + volume = localData.MassProperties (com, inertia, crossInertia); + return volume; +*/ +} + + +dgMeshEffect* dgMeshEffect::GetNextLayer (dgInt32 mark) +{ + Iterator iter(*this); + dgEdge* edge = NULL; + for (iter.Begin (); iter; iter ++) { + edge = &(*iter); + if ((edge->m_mark < mark) && (edge->m_incidentFace > 0)) { + break; + } + } + + if (!edge) { + return NULL; + } + + const dgInt32 layer = m_points.m_layers.m_count ? m_points.m_layers[edge->m_incidentVertex] : 0; + dgPolyhedra polyhedra(GetAllocator()); + + polyhedra.BeginFace (); + for (iter.Begin (); iter; iter ++) { + dgEdge* const edge1 = &(*iter); + if ((edge1->m_mark < mark) && (edge1->m_incidentFace > 0)) { + const dgInt32 thislayer = m_points.m_layers.m_count ? m_points.m_layers[edge1->m_incidentVertex] : 0; + if (thislayer == layer) { + dgEdge* ptr = edge1; + dgInt32 count = 0; + dgInt32 faceIndex[256]; + dgInt64 faceDataIndex[256]; + do { + ptr->m_mark = mark; + faceIndex[count] = ptr->m_incidentVertex; + faceDataIndex[count] = ptr->m_userData; + count ++; + dgAssert (count < dgInt32 (sizeof (faceIndex)/ sizeof(faceIndex[0]))); + ptr = ptr->m_next; + } while (ptr != edge1); + polyhedra.AddFace (count, &faceIndex[0], &faceDataIndex[0]); + } + } + } + polyhedra.EndFace (); + + dgMeshEffect* solid = NULL; + if (polyhedra.GetCount()) { + solid = new (GetAllocator()) dgMeshEffect(polyhedra, *this); + solid->SetLRU(mark); + } + return solid; +} + + + +void dgMeshEffect::MergeFaces (const dgMeshEffect* const source) +{ + dgInt32 mark = source->IncLRU(); + dgPolyhedra::Iterator iter (*source); + for(iter.Begin(); iter; iter ++){ + dgEdge* const edge = &(*iter); + if ((edge->m_incidentFace > 0) && (edge->m_mark < mark)) { + BeginBuildFace (); + dgEdge* ptr = edge; + do { + ptr->m_mark = mark; + dgInt32 vIndex = ptr->m_incidentVertex; + dgInt32 aIndex = dgInt32 (ptr->m_userData); + AddPoint (source->m_points.m_vertex[vIndex].m_x, source->m_points.m_vertex[vIndex].m_y, source->m_points.m_vertex[vIndex].m_z); + if (source->m_points.m_layers.m_count) { + AddLayer (source->m_points.m_layers[vIndex]); + } + + if (source->m_attrib.m_materialChannel.m_count) { + AddMaterial (source->m_attrib.m_materialChannel[aIndex]); + } + if (source->m_attrib.m_colorChannel.m_count) { + AddVertexColor(source->m_attrib.m_colorChannel[aIndex].m_x, source->m_attrib.m_colorChannel[aIndex].m_y, source->m_attrib.m_colorChannel[aIndex].m_z, source->m_attrib.m_colorChannel[aIndex].m_w); + } + if (source->m_attrib.m_normalChannel.m_count) { + AddNormal(source->m_attrib.m_normalChannel[aIndex].m_x, source->m_attrib.m_normalChannel[aIndex].m_y, source->m_attrib.m_normalChannel[aIndex].m_z); + } + if (source->m_attrib.m_binormalChannel.m_count) { + AddBinormal(source->m_attrib.m_binormalChannel[aIndex].m_x, source->m_attrib.m_binormalChannel[aIndex].m_y, source->m_attrib.m_binormalChannel[aIndex].m_z); + } + if (source->m_attrib.m_uv0Channel.m_count) { + AddUV0(source->m_attrib.m_uv0Channel[aIndex].m_u, source->m_attrib.m_uv0Channel[aIndex].m_v); + } + if (source->m_attrib.m_uv1Channel.m_count) { + AddUV1(source->m_attrib.m_uv1Channel[aIndex].m_u, source->m_attrib.m_uv1Channel[aIndex].m_v); + } + ptr = ptr->m_next; + } while (ptr != edge); + EndBuildFace (); + } + } +} + +bool dgMeshEffect::SeparateDuplicateLoops (dgEdge* const face) +{ + for (dgEdge* ptr0 = face; ptr0 != face->m_prev; ptr0 = ptr0->m_next) { + dgInt32 index = ptr0->m_incidentVertex; + + dgEdge* ptr1 = ptr0->m_next; + do { + if (ptr1->m_incidentVertex == index) { + dgEdge* const ptr00 = ptr0->m_prev; + dgEdge* const ptr11 = ptr1->m_prev; + + ptr00->m_next = ptr1; + ptr1->m_prev = ptr00; + + ptr11->m_next = ptr0; + ptr0->m_prev = ptr11; + + return true; + } + + ptr1 = ptr1->m_next; + } while (ptr1 != face); + } + return false; +} + +void dgMeshEffect::RepairTJoints () +{ + dgAssert (Sanity ()); + + // delete edge of zero length + bool dirty = true; + while (dirty) { + dgFloat64 tol = dgFloat64 (1.0e-5f); + dgFloat64 tol2 = tol * tol; + dirty = false; + dgPolyhedra::Iterator iter (*this); + for (iter.Begin(); iter; ) { + dgEdge* const edge = &(*iter); + iter ++; + const dgBigVector& p0 = m_points.m_vertex[edge->m_incidentVertex]; + const dgBigVector& p1 = m_points.m_vertex[edge->m_twin->m_incidentVertex]; + dgBigVector dist (p1 - p0); + dgAssert(dist.m_w == dgFloat32(0.0f)); + dgFloat64 mag2 = dist.DotProduct(dist).GetScalar(); + if (mag2 < tol2) { + bool move = true; + while (move) { + move = false; + dgEdge* ptr = edge->m_twin; + do { + if ((&(*iter) == ptr) || (&(*iter) == ptr->m_twin)) { + move = true; + iter ++; + } + ptr = ptr->m_twin->m_next; + } while (ptr != edge->m_twin); + + ptr = edge; + do { + if ((&(*iter) == ptr) || (&(*iter) == ptr->m_twin)) { + move = true; + iter ++; + } + ptr = ptr->m_twin->m_next; + } while (ptr != edge); + } + + dgEdge* const collapsedEdge = CollapseEdge(edge); + if (collapsedEdge) { + //dgAssert (0); + dgTrace(("remember to finish this!!: %s %s line:%d\n", __FILE__, __FUNCTION__, __LINE__)); + dirty = true; + dgBigVector q (m_points.m_vertex[collapsedEdge->m_incidentVertex]); + dgEdge* ptr = collapsedEdge; + do { + if (ptr->m_incidentFace > 0) { + //dgAssert (0); + //m_attrib[ptr->m_userData].m_vertex = q; + dgTrace(("remember to finish this!!: %s %s line:%d\n", __FILE__, __FUNCTION__, __LINE__)); + } + ptr = ptr->m_twin->m_next; + } while (ptr != collapsedEdge); + } + } + } + } + dgAssert (Sanity ()); + + // repair straight open edges + dgInt32 mark = IncLRU(); + dgPolyhedra::Iterator iter (*this); + for (iter.Begin(); iter; iter ++) { + dgEdge* const edge = &(*iter); + if ((edge->m_mark) != mark && (edge->m_incidentFace < 0)) { + + while (SeparateDuplicateLoops (edge)); + dgEdge* ptr = edge; + do { + ptr->m_mark = mark; + ptr = ptr->m_next; + } while (ptr != edge); + } + } + + dgAssert (Sanity ()); + DeleteDegenerateFaces(&m_points.m_vertex[0].m_x, sizeof (dgBigVector), dgFloat64 (1.0e-7f)); + dgAssert (Sanity ()); + + // delete straight line edges + dirty = true; +// dirty = false; + while (dirty) { + dgFloat64 tol = dgFloat64(1.0 - 1.0e-8); + dgFloat64 tol2 = tol * tol; + + dirty = false; + dgAssert (Sanity ()); + + dgPolyhedra::Iterator iter1 (*this); + for (iter1.Begin(); iter1; ) { + dgEdge* const edge = &(*iter1); + iter1 ++; + + const dgBigVector& p0 = m_points.m_vertex[edge->m_incidentVertex]; + const dgBigVector& p1 = m_points.m_vertex[edge->m_next->m_incidentVertex]; + const dgBigVector& p2 = m_points.m_vertex[edge->m_next->m_next->m_incidentVertex]; + + dgBigVector A (p1 - p0); + dgBigVector B (p2 - p1); + dgAssert(A.m_w == dgFloat32(0.0f)); + dgAssert(B.m_w == dgFloat32(0.0f)); + dgFloat64 ab = A.DotProduct(B).GetScalar(); + if (ab >= 0.0f) { + dgFloat64 aa = A.DotProduct(A).GetScalar(); + dgFloat64 bb = B.DotProduct(B).GetScalar(); + + dgFloat64 magab2 = ab * ab; + dgFloat64 magaabb = aa * bb * tol2; + if (magab2 >= magaabb) { + if ((edge->m_incidentFace > 0) && (edge->m_twin->m_incidentFace > 0)) { + if (edge->m_twin->m_prev == edge->m_next->m_twin) { + dgEdge* const newEdge = AddHalfEdge(edge->m_incidentVertex, edge->m_next->m_next->m_incidentVertex); + if (newEdge) { + dirty = true; + dgEdge* const newTwin = AddHalfEdge(edge->m_next->m_next->m_incidentVertex, edge->m_incidentVertex); + dgAssert (newEdge); + dgAssert (newTwin); + + newEdge->m_twin = newTwin; + newTwin->m_twin = newEdge; + + newEdge->m_userData = edge->m_userData; + newTwin->m_userData = edge->m_twin->m_prev->m_userData; + + newEdge->m_incidentFace = edge->m_incidentFace; + newTwin->m_incidentFace = edge->m_twin->m_incidentFace; + + dgEdge* const nextEdge = edge->m_next; + + nextEdge->m_twin->m_prev->m_next = newTwin; + newTwin->m_prev = nextEdge->m_twin->m_prev; + + edge->m_twin->m_next->m_prev = newTwin; + newTwin->m_next = edge->m_twin->m_next; + + nextEdge->m_next->m_prev = newEdge; + newEdge->m_next = nextEdge->m_next; + + edge->m_prev->m_next = newEdge; + newEdge->m_prev = edge->m_prev; + + while ((&(*iter1) == edge->m_twin) || (&(*iter1) == nextEdge) || (&(*iter1) == nextEdge->m_twin)) { + iter1 ++; + } + + nextEdge->m_twin->m_prev = nextEdge; + nextEdge->m_twin->m_next = nextEdge; + nextEdge->m_prev = nextEdge->m_twin; + nextEdge->m_next = nextEdge->m_twin; + + edge->m_twin->m_prev = edge; + edge->m_twin->m_next = edge; + edge->m_prev = edge->m_twin; + edge->m_next = edge->m_twin; + + DeleteEdge(edge); + DeleteEdge(nextEdge); + //dgAssert (Sanity ()); + + } else if (edge->m_next->m_next->m_next == edge) { + dgAssert (0); +/* + dirty = true; + dgEdge* const openEdge = edge; + dgEdge* const nextEdge = openEdge->m_next; + dgEdge* const deletedEdge = openEdge->m_prev; + while ((&(*iter) == deletedEdge) || (&(*iter) == deletedEdge->m_twin)) { + iter ++; + } + + openEdge->m_userData = deletedEdge->m_twin->m_userData; + + dgBigVector p2p0 (p2 - p0); + dgFloat64 den = p2p0.DotProduct3(p2p0); + dgFloat64 param1 = p2p0.DotProduct3(p1 - p0) / den; + dgVertexAtribute attib1 = AddInterpolateEdgeAttibute (deletedEdge->m_twin, param1); + AddAtribute(attib1); + openEdge->m_next->m_userData = m_atribCount - 1; + + openEdge->m_incidentFace = deletedEdge->m_twin->m_incidentFace; + openEdge->m_next->m_incidentFace = deletedEdge->m_twin->m_incidentFace; + + deletedEdge->m_twin->m_prev->m_next = openEdge; + openEdge->m_prev = deletedEdge->m_twin->m_prev; + + deletedEdge->m_twin->m_next->m_prev = nextEdge; + nextEdge->m_next = deletedEdge->m_twin->m_next; + + deletedEdge->m_twin->m_next = deletedEdge; + deletedEdge->m_twin->m_prev = deletedEdge; + deletedEdge->m_next = deletedEdge->m_twin; + deletedEdge->m_prev = deletedEdge->m_twin; + DeleteEdge(deletedEdge); + //dgAssert (Sanity ()); +*/ + } + } + } else if (FindEdge(edge->m_incidentVertex, edge->m_next->m_next->m_incidentVertex)) { + dgEdge* const openEdge = edge; + //dgAssert (openEdge->m_incidentFace <= 0); + dgTrace(("remember to finish this!!: %s %s line:%d\n", __FILE__, __FUNCTION__, __LINE__)); + dgEdge* const nextEdge = openEdge->m_next; + dgEdge* const deletedEdge = openEdge->m_prev; + if (deletedEdge == openEdge->m_next->m_next) { + dirty = true; + while ((&(*iter1) == deletedEdge) || (&(*iter1) == deletedEdge->m_twin)) { + iter1 ++; + } + + //dgAssert (deletedEdge->m_twin->m_incidentFace > 0); + dgTrace(("remember to finish this!!: %s %s line:%d\n", __FILE__, __FUNCTION__, __LINE__)); + openEdge->m_incidentFace = deletedEdge->m_twin->m_incidentFace; + openEdge->m_next->m_incidentFace = deletedEdge->m_twin->m_incidentFace; + + dgInt32 attibuteIndex = AddInterpolatedHalfAttribute(deletedEdge->m_twin, nextEdge->m_incidentVertex); + openEdge->m_next->m_userData = attibuteIndex; + openEdge->m_userData = deletedEdge->m_twin->m_userData; + + deletedEdge->m_twin->m_prev->m_next = openEdge; + openEdge->m_prev = deletedEdge->m_twin->m_prev; + + deletedEdge->m_twin->m_next->m_prev = nextEdge; + nextEdge->m_next = deletedEdge->m_twin->m_next; + + deletedEdge->m_twin->m_next = deletedEdge; + deletedEdge->m_twin->m_prev = deletedEdge; + deletedEdge->m_next = deletedEdge->m_twin; + deletedEdge->m_prev = deletedEdge->m_twin; + DeleteEdge(deletedEdge); + dgAssert (Sanity ()); + } + + } else { + dgEdge* const openEdge = (edge->m_incidentFace <= 0) ? edge : edge->m_twin; + dgAssert (openEdge->m_incidentFace <= 0); + + const dgBigVector& p3 = m_points.m_vertex[openEdge->m_next->m_next->m_next->m_incidentVertex]; + + dgBigVector A0 (p3 - p2); + dgBigVector B0 (p2 - p1); + dgAssert(A0.m_w == dgFloat32(0.0f)); + dgAssert(B0.m_w == dgFloat32(0.0f)); + + dgFloat64 ab0 = A0.DotProduct(B0).GetScalar(); + if (ab0 >= 0.0) { + dgFloat64 aa0 = A0.DotProduct(A0).GetScalar(); + dgFloat64 bb0 = B0.DotProduct(B0).GetScalar(); + + dgFloat64 ab0ab0 = ab0 * ab0; + dgFloat64 aa0bb0 = aa0 * bb0 * tol2; + if (ab0ab0 >= aa0bb0) { + if (openEdge->m_next->m_next->m_next->m_next != openEdge) { + const dgBigVector& p4 = m_points.m_vertex[openEdge->m_prev->m_incidentVertex]; + dgBigVector A1 (p1 - p0); + dgBigVector B1 (p1 - p4); + dgAssert(A1.m_w == dgFloat32(0.0f)); + dgAssert(B1.m_w == dgFloat32(0.0f)); + dgFloat64 ab1 = A1.DotProduct(B1).GetScalar(); + if (ab1 < 0.0f) { + dgFloat64 ab1ab1 = ab1 * ab1; + dgFloat64 aa1bb1 = aa0 * bb0 * tol2; + if (ab1ab1 >= aa1bb1) { + dgEdge* const newFace = ConnectVertex (openEdge->m_prev, openEdge->m_next); + dirty |= newFace ? true : false; + } + } + //dgAssert (Sanity ()); + } else if (openEdge->m_prev->m_twin->m_incidentFace > 0) { + dirty = true; + + dgEdge* const deletedEdge = openEdge->m_prev; + while ((&(*iter1) == deletedEdge) || (&(*iter1) == deletedEdge->m_twin)) { + iter1 ++; + } + + openEdge->m_incidentFace = deletedEdge->m_twin->m_incidentFace; + openEdge->m_next->m_incidentFace = deletedEdge->m_twin->m_incidentFace; + openEdge->m_next->m_next->m_incidentFace = deletedEdge->m_twin->m_incidentFace; + + dgInt32 attibuteIndex0 = AddInterpolatedHalfAttribute(deletedEdge->m_twin, openEdge->m_next->m_incidentVertex); + dgInt32 attibuteIndex1 = AddInterpolatedHalfAttribute(deletedEdge->m_twin, openEdge->m_next->m_next->m_incidentVertex); + + openEdge->m_userData = deletedEdge->m_twin->m_userData; + openEdge->m_next->m_userData = attibuteIndex0; + openEdge->m_next->m_next->m_userData = attibuteIndex1; + + + deletedEdge->m_twin->m_prev->m_next = openEdge; + openEdge->m_prev = deletedEdge->m_twin->m_prev; + + deletedEdge->m_twin->m_next->m_prev = deletedEdge->m_prev; + deletedEdge->m_prev->m_next = deletedEdge->m_twin->m_next; + + deletedEdge->m_twin->m_next = deletedEdge; + deletedEdge->m_twin->m_prev = deletedEdge; + deletedEdge->m_next = deletedEdge->m_twin; + deletedEdge->m_prev = deletedEdge->m_twin; + DeleteEdge(deletedEdge); + //dgAssert (Sanity ()); + } + } + } + } + } + } + } + } + dgAssert (Sanity ()); + + DeleteDegenerateFaces(&m_points.m_vertex[0].m_x, sizeof (dgBigVector), dgFloat64 (1.0e-7f)); +/* + for (iter.Begin(); iter; iter ++) { + dgEdge* const edge = &iter.GetNode()->GetInfo(); + if (edge->m_incidentFace > 0) { + dgBigVector p0 (m_points[edge->m_incidentVertex]); + m_attrib[edge->m_userData].m_vertex.m_x = p0.m_x; + m_attrib[edge->m_userData].m_vertex.m_y = p0.m_y; + m_attrib[edge->m_userData].m_vertex.m_z = p0.m_z; + } + } +*/ + dgAssert (Sanity ()); +} + diff --git a/thirdparty/src/newton/dgMeshUtil/dgMeshEffect2.cpp b/thirdparty/src/newton/dgMeshUtil/dgMeshEffect2.cpp new file mode 100644 index 000000000..c380f4b69 --- /dev/null +++ b/thirdparty/src/newton/dgMeshUtil/dgMeshEffect2.cpp @@ -0,0 +1,1221 @@ +/* Copyright (c) <2003-2019> +* +* 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 "dgPhysicsStdafx.h" +#include "dgBody.h" +#include "dgWorld.h" +#include "dgMeshEffect.h" +#include "dgCollisionConvexHull.h" + + + +class dgRayTrataAABBAccelerator: public dgMeshEffect::dgMeshBVH +{ + public: + dgRayTrataAABBAccelerator(const dgMeshEffect* const tetraMesh) + :dgMeshEffect::dgMeshBVH(tetraMesh) + { + Build(); + } + + dgMeshBVHNode* CreateLeafNode(dgEdge* const face, void* const userData) + { + dgMemoryAllocator* const allocator = m_mesh->GetAllocator(); + dgMeshBVHNode* const node = new (allocator)dgMeshBVHNode(m_mesh, face, userData); + + dgInt32 mark = m_mesh->GetLRU(); + + dgVector p0(m_mesh->GetVertex(face->m_twin->m_prev->m_incidentVertex)); + dgVector p1(p0); + dgEdge* faceEdge = face; + do { + dgEdge* twinFace = faceEdge->m_twin; + do { + twinFace->m_mark = mark; + twinFace = twinFace->m_next; + } while (twinFace != faceEdge->m_twin); + + dgVector point(m_mesh->GetVertex(faceEdge->m_incidentVertex)); + p0 = point.GetMin(p0); + p1 = point.GetMax(p1); + + faceEdge = faceEdge->m_next; + } while (faceEdge != face); + + dgVector padding(dgFloat32(0.01f)); + p0 -= padding; + p1 += padding; + node->SetBox(p0, p1); + return node; + } +}; + + +// idea taken from paper: Fast Tetrahedral Meshes with Good Dihedral Angles, by Francois Labelle Jonathan Richard Shewchuk +// but quite different approach. +class dgTetraIsoSufaceStuffing +{ + public: + enum dgVertexSign + { + m_onSuface, + m_inside, + m_outside, + }; + + class dgGridDimension + { + public: + dgBigVector m_origin; + dgFloat64 m_cellSize; + dgFloat64 m_diameter; + dgInt32 m_gridSizeX; + dgInt32 m_gridSizeY; + dgInt32 m_gridSizeZ; + dgInt32 m_innerSize; + dgInt32 m_outerSize; + }; + + template + class dgAssessor + { + public: + dgAssessor() + :m_count(0) + { + } + + dgInt32 GetCount() const + { + return m_count; + } + + const T& operator[] (dgInt32 i) const + { + dgAssert(i >= 0); + dgAssert(i < size); + return m_elements[i]; + } + + T& operator[] (dgInt32 i) + { + dgAssert (i >= 0); + dgAssert (i < size); + return m_elements[i]; + } + + void PushBack (const T data) + { + dgAssert(m_count >= 0); + dgAssert(m_count < size); + m_elements[m_count] = data; + m_count ++; + } + + private: + dgInt32 m_count; + T m_elements[size]; + }; + + typedef dgAssessor dgTetrahedra; + typedef dgAssessor dgTetraEdgeCuts; + typedef dgAssessor dgTetraToVertexNode; + + class dgNormalMap + { + public: + dgNormalMap() + :m_count(sizeof (m_normal)/sizeof (m_normal[0])) + { + dgVector p0(dgFloat32(1.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f)); + dgVector p1(dgFloat32(-1.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f)); + dgVector p2(dgFloat32(0.0f), dgFloat32(1.0f), dgFloat32(0.0f), dgFloat32(0.0f)); + dgVector p3(dgFloat32(0.0f), dgFloat32(-1.0f), dgFloat32(0.0f), dgFloat32(0.0f)); + dgVector p4(dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(1.0f), dgFloat32(0.0f)); + dgVector p5(dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(-1.0f), dgFloat32(0.0f)); + + dgInt32 count = 0; + dgInt32 subdivitions = 1; + TessellateTriangle(subdivitions, p4, p0, p2, count); + TessellateTriangle(subdivitions, p0, p5, p2, count); + TessellateTriangle(subdivitions, p5, p1, p2, count); + TessellateTriangle(subdivitions, p1, p4, p2, count); + TessellateTriangle(subdivitions, p0, p4, p3, count); + TessellateTriangle(subdivitions, p5, p0, p3, count); + TessellateTriangle(subdivitions, p1, p5, p3, count); + TessellateTriangle(subdivitions, p4, p1, p3, count); + } + + private: + void TessellateTriangle(dgInt32 level, const dgVector& p0, const dgVector& p1, const dgVector& p2, dgInt32& count) + { + if (level) { + dgAssert(dgAbs(p0.DotProduct(p0).GetScalar() - dgFloat32(1.0f)) < dgFloat32(1.0e-4f)); + dgAssert(dgAbs(p1.DotProduct(p1).GetScalar() - dgFloat32(1.0f)) < dgFloat32(1.0e-4f)); + dgAssert(dgAbs(p2.DotProduct(p2).GetScalar() - dgFloat32(1.0f)) < dgFloat32(1.0e-4f)); + dgVector p01(p0 + p1); + dgVector p12(p1 + p2); + dgVector p20(p2 + p0); + + p01 = p01.Scale(dgRsqrt(p01.DotProduct(p01).GetScalar())); + p12 = p12.Scale(dgRsqrt(p12.DotProduct(p12).GetScalar())); + p20 = p20.Scale(dgRsqrt(p20.DotProduct(p20).GetScalar())); + + dgAssert(dgAbs(p01.DotProduct(p01).GetScalar() - dgFloat32(1.0f)) < dgFloat32(1.0e-4f)); + dgAssert(dgAbs(p12.DotProduct(p12).GetScalar() - dgFloat32(1.0f)) < dgFloat32(1.0e-4f)); + dgAssert(dgAbs(p20.DotProduct(p20).GetScalar() - dgFloat32(1.0f)) < dgFloat32(1.0e-4f)); + + TessellateTriangle(level - 1, p0, p01, p20, count); + TessellateTriangle(level - 1, p1, p12, p01, count); + TessellateTriangle(level - 1, p2, p20, p12, count); + TessellateTriangle(level - 1, p01, p12, p20, count); + } else { + dgBigPlane n(p0, p1, p2); + n = n.Scale(dgFloat64(1.0f) / sqrt(n.DotProduct3(n))); + n.m_w = dgFloat64(0.0f); + dgInt32 index = dgBitReversal(count, sizeof (m_normal) / sizeof (m_normal[0])); + m_normal[index] = n; + count++; + dgAssert(count <= sizeof (m_normal) / sizeof (m_normal[0])); + } + } + + public: + dgBigVector m_normal[32]; + dgInt32 m_count; + }; + + class dgClosePointsAccelerator: public dgMeshEffect::dgMeshBVH + { + public: + dgClosePointsAccelerator(const dgMeshEffect* const mesh) + :dgMeshEffect::dgMeshBVH(mesh) + { + Build(); + } + + dgMeshBVHNode* CreateLeafNode(dgEdge* const face, void* const userData) + { + dgMemoryAllocator* const allocator = m_mesh->GetAllocator(); + dgMeshBVHNode* const node = new (allocator)dgMeshBVHNode(m_mesh, face, userData); + + dgInt32 mark = m_mesh->GetLRU(); + dgAssert(mark != face->m_mark); + + dgEdge* faceEdge = face; + do { + faceEdge->m_mark = mark; + faceEdge = faceEdge->m_twin->m_next; + } while (faceEdge != face); + + dgVector padding(dgFloat32(1.0f / 32.0f)); + dgVector p(m_mesh->GetVertex(face->m_incidentVertex)); + dgVector p0(p - padding); + dgVector p1(p + padding); + node->SetBox(p0, p1); + return node; + } + + bool DoesTetrahedrumHasInsidePoints (const dgArray& points, const dgTetrahedra& tetra) const + { + dgBigVector box0(dgFloat64( 1.0e10f)); + dgBigVector box1(dgFloat64(-1.0e10f)); + for (dgInt32 i = 0; i < 4; i++) { + box0 = box0.GetMin(points[tetra[i]]); + box1 = box1.GetMax(points[tetra[i]]); + } + + dgBigVector padding (dgFloat64 (0.01f)); + box0 -= padding; + box1 += padding; + + dgList overlapNodes(m_mesh->GetAllocator()); + GetOverlapNodes(overlapNodes, box0, box1); + if (overlapNodes.GetCount()) { + dgBigVector p0(points[tetra[0]]); + dgBigVector p1(points[tetra[1]]); + dgBigVector p2(points[tetra[2]]); + dgBigVector p3(points[tetra[3]]); + + for (dgList::dgListNode* node = overlapNodes.GetFirst(); node; node = node->GetNext()) { + dgEdge* const edge = node->GetInfo()->m_face; + dgBigVector point(m_mesh->GetVertex(edge->m_incidentVertex)); + dgBigVector closestPoint(dgPointToTetrahedrumDistance(point, p0, p1, p2, p3)); + dgBigVector error(closestPoint - point); + dgFloat64 error2 = error.DotProduct3(error); + if (error2 < dgFloat64(1.0e-8f)) { + return true; + } + } + } + + return false; + } + }; + + class dgRayTraceAccelerator: public dgMeshEffect::dgMeshBVH + { + enum dgTraceType + { + m_pointSide, + m_pointOnSurface, + }; + + class dgRayTraceBase + { + public: + dgRayTraceBase(dgTraceType type) + :m_type(type) + { + } + dgTraceType m_type; + }; + + class dgRayTracePointSide: public dgRayTraceBase + { + public: + dgRayTracePointSide() + :dgRayTraceBase(m_pointSide) + ,m_hitCount(0) + ,m_rayIsDegenerate(true) + { + } + dgInt32 m_hitCount; + bool m_rayIsDegenerate; + }; + + class dgRayPointOnSurface: public dgRayTraceBase + { + public: + dgRayPointOnSurface() + :dgRayTraceBase(m_pointOnSurface) + ,m_param(2.0f) + { + } + + dgFloat32 m_param; + }; + + public: + dgRayTraceAccelerator(const dgMeshEffect* const mesh, dgFloat64 diameter) + :dgMeshEffect::dgMeshBVH(mesh) + ,m_normals() + ,m_diameter(diameter) + { + Build(); + } + + dgMeshBVHNode* CreateLeafNode(dgEdge* const face, void* const userData) + { + dgMemoryAllocator* const allocator = m_mesh->GetAllocator(); + dgMeshBVHNode* const node = new (allocator)dgMeshBVHNode(m_mesh, face, userData); + dgInt32 mark = m_mesh->GetLRU(); + dgAssert(mark != face->m_mark); + + dgEdge* faceEdge = face; + do { + faceEdge->m_mark = mark; + faceEdge = faceEdge->m_next; + } while (faceEdge != face); + return node; + } + + dgFloat64 dgPointToRayDistance (const dgBigVector& point, const dgBigVector& ray_p0, const dgBigVector& ray_p1, dgRayTracePointSide* const rayType) const + { + dgBigVector dp(ray_p1 - ray_p0); + dgFloat64 den = dp.DotProduct3(dp); + dgFloat64 num = dp.DotProduct3(point - ray_p0); + if ((num >= dgFloat64 (0.0f)) && (num <= den)) { + dgBigVector p (ray_p0 + dp.Scale (num / den)); + dgBigVector dist (point - p); + if (dist.DotProduct3(dist) < dgFloat64 (1.0e-12f)) { + rayType->m_rayIsDegenerate = true; + return dgFloat64 (-2.0f); + } + } + return dgFloat64 (2.0f); + } + + dgFloat64 PointSideTest(const dgMeshBVHNode* const faceNode, const dgBigVector& point0, const dgBigVector& point1, dgRayTracePointSide* const rayType) const + { + const dgEdge* const edge = faceNode->m_face; + const dgBigVector p0 (m_mesh->GetVertex(edge->m_incidentVertex)); + const dgBigVector p1 (m_mesh->GetVertex(edge->m_next->m_incidentVertex)); + const dgBigVector p2 (m_mesh->GetVertex(edge->m_next->m_next->m_incidentVertex)); + + 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(point0 - p0); + dgBigVector normal(e10.CrossProduct(e20)); + dgFloat64 t = -normal.DotProduct3(p0Point) / normal.DotProduct3(point1 - point0); + if ((t > dgFloat64(0.0f)) && (t < dgFloat64(1.0f))) { + dgBigVector point(point0 + (point1 - point0).Scale(t)); + dgBigVector variPoint(point - p0); + const dgFloat64 b0 = e10.DotProduct(variPoint).GetScalar(); + const dgFloat64 b1 = e20.DotProduct(variPoint).GetScalar(); + + dgFloat64 beta = b1 * a00 - a01 * b0; + dgFloat64 alpha = b0 * a11 - a01 * b1; + + if (beta <= dgFloat32(0.0f)) { + return dgPointToRayDistance (point, p0, p1, rayType); + } else if (alpha <= dgFloat32(0.0f)) { + return dgPointToRayDistance (point, p0, p2, rayType); + } else if ((alpha + beta) >= det) { + return dgPointToRayDistance (point, p1, p2, rayType); + } + rayType->m_hitCount ++; + } + } + return dgFloat64 (2.0f); + } + + + dgFloat64 PointSurfaceHit(const dgMeshBVHNode* const faceNode, const dgBigVector& point0, const dgBigVector& point1, dgRayPointOnSurface* const rayType) const + { + const dgEdge* const edge = faceNode->m_face; + const dgBigVector p0(m_mesh->GetVertex(edge->m_incidentVertex)); + const dgBigVector p1(m_mesh->GetVertex(edge->m_next->m_incidentVertex)); + const dgBigVector p2(m_mesh->GetVertex(edge->m_next->m_next->m_incidentVertex)); + + 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(point0 - p0); + dgBigVector normal(e10.CrossProduct(e20)); + dgFloat64 t = -normal.DotProduct3(p0Point) / normal.DotProduct3(point1 - point0); + if ((t > dgFloat64(0.0f)) && (t < dgFloat64(1.0f))) { + dgBigVector point(point0 + (point1 - point0).Scale(t)); + dgBigVector variPoint(point - p0); + const dgFloat64 b0 = e10.DotProduct(variPoint).GetScalar(); + const dgFloat64 b1 = e20.DotProduct(variPoint).GetScalar(); + + dgFloat64 beta = b1 * a00 - a01 * b0; + dgFloat64 alpha = b0 * a11 - a01 * b1; + + if (beta <= dgFloat32(0.0f)) { + return dgFloat64(2.0f); + } else if (alpha <= dgFloat32(0.0f)) { + return dgFloat64(2.0f); + } else if ((alpha + beta) >= det) { + return dgFloat64(2.0f); + } + if (t < rayType->m_param) { + rayType->m_param = dgFloat32 (t); + } + } + } + return dgFloat64(2.0f); + } + + dgFloat64 RayFaceIntersect(const dgMeshBVHNode* const faceNode, const dgBigVector& point0, const dgBigVector& point1, void* const userData) const + { + dgRayTraceBase* const rayType = (dgRayTraceBase*)userData; + + switch (rayType->m_type) + { + case m_pointSide: + return PointSideTest(faceNode, point0, point1, (dgRayTracePointSide*) rayType); + + case m_pointOnSurface: + return PointSurfaceHit(faceNode, point0, point1, (dgRayPointOnSurface*) rayType); + } + + dgAssert (0); + return dgFloat64 (-1.0f); + } + + dgVertexSign CalculateVertexSide (const dgBigVector& point0) const + { + dgRayTracePointSide hits; + for (dgInt32 i = 0; (i < m_normals.m_count) && hits.m_rayIsDegenerate; i ++) { + hits.m_hitCount = 0; + hits.m_rayIsDegenerate = false; + dgBigVector point1 (point0 + dgBigVector (m_normals.m_normal[i].Scale (m_diameter))); + FaceRayCast (point0, point1, &hits); + } + dgAssert (!hits.m_rayIsDegenerate); + return (hits.m_hitCount & 1) ? m_inside : m_outside; + } + + dgFloat32 CalculateEdgeCut (const dgBigVector& point0, const dgBigVector& point1) const + { + dgRayPointOnSurface pointOnSurface; + FaceRayCast (point0, point1, &pointOnSurface); + return pointOnSurface.m_param; + } + + dgNormalMap m_normals; + dgFloat64 m_diameter; + }; + + dgTetraIsoSufaceStuffing(const dgMeshEffect* const mesh, dgFloat64 cellSize) + :m_points(mesh->GetAllocator()) + ,m_tetraList(mesh->GetAllocator()) + ,m_pointCount(0) + ,m_tetraCount(0) + { + dgArray vertexSide(mesh->GetAllocator()); + dgArray tetraGraph(mesh->GetAllocator()); + dgArray tetraEdgeCuts(mesh->GetAllocator()); + + dgGridDimension gridDim (CalculateGridSize(mesh, cellSize)); + dgClosePointsAccelerator closePointaAccelerator (mesh); + dgRayTraceAccelerator rayAccelerator (mesh, gridDim.m_diameter); + + PopulateGridPoints (gridDim); + CalculateVertexSide (vertexSide, rayAccelerator); + BuildTetraGraph (gridDim, vertexSide, closePointaAccelerator, tetraGraph); + //CalculateEdgeCuts (tetraEdgeCuts, tetraGraph, vertexSide, rayAccelerator); + //SnapClosePoints (tetraEdgeCuts, tetraGraph, vertexSide, rayAccelerator); + } + + void CalculateEdgeCuts (dgArray& tetraEdgeCuts, const dgArray& tetraGraph, const dgArray& vertexSide, const dgRayTraceAccelerator& rayAccelerator) + { + tetraEdgeCuts.Resize(m_tetraCount); + for (dgInt32 i = 0; i < m_tetraCount; i ++) { + tetraEdgeCuts[i] = dgTetraEdgeCuts(); + for (dgInt32 j = 0; j < 6; j ++) { + tetraEdgeCuts[i].PushBack(dgFloat32(-1.0f)); + } + } + + for (dgInt32 i = 0; i < m_pointCount; i ++) { + if (vertexSide[i] == m_outside) { + const dgTetraToVertexNode& graphNode = tetraGraph[i]; + for (dgInt32 j = 0; j < graphNode.GetCount(); j ++) { + dgTetraEdgeCuts& cuts = tetraEdgeCuts[graphNode[j]]; + const dgTetrahedra& tetra = m_tetraList[graphNode[j]]; + dgAssert ((tetra[0] == i) || (tetra[1] == i) || (tetra[2] == i) || (tetra[3] == i)); + + dgInt32 index = 0; + for (dgInt32 i0 = 0; i0 < 3; i0 ++) { + const dgBigVector& p0 = m_points[tetra[i0]]; + for (dgInt32 i1 = i0 + 1; i1 < 4; i1 ++) { + if ((tetra[i0] == i) && (vertexSide[tetra[i1]] == m_inside)) { + const dgBigVector& p1 = m_points[tetra[i1]]; + dgFloat32 param = rayAccelerator.CalculateEdgeCut (p0, p1); + cuts[index] = param; + } + index ++; + dgAssert (index <= 6); + } + } + } + } + } + } + + void SnapClosePoints (dgArray& tetraEdgeCuts, const dgArray& tetraGraph, const dgArray& vertexSide, const dgRayTraceAccelerator& rayAccelerator) + { + for (dgInt32 i = 0; i < m_pointCount; i++) { + if (vertexSide[i] == m_outside) { +/* + const dgTetraToVertexNode& graphNode = tetraGraph[i]; + for (dgInt32 j = 0; j < graphNode.GetCount(); j++) { + dgTetraEdgeCuts& cuts = tetraEdgeCuts[graphNode[j]]; + const dgTetrahedra& tetra = m_tetraList[graphNode[j]]; + dgAssert((tetra[0] == i) || (tetra[1] == i) || (tetra[2] == i) || (tetra[3] == i)); + + dgInt32 index = 0; + for (dgInt32 i0 = 0; i0 < 3; i0++) { + const dgBigVector& p0 = m_points[tetra[i0]]; + for (dgInt32 i1 = i0 + 1; i1 < 4; i1++) { + if ((tetra[i0] == i) && (vertexSide[tetra[i1]] == m_inside)) { + const dgBigVector& p1 = m_points[tetra[i1]]; + dgFloat32 param = rayAccelerator.CalculateEdgeCut(p0, p1); + cuts[index] = param; + } + index++; + dgAssert(index <= 6); + } + } + } +*/ + } + } + } + + void CalculateVertexSide (dgArray& vertexSide, const dgRayTraceAccelerator& rayAccelerator) + { + vertexSide.Resize(m_pointCount); + for (dgInt32 i = 0; i < m_pointCount; i ++) { + vertexSide[i] = rayAccelerator.CalculateVertexSide (m_points[i]); + } + } + + dgGridDimension CalculateGridSize(const dgMeshEffect* const mesh, dgFloat64 cellsize) const + { + dgBigVector minBox; + dgBigVector maxBox; + mesh->CalculateAABB(minBox, maxBox); + minBox -= (maxBox - minBox).Scale(dgFloat64(1.e-3f)); + maxBox += (maxBox - minBox).Scale(dgFloat64(1.e-3f)); + + dgBigVector mMinInt((minBox.Scale(dgFloat64(1.0f) / cellsize)).Floor()); + dgBigVector mMaxInt((maxBox.Scale(dgFloat64(1.0f) / cellsize)).Floor() + dgBigVector::m_one); + dgBigVector gridSize(mMaxInt - mMinInt + dgBigVector::m_one); + + dgBigVector size(maxBox - minBox); + + dgGridDimension gridDimension; + gridDimension.m_origin = minBox; + gridDimension.m_cellSize = cellsize; + gridDimension.m_diameter = sqrt (size.DotProduct3(size)); + gridDimension.m_gridSizeX = dgInt32(gridSize.m_x); + gridDimension.m_gridSizeY = dgInt32(gridSize.m_y); + gridDimension.m_gridSizeZ = dgInt32(gridSize.m_z); + + gridDimension.m_innerSize = gridDimension.m_gridSizeX * gridDimension.m_gridSizeY * gridDimension.m_gridSizeZ; + gridDimension.m_outerSize = gridDimension.m_innerSize + (gridDimension.m_gridSizeX + 1) * (gridDimension.m_gridSizeY + 1) * (gridDimension.m_gridSizeZ + 1); + return gridDimension; + } + + void PopulateGridPoints(const dgGridDimension& gridDimension) + { + m_pointCount = 0; + m_points.Resize(gridDimension.m_outerSize); + + for (dgInt32 z = 0; z < gridDimension.m_gridSizeZ; z++) { + for (dgInt32 y = 0; y < gridDimension.m_gridSizeY; y++) { + for (dgInt32 x = 0; x < gridDimension.m_gridSizeX; x++) { + m_points[m_pointCount] = gridDimension.m_origin + dgBigVector(x * gridDimension.m_cellSize, y * gridDimension.m_cellSize, z * gridDimension.m_cellSize, dgFloat64(0.0f)); + m_pointCount++; + } + } + } + + dgBigVector outerOrigin(gridDimension.m_origin - dgBigVector(gridDimension.m_cellSize * dgFloat64(0.5f))); + outerOrigin.m_w = dgFloat64 (0.0f); + for (dgInt32 z = 0; z < gridDimension.m_gridSizeZ + 1; z++) { + for (dgInt32 y = 0; y < gridDimension.m_gridSizeY + 1; y++) { + for (dgInt32 x = 0; x < gridDimension.m_gridSizeX + 1; x++) { + m_points[m_pointCount] = outerOrigin + dgBigVector(x * gridDimension.m_cellSize, y * gridDimension.m_cellSize, z * gridDimension.m_cellSize, dgFloat64(0.0f)); + m_pointCount++; + } + } + } + } + + void AddTetra(dgArray& graph, const dgTetrahedra& tetra, const dgArray& vertexSigns, const dgClosePointsAccelerator& closePoint) + { + dgAssert(CalculateVolume(tetra) > dgFloat64(0.0f)); + bool hasInsizePoints = false; + hasInsizePoints = hasInsizePoints || (vertexSigns[tetra[0]] == m_inside); + hasInsizePoints = hasInsizePoints || (vertexSigns[tetra[1]] == m_inside); + hasInsizePoints = hasInsizePoints || (vertexSigns[tetra[2]] == m_inside); + hasInsizePoints = hasInsizePoints || (vertexSigns[tetra[3]] == m_inside); + hasInsizePoints = hasInsizePoints || closePoint.DoesTetrahedrumHasInsidePoints(m_points, tetra); + + if (hasInsizePoints) { + m_tetraList[m_tetraCount] = tetra; + dgTetrahedra& tetraEntry = m_tetraList[m_tetraCount]; + for (dgInt32 i = 0; i < 4; i ++) { + dgInt32 vertexIndex = tetra[i]; + tetraEntry.PushBack(vertexIndex); + graph[vertexIndex].PushBack(m_tetraCount); + } + m_tetraCount ++; + } + } + + void BuildTetraGraph(const dgGridDimension& gridDimension, const dgArray& vertexSigns, const dgClosePointsAccelerator& closePoint, dgArray& graph) + { + graph.Resize(m_pointCount); + for (dgInt32 i = 0; i < m_pointCount; i ++) { + graph[i] = dgTetraToVertexNode(); + } + + dgDelaunayTetrahedralization delaunayTetrahedras(m_points.GetAllocator(), &m_points[0].m_x, m_pointCount, sizeof (dgBigVector), dgFloat32(0.0f)); + delaunayTetrahedras.RemoveUpperHull(); + + for (dgDelaunayTetrahedralization::dgListNode* node = delaunayTetrahedras.GetFirst(); node; node = node->GetNext()) { + dgTetrahedra stuffingTetra; + dgConvexHull4dTetraherum& delaunayTetra = node->GetInfo(); + + for (dgInt32 i = 0; i < 4; i ++) { + stuffingTetra[i] = delaunayTetra.m_faces[0].m_index[i]; + } + dgFloat64 volume = CalculateVolume(stuffingTetra); + if (volume < dgFloat64 (0.0f)) { + dgSwap(stuffingTetra[0], stuffingTetra[1]); + } + AddTetra(graph, stuffingTetra, vertexSigns, closePoint); + } + + +/* + const dgInt32 base = gridDimension.m_innerSize; + for (dgInt32 z = 0; z < gridDimension.m_gridSizeZ; z++) { + for (dgInt32 y = 0; y < gridDimension.m_gridSizeY; y++) { + for (dgInt32 x = 0; x < gridDimension.m_gridSizeX - 1; x++) { + dgTetrahedra tetra; + tetra[0] = ((z * gridDimension.m_gridSizeY + y) * gridDimension.m_gridSizeX) + x; + tetra[1] = ((z * gridDimension.m_gridSizeY + y) * gridDimension.m_gridSizeX) + x + 1; + tetra[2] = base + (((z + 0) * (gridDimension.m_gridSizeY + 1) + y + 0) * (gridDimension.m_gridSizeX + 1)) + x + 1; + tetra[3] = base + (((z + 0) * (gridDimension.m_gridSizeY + 1) + y + 1) * (gridDimension.m_gridSizeX + 1)) + x + 1; + AddTetra(graph, tetra, vertexSigns, closePoint); + + tetra[0] = ((z * gridDimension.m_gridSizeY + y) * gridDimension.m_gridSizeX) + x; + tetra[1] = ((z * gridDimension.m_gridSizeY + y) * gridDimension.m_gridSizeX) + x + 1; + tetra[3] = base + (((z + 1) * (gridDimension.m_gridSizeY + 1) + y + 0) * (gridDimension.m_gridSizeX + 1)) + x + 1; + tetra[2] = base + (((z + 1) * (gridDimension.m_gridSizeY + 1) + y + 1) * (gridDimension.m_gridSizeX + 1)) + x + 1; + AddTetra(graph, tetra, vertexSigns, closePoint); + } + } + } + + for (dgInt32 z = 0; z < gridDimension.m_gridSizeZ; z++) { + for (dgInt32 y = 0; y < gridDimension.m_gridSizeY - 1; y++) { + for (dgInt32 x = 0; x < gridDimension.m_gridSizeX; x++) { + dgTetrahedra tetra; + tetra[0] = ((z * gridDimension.m_gridSizeY + (y + 0)) * gridDimension.m_gridSizeX) + x; + tetra[1] = ((z * gridDimension.m_gridSizeY + (y + 1)) * gridDimension.m_gridSizeX) + x; + tetra[3] = base + (((z + 0) * (gridDimension.m_gridSizeY + 1) + y + 1) * (gridDimension.m_gridSizeX + 1)) + x + 1; + tetra[2] = base + (((z + 1) * (gridDimension.m_gridSizeY + 1) + y + 1) * (gridDimension.m_gridSizeX + 1)) + x + 1; + AddTetra(graph, tetra, vertexSigns, closePoint); + + tetra[0] = ((z * gridDimension.m_gridSizeY + (y + 0)) * gridDimension.m_gridSizeX) + x; + tetra[1] = ((z * gridDimension.m_gridSizeY + (y + 1)) * gridDimension.m_gridSizeX) + x; + tetra[2] = base + (((z + 0) * (gridDimension.m_gridSizeY + 1) + y + 1) * (gridDimension.m_gridSizeX + 1)) + x + 0; + tetra[3] = base + (((z + 1) * (gridDimension.m_gridSizeY + 1) + y + 1) * (gridDimension.m_gridSizeX + 1)) + x + 0; + AddTetra(graph, tetra, vertexSigns, closePoint); + } + } + } + + for (dgInt32 z = 0; z < gridDimension.m_gridSizeZ - 1; z++) { + for (dgInt32 y = 0; y < gridDimension.m_gridSizeY; y++) { + for (dgInt32 x = 0; x < gridDimension.m_gridSizeX; x++) { + dgTetrahedra tetra; + tetra[0] = (((z + 0) * gridDimension.m_gridSizeY + y + 0) * gridDimension.m_gridSizeX) + x; + tetra[1] = (((z + 1) * gridDimension.m_gridSizeY + y + 0) * gridDimension.m_gridSizeX) + x; + tetra[3] = base + (((z + 1) * (gridDimension.m_gridSizeY + 1) + y + 0) * (gridDimension.m_gridSizeX + 1)) + x + 0; + tetra[2] = base + (((z + 1) * (gridDimension.m_gridSizeY + 1) + y + 1) * (gridDimension.m_gridSizeX + 1)) + x + 0; + AddTetra(graph, tetra, vertexSigns, closePoint); + + tetra[0] = (((z + 0) * gridDimension.m_gridSizeY + y + 0) * gridDimension.m_gridSizeX) + x; + tetra[1] = (((z + 1) * gridDimension.m_gridSizeY + y + 0) * gridDimension.m_gridSizeX) + x; + tetra[2] = base + (((z + 1) * (gridDimension.m_gridSizeY + 1) + y + 0) * (gridDimension.m_gridSizeX + 1)) + x + 1; + tetra[3] = base + (((z + 1) * (gridDimension.m_gridSizeY + 1) + y + 1) * (gridDimension.m_gridSizeX + 1)) + x + 1; + AddTetra(graph, tetra, vertexSigns, closePoint); + } + } + } + + for (dgInt32 z = 0; z < gridDimension.m_gridSizeZ; z++) { + for (dgInt32 y = 0; y < gridDimension.m_gridSizeY - 1; y++) { + for (dgInt32 x = 0; x < gridDimension.m_gridSizeX; x++) { + dgTetrahedra tetra; + tetra[0] = ((z * gridDimension.m_gridSizeY + (y + 0)) * gridDimension.m_gridSizeX) + x; + tetra[1] = ((z * gridDimension.m_gridSizeY + (y + 1)) * gridDimension.m_gridSizeX) + x; + tetra[2] = base + (((z + 1) * (gridDimension.m_gridSizeY + 1) + y + 1) * (gridDimension.m_gridSizeX + 1)) + x + 0; + tetra[3] = base + (((z + 1) * (gridDimension.m_gridSizeY + 1) + y + 1) * (gridDimension.m_gridSizeX + 1)) + x + 1; + AddTetra(graph, tetra, vertexSigns, closePoint); + + tetra[0] = ((z * gridDimension.m_gridSizeY + (y + 0)) * gridDimension.m_gridSizeX) + x; + tetra[1] = ((z * gridDimension.m_gridSizeY + (y + 1)) * gridDimension.m_gridSizeX) + x; + tetra[3] = base + (((z + 0) * (gridDimension.m_gridSizeY + 1) + y + 1) * (gridDimension.m_gridSizeX + 1)) + x + 0; + tetra[2] = base + (((z + 0) * (gridDimension.m_gridSizeY + 1) + y + 1) * (gridDimension.m_gridSizeX + 1)) + x + 1; + AddTetra(graph, tetra, vertexSigns, closePoint); + } + } + } +*/ + } + + dgFloat64 CalculateVolume(const dgTetrahedra& tetra) const + { + const dgBigVector& p0 = m_points[tetra[0]]; + const dgBigVector& p1 = m_points[tetra[1]]; + const dgBigVector& p2 = m_points[tetra[2]]; + const dgBigVector& p3 = m_points[tetra[3]]; + dgBigVector p10(p1 - p0); + dgBigVector p20(p2 - p0); + dgBigVector p30(p3 - p0); + return p10.DotProduct3(p20.CrossProduct(p30)); + } + + dgArray m_points; + dgArray m_tetraList; + dgInt32 m_pointCount; + dgInt32 m_tetraCount; +}; + +void dgMeshEffect::LoadOffMesh(const char* const fileName) +{ + class ParceOFF + { + public: + enum Token + { + m_off, + m_value, + m_end, + }; + + ParceOFF(FILE* const file) + :m_file(file) + { + } + + Token GetToken(char* const buffer) const + { + while (!feof(m_file) && fscanf(m_file, "%s", buffer)) { + if (buffer[0] == '#') { + SkipLine(); + } else { + if (!_stricmp(buffer, "OFF")) { + return m_off; + } + return m_value; + } + } + return m_end; + } + + char* SkipLine() const + { + char tmp[1024]; + return fgets(tmp, sizeof (tmp), m_file); + } + + dgInt32 GetInteger() const + { + char buffer[1024]; + GetToken(buffer); + return atoi(buffer); + } + + dgFloat64 GetFloat() const + { + char buffer[1024]; + GetToken(buffer); + return atof(buffer); + } + + FILE* m_file; + }; + + FILE* const file = fopen(fileName, "rb"); + if (file) { + ParceOFF parcel(file); + + dgInt32 vertexCount = 0; + dgInt32 faceCount = 0; + //dgInt32 edgeCount = 0; + + char buffer[1024]; + bool stillData = true; + while (stillData) { + ParceOFF::Token token = parcel.GetToken(buffer); + switch (token) + { + case ParceOFF::m_off: + { + vertexCount = parcel.GetInteger(); + faceCount = parcel.GetInteger(); + // edgeCount = parcel.GetInteger(); + parcel.SkipLine(); + + dgArray points(GetAllocator()); + for (dgInt32 i = 0; i < vertexCount; i++) { + dgFloat64 x = parcel.GetFloat(); + dgFloat64 y = parcel.GetFloat(); + dgFloat64 z = parcel.GetFloat(); + dgBigVector p(x, y, z, dgFloat32(0.0f)); + points[i] = p; + } + + dgArray indexList(GetAllocator()); + dgArray faceVertex(GetAllocator()); + dgInt32 index = 0; + for (dgInt32 i = 0; i < faceCount; i++) { + const dgInt32 faceVertexCount = parcel.GetInteger(); + faceVertex[i] = faceVertexCount; + for (dgInt32 j = 0; j < faceVertexCount; j++) { + indexList[index] = parcel.GetInteger(); + index++; + } + parcel.SkipLine(); + } + + dgMeshVertexFormat vertexFormat; + vertexFormat.m_faceCount = faceCount; + vertexFormat.m_faceIndexCount = &faceVertex[0]; + + vertexFormat.m_vertex.m_data = &points[0].m_x; + vertexFormat.m_vertex.m_strideInBytes = sizeof (dgBigVector); + vertexFormat.m_vertex.m_indexList = &indexList[0]; + BuildFromIndexList(&vertexFormat); + + CalculateNormals(30.0f * dgDegreeToRad); + stillData = false; + break; + } + + default:; + } + } + + fclose(file); + } +} + +void dgMeshEffect::LoadTetraMesh (const char* const filename) +{ + FILE* const file = fopen(filename, "rb"); + if (file) { + dgInt32 vertexCount; + size_t ret = fscanf(file, "%d", &vertexCount); + dgArray points(GetAllocator()); + for (dgInt32 i = 0; i < vertexCount; i ++) { + float x; + float y; + float z; + ret = fscanf(file, "%f %f %f", &x, &y, &z); + points[i] = dgBigVector (x, y, z, dgFloat32 (0.0f)); + } + + BeginBuild(); + dgInt32 tetras; + ret = fscanf(file, "%d", &tetras); + dgMemoryAllocator* const allocator = GetAllocator(); + for (dgInt32 layers = 0; layers < tetras; layers ++) { + dgInt32 tetra[4]; + ret = fscanf(file, "%d %d %d %d", &tetra[0], &tetra[1], &tetra[2], &tetra[3]); + ret = 0; + dgBigVector pointArray[4]; + for (dgInt32 i = 0; i < 4; i++) { + dgInt32 index = tetra[i]; + pointArray[i] = points[index]; + } + + dgMeshEffect convexMesh(allocator, &pointArray[0].m_x, 4, sizeof (dgBigVector), dgFloat64(0.0f)); + + dgAssert(convexMesh.GetCount()); + convexMesh.CalculateNormals(dgFloat32(30.0f * dgDegreeToRad)); + for (dgInt32 i = 0; i < convexMesh.m_points.m_vertex.m_count; i++) { + convexMesh.m_points.m_layers[i] = layers; + } + MergeFaces(&convexMesh); + } + EndBuild(dgFloat64(1.0e-8f), false); + fclose(file); + } +} + +dgMeshEffect* dgMeshEffect::CreateVoronoiConvexDecomposition (dgMemoryAllocator* const allocator, dgInt32 pointCount, dgInt32 pointStrideInBytes, const dgFloat32* const pointCloud, dgInt32 materialId, const dgMatrix& textureProjectionMatrix) +{ + dgStack buffer(pointCount + 16); + dgBigVector* const pool = &buffer[0]; + dgInt32 count = 0; + dgFloat64 quantizeFactor = dgFloat64 (16.0f); + dgFloat64 invQuantizeFactor = dgFloat64 (1.0f) / quantizeFactor; + dgInt32 stride = pointStrideInBytes / sizeof (dgFloat32); + + dgBigVector pMin (dgFloat32 (1.0e10f), dgFloat32 (1.0e10f), dgFloat32 (1.0e10f), dgFloat32 (0.0f)); + dgBigVector pMax (dgFloat32 (-1.0e10f), dgFloat32 (-1.0e10f), dgFloat32 (-1.0e10f), dgFloat32 (0.0f)); + for (dgInt32 i = 0; i < pointCount; i ++) { + dgFloat64 x = pointCloud[i * stride + 0]; + dgFloat64 y = pointCloud[i * stride + 1]; + dgFloat64 z = pointCloud[i * stride + 2]; + x = floor (x * quantizeFactor) * invQuantizeFactor; + y = floor (y * quantizeFactor) * invQuantizeFactor; + z = floor (z * quantizeFactor) * invQuantizeFactor; + dgBigVector p (x, y, z, dgFloat64 (0.0f)); + pMin = dgBigVector (dgMin (x, pMin.m_x), dgMin (y, pMin.m_y), dgMin (z, pMin.m_z), dgFloat64 (0.0f)); + pMax = dgBigVector (dgMax (x, pMax.m_x), dgMax (y, pMax.m_y), dgMax (z, pMax.m_z), dgFloat64 (0.0f)); + pool[count] = p; + count ++; + } + // add the bbox as a barrier + pool[count + 0] = dgBigVector ( pMin.m_x, pMin.m_y, pMin.m_z, dgFloat64 (0.0f)); + pool[count + 1] = dgBigVector ( pMax.m_x, pMin.m_y, pMin.m_z, dgFloat64 (0.0f)); + pool[count + 2] = dgBigVector ( pMin.m_x, pMax.m_y, pMin.m_z, dgFloat64 (0.0f)); + pool[count + 3] = dgBigVector ( pMax.m_x, pMax.m_y, pMin.m_z, dgFloat64 (0.0f)); + pool[count + 4] = dgBigVector ( pMin.m_x, pMin.m_y, pMax.m_z, dgFloat64 (0.0f)); + pool[count + 5] = dgBigVector ( pMax.m_x, pMin.m_y, pMax.m_z, dgFloat64 (0.0f)); + pool[count + 6] = dgBigVector ( pMin.m_x, pMax.m_y, pMax.m_z, dgFloat64 (0.0f)); + pool[count + 7] = dgBigVector ( pMax.m_x, pMax.m_y, pMax.m_z, dgFloat64 (0.0f)); + count += 8; + + dgStack indexList(count); + count = dgVertexListToIndexList(&pool[0].m_x, sizeof (dgBigVector), 3, count, &indexList[0], dgFloat64 (5.0e-2f)); + dgAssert (count >= 8); + + dgFloat64 maxSize = dgMax(pMax.m_x - pMin.m_x, pMax.m_y - pMin.m_y, pMax.m_z - pMin.m_z); + pMin -= dgBigVector (maxSize, maxSize, maxSize, dgFloat64 (0.0f)); + pMax += dgBigVector (maxSize, maxSize, maxSize, dgFloat64 (0.0f)); + + // add the a guard zone, so that we do no have to clip + dgInt32 guardVertexKey = count; + pool[count + 0] = dgBigVector ( pMin.m_x, pMin.m_y, pMin.m_z, dgFloat64 (0.0f)); + pool[count + 1] = dgBigVector ( pMax.m_x, pMin.m_y, pMin.m_z, dgFloat64 (0.0f)); + pool[count + 2] = dgBigVector ( pMin.m_x, pMax.m_y, pMin.m_z, dgFloat64 (0.0f)); + pool[count + 3] = dgBigVector ( pMax.m_x, pMax.m_y, pMin.m_z, dgFloat64 (0.0f)); + pool[count + 4] = dgBigVector ( pMin.m_x, pMin.m_y, pMax.m_z, dgFloat64 (0.0f)); + pool[count + 5] = dgBigVector ( pMax.m_x, pMin.m_y, pMax.m_z, dgFloat64 (0.0f)); + pool[count + 6] = dgBigVector ( pMin.m_x, pMax.m_y, pMax.m_z, dgFloat64 (0.0f)); + pool[count + 7] = dgBigVector ( pMax.m_x, pMax.m_y, pMax.m_z, dgFloat64 (0.0f)); + count += 8; + + dgDelaunayTetrahedralization delaunayTetrahedras (allocator, &pool[0].m_x, count, sizeof (dgBigVector), dgFloat32 (0.0f)); + delaunayTetrahedras.RemoveUpperHull (); + +// delaunayTetrahedras.Save("xxx0.txt"); + dgInt32 tetraCount = delaunayTetrahedras.GetCount(); + dgStack voronoiPoints(tetraCount + 32); + dgStack tetradrumNode(tetraCount); + dgTree, dgInt32> delaunayNodes (allocator); + + dgInt32 index = 0; + const dgConvexHull4dVector* const convexHulPoints = delaunayTetrahedras.GetHullVertexArray(); + for (dgDelaunayTetrahedralization::dgListNode* node = delaunayTetrahedras.GetFirst(); node; node = node->GetNext()) { + dgConvexHull4dTetraherum& tetra = node->GetInfo(); + voronoiPoints[index] = tetra.CircumSphereCenter (convexHulPoints); + tetradrumNode[index] = node; + + for (dgInt32 i = 0; i < 4; i ++) { + dgTree, dgInt32>::dgTreeNode* header = delaunayNodes.Find(tetra.m_faces[0].m_index[i]); + if (!header) { + dgList list (allocator); + header = delaunayNodes.Insert(list, tetra.m_faces[0].m_index[i]); + } + header->GetInfo().Append (index); + } + index ++; + } + + const dgFloat32 normalAngleInRadians = dgFloat32 (30.0f * dgDegreeToRad); + dgMeshEffect* const voronoiPartition = new (allocator) dgMeshEffect (allocator); + voronoiPartition->BeginBuild(); + dgInt32 layer = 0; + dgTree, dgInt32>::Iterator iter (delaunayNodes); + for (iter.Begin(); iter; iter ++) { + dgTree, dgInt32>::dgTreeNode* const nodeNode = iter.GetNode(); + const dgList& list = nodeNode->GetInfo(); + dgInt32 key = nodeNode->GetKey(); + + if (key < guardVertexKey) { + dgBigVector pointArray[512]; + dgInt32 indexArray[512]; + + dgInt32 count1 = 0; + for (dgList::dgListNode* ptr = list.GetFirst(); ptr; ptr = ptr->GetNext()) { + dgInt32 i = ptr->GetInfo(); + pointArray[count1] = voronoiPoints[i]; + count1 ++; + dgAssert (count1 < dgInt32 (sizeof (pointArray) / sizeof (pointArray[0]))); + } + + count1 = dgVertexListToIndexList(&pointArray[0].m_x, sizeof (dgBigVector), 3, count1, &indexArray[0], dgFloat64 (1.0e-3f)); + if (count1 >= 4) { + dgMeshEffect convexMesh (allocator, &pointArray[0].m_x, count1, sizeof (dgBigVector), dgFloat64 (0.0f)); + if (convexMesh.GetCount()) { + convexMesh.CalculateNormals(normalAngleInRadians); + convexMesh.UniformBoxMapping (materialId, textureProjectionMatrix); + for (dgInt32 i = 0; i < convexMesh.m_points.m_vertex.m_count; i ++) { + convexMesh.m_points.m_layers[i] = layer; + } + voronoiPartition->MergeFaces(&convexMesh); + layer ++; + } + } + } + } + voronoiPartition->EndBuild(dgFloat64 (1.0e-8f), false); + //voronoiPartition->SaveOFF("xxx0.off"); + return voronoiPartition; +} + + +dgMeshEffect* dgMeshEffect::CreateTetrahedraIsoSurface() const +{ +/* +dgMeshEffect xxxx (GetAllocator()); +xxxx.BeginBuild(); + +xxxx.BeginBuildFace (); +xxxx.AddPoint (0.0, 0.0, -1.0); +xxxx.AddLayer (0); + +xxxx.AddPoint (1.0, 0.0, 0.0); +xxxx.AddLayer (0); + +xxxx.AddPoint (0.0, 0.0, 1.0); +xxxx.AddLayer (0); +xxxx.EndBuildFace (); + +xxxx.BeginBuildFace (); +xxxx.AddPoint (0.0, 0.0, -1.0); +xxxx.AddLayer (1); + +xxxx.AddPoint (0.0, 0.0, 1.0); +xxxx.AddLayer (1); + +xxxx.AddPoint (-1.0, 0.0, 0.0); +xxxx.AddLayer (1); + +xxxx.EndBuildFace (); +xxxx.EndBuild(dgFloat64(1.0e-8f), false); +*/ + + + dgTetraIsoSufaceStuffing tetraIsoStuffing (this, dgFloat64(0.125f)); + + dgMeshEffect* delaunayPartition = NULL; + if (tetraIsoStuffing.m_tetraCount) { + dgMemoryAllocator* const allocator = GetAllocator(); + delaunayPartition = new (allocator) dgMeshEffect (allocator); + delaunayPartition->BeginBuild(); + dgInt32 layer = 0; + dgBigVector pointArray[4]; + for (dgInt32 j = 0; j < tetraIsoStuffing.m_tetraCount; j ++) { + dgTetraIsoSufaceStuffing::dgTetrahedra& tetra = tetraIsoStuffing.m_tetraList[j]; + for (dgInt32 i = 0; i < 4; i ++) { + dgInt32 index = tetra[i]; + pointArray[i] = tetraIsoStuffing.m_points[index]; + } + dgMeshEffect convexMesh(allocator, &pointArray[0].m_x, 4, sizeof (dgBigVector), dgFloat64(0.0f)); + //dgAssert (convexMesh.GetCount()); + //convexMesh.CalculateNormals(dgFloat32 (30.0f * dgDEG2RAD)); + for (dgInt32 i = 0; i < convexMesh.m_points.m_vertex.m_count; i++) { + convexMesh.m_points.m_layers[i] = layer; + } + delaunayPartition->MergeFaces(&convexMesh); + layer++; + } + delaunayPartition->EndBuild(dgFloat64(1.0e-8f), false); + } + + return delaunayPartition; +} + +void dgMeshEffect::CreateTetrahedraLinearBlendSkinWeightsChannel (const dgMeshEffect* const tetrahedraMesh) +{ +dgAssert(0); +/* + dgRayTrataAABBAccelerator accelerator (tetrahedraMesh); + m_points.m_weights.Clear(); + m_points.m_weights.Reserve(m_points.m_vertex.m_count); + + dgBigVector padding (dgFloat64(1.0f / 32.0f)); + for (dgInt32 i = 0; i < m_points.m_weights.m_count; i ++) { + dgBigVector p (m_points.m_vertex[i]); + dgBigVector p0 (p - padding); + dgBigVector p1 (p + padding); + dgList overlapNodes (GetAllocator()); + accelerator.GetOverlapNodes (overlapNodes, p0, p1); + dgAssert (overlapNodes.GetCount()); + + bool weightFound = false; + for (dgList::dgListNode* node = overlapNodes.GetFirst(); node; node = node->GetNext()) { + dgEdge* const edge = node->GetInfo()->m_face; + + dgInt32 i0 = edge->m_incidentVertex; + dgInt32 i1 = edge->m_next->m_incidentVertex; + dgInt32 i2 = edge->m_prev->m_incidentVertex; + dgInt32 i3 = edge->m_twin->m_prev->m_incidentVertex; + dgBigVector q0 (tetrahedraMesh->m_points.m_vertex[i0]); + dgBigVector q1 (tetrahedraMesh->m_points.m_vertex[i1]); + dgBigVector q2 (tetrahedraMesh->m_points.m_vertex[i2]); + dgBigVector q3 (tetrahedraMesh->m_points.m_vertex[i3]); + + const dgBigVector e10(q1 - q0); + const dgBigVector e20(q2 - q0); + const dgBigVector e30(q3 - q0); + + dgAssert (e10.DotProduct(e10).GetScalar() > dgFloat32 (0.0f)); + const dgFloat64 d0 = sqrt(e10.DotProduct(e10).GetScalar()); + const dgFloat64 invd0 = dgFloat64(1.0f) / d0; + const dgFloat64 l10 = e20.DotProduct(e10).GetScalar() * invd0; + const dgFloat64 l20 = e30.DotProduct(e10).GetScalar() * invd0; + + dgAssert ((e20.DotProduct(e20).GetScalar() - l10 * l10) > dgFloat32 (0.0f)); + const dgFloat64 desc11 = e20.DotProduct(e20).GetScalar() - l10 * l10; + + const dgFloat64 d1 = sqrt(desc11); + const dgFloat64 invd1 = dgFloat64(1.0f) / d1; + const dgFloat64 l21 = (e30.DotProduct(e20).GetScalar() - l20 * l10) * invd1; + dgAssert (e30.DotProduct(e30).GetScalar() - l20 * l20 - l21 * l21 > dgFloat32 (0.0f)); + const dgFloat64 desc22 = e30.DotProduct(e30).GetScalar() - l20 * l20 - l21 * l21; + + dgBigVector p0Point(p - q0); + 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; + + u3 = u3 * invd2; + u2 = (u2 - l21 * u3) * invd1; + u1 = (u1 - l10 * u2 - l20 * u3) * invd0; + if ((u1 >= dgFloat64(0.0f)) && (u2 >= dgFloat64(0.0f)) && (u3 >= dgFloat64(0.0f)) && ((u1 + u2 + u3) <= dgFloat64(1.0f))) { + dgBigVector r0 (q0 + e10.Scale(u1) + e20.Scale(u2) + e30.Scale(u3)); + + dgFloat64 u0 = dgFloat64 (1.0f) - u1 - u2 - u3; + dgBigVector r1 (q0.Scale (u0) + q1.Scale (u1) + q2.Scale (u2) + q3.Scale (u3)); + dgWeights& weighSet = m_points.m_weights[i]; + + weighSet.m_controlIndex[0] = i0; + weighSet.m_weightBlends[0] = dgFloat32(u0); + + weighSet.m_controlIndex[1] = i1; + weighSet.m_weightBlends[1] = dgFloat32(u1); + + weighSet.m_controlIndex[2] = i2; + weighSet.m_weightBlends[2] = dgFloat32(u2); + + weighSet.m_controlIndex[3] = i3; + weighSet.m_weightBlends[3] = dgFloat32(u3); + + weightFound = true; + break; + } + } + dgAssert (weightFound); + if (!weightFound) { + dgTrace (("%d %f %f %f\n", i, p.m_x, p.m_y, p.m_z)); + } + + overlapNodes.RemoveAll(); + } +*/ +} diff --git a/thirdparty/src/newton/dgMeshUtil/dgMeshEffect3.cpp b/thirdparty/src/newton/dgMeshUtil/dgMeshEffect3.cpp new file mode 100644 index 000000000..b2031e70b --- /dev/null +++ b/thirdparty/src/newton/dgMeshUtil/dgMeshEffect3.cpp @@ -0,0 +1,1371 @@ +/* Copyright (c) <2003-2019> +* +* 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 "dgPhysicsStdafx.h" +#include "dgBody.h" +#include "dgWorld.h" +#include "dgMeshEffect.h" +#include "dgCollisionConvexHull.h" + + + +// based of the paper Hierarchical Approximate Convex Decomposition by Khaled Mamou +// with his permission to adapt his algorithm to be more efficient. +// also making some addition to his heuristic for better convex clusters selections +// for the details http://kmamou.blogspot.com/ + + +#define DG_CONCAVITY_SCALE dgFloat64 (100.0f) +#define DG_CONCAVITY_PERIMETER_HANDICAP dgFloat64 (0.5f) + +class dgHACDEdge +{ + public: + dgHACDEdge () + :m_mark(0) + ,m_proxyListNode(NULL) + ,m_backFaceHandicap(dgFloat64 (1.0)) + { + } + ~dgHACDEdge () + { + } + + dgInt32 m_mark; + void* m_proxyListNode; + dgFloat64 m_backFaceHandicap; +}; + +class dgHACDClusterFace +{ + public: + dgHACDClusterFace() + :m_edge(NULL) + ,m_area(dgFloat64(0.0f)) + { + } + ~dgHACDClusterFace() + { + } + + dgEdge* m_edge; + dgFloat64 m_area; + dgBigVector m_normal; +}; + + +class dgHACDCluster: public dgList +{ + public: + dgHACDCluster () + :dgList(NULL) + ,m_color(0) + ,m_hierachicalClusterIndex(0) + ,m_area(dgFloat64 (0.0f)) + ,m_concavity(dgFloat64 (0.0f)) + { + } + + bool IsCoplanar(const dgBigPlane& plane, const dgMeshEffect& mesh, dgFloat64 tolerance) const + { + const dgBigVector* const points = (dgBigVector*) mesh.GetVertexPool(); + for (dgListNode* node = GetFirst(); node; node = node->GetNext()) { + const dgHACDClusterFace& info = node->GetInfo(); + dgEdge* ptr = info.m_edge; + do { + const dgBigVector& p = points[ptr->m_incidentVertex]; + dgFloat64 dist = fabs(plane.Evalue(p)); + if (dist > tolerance) { + return false; + } + ptr = ptr->m_next; + } while (ptr != info.m_edge); + } + return true; + } + + + dgInt32 m_color; + dgInt32 m_hierachicalClusterIndex; + dgFloat64 m_area; + dgFloat64 m_concavity; +}; + + +class dgHACDClusterGraph: public dgGraph +{ + public: + class dgHACDConveHull: public dgConvexHull3d + { + class dgConvexHullRayCastData + { + public: + dgFloat64 m_normalProjection; + dgConvexHull3DFace* m_face; + }; + + public: + dgHACDConveHull (const dgHACDConveHull& hull) + :dgConvexHull3d(hull) + ,m_mark(1) + { + } + + dgHACDConveHull (dgMemoryAllocator* const allocator, const dgBigVector* const points, dgInt32 count) + :dgConvexHull3d(allocator, &points[0].m_x, sizeof (dgBigVector),count, dgFloat64 (0.0f)) + ,m_mark(1) + { + } + + + dgFloat64 CalculateTriangleConcavity(const dgBigVector& normal, dgInt32 i0, dgInt32 i1, dgInt32 i2, const dgBigVector* const points) + { + dgUnsigned32 head = 1; + dgUnsigned32 tail = 0; + dgBigVector pool[1<<8][3]; + + pool[0][0] = points[i0]; + pool[0][1] = points[i1]; + pool[0][2] = points[i2]; + + const dgFloat64 rayLength = dgFloat64(4.0f) * GetDiagonal(); + const dgBigVector step(normal.Scale(rayLength)); + + dgFloat64 concavity = dgFloat32(0.0f); + dgFloat64 minArea = dgFloat32(0.125f); + dgFloat64 minArea2 = minArea * minArea * 0.5f; + + dgInt32 maxCount = 4; + dgUnsigned32 mask = (sizeof (pool) / (3 * sizeof (pool[0][0]))) - 1; + + dgConvexHull3DFace* firstGuess = NULL; + while ((tail != head) && (maxCount >= 0)) { + maxCount --; + dgBigVector p0(pool[tail][0]); + dgBigVector p1(pool[tail][1]); + dgBigVector p2(pool[tail][2]); + p0.m_w = dgFloat32 (0.0f); + p1.m_w = dgFloat32 (0.0f); + p2.m_w = dgFloat32 (0.0f); + + tail = (tail + 1) & mask; + + dgBigVector q1((p0 + p1 + p2).Scale(dgFloat64(1.0f / 3.0f))); + dgBigVector q0(q1 + step); + + dgFloat64 param = RayCast(q0, q1, &firstGuess); + if (param > dgFloat64(1.0f)) { + param = dgFloat64(1.0f); + } + dgBigVector dq(step.Scale(dgFloat32(1.0f) - param)); + dgAssert(dq.m_w == dgFloat32(0.0f)); + dgFloat64 lenght2 = sqrt (dq.DotProduct(dq).GetScalar()); + if (lenght2 > concavity) { + concavity = lenght2; + } + + if (((head + 1) & mask) != tail) { + dgBigVector edge10(p1 - p0); + dgBigVector edge20(p2 - p0); + dgBigVector n(edge10.CrossProduct(edge20)); + dgAssert(n.m_w == dgFloat32(0.0f)); + dgFloat64 area2 = n.DotProduct(n).GetScalar(); + if (area2 > minArea2) { + dgBigVector p01((p0 + p1).Scale(dgFloat64(0.5f))); + dgBigVector p12((p1 + p2).Scale(dgFloat64(0.5f))); + dgBigVector p20((p2 + p0).Scale(dgFloat64(0.5f))); + + pool[head][0] = p0; + pool[head][1] = p01; + pool[head][2] = p20; + head = (head + 1) & mask; + + if (((head + 1) & mask) != tail) { + pool[head][0] = p1; + pool[head][1] = p12; + pool[head][2] = p01; + head = (head + 1) & mask; + + if (((head + 1) & mask) != tail) { + pool[head][0] = p2; + pool[head][1] = p20; + pool[head][2] = p12; + head = (head + 1) & mask; + } + } + } + } + } + return concavity; + } + + + + dgFloat64 FaceRayCast (const dgConvexHull3DFace* const face, const dgBigVector& origin, const dgBigVector& dist, dgFloat64& normalProjection) const + { + dgInt32 i0 = face->m_index[0]; + dgInt32 i1 = face->m_index[1]; + dgInt32 i2 = face->m_index[2]; + + const dgBigVector& p0 = m_points[i0]; + dgBigVector normal ((m_points[i1] - p0).CrossProduct(m_points[i2] - p0)); + + //dgFloat64 N = (origin - p0) % normal; + dgAssert(normal.m_w == dgFloat32(0.0f)); + dgFloat64 N = normal.DotProduct(origin - p0).GetScalar(); + dgFloat64 D = normal.DotProduct(dist).GetScalar(); + + if (fabs(D) < dgFloat64 (1.0e-16f)) { // + normalProjection = dgFloat32 (0.0); + if (N > dgFloat64 (0.0f)) { + return dgFloat32 (-1.0e30); + } else { + + return dgFloat32 (1.0e30); + } + } + normalProjection = D; + return - N / D; + } + + dgConvexHull3DFace* ClosestFaceVertexToPoint (const dgBigVector& point) + { + // note, for this function to be effective point should be an already close point to the Hull. + // for example casting the point to the OBB or the AABB of the full is a good first guess. + dgConvexHull3DFace* closestFace = &GetFirst()->GetInfo(); + dgInt8 pool[256 * (sizeof (dgConvexHull3DFace*) + sizeof (dgFloat64))]; + dgUpHeap heap (pool, sizeof (pool)); + + for (dgInt32 i = 0; i < 3; i ++) { + dgBigVector dist (m_points[closestFace->m_index[i]] - point); + dgAssert(dist.m_w == dgFloat32(0.0f)); + heap.Push(closestFace, dist.DotProduct(dist).GetScalar()); + } + + m_mark ++; + dgFloat64 minDist = heap.Value(); + while (heap.GetCount()) { + dgConvexHull3DFace* const face = heap[0]; + if (heap.Value() < minDist) { + minDist = heap.Value(); + closestFace = face; + } + heap.Pop(); + //face->m_mark = m_mark; + face->SetMark(m_mark); + for (dgInt32 i = 0; i < 3; i ++) { + dgConvexHull3DFace* twin = &face->GetTwin(i)->GetInfo(); + if (twin->GetMark() != m_mark) { + dgBigVector dist (m_points[twin->m_index[i]] - point); + // use hysteresis to prevent stops at a local minimal, but at the same time fast descend + dgAssert(dist.m_w == dgFloat32(0.0f)); + dgFloat64 dist2 = dist.DotProduct(dist).GetScalar(); + if (dist2 < (minDist * dgFloat64 (1.001f))) { + heap.Push(twin, dist2); + } + } + } + } + + return closestFace; + } + + + // this version have input sensitive complexity (approximately log2) + // when casting parallel rays and using the last face as initial guess this version has const time complexity + dgFloat64 RayCast (const dgBigVector& localP0, const dgBigVector& localP1, dgConvexHull3DFace** firstFaceGuess) + { + dgConvexHull3DFace* face = &GetFirst()->GetInfo(); + if (firstFaceGuess && *firstFaceGuess) { + face = *firstFaceGuess; + } else { + if (GetCount() > 32) { + dgVector q0 (localP0); + dgVector q1 (localP1); + if (dgRayBoxClip (q0, q1, m_aabbP0, m_aabbP1)) { + face = ClosestFaceVertexToPoint (q0); + } + } + } + + m_mark ++; + face->SetMark (m_mark); + dgInt8 pool[256 * (sizeof (dgConvexHullRayCastData) + sizeof (dgFloat64))]; + dgDownHeap heap (pool, sizeof (pool)); + + dgFloat64 t0 = dgFloat64 (-1.0e20); //for the maximum entering segment parameter; + dgFloat64 t1 = dgFloat64 ( 1.0e20); //for the minimum leaving segment parameter; + dgBigVector dS (localP1 - localP0); // is the segment direction vector; + dgConvexHullRayCastData data; + data.m_face = face; + dgFloat64 t = FaceRayCast (face, localP0, dS, data.m_normalProjection); + if (data.m_normalProjection >= dgFloat32 (0.0f)) { + t = dgFloat64 (-1.0e30); + } + + heap.Push (data, t); + while (heap.GetCount()) { + dgConvexHullRayCastData data1 (heap[0]); + t = heap.Value(); + dgConvexHull3DFace* const face1 = data1.m_face; + dgFloat64 normalDistProjection = data1.m_normalProjection; + heap.Pop(); + bool foundThisBestFace = true; + if (normalDistProjection < dgFloat64 (0.0f)) { + if (t > t0) { + t0 = t; + } + if (t0 > t1) { + return dgFloat64 (1.2f); + } + } else { + foundThisBestFace = false; + } + + for (dgInt32 i = 0; i < 3; i ++) { + dgConvexHull3DFace* const face2 = &face1->GetTwin(i)->GetInfo(); + + if (face2->GetMark() != m_mark) { + face2->SetMark (m_mark); + dgConvexHullRayCastData data2; + data2.m_face = face2; + t = FaceRayCast (face2, localP0, dS, data2.m_normalProjection); + if (data2.m_normalProjection >= dgFloat32 (0.0)) { + t = dgFloat64 (-1.0e30); + } else if (t > t0) { + foundThisBestFace = false; + } else if (fabs (t - t0) < dgFloat64 (1.0e-10f)) { + return dgConvexHull3d::RayCast (localP0, localP1); + } + if ((heap.GetCount() + 2)>= heap.GetMaxCount()) { + // remove t values that are old and way outside interval [0.0, 1.0] + for (dgInt32 j = heap.GetCount() - 1; j >= 0; j--) { + dgFloat64 val = heap.Value(j); + if ((val < dgFloat64 (-100.0f)) || (val > dgFloat64 (100.0f))) { + heap.Remove(j); + } + } + } + heap.Push (data2, t); + } + } + if (foundThisBestFace) { + if ((t0 >= dgFloat64 (0.0f)) && (t0 <= dgFloat64 (1.0f))) { + if (firstFaceGuess) { + *firstFaceGuess = face1; + } + return t0; + } + break; + } + } + return dgFloat64 (1.2f); + } + + + dgInt32 m_mark; + }; + + class dgHACDConvacityLookAheadTree + { + public: + DG_CLASS_ALLOCATOR(allocator) + + dgHACDConvacityLookAheadTree (dgMemoryAllocator* const allocator, dgEdge* const face, dgFloat64 concavity) + :m_concavity(concavity) + ,m_faceList (allocator) + ,m_left (NULL) + ,m_right (NULL) + { + m_faceList.Append(face); + } + + + dgHACDConvacityLookAheadTree (dgMemoryAllocator* const allocator, dgHACDConvacityLookAheadTree* const leftChild, dgHACDConvacityLookAheadTree* const rightChild, dgFloat64 concavity) + :m_concavity(concavity) + ,m_faceList (allocator) + ,m_left (leftChild) + ,m_right (rightChild) + { + dgAssert (leftChild); + dgAssert (rightChild); + + dgFloat64 concavityTest = m_concavity - dgFloat64 (1.0e-5f); + if ((((m_left->m_faceList.GetCount() == 1) || (m_right->m_faceList.GetCount() == 1))) || + ((concavityTest <= m_left->m_concavity) && (concavityTest <= m_right->m_concavity))) { + //The the parent has lower concavity this mean that the two do no add more detail, + //the can be deleted and replaced the parent node + // for example the two children can be two convex strips that are part of a larger convex piece + // but each part has a non zero concavity, while the convex part has a lower concavity + m_faceList.Merge (m_left->m_faceList); + m_faceList.Merge (m_right->m_faceList); + + delete m_left; + delete m_right; + m_left = NULL; + m_right = NULL; + } else { + for (dgList::dgListNode* node = m_left->m_faceList.GetFirst(); node; node = node->GetNext()) { + m_faceList.Append(node->GetInfo()); + } + for (dgList::dgListNode* node = m_right->m_faceList.GetFirst(); node; node = node->GetNext()) { + m_faceList.Append(node->GetInfo()); + } + } + } + + ~dgHACDConvacityLookAheadTree () + { + if (m_left) { + dgAssert (m_right); + delete m_left; + delete m_right; + } + } + + dgInt32 GetNodesCount () const + { + dgInt32 count = 0; + dgInt32 stack = 1; + const dgHACDConvacityLookAheadTree* pool[1024]; + pool[0] = this; + while (stack) { + stack --; + count ++; + const dgHACDConvacityLookAheadTree* const root = pool[stack]; + if (root->m_left) { + dgAssert (root->m_right); + pool[stack] = root->m_left; + stack ++; + dgAssert (stack < sizeof (pool)/sizeof (pool[0])); + pool[stack] = root->m_right; + stack ++; + dgAssert (stack < sizeof (pool)/sizeof (pool[0])); + } + } + return count; + } + + void ReduceByCount (dgInt32 count, dgDownHeap& approximation) + { + if (count < 1) { + count = 1; + } + + approximation.Flush(); + dgHACDConvacityLookAheadTree* tmp = this; + approximation.Push(tmp, m_concavity); + while ((approximation.GetCount() < count) && (approximation.Value() >= dgFloat32 (0.0f))) { + dgHACDConvacityLookAheadTree* worseCluster = approximation[0]; + dgFloat64 concavity = approximation.Value(); + if (!worseCluster->m_left && (concavity >= dgFloat32 (0.0f))) { + dgAssert (!worseCluster->m_right); + approximation.Pop(); + approximation.Push(worseCluster, concavity - dgFloat64 (1.0e10f)); + } else { + dgAssert (worseCluster->m_left); + dgAssert (worseCluster->m_right); + approximation.Pop(); + approximation.Push(worseCluster->m_left, worseCluster->m_left->m_concavity); + approximation.Push(worseCluster->m_right, worseCluster->m_right->m_concavity); + } + } + } + + + void ReduceByConcavity (dgFloat64 concavity, dgDownHeap& approximation) + { + approximation.Flush(); + dgHACDConvacityLookAheadTree* tmp = this; + + approximation.Push(tmp, m_concavity); + while (approximation.Value() > concavity) { + dgHACDConvacityLookAheadTree* worseCluster = approximation[0]; + if (!worseCluster->m_left && approximation.Value() >= dgFloat32 (0.0f)) { + approximation.Pop(); + approximation.Push(worseCluster, dgFloat32 (-1.0f)); + } else { + dgAssert (worseCluster->m_left); + dgAssert (worseCluster->m_right); + approximation.Pop(); + approximation.Push(worseCluster->m_left, worseCluster->m_left->m_concavity); + approximation.Push(worseCluster->m_right, worseCluster->m_right->m_concavity); + } + } + } + + dgFloat64 m_concavity; + dgList m_faceList; + dgHACDConvacityLookAheadTree* m_left; + dgHACDConvacityLookAheadTree* m_right; + }; + + class dgPairProxy + { + public: + dgPairProxy() + :m_nodeA(NULL) + ,m_nodeB(NULL) + ,m_hierachicalClusterIndexA(0) + ,m_hierachicalClusterIndexB(0) + ,m_area(dgFloat64(0.0f)) + { + } + + ~dgPairProxy() + { + } + + dgListNode* m_nodeA; + dgListNode* m_nodeB; + dgInt32 m_hierachicalClusterIndexA; + dgInt32 m_hierachicalClusterIndexB; + dgFloat64 m_area; + dgFloat64 m_distanceConcavity; + }; + + + class dgBackFaceFinder: public dgMeshEffect::dgMeshBVH + { + public: + dgBackFaceFinder(dgMeshEffect* const mesh, dgHACDClusterGraph* const graph) + :dgMeshEffect::dgMeshBVH(mesh) + ,m_clusterA(NULL) + ,m_graph(graph) + { + for (dgListNode* clusterNode = graph->GetFirst(); clusterNode; clusterNode = clusterNode->GetNext()) { + dgHACDCluster& cluster = clusterNode->GetInfo().m_nodeData; + dgHACDClusterFace& face = cluster.GetFirst()->GetInfo(); + dgEdge* const edge = face.m_edge; + AddFaceNode(edge, &cluster); + } + } + + //dgFloat64 RayFaceIntersect (const dgMeshBVHNode* const face, const dgBigVector& p0, const dgBigVector& p1, bool doublesided) const + dgFloat64 RayFaceIntersect (const dgMeshBVHNode* const face, const dgBigVector& p0, const dgBigVector& p1, void* const userData) const + { + dgHACDCluster* const clusterFace = (dgHACDCluster*) face->m_userData; + + dgFloat64 param = dgFloat32 (100.0f); + if (clusterFace->m_color != m_clusterA->m_color) { + param = dgMeshEffect::dgMeshBVH::RayFaceIntersect (face, p1, p0, NULL); + if ((param >= dgFloat32 (0.0f)) && (param <= dgFloat32(1.0f))) { + param = dgFloat32 (1.0f) - param; + } + } + return param; + } + + void CastBackFace (dgListNode* const clusterNodeA, const dgBigVector& p0, const dgBigVector& p1, const dgBigVector& p2, dgFloat32 distanceThreshold) + { + dgAssert(0); + /* + dgBigVector origin ((p0 + p1 + p2).Scale (dgFloat32 (1.0f/3.0f))); + + dgFloat32 rayDistance = distanceThreshold * dgFloat32 (2.0f); + + + m_clusterA = &clusterNodeA->GetInfo().m_nodeData; + dgHACDClusterFace& faceA = m_clusterA->GetFirst()->GetInfo(); + dgBigVector end (origin - faceA.m_normal.Scale (rayDistance)); + + dgFloat64 paramOut; + //dgMeshBVHNode* const node = FaceRayCast (origin, end, paramOut, false); + + dgMeshBVHNode* node; + FaceRayCast (origin, end, paramOut, &node); + + if (node) { + dgHACDCluster* const clusterB = (dgHACDCluster*) node->m_userData; + dgAssert (clusterB->m_color != m_clusterA->m_color); + dgFloat64 distance = rayDistance * paramOut; + + if (distance < distanceThreshold) { + dgHACDClusterFace& faceB = clusterB->GetFirst()->GetInfo(); + dgEdge* const edgeB = faceB.m_edge; + + + bool isAdjacent = false; + dgEdge* ptrA = faceA.m_edge; + do { + dgEdge* ptrB = edgeB; + do { + if (ptrB->m_twin == ptrA) { + ptrA = faceA.m_edge->m_prev; + isAdjacent = true; + break; + } + ptrB = ptrB->m_next; + } while (ptrB != edgeB); + + ptrA = ptrA->m_next; + } while (ptrA != faceA.m_edge); + + if (!isAdjacent) { + isAdjacent = false; + dgHACDClusterGraph::dgListNode* const clusterNodeB = m_graph->GetNodeFromNodeData (clusterB); + for (dgGraphNode::dgListNode* edgeNode = clusterNodeA->GetInfo().GetFirst(); edgeNode; edgeNode = edgeNode->GetNext()) { + if (edgeNode->GetInfo().m_node == clusterNodeB) { + isAdjacent = true; + break; + } + } + + if (!isAdjacent) { + dgGraphNode::dgListNode* const edgeNodeAB = clusterNodeA->GetInfo().AddEdge (clusterNodeB); + dgGraphNode::dgListNode* const edgeNodeBA = clusterNodeB->GetInfo().AddEdge (clusterNodeA); + + dgHACDEdge& edgeAB = edgeNodeAB->GetInfo().m_edgeData; + dgHACDEdge& edgeBA = edgeNodeBA->GetInfo().m_edgeData; + edgeAB.m_backFaceHandicap = DG_CONCAVITY_PERIMETER_HANDICAP; + edgeBA.m_backFaceHandicap = DG_CONCAVITY_PERIMETER_HANDICAP; + } + } + } + } +*/ + } + + + dgHACDCluster* m_clusterA; + dgHACDClusterGraph* m_graph; + }; + + dgHACDClusterGraph(dgMeshEffect& mesh, dgFloat32 backFaceDistanceFactor, dgReportProgress reportProgressCallback, void* const reportProgressUserData) + :dgGraph (mesh.GetAllocator()) + ,m_mark(0) + ,m_faceCount(0) + ,m_vertexMark(0) + ,m_progress(0) + ,m_concavityTreeIndex(0) + ,m_invFaceCount(dgFloat32 (1.0f)) + ,m_diagonal(dgFloat64(1.0f)) + ,m_vertexMarks(NULL) + ,m_vertexPool(NULL) + ,m_proxyList(mesh.GetAllocator()) + ,m_concavityTreeArray(NULL) + ,m_convexProximation(mesh.GetAllocator()) + ,m_priorityHeap (mesh.GetCount() * 2 + 2048, mesh.GetAllocator()) + ,m_reportProgressCallback(reportProgressCallback) + ,m_reportProgressUserData(reportProgressUserData) + { + m_faceCount = mesh.GetTotalFaceCount(); + + dgMemoryAllocator* const allocator = mesh.GetAllocator(); + m_invFaceCount = dgFloat32 (1.0f) / (m_faceCount); + + // init some auxiliary structures + dgInt32 vertexCount = mesh.GetVertexCount(); + m_vertexMarks = (dgInt32*) dgMallocStack(vertexCount * sizeof(dgInt32)); + m_vertexPool = (dgBigVector*) dgMallocStack(vertexCount * sizeof(dgBigVector)); + memset(m_vertexMarks, 0, vertexCount * sizeof(dgInt32)); + + m_concavityTreeIndex = m_faceCount + 1; + m_concavityTreeArray = (dgHACDConvacityLookAheadTree**) dgMallocStack(2 * m_concavityTreeIndex * sizeof(dgHACDConvacityLookAheadTree*)); + memset(m_concavityTreeArray, 0, 2 * m_concavityTreeIndex * sizeof(dgHACDConvacityLookAheadTree*)); + + // scan the mesh and and add a node for each face + dgInt32 color = 1; + dgMeshEffect::Iterator iter(mesh); + + dgInt32 meshMask = mesh.IncLRU(); + const dgBigVector* const points = (dgBigVector*) mesh.GetVertexPool(); + for (iter.Begin(); iter; iter++) { + dgEdge* const edge = &(*iter); + if ((edge->m_mark != meshMask) && (edge->m_incidentFace > 0)) { + + dgListNode* const clusterNode = AddNode (); + dgHACDCluster& cluster = clusterNode->GetInfo().m_nodeData; + cluster.SetAllocator(mesh.GetAllocator()); + + dgFloat64 perimeter = dgFloat64(0.0f); + dgEdge* ptr = edge; + do { + dgBigVector p1p0(points[ptr->m_incidentVertex] - points[ptr->m_prev->m_incidentVertex]); + dgAssert(p1p0.m_w == dgFloat32(0.0f)); + perimeter += sqrt(p1p0.DotProduct(p1p0).GetScalar()); + ptr->m_incidentFace = color; + ptr->m_userData = dgUnsigned64 (clusterNode); + ptr->m_mark = meshMask; + ptr = ptr->m_next; + } while (ptr != edge); + + dgBigVector normal (mesh.FaceNormal(edge, &points[0][0], sizeof(dgBigVector))); + dgFloat64 mag = sqrt(normal.DotProduct(normal).GetScalar()); + + cluster.m_color = color; + cluster.m_hierachicalClusterIndex = color; + cluster.m_area = dgFloat64(0.5f) * mag; + cluster.m_concavity = CalculateConcavityMetric (dgFloat64 (0.0f), cluster.m_area, perimeter, 1, 0); + + dgHACDClusterFace& face = cluster.Append()->GetInfo(); + face.m_edge = edge; + face.m_area = dgFloat64(0.5f) * mag; + face.m_normal = normal.Scale(dgFloat64(1.0f) / mag); + + m_concavityTreeArray[color] = new (allocator) dgHACDConvacityLookAheadTree (allocator, edge, dgFloat64 (0.0f)); + + color ++; + } + } + + // add all link adjacent faces links + for (dgListNode* clusterNode = GetFirst(); clusterNode; clusterNode = clusterNode->GetNext()) { + + dgHACDCluster& cluster = clusterNode->GetInfo().m_nodeData; + dgHACDClusterFace& face = cluster.GetFirst()->GetInfo(); + dgEdge* const edge = face.m_edge; + dgEdge* ptr = edge; + do { + if (ptr->m_twin->m_incidentFace > 0) { + dgAssert (ptr->m_twin->m_userData); + dgListNode* const twinClusterNode = (dgListNode*) ptr->m_twin->m_userData; + dgAssert (twinClusterNode); + + bool doubleEdge = false; + for (dgGraphNode::dgListNode* edgeNode = clusterNode->GetInfo().GetFirst(); edgeNode; edgeNode = edgeNode->GetNext()) { + if (edgeNode->GetInfo().m_node == twinClusterNode) { + doubleEdge = true; + break; + } + } + if (!doubleEdge) { + clusterNode->GetInfo().AddEdge (twinClusterNode); + } + } + ptr = ptr->m_next; + } while (ptr != edge); + } + + Trace(); + dgAssert (0); +/* + // add links to back faces + dgBigVector minAABB; + dgBigVector maxAABB; + mesh.CalculateAABB (minAABB, maxAABB); + maxAABB -= minAABB; + dgAssert(maxAABB.m_w == dgFloat32(0.0f)); + dgFloat32 rayDiagonalLength = dgFloat32 (sqrt (maxAABB.DotProduct(maxAABB).GetScalar())); + m_diagonal = rayDiagonalLength; + + dgBackFaceFinder backFaces(&mesh, this); + dgFloat32 distanceThreshold = rayDiagonalLength * backFaceDistanceFactor; + dgAssert (distanceThreshold >= dgFloat32 (0.0f)); + for (dgListNode* clusterNodeA = GetFirst(); clusterNodeA; clusterNodeA = clusterNodeA->GetNext()) { + + dgHACDCluster& clusterA = clusterNodeA->GetInfo().m_nodeData; + dgHACDClusterFace& faceA = clusterA.GetFirst()->GetInfo(); + dgEdge* const edgeA = faceA.m_edge; + dgEdge* ptr = edgeA; + + dgBigVector p0 (points[ptr->m_incidentVertex]); + dgBigVector p1 (points[ptr->m_next->m_incidentVertex]); + ptr = ptr->m_next->m_next; + do { + dgBigVector p2 (points[ptr->m_incidentVertex]); + dgBigVector p01 ((p0 + p1).Scale (dgFloat32 (0.5f))); + dgBigVector p12 ((p1 + p2).Scale (dgFloat32 (0.5f))); + dgBigVector p20 ((p2 + p0).Scale (dgFloat32 (0.5f))); + + backFaces.CastBackFace (clusterNodeA, p0, p01, p20, distanceThreshold); + backFaces.CastBackFace (clusterNodeA, p1, p12, p01, distanceThreshold); + backFaces.CastBackFace (clusterNodeA, p2, p20, p12, distanceThreshold); + backFaces.CastBackFace (clusterNodeA, p01, p12, p20, distanceThreshold); + + p1 = p2; + ptr = ptr->m_next; + } while (ptr != edgeA); + } +*/ + Trace(); + } + + ~dgHACDClusterGraph () + { + for (dgInt32 i = 0; i < m_faceCount * 2; i ++) { + if (m_concavityTreeArray[i]) { + delete m_concavityTreeArray[i]; + } + } + + dgFreeStack(m_concavityTreeArray); + dgFreeStack(m_vertexPool); + dgFreeStack(m_vertexMarks); + } + + + + + void Trace() const + { +#if 0 + for (dgListNode* clusterNodeA = GetFirst(); clusterNodeA; clusterNodeA = clusterNodeA->GetNext()) { + dgHACDCluster& clusterA = clusterNodeA->GetInfo().m_nodeData; + //dgHACDClusterFace& faceA = clusterA.GetFirst()->GetInfo(); + //dgEdge* const edgeA = faceA.m_edge; + + dgTrace (("cluster node: %d\n", clusterA.m_color)); + dgTrace ((" links: ")); + for (dgGraphNode::dgListNode* edgeNodeA = clusterNodeA->GetInfo().GetFirst(); edgeNodeA; edgeNodeA = edgeNodeA->GetNext()) { + dgListNode* const clusterNodeB = edgeNodeA->GetInfo().m_node; + dgHACDCluster& clusterB = clusterNodeB->GetInfo().m_nodeData; + dgTrace (("%d ", clusterB.m_color)); + } + dgTrace (("\n")); + } + dgTrace (("\n")); +#endif + } + + + // you can insert callback here to print the progress as it collapse clusters + bool ReportProgress () + { + bool state = true; + m_progress ++; + if (m_reportProgressCallback) { + dgFloat32 progress = dgFloat32(m_progress) * m_invFaceCount; + state = m_reportProgressCallback (progress * dgFloat32 (0.5f) + 0.5f, m_reportProgressUserData); + } + return state; + } + + dgMeshEffect* CreatePartitionMesh (dgMeshEffect& mesh, dgInt32 maxVertexPerHull) + { + dgMemoryAllocator* const allocator = mesh.GetAllocator(); + dgMeshEffect* const convexPartionMesh = new (allocator) dgMeshEffect(allocator); + + dgArray convexVertexBuffer(mesh.m_points.m_vertex, mesh.m_points.m_vertex.m_count); + const dgBigVector* const points = (dgBigVector*) mesh.GetVertexPool(); + + dgInt32 layer = 0; + convexPartionMesh->BeginBuild(); + for (dgList::dgListNode* clusterNode = m_convexProximation.GetFirst(); clusterNode; clusterNode = clusterNode->GetNext()) { + dgHACDConvacityLookAheadTree* const cluster = clusterNode->GetInfo(); + + dgInt32 vertexCount = 0; + for (dgList::dgListNode* faceNode = cluster->m_faceList.GetFirst(); faceNode; faceNode = faceNode->GetNext()) { + dgEdge* const edge = faceNode->GetInfo(); + dgEdge* ptr = edge; + do { + dgInt32 index = ptr->m_incidentVertex; + convexVertexBuffer[vertexCount] = points[index]; + vertexCount++; + ptr = ptr->m_next; + } while (ptr != edge); + } + + //dgConvexHull3d convexHull(allocator, &convexVertexBuffer[0].m_x, sizeof(dgBigVector), vertexCount, 0.0, maxVertexPerHull); + dgMeshEffect convexMesh(allocator, &convexVertexBuffer[0].m_x, vertexCount, sizeof(dgBigVector), dgFloat64(0.0f)); + if (convexMesh.GetCount()) { + for (dgInt32 i = 0; i < convexMesh.m_points.m_vertex.m_count; i++) { + convexMesh.m_points.m_layers[i] = layer; + } + convexPartionMesh->MergeFaces(&convexMesh); + layer++; + } + } + convexPartionMesh->EndBuild(1.0e-5f); + + m_progress = m_faceCount - 1; + ReportProgress(); + return convexPartionMesh; + } + + dgFloat64 ConcavityByFaceMedian (dgInt32 faceCountA, dgInt32 faceCountB) const + { + dgFloat64 faceCountCost = DG_CONCAVITY_SCALE * dgFloat64 (0.1f) * (faceCountA + faceCountB) * m_invFaceCount; + return faceCountCost; + } + + dgFloat64 CalculateConcavityMetric (dgFloat64 convexConcavity, dgFloat64 area, dgFloat64 perimeter, dgInt32 faceCountA, dgInt32 faceCountB) const + { + dgFloat64 edgeCost = perimeter * perimeter / (dgFloat64(4.0f * dgPi) * area); + return convexConcavity * DG_CONCAVITY_SCALE + edgeCost + ConcavityByFaceMedian (faceCountA, faceCountB); + } + + void SubmitInitialEdgeCosts (dgMeshEffect& mesh) + { + m_mark ++; + for (dgListNode* clusterNodeA = GetFirst(); clusterNodeA; clusterNodeA = clusterNodeA->GetNext()) { + // call the progress callback + for (dgGraphNode::dgListNode* edgeNodeAB = clusterNodeA->GetInfo().GetFirst(); edgeNodeAB; edgeNodeAB = edgeNodeAB->GetNext()) { + dgHACDEdge& edgeAB = edgeNodeAB->GetInfo().m_edgeData; + dgFloat64 weight = edgeAB.m_backFaceHandicap; + if (edgeAB.m_mark != m_mark) { + edgeAB.m_mark = m_mark; + dgListNode* const clusterNodeB = edgeNodeAB->GetInfo().m_node; + for (dgGraphNode::dgListNode* edgeNodeBA = clusterNodeB->GetInfo().GetFirst(); edgeNodeBA; edgeNodeBA = edgeNodeBA->GetNext()) { + dgListNode* const clusterNode = edgeNodeBA->GetInfo().m_node; + if (clusterNode == clusterNodeA) { + dgHACDEdge& edgeBA = edgeNodeBA->GetInfo().m_edgeData; + edgeBA.m_mark = m_mark; + dgAssert (!edgeAB.m_proxyListNode); + dgAssert (!edgeBA.m_proxyListNode); + + dgAssert (edgeBA.m_backFaceHandicap == weight); + dgList::dgListNode* const proxyNode = SubmitEdgeCost (mesh, clusterNodeA, clusterNodeB, weight * edgeBA.m_backFaceHandicap); + edgeAB.m_proxyListNode = proxyNode; + edgeBA.m_proxyListNode = proxyNode; + break; + } + } + } + } + } + } + + dgInt32 CopyVertexToPool(const dgMeshEffect& mesh, const dgHACDCluster& cluster, dgInt32 start) + { + dgInt32 count = start; + + const dgBigVector* const points = (dgBigVector*) mesh.GetVertexPool(); + for (dgList::dgListNode* node = cluster.GetFirst(); node; node = node->GetNext()) { + const dgHACDClusterFace& clusterFace = node->GetInfo(); + dgEdge* edge = clusterFace.m_edge; + do { + dgInt32 index = edge->m_incidentVertex; + if (m_vertexMarks[index] != m_vertexMark) { + m_vertexMarks[index] = m_vertexMark; + m_vertexPool[count] = points[index]; + count++; + } + edge = edge->m_next; + } while (edge != clusterFace.m_edge); + } + return count; + } + + + void MarkInteriorClusterEdges (dgMeshEffect& mesh, dgInt32 mark, const dgHACDCluster& cluster, dgInt32 colorA, dgInt32 colorB) const + { + dgAssert (colorA != colorB); + for (dgList::dgListNode* node = cluster.GetFirst(); node; node = node->GetNext()) { + dgHACDClusterFace& clusterFace = node->GetInfo(); + dgEdge* edge = clusterFace.m_edge; + do { + if ((edge->m_twin->m_incidentFace == colorA) || (edge->m_twin->m_incidentFace == colorB)) { + edge->m_mark = mark; + edge->m_twin->m_mark = mark; + } + edge = edge->m_next; + } while (edge != clusterFace.m_edge); + } + } + + dgFloat64 CalculateClusterPerimeter (dgMeshEffect& mesh, dgInt32 mark, const dgHACDCluster& cluster, dgInt32 colorA, dgInt32 colorB) const + { + dgAssert (colorA != colorB); + dgFloat64 perimeter = dgFloat64 (0.0f); + const dgBigVector* const points = (dgBigVector*) mesh.GetVertexPool(); + for (dgList::dgListNode* node = cluster.GetFirst(); node; node = node->GetNext()) { + dgHACDClusterFace& clusterFace = node->GetInfo(); + dgEdge* edge = clusterFace.m_edge; + do { + if (!((edge->m_twin->m_incidentFace == colorA) || (edge->m_twin->m_incidentFace == colorB))) { + dgBigVector p1p0(points[edge->m_twin->m_incidentVertex] - points[edge->m_incidentVertex]); + dgAssert(p1p0.m_w == dgFloat32(0.0f)); + perimeter += sqrt(p1p0.DotProduct(p1p0).GetScalar()); + } + edge = edge->m_next; + } while (edge != clusterFace.m_edge); + } + + return perimeter; + } + + void HeapCollectGarbage () + { + if ((m_priorityHeap.GetCount() + 20) > m_priorityHeap.GetMaxCount()) { + for (dgInt32 i = m_priorityHeap.GetCount() - 1; i >= 0; i--) { + dgList::dgListNode* const emptyNode = m_priorityHeap[i]; + dgPairProxy& emptyPair = emptyNode->GetInfo(); + if ((emptyPair.m_nodeA == NULL) && (emptyPair.m_nodeB == NULL)) { + m_priorityHeap.Remove(i); + } + } + } + } + + + dgFloat64 CalculateConcavity(dgHACDConveHull& hull, const dgMeshEffect& mesh, const dgHACDCluster& cluster) + { + dgFloat64 concavity = dgFloat32(0.0f); + + const dgBigVector* const points = (dgBigVector*) mesh.GetVertexPool(); + for (dgList::dgListNode* node = cluster.GetFirst(); node; node = node->GetNext()) { + dgHACDClusterFace& clusterFace = node->GetInfo(); + dgEdge* edge = clusterFace.m_edge; + dgInt32 i0 = edge->m_incidentVertex; + dgInt32 i1 = edge->m_next->m_incidentVertex; + for (dgEdge* ptr = edge->m_next->m_next; ptr != edge; ptr = ptr->m_next) { + dgInt32 i2 = ptr->m_incidentVertex; + dgFloat64 val = hull.CalculateTriangleConcavity(clusterFace.m_normal, i0, i1, i2, points); + if (val > concavity) { + concavity = val; + } + i1 = i2; + } + } + + return concavity; + } + + dgFloat64 CalculateConcavity (dgHACDConveHull& hull, dgMeshEffect& mesh, dgHACDCluster& clusterA, dgHACDCluster& clusterB) + { + return dgMax(CalculateConcavity(hull, mesh, clusterA), CalculateConcavity(hull, mesh, clusterB)); + } + + + dgList::dgListNode* SubmitEdgeCost (dgMeshEffect& mesh, dgListNode* const clusterNodeA, dgListNode* const clusterNodeB, dgFloat64 perimeterHandicap) + { + dgHACDCluster& clusterA = clusterNodeA->GetInfo().m_nodeData; + dgHACDCluster& clusterB = clusterNodeB->GetInfo().m_nodeData; + const dgBigVector* const points = (dgBigVector*) mesh.GetVertexPool(); + + bool flatStrip = true; + dgFloat64 tol = dgFloat64 (1.0e-5f) * m_diagonal; + dgHACDClusterFace& clusterFaceA = clusterA.GetFirst()->GetInfo(); + dgAssert(clusterFaceA.m_normal.m_w == dgFloat32(0.0f)); + dgBigPlane plane(clusterFaceA.m_normal, - points[clusterFaceA.m_edge->m_incidentVertex].DotProduct(clusterFaceA.m_normal).GetScalar()); + + if (clusterA.GetCount() > 1) { + flatStrip = clusterA.IsCoplanar(plane, mesh, tol); + } + + if (flatStrip) { + flatStrip = clusterB.IsCoplanar(plane, mesh, tol); + } + + dgList::dgListNode* pairNode = NULL; + if (!flatStrip) { + m_vertexMark ++; + dgInt32 vertexCount = CopyVertexToPool(mesh, clusterA, 0); + vertexCount = CopyVertexToPool(mesh, clusterB, vertexCount); + + dgHACDConveHull convexHull(mesh.GetAllocator(), m_vertexPool, vertexCount); + + if (convexHull.GetVertexCount()) { + dgInt32 mark = mesh.IncLRU(); + MarkInteriorClusterEdges (mesh, mark, clusterA, clusterA.m_color, clusterB.m_color); + MarkInteriorClusterEdges (mesh, mark, clusterB, clusterA.m_color, clusterB.m_color); + + dgFloat64 area = clusterA.m_area + clusterB.m_area; + dgFloat64 perimeter = CalculateClusterPerimeter (mesh, mark, clusterA, clusterA.m_color, clusterB.m_color) + + CalculateClusterPerimeter (mesh, mark, clusterB, clusterA.m_color, clusterB.m_color); + dgFloat64 concavity = CalculateConcavity (convexHull, mesh, clusterA, clusterB); + + if (concavity < dgFloat64(1.0e-3f)) { + concavity = dgFloat64(0.0f); + } + + // see if the heap will overflow + HeapCollectGarbage (); + + // add a new pair to the heap + dgList::dgListNode* pairNode1 = m_proxyList.Append(); + dgPairProxy& pair = pairNode1->GetInfo(); + pair.m_nodeA = clusterNodeA; + pair.m_nodeB = clusterNodeB; + pair.m_distanceConcavity = concavity; + pair.m_hierachicalClusterIndexA = clusterA.m_hierachicalClusterIndex; + pair.m_hierachicalClusterIndexB = clusterB.m_hierachicalClusterIndex; + + pair.m_area = area; + dgFloat64 cost = CalculateConcavityMetric (concavity, area * perimeterHandicap, perimeter * perimeterHandicap, clusterA.GetCount(), clusterB.GetCount()); + m_priorityHeap.Push(pairNode1, cost); + + return pairNode1; + } + } + return pairNode; + } + + + bool CollapseEdge (dgList::dgListNode* const pairNode, dgMeshEffect& mesh, dgFloat64 concavity) + { + dgListNode* adjacentNodes[1024]; + dgPairProxy& pair = pairNode->GetInfo(); + + dgMemoryAllocator* const allocator = mesh.GetAllocator(); + + bool continueColapsing = true; + dgAssert((pair.m_nodeA && pair.m_nodeB) || (!pair.m_nodeA && !pair.m_nodeB)); + if (pair.m_nodeA && pair.m_nodeB && continueColapsing) { + // call the progress callback + continueColapsing = ReportProgress(); + + dgListNode* const clusterNodeA = pair.m_nodeA; + dgListNode* const clusterNodeB = pair.m_nodeB; + dgAssert (clusterNodeA != clusterNodeB); + + dgHACDCluster& clusterA = clusterNodeA->GetInfo().m_nodeData; + dgHACDCluster& clusterB = clusterNodeB->GetInfo().m_nodeData; + + dgAssert (&clusterA != &clusterB); + dgAssert(clusterA.m_color != clusterB.m_color); + + dgHACDConvacityLookAheadTree* const leftTree = m_concavityTreeArray[pair.m_hierachicalClusterIndexA]; + dgHACDConvacityLookAheadTree* const rightTree = m_concavityTreeArray[pair.m_hierachicalClusterIndexB]; + dgAssert (leftTree); + dgAssert (rightTree); + m_concavityTreeArray[pair.m_hierachicalClusterIndexA] = NULL; + m_concavityTreeArray[pair.m_hierachicalClusterIndexB] = NULL; + dgAssert (m_concavityTreeIndex < (2 * (m_faceCount + 1))); + + dgFloat64 treeConcavity = pair.m_distanceConcavity; +// dgAssert (treeConcavity < 0.1); + m_concavityTreeArray[m_concavityTreeIndex] = new (allocator) dgHACDConvacityLookAheadTree (allocator, leftTree, rightTree, treeConcavity); + clusterA.m_hierachicalClusterIndex = m_concavityTreeIndex; + clusterB.m_hierachicalClusterIndex = m_concavityTreeIndex; + m_concavityTreeIndex ++; + + // merge two clusters + while (clusterB.GetCount()) { + + dgHACDCluster::dgListNode* const nodeB = clusterB.GetFirst(); + clusterB.Unlink(nodeB); + + // now color code all faces of the merged cluster + dgHACDClusterFace& faceB = nodeB->GetInfo(); + dgEdge* ptr = faceB.m_edge; + do { + ptr->m_incidentFace = clusterA.m_color; + ptr = ptr->m_next; + } while (ptr != faceB.m_edge); + clusterA.Append(nodeB); + } + clusterA.m_area = pair.m_area; + clusterA.m_concavity = concavity; + + // invalidate all proxies that are still in the heap + dgInt32 adjacentCount = 1; + adjacentNodes[0] = clusterNodeA; + for (dgGraphNode::dgListNode* edgeNodeAB = clusterNodeA->GetInfo().GetFirst(); edgeNodeAB; edgeNodeAB = edgeNodeAB->GetNext()) { + dgHACDEdge& edgeAB = edgeNodeAB->GetInfo().m_edgeData; + dgList::dgListNode* const proxyNode = (dgList::dgListNode*) edgeAB.m_proxyListNode; + if (proxyNode) { + dgPairProxy& pairProxy = proxyNode->GetInfo(); + dgAssert ((edgeNodeAB->GetInfo().m_node == pairProxy.m_nodeA) || (edgeNodeAB->GetInfo().m_node == pairProxy.m_nodeB)); + pairProxy.m_nodeA = NULL; + pairProxy.m_nodeB = NULL; + edgeAB.m_proxyListNode = NULL; + } + + adjacentNodes[adjacentCount] = edgeNodeAB->GetInfo().m_node; + adjacentCount ++; + dgAssert (adjacentCount < sizeof (adjacentNodes)/ sizeof (adjacentNodes[0])); + } + + for (dgGraphNode::dgListNode* edgeNodeBA = clusterNodeB->GetInfo().GetFirst(); edgeNodeBA; edgeNodeBA = edgeNodeBA->GetNext()) { + dgHACDEdge& edgeBA = edgeNodeBA->GetInfo().m_edgeData; + dgList::dgListNode* const proxyNode = (dgList::dgListNode*) edgeBA.m_proxyListNode; + if (proxyNode) { + dgPairProxy& pairProxy = proxyNode->GetInfo(); + pairProxy.m_nodeA = NULL; + pairProxy.m_nodeB = NULL; + edgeBA.m_proxyListNode = NULL; + } + + bool alreadyLinked = false; + dgListNode* const node = edgeNodeBA->GetInfo().m_node; + for (dgInt32 i = 0; i < adjacentCount; i ++) { + if (node == adjacentNodes[i]) { + alreadyLinked = true; + break; + } + } + if (!alreadyLinked) { + clusterNodeA->GetInfo().AddEdge (node); + node->GetInfo().AddEdge (clusterNodeA); + } + } + DeleteNode (clusterNodeB); + + // submit all new costs for each edge connecting this new node to any other node + for (dgGraphNode::dgListNode* edgeNodeAB = clusterNodeA->GetInfo().GetFirst(); edgeNodeAB; edgeNodeAB = edgeNodeAB->GetNext()) { + dgHACDEdge& edgeAB = edgeNodeAB->GetInfo().m_edgeData; + dgFloat64 weigh = edgeAB.m_backFaceHandicap; + dgListNode* const clusterNodeB1 = edgeNodeAB->GetInfo().m_node; + for (dgGraphNode::dgListNode* edgeNodeBA = clusterNodeB1->GetInfo().GetFirst(); edgeNodeBA; edgeNodeBA = edgeNodeBA->GetNext()) { + dgListNode* const clusterNode = edgeNodeBA->GetInfo().m_node; + if (clusterNode == clusterNodeA) { + dgHACDEdge& edgeBA = edgeNodeBA->GetInfo().m_edgeData; + dgList::dgListNode* const proxyNode = SubmitEdgeCost (mesh, clusterNodeA, clusterNodeB1, weigh * edgeBA.m_backFaceHandicap); + if (proxyNode) { + edgeBA.m_proxyListNode = proxyNode; + edgeAB.m_proxyListNode = proxyNode; + } + break; + } + } + } + } + m_proxyList.Remove(pairNode); + + return continueColapsing; + } + + bool CollapseClusters (dgMeshEffect& mesh, dgFloat64 maxConcavity___, dgInt32 maxClustesCount) + { + bool collapseEdgeState = true; + while (m_priorityHeap.GetCount() && collapseEdgeState) { + dgFloat64 concavity = m_priorityHeap.Value(); + dgList::dgListNode* const pairNode = m_priorityHeap[0]; + m_priorityHeap.Pop(); + collapseEdgeState = CollapseEdge (pairNode, mesh, concavity); + } + + if (collapseEdgeState) { + dgInt32 treeCounts = 0; + for (dgInt32 i = 0; i < m_concavityTreeIndex; i ++) { + if (m_concavityTreeArray[i]) { + m_concavityTreeArray[treeCounts] = m_concavityTreeArray[i]; + m_concavityTreeArray[i] = NULL; + treeCounts ++; + } + } + + if (treeCounts > 1) { + for (dgInt32 i = 0; i < treeCounts; i ++) { + dgAssert (m_concavityTreeArray[i]); + if (m_concavityTreeArray[i]->m_faceList.GetCount()==1) { + delete m_concavityTreeArray[i]; + m_concavityTreeArray[i] = m_concavityTreeArray[treeCounts-1]; + m_concavityTreeArray[treeCounts-1]= NULL; + treeCounts --; + i--; + } + } + + + dgFloat32 largeConcacvity = 10000; + while (treeCounts > 1) { + dgHACDConvacityLookAheadTree* const leftTree = m_concavityTreeArray[treeCounts-1]; + dgHACDConvacityLookAheadTree* const rightTree = m_concavityTreeArray[treeCounts-2]; + m_concavityTreeArray[treeCounts-1] = NULL; + m_concavityTreeArray[treeCounts-2] = new (mesh.GetAllocator()) dgHACDConvacityLookAheadTree (mesh.GetAllocator(), leftTree, rightTree, largeConcacvity); + largeConcacvity *= 2; + treeCounts --; + } + + } + + dgHACDConvacityLookAheadTree* const tree = m_concavityTreeArray[0]; + if (tree) { + dgDownHeap approximation(maxClustesCount * 2, mesh.GetAllocator()); + + tree->ReduceByCount (maxClustesCount, approximation); + //tree->ReduceByConcavity (maxConcavity, approximation); + +//while ((approximation.Value() + dgFloat64 (1.0e10f)) > 1.0e-5) { +//approximation.Pop(); +//} + + while (approximation.GetCount()) { + m_convexProximation.Append(approximation[0]); + approximation.Pop(); + } + } + } + return collapseEdgeState; + } + + + dgInt32 m_mark; + dgInt32 m_faceCount; + dgInt32 m_vertexMark; + dgInt32 m_progress; + dgInt32 m_concavityTreeIndex; + dgFloat32 m_invFaceCount; + dgFloat64 m_diagonal; + dgInt32* m_vertexMarks; + dgBigVector* m_vertexPool; + dgList m_proxyList; + dgHACDConvacityLookAheadTree** m_concavityTreeArray; + dgList m_convexProximation; + dgUpHeap::dgListNode*, dgFloat64> m_priorityHeap; + dgReportProgress m_reportProgressCallback; + void* m_reportProgressUserData; +}; + +dgMeshEffect* dgMeshEffect::CreateConvexApproximation(dgFloat32 maxConcavity, dgFloat32 backFaceDistanceFactor, dgInt32 maxHullsCount, dgInt32 maxVertexPerHull, dgReportProgress reportProgressCallback, void* const progressReportUserData) const +{ + // dgMeshEffect triangleMesh(*this); + if (maxHullsCount <= 1) { + maxHullsCount = 1; + } + if (maxConcavity <= dgFloat32 (1.0e-5f)) { + maxConcavity = dgFloat32 (1.0e-5f); + } + + if (maxVertexPerHull < 4) { + maxVertexPerHull = 4; + } + backFaceDistanceFactor = dgClamp(backFaceDistanceFactor, dgFloat32 (1.0e-6f), dgFloat32 (1.0f)); + + dgMeshEffect* partition = NULL; + + // make a copy of the mesh + dgMeshEffect mesh(*this); + mesh.m_attrib.m_materialChannel.Clear(); + mesh.m_attrib.m_normalChannel.Clear(); + mesh.m_attrib.m_binormalChannel.Clear(); + mesh.m_attrib.m_colorChannel.Clear(); + mesh.m_attrib.m_uv0Channel.Clear(); + mesh.m_attrib.m_uv1Channel.Clear(); + mesh.Triangulate (); + + mesh.UnpackAttibuteData(); + mesh.PackAttibuteData(); + mesh.UnpackPoints(); + bool state = mesh.Optimize (&mesh.m_points.m_vertex[0].m_x, sizeof (dgBigVector), reportProgressCallback, progressReportUserData, dgFloat32 (1.0e-3f), 1500); + // optimized override userdata + dgPolyhedra::Iterator iter(mesh); + for (iter.Begin(); iter; iter++) { + dgEdge* const edge = &iter.GetNode()->GetInfo(); + if (edge->m_incidentFace > 0) { + edge->m_userData = edge->m_incidentVertex; + } + } + mesh.PackPoints(dgFloat32 (1.0e-24f)); + if (state) { + mesh.DeleteDegenerateFaces (&mesh.m_points.m_vertex[0].m_x, sizeof (dgBigVector), dgFloat32 (1.0e-12f)); + mesh.RepairTJoints(); + mesh.ConvertToPolygons(); + //mesh.SaveOFF ("xxxxxx.off"); + + // create a general connectivity graph + dgHACDClusterGraph graph (mesh, backFaceDistanceFactor, reportProgressCallback, progressReportUserData); + + // calculate initial edge costs + graph.SubmitInitialEdgeCosts (mesh); + + // collapse the graph + if (graph.CollapseClusters (mesh, maxConcavity, maxHullsCount)) { + // Create Partition Mesh + partition = graph.CreatePartitionMesh (mesh, maxVertexPerHull); + } + } + + return partition; +} diff --git a/thirdparty/src/newton/dgMeshUtil/dgMeshEffect4.cpp b/thirdparty/src/newton/dgMeshUtil/dgMeshEffect4.cpp new file mode 100644 index 000000000..38435cdf9 --- /dev/null +++ b/thirdparty/src/newton/dgMeshUtil/dgMeshEffect4.cpp @@ -0,0 +1,1625 @@ +/* Copyright (c) <2003-2019> +* +* 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 "dgPhysicsStdafx.h" +#include "dgBody.h" +#include "dgWorld.h" +#include "dgMeshEffect.h" +#include "dgCollisionConvexHull.h" + + + +#if 0 +#define DG_BUILD_HIERACHICAL_HACD + +#define DG_CONCAVITY_MAX_THREADS 8 +#define DG_CONCAVITY_SCALE dgFloat64 (100.0f) + + + + +class dgHACDEdge +{ + public: + dgHACDEdge () + :m_mark(0) + ,m_proxyListNode(NULL) + ,m_backFaceHandicap(dgFloat64 (1.0)) + { + } + ~dgHACDEdge () + { + } + + dgInt32 m_mark; + void* m_proxyListNode; + dgFloat64 m_backFaceHandicap; +}; + +class dgHACDClusterFace +{ + public: + dgHACDClusterFace() + :m_edge(NULL) + ,m_area(dgFloat64(0.0f)) + { + } + ~dgHACDClusterFace() + { + } + + dgEdge* m_edge; + dgFloat64 m_area; + dgBigVector m_normal; +}; + + +class dgHACDCluster: public dgList +{ + public: + dgHACDCluster () + :dgList(NULL) + ,m_color(0) + ,m_hierachicalClusterIndex(0) + ,m_area(dgFloat64 (0.0f)) + ,m_concavity(dgFloat64 (0.0f)) + { + } + + bool IsCoplanar(const dgBigPlane& plane, const dgMeshEffect& mesh, dgFloat64 tolerance) const + { + const dgBigVector* const points = (dgBigVector*) mesh.GetVertexPool(); + for (dgListNode* node = GetFirst(); node; node = node->GetNext()) { + const dgHACDClusterFace& info = node->GetInfo(); + dgEdge* ptr = info.m_edge; + do { + const dgBigVector& p = points[ptr->m_incidentVertex]; + dgFloat64 dist = fabs(plane.Evalue(p)); + if (dist > tolerance) { + return false; + } + ptr = ptr->m_next; + } while (ptr != info.m_edge); + } + return true; + } + + + dgInt32 m_color; + dgInt32 m_hierachicalClusterIndex; + dgFloat64 m_area; + dgFloat64 m_concavity; +}; + + +class dgHACDClusterGraph + :public dgGraph + ,public dgAABBPolygonSoup +{ + public: + + class dgHACDConveHull: public dgConvexHull3d + { + class dgConvexHullRayCastData + { + public: + dgFloat64 m_normalProjection; + dgConvexHull3DFace* m_face; + }; + + public: + dgHACDConveHull (const dgHACDConveHull& hull) + :dgConvexHull3d(hull) + ,m_mark(1) + { + } + + dgHACDConveHull (dgMemoryAllocator* const allocator, const dgBigVector* const points, dgInt32 count) + :dgConvexHull3d(allocator, &points[0].m_x, sizeof (dgBigVector),count, dgFloat64 (0.0f)) + ,m_mark(1) + { + + } + + dgFloat64 CalculateTriangleConcavity(const dgBigVector& normal, dgInt32 i0, dgInt32 i1, dgInt32 i2, const dgBigVector* const points) + { + dgUnsigned32 head = 1; + dgUnsigned32 tail = 0; + dgBigVector pool[1<<8][3]; + + pool[0][0] = points[i0]; + pool[0][1] = points[i1]; + pool[0][2] = points[i2]; + + const dgBigVector step(normal.Scale(dgFloat64(4.0f) * GetDiagonal())); + + dgFloat64 concavity = dgFloat32(0.0f); + dgFloat64 minArea = dgFloat32(0.125f); + dgFloat64 minArea2 = minArea * minArea * 0.5f; + + dgInt32 maxCount = 4; + dgUnsigned32 mask = (sizeof (pool) / (3 * sizeof (pool[0][0]))) - 1; + + dgConvexHull3DFace* firstGuess = NULL; + while ((tail != head) && (maxCount >= 0)) { + maxCount --; + dgBigVector p0(pool[tail][0]); + dgBigVector p1(pool[tail][1]); + dgBigVector p2(pool[tail][2]); + tail = (tail + 1) & mask; + + dgBigVector q1((p0 + p1 + p2).Scale(dgFloat64(1.0f / 3.0f))); + dgBigVector q0(q1 + step); + + //dgFloat64 param = convexHull.RayCast(q0, q1, &firstGuess); + dgFloat64 param = FastRayCast(q0, q1, &firstGuess); + if (param > dgFloat64(1.0f)) { + param = dgFloat64(1.0f); + } + dgBigVector dq(step.Scale(dgFloat32(1.0f) - param)); + dgFloat64 lenght2 = sqrt (dq % dq); + //dgAssert (lenght2 < GetDiagonal()); + if (lenght2 > concavity) { + concavity = lenght2; + } + + if (((head + 1) & mask) != tail) { + dgBigVector edge10(p1 - p0); + dgBigVector edge20(p2 - p0); + dgBigVector n(edge10 * edge20); + dgFloat64 area2 = n % n; + if (area2 > minArea2) { + dgBigVector p01((p0 + p1).Scale(dgFloat64(0.5f))); + dgBigVector p12((p1 + p2).Scale(dgFloat64(0.5f))); + dgBigVector p20((p2 + p0).Scale(dgFloat64(0.5f))); + + pool[head][0] = p0; + pool[head][1] = p01; + pool[head][2] = p20; + head = (head + 1) & mask; + + if (((head + 1) & mask) != tail) { + pool[head][0] = p1; + pool[head][1] = p12; + pool[head][2] = p01; + head = (head + 1) & mask; + + if (((head + 1) & mask) != tail) { + pool[head][0] = p2; + pool[head][1] = p20; + pool[head][2] = p12; + head = (head + 1) & mask; + } + } + } + } + } + return concavity; + } + + + + dgFloat64 FaceRayCast (const dgConvexHull3DFace* const face, const dgBigVector& origin, const dgBigVector& dist, dgFloat64& normalProjection) const + { + dgInt32 i0 = face->m_index[0]; + dgInt32 i1 = face->m_index[1]; + dgInt32 i2 = face->m_index[2]; + + const dgBigVector& p0 = m_points[i0]; + dgBigVector normal ((m_points[i1] - p0) * (m_points[i2] - p0)); + + dgFloat64 N = (origin - p0) % normal; + dgFloat64 D = dist % normal; + + if (fabs(D) < dgFloat64 (1.0e-16f)) { // + normalProjection = dgFloat32 (0.0); + if (N > dgFloat64 (0.0f)) { + return dgFloat32 (-1.0e30); + } else { + + return dgFloat32 (1.0e30); + } + } + normalProjection = D; + return - N / D; + } + + dgConvexHull3DFace* ClosestFaceVertexToPoint (const dgBigVector& point) + { + // note, for this function to be effective point should be an already close point to the Hull. + // for example casting the point to the OBB or the AABB of the full is a good first guess. + dgConvexHull3DFace* closestFace = &GetFirst()->GetInfo(); + dgInt8 pool[256 * (sizeof (dgConvexHull3DFace*) + sizeof (dgFloat64))]; + dgUpHeap heap (pool, sizeof (pool)); + + for (dgInt32 i = 0; i < 3; i ++) { + dgBigVector dist (m_points[closestFace->m_index[i]] - point); + heap.Push(closestFace, dist % dist); + } + + m_mark ++; + dgFloat64 minDist = heap.Value(); + while (heap.GetCount()) { + dgConvexHull3DFace* const face = heap[0]; + if (heap.Value() < minDist) { + minDist = heap.Value(); + closestFace = face; + } + heap.Pop(); + //face->m_mark = m_mark; + face->SetMark(m_mark); + for (dgInt32 i = 0; i < 3; i ++) { + //const dgConvexHull3DFace* twin = &face->m_twin[i]->GetInfo(); + dgConvexHull3DFace* twin = &face->GetTwin(i)->GetInfo(); + //if (twin->m_mark != m_mark) { + if (twin->GetMark() != m_mark) { + dgBigVector dist (m_points[twin->m_index[i]] - point); + // use hysteresis to prevent stops at a local minimal, but at the same time fast descend + dgFloat64 dist2 = dist % dist; + if (dist2 < (minDist * dgFloat64 (1.001f))) { + heap.Push(twin, dist2); + } + } + } + } + + return closestFace; + } + + + // this version have input sensitive complexity (approximately log2) + // when casting parallel rays and using the last face as initial guess this version has const time complexity + dgFloat64 RayCast (const dgBigVector& localP0, const dgBigVector& localP1, dgConvexHull3DFace** firstFaceGuess) + { + dgConvexHull3DFace* face = &GetFirst()->GetInfo(); + if (firstFaceGuess && *firstFaceGuess) { + face = *firstFaceGuess; + } else { + if (GetCount() > 32) { + dgVector q0 (localP0); + dgVector q1 (localP1); + if (dgRayBoxClip (q0, q1, m_aabbP0, m_aabbP1)) { + face = ClosestFaceVertexToPoint (q0); + } + } + } + + m_mark ++; + //face->m_mark = m_mark; + face->SetMark (m_mark); + dgInt8 pool[256 * (sizeof (dgConvexHullRayCastData) + sizeof (dgFloat64))]; + dgDownHeap heap (pool, sizeof (pool)); + + dgFloat64 t0 = dgFloat64 (-1.0e20); //for the maximum entering segment parameter; + dgFloat64 t1 = dgFloat64 ( 1.0e20); //for the minimum leaving segment parameter; + dgBigVector dS (localP1 - localP0); // is the segment direction vector; + dgConvexHullRayCastData data; + data.m_face = face; + dgFloat64 t = FaceRayCast (face, localP0, dS, data.m_normalProjection); + if (data.m_normalProjection >= dgFloat32 (0.0)) { + t = dgFloat64 (-1.0e30); + } + + heap.Push (data, t); + while (heap.GetCount()) { + dgConvexHullRayCastData data (heap[0]); + dgFloat64 t = heap.Value(); + dgConvexHull3DFace* face = data.m_face; + dgFloat64 normalDistProjection = data.m_normalProjection; + heap.Pop(); + bool foundThisBestFace = true; + if (normalDistProjection < dgFloat64 (0.0f)) { + if (t > t0) { + t0 = t; + } + if (t0 > t1) { + return dgFloat64 (1.2f); + } + } else { + foundThisBestFace = false; + } + + for (dgInt32 i = 0; i < 3; i ++) { + //dgConvexHull3DFace* const face1 = &face->m_twin[i]->GetInfo(); + dgConvexHull3DFace* const face1 = &face->GetTwin(i)->GetInfo(); + + //if (face1->m_mark != m_mark) { + if (face1->GetMark() != m_mark) { + //face1->m_mark = m_mark; + face1->SetMark (m_mark); + dgConvexHullRayCastData data; + data.m_face = face1; + dgFloat64 t = FaceRayCast (face1, localP0, dS, data.m_normalProjection); + if (data.m_normalProjection >= dgFloat32 (0.0)) { + t = dgFloat64 (-1.0e30); + } else if (t > t0) { + foundThisBestFace = false; + } else if (fabs (t - t0) < dgFloat64 (1.0e-10f)) { + return dgConvexHull3d::RayCast (localP0, localP1); + } + if ((heap.GetCount() + 2)>= heap.GetMaxCount()) { + // remove t values that are old and way outside interval [0.0, 1.0] + for (dgInt32 i = heap.GetCount() - 1; i >= 0; i--) { + dgFloat64 val = heap.Value(i); + if ((val < dgFloat64 (-100.0f)) || (val > dgFloat64 (100.0f))) { + heap.Remove(i); + } + } + } + heap.Push (data, t); + } + } + if (foundThisBestFace) { + if ((t0 >= dgFloat64 (0.0f)) && (t0 <= dgFloat64 (1.0f))) { + if (firstFaceGuess) { + *firstFaceGuess = face; + } + return t0; + } + break; + } + } + + return dgFloat64 (1.2f); + + } + + dgFloat64 FastRayCast (const dgBigVector& localP0, const dgBigVector& localP1, dgConvexHull3DFace** guess) + { +#if 0 + #ifdef _DEBUG + dgFloat64 t0 = dgConvexHull3d::RayCast (localP0, localP1); + dgFloat64 t1 = RayCast (localP0, localP1, guess); + dgAssert (fabs(t0 - t1) < dgFloat64 (1.0e-5f)); + #endif +#endif + + //return dgConvexHull3d::RayCast (localP0, localP1); + return RayCast (localP0, localP1, guess); + } + + dgInt32 m_mark; + }; + + class dgHACDConvacityLookAheadTree + { + public: + DG_CLASS_ALLOCATOR(allocator) + + dgHACDConvacityLookAheadTree (dgMemoryAllocator* const allocator, dgEdge* const face, dgFloat64 concavity) + :m_concavity(concavity) + ,m_faceList (allocator) + ,m_left (NULL) + ,m_right (NULL) + { + m_faceList.Append(face); + } + + + dgHACDConvacityLookAheadTree (dgMemoryAllocator* const allocator, dgHACDConvacityLookAheadTree* const leftChild, dgHACDConvacityLookAheadTree* const rightChild, dgFloat64 concavity) + :m_concavity(concavity) + ,m_faceList (allocator) + ,m_left (leftChild) + ,m_right (rightChild) + { + dgAssert (leftChild); + dgAssert (rightChild); + + dgFloat64 concavityTest = m_concavity - dgFloat64 (1.0e-5f); + //if ((m_left->m_faceList.GetCount() == 1) || (m_right->m_faceList.GetCount() == 1)) { + if ((((m_left->m_faceList.GetCount() == 1) || (m_right->m_faceList.GetCount() == 1))) || + ((concavityTest <= m_left->m_concavity) && (concavityTest <= m_right->m_concavity))) { + //The the parent has lower concavity this mean that the two do no add more detail, + //the can be deleted and replaced the parent node + // for example the two children can be two convex strips that are part of a larger convex piece + // but each part has a non zero concavity, while the convex part has a lower concavity + m_faceList.Merge (m_left->m_faceList); + m_faceList.Merge (m_right->m_faceList); + + delete m_left; + delete m_right; + m_left = NULL; + m_right = NULL; + } else { + for (dgList::dgListNode* node = m_left->m_faceList.GetFirst(); node; node = node->GetNext()) { + m_faceList.Append(node->GetInfo()); + } + for (dgList::dgListNode* node = m_right->m_faceList.GetFirst(); node; node = node->GetNext()) { + m_faceList.Append(node->GetInfo()); + } + } + } + + ~dgHACDConvacityLookAheadTree () + { + if (m_left) { + dgAssert (m_right); + delete m_left; + delete m_right; + } + } + + dgInt32 GetNodesCount () const + { + dgInt32 count = 0; + dgInt32 stack = 1; + const dgHACDConvacityLookAheadTree* pool[1024]; + pool[0] = this; + while (stack) { + stack --; + count ++; + const dgHACDConvacityLookAheadTree* const root = pool[stack]; + if (root->m_left) { + dgAssert (root->m_right); + pool[stack] = root->m_left; + stack ++; + dgAssert (stack < sizeof (pool)/sizeof (pool[0])); + pool[stack] = root->m_right; + stack ++; + dgAssert (stack < sizeof (pool)/sizeof (pool[0])); + } + } + return count; + } + + void ReduceByCount (dgInt32 count, dgDownHeap& approximation) + { + if (count < 1) { + count = 1; + } +// dgInt32 nodesCount = GetNodesCount(); + + approximation.Flush(); + dgHACDConvacityLookAheadTree* tmp = this; + approximation.Push(tmp, m_concavity); +// nodesCount --; + //while (nodesCount && (approximation.GetCount() < count) && (approximation.Value() >= dgFloat32 (0.0f))) { + while ((approximation.GetCount() < count) && (approximation.Value() >= dgFloat32 (0.0f))) { + dgHACDConvacityLookAheadTree* worseCluster = approximation[0]; + if (!worseCluster->m_left && approximation.Value() >= dgFloat32 (0.0f)) { + approximation.Pop(); + approximation.Push(worseCluster, dgFloat32 (-1.0f)); + } else { + dgAssert (worseCluster->m_left); + dgAssert (worseCluster->m_right); + approximation.Pop(); + approximation.Push(worseCluster->m_left, worseCluster->m_left->m_concavity); + approximation.Push(worseCluster->m_right, worseCluster->m_right->m_concavity); +// nodesCount -= 2; + } + } + } + + + void ReduceByConcavity (dgFloat64 concavity, dgDownHeap& approximation) + { + approximation.Flush(); + dgHACDConvacityLookAheadTree* tmp = this; + + approximation.Push(tmp, m_concavity); + while (approximation.Value() > concavity) { + dgHACDConvacityLookAheadTree* worseCluster = approximation[0]; + if (!worseCluster->m_left && approximation.Value() >= dgFloat32 (0.0f)) { + approximation.Pop(); + approximation.Push(worseCluster, dgFloat32 (-1.0f)); + } else { + dgAssert (worseCluster->m_left); + dgAssert (worseCluster->m_right); + approximation.Pop(); + approximation.Push(worseCluster->m_left, worseCluster->m_left->m_concavity); + approximation.Push(worseCluster->m_right, worseCluster->m_right->m_concavity); + } + } + } + + dgFloat64 m_concavity; + dgList m_faceList; + dgHACDConvacityLookAheadTree* m_left; + dgHACDConvacityLookAheadTree* m_right; + }; + + class dgPairProxy + { + public: + dgPairProxy() + :m_nodeA(NULL) + ,m_nodeB(NULL) + ,m_hierachicalClusterIndexA(0) + ,m_hierachicalClusterIndexB(0) + ,m_area(dgFloat64(0.0f)) + { + } + + ~dgPairProxy() + { + } + + dgListNode* m_nodeA; + dgListNode* m_nodeB; + dgInt32 m_hierachicalClusterIndexA; + dgInt32 m_hierachicalClusterIndexB; + dgFloat64 m_area; + dgFloat64 m_distanceConcavity; + }; + + class dgHACDRayCasterContext: public dgFastRayTest + { + public: + dgHACDRayCasterContext (const dgVector& l0, const dgVector& l1, dgHACDClusterGraph* const me, dgInt32 mycolor) + :dgFastRayTest (l0, l1) + ,m_myColor(mycolor) + ,m_colorHit(-1) + ,m_param (1.0f) + ,m_me (me) + { + } + + dgInt32 m_myColor; + dgInt32 m_colorHit; + dgFloat32 m_param; + dgHACDClusterGraph* m_me; + }; + + + dgHACDClusterGraph(dgMeshEffect& mesh, dgFloat32 backFaceDistanceFactor, dgReportProgress reportProgressCallback) + :dgGraph (mesh.GetAllocator()) + ,dgAABBPolygonSoup() + ,m_mark(0) + ,m_faceCount(0) + ,m_vertexMark(0) + ,m_progress(0) + ,m_cancavityTreeIndex(0) + ,m_invFaceCount(dgFloat32 (1.0f)) + ,m_vertexMarks(NULL) + ,m_diagonal(dgFloat64(1.0f)) + ,m_vertexPool(NULL) + ,m_proxyList(mesh.GetAllocator()) + ,m_concavityTreeArray(NULL) + ,m_convexProximation(mesh.GetAllocator()) + ,m_priorityHeap (mesh.GetCount() + 2048, mesh.GetAllocator()) + ,m_reportProgressCallback (reportProgressCallback) + ,m_parallerConcavityCalculator(mesh.GetAllocator()) + { + + m_parallerConcavityCalculator.SetThreadsCount(DG_CONCAVITY_MAX_THREADS); + + // precondition the mesh for better approximation + mesh.ConvertToPolygons(); + + m_faceCount = mesh.GetTotalFaceCount(); + + dgMemoryAllocator* const allocator = mesh.GetAllocator(); + m_invFaceCount = dgFloat32 (1.0f) / (m_faceCount); + + // init some auxiliary structures + dgInt32 vertexCount = mesh.GetVertexCount(); + m_vertexMarks = (dgInt32*) dgMallocStack(vertexCount * sizeof(dgInt32)); + m_vertexPool = (dgBigVector*) dgMallocStack(vertexCount * sizeof(dgBigVector)); + memset(m_vertexMarks, 0, vertexCount * sizeof(dgInt32)); + + m_cancavityTreeIndex = m_faceCount + 1; + m_concavityTreeArray = (dgHACDConvacityLookAheadTree**) dgMallocStack(2 * m_cancavityTreeIndex * sizeof(dgHACDConvacityLookAheadTree*)); + memset(m_concavityTreeArray, 0, 2 * m_cancavityTreeIndex * sizeof(dgHACDConvacityLookAheadTree*)); + + // scan the mesh and and add a node for each face + dgInt32 color = 1; + dgMeshEffect::Iterator iter(mesh); + + dgInt32 meshMask = mesh.IncLRU(); + const dgBigVector* const points = (dgBigVector*) mesh.GetVertexPool(); + for (iter.Begin(); iter; iter++) { + dgEdge* const edge = &(*iter); + if ((edge->m_mark != meshMask) && (edge->m_incidentFace > 0)) { + + // call the progress callback + //ReportProgress(); + + dgListNode* const clusterNode = AddNode (); + dgHACDCluster& cluster = clusterNode->GetInfo().m_nodeData; + cluster.SetAllocator(mesh.GetAllocator()); + + dgFloat64 perimeter = dgFloat64(0.0f); + dgEdge* ptr = edge; + do { + dgBigVector p1p0(points[ptr->m_incidentVertex] - points[ptr->m_prev->m_incidentVertex]); + perimeter += sqrt(p1p0 % p1p0); + ptr->m_incidentFace = color; + ptr->m_userData = dgUnsigned64 (clusterNode); + ptr->m_mark = meshMask; + ptr = ptr->m_next; + } while (ptr != edge); + + dgBigVector normal = mesh.FaceNormal(edge, &points[0][0], sizeof(dgBigVector)); + dgFloat64 mag = sqrt(normal % normal); + + cluster.m_color = color; + cluster.m_hierachicalClusterIndex = color; + cluster.m_area = dgFloat64(0.5f) * mag; + cluster.m_concavity = CalculateConcavityMetric (dgFloat64 (0.0f), cluster.m_area, perimeter, 1, 0); + + dgHACDClusterFace& face = cluster.Append()->GetInfo(); + face.m_edge = edge; + face.m_area = dgFloat64(0.5f) * mag; + face.m_normal = normal.Scale(dgFloat64(1.0f) / mag); + + //m_concavityTreeArray[color] = new (allocator) dgHACDConvacityLookAheadTree (allocator, edge, cluster.m_concavity); + m_concavityTreeArray[color] = new (allocator) dgHACDConvacityLookAheadTree (allocator, edge, dgFloat64 (0.0f)); + + color ++; + } + } + + // add all link adjacent faces links + for (dgListNode* clusterNode = GetFirst(); clusterNode; clusterNode = clusterNode->GetNext()) { + + // call the progress callback + //ReportProgress(); + + dgHACDCluster& cluster = clusterNode->GetInfo().m_nodeData; + dgHACDClusterFace& face = cluster.GetFirst()->GetInfo(); + dgEdge* const edge = face.m_edge; + dgEdge* ptr = edge; + do { + if (ptr->m_twin->m_incidentFace > 0) { + dgAssert (ptr->m_twin->m_userData); + dgListNode* const twinClusterNode = (dgListNode*) ptr->m_twin->m_userData; + dgAssert (twinClusterNode); + + bool doubleEdge = false; + for (dgGraphNode::dgListNode* edgeNode = clusterNode->GetInfo().GetFirst(); edgeNode; edgeNode = edgeNode->GetNext()) { + if (edgeNode->GetInfo().m_node == twinClusterNode) { + doubleEdge = true; + break; + } + } + if (!doubleEdge) { + clusterNode->GetInfo().AddEdge (twinClusterNode); + } + } + ptr = ptr->m_next; + } while (ptr != edge); + } + + Trace(); + + // add links to back faces + dgPolygonSoupDatabaseBuilder builder (mesh.GetAllocator()); + dgVector polygon[64]; + dgInt32 indexList[64]; + + dgMatrix matrix (dgGetIdentityMatrix()); + for (dgInt32 i = 0; i < sizeof (polygon) / sizeof (polygon[0]); i ++) { + indexList[i] = i; + } + + dgBigVector minAABB; + dgBigVector maxAABB; + mesh.CalculateAABB (minAABB, maxAABB); + maxAABB -= minAABB; + dgFloat32 rayDiagonalLength = dgFloat32 (sqrt (maxAABB % maxAABB)); + m_diagonal = rayDiagonalLength; + + builder.Begin(); + dgTree clusterMap (GetAllocator()); + for (dgListNode* clusterNode = GetFirst(); clusterNode; clusterNode = clusterNode->GetNext()) { + + // call the progress callback + //ReportProgress(); + + dgHACDCluster& cluster = clusterNode->GetInfo().m_nodeData; + clusterMap.Insert(clusterNode, cluster.m_color); + dgHACDClusterFace& face = cluster.GetFirst()->GetInfo(); + dgEdge* const edge = face.m_edge; + dgInt32 count = 0; + dgEdge* ptr = edge; + do { + polygon[count] = points[ptr->m_incidentVertex]; + count ++; + ptr = ptr->m_prev; + } while (ptr != edge); + + builder.AddMesh(&polygon[0].m_x, count, sizeof (dgVector), 1, &count, indexList, &cluster.m_color, matrix); + } + builder.End(false); + Create (builder, false); + + + dgFloat32 distanceThreshold = rayDiagonalLength * backFaceDistanceFactor; + for (dgListNode* clusterNodeA = GetFirst(); clusterNodeA; clusterNodeA = clusterNodeA->GetNext()) { + + // call the progress callback + //ReportProgress(); + dgHACDCluster& clusterA = clusterNodeA->GetInfo().m_nodeData; + dgHACDClusterFace& faceA = clusterA.GetFirst()->GetInfo(); + dgEdge* const edgeA = faceA.m_edge; + dgEdge* ptr = edgeA; + + dgVector p0 (points[ptr->m_incidentVertex]); + dgVector p1 (points[ptr->m_next->m_incidentVertex]); + ptr = ptr->m_next->m_next; + do { + dgVector p2 (points[ptr->m_incidentVertex]); + dgVector p01 ((p0 + p1).Scale (dgFloat32 (0.5f))); + dgVector p12 ((p1 + p2).Scale (dgFloat32 (0.5f))); + dgVector p20 ((p2 + p0).Scale (dgFloat32 (0.5f))); + + CastBackFace (clusterNodeA, p0, p01, p20, distanceThreshold, clusterMap); + CastBackFace (clusterNodeA, p1, p12, p01, distanceThreshold, clusterMap); + CastBackFace (clusterNodeA, p2, p20, p12, distanceThreshold, clusterMap); + CastBackFace (clusterNodeA, p01, p12, p20, distanceThreshold, clusterMap); + + p1 = p2; + ptr = ptr->m_next; + } while (ptr != edgeA); + } + + Trace(); + } + + ~dgHACDClusterGraph () + { + for (dgInt32 i = 0; i < m_faceCount * 2; i ++) { + if (m_concavityTreeArray[i]) { + delete m_concavityTreeArray[i]; + } + } + + dgFreeStack(m_concavityTreeArray); + dgFreeStack(m_vertexPool); + dgFreeStack(m_vertexMarks); + } + + + void CastBackFace ( + dgListNode* const clusterNodeA, + const dgVector& p0, + const dgVector& p1, + const dgVector& p2, + dgFloat32 distanceThreshold, + dgTree& clusterMap) + { + dgVector origin ((p0 + p1 + p2).Scale (dgFloat32 (1.0f/3.0f))); + + dgFloat32 rayDistance = distanceThreshold * dgFloat32 (2.0f); + + dgHACDCluster& clusterA = clusterNodeA->GetInfo().m_nodeData; + dgHACDClusterFace& faceA = clusterA.GetFirst()->GetInfo(); + dgVector end (origin - dgVector (faceA.m_normal).Scale (rayDistance)); + + dgHACDRayCasterContext ray (origin, end, this, clusterA.m_color); + ForAllSectorsRayHit(ray, RayHit, &ray); + + if (ray.m_colorHit != -1) { + dgAssert (ray.m_colorHit != ray.m_myColor); + dgFloat32 distance = rayDistance * ray.m_param; + + if (distance < distanceThreshold) { + + dgAssert (ray.m_colorHit != clusterA.m_color); + dgAssert (clusterMap.Find(ray.m_colorHit)); + dgListNode* const clusterNodeB = clusterMap.Find(ray.m_colorHit)->GetInfo(); + dgHACDCluster& clusterB = clusterNodeB->GetInfo().m_nodeData; + + dgHACDClusterFace& faceB = clusterB.GetFirst()->GetInfo(); + dgEdge* const edgeB = faceB.m_edge; + + bool isAdjacent = false; + dgEdge* ptrA = faceA.m_edge; + do { + dgEdge* ptrB = edgeB; + do { + if (ptrB->m_twin == ptrA) { + ptrA = faceA.m_edge->m_prev; + isAdjacent = true; + break; + } + ptrB = ptrB->m_next; + } while (ptrB != edgeB); + + ptrA = ptrA->m_next; + } while (ptrA != faceA.m_edge); + + if (!isAdjacent) { + + isAdjacent = false; + for (dgGraphNode::dgListNode* edgeNode = clusterNodeA->GetInfo().GetFirst(); edgeNode; edgeNode = edgeNode->GetNext()) { + if (edgeNode->GetInfo().m_node == clusterNodeB) { + isAdjacent = true; + break; + } + } + + if (!isAdjacent) { + + dgGraphNode::dgListNode* const edgeNodeAB = clusterNodeA->GetInfo().AddEdge (clusterNodeB); + dgGraphNode::dgListNode* const edgeNodeBA = clusterNodeB->GetInfo().AddEdge (clusterNodeA); + + dgHACDEdge& edgeAB = edgeNodeAB->GetInfo().m_edgeData; + dgHACDEdge& edgeBA = edgeNodeBA->GetInfo().m_edgeData; + edgeAB.m_backFaceHandicap = dgFloat64 (0.5f); + edgeBA.m_backFaceHandicap = dgFloat64 (0.5f); + } + } + } + } + } + + + void Trace() const + { + /* + for (dgListNode* clusterNodeA = GetFirst(); clusterNodeA; clusterNodeA = clusterNodeA->GetNext()) { + dgHACDCluster& clusterA = clusterNodeA->GetInfo().m_nodeData; + //dgHACDClusterFace& faceA = clusterA.GetFirst()->GetInfo(); + //dgEdge* const edgeA = faceA.m_edge; + + dgTrace (("cluster node: %d\n", clusterA.m_color)); + dgTrace ((" links: ")); + for (dgGraphNode::dgListNode* edgeNodeA = clusterNodeA->GetInfo().GetFirst(); edgeNodeA; edgeNodeA = edgeNodeA->GetNext()) { + dgListNode* const clusterNodeB = edgeNodeA->GetInfo().m_node; + dgHACDCluster& clusterB = clusterNodeB->GetInfo().m_nodeData; + dgTrace (("%d ", clusterB.m_color)); + } + dgTrace (("\n")); + } + dgTrace (("\n")); + */ + } + + + // you can insert cal callback here to print the progress as it collapse clusters + void ReportProgress () + { + m_progress ++; + if (m_reportProgressCallback) { + dgFloat32 progress = dgFloat32(m_progress) * m_invFaceCount; + m_reportProgressCallback (progress); + } + } + + dgMeshEffect* CreatePatitionMesh (dgMeshEffect& mesh, dgInt32 maxVertexPerHull) + { + dgMemoryAllocator* const allocator = mesh.GetAllocator(); + dgMeshEffect* const convexPartionMesh = new (allocator) dgMeshEffect(allocator, true); + + dgMeshEffect::dgVertexAtribute polygon[256]; + memset(polygon, 0, sizeof(polygon)); + dgArray convexVertexBuffer(mesh.GetCount(), GetAllocator()); + const dgBigVector* const points = (dgBigVector*) mesh.GetVertexPool(); + + convexPartionMesh->BeginBuild(); + dgFloat64 layer = dgFloat64 (0.0f); + for (dgList::dgListNode* clusterNode = m_convexProximation.GetFirst(); clusterNode; clusterNode = clusterNode->GetNext()) { + dgHACDConvacityLookAheadTree* const cluster = clusterNode->GetInfo(); + + dgInt32 vertexCount = 0; + for (dgList::dgListNode* faceNode = cluster->m_faceList.GetFirst(); faceNode; faceNode = faceNode->GetNext()) { + dgEdge* const edge = faceNode->GetInfo(); + dgEdge* ptr = edge; + do { + dgInt32 index = ptr->m_incidentVertex; + convexVertexBuffer[vertexCount] = points[index]; + vertexCount++; + ptr = ptr->m_next; + } while (ptr != edge); + } + dgConvexHull3d convexHull(allocator, &convexVertexBuffer[0].m_x, sizeof(dgBigVector), vertexCount, 0.0, maxVertexPerHull); + if (convexHull.GetCount()) { + const dgBigVector* const vertex = convexHull.GetVertexPool(); + for (dgConvexHull3d::dgListNode* node = convexHull.GetFirst(); node; node = node->GetNext()) { + const dgConvexHull3DFace* const face = &node->GetInfo(); + + dgInt32 i0 = face->m_index[0]; + dgInt32 i1 = face->m_index[1]; + dgInt32 i2 = face->m_index[2]; + + polygon[0].m_vertex = vertex[i0]; + polygon[0].m_vertex.m_w = layer; + + polygon[1].m_vertex = vertex[i1]; + polygon[1].m_vertex.m_w = layer; + + polygon[2].m_vertex = vertex[i2]; + polygon[2].m_vertex.m_w = layer; + + convexPartionMesh->AddPolygon(3, &polygon[0].m_vertex.m_x, sizeof(dgMeshEffect::dgVertexAtribute), 0); + } + layer += dgFloat64 (1.0f); + } + } + convexPartionMesh->EndBuild(1.0e-5f); + + m_progress = m_faceCount - 1; + ReportProgress(); + + return convexPartionMesh; + } + + + + static dgFloat32 RayHit (void* const context, const dgFloat32* const polygon, dgInt32 strideInBytes, const dgInt32* const indexArray, dgInt32 indexCount) + { + dgHACDRayCasterContext& me = *((dgHACDRayCasterContext*) context); + dgVector normal (&polygon[indexArray[indexCount] * (strideInBytes / sizeof (dgFloat32))]); + dgFloat32 t = me.PolygonIntersect (normal, polygon, strideInBytes, indexArray, indexCount); + if (t < me.m_param) { + dgInt32 faceColor = me.m_me->GetTagId(indexArray); + if (faceColor != me.m_myColor) { + me.m_param = t; + me.m_colorHit = faceColor; + } + } + return t; + } + + + dgFloat64 ConcavityByFaceMedian (dgInt32 faceCountA, dgInt32 faceCountB) const + { + dgFloat64 faceCountCost = DG_CONCAVITY_SCALE * dgFloat64 (0.1f) * (faceCountA + faceCountB) * m_invFaceCount; + //faceCountCost *= 0; + return faceCountCost; + } + + dgFloat64 CalculateConcavityMetric (dgFloat64 convexConcavity, dgFloat64 area, dgFloat64 perimeter, dgInt32 faceCountA, dgInt32 faceCountB) const + { + dgFloat64 edgeCost = perimeter * perimeter / (dgFloat64(4.0f * dgPi) * area); + return convexConcavity * DG_CONCAVITY_SCALE + edgeCost + ConcavityByFaceMedian (faceCountA, faceCountB); + } + + void SubmitInitialEdgeCosts (dgMeshEffect& mesh) + { + m_mark ++; + for (dgListNode* clusterNodeA = GetFirst(); clusterNodeA; clusterNodeA = clusterNodeA->GetNext()) { + // call the progress callback + //ReportProgress(); + + for (dgGraphNode::dgListNode* edgeNodeAB = clusterNodeA->GetInfo().GetFirst(); edgeNodeAB; edgeNodeAB = edgeNodeAB->GetNext()) { + dgHACDEdge& edgeAB = edgeNodeAB->GetInfo().m_edgeData; + dgFloat64 weight = edgeAB.m_backFaceHandicap; + if (edgeAB.m_mark != m_mark) { + edgeAB.m_mark = m_mark; + dgListNode* const clusterNodeB = edgeNodeAB->GetInfo().m_node; + for (dgGraphNode::dgListNode* edgeNodeBA = clusterNodeB->GetInfo().GetFirst(); edgeNodeBA; edgeNodeBA = edgeNodeBA->GetNext()) { + dgListNode* const clusterNode = edgeNodeBA->GetInfo().m_node; + if (clusterNode == clusterNodeA) { + dgHACDEdge& edgeBA = edgeNodeBA->GetInfo().m_edgeData; + edgeBA.m_mark = m_mark; + dgAssert (!edgeAB.m_proxyListNode); + dgAssert (!edgeBA.m_proxyListNode); + + dgList::dgListNode* const proxyNode = SubmitEdgeCost (mesh, clusterNodeA, clusterNodeB, weight * edgeBA.m_backFaceHandicap); + edgeAB.m_proxyListNode = proxyNode; + edgeBA.m_proxyListNode = proxyNode; + break; + } + } + } + } + } + } + + dgInt32 CopyVertexToPool(const dgMeshEffect& mesh, const dgHACDCluster& cluster, dgInt32 start) + { + dgInt32 count = start; + + const dgBigVector* const points = (dgBigVector*) mesh.GetVertexPool(); + for (dgList::dgListNode* node = cluster.GetFirst(); node; node = node->GetNext()) { + const dgHACDClusterFace& clusterFace = node->GetInfo(); + dgEdge* edge = clusterFace.m_edge; + do { + dgInt32 index = edge->m_incidentVertex; + if (m_vertexMarks[index] != m_vertexMark) { + m_vertexMarks[index] = m_vertexMark; + m_vertexPool[count] = points[index]; + count++; + } + edge = edge->m_next; + } while (edge != clusterFace.m_edge); + } + return count; + } + + + void MarkInteriorClusterEdges (dgMeshEffect& mesh, dgInt32 mark, const dgHACDCluster& cluster, dgInt32 colorA, dgInt32 colorB) const + { + dgAssert (colorA != colorB); + for (dgList::dgListNode* node = cluster.GetFirst(); node; node = node->GetNext()) { + dgHACDClusterFace& clusterFace = node->GetInfo(); + dgEdge* edge = clusterFace.m_edge; + do { + if ((edge->m_twin->m_incidentFace == colorA) || (edge->m_twin->m_incidentFace == colorB)) { + edge->m_mark = mark; + edge->m_twin->m_mark = mark; + } + edge = edge->m_next; + } while (edge != clusterFace.m_edge); + } + } + + dgFloat64 CalculateClusterPerimeter (dgMeshEffect& mesh, dgInt32 mark, const dgHACDCluster& cluster, dgInt32 colorA, dgInt32 colorB) const + { + dgAssert (colorA != colorB); + dgFloat64 perimeter = dgFloat64 (0.0f); + const dgBigVector* const points = (dgBigVector*) mesh.GetVertexPool(); + for (dgList::dgListNode* node = cluster.GetFirst(); node; node = node->GetNext()) { + dgHACDClusterFace& clusterFace = node->GetInfo(); + dgEdge* edge = clusterFace.m_edge; + do { + if (!((edge->m_twin->m_incidentFace == colorA) || (edge->m_twin->m_incidentFace == colorB))) { + dgBigVector p1p0(points[edge->m_twin->m_incidentVertex] - points[edge->m_incidentVertex]); + perimeter += sqrt(p1p0 % p1p0); + } + edge = edge->m_next; + } while (edge != clusterFace.m_edge); + } + + return perimeter; + } + + void HeapCollectGarbage () + { + if ((m_priorityHeap.GetCount() + 20) > m_priorityHeap.GetMaxCount()) { + for (dgInt32 i = m_priorityHeap.GetCount() - 1; i >= 0; i--) { + dgList::dgListNode* const emptyNode = m_priorityHeap[i]; + dgPairProxy& emptyPair = emptyNode->GetInfo(); + if ((emptyPair.m_nodeA == NULL) && (emptyPair.m_nodeB == NULL)) { + m_priorityHeap.Remove(i); + } + } + } + } + + + dgFloat64 CalculateConcavity(dgHACDConveHull& hull, const dgMeshEffect& mesh, const dgHACDCluster& cluster) + { + dgFloat64 concavity = dgFloat32(0.0f); + + const dgBigVector* const points = (dgBigVector*) mesh.GetVertexPool(); + for (dgList::dgListNode* node = cluster.GetFirst(); node; node = node->GetNext()) { + dgHACDClusterFace& clusterFace = node->GetInfo(); + dgEdge* edge = clusterFace.m_edge; + dgInt32 i0 = edge->m_incidentVertex; + dgInt32 i1 = edge->m_next->m_incidentVertex; + for (dgEdge* ptr = edge->m_next->m_next; ptr != edge; ptr = ptr->m_next) { + dgInt32 i2 = ptr->m_incidentVertex; + dgFloat64 val = hull.CalculateTriangleConcavity(clusterFace.m_normal, i0, i1, i2, points); + if (val > concavity) { + concavity = val; + } + i1 = i2; + } + } + + return concavity; + } + + dgFloat64 CalculateConcavitySingleThread (dgHACDConveHull& hull, dgMeshEffect& mesh, dgHACDCluster& clusterA, dgHACDCluster& clusterB) + { + return dgMax(CalculateConcavity(hull, mesh, clusterA), CalculateConcavity(hull, mesh, clusterB)); + } + + + class dgConvexHullRayCastContext + { + public: + dgConvexHullRayCastContext (dgHACDConveHull& hull, dgMeshEffect& mesh, dgThreadHive* const manager) + :m_atomicLock(0) + ,m_mesh(&mesh) + ,m_cluster(NULL) + ,m_threadManager(manager) + ,m_faceNode(NULL) + { + for(dgInt32 i = 0; i < DG_CONCAVITY_MAX_THREADS; i ++) { + hullArray[i] = new (mesh.GetAllocator()) dgHACDConveHull (hull); + } + } + + ~dgConvexHullRayCastContext () + { + for(dgInt32 i = 0; i < DG_CONCAVITY_MAX_THREADS; i ++) { + delete hullArray[i]; + } + } + + void SetCluster (dgHACDCluster& cluster) + { + m_cluster = &cluster; + m_node = m_cluster->GetFirst(); + memset (m_concavity, 0, sizeof (m_concavity)); + } + + dgFloat64 GetConcavity() const + { + dgFloat64 concavity = dgFloat32(0.0f); + for (dgInt32 i = 0; i < DG_CONCAVITY_MAX_THREADS; i ++) { + if (concavity < m_concavity[i]) { + concavity = m_concavity[i]; + } + } + return concavity; + } + + + static void RayCastKernel (void* const context, dgInt32 threadID) + { + dgConvexHullRayCastContext* const data = (dgConvexHullRayCastContext*) context; + const dgBigVector* const points = (dgBigVector*) data->m_mesh->GetVertexPool(); + + data->m_threadManager->GetIndirectLock(&data->m_atomicLock, threadID); + dgList::dgListNode* node = data->m_node; + if (node) { + data->m_node = node->GetNext(); + } + data->m_threadManager->ReleaseIndirectLock (&data->m_atomicLock); + for (; node;) { + + dgHACDClusterFace& clusterFace = node->GetInfo(); + dgEdge* edge = clusterFace.m_edge; + dgInt32 i0 = edge->m_incidentVertex; + dgInt32 i1 = edge->m_next->m_incidentVertex; + for (dgEdge* ptr = edge->m_next->m_next; ptr != edge; ptr = ptr->m_next) { + dgInt32 i2 = ptr->m_incidentVertex; + dgFloat64 val = data->hullArray[threadID]->CalculateTriangleConcavity(clusterFace.m_normal, i0, i1, i2, points); + if (val > data->m_concavity[threadID]) { + data->m_concavity[threadID] = val; + } + i1 = i2; + } + + data->m_threadManager->GetIndirectLock(&data->m_atomicLock, threadID); + node = data->m_node; + if (node) { + data->m_node = node->GetNext();; + } + data->m_threadManager->ReleaseIndirectLock (&data->m_atomicLock); + } + } + + + dgInt32 m_atomicLock; + dgMeshEffect* m_mesh; + dgHACDCluster* m_cluster; + dgThreadHive* m_threadManager; + dgList::dgListNode* m_node; + + dgList::dgListNode* m_faceNode; + dgFloat64 m_concavity[DG_CONCAVITY_MAX_THREADS]; + dgHACDConveHull* hullArray[DG_CONCAVITY_MAX_THREADS]; + }; + + + dgFloat64 CalculateConcavityMultiThread (dgHACDConveHull& hull, dgMeshEffect& mesh, dgHACDCluster& clusterA, dgHACDCluster& clusterB) + { + dgConvexHullRayCastContext data (hull, mesh, &m_parallerConcavityCalculator); + + dgInt32 threadsCount = m_parallerConcavityCalculator.GetThreadCount(); + data.SetCluster (clusterA); + for (dgInt32 i = 0; i < threadsCount; i ++) { + m_parallerConcavityCalculator.QueueJob(dgConvexHullRayCastContext::RayCastKernel, &data); + } + m_parallerConcavityCalculator.SynchronizationBarrier(); + dgFloat64 concavity = data.GetConcavity(); + + data.SetCluster (clusterB); + for (dgInt32 i = 0; i < threadsCount; i ++) { + m_parallerConcavityCalculator.QueueJob(dgConvexHullRayCastContext::RayCastKernel, &data); + } + m_parallerConcavityCalculator.SynchronizationBarrier(); + + concavity = dgMax(concavity, data.GetConcavity()); + //dgFloat64 xxx = CalculateConcavitySingleThread (hull, mesh, clusterA, clusterB); + //dgAssert (fabs(concavity - xxx) < dgFloat64 (1.0e-5f)); + return concavity; + } + + dgList::dgListNode* SubmitEdgeCost (dgMeshEffect& mesh, dgListNode* const clusterNodeA, dgListNode* const clusterNodeB, dgFloat64 perimeterHandicap) + { + dgHACDCluster& clusterA = clusterNodeA->GetInfo().m_nodeData; + dgHACDCluster& clusterB = clusterNodeB->GetInfo().m_nodeData; + const dgBigVector* const points = (dgBigVector*) mesh.GetVertexPool(); + + bool flatStrip = true; + dgFloat64 tol = dgFloat64 (1.0e-5f) * m_diagonal; + dgHACDClusterFace& clusterFaceA = clusterA.GetFirst()->GetInfo(); + dgBigPlane plane(clusterFaceA.m_normal, -(points[clusterFaceA.m_edge->m_incidentVertex] % clusterFaceA.m_normal)); + + if (clusterA.GetCount() > 1) { + flatStrip = clusterA.IsCoplanar(plane, mesh, tol); + } + + if (flatStrip) { + flatStrip = clusterB.IsCoplanar(plane, mesh, tol); + } + + dgList::dgListNode* pairNode = NULL; + if (!flatStrip) { + m_vertexMark ++; + dgInt32 vertexCount = CopyVertexToPool(mesh, clusterA, 0); + vertexCount = CopyVertexToPool(mesh, clusterB, vertexCount); + + dgHACDConveHull convexHull(mesh.GetAllocator(), m_vertexPool, vertexCount); + + if (convexHull.GetVertexCount()) { + dgInt32 mark = mesh.IncLRU(); + MarkInteriorClusterEdges (mesh, mark, clusterA, clusterA.m_color, clusterB.m_color); + MarkInteriorClusterEdges (mesh, mark, clusterB, clusterA.m_color, clusterB.m_color); + + dgFloat64 area = clusterA.m_area + clusterB.m_area; + dgFloat64 perimeter = CalculateClusterPerimeter (mesh, mark, clusterA, clusterA.m_color, clusterB.m_color) + + CalculateClusterPerimeter (mesh, mark, clusterB, clusterA.m_color, clusterB.m_color); + + + dgFloat64 concavity = dgFloat64 (0.0f); + if ((convexHull.GetCount() > 128) && ((clusterA.GetCount() > 256) || (clusterB.GetCount() > 256))) { + concavity = CalculateConcavityMultiThread (convexHull, mesh, clusterA, clusterB); + } else { + concavity = CalculateConcavitySingleThread (convexHull, mesh, clusterA, clusterB); + } + + if (concavity < dgFloat64(1.0e-3f)) { + concavity = dgFloat64(0.0f); + } + + // see if the heap will overflow + HeapCollectGarbage (); + + // add a new pair to the heap + dgList::dgListNode* pairNode = m_proxyList.Append(); + dgPairProxy& pair = pairNode->GetInfo(); + pair.m_nodeA = clusterNodeA; + pair.m_nodeB = clusterNodeB; + pair.m_distanceConcavity = concavity; + pair.m_hierachicalClusterIndexA = clusterA.m_hierachicalClusterIndex; + pair.m_hierachicalClusterIndexB = clusterB.m_hierachicalClusterIndex; + + pair.m_area = area; + dgFloat64 cost = CalculateConcavityMetric (concavity, area, perimeter * perimeterHandicap, clusterA.GetCount(), clusterB.GetCount()); + m_priorityHeap.Push(pairNode, cost); + + return pairNode; + } + } + return pairNode; + } + + + void CollapseEdge (dgList::dgListNode* const pairNode, dgMeshEffect& mesh, dgFloat64 concavity) + { + dgListNode* adjacentNodes[1024]; + dgPairProxy& pair = pairNode->GetInfo(); + + dgMemoryAllocator* const allocator = mesh.GetAllocator(); + + + dgAssert((pair.m_nodeA && pair.m_nodeB) || (!pair.m_nodeA && !pair.m_nodeB)); + if (pair.m_nodeA && pair.m_nodeB) { + // call the progress callback + ReportProgress(); + + dgListNode* const clusterNodeA = pair.m_nodeA; + dgListNode* const clusterNodeB = pair.m_nodeB; + dgAssert (clusterNodeA != clusterNodeB); + + dgHACDCluster& clusterA = clusterNodeA->GetInfo().m_nodeData; + dgHACDCluster& clusterB = clusterNodeB->GetInfo().m_nodeData; + + dgAssert (&clusterA != &clusterB); + dgAssert(clusterA.m_color != clusterB.m_color); + + dgHACDConvacityLookAheadTree* const leftTree = m_concavityTreeArray[pair.m_hierachicalClusterIndexA]; + dgHACDConvacityLookAheadTree* const rightTree = m_concavityTreeArray[pair.m_hierachicalClusterIndexB]; + dgAssert (leftTree); + dgAssert (rightTree); + m_concavityTreeArray[pair.m_hierachicalClusterIndexA] = NULL; + m_concavityTreeArray[pair.m_hierachicalClusterIndexB] = NULL; + dgAssert (m_cancavityTreeIndex < (2 * (m_faceCount + 1))); + + dgFloat64 treeConcavity = pair.m_distanceConcavity; +// dgAssert (treeConcavity < 0.1); + m_concavityTreeArray[m_cancavityTreeIndex] = new (allocator) dgHACDConvacityLookAheadTree (allocator, leftTree, rightTree, treeConcavity); + clusterA.m_hierachicalClusterIndex = m_cancavityTreeIndex; + clusterB.m_hierachicalClusterIndex = m_cancavityTreeIndex; + m_cancavityTreeIndex ++; + + // merge two clusters + while (clusterB.GetCount()) { + + dgHACDCluster::dgListNode* const nodeB = clusterB.GetFirst(); + clusterB.Unlink(nodeB); + + // now color code all faces of the merged cluster + dgHACDClusterFace& faceB = nodeB->GetInfo(); + dgEdge* ptr = faceB.m_edge; + do { + ptr->m_incidentFace = clusterA.m_color; + ptr = ptr->m_next; + } while (ptr != faceB.m_edge); + clusterA.Append(nodeB); + } + clusterA.m_area = pair.m_area; + clusterA.m_concavity = concavity; + + // invalidate all proxies that are still in the heap + dgInt32 adjacentCount = 1; + adjacentNodes[0] = clusterNodeA; + for (dgGraphNode::dgListNode* edgeNodeAB = clusterNodeA->GetInfo().GetFirst(); edgeNodeAB; edgeNodeAB = edgeNodeAB->GetNext()) { + dgHACDEdge& edgeAB = edgeNodeAB->GetInfo().m_edgeData; + dgList::dgListNode* const proxyNode = (dgList::dgListNode*) edgeAB.m_proxyListNode; + if (proxyNode) { + dgPairProxy& pairProxy = proxyNode->GetInfo(); + dgAssert ((edgeNodeAB->GetInfo().m_node == pairProxy.m_nodeA) || (edgeNodeAB->GetInfo().m_node == pairProxy.m_nodeB)); + pairProxy.m_nodeA = NULL; + pairProxy.m_nodeB = NULL; + edgeAB.m_proxyListNode = NULL; + } + + adjacentNodes[adjacentCount] = edgeNodeAB->GetInfo().m_node; + adjacentCount ++; + dgAssert (adjacentCount < sizeof (adjacentNodes)/ sizeof (adjacentNodes[0])); + } + + for (dgGraphNode::dgListNode* edgeNodeBA = clusterNodeB->GetInfo().GetFirst(); edgeNodeBA; edgeNodeBA = edgeNodeBA->GetNext()) { + dgHACDEdge& edgeBA = edgeNodeBA->GetInfo().m_edgeData; + dgList::dgListNode* const proxyNode = (dgList::dgListNode*) edgeBA.m_proxyListNode; + if (proxyNode) { + dgPairProxy& pairProxy = proxyNode->GetInfo(); + pairProxy.m_nodeA = NULL; + pairProxy.m_nodeB = NULL; + edgeBA.m_proxyListNode = NULL; + } + + bool alreadyLinked = false; + dgListNode* const node = edgeNodeBA->GetInfo().m_node; + for (dgInt32 i = 0; i < adjacentCount; i ++) { + if (node == adjacentNodes[i]) { + alreadyLinked = true; + break; + } + } + if (!alreadyLinked) { + clusterNodeA->GetInfo().AddEdge (node); + node->GetInfo().AddEdge (clusterNodeA); + } + } + DeleteNode (clusterNodeB); + + // submit all new costs for each edge connecting this new node to any other node + for (dgGraphNode::dgListNode* edgeNodeAB = clusterNodeA->GetInfo().GetFirst(); edgeNodeAB; edgeNodeAB = edgeNodeAB->GetNext()) { + dgHACDEdge& edgeAB = edgeNodeAB->GetInfo().m_edgeData; + dgListNode* const clusterNodeB = edgeNodeAB->GetInfo().m_node; + dgFloat64 weigh = edgeAB.m_backFaceHandicap; + for (dgGraphNode::dgListNode* edgeNodeBA = clusterNodeB->GetInfo().GetFirst(); edgeNodeBA; edgeNodeBA = edgeNodeBA->GetNext()) { + dgListNode* const clusterNode = edgeNodeBA->GetInfo().m_node; + if (clusterNode == clusterNodeA) { + dgHACDEdge& edgeBA = edgeNodeBA->GetInfo().m_edgeData; + dgList::dgListNode* const proxyNode = SubmitEdgeCost (mesh, clusterNodeA, clusterNodeB, weigh * edgeBA.m_backFaceHandicap); + if (proxyNode) { + edgeBA.m_proxyListNode = proxyNode; + edgeAB.m_proxyListNode = proxyNode; + } + break; + } + } + } + } + m_proxyList.Remove(pairNode); + } + +#ifdef DG_BUILD_HIERACHICAL_HACD + void CollapseClusters (dgMeshEffect& mesh, dgFloat64 maxConcavity, dgInt32 maxClustesCount) + { + + maxConcavity *= (m_diagonal * DG_CONCAVITY_SCALE); + while (m_priorityHeap.GetCount()) { + dgFloat64 concavity = m_priorityHeap.Value(); + dgList::dgListNode* const pairNode = m_priorityHeap[0]; + m_priorityHeap.Pop(); + CollapseEdge (pairNode, mesh, concavity); + +//if (m_progress == 24) +//break; + + } + + + + dgInt32 treeCounts = 0; + for (dgInt32 i = 0; i < m_cancavityTreeIndex; i ++) { + if (m_concavityTreeArray[i]) { + m_concavityTreeArray[treeCounts] = m_concavityTreeArray[i]; + m_concavityTreeArray[i] = NULL; + treeCounts ++; + } + } + + if (treeCounts > 1) { + + for (dgInt32 i = 0; i < treeCounts; i ++) { + if (m_concavityTreeArray[i]->m_faceList.GetCount()==1) { + delete m_concavityTreeArray[i]; + m_concavityTreeArray[i] = m_concavityTreeArray[treeCounts-1]; + m_concavityTreeArray[treeCounts-1]= NULL; + treeCounts --; + i--; + } + } + + + dgFloat32 C = 10000; + while (treeCounts > 1) { + dgHACDConvacityLookAheadTree* const leftTree = m_concavityTreeArray[treeCounts-1]; + dgHACDConvacityLookAheadTree* const rightTree = m_concavityTreeArray[treeCounts-2]; + m_concavityTreeArray[treeCounts-1] = NULL; + m_concavityTreeArray[treeCounts-2] = new (mesh.GetAllocator()) dgHACDConvacityLookAheadTree (mesh.GetAllocator(), leftTree, rightTree, C); + C *= 2; + treeCounts --; + } + + } + + dgHACDConvacityLookAheadTree* const tree = m_concavityTreeArray[0]; + dgDownHeap approximation(maxClustesCount * 2, mesh.GetAllocator()); + + tree->ReduceByCount (maxClustesCount, approximation); + // tree->ReduceByConcavity (maxConcavity, approximation); + + while (approximation.GetCount()) { + m_convexProximation.Append(approximation[0]); + approximation.Pop(); + } + } +#else + void CollapseClusters (dgMeshEffect& mesh, dgFloat64 maxConcavity, dgInt32 maxClustesCount) + { + maxConcavity *= (m_diagonal * DG_CONCAVITY_SCALE); + + bool terminate = false; + while (m_priorityHeap.GetCount() && !terminate) { + dgFloat64 concavity = m_priorityHeap.Value(); + dgList::dgListNode* const pairNode = m_priorityHeap[0]; + if ((concavity < maxConcavity) && (GetCount() < maxClustesCount)) { + terminate = true; + } else { + m_priorityHeap.Pop(); + CollapseEdge (pairNode, mesh, concavity); + } + } + } +#endif + + dgInt32 m_mark; + dgInt32 m_faceCount; + dgInt32 m_vertexMark; + dgInt32 m_progress; + dgInt32 m_cancavityTreeIndex; + dgInt32* m_vertexMarks; + dgFloat32 m_invFaceCount; + dgFloat64 m_diagonal; + dgBigVector* m_vertexPool; + dgList m_proxyList; + dgHACDConvacityLookAheadTree** m_concavityTreeArray; + dgList m_convexProximation; + dgUpHeap::dgListNode*, dgFloat64> m_priorityHeap; + dgReportProgress m_reportProgressCallback; + dgThreadHive m_parallerConcavityCalculator; +}; + +#endif + +dgMeshEffect* dgMeshEffect::CreateSimplification(dgInt32 maxVertexCount, dgReportProgress reportProgressCallback, void* const reportPrgressUserData) const +{ + if (GetVertexCount() <= maxVertexCount) { + return new (GetAllocator()) dgMeshEffect(*this); + } +dgAssert (0); +return new (GetAllocator()) dgMeshEffect(*this); +/* + // dgMeshEffect triangleMesh(*this); + if (maxHullsCount <= 1) { + maxHullsCount = 1; + } + if (maxConcavity <= dgFloat32 (1.0e-5f)) { + maxConcavity = dgFloat32 (1.0e-5f); + } + + if (maxVertexPerHull < 4) { + maxVertexPerHull = 4; + } + ClampValue(backFaceDistanceFactor, dgFloat32 (0.01f), dgFloat32 (1.0f)); + + if (reportProgressCallback) { + reportProgressCallback (0.0f); + } + + + // make a copy of the mesh + dgMeshEffect mesh(*this); + mesh.ClearAttributeArray(); + + + dgInt32 faceCount = mesh.GetTotalFaceCount(); + if (faceCount > meshSimplicationMaxFaceCount) { + mesh.Triangulate(); + + dgPolyhedra polygon(GetAllocator()); + dgInt32 mark = mesh.IncLRU(); + polygon.BeginFace(); + dgPolyhedra::Iterator iter (mesh); + for (iter.Begin(); iter; iter ++){ + dgEdge* const face = &(*iter); + + if ((face->m_mark != mark) && (face->m_incidentFace > 0)) { + dgInt32 index[DG_MESH_EFFECT_POINT_SPLITED]; + + dgEdge* ptr = face; + dgInt32 indexCount = 0; + do { + index[indexCount] = ptr->m_incidentVertex; + ptr->m_mark = mark; + indexCount ++; + ptr = ptr->m_next; + } while (ptr != face); + polygon.AddFace(indexCount, index); + } + } + polygon.EndFace(); + + polygon.Optimize(&mesh.m_points[0].m_x, sizeof (dgFloat64), 1000.0f, meshSimplicationMaxFaceCount); + + mesh.RemoveAll(); + + mark = polygon.IncLRU(); + mesh.BeginFace(); + dgPolyhedra::Iterator iter1 (polygon); + for (iter1.Begin(); iter1; iter1 ++){ + dgEdge* const face = &(*iter1); + if ((face->m_mark != mark) && (face->m_incidentFace > 0)) { + dgInt32 index[DG_MESH_EFFECT_POINT_SPLITED]; + dgEdge* ptr = face; + dgInt32 indexCount = 0; + do { + ptr->m_mark = mark; + index[indexCount] = dgInt32 (ptr->m_incidentVertex); + indexCount ++; + ptr = ptr->m_next; + } while (ptr != face); + mesh.AddFace(indexCount, index); + } + } + mesh.EndFace(); + + faceCount = mesh.GetTotalFaceCount(); + mesh.ClearAttributeArray(); + } + + // create a general connectivity graph + dgHACDClusterGraph graph (mesh, backFaceDistanceFactor, reportProgressCallback); + + // calculate initial edge costs + graph.SubmitInitialEdgeCosts (mesh); + + // collapse the graph + graph.CollapseClusters (mesh, maxConcavity, maxHullsCount); + + // Create Partition Mesh + return graph.CreatePatitionMesh (mesh, maxVertexPerHull); +*/ +} diff --git a/thirdparty/src/newton/dgMeshUtil/dgMeshEffect5.cpp b/thirdparty/src/newton/dgMeshUtil/dgMeshEffect5.cpp new file mode 100644 index 000000000..c33ec3786 --- /dev/null +++ b/thirdparty/src/newton/dgMeshUtil/dgMeshEffect5.cpp @@ -0,0 +1,1160 @@ +/* Copyright (c) <2003-2019> +* +* 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 "dgPhysicsStdafx.h" +#include "dgBody.h" +#include "dgWorld.h" +#include "dgMeshEffect.h" +#include "dgCollisionConvexHull.h" + + + +bool dgMeshEffect::PlaneClip(const dgMeshEffect& convexMesh, const dgEdge* const convexFace) +{ + dgAssert (convexFace->m_incidentFace > 0); + + dgBigVector normal (convexMesh.FaceNormal(convexFace, &convexMesh.m_points.m_vertex[0].m_x, sizeof(dgBigVector))); + dgFloat64 mag2 = normal.DotProduct(normal).GetScalar(); + if (mag2 < dgFloat64 (1.0e-30)) { + dgAssert (0); + return true; + } + + normal = normal.Normalize(); + dgBigVector origin (convexMesh.m_points.m_vertex[convexFace->m_incidentVertex]); + dgBigPlane plane (normal, - origin.DotProduct(normal).GetScalar()); + + dgAssert (!HasOpenEdges()); + + dgInt32 pointCount = GetVertexCount(); + dgStack testPool (2 * pointCount + 1024); + dgFloat64* const test = &testPool[0]; + for (dgInt32 i = 0; i < pointCount; i ++) { + test[i] = plane.Evalue (m_points.m_vertex[i]); + if (fabs (test[i]) < dgFloat32 (1.0e-5f)) { + test[i] = dgFloat32 (0.0f); + } + } + + dgInt32 positive = 0; + dgInt32 negative = 0; + dgPolyhedra::Iterator iter (*this); + for (iter.Begin(); iter && !(positive && negative); iter ++){ + dgEdge* const edge = &(*iter); + positive += test[edge->m_incidentVertex] > dgFloat32 (0.0f); + negative += test[edge->m_incidentVertex] < dgFloat32 (0.0f); + } + if (positive && !negative) { + return false; + } + + if (positive && negative) { + const dgEdge* e0 = convexFace; + const dgEdge* e1 = e0->m_next; + const dgEdge* e2 = e1->m_next; + + dgMatrix matrix; + dgBigVector p1 (convexMesh.m_points.m_vertex[e1->m_incidentVertex]); + + dgBigVector xDir (p1 - origin); + dgAssert(xDir.m_w == dgFloat32(0.0f)); + dgAssert (xDir.DotProduct(xDir).GetScalar() > dgFloat32 (0.0f)); + matrix[2] = dgVector (normal); + matrix[0] = dgVector(xDir.Scale(dgFloat64 (1.0f) / sqrt (xDir.DotProduct(xDir).GetScalar()))); + matrix[1] = matrix[2].CrossProduct(matrix[0]); + matrix[3] = dgVector (origin); + matrix[3][3] = dgFloat32 (1.0f); + + dgVector q0 (matrix.UntransformVector(dgVector(convexMesh.m_points.m_vertex[e0->m_incidentVertex]))); + dgVector q1 (matrix.UntransformVector(dgVector(convexMesh.m_points.m_vertex[e1->m_incidentVertex]))); + dgVector q2 (matrix.UntransformVector(dgVector(convexMesh.m_points.m_vertex[e2->m_incidentVertex]))); + + dgVector p10 (q1 - q0); + dgVector p20 (q2 - q0); + dgVector faceNormal (matrix.UnrotateVector (dgVector(normal))); + dgAssert(faceNormal.m_w == dgFloat32(0.0f)); + dgFloat32 areaInv = faceNormal.DotProduct(p10.CrossProduct(p20)).GetScalar(); + if (e2->m_next != e0) { + const dgEdge* edge = e2; + dgVector r1 (q2); + dgVector q10 (p20); + do { + dgVector r2 (matrix.UntransformVector(dgVector(convexMesh.m_points.m_vertex[edge->m_next->m_incidentVertex]))); + dgVector q20 (r2 - q0); + dgFloat32 areaInv1 = faceNormal.DotProduct(q10.CrossProduct(q20)).GetScalar(); + if (areaInv1 > areaInv) { + e1 = edge; + e2 = edge->m_next; + q1 = r1; + q2 = r2; + areaInv = areaInv1; + } + r1 = r2; + q10 = q20; + edge = edge->m_next; + } while (edge->m_next != e0); + } + + dgAssert (areaInv > dgFloat32 (0.0f)); + areaInv = dgFloat32 (1.0f) / areaInv; + + dgVector uv0[3]; + dgVector uv1[3]; + memset(uv0, 0, sizeof(uv0)); + memset(uv1, 0, sizeof(uv1)); + if (m_attrib.m_uv0Channel.m_count && convexMesh.m_attrib.m_uv0Channel.m_count) { + uv0[0] = dgVector (dgFloat32(convexMesh.m_attrib.m_uv0Channel[dgInt32 (e0->m_userData)].m_u), dgFloat32(convexMesh.m_attrib.m_uv0Channel[dgInt32(e0->m_userData)].m_v), dgFloat32(0.0f), dgFloat32(0.0f)); + uv0[1] = dgVector(dgFloat32(convexMesh.m_attrib.m_uv0Channel[dgInt32 (e1->m_userData)].m_u), dgFloat32(convexMesh.m_attrib.m_uv0Channel[dgInt32(e1->m_userData)].m_v), dgFloat32(0.0f), dgFloat32(0.0f)); + uv0[2] = dgVector(dgFloat32(convexMesh.m_attrib.m_uv0Channel[dgInt32 (e2->m_userData)].m_u), dgFloat32(convexMesh.m_attrib.m_uv0Channel[dgInt32(e2->m_userData)].m_v), dgFloat32(0.0f), dgFloat32(0.0f)); + } + + if (m_attrib.m_uv1Channel.m_count && convexMesh.m_attrib.m_uv1Channel.m_count) { + uv1[0] = dgVector(dgFloat32(convexMesh.m_attrib.m_uv1Channel[dgInt32(e0->m_userData)].m_u), dgFloat32(convexMesh.m_attrib.m_uv1Channel[dgInt32(e0->m_userData)].m_v), dgFloat32(0.0f), dgFloat32(0.0f)); + uv1[1] = dgVector(dgFloat32(convexMesh.m_attrib.m_uv1Channel[dgInt32(e1->m_userData)].m_u), dgFloat32(convexMesh.m_attrib.m_uv1Channel[dgInt32(e1->m_userData)].m_v), dgFloat32(0.0f), dgFloat32(0.0f)); + uv1[2] = dgVector(dgFloat32(convexMesh.m_attrib.m_uv1Channel[dgInt32(e2->m_userData)].m_u), dgFloat32(convexMesh.m_attrib.m_uv1Channel[dgInt32(e2->m_userData)].m_v), dgFloat32(0.0f), dgFloat32(0.0f)); + } + + for (iter.Begin(); iter; iter ++){ + dgEdge* const edge = &(*iter); + + dgFloat64 side0 = test[edge->m_prev->m_incidentVertex]; + dgFloat64 side1 = test[edge->m_incidentVertex]; + + if ((side0 < dgFloat32 (0.0f)) && (side1 > dgFloat64 (0.0f))) { + dgBigVector dp (m_points.m_vertex[edge->m_incidentVertex] - m_points.m_vertex[edge->m_prev->m_incidentVertex]); + dgAssert(dp.m_w == dgFloat32(0.0f)); + dgFloat64 param = - side0 / plane.DotProduct(dp).GetScalar(); + + dgEdge* const splitEdge = InsertEdgeVertex (edge->m_prev, param); + test[splitEdge->m_next->m_incidentVertex] = dgFloat64 (0.0f); + } + } + + dgInt32 colorMark = IncLRU(); + for (iter.Begin(); iter; iter ++) { + dgEdge* const edge = &(*iter); + dgFloat64 side0 = test[edge->m_incidentVertex]; + dgFloat64 side1 = test[edge->m_next->m_incidentVertex]; + + if ((side0 > dgFloat32 (0.0f)) || (side1 > dgFloat64 (0.0f))) { + edge->m_mark = colorMark; + } + } + + for (iter.Begin(); iter; iter ++) { + dgEdge* const edge = &(*iter); + dgFloat64 side0 = test[edge->m_incidentVertex]; + dgFloat64 side1 = test[edge->m_next->m_incidentVertex]; + if ((side0 == dgFloat32 (0.0f)) && (side1 == dgFloat64 (0.0f))) { + dgEdge* ptr = edge->m_next; + do { + if (ptr->m_mark == colorMark) { + edge->m_mark = colorMark; + break; + } + ptr = ptr->m_next; + } while (ptr != edge); + } + } + + + for (iter.Begin(); iter; iter ++) { + dgEdge* const edge = &(*iter); + if ((edge->m_mark == colorMark) && (edge->m_next->m_mark < colorMark)) { + dgEdge* const startEdge = edge->m_next; + dgEdge* end = startEdge; + do { + if (end->m_mark == colorMark) { + break; + } + + end = end->m_next; + } while (end != startEdge); + dgAssert (end != startEdge); + dgEdge* const devideEdge = ConnectVertex (startEdge, end); + dgAssert (devideEdge); + dgAssert (devideEdge->m_next->m_mark != colorMark); + dgAssert (devideEdge->m_prev->m_mark != colorMark); + dgAssert (devideEdge->m_twin->m_next->m_mark == colorMark); + dgAssert (devideEdge->m_twin->m_prev->m_mark == colorMark); + devideEdge->m_mark = colorMark - 1; + devideEdge->m_twin->m_mark = colorMark; + } + } + + dgInt32 mark = IncLRU(); + dgList faceList (GetAllocator()); + for (iter.Begin(); iter; iter ++){ + dgEdge* const face = &(*iter); + if ((face->m_mark >= colorMark) && (face->m_mark != mark)) { + faceList.Append(face); + dgEdge* edge = face; + do { + edge->m_mark = mark; + edge = edge->m_next; + } while (edge != face); + } + } + + for (dgList::dgListNode* node = faceList.GetFirst(); node; node = node->GetNext()) { + dgEdge* const face = node->GetInfo(); + DeleteFace(face); + } + + mark = IncLRU(); + faceList.RemoveAll(); + for (iter.Begin(); iter; iter ++){ + dgEdge* const face = &(*iter); + if ((face->m_mark != mark) && (face->m_incidentFace < 0)) { + faceList.Append(face); + dgEdge* edge = face; + do { + edge->m_mark = mark; + edge = edge->m_next; + } while (edge != face); + } + } + + + const dgInt32 capAttribute = convexMesh.m_attrib.m_materialChannel.m_count ? convexMesh.m_attrib.m_materialChannel[dgInt32 (convexFace->m_userData)] : 0; + for (dgList::dgListNode* node = faceList.GetFirst(); node; node = node->GetNext()) { + dgEdge* const face = node->GetInfo(); + + dgEdge* edge = face; + do { + edge->m_incidentFace = 1; + edge->m_userData = m_attrib.m_pointChannel.m_count; + + m_attrib.m_pointChannel.PushBack(edge->m_incidentVertex); + if (m_attrib.m_normalChannel.m_count) { + dgTriplex n; + n.m_x = dgFloat32(normal.m_x); + n.m_y = dgFloat32(normal.m_y); + n.m_z = dgFloat32(normal.m_z); + m_attrib.m_normalChannel.PushBack(n); + } + + if (m_attrib.m_binormalChannel.m_count) { + dgAssert(0); + } + + if (m_attrib.m_colorChannel.m_count) { + dgAssert(0); + } + + if (m_attrib.m_materialChannel.m_count) { + m_attrib.m_materialChannel.PushBack(capAttribute); + } + + //dgVector p (matrix.UntransformVector (attibute.m_vertex)); + dgVector p (matrix.UntransformVector(m_points.m_vertex[edge->m_incidentVertex])); + dgVector p_p0 (p - q0); + dgVector p_p1 (p - q1); + dgVector p_p2 (p - q2); + dgAssert(faceNormal.m_w == dgFloat32 (0.0f)); + dgFloat32 alpha0 = faceNormal.DotProduct(p_p1.CrossProduct(p_p2)).GetScalar() * areaInv; + dgFloat32 alpha1 = faceNormal.DotProduct(p_p2.CrossProduct(p_p0)).GetScalar() * areaInv; + dgFloat32 alpha2 = faceNormal.DotProduct(p_p0.CrossProduct(p_p1)).GetScalar() * areaInv; + + //alpha0 = 0.0f; + //alpha1 = 0.0f; + //alpha2 = 0.0;; + if (m_attrib.m_uv0Channel.m_count && convexMesh.m_attrib.m_uv0Channel.m_count) { + dgAttibutFormat::dgUV uv; + uv.m_u = uv0[0].m_x * alpha0 + uv0[1].m_x * alpha1 + uv0[2].m_x * alpha2; + uv.m_v = uv0[0].m_y * alpha0 + uv0[1].m_y * alpha1 + uv0[2].m_y * alpha2; + m_attrib.m_uv0Channel.PushBack(uv); + } + + if (m_attrib.m_uv1Channel.m_count && convexMesh.m_attrib.m_uv1Channel.m_count) { + dgAttibutFormat::dgUV uv; + uv.m_u = uv1[0].m_x * alpha0 + uv1[1].m_x * alpha1 + uv1[2].m_x * alpha2; + uv.m_v = uv1[0].m_y * alpha0 + uv1[1].m_y * alpha1 + uv1[2].m_y * alpha2; + m_attrib.m_uv1Channel.PushBack(uv); + } + + edge = edge->m_next; + } while (edge != face); + } + } + + return true; +} + + +dgMeshEffect* dgMeshEffect::ConvexMeshIntersection (const dgMeshEffect* const convexMeshSrc) const +{ + dgMeshEffect convexMesh (*convexMeshSrc); + convexMesh.ConvertToPolygons(); + dgMeshEffect* const convexIntersection = new (GetAllocator()) dgMeshEffect (*this); + + dgInt32 mark = convexMesh.IncLRU(); + dgPolyhedra::Iterator iter (convexMesh); + + for (iter.Begin(); iter; iter ++){ + dgEdge* const convexFace = &(*iter); + if ((convexFace->m_incidentFace > 0) && (convexFace->m_mark != mark)) { + dgEdge* ptr = convexFace; + do { + ptr->m_mark = mark; + ptr = ptr->m_next; + } while (ptr != convexFace); + if (!convexIntersection->PlaneClip(convexMesh, convexFace)) { + delete convexIntersection; + return NULL; + } + } + } + + if (!convexIntersection->GetVertexCount()) { + delete convexIntersection; + return NULL; + } + convexIntersection->RemoveUnusedVertices(NULL); + return convexIntersection; +} + + +void dgMeshEffect::ClipMesh (const dgMatrix& matrix, const dgMeshEffect* const clipMesh, dgMeshEffect** const back, dgMeshEffect** const front) const +{ + dgAssert (0); +/* + dgMeshEffect clipper (*clipMesh); + clipper.TransformMesh (matrix); + + dgMeshEffect* backMeshSource = NULL; + dgMeshEffect* frontMeshSource = NULL; + dgMeshEffect* backMeshClipper = NULL; + dgMeshEffect* frontMeshClipper = NULL; + + ClipMesh (&clipper, &backMeshSource, &frontMeshSource); + if (backMeshSource && frontMeshSource) { + clipper.ClipMesh (this, &backMeshClipper, &frontMeshClipper); + if (backMeshSource && frontMeshSource) { + + dgMeshEffect* backMesh; + dgMeshEffect* frontMesh; + + backMesh = new (GetAllocator()) dgMeshEffect (GetAllocator(), true); + frontMesh = new (GetAllocator()) dgMeshEffect (GetAllocator(), true); + + backMesh->BeginPolygon(); + frontMesh->BeginPolygon(); + + backMesh->MergeFaces(backMeshSource); + backMesh->MergeFaces(backMeshClipper); + + frontMesh->MergeFaces(frontMeshSource); + frontMesh->ReverseMergeFaces(backMeshClipper); + + backMesh->EndPolygon(dgFloat64 (1.0e-5f)); + frontMesh->EndPolygon(dgFloat64 (1.0e-5f)); + + *back = backMesh; + *front = frontMesh; + } + } + + if (backMeshClipper) { + delete backMeshClipper; + } + + if (frontMeshClipper) { + delete frontMeshClipper; + } + + if (backMeshSource) { + delete backMeshSource; + } + + if (frontMeshSource) { + delete frontMeshSource; + } +*/ +} + + + + +dgMeshEffect* dgMeshEffect::Union (const dgMatrix& matrix, const dgMeshEffect* const clipperMesh) const +{ + dgAssert (0); + return NULL; +/* + dgMeshEffect copy (*this); + dgMeshEffect clipper (*clipperMesh); + clipper.TransformMesh (matrix); + + dgBooleanMeshClipper::ClipMeshesAndColorize (©, &clipper); + + dgMeshEffect* const mesh = new (GetAllocator()) dgMeshEffect (GetAllocator()); + mesh->BeginFace(); + dgBooleanMeshClipper::CopyPoints(mesh, ©); + dgBooleanMeshClipper::AddExteriorFaces (mesh, ©); + + dgBooleanMeshClipper::AddExteriorFaces (mesh, &clipper); + mesh->EndFace (); + mesh->RepairTJoints(); + mesh->RemoveUnusedVertices(NULL); + return mesh; +*/ +} + +dgMeshEffect* dgMeshEffect::Difference (const dgMatrix& matrix, const dgMeshEffect* const clipperMesh) const +{ +/* + dgMeshEffect copy (*this); + dgMeshEffect clipper (*clipperMesh); + clipper.TransformMesh (matrix); + + dgBooleanMeshClipper::ClipMeshesAndColorize (©, &clipper); + + dgMeshEffect* const mesh = new (GetAllocator()) dgMeshEffect (GetAllocator()); + mesh->BeginFace(); + dgBooleanMeshClipper::CopyPoints(mesh, ©); + dgBooleanMeshClipper::AddExteriorFaces (mesh, ©); + dgBooleanMeshClipper::AddInteriorFacesInvertWinding (mesh, &clipper); + mesh->EndFace (); + mesh->RepairTJoints(); + mesh->RemoveUnusedVertices(NULL); + return mesh; +*/ + + dgAssert (0); + return NULL; +} + + +class dgBooleanMeshClipper: public dgMeshEffect::dgMeshBVH +{ + class dgPoint: public dgBigVector + { + public: + dgPoint() + :dgBigVector(dgFloat32 (0.0f)) + ,m_links(NULL) + ,m_lru(0) + { + dgAssert (0); + } + + dgPoint(const dgBigVector& point, dgMemoryAllocator* const allocator) + :dgBigVector(point) + ,m_links(allocator) + ,m_lru(0) + { + } + + dgList::dgTreeNode*> m_links; + dgInt32 m_lru; + }; + + class dgCurvesNetwork: public dgTree + { + public: + dgCurvesNetwork () + :dgTree(NULL) + { + dgAssert (0); + } + + dgCurvesNetwork (dgMemoryAllocator* const allocator) + :dgTree(allocator) + { + } + + dgTreeNode* AddVertex(const dgBigVector& point, dgMemoryAllocator* const allocator) + { + dgFloat64 key = ((point.m_z * dgFloat64 (1024.0f) + point.m_y) * dgFloat64 (1024.0f)) + point.m_x; + dgTreeNode* node = Find(key); + if (!node) { + dgPoint entry (point, allocator); + node = Insert(entry, key); + } + return node; + } + +/* + dgCurvesNetwork(dgBooleanMeshClipper* const BVHmeshA, dgBooleanMeshClipper* const BVHmeshB) + :dgTree(BVHmeshA->m_mesh->GetAllocator()) +// ,m_meshA(BVHmeshA->m_mesh) +// ,m_meshB(BVHmeshB->m_mesh) +// ,m_pointBaseA(m_meshA->GetVertexCount()) +// ,m_pointBaseB(m_meshB->GetVertexCount()) +// ,m_lru(0) + { + } +*/ +/* + dgHugeVector CalculateFaceNormal (const dgMeshEffect* const mesh, dgEdge* const face) + { + dgHugeVector plane(dgGoogol::m_zero, dgGoogol::m_zero, dgGoogol::m_zero, dgGoogol::m_zero); + dgEdge* edge = face; + dgHugeVector p0(mesh->GetVertex(edge->m_incidentVertex)); + edge = edge->m_next; + dgHugeVector p1(mesh->GetVertex(edge->m_incidentVertex)); + dgHugeVector p1p0(p1 - p0); + edge = edge->m_next; + do { + dgHugeVector p2(mesh->GetVertex(edge->m_incidentVertex)); + dgHugeVector p2p0(p2 - p0); + plane += p2p0 * p1p0; + p1p0 = p2p0; + edge = edge->m_next; + } while (edge != face); + + plane.m_w = dgGoogol::m_zero - (plane % p0); + return plane; + } + + + bool IsPointInFace (const dgHugeVector& point, const dgMeshEffect* const mesh, dgEdge* const face, const dgHugeVector& normal) const + { + dgEdge* edge = face; + + dgTrace (("%f %f %f\n", dgFloat64 (point.m_x), dgFloat64 (point.m_y), dgFloat64 (point.m_z))); + do { + dgBigVector p1(mesh->GetVertex(edge->m_incidentVertex)); + dgTrace (("%f %f %f\n", dgFloat64 (p1.m_x), dgFloat64 (p1.m_y), dgFloat64 (p1.m_z))); + edge = edge->m_next; + } while (edge != face); + + dgHugeVector p0(mesh->GetVertex(face->m_incidentVertex)); + do { + dgHugeVector p1(mesh->GetVertex(edge->m_twin->m_incidentVertex)); + dgHugeVector p1p0(p1 - p0); + dgHugeVector q1p0(point - p0); + dgGoogol side (q1p0 % (normal * p1p0)); + if (side >= dgGoogol::m_zero) { + return false; + } + p0 = p1; + edge = edge->m_next; + } while (edge != face); + + return true; + } + + dgFloat64 ClipEdgeFace(const dgMeshEffect* const meshEdge, dgEdge* const edge, const dgMeshEffect* const meshFace, dgEdge* const face, const dgHugeVector& plane) + { + dgHugeVector p0 (meshEdge->GetVertex(edge->m_incidentVertex)); + dgHugeVector p1 (meshEdge->GetVertex(edge->m_twin->m_incidentVertex)); + + dgGoogol test0 (plane.EvaluePlane(p0)); + dgGoogol test1 (plane.EvaluePlane(p1)); + + if ((test0 * test1) > dgGoogol::m_zero) { + // both point are in one side + return -1.0f; + } + + if ((test0 * test1) < dgGoogol::m_zero) { + //point on different size, clip the line + dgHugeVector p1p0 (p1 - p0); + dgGoogol param = dgGoogol::m_zero - plane.EvaluePlane(p0) / (plane % p1p0); + dgHugeVector p (p0 + p1p0.Scale (param)); + if (IsPointInFace (p, meshFace, face, plane)) { + return param; + } + return -1.0f; + } else { + dgAssert (0); + //special cases; + } + + return -1.0f; + } + + void AddPoint (dgMeshEffect* const edgeOwnerMesh, dgEdge* const edgeStart, dgMeshEffect* const faceOwnerMesh, dgEdge* const face, const dgHugeVector& plane, dgTreeNode** nodes, dgInt32& index) + { + dgEdge* edge = edgeStart; + do { + dgFloat64 param = ClipEdgeFace(edgeOwnerMesh, edge, faceOwnerMesh, face, plane); + if (param > 0.0f) { + dgPoint point(edgeOwnerMesh, edge, param, faceOwnerMesh, face); + dgTreeNode* node = Find(dgNodeKey(edge, param)); + if (!node) { + node = Insert(point, dgNodeKey(edge, param)); + } + nodes[index] = node; + index ++; + } + edge = edge->m_next; + } while (edge != edgeStart); + + } + + void ClipMeshesFaces(dgEdge* const faceA, dgEdge* const faceB) + { + dgAssert (m_meshA->FindEdge(faceA->m_incidentVertex, faceA->m_twin->m_incidentVertex) == faceA); + dgAssert (m_meshB->FindEdge(faceB->m_incidentVertex, faceB->m_twin->m_incidentVertex) == faceB); + + dgHugeVector planeA (CalculateFaceNormal (m_meshA, faceA)); + dgHugeVector planeB (CalculateFaceNormal (m_meshB, faceB)); + + dgInt32 index = 0; + dgTreeNode* nodes[16]; + AddPoint (m_meshA, faceA, m_meshB, faceB, planeB, nodes, index); + AddPoint (m_meshB, faceB, m_meshA, faceA, planeA, nodes, index); + dgAssert ((index == 0) || (index == 2)); + if (index == 2) { + dgPoint& pointA = nodes[0]->GetInfo(); + dgPoint& pointB = nodes[1]->GetInfo(); + pointA.m_links.Append(nodes[1]); + pointB.m_links.Append(nodes[0]); + } + } + + void GetCurve (dgList& curve, dgTreeNode* const node) + { + dgInt32 stack = 1; + dgTreeNode* pool[64]; + + pool[0] = node; + while (stack) { + stack --; + dgTreeNode* const ptr = pool[stack]; + dgPoint& point = ptr->GetInfo(); + if (point.m_lru != m_lru) { + point.m_lru = m_lru; + curve.Append(ptr); + for (dgList::dgTreeNode*>::dgListNode* ptrPoint = point.m_links.GetFirst(); ptrPoint; ptrPoint = ptrPoint->GetNext()) { + dgTreeNode* const nextnode = ptrPoint->GetInfo(); + dgPoint& nextPoint = nextnode->GetInfo(); + if (nextPoint.m_lru != m_lru) { + pool[stack] = nextnode; + stack ++; + } + } + } + } + } + + void EmbedCurveToSingleFace (dgList& curve, dgMeshEffect* const mesh) + { + dgEdge* const face = curve.GetFirst()->GetInfo()->GetInfo().m_face; + + dgInt32 indexBase = mesh->GetVertexCount(); + dgInt32 indexAttribBase = mesh->GetPropertiesCount(); + + for (dgList::dgListNode* node = curve.GetFirst(); node; node = node->GetNext()) { + dgPoint& point = node->GetInfo()->GetInfo(); + dgAssert (point.m_face == face); + dgMeshEffect::dgVertexAtribute attribute(mesh->InterpolateVertex(point.m_posit, face)); + mesh->AddVertex(point.m_posit); + mesh->AddAtribute(attribute); + } + + dgList list(GetAllocator()); + dgInt32 i0 = curve.GetCount() - 1; + for (dgInt32 i = 0; i < curve.GetCount(); i++) { + dgEdge* const edge = mesh->AddHalfEdge(indexBase + i0, indexBase + i); + dgEdge* const twin = mesh->AddHalfEdge(indexBase + i, indexBase + i0); + + edge->m_incidentFace = 1; + twin->m_incidentFace = 1; + edge->m_userData = indexAttribBase + i0; + twin->m_userData = indexAttribBase + i; + twin->m_twin = edge; + edge->m_twin = twin; + i0 = i; + list.Append(edge); + } + + dgEdge* closestEdge = NULL; + dgFloat64 dist2 = dgFloat64 (1.0e10f); + dgBigVector p(mesh->GetVertex(face->m_incidentVertex)); + + list.Append(list.GetFirst()->GetInfo()); + list.Addtop(list.GetLast()->GetInfo()); + for (dgList::dgListNode* node = list.GetFirst()->GetNext(); node != list.GetLast(); node = node->GetNext()) { + dgEdge* const edge = node->GetInfo(); + + dgEdge* const prev = node->GetPrev()->GetInfo(); + edge->m_prev = prev; + prev->m_next = edge; + edge->m_twin->m_next = prev->m_twin; + prev->m_twin->m_prev = edge->m_twin; + + dgEdge* const next = node->GetNext()->GetInfo(); + edge->m_next = next; + next->m_prev = edge; + edge->m_twin->m_prev = next->m_twin; + next->m_twin->m_next = edge->m_twin; + + dgBigVector dist(mesh->GetVertex(edge->m_incidentVertex) - p); + dgFloat64 err2 = dist % dist; + if (err2 < dist2) { + closestEdge = edge; + dist2 = err2; + } + } + + dgBigVector faceNormal (mesh->FaceNormal(face, mesh->GetVertexPool(), mesh->GetVertexStrideInByte())); + dgBigVector clipNormal (mesh->FaceNormal(closestEdge, mesh->GetVertexPool(), mesh->GetVertexStrideInByte())); + if ((clipNormal % faceNormal) > dgFloat64(0.0f)) { + closestEdge = closestEdge->m_twin->m_next; + } + dgEdge* const glueEdge = mesh->ConnectVertex (closestEdge, face); + dgAssert (glueEdge); + mesh->PolygonizeFace(glueEdge, mesh->GetVertexPool(), sizeof (dgBigVector)); + } + + void EmbedCurveToMulipleFaces (dgList& curve, dgMeshEffect* const mesh) + { + for (dgList::dgListNode* node = curve.GetFirst(); node; node = node->GetNext()) { + dgPoint& point = node->GetInfo()->GetInfo(); + if (point.m_edgeOwnerMesh == mesh) { + dgEdge* const edge = point.m_edge; + dgBigVector p0 (mesh->GetVertex(edge->m_incidentVertex)); + dgBigVector p1 (mesh->GetVertex(edge->m_twin->m_incidentVertex)); + dgVector p1p0 (p1 - p0); + dgVector qp0 (point.m_posit - p0); + dgFloat64 param = (qp0 % p1p0) / (p1p0 % p1p0); + dgAssert (param >= dgFloat64 (0.0f)); + dgAssert (param <= dgFloat64 (1.0f)); + dgEdge* const newEdge = mesh->InsertEdgeVertex (edge, param); + } +// mesh->AddVertex(point.m_posit); +// mesh->AddAtribute(attribute); + } + } + + + void AddCurveToMesh (dgList& curve, dgMeshEffect* const mesh) + { + bool isIscribedInFace = true; + dgEdge* const face = curve.GetFirst()->GetInfo()->GetInfo().m_face; + for (dgList::dgListNode* node = curve.GetFirst(); isIscribedInFace && node; node = node->GetNext()) { + dgPoint& point = node->GetInfo()->GetInfo(); + isIscribedInFace = isIscribedInFace && (point.m_face == face); + isIscribedInFace = isIscribedInFace && (point.m_faceOwnerMesh == mesh); + } + + if (isIscribedInFace) { + EmbedCurveToSingleFace (curve, mesh); + } else { + EmbedCurveToMulipleFaces (curve, mesh); + } + } + + void Colorize() + { + m_lru ++; + Iterator iter (*this); + for (iter.Begin(); iter; iter ++) { + dgPoint& point = iter.GetNode()->GetInfo(); + if (point.m_lru != m_lru) { + dgList curve (GetAllocator()); + GetCurve (curve, iter.GetNode()); + AddCurveToMesh (curve, m_meshB); + AddCurveToMesh (curve, m_meshA); + } + } + + m_meshA->SaveOFF("xxxA0.off"); + m_meshB->SaveOFF("xxxB0.off"); + } + + dgMeshEffect* m_meshA; + dgMeshEffect* m_meshB; + dgInt32 m_pointBaseA; + dgInt32 m_pointBaseB; + dgInt32 m_lru; +*/ + }; + + class dgClippedFace: public dgMeshEffect + { + public: + dgClippedFace () + :dgMeshEffect() + ,m_curveNetwork() + { + dgAssert (0); + } + + dgClippedFace (dgMemoryAllocator* const allocator) + :dgMeshEffect(allocator) + ,m_curveNetwork(allocator) + { + } + + dgClippedFace (const dgClippedFace& copy) + :dgMeshEffect(copy) + ,m_curveNetwork(copy.m_curveNetwork) + { + } + + void InitFace(dgMeshEffect* const mesh, dgEdge* const face) + { + dgInt32 indexCount = 0; + dgInt32 faceIndex[256]; + dgInt64 faceDataIndex[256]; + BeginFace (); + dgEdge* ptr = face; + do { + dgAssert (0); + //const dgMeshEffect::dgVertexAtribute& point = mesh->GetAttribute(dgInt32 (ptr->m_userData)); + //AddPoint (&point.m_vertex.m_x, dgInt32 (point.m_material)); + faceIndex[indexCount] = indexCount; + faceDataIndex[indexCount] = indexCount; + indexCount ++; + ptr = ptr->m_next; + } while (ptr != face); + AddFace (indexCount, faceIndex, faceDataIndex); + EndFace (); + } + + + void AddSegment (const dgBigVector& plane, const dgBigVector* const segment) + { + dgAssert (0); +/* + dgCurvesNetwork::dgTreeNode* const node0 = m_curveNetwork.AddVertex (segment[0], GetAllocator()); + dgCurvesNetwork::dgTreeNode* const node1 = m_curveNetwork.AddVertex (segment[1], GetAllocator()); + + dgPoint& pointA = node0->GetInfo(); + dgPoint& pointB = node1->GetInfo(); + pointA.m_links.Append(node1); + pointB.m_links.Append(node0); +*/ + } + + dgCurvesNetwork m_curveNetwork; + }; + + class dgClipppedFaces: public dgTree + { + public: + dgClipppedFaces(dgMeshEffect* const mesh) + :dgTree(mesh->GetAllocator()) + ,m_parentMesh (mesh) + { + } + + void ClipMeshesFaces(dgEdge* const faceA, const dgMeshEffect* const meshB, dgEdge* const faceB, const dgBigVector& planeB, const dgBigVector* const segment) + { + dgTreeNode* node = Find (faceA); + if (!node) { + dgClippedFace tmp (m_parentMesh->GetAllocator()); + node = Insert (tmp, faceA); + dgClippedFace& faceHead = node->GetInfo(); + faceHead.InitFace (m_parentMesh, faceA); + } + dgAssert (node); + dgClippedFace& faceHead = node->GetInfo(); + faceHead.AddSegment(planeB, segment); + } + + dgMeshEffect* m_parentMesh; + }; + + + public: + dgBooleanMeshClipper(dgMeshEffect* const mesh) + :dgMeshBVH(mesh) + ,m_clippedFaces(mesh) + { + dgMeshBVH::Build(); + } + + ~dgBooleanMeshClipper() + { + } + +/* + dgFloat64 IntersetionSegment(const dgMeshEffect* const meshEdge, dgEdge* const edge, const dgMeshEffect* const meshFace, dgEdge* const face, const dgHugeVector& plane) + { + dgHugeVector p0 (meshEdge->GetVertex(edge->m_incidentVertex)); + dgHugeVector p1 (meshEdge->GetVertex(edge->m_twin->m_incidentVertex)); + + dgGoogol test0 (plane.EvaluePlane(p0)); + dgGoogol test1 (plane.EvaluePlane(p1)); + + if ((test0 * test1) > dgGoogol::m_zero) { + // both point are in one side + return -1.0f; + } + + if ((test0 * test1) < dgGoogol::m_zero) { + //point on different size, clip the line + dgHugeVector p1p0 (p1 - p0); + dgGoogol param = dgGoogol::m_zero - plane.EvaluePlane(p0) / (plane % p1p0); + dgHugeVector p (p0 + p1p0.Scale (param)); + if (IsPointInFace (p, meshFace, face, plane)) { + return param; + } + return -1.0f; + } else { + dgAssert (0); + //special cases; + } + + return -1.0f; + } +*/ + + static dgHugeVector CalculateFaceNormal (const dgMeshEffect* const mesh, dgEdge* const face) + { + dgHugeVector plane(dgGoogol::m_zero, dgGoogol::m_zero, dgGoogol::m_zero, dgGoogol::m_zero); + dgEdge* edge = face; + dgHugeVector p0(mesh->GetVertex(edge->m_incidentVertex)); + edge = edge->m_next; + dgHugeVector p1(mesh->GetVertex(edge->m_incidentVertex)); + dgHugeVector p1p0(p1 - p0); + edge = edge->m_next; + do { + dgHugeVector p2(mesh->GetVertex(edge->m_incidentVertex)); + dgHugeVector p2p0(p2 - p0); + plane += p1p0.CrossProduct(p2p0); + p1p0 = p2p0; + edge = edge->m_next; + } while (edge != face); + + dgAssert(plane.m_w == dgGoogol(0.0)); + plane.m_w = dgGoogol::m_zero - plane.DotProduct(p0).GetScalar(); + return plane; + } + + static bool IsPointInFace (const dgHugeVector& point, const dgMeshEffect* const mesh, dgEdge* const face, const dgHugeVector& normal) + { + dgEdge* edge = face; + dgHugeVector p0(mesh->GetVertex(face->m_incidentVertex)); + do { + dgHugeVector p1(mesh->GetVertex(edge->m_twin->m_incidentVertex)); + dgHugeVector p1p0(p1 - p0); + dgHugeVector q1p0(point - p0); + dgAssert(p1p0.m_w == dgGoogol(0.0)); + dgGoogol side (q1p0.DotProduct(p1p0.CrossProduct(normal)).GetScalar()); + if (side >= dgGoogol::m_zero) { + return false; + } + p0 = p1; + edge = edge->m_next; + } while (edge != face); + + return true; + } + + static bool ClipEdgeFace(dgBigVector& point, const dgMeshEffect* const meshEdge, dgEdge* const edgeSrc, const dgMeshEffect* const meshFace, dgEdge* const face, const dgHugeVector& plane) + { + const dgEdge* const edge = (edgeSrc->m_incidentVertex < edgeSrc->m_twin->m_incidentVertex) ? edgeSrc : edgeSrc->m_twin; + dgHugeVector p0 (meshEdge->GetVertex(edge->m_incidentVertex)); + dgHugeVector p1 (meshEdge->GetVertex(edge->m_twin->m_incidentVertex)); + + dgGoogol test0 (plane.EvaluePlane(p0)); + dgGoogol test1 (plane.EvaluePlane(p1)); + + if ((test0 * test1) > dgGoogol::m_zero) { + // both point are in one side + return false; + } + + if ((test0 * test1) < dgGoogol::m_zero) { + //point on different size, clip the line + dgHugeVector p1p0 (p1 - p0); + dgAssert(p1p0.m_w == dgGoogol(0.0)); + dgGoogol param = dgGoogol::m_zero - plane.EvaluePlane(p0) / plane.DotProduct(p1p0).GetScalar(); + dgHugeVector p (p0 + p1p0.Scale (param)); + if (IsPointInFace (p, meshFace, face, plane)) { + point = dgBigVector(p.m_x, p.m_y, p.m_z, p.m_w); + return true; + } + return false; + } else { + dgAssert (0); + //special cases; + } + + return false; + } + + static void CalculateIntersection (const dgMeshEffect* const edgeOwnerMesh, dgEdge* const edgeStart, const dgMeshEffect* const faceOwnerMesh, dgEdge* const face, const dgHugeVector& facePlane, dgBigVector* const data, dgInt32& index) + { + dgEdge* edge = edgeStart; + do { + bool isCleipped = ClipEdgeFace(data[index], edgeOwnerMesh, edge, faceOwnerMesh, face, facePlane); + if (isCleipped) { + index ++; + } + edge = edge->m_next; + } while (edge != edgeStart); + } + + static void ClipMeshesFaces(dgBooleanMeshClipper& bvhMeshA, dgEdge* const faceA, dgBooleanMeshClipper& bvhMeshB, dgEdge* const faceB) + { + const dgMeshEffect* const meshA = bvhMeshA.m_mesh; + const dgMeshEffect* const meshB = bvhMeshB.m_mesh; + dgAssert (meshA->FindEdge(faceA->m_incidentVertex, faceA->m_twin->m_incidentVertex) == faceA); + dgAssert (meshB->FindEdge(faceB->m_incidentVertex, faceB->m_twin->m_incidentVertex) == faceB); + + dgHugeVector planeA (CalculateFaceNormal (meshA, faceA)); + dgHugeVector planeB (CalculateFaceNormal (meshB, faceB)); + + dgBigVector points[16]; + dgInt32 pointCount = 0; + CalculateIntersection (meshA, faceA, meshB, faceB, planeB, points, pointCount); + CalculateIntersection (meshB, faceB, meshA, faceA, planeA, points, pointCount); + dgAssert ((pointCount == 0) || (pointCount == 2)); + if (pointCount == 2) { + dgBigVector facePlaneA (planeA.m_x, planeA.m_y, planeA.m_z, planeA.m_w); + dgBigVector facePlaneB (planeB.m_x, planeB.m_y, planeB.m_z, planeB.m_w); + + bvhMeshA.m_clippedFaces.ClipMeshesFaces(faceA, meshB, faceB, facePlaneB, points); + bvhMeshB.m_clippedFaces.ClipMeshesFaces(faceB, meshA, faceA, facePlaneA, points); + } + } + + static void ClipMeshesAndColorize(dgMeshEffect* const meshA, dgMeshEffect* const meshB) + { + dgAssert (0); +/* + dgBooleanMeshClipper BVHmeshA(meshA); + dgBooleanMeshClipper BVHmeshB(meshB); + + int stack = 1; + + dgMeshBVHNode* stackPool[2 * DG_MESH_EFFECT_BVH_STACK_DEPTH][2]; + + stackPool[0][0] = BVHmeshA.m_rootNode; + stackPool[0][1] = BVHmeshB.m_rootNode; + while (stack) { + stack --; + dgMeshBVHNode* const nodeA = stackPool[stack][0]; + dgMeshBVHNode* const nodeB = stackPool[stack][1]; + if (dgOverlapTest (nodeA->m_p0, nodeA->m_p1, nodeB->m_p0, nodeB->m_p1)) { + if (nodeA->m_face && nodeB->m_face) { + ClipMeshesFaces(BVHmeshA, nodeA->m_face, BVHmeshB, nodeB->m_face); + } else if (nodeA->m_face) { + stackPool[stack][0] = nodeA; + stackPool[stack][1] = nodeB->m_left; + stack++; + dgAssert(stack < sizeof (stackPool) / sizeof (stackPool[0])); + + stackPool[stack][0] = nodeA; + stackPool[stack][1] = nodeB->m_right; + stack++; + dgAssert(stack < sizeof (stackPool) / sizeof (stackPool[0])); + + } else if (nodeB->m_face) { + stackPool[stack][0] = nodeA->m_left; + stackPool[stack][1] = nodeB; + stack++; + dgAssert(stack < sizeof (stackPool) / sizeof (stackPool[0])); + + stackPool[stack][0] = nodeA->m_right; + stackPool[stack][1] = nodeB; + stack++; + dgAssert(stack < sizeof (stackPool) / sizeof (stackPool[0])); + + } else { + stackPool[stack][0] = nodeA->m_left; + stackPool[stack][1] = nodeB->m_left; + stack ++; + dgAssert (stack < sizeof (stackPool) / sizeof (stackPool[0])); + + stackPool[stack][0] = nodeA->m_left; + stackPool[stack][1] = nodeB->m_right; + stack++; + dgAssert(stack < sizeof (stackPool) / sizeof (stackPool[0])); + + stackPool[stack][0] = nodeA->m_right; + stackPool[stack][1] = nodeB->m_left; + stack++; + dgAssert(stack < sizeof (stackPool) / sizeof (stackPool[0])); + + stackPool[stack][0] = nodeA->m_right; + stackPool[stack][1] = nodeB->m_right; + stack++; + dgAssert(stack < sizeof (stackPool) / sizeof (stackPool[0])); + } + } + } +*/ + dgAssert (0); +// network.Colorize(); + + +/* + dgInt32 baseAttibuteCountB = BVHmeshB.m_mesh->GetPropertiesCount(); + + BVHmeshA.m_mesh->SaveOFF("xxxA0.off"); + BVHmeshB.m_mesh->SaveOFF("xxxB0.off"); + + // edge-face, edge-edge and edge-vertex intersections until not more intersections are found + for (bool intersectionFound = true; intersectionFound;) { + intersectionFound = false; + + intersectionFound |= BVHmeshA.CalculateEdgeFacesIntersetions(BVHmeshB); + intersectionFound |= BVHmeshB.CalculateEdgeFacesIntersetions(BVHmeshA); + + intersectionFound |= BVHmeshA.CalculateVertexFacesIntersetions(BVHmeshB); + intersectionFound |= BVHmeshB.CalculateVertexFacesIntersetions(BVHmeshA); + + + BVHmeshA.m_mesh->SaveOFF("xxxA1.off"); + BVHmeshB.m_mesh->SaveOFF("xxxB1.off"); + + intersectionFound |= BVHmeshA.CalculateEdgeEdgeIntersetions(BVHmeshB); + + BVHmeshA.m_mesh->SaveOFF("xxxA2.off"); + BVHmeshB.m_mesh->SaveOFF("xxxB2.off"); + + intersectionFound |= BVHmeshA.CalculateEdgeVertexIntersetions(BVHmeshB); + intersectionFound |= BVHmeshB.CalculateEdgeVertexIntersetions(BVHmeshA); + + BVHmeshA.m_mesh->SaveOFF("xxxA3.off"); + BVHmeshB.m_mesh->SaveOFF("xxxB3.off"); + } +*/ + } + + dgClipppedFaces m_clippedFaces; +}; + + + +dgMeshEffect* dgMeshEffect::Intersection (const dgMatrix& matrix, const dgMeshEffect* const clipperMesh) const +{ + dgMeshEffect copy (*this); + dgMeshEffect clipper (*clipperMesh); + clipper.TransformMesh (matrix); + + dgBooleanMeshClipper::ClipMeshesAndColorize (©, &clipper); +/* + dgMeshEffect* const mesh = new (GetAllocator()) dgMeshEffect (GetAllocator()); + mesh->BeginFace(); + dgBooleanMeshClipper::CopyPoints(mesh, ©); + dgBooleanMeshClipper::AddInteriorFaces (mesh, ©); + dgBooleanMeshClipper::AddInteriorFaces (mesh, &clipper); + mesh->EndFace (); + mesh->RepairTJoints(); + mesh->RemoveUnusedVertices(NULL); + + return mesh; +*/ + + dgAssert (0); + return NULL; +} + + diff --git a/thirdparty/src/newton/dgMeshUtil/dgMeshEffect6.cpp b/thirdparty/src/newton/dgMeshUtil/dgMeshEffect6.cpp new file mode 100644 index 000000000..25aacadc5 --- /dev/null +++ b/thirdparty/src/newton/dgMeshUtil/dgMeshEffect6.cpp @@ -0,0 +1,1917 @@ +/* Copyright (c) <2003-2019> +* +* 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. +*/ + +// algorithm from paper: Non-Distorted Texture Mapping Using Angle Based Flattening +// by: A. Sheffer and E. de Sturler +// http://www.math.vt.edu/people/sturler/Publications/UIUCDCS-R-2001-2257.pdf +// +// also improvement from paper: ABF++: Fast and Robust Angle Based Flattening +// http://hal.archives-ouvertes.fr/docs/00/10/56/89/PDF/abf_plus_plus_temp.pdf +// by: Alla Sheffer, Bruno Lévy, Inria Lorraine, Maxim Mogilnitsky and Alexander Bogomyakov +// +// also looking at paper +// Least Squares Conformal Maps for Automatic Texture Atlas Generation +// http://www.cs.jhu.edu/~misha/Fall09/Levy02.pdf +// by Bruno Lévy Sylvain Petitjean Nicolas Ray Jérome Maillot +// for automatic seam and atlas generation + +#include "dgPhysicsStdafx.h" +#include "dgWorld.h" +#include "dgMeshEffect.h" + +#define dgABF_MAX_ITERATIONS 5 +#define dgABF_TOL2 dgFloat64 (1.0e-12) +#define dgABF_LINEAR_SOLVER_TOL dgFloat64 (1.0e-14) +#define dgABF_PI dgFloat64 (3.1415926535) + +#define dgABF_UV_TOL2 dgFloat64 (1.0e-8) + +#if 1 + #define DG_DEBUG_UV dgTrace +#else + #define DG_DEBUG_UV +#endif + + +class dgTriangleAnglesToUV: public dgSymmetricConjugateGradientSolver +{ + public: + dgTriangleAnglesToUV (dgMeshEffect* const mesh, dgInt32 material, dgReportProgress progressReportCallback, void* const userData, const dgFloat64* const pinnedPoint, dgFloat64* const triangleAnglesVector = NULL) + :m_hessianCoLumnIndex (mesh->GetAllocator()) + ,m_hessianCoLumnValue(mesh->GetAllocator()) + ,m_mesh(mesh) + ,m_triangleAngles(triangleAnglesVector) + ,m_pinnedPoints(pinnedPoint) + ,m_trianglesCount(0) + ,m_matrixElementCount(0) + ,m_allocated(false) + { + dgInt32 mark = m_mesh->IncLRU(); + dgMeshEffect::Iterator iter (*m_mesh); + for (iter.Begin(); iter; iter ++) { + dgEdge* const edge = &iter.GetNode()->GetInfo(); + if ((edge->m_incidentFace > 0) && (edge->m_mark != mark)) { + dgEdge *ptr = edge; + do { + ptr->m_mark = mark; + ptr = ptr->m_next; + } while (ptr != edge); + m_trianglesCount ++; + dgAssert (edge->m_next->m_next->m_next == edge); + } + } + + m_triangles = (dgEdge**) m_mesh->GetAllocator()->MallocLow (m_trianglesCount * sizeof (dgEdge*)); + + dgInt32 count = 0; + mark = m_mesh->IncLRU(); + for (iter.Begin(); iter; iter ++) { + dgEdge* const edge = &iter.GetNode()->GetInfo(); + if ((edge->m_incidentFace > 0) && (edge->m_mark != mark)) { + dgEdge *ptr = edge; + do { + ptr->m_incidentFace = count + 1; + ptr->m_mark = mark; + ptr = ptr->m_next; + } while (ptr != edge); + m_triangles[count] = ptr; + count ++; + dgAssert (count <= m_trianglesCount); + } + } + + if (!m_triangleAngles) { + dgAssert (0); + AnglesFromUV (); + } + + m_uvArray = (dgFloat64*) m_mesh->GetAllocator()->MallocLow (2 * m_mesh->GetVertexCount() * sizeof (dgFloat64)); + mark = m_mesh->IncLRU(); + for (iter.Begin(); iter; iter ++) { + dgEdge* const edge = &iter.GetNode()->GetInfo(); + if (edge->m_mark != mark) { + dgEdge* ptr = edge; + dgEdge* uvEdge = edge; + do { + if ((uvEdge->m_incidentFace < 0) && (ptr->m_incidentFace > 0)) { + uvEdge = ptr; + } + ptr->m_mark = mark; + ptr = ptr->m_twin->m_next; + } while (ptr != edge); + dgAssert (0); +/* + dgInt32 index = dgInt32 (uvEdge->m_userData); + dgMeshEffect::dgVertexAtribute& attribute = m_mesh->GetAttribute (index); + m_uvArray[index * 2 + 0] = attribute.m_u0; + m_uvArray[index * 2 + 1] = attribute.m_v0; +*/ + } + } + + m_sinTable = (dgFloat64*) m_mesh->GetAllocator()->MallocLow (3 * m_trianglesCount * sizeof (dgFloat64)); + m_cosTable = (dgFloat64*) m_mesh->GetAllocator()->MallocLow (3 * m_trianglesCount * sizeof (dgFloat64)); + + // pre-compute sin cos tables + for (dgInt32 i = 0; i < m_trianglesCount * 3; i ++) { + m_sinTable[i] = sin (m_triangleAngles[i]); + m_cosTable[i] = cos (m_triangleAngles[i]); + } + + m_vertexEdge = (dgEdge**) m_mesh->GetAllocator()->MallocLow (m_mesh->GetVertexCount() * sizeof (dgEdge*)); + mark = m_mesh->IncLRU(); + for (iter.Begin(); iter; iter ++) { + dgEdge* const vertex = &iter.GetNode()->GetInfo(); + if (vertex->m_mark != mark) { + dgInt32 index = vertex->m_incidentVertex; + m_vertexEdge[index] = vertex; + dgEdge* ptr = vertex; + do { + ptr->m_mark = mark; + ptr = ptr->m_twin->m_next ; + } while (ptr != vertex); + } + } + + m_gradients = (dgFloat64*) m_mesh->GetAllocator()->MallocLow (2 * m_mesh->GetVertexCount() * sizeof (dgFloat64)); + m_diagonal = (dgFloat64*) m_mesh->GetAllocator()->MallocLow (2 * m_mesh->GetVertexCount() * sizeof (dgFloat64)); + + LagrangeOptimization(); + + dgAssert (0); +/* + dgStackattribArray (m_mesh->GetCount()); +// dgInt32 attribCount = m_mesh->EnumerateAttributeArray (&attribArray[0]); + mark = m_mesh->IncLRU(); + for (iter.Begin(); iter; iter ++) { + dgEdge* const edge = &iter.GetNode()->GetInfo(); + if (edge->m_mark != mark) { + dgInt32 vertexIndex = edge->m_incidentVertex; + dgEdge* const vertexEdge = m_vertexEdge[vertexIndex]; + dgFloat64 u = m_uvArray[vertexIndex * 2 + 0]; + dgFloat64 v = m_uvArray[vertexIndex * 2 + 1]; + dgEdge* ptr = vertexEdge; + do { + if (ptr->m_incidentFace > 0) { + dgInt32 index = dgInt32 (ptr->m_userData); + dgMeshEffect::dgVertexAtribute& attribute = m_mesh->GetAttribute (index); + attribute.m_u0 = u; + attribute.m_v0 = v; + attribute.m_material = material; + } + ptr->m_mark = mark; + ptr = ptr->m_twin->m_next; + } while (ptr != vertexEdge); + } + } +*/ + dgAssert (0); + //m_mesh->ApplyAttributeArray(&attribArray[0], attribCount); + } + + ~dgTriangleAnglesToUV() + { + m_mesh->GetAllocator()->FreeLow (m_diagonal); + m_mesh->GetAllocator()->FreeLow (m_gradients); + m_mesh->GetAllocator()->FreeLow (m_vertexEdge); + m_mesh->GetAllocator()->FreeLow (m_sinTable); + m_mesh->GetAllocator()->FreeLow (m_cosTable); + m_mesh->GetAllocator()->FreeLow (m_uvArray); + m_mesh->GetAllocator()->FreeLow (m_triangles); + + if (m_allocated) { + m_mesh->GetAllocator()->FreeLow (m_triangleAngles); + } + } + +/* + void GenerateUVCoordinates () + { + m_mesh->SaveOFF("xxx.off"); + + dgStack attibuteUsed (m_attibuteCount); + memset (&attibuteUsed[0], 0, attibuteUsed.GetSizeInBytes()); + dgInt32 mark = m_mesh->IncLRU(); + for (dgInt32 i = 0; i < m_triangleCount; i ++) { + dgEdge* const face = m_betaEdge[i * 3]; + if (face->m_mark != mark) { + dgEdge* ptr = face; + do { + if (ptr->m_incidentFace > 0) { + dgInt32 index = dgInt32 (ptr->m_userData); + attibuteUsed[index] = 1; + m_uvArray[index].m_u0 = dgFloat32 (0.0f); + m_uvArray[index].m_v0 = dgFloat32 (0.0f); + } + ptr = ptr->m_twin->m_next; + } while (ptr != face); + + dgEdge* const twinFace = face->m_twin; + const dgBigVector& p0 = m_mesh->GetVertex(face->m_incidentVertex); + const dgBigVector& p1 = m_mesh->GetVertex(twinFace->m_incidentVertex); + dgBigVector p10 (p1 - p0); + dgFloat64 e0length = sqrt (p10 % p10); + + ptr = twinFace; + do { + if (ptr->m_incidentFace > 0) { + dgInt32 index = dgInt32 (ptr->m_userData); + attibuteUsed[index] = 1; + m_uvArray[index].m_u0 = e0length; + m_uvArray[index].m_v0 = dgFloat32 (0.0f); + } + ptr = ptr->m_twin->m_next; + } while (ptr != twinFace); + + dgList stack(m_mesh->GetAllocator()); + stack.Append(face); + while (stack.GetCount()) { + dgList::dgListNode* const node = stack.GetFirst(); + dgEdge* const face = node->GetInfo(); + stack.Remove (node); + if (face->m_mark != mark) { + dgInt32 uvIndex2 = dgInt32 (face->m_prev->m_userData); + if (!attibuteUsed[uvIndex2]) { + + dgInt32 uvIndex0 = dgInt32 (face->m_userData); + dgInt32 uvIndex1 = dgInt32 (face->m_next->m_userData); + + dgInt32 edgeIndex0 = GetAlphaLandaIndex (face); + dgInt32 edgeIndex1 = GetAlphaLandaIndex (face->m_next); + dgInt32 edgeIndex2 = GetAlphaLandaIndex (face->m_prev); + + dgFloat64 refAngleCos = cos (m_variables[edgeIndex0]); + dgFloat64 refAngleSin = sin (m_variables[edgeIndex0]); + dgFloat64 scale = sin (m_variables[edgeIndex1]) / sin (m_variables[edgeIndex2]); + + dgFloat64 du = (m_uvArray[uvIndex1].m_u0 - m_uvArray[uvIndex0].m_u0) * scale; + dgFloat64 dv = (m_uvArray[uvIndex1].m_v0 - m_uvArray[uvIndex0].m_v0) * scale; + dgFloat64 u = m_uvArray[uvIndex0].m_u0 + du * refAngleCos - dv * refAngleSin; + dgFloat64 v = m_uvArray[uvIndex0].m_v0 + du * refAngleSin + dv * refAngleCos; + + dgEdge* ptr = face->m_prev; + do { + if (ptr->m_incidentFace > 0) { + dgInt32 index = dgInt32 (ptr->m_userData); + attibuteUsed[index] = 1; + m_uvArray[index].m_u0 = u; + m_uvArray[index].m_v0 = v; + } + ptr = ptr->m_twin->m_next; + } while (ptr != face->m_prev); + } + + face->m_mark = mark; + face->m_next->m_mark = mark; + face->m_prev->m_mark = mark; + + if (face->m_next->m_twin->m_incidentFace > 0) { + stack.Append(face->m_next->m_twin); + } + + if (face->m_prev->m_twin->m_incidentFace > 0) { + stack.Append(face->m_prev->m_twin); + } + } + } + } + } + } +*/ + + dgInt32 GetAlphaLandaIndex (const dgEdge* const edge) const + { + return edge->m_incidentFace - 1; + } + + void AnglesFromUV () + { + m_allocated = true; + m_triangleAngles = (dgFloat64*) m_mesh->GetAllocator()->MallocLow (3 * m_trianglesCount * sizeof (dgFloat64)); + + // calculate initial beta angle for each triangle + for (dgInt32 i = 0; i < m_trianglesCount; i ++) { + dgEdge* const edge = m_triangles[i]; + + const dgBigVector& p0 = m_mesh->GetVertex(edge->m_incidentVertex); + const dgBigVector& p1 = m_mesh->GetVertex(edge->m_next->m_incidentVertex); + const dgBigVector& p2 = m_mesh->GetVertex(edge->m_prev->m_incidentVertex); + + dgBigVector e10 (p1 - p0); + dgBigVector e20 (p2 - p0); + dgBigVector e12 (p2 - p1); + dgAssert(e10.m_w == dgFloat32(0.0f)); + dgAssert(e20.m_w == dgFloat32(0.0f)); + dgAssert(e12.m_w == dgFloat32(0.0f)); + + e10 = e10.Scale (dgFloat64 (1.0) / sqrt (e10.DotProduct(e10).GetScalar())); + e20 = e20.Scale (dgFloat64 (1.0) / sqrt (e20.DotProduct(e20).GetScalar())); + e12 = e20.Scale (dgFloat64 (1.0) / sqrt (e12.DotProduct(e12).GetScalar())); + + m_triangleAngles[i * 3 + 0] = acos (dgClamp(e10.DotProduct(e20).GetScalar(), dgFloat64 (-1.0f), dgFloat64 (1.0f))); + m_triangleAngles[i * 3 + 1] = acos (dgClamp(e10.DotProduct(e20).GetScalar(), dgFloat64 (-1.0f), dgFloat64 (1.0f))); + m_triangleAngles[i * 3 + 2] = dgABF_PI - m_triangleAngles[i * 3 + 0] - m_triangleAngles[i * 3 + 1]; + } + } + +/* + // objective function + f[u2_,v2_,u3_,v3_,u4_,v4_,u5_,v5_,u6_,v6_] := + (cos[a0] * sin[b0] * (u1 - u0) + sin[a0] * sin[b0] * (v1 - v0) - sin[c0] * (u6 - u0)) ^ 2 + + (cos[a0] * sin[b0] * (v1 - v0) + sin[a0] * sin[b0] * (u1 - u0) - sin[c0] * (v6 - v0)) ^ 2 + + (cos[a1] * sin[b1] * (u2 - u0) + sin[a1] * sin[b1] * (v2 - v0) - sin[c1] * (u1 - u0)) ^ 2 + + (cos[a1] * sin[b1] * (v2 - v0) + sin[a1] * sin[b1] * (u2 - u0) - sin[c1] * (v1 - v0)) ^ 2 + + (cos[a2] * sin[b2] * (u5 - u0) + sin[a2] * sin[b2] * (v5 - v0) - sin[c2] * (u2 - u0)) ^ 2 + + (cos[a2] * sin[b2] * (v5 - v0) + sin[a2] * sin[b2] * (u5 - u0) - sin[c2] * (v2 - v0)) ^ 2 + + (cos[a3] * sin[b3] * (u2 - u1) + sin[a3] * sin[b3] * (v2 - v1) - sin[c3] * (u3 - u1)) ^ 2 + + (cos[a3] * sin[b3] * (v2 - v1) + sin[a3] * sin[b3] * (u2 - u1) - sin[c3] * (v3 - v1)) ^ 2 + + (cos[a4] * sin[b4] * (u3 - u1) + sin[a4] * sin[b4] * (v3 - v1) - sin[c4] * (u4 - u1)) ^ 2 + + (cos[a4] * sin[b4] * (v3 - v1) + sin[a4] * sin[b4] * (u3 - u1) - sin[c4] * (v4 - v1)) ^ 2 + + (cos[a5] * sin[b5] * (u4 - u1) + sin[a5] * sin[b5] * (v4 - v1) - sin[c5] * (u6 - u1)) ^ 2 + + (cos[a5] * sin[b5] * (v4 - v1) + sin[a5] * sin[b5] * (u4 - u1) - sin[c5] * (v6 - v1)) ^ 2 + + (cos[a6] * sin[b6] * (u4 - u2) + sin[a6] * sin[b6] * (v4 - v2) - sin[c6] * (u3 - u2)) ^ 2 + + (cos[a6] * sin[b6] * (v4 - v2) + sin[a6] * sin[b6] * (u4 - u2) - sin[c6] * (v3 - v2)) ^ 2 + + (cos[a7] * sin[b7] * (u5 - u2) + sin[a7] * sin[b7] * (v5 - v2) - sin[c7] * (u4 - u2)) ^ 2 + + (cos[a7] * sin[b7] * (v5 - v2) + sin[a7] * sin[b7] * (u5 - u2) - sin[c7] * (v4 - v2)) ^ 2 + + (cos[a8] * sin[b8] * (u5 - u4) + sin[a8] * sin[b8] * (v5 - v4) - sin[c8] * (u6 - u4)) ^ 2 + + (cos[a8] * sin[b8] * (v5 - v4) + sin[a8] * sin[b8] * (u5 - u4) - sin[c8] * (v6 - v4)) ^ 2 +*/ + void TraceObjectiveFunction() const + { + DG_DEBUG_UV (("f[")); + for (dgInt32 i = 2; i < m_mesh->GetVertexCount(); i ++) { + DG_DEBUG_UV (("u%d_,v%d_", i, i)); + if (i != (m_mesh->GetVertexCount() - 1)) { + DG_DEBUG_UV ((",")); + } + } + DG_DEBUG_UV (("] := \n")); + + for (dgInt32 i = 0; i < m_trianglesCount; i ++) { + dgEdge* const face = m_triangles[i]; + + dgInt32 v0 = face->m_incidentVertex; + dgInt32 v1 = face->m_next->m_incidentVertex; + dgInt32 v2 = face->m_prev->m_incidentVertex; + (void)(v0); + (void)(v1); + (void)(v2); + DG_DEBUG_UV (("(cos[a%d] * sin[b%d] * (u%d - u%d) + sin[a%d] * sin[b%d] * (v%d - v%d) - sin[c%d] * (u%d - u%d)) ^ 2 +\n", i, i, v1, v0, i, i, v1, v0, i, v2, v0)); + DG_DEBUG_UV (("(cos[a%d] * sin[b%d] * (v%d - v%d) + sin[a%d] * sin[b%d] * (u%d - u%d) - sin[c%d] * (v%d - v%d)) ^ 2", i, i, v1, v0, i, i, v1, v0, i, v2, v0)); + if (i != (m_trianglesCount - 1)) { + DG_DEBUG_UV ((" + \n")); + } else { + DG_DEBUG_UV (("\n")); + } + } + } + + dgFloat64 CalculateExpression_U_face (const dgEdge* const face) const + { + dgInt32 faceIndex = GetAlphaLandaIndex (face); + dgEdge* const faceStartEdge = m_triangles[faceIndex]; + + dgInt32 uvIndex0 = dgInt32 (faceStartEdge->m_incidentVertex); + dgInt32 uvIndex1 = dgInt32 (faceStartEdge->m_next->m_incidentVertex); + dgInt32 uvIndex2 = dgInt32 (faceStartEdge->m_prev->m_incidentVertex); + + dgInt32 alphaIndex0 = faceIndex * 3; + dgInt32 alphaIndex1 = faceIndex * 3 + 1; + dgInt32 alphaIndex2 = faceIndex * 3 + 2; + + DG_DEBUG_UV (("(")); + DG_DEBUG_UV (("cos(a%d) * sin(b%d) * (u%d - u%d) + ", faceIndex, faceIndex, uvIndex1, uvIndex0)); + DG_DEBUG_UV (("sin(a%d) * sin(b%d) * (v%d - v%d) + ", faceIndex, faceIndex, uvIndex1, uvIndex0)); + DG_DEBUG_UV (("sin(c%d) * (u%d - u%d)", faceIndex, uvIndex2, uvIndex0)); + DG_DEBUG_UV ((")")); + + dgFloat64 gradient = m_cosTable[alphaIndex0] * m_sinTable[alphaIndex1] * (m_uvArray[uvIndex1 * 2] - m_uvArray[uvIndex0 * 2]) + + m_sinTable[alphaIndex0] * m_sinTable[alphaIndex1] * (m_uvArray[uvIndex1 * 2 + 1] - m_uvArray[uvIndex0 * 2 + 1]) + + m_sinTable[alphaIndex2] * (m_uvArray[uvIndex2 * 2] - m_uvArray[uvIndex0 * 2]); + return gradient; + } + + dgFloat64 CalculateExpression_V_face (const dgEdge* const face) const + { + dgInt32 faceIndex = GetAlphaLandaIndex (face); + dgEdge* const faceStartEdge = m_triangles[faceIndex]; + + dgInt32 uvIndex0 = dgInt32 (faceStartEdge->m_incidentVertex); + dgInt32 uvIndex1 = dgInt32 (faceStartEdge->m_next->m_incidentVertex); + dgInt32 uvIndex2 = dgInt32 (faceStartEdge->m_prev->m_incidentVertex); + + dgInt32 alphaIndex0 = faceIndex * 3; + dgInt32 alphaIndex1 = faceIndex * 3 + 1; + dgInt32 alphaIndex2 = faceIndex * 3 + 2; + + DG_DEBUG_UV (("(")); + DG_DEBUG_UV (("cos(a%d) * sin(b%d) * (v%d - v%d) + ", faceIndex, faceIndex, uvIndex1, uvIndex0)); + DG_DEBUG_UV (("sin(a%d) * sin(b%d) * (u%d - u%d) + ", faceIndex, faceIndex, uvIndex1, uvIndex0)); + DG_DEBUG_UV (("sin(c%d) * (v%d - v%d)", faceIndex, uvIndex2, uvIndex0)); + DG_DEBUG_UV ((")")); + + dgFloat64 gradient = m_cosTable[alphaIndex0] * m_sinTable[alphaIndex1] * (m_uvArray[uvIndex1 * 2 + 1] - m_uvArray[uvIndex0 * 2 + 1]) + + m_sinTable[alphaIndex0] * m_sinTable[alphaIndex1] * (m_uvArray[uvIndex1 * 2] - m_uvArray[uvIndex0 * 2]) + + m_sinTable[alphaIndex2] * (m_uvArray[uvIndex2 * 2 + 1] - m_uvArray[uvIndex0 * 2 + 1]); + return gradient; + } + + + dgFloat64 CalculateGradient_U_Coefficent (const dgEdge* const edge, bool u) const + { + DG_DEBUG_UV (("(")); + dgInt32 faceIndex = GetAlphaLandaIndex (edge); + dgEdge* const faceStartEdge = m_triangles[faceIndex]; + + dgFloat64 gradient = dgFloat64 (0.0f); + + dgInt32 alphaIndex0 = faceIndex * 3; + dgInt32 alphaIndex1 = faceIndex * 3 + 1; + dgInt32 alphaIndex2 = faceIndex * 3 + 2; + if (faceStartEdge == edge) { + if (u) { + DG_DEBUG_UV ((" - cos(a%d) * sin(b%d) - sin(c%d)", faceIndex, faceIndex, faceIndex)); + gradient = - m_cosTable[alphaIndex0] * m_sinTable[alphaIndex1] - m_sinTable[alphaIndex2]; + } else { + DG_DEBUG_UV ((" - sin(a%d) * sin(b%d) - sin(c%d)", faceIndex, faceIndex, faceIndex)); + gradient = - m_sinTable[alphaIndex0] * m_sinTable[alphaIndex1] - m_sinTable[alphaIndex2]; + } + } else if (faceStartEdge->m_next == edge) { + if (u) { + DG_DEBUG_UV (("cos(a%d) * sin(b%d)", faceIndex, faceIndex)); + gradient = m_cosTable[alphaIndex0] * m_sinTable[alphaIndex1]; + } else { + DG_DEBUG_UV (("sin(a%d) * sin(b%d)", faceIndex, faceIndex)); + gradient = m_sinTable[alphaIndex0] * m_sinTable[alphaIndex1]; + } + } else { + dgAssert (faceStartEdge->m_prev == edge); + if (u) { + DG_DEBUG_UV ((" - sin(c%d)", faceIndex)); + gradient = -m_sinTable[alphaIndex2]; + } else { + DG_DEBUG_UV (("0")); + } + } + DG_DEBUG_UV ((")")); + return gradient; + } + + dgFloat64 CalculateGradient_V_Coefficent (const dgEdge* const edge, bool u) const + { + DG_DEBUG_UV (("(")); + dgInt32 faceIndex = GetAlphaLandaIndex (edge); + dgEdge* const faceStartEdge = m_triangles[faceIndex]; + + dgInt32 alphaIndex0 = faceIndex * 3; + dgInt32 alphaIndex1 = faceIndex * 3 + 1; + dgInt32 alphaIndex2 = faceIndex * 3 + 2; + + dgFloat64 gradient = dgFloat64 (0.0f); + if (faceStartEdge == edge) { + if (!u) { + DG_DEBUG_UV ((" - cos(a%d) * sin(b%d) - sin(c%d)", faceIndex, faceIndex, faceIndex)); + gradient = - m_cosTable[alphaIndex0] * m_sinTable[alphaIndex1] - m_sinTable[alphaIndex2]; + } else { + DG_DEBUG_UV ((" - sin(a%d) * sin(b%d) - sin(c%d)", faceIndex, faceIndex, faceIndex)); + gradient = - m_sinTable[alphaIndex0] * m_sinTable[alphaIndex1] - m_sinTable[alphaIndex2]; + } + } else if (faceStartEdge->m_next == edge) { + if (!u) { + DG_DEBUG_UV (("cos(a%d) * sin(b%d)", faceIndex, faceIndex)); + gradient = m_cosTable[alphaIndex0] * m_sinTable[alphaIndex1]; + } else { + DG_DEBUG_UV (("sin(a%d) * sin(b%d)", faceIndex, faceIndex)); + gradient = m_sinTable[alphaIndex0] * m_sinTable[alphaIndex1]; + } + } else { + dgAssert (faceStartEdge->m_prev == edge); + if (!u) { + DG_DEBUG_UV ((" - sin(c%d)", faceIndex)); + gradient = -m_sinTable[alphaIndex2]; + } else { + DG_DEBUG_UV (("0")); + } + } + DG_DEBUG_UV ((")")); + return gradient; + } + + + dgFloat64 CalculateHessianExpression_U_V (const dgEdge* const face) const + { + dgInt32 faceIndex = GetAlphaLandaIndex (face); + //dgEdge* const faceStartEdge = m_triangles[faceIndex]; + //dgInt32 uvIndex0 = dgInt32 (faceStartEdge->m_incidentVertex); + //dgInt32 uvIndex1 = dgInt32 (faceStartEdge->m_next->m_incidentVertex); + //dgInt32 uvIndex2 = dgInt32 (faceStartEdge->m_prev->m_incidentVertex); + dgInt32 alphaIndex0 = faceIndex * 3; + dgInt32 alphaIndex1 = faceIndex * 3 + 1; + //dgInt32 alphaIndex2 = faceIndex * 3 + 2; + DG_DEBUG_UV (("( - sin(a%d) * sin(b%d))", faceIndex, faceIndex)); + return - m_sinTable[alphaIndex0] * m_sinTable[alphaIndex1]; + } + + + dgFloat64 CalculateHessianExpression_V_V (const dgEdge* const face) const + { + dgInt32 faceIndex = GetAlphaLandaIndex (face); + //dgEdge* const faceStartEdge = m_triangles[faceIndex]; + //dgInt32 uvIndex0 = dgInt32 (faceStartEdge->m_incidentVertex); + //dgInt32 uvIndex1 = dgInt32 (faceStartEdge->m_next->m_incidentVertex); + //dgInt32 uvIndex2 = dgInt32 (faceStartEdge->m_prev->m_incidentVertex); + + dgInt32 alphaIndex0 = faceIndex * 3; + dgInt32 alphaIndex1 = faceIndex * 3 + 1; + dgInt32 alphaIndex2 = faceIndex * 3 + 2; + DG_DEBUG_UV (("(- cos(a%d) * sin(b%d) - sin(c%d))", faceIndex, faceIndex, faceIndex)); + return - m_cosTable[alphaIndex0] * m_sinTable[alphaIndex1] - m_sinTable[alphaIndex2]; + } + + + void CalculateGradientU (dgInt32 vertexIndex) + { + // calculate U Gradient derivative + const dgEdge* const vertex = m_vertexEdge[vertexIndex]; + DG_DEBUG_UV (("du%d =\n", vertexIndex)); + dgFloat64 gradient = dgFloat64 (0.0f); + const dgEdge* ptr = vertex; + do { + if (ptr->m_incidentFace > 0) { + DG_DEBUG_UV (("2 * ")); + dgAssert (ptr->m_incidentVertex == vertexIndex); + dgFloat64 a = CalculateGradient_U_Coefficent (ptr, true) ; + DG_DEBUG_UV ((" * ")); + gradient += a * CalculateExpression_U_face (ptr); + DG_DEBUG_UV ((" +\n")); + + DG_DEBUG_UV (("2 * ")); + a = CalculateGradient_U_Coefficent (ptr, false); + DG_DEBUG_UV ((" * ")); + gradient += a * CalculateExpression_V_face (ptr); + DG_DEBUG_UV ((" +\n")); + } + ptr = ptr->m_twin->m_next; + } while (ptr != vertex); + m_gradients[2 * vertexIndex] = - gradient; + DG_DEBUG_UV (("\n")); + + // calculate diagonal derivative + DG_DEBUG_UV (("H(u%d,u%d) =\n", vertexIndex, vertexIndex)); + dgFloat64 diagonal = dgFloat64 (0.0f); + ptr = vertex; + do { + if (ptr->m_incidentFace > 0) { + DG_DEBUG_UV (("2 * ")); + dgAssert (ptr->m_incidentVertex == vertexIndex); + dgFloat64 diag = CalculateGradient_U_Coefficent (ptr, true); + diagonal += diag * diag; + DG_DEBUG_UV (("^2 +\n")); + + DG_DEBUG_UV (("2 * ")); + diag = CalculateGradient_U_Coefficent (ptr, false); + diagonal += diag * diag; + DG_DEBUG_UV (("^2 +\n")); + } + ptr = ptr->m_twin->m_next; + } while (ptr != vertex); + dgAssert (diagonal > dgFloat32 (0.0f)); + + m_hessianCoLumnValue[m_matrixElementCount] = diagonal; + m_hessianCoLumnIndex[m_matrixElementCount] = vertexIndex * 2 + 0; + m_matrixElementCount ++; + m_diagonal[2 * vertexIndex] = diagonal; + DG_DEBUG_UV (("\n")); + + // calculate of diagonal UiVi derivative + DG_DEBUG_UV (("H(u%d,v%d) =\n", vertexIndex, vertexIndex)); + dgFloat64 hessianUV = dgFloat64 (0.0); + ptr = vertex; + do { + if (ptr->m_incidentFace > 0) { + DG_DEBUG_UV (("2 * ")); + dgAssert (ptr->m_incidentVertex == vertexIndex); + dgFloat64 a = CalculateGradient_U_Coefficent (ptr, true); + DG_DEBUG_UV ((" * ")); + hessianUV += a * CalculateHessianExpression_U_V (ptr); + DG_DEBUG_UV ((" +\n")); + + DG_DEBUG_UV (("2 * ")); + a = CalculateGradient_U_Coefficent (ptr, false); + DG_DEBUG_UV ((" * ")); + hessianUV += a * CalculateHessianExpression_V_V (ptr); + DG_DEBUG_UV ((" +\n")); + } + ptr = ptr->m_twin->m_next; + } while (ptr != vertex); + + m_hessianCoLumnValue[m_matrixElementCount] = hessianUV; + m_hessianCoLumnIndex[m_matrixElementCount] = vertexIndex * 2 + 1; + m_matrixElementCount ++; + DG_DEBUG_UV (("\n")); + + +/* + // calculate off diagonal partial derivatives + ptr = vertex; + do { + // derivative respect to U(i, j) + dgInt32 vertexIndex2 = ptr->m_twin->m_incidentVertex; + DG_DEBUG_UV (("H(u%d,u%d) =\n", vertexIndex, vertexIndex2)); + if (ptr->m_incidentFace > 0) { + DG_DEBUG_UV (("2 * ")); + dgAssert (ptr->m_incidentVertex == vertexIndex); + dgFloat64 a = CalculateGradient_U_Coefficent (ptr, true); + DG_DEBUG_UV ((" * ")); +// gradient += diag * TraceExpression_U_face (ptr); + DG_DEBUG_UV ((" +\n")); + + DG_DEBUG_UV (("2 * ")); + a = CalculateGradient_U_Coefficent (ptr, false); + DG_DEBUG_UV ((" * ")); +// gradient += diag * TraceExpression_V_face (ptr); + DG_DEBUG_UV ((" +\n")); + } + + if (ptr->m_twin->m_incidentFace > 0) { + DG_DEBUG_UV (("2 * ")); + dgAssert (ptr->m_incidentVertex == vertexIndex); + dgFloat64 a = CalculateGradient_U_Coefficent (ptr->m_twin->m_next, true); + DG_DEBUG_UV ((" * ")); + // gradient += diag * TraceExpression_U_face (ptr); + DG_DEBUG_UV ((" +\n")); + + DG_DEBUG_UV (("2 * ")); + a = CalculateGradient_U_Coefficent (ptr->m_twin->m_next, false); + DG_DEBUG_UV ((" * ")); + // gradient += diag * TraceExpression_V_face (ptr); + DG_DEBUG_UV ((" +\n")); + } + + // derivative respect to V(i, j) + DG_DEBUG_UV (("H(u%d,v%d) =\n", vertexIndex, vertexIndex2)); + if (ptr->m_incidentFace > 0) { + + DG_DEBUG_UV (("2 * ")); + dgAssert (ptr->m_incidentVertex == vertexIndex); + dgFloat64 a = CalculateGradient_U_Coefficent (ptr, true); + DG_DEBUG_UV ((" * ")); + // gradient += diag * TraceExpression_U_face (ptr); + DG_DEBUG_UV ((" +\n")); + + DG_DEBUG_UV (("2 * ")); + a = CalculateGradient_U_Coefficent (ptr, false); + DG_DEBUG_UV ((" * ")); + // gradient += diag * TraceExpression_V_face (ptr); + DG_DEBUG_UV ((" +\n")); + + } + + if (ptr->m_twin->m_incidentFace > 0) { + + DG_DEBUG_UV (("2 * ")); + dgAssert (ptr->m_incidentVertex == vertexIndex); + dgFloat64 a = CalculateGradient_U_Coefficent (ptr->m_twin->m_next, true); + DG_DEBUG_UV ((" * ")); + // gradient += diag * TraceExpression_U_face (ptr); + DG_DEBUG_UV ((" +\n")); + + DG_DEBUG_UV (("2 * ")); + a = CalculateGradient_U_Coefficent (ptr->m_twin->m_next, false); + DG_DEBUG_UV ((" * ")); + // gradient += diag * TraceExpression_V_face (ptr); + DG_DEBUG_UV ((" +\n")); + + } + + DG_DEBUG_UV (("\n")); + ptr = ptr->m_twin->m_next; + } while (ptr != vertex); +*/ + } + + + void CalculateGradientV (dgInt32 vertexIndex) + { + // calculate U Gradient derivative + const dgEdge* const vertex = m_vertexEdge[vertexIndex]; + DG_DEBUG_UV (("dv%d =\n", vertexIndex)); + + dgFloat64 gradient = dgFloat64 (0.0f); + const dgEdge* ptr = vertex; + do { + if (ptr->m_incidentFace > 0) { + DG_DEBUG_UV (("2 * ")); + dgAssert (ptr->m_incidentVertex == vertexIndex); + dgFloat64 a = CalculateGradient_V_Coefficent (ptr, true); + DG_DEBUG_UV ((" * ")); + gradient += a * CalculateExpression_U_face (ptr); + DG_DEBUG_UV ((" +\n")); + + DG_DEBUG_UV (("2 * ")); + a = CalculateGradient_V_Coefficent (ptr, false); + DG_DEBUG_UV ((" * ")); + gradient += a * CalculateExpression_V_face (ptr); + DG_DEBUG_UV ((" +\n")); + } + ptr = ptr->m_twin->m_next; + } while (ptr != vertex); + m_gradients[2 * vertexIndex + 1] = - gradient; + DG_DEBUG_UV (("\n")); + + + // calculate diagonal derivative + DG_DEBUG_UV (("H(v%d,v%d) =\n", vertexIndex, vertexIndex)); + dgFloat64 diagonal = dgFloat64 (0.0f); + ptr = vertex; + do { + if (ptr->m_incidentFace > 0) { + DG_DEBUG_UV (("2 * ")); + dgAssert (ptr->m_incidentVertex == vertexIndex); + dgFloat64 diag = CalculateGradient_V_Coefficent (ptr, true); + diagonal += diag * diag; + DG_DEBUG_UV (("^2 +\n")); + + DG_DEBUG_UV (("2 * ")); + diag = CalculateGradient_V_Coefficent (ptr, false); + diagonal += diag * diag; + DG_DEBUG_UV (("^2 +\n")); + } + ptr = ptr->m_twin->m_next; + } while (ptr != vertex); + dgAssert (diagonal > dgFloat32 (0.0f)); + + m_hessianCoLumnValue[m_matrixElementCount] = diagonal; + m_hessianCoLumnIndex[m_matrixElementCount] = vertexIndex * 2 + 1; + m_matrixElementCount ++; + m_diagonal[2 * vertexIndex + 1] = diagonal; + DG_DEBUG_UV (("\n")); + + + + + + } + + + + void CalculateGradientVectorAndHessianMatrix () + { + // trace objective function +// TraceObjectiveFunction(); + + // trace gradients + DG_DEBUG_UV (("\n")); + dgInt32 count = m_mesh->GetVertexCount(); + for (dgInt32 i = 0; i < count; i ++) { + CalculateGradientU (i); + CalculateGradientV (i); + } + DG_DEBUG_UV (("\n")); + } + + void InversePrecoditionerTimeVector (dgFloat64* const out, const dgFloat64* const v) const + { + const dgInt32 count = m_mesh->GetVertexCount(); + for (dgInt32 i = 0; i < count; i ++) { + out[2 * i + 0] = m_pinnedPoints[i] * v[i * 2 + 0] / m_diagonal[2 * i + 0]; + out[2 * i + 1] = m_pinnedPoints[i] * v[i * 2 + 1] / m_diagonal[2 * i + 1]; + } + } + + void MatrixTimeVector (dgFloat64* const out, const dgFloat64* const v) const + { +/* + const dgInt32 count = m_mesh->GetVertexCount(); + for (dgInt32 i = 0; i < count; i ++) { + dgEdge* const vertex = m_vertexEdge[i]; + dgAssert (vertex->m_incidentVertex == i); + out[i * 2 + 0] = m_diagonal[2 * i + 0] * v[2 * i + 0]; + out[i * 2 + 1] = m_diagonal[2 * i + 1] * v[2 * i + 1]; + } + DG_DEBUG_UV (("\n")); + dgInt32 count = m_mesh->GetVertexCount(); + for (dgInt32 i = 0; count; i ++) { + CalculateHessianDiagonalUU (i); + } + DG_DEBUG_UV (("\n")); +*/ + } + + void LagrangeOptimization() + { + CalculateGradientVectorAndHessianMatrix (); + dgStack r0(2 * m_mesh->GetVertexCount()); + dgStack z0(2 * m_mesh->GetVertexCount()); + dgStack p0(2 * m_mesh->GetVertexCount()); + dgStack q0(2 * m_mesh->GetVertexCount()); + SetBuffers(&r0[0], &z0[0], &p0[0], &q0[0]); + Solve(2 * m_mesh->GetVertexCount(), dgABF_UV_TOL2, m_uvArray, m_gradients); + SetBuffers(NULL, NULL, NULL, NULL); + } + + dgArray m_hessianCoLumnIndex; + dgArray m_hessianCoLumnValue; + dgMeshEffect* m_mesh; + dgEdge** m_triangles; + dgEdge** m_vertexEdge; + dgFloat64* m_uvArray; + dgFloat64* m_sinTable; + dgFloat64* m_cosTable; + dgFloat64* m_gradients; + dgFloat64* m_diagonal; + dgFloat64* m_triangleAngles; + const dgFloat64* m_pinnedPoints; + + dgInt32 m_trianglesCount; + dgInt32 m_matrixElementCount; + bool m_allocated; +}; + +class dgAngleBasedFlatteningMapping: public dgSymmetricConjugateGradientSolver +{ + public: + dgAngleBasedFlatteningMapping (dgMeshEffect* const mesh, dgInt32 material, dgReportProgress progressReportCallback, void* const userData) + :m_mesh(mesh) + ,m_progressReportUserData(userData) + ,m_progressReportCallback(progressReportCallback) + { +dgAssert (0); +/* + AllocVectors(); + InitEdgeVector(); + CalculateInitialAngles (); + LagrangeOptimization(); + + dgEdge* const face = m_betaEdge[0]; + dgEdge* ptr = face; + do { + if (ptr->m_incidentFace > 0) { + dgInt32 index = dgInt32 (ptr->m_userData); + dgMeshEffect::dgVertexAtribute& attribute = m_mesh->GetAttribute (index); + attribute.m_u0 = dgFloat32 (0.0f); + attribute.m_v0 = dgFloat32 (0.0f); + } + ptr = ptr->m_twin->m_next; + } while (ptr != face); + + dgEdge* const twinFace = face->m_twin; + const dgBigVector& p0 = m_mesh->GetVertex(face->m_incidentVertex); + const dgBigVector& p1 = m_mesh->GetVertex(twinFace->m_incidentVertex); + dgBigVector p10 (p1 - p0); + dgAssert(p10.m_w == dgFloat32(0.0f)); + dgFloat64 e0length = sqrt (p10.DotProduct(p10).GetScalar()); + + ptr = twinFace; + do { + if (ptr->m_incidentFace > 0) { + dgInt32 index = dgInt32 (ptr->m_userData); + dgMeshEffect::dgVertexAtribute& attribute = m_mesh->GetAttribute (index); + attribute.m_u0 = e0length; + attribute.m_v0 = dgFloat32 (0.0f); + } + ptr = ptr->m_twin->m_next; + } while (ptr != twinFace); + + DeleteAuxiliaryVectors(); + + m_deltaVariables[0] = 0.0f; + m_deltaVariables[1] = 0.0f; + for (dgInt32 i = 2; i < m_totalVariablesCount; i ++) { + m_deltaVariables[i] = 1.0f; + } + dgTriangleAnglesToUV anglesToUV (mesh, material, progressReportCallback, userData, m_deltaVariables, m_variables); +*/ + } + + ~dgAngleBasedFlatteningMapping() + { + m_mesh->GetAllocator()->FreeLow (m_variables); + m_mesh->GetAllocator()->FreeLow (m_deltaVariables); + } + + + void AllocVectors() + { + CalculateNumberOfVariables(); + dgInt32 vertexCount = m_mesh->GetVertexCount(); + + // alloc intermediate vectors + m_betaEdge = (dgEdge**) m_mesh->GetAllocator()->MallocLow(m_anglesCount * sizeof (dgEdge*)); + m_interiorIndirectMap = (dgInt32*) m_mesh->GetAllocator()->MallocLow (vertexCount * sizeof (dgInt32)); + m_beta = (dgFloat64*) m_mesh->GetAllocator()->MallocLow (m_anglesCount * sizeof (dgFloat64)); + m_weight= (dgFloat64*) m_mesh->GetAllocator()->MallocLow (m_anglesCount * sizeof (dgFloat64)); + m_sinTable = (dgFloat64*) m_mesh->GetAllocator()->MallocLow (m_anglesCount * sizeof (dgFloat64)); + m_cosTable = (dgFloat64*) m_mesh->GetAllocator()->MallocLow (m_anglesCount * sizeof (dgFloat64)); + m_gradients = (dgFloat64*) m_mesh->GetAllocator()->MallocLow (m_totalVariablesCount * sizeof (dgFloat64)); + + // allocate angle and internal vertex vector + m_variables = (dgFloat64*) m_mesh->GetAllocator()->MallocLow (m_totalVariablesCount * sizeof (dgFloat64)); + m_deltaVariables = (dgFloat64*) m_mesh->GetAllocator()->MallocLow (m_totalVariablesCount * sizeof (dgFloat64)); + } + + void DeleteAuxiliaryVectors() + { + // delete intermediate vectors + m_mesh->GetAllocator()->FreeLow (m_betaEdge); + m_mesh->GetAllocator()->FreeLow (m_interiorIndirectMap); + m_mesh->GetAllocator()->FreeLow (m_sinTable); + m_mesh->GetAllocator()->FreeLow (m_cosTable); + m_mesh->GetAllocator()->FreeLow (m_beta); + m_mesh->GetAllocator()->FreeLow (m_weight); + m_mesh->GetAllocator()->FreeLow (m_gradients); + + m_beta = NULL; + m_weight = NULL; + m_betaEdge = NULL; + m_sinTable = NULL; + m_cosTable = NULL; + m_gradients = NULL; + m_interiorIndirectMap = NULL; + } + + + void CalculateNumberOfVariables() + { + //m_mesh->SaveOFF("xxx.off"); + m_anglesCount = 0; + m_triangleCount = 0; + m_interiorVertexCount = 0; + + dgInt32 mark = m_mesh->IncLRU(); + dgMeshEffect::Iterator iter (*m_mesh); + for (iter.Begin(); iter; iter ++) { + dgEdge* const edge = &iter.GetNode()->GetInfo(); + if ((edge->m_incidentFace > 0) && (edge->m_mark != mark)) { + dgEdge *ptr = edge; + do { + m_anglesCount ++; + ptr->m_mark = mark; + ptr = ptr->m_next; + } while (ptr != edge); + m_triangleCount ++; + dgAssert (edge->m_next->m_next->m_next == edge); + } + } + + mark = m_mesh->IncLRU(); + for (iter.Begin(); iter; iter ++) { + dgEdge* const edge = &iter.GetNode()->GetInfo(); + if ((edge->m_incidentFace > 0) && (edge->m_mark != mark)) { + bool isInterior = true; + dgEdge *ptr = edge; + do { + isInterior &= (ptr->m_incidentFace > 0); + ptr->m_mark = mark; + ptr = ptr->m_twin->m_next; + } while (ptr != edge); + m_interiorVertexCount += isInterior ? 1 : 0; + } + } + m_totalVariablesCount = m_anglesCount + m_triangleCount + 2 * m_interiorVertexCount; + } + + void InitEdgeVector() + { + dgInt32 count = 0; + dgInt32 mark = m_mesh->IncLRU(); + dgMeshEffect::Iterator iter (*m_mesh); + for (iter.Begin(); iter; iter ++) { + dgEdge* const edge = &iter.GetNode()->GetInfo(); + if ((edge->m_incidentFace > 0) && (edge->m_mark != mark)) { + dgEdge *ptr = edge; + do { + ptr->m_mark = mark; + m_betaEdge[count] = ptr; + ptr->m_incidentFace = count + 1; + count ++; + dgAssert (count <= m_anglesCount); + ptr = ptr->m_next; + } while (ptr != edge); + } + } + + count = 0; + mark = m_mesh->IncLRU(); + memset (m_interiorIndirectMap, -1, m_mesh->GetVertexCount() * sizeof (m_interiorIndirectMap[0])); + for (iter.Begin(); iter; iter ++) { + dgEdge* const edge = &iter.GetNode()->GetInfo(); + if ((edge->m_incidentFace > 0) && (edge->m_mark != mark)) { + + bool isInterior = true; + dgEdge* ptr = edge; + do { + isInterior &= (ptr->m_incidentFace > 0); + ptr->m_mark = mark; + ptr = ptr->m_twin->m_next; + } while (ptr != edge); + if (isInterior) { + m_interiorIndirectMap[edge->m_incidentVertex] = m_anglesCount + m_triangleCount + count; + count ++; + } + } + } + } + + dgInt32 GetAlphaLandaIndex (const dgEdge* const edge) const + { + return edge->m_incidentFace - 1; + } + + dgInt32 GetTriangleIndex (const dgInt32 alphaIndex) const + { + return alphaIndex / 3 + m_anglesCount; + } + + dgInt32 GetTriangleIndex (const dgEdge* const edge) const + { + return GetAlphaLandaIndex(edge) / 3 + m_anglesCount; + } + + dgInt32 GetInteriorVertex(const dgEdge* const edge) const + { + return m_interiorIndirectMap[edge->m_incidentVertex]; + } + + void CalculateInitialAngles () + { + // calculate initial beta angle for each triangle + for (dgInt32 i = 0; i < m_anglesCount; i ++) { + dgEdge* const edge = m_betaEdge[i]; + + const dgBigVector& p0 = m_mesh->GetVertex(edge->m_incidentVertex); + const dgBigVector& p1 = m_mesh->GetVertex(edge->m_next->m_incidentVertex); + const dgBigVector& p2 = m_mesh->GetVertex(edge->m_prev->m_incidentVertex); + + dgBigVector e10 (p1 - p0); + dgBigVector e20 (p2 - p0); + + e10 = e10.Scale (dgFloat64 (1.0) / sqrt (e10.DotProduct(e10).GetScalar())); + e20 = e20.Scale (dgFloat64 (1.0) / sqrt (e20.DotProduct(e20).GetScalar())); + dgAssert(e10.m_w == dgFloat32(0.0f)); + dgAssert(e20.m_w == dgFloat32(0.0f)); + + m_beta[i] = acos (dgClamp(e10.DotProduct(e20).GetScalar(), dgFloat64 (-1.0f), dgFloat64 (1.0f))); + dgAssert (m_beta[i] > dgFloat64 (0.0f)); + } + + #ifdef _DEBUG + for (dgInt32 i = 0; i < m_triangleCount; i ++) { + dgInt32 i0 = i * 3 + 0; + dgInt32 i1 = i * 3 + 1; + dgInt32 i2 = i * 3 + 2; + dgAssert (fabs (m_beta[i0] + m_beta[i1] + m_beta[i2] - dgABF_PI) < dgFloat64 (1.0e-6f)); + } + #endif + + // for each interior vertex apply the scale factor + dgInt32 mark = m_mesh->IncLRU(); + for (dgInt32 i = 0; i < m_anglesCount; i ++) { + dgEdge* const edge = m_betaEdge[i]; + if ((edge->m_mark != mark) && (GetInteriorVertex(edge) >= 0)) { + dgFloat64 scale = dgFloat64 (0.0f); + dgEdge* ptr = edge; + do { + dgInt32 index = GetAlphaLandaIndex (ptr); + dgAssert (index >= 0); + dgAssert (index <= m_anglesCount); + scale += m_beta[index]; + ptr->m_mark = mark; + ptr = ptr->m_twin->m_next; + } while (ptr != edge); + dgAssert (scale > dgFloat32 (0.0f)); + + scale = dgFloat64 (2.0f) * dgABF_PI / scale; + ptr = edge; + do { + dgInt32 index = GetAlphaLandaIndex (ptr); + dgAssert (index >= 0); + dgAssert (index <= m_anglesCount); + m_beta[index] *= scale; + ptr = ptr->m_twin->m_next; + } while (ptr != edge); + } + } + + // initialized each alpha lambda to the beta angle and also calcual ethe derivatoe coeficent (2.0 / (betai * betai)) + for (dgInt32 i = 0; i < m_anglesCount; i ++) { + dgAssert (m_beta[i] > dgFloat64 (0.0f)); + m_variables[i] = m_beta[i]; + m_weight[i] = dgFloat64 (2.0f) / (m_beta[i] * m_beta[i]); + } + } + + // angular derivative component + // (wi * (xi - bi) + T0 + // where wi = 2.0 / (bi ^ 2) + dgFloat64 CalculateAngularGradientDerivative (dgInt32 alphaIndex) const + { + dgFloat64 gradient = (m_variables[alphaIndex] - m_beta[alphaIndex]) * m_weight[alphaIndex] + m_variables[GetTriangleIndex(alphaIndex)]; + dgAssert (fabs(gradient) < dgFloat64(1.0e10f)); + return gradient; + } + + // Vi if the the edge is an interior vertex + dgFloat64 CalculateInteriorVertexGradient (dgInt32 alphaIndex) const + { + dgInt32 index = GetInteriorVertex(m_betaEdge[alphaIndex]); + dgFloat64 gradient = (index != -1) ? m_variables[index] : dgFloat32 (0.0f); + dgAssert (fabs(gradient) < dgFloat64(1.0e10f)); + return gradient; + } + + // Wj * cos(alpha) * sum (alphai) for eadh previsu or next interior incdent vertex + dgFloat64 CalculatePlanarityGradient (dgInt32 alphaIndex) const + { + dgFloat64 gradient = dgFloat64 (0.0f); + + dgEdge* const incidentEdge = m_betaEdge[alphaIndex]; + + if (GetInteriorVertex (incidentEdge->m_next) != -1) { + dgEdge* const edge = m_betaEdge[GetAlphaLandaIndex(incidentEdge->m_next)]; + dgFloat64 product = m_cosTable[GetAlphaLandaIndex(edge->m_prev)]; + dgEdge* ptr = edge->m_twin->m_next; + do { + product *= m_sinTable[GetAlphaLandaIndex(ptr->m_prev)]; + ptr = ptr->m_twin->m_next; + } while (ptr != edge); + dgInt32 interiorVertexIndex = GetInteriorVertex (incidentEdge->m_next) + m_interiorVertexCount; + gradient -= m_variables[interiorVertexIndex] * product; + } + + if (GetInteriorVertex (incidentEdge->m_prev) != -1) { + dgEdge* const edge = m_betaEdge[GetAlphaLandaIndex(incidentEdge->m_prev)]; + dgFloat64 product = m_cosTable[GetAlphaLandaIndex(edge->m_next)]; + dgEdge* ptr = edge->m_twin->m_next; + do { + product *= m_sinTable[GetAlphaLandaIndex(ptr->m_next)]; + ptr = ptr->m_twin->m_next; + } while (ptr != edge); + dgInt32 interiorVertexIndex = GetInteriorVertex (incidentEdge->m_prev) + m_interiorVertexCount; + gradient += m_variables[interiorVertexIndex] * product; + } + dgAssert (fabs(gradient) < dgFloat64(1.0e10f)); + return gradient; + } + + // sample of the Gradient Vector according to Mathematic to a generic mesh, this can be generalize for and arbitrary mesh topology + // x0 - x14 are the planar angles in 2d + // b0 - b14 are the mesh angles in 3d. + // T0 - T4 are the triangle lambdas + // V0 - V1 interior vertex lambdas + // W0 - W1 interior vertex wheel lambdas + // + // Gradient derivatives: + //0 (2 (-b0 + x0))/b0^2 + T0 + W2 Cos[x0] Sin[x5] Sin[x9] + //1 (2 (-b1 + x1))/b1^2 + T0 - W2 Cos[x1] Sin[x10] Sin[x3] + //2 (2 (-b2 + x2))/b2^2 + T0 + V2 + //3 (2 (-b3 + x3))/b3^2 + T1 - W2 Cos[x3] Sin[x1] Sin[x10] + W3 Cos[x3] Sin[x11] Sin[x12] Sin[x8] + //4 (2 (-b4 + x4))/b4^2 + T1 + V2 - W3 Cos[x4] Sin[x13] Sin[x6] Sin[x9] + //5 (2 (-b5 + x5))/b5^2 + T1 + V3 + W2 Cos[x5] Sin[x0] Sin[x9] + //6 (2 (-b6 + x6))/b6^2 + T2 - W3 Cos[x6] Sin[x13] Sin[x4] Sin[x9] + //7 (2 (-b7 + x7))/b7^2 + T2 + V3 + //8 (2 (-b8 + x8))/b8^2 + T2 + W3 Cos[x8] Sin[x11] Sin[x12] Sin[x3] + //9 (2 (-b09 + x09))/b09^2 + T3 + W2 Cos[x9] Sin[x0] Sin[x5] - W3 Cos[x9] Sin[x13] Sin[x4] Sin[x6] + //10 (2 (-b10 + x10))/b10^2 + T3 + V3 - W2 Cos[x10] Sin[x1] Sin[x3] + //11 (2 (-b11 + x11))/b11^2 + T3 + V2 + W3 Cos[x11] Sin[x12] Sin[x3] Sin[x8] + //12 (2 (-b12 + x12))/b12^2 + T4 + W3 Cos[x12] Sin[x11] Sin[x3] Sin[x8] + //13 (2 (-b13 + x13))/b13^2 + T4 - W3 Cos[x13] Sin[x4] Sin[x6] Sin[x9] + //14 (2 (-b14 + x14))/b14^2 + T4 + V3 + // + //15 x0 + x1 + x2 - pi + //16 x3 + x4 + x5 - pi + //17 x6 + x7 + x8 - pi + //18 x10 + x11 + x9 - pi + //19 x12 + x13 + x14 - pi + // + //20 x11 + x2 + x4 - 2 pi + //21 x10 + x14 + x5 + x7 - 2 pi + // + //22 Sin[x0] Sin[x5] Sin[x9] - Sin[x1] Sin[x10] Sin[x3] + //23 Sin[x11] Sin[x12] Sin[x3] Sin[x8] - Sin[x13] Sin[x4] Sin[x6] Sin[x9] + dgFloat64 CalculateGradientVector () + { + // pre-compute sin cos tables + for (dgInt32 i = 0; i < m_anglesCount; i ++) { + m_sinTable[i] = sin (m_variables[i]); + m_cosTable[i] = cos (m_variables[i]); + } + + dgFloat64 gradientNorm = dgFloat64 (0.0f); + + // calculate gradients due to the difference between a matching edge angle and it projected angle msu be mminimal Wei * (Xei - Bei) ^ e = minimal + for (dgInt32 i = 0; i < m_anglesCount; i ++) { + dgFloat64 gradient = CalculateAngularGradientDerivative (i) + CalculateInteriorVertexGradient (i) + CalculatePlanarityGradient (i); + m_gradients[i] = -gradient; + gradientNorm += gradient * gradient; + } + + // calculate gradient due to the equality that the sum on the internal angle of a triangle must add to 180 degree. (Xt0 + Xt1 + Xt2 - pi) = 0 + for (dgInt32 i = 0; i < m_triangleCount; i ++) { + dgFloat64 gradient = m_variables[i * 3 + 0] + m_variables[i * 3 + 1] + m_variables[i * 3 + 2] - dgABF_PI; + m_gradients[m_anglesCount + i] = -gradient; + gradientNorm += gradient * gradient; + } + + // calculate the gradient due to the equality that the sum of all the angle incident to and interior vertex must be 3060 degree sum (Xvi) - 2 * pi = 0 + dgInt32 mark = m_mesh->IncLRU(); + for (dgInt32 i = 0; i < m_anglesCount; i ++) { + dgEdge* const edge = m_betaEdge[i]; + + if ((edge->m_mark != mark) && GetInteriorVertex(edge) != -1) { + dgInt32 vertexIndex = GetInteriorVertex(edge); + dgFloat64 gradient = - dgFloat64 (2.0f) * dgABF_PI; + + dgEdge* ptr = edge; + do { + dgInt32 index = GetAlphaLandaIndex(ptr); + gradient += m_variables[index]; + ptr->m_mark = mark; + ptr = ptr->m_twin->m_next; + } while (ptr != edge); + m_gradients[vertexIndex] = - gradient; + gradientNorm += gradient * gradient; + } + } + + // calculate the gradient due to the equality that the difference of the product of the sin of the angle to the + // incident to an interior vertex must be zero product (sin (Xvi + 1) - product (sin (Xvi - 1) = 0 + mark = m_mesh->IncLRU(); + for (dgInt32 i = 0; i < m_anglesCount; i ++) { + dgEdge* const edge = m_betaEdge[i]; + + dgInt32 vertexIndex = GetInteriorVertex(edge); + if ((edge->m_mark != mark) && (vertexIndex != -1)) { + vertexIndex += m_interiorVertexCount; + dgFloat64 partialProdut0 = dgFloat64 (1.0f); + dgFloat64 partialProdut1 = dgFloat64 (1.0f); + dgEdge* ptr = edge; + do { + dgInt32 index0 = GetAlphaLandaIndex(ptr->m_next); + dgInt32 index1 = GetAlphaLandaIndex(ptr->m_prev); + partialProdut0 *= m_sinTable[index0]; + partialProdut1 *= m_sinTable[index1]; + ptr->m_mark = mark; + ptr = ptr->m_twin->m_next; + } while (ptr != edge); + dgFloat64 gradient = partialProdut0 - partialProdut1; + m_gradients[vertexIndex] = - gradient; + gradientNorm += gradient * gradient; + } + } + + return gradientNorm; + } + + // the Hessian matrix is compose of these second partial derivatives + // these derivatives are too complex and make the solver to spend too much time, + // [0][0] 2/b0^2 - W2 Sin[x0] Sin[x5] Sin[x9], 0, 0, 0, 0, W2 Cos[x0] Cos[x5] Sin[x9], 0, 0, 0, W2 Cos[x0] Cos[x9] Sin[x5], 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, Cos[x0] Sin[x5] Sin[x9], 0, + // [0][[1] {0, 2/b1^2 + W2 Sin[x1] Sin[x10] Sin[x3], 0, -W2 Cos[x1] Cos[x3] Sin[x10], 0, 0, 0, 0, 0, 0, -W2 Cos[x1] Cos[x10] Sin[x3], 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, -Cos[x1] Sin[x10] Sin[x3], 0}, + // ... + + // the optimize version of the algorithms assume that the second derivatives are linear, therefore all sine terms are neglected, I will do the same + // [ 0][0-n] 2/b0^2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, Cos[x0] Sin[x5] Sin[x9], 0 + // [ 1][0-n] 0, 2/b1^2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, -Cos[x1] Sin[x10] Sin[x3], 0 + // [ 2][0-n] 0, 0, 2/b2^2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0 + // [ 3][0-n] 0, 0, 0, 2/b3^2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, -Cos[x3] Sin[x1] Sin[x10], Cos[x3] Sin[x11] Sin[x12] Sin[x8] + // [ 4][0-n] 0, 0, 0, 0, 2/b4^2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, -Cos[x4] Sin[x13] Sin[x6] Sin[x9]} + // [ 5][0-n] 0, 0, 0, 0, 0, 2/b5^2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, Cos[x5] Sin[x0] Sin[x9], 0 + // [ 6][0-n] 0, 0, 0, 0, 0, 0, 2/b6^2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, -Cos[x6] Sin[x13] Sin[x4] Sin[x9] + // [ 7][0-n] 0, 0, 0, 0, 0, 0, 0, 2/b7^2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0 + // [ 8][0-n] 0, 0, 0, 0, 0, 0, 0, 0, 2/b8^2, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, Cos[x8] Sin[x11] Sin[x12] Sin[x3] + // [ 9][0-n] 0, 0, 0, 0, 0, 0, 0, 0, 0, 2/b9^2, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, Cos[x9] Sin[x0] Sin[x5], -Cos[x9] Sin[x13] Sin[x4] Sin[x6] + // [10][0-n] 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 2/b10^2, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, -Cos[x10] Sin[x1] Sin[x3], 0}, + // [11][0-n] 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 2/b11^2, 0, 0, 0, 0, 0, 0, 1, 0, 1, 0, 0, Cos[x11] Sin[x12] Sin[x3] Sin[x8] + // [12][0-n] 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 2/b12^2, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, Cos[x12] Sin[x11] Sin[x3] Sin[x8] + // [13][0-n] 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 2/b13^2, 0, 0, 0, 0, 0, 1, 0, 0, 0, -Cos[x13] Sin[x4] Sin[x6] Sin[x9] + // [14][0-n] 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 2/b14^2, 0, 0, 0, 0, 1, 0, 1, 0, 0 + + // [15][0-n] 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 + // [16][0-n] 0, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 + // [17][0-n] 0, 0, 0, 0, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 + // [18][0-n] 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 + // [19][0-n] 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0 + + // [20][0-n] 0, 0, 1, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 + // [21][0-n] 0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0 + + // [22][0-n] Cos[x0] Sin[x5] Sin[x9], -Cos[x1] Sin[x10] Sin[x3], 0, -Cos[x3] Sin[x1] Sin[x10], 0, Cos[x5] Sin[x0] Sin[x9], 0, 0, 0, Cos[x9] Sin[x0] Sin[x5], -Cos[x10] Sin[x1] Sin[x3], 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 + // [23][0-n] 0, 0, 0, Cos[x3] Sin[x11] Sin[x12] Sin[x8], -Cos[x4] Sin[x13] Sin[x6] Sin[x9], 0, -Cos[x6] Sin[x13] Sin[x4] Sin[x9], 0, Cos[x8] Sin[x11] Sin[x12] Sin[x3], -Cos[x9] Sin[x13] Sin[x4] Sin[x6], 0, Cos[x11] Sin[x12] Sin[x3] Sin[x8], Cos[x12] Sin[x11] Sin[x3] Sin[x8], -Cos[x13] Sin[x4] Sin[x6] Sin[x9], 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 + void MatrixTimeVector (dgFloat64* const out, const dgFloat64* const v) const + { + for (dgInt32 i = 0; i < m_interiorVertexCount; i ++) { + out[i + m_anglesCount + m_triangleCount] = dgFloat64 (0.0f); + out[i + m_anglesCount + m_triangleCount + m_interiorVertexCount] = dgFloat64 (0.0f); + } + + for (dgInt32 i = 0; i < m_anglesCount; i ++) { + out[i] = m_weight[i] * v[i]; + + dgEdge* const edge = m_betaEdge[i]; + dgInt32 vertexIndex = GetInteriorVertex(edge); + if (vertexIndex >= 0) { + out[i] += v[vertexIndex]; + out[vertexIndex] += v[i]; + } + } + + for (dgInt32 i = 0; i < m_triangleCount; i ++) { + dgInt32 j = i * 3; + out[j + 0] += v[i + m_anglesCount]; + out[j + 1] += v[i + m_anglesCount]; + out[j + 2] += v[i + m_anglesCount]; + out[i + m_anglesCount] = v[j + 0] + v[j + 1] + v[j + 2]; + } + + for (dgInt32 i = 0; i < m_anglesCount; i ++) { + { + dgEdge* const edge = m_betaEdge[i]->m_prev; + dgInt32 vertexIndex = GetInteriorVertex(edge); + if (vertexIndex >= 0) { + dgInt32 index = GetAlphaLandaIndex(edge->m_next); + dgFloat64 product = m_cosTable[index]; + dgEdge* ptr = edge->m_twin->m_next; + do { + dgInt32 m = GetAlphaLandaIndex(ptr->m_next); + product *= m_sinTable[m]; + ptr = ptr->m_twin->m_next; + } while (ptr != edge); + out[i] += v[vertexIndex + m_interiorVertexCount] * product; + out[vertexIndex + m_interiorVertexCount] += product * v[i]; + } + } + + { + dgEdge* const edge = m_betaEdge[i]->m_next; + dgInt32 vertexIndex = GetInteriorVertex(edge); + if (vertexIndex >= 0) { + dgInt32 index = GetAlphaLandaIndex(edge->m_prev); + dgFloat64 product = m_cosTable[index]; + dgEdge* ptr = edge->m_twin->m_next; + do { + dgInt32 m = GetAlphaLandaIndex(ptr->m_prev); + product *= m_sinTable[m]; + ptr = ptr->m_twin->m_next; + } while (ptr != edge); + out[i] -= v[vertexIndex + m_interiorVertexCount] * product; + out[vertexIndex + m_interiorVertexCount] -= product * v[i]; + } + } + } + } + + void InversePrecoditionerTimeVector (dgFloat64* const out, const dgFloat64* const v) const + { + for (dgInt32 i = 0; i < m_anglesCount; i ++) { + out[i] = v[i] / m_weight[i]; + } + for (dgInt32 i = 0; i < m_triangleCount; i ++) { + out[i + m_anglesCount] = v[i + m_anglesCount]; + } + + for (dgInt32 i = 0; i < m_interiorVertexCount; i ++) { + out[i + m_anglesCount + m_triangleCount] = v[i + m_anglesCount + m_triangleCount]; + out[i + m_anglesCount + m_triangleCount + m_interiorVertexCount] = v[i + m_anglesCount + m_triangleCount + m_interiorVertexCount]; + } + + m_progressNum ++; + if (m_progressReportCallback) { + if ((m_progressNum & 127) == 127) { + m_continueExecution = m_progressReportCallback (dgMin (dgFloat32 (m_progressNum) / m_progressDen, dgFloat32 (1.0f)), m_progressReportUserData); + } + } + } + + void LagrangeOptimization() + { + memset (m_deltaVariables, 0, m_totalVariablesCount * sizeof (dgFloat64)); + memset (&m_variables[m_anglesCount], 0, m_triangleCount * sizeof (dgFloat64)); + + for (dgInt32 i = 0; i < m_interiorVertexCount; i ++) { + m_variables[i + m_anglesCount + m_triangleCount] = dgFloat32 (1.0f); + m_variables[i + m_anglesCount + m_triangleCount + m_interiorVertexCount] = dgFloat32 (1.0f); + } + + m_progressNum = 0; + m_continueExecution = true; + +/* + dgStack r0(2 * m_mesh->GetVertexCount()); + dgStack z0(2 * m_mesh->GetVertexCount()); + dgStack p0(2 * m_mesh->GetVertexCount()); + dgStack q0(2 * m_mesh->GetVertexCount()); + SetBuffers(&r0[0], &z0[0], &p0[0], &q0[0]); + dgFloat64 gradientNorm = CalculateGradientVector (); + for (dgInt32 iter = 0; (iter < dgABF_MAX_ITERATIONS) && (gradientNorm > dgABF_TOL2) && m_continueExecution; iter++) { + m_progressDen = m_progressNum + m_totalVariablesCount; + Solve(m_totalVariablesCount, dgABF_LINEAR_SOLVER_TOL, m_deltaVariables, m_gradients); + for (dgInt32 i = 0; i < m_totalVariablesCount; i ++) { + m_variables[i] += m_deltaVariables[i]; + } + gradientNorm = CalculateGradientVector (); + } + SetBuffers(NULL, NULL, NULL, NULL); +*/ + +#ifdef _DEBUG + // calculate gradient due to the equality that the sum on the internal angle of a triangle must add to 180 degree. (Xt0 + Xt1 + Xt2 - pi) = 0 +// for (dgInt32 i = 0; i < m_triangleCount; i ++) { +// dgFloat64 gradient = m_variables[i * 3 + 0] + m_variables[i * 3 + 1] + m_variables[i * 3 + 2] - dgABF_PI; +// dgAssert (fabs (gradient) < dgFloat64 (1.0e-2f)); +// } +#endif + } + + dgMeshEffect* m_mesh; + dgEdge** m_betaEdge; + dgInt32* m_interiorIndirectMap; + + dgFloat64* m_beta; + dgFloat64* m_weight; + dgFloat64* m_sinTable; + dgFloat64* m_cosTable; + dgFloat64* m_variables; + dgFloat64* m_gradients; + dgFloat64* m_deltaVariables; + + dgInt32 m_anglesCount; + dgInt32 m_triangleCount; + dgInt32 m_interiorVertexCount; + dgInt32 m_totalVariablesCount; + + void* m_progressReportUserData; + dgReportProgress m_progressReportCallback; + mutable dgInt32 m_progressNum; + mutable dgInt32 m_progressDen; + mutable bool m_continueExecution; +}; + +dgBigVector dgMeshEffect::GetOrigin ()const +{ + dgBigVector origin (dgFloat64 (0.0f), dgFloat64 (0.0f), dgFloat64 (0.0f), dgFloat64 (0.0f)); + for (dgInt32 i = 0; i < m_points.m_vertex.m_count; i ++) { + origin += m_points.m_vertex[i]; + } + return origin.Scale (dgFloat64 (1.0f) / m_points.m_vertex.m_count); +} + +/* +void dgMeshEffect::ClearAttributeArray () +{ + dgAssert(0); + + dgStackattribArray (m_pointCount); + + memset (&attribArray[0], 0, m_pointCount * sizeof (dgVertexAtribute)); + dgInt32 mark = IncLRU(); + dgPolyhedra::Iterator iter (*this); + for(iter.Begin(); iter; iter ++){ + dgEdge* const edge = &(*iter); + if (edge->m_mark < mark){ + dgEdge* ptr = edge; + + dgInt32 index = ptr->m_incidentVertex; + dgVertexAtribute& attrib = attribArray[index]; + attrib.m_vertex = m_points[index]; + do { + ptr->m_mark = mark; + ptr->m_userData = index; + ptr = ptr->m_twin->m_next; + } while (ptr != edge); + + } + } + ApplyAttributeArray (&attribArray[0], m_pointCount); +} +*/ + +void dgMeshEffect::CalculateNormals (dgFloat64 angleInRadians) +{ + dgEdge* edgeBuffer[256]; + dgBigVector faceNormal[256]; + + UnpackAttibuteData (); + m_attrib.m_normalChannel.Reserve(m_attrib.m_pointChannel.m_count); + + dgInt32 mark = IncLRU(); + dgPolyhedra::Iterator iter (*this); + dgFloat32 smoothValue = dgCos (angleInRadians); + + dgTree normalsMap(GetAllocator()) ; + for(iter.Begin(); iter; iter ++) { + dgEdge* const edge = &(*iter); + if ((edge->m_mark < mark) && (edge->m_incidentFace > 0)) { + dgInt32 edgeIndex = 0; + normalsMap.RemoveAll(); + dgEdge* edgePtr = edge; + do { + dgVector normal (FaceNormal (edgePtr, &m_points.m_vertex[0].m_x, sizeof (dgBigVector))); + dgAssert (normal.m_w == dgFloat32 (0.0f)); + normal = normal.Scale (dgFloat32 (1.0f) / dgFloat32 (sqrt(normal.DotProduct(normal).GetScalar()) + dgFloat32(1.0e-16f))); + faceNormal[edgeIndex] = normal; + normalsMap.Insert(edgeIndex, edgePtr); + edgeIndex ++; + edgePtr = edgePtr->m_twin->m_next; + } while (edgePtr != edge); + + dgEdge* startEdge = edge; + dgVector normal0 (faceNormal[normalsMap.Find(startEdge)->GetInfo()]); + for (dgEdge* ptr = edge->m_prev->m_twin ; (ptr->m_mark != mark) && (ptr != edge) && (ptr->m_incidentFace > 0); ptr = ptr->m_prev->m_twin) { + const dgVector& normal1 (faceNormal[normalsMap.Find(ptr)->GetInfo()]); + dgAssert(normal0.m_w == dgFloat32(0.0f)); + dgFloat32 dot = normal0.DotProduct(normal1).GetScalar(); + if (dot < smoothValue) { + break; + } + startEdge = ptr; + normal0 = normal1; + } + + dgInt32 attribCount = 1; + edgeBuffer[0] = startEdge; + normal0 = faceNormal[normalsMap.Find(startEdge)->GetInfo()]; + dgVector normal (normal0); + for (dgEdge* ptr = startEdge->m_twin->m_next; (ptr->m_mark != mark) && (ptr != startEdge) && (ptr->m_incidentFace > 0); ptr = ptr->m_twin->m_next) { + const dgVector& normal1 (faceNormal[normalsMap.Find(ptr)->GetInfo()]); + dgAssert(normal0.m_w == dgFloat32(0.0f)); + dgFloat32 dot = normal0.DotProduct(normal1).GetScalar(); + if (dot < smoothValue) { + break; + } + edgeBuffer[attribCount] = ptr; + attribCount ++; + normal += normal1; + normal0 = normal1; + } + + dgAssert(normal.m_w == dgFloat32(0.0f)); + normal = normal.Scale (dgFloat32 (1.0f) / dgFloat32(sqrt(normal.DotProduct(normal).GetScalar()) + dgFloat32(1.0e-16f))); + dgTriplex n; + n.m_x = normal.m_x; + n.m_y = normal.m_y; + n.m_z = normal.m_z; + for (dgInt32 i = 0; i < attribCount; i ++) { + edgeBuffer[i]->m_mark = mark; + dgInt32 index = dgInt32 (edgeBuffer[i]->m_userData); + m_attrib.m_normalChannel[index] = n; + } + } + } + PackAttibuteData (); +} + +void dgMeshEffect::SphericalMapping (dgInt32 material, const dgMatrix& uvAligment) +{ + dgBigVector origin (GetOrigin()); + dgStacksphere (m_points.m_vertex.m_count); + for (dgInt32 i = 0; i < m_points.m_vertex.m_count; i ++) { + dgBigVector point(uvAligment.RotateVector(m_points.m_vertex[i] - origin)); + dgAssert(point.m_w == dgFloat32(0.0f)); + dgAssert(point.DotProduct(point).GetScalar() > dgFloat32(0.0f)); + point = point.Normalize(); + + dgFloat64 u = dgAsin(dgClamp(point.m_x, dgFloat64(-1.0f + 1.0e-6f), dgFloat64(1.0f - 1.0e-6f))); + dgFloat64 v = dgAtan2(point.m_y, point.m_z); + + u = dgFloat32(1.0f) - (dgFloat64(dgPi / 2.0f) - u) / dgFloat64(dgPi); + dgAssert(u >= dgFloat32(0.0f)); + dgAssert(u <= dgFloat32(1.0f)); + + v = v + dgPi; + sphere[i].m_x = u; + sphere[i].m_y = v; + } + + UnpackAttibuteData (); + m_attrib.m_uv0Channel.Reserve(m_attrib.m_pointChannel.m_count); + m_attrib.m_materialChannel.Reserve(m_attrib.m_pointChannel.m_count); + + dgPolyhedra::Iterator iter (*this); + for(iter.Begin(); iter; iter ++){ + dgEdge* const edge = &(*iter); + dgAttibutFormat::dgUV uv; + uv.m_u = dgFloat32 (sphere[edge->m_incidentVertex].m_x); + uv.m_v = dgFloat32 (sphere[edge->m_incidentVertex].m_y); + m_attrib.m_uv0Channel[dgInt32 (edge->m_userData)] = uv; + m_attrib.m_materialChannel[dgInt32 (edge->m_userData)] = material; + } + + dgInt32 mark = IncLRU (); + for(iter.Begin(); iter; iter ++){ + dgEdge* const edge = &(*iter); + if ((edge->m_incidentFace > 0) && (edge->m_mark != mark)) { + dgAttibutFormat::dgUV uvRef(m_attrib.m_uv0Channel[dgInt32(edge->m_userData)]); + dgFloat32 UVrefSin = dgSin(uvRef.m_v); + dgFloat32 UVrefCos = dgCos(uvRef.m_v); + dgEdge* ptr = edge; + do { + ptr->m_mark = mark; + dgAttibutFormat::dgUV uv(m_attrib.m_uv0Channel[dgInt32(ptr->m_userData)]); + dgFloat32 sinAngle = UVrefCos * dgSin(uv.m_v) - UVrefSin * dgCos(uv.m_v); + dgFloat32 cosAngle = UVrefCos * dgCos(uv.m_v) + UVrefSin * dgSin(uv.m_v); + dgFloat32 deltaAngle = dgAtan2(sinAngle, cosAngle); + uv.m_v = (uvRef.m_v + deltaAngle) / dgPI2; + m_attrib.m_uv0Channel[dgInt32(ptr->m_userData)] = uv; + ptr = ptr->m_next; + } while (ptr != edge); + } + } + + PackAttibuteData(); +} + +void dgMeshEffect::CylindricalMapping (dgInt32 cylinderMaterial, dgInt32 capMaterial, const dgMatrix& uvAligment) +{ + dgBigVector origin (GetOrigin()); + dgStack buffer(m_points.m_vertex.m_count); + dgBigVector pMin(dgFloat64(1.0e10f), dgFloat64(1.0e10f), dgFloat64(1.0e10f), dgFloat64(0.0f)); + dgBigVector pMax(dgFloat64(-1.0e10f), dgFloat64(-1.0e10f), dgFloat64(-1.0e10f), dgFloat64(0.0f)); + + for (dgInt32 i = 0; i < m_points.m_vertex.m_count; i ++) { + buffer[i] = uvAligment.RotateVector (m_points.m_vertex[i] - origin); + const dgBigVector& tmp = buffer[i]; + pMin.m_x = dgMin (pMin.m_x, tmp.m_x); + pMax.m_x = dgMax (pMax.m_x, tmp.m_x); + pMin.m_y = dgMin (pMin.m_y, tmp.m_y); + pMax.m_y = dgMax (pMax.m_y, tmp.m_y); + pMin.m_z = dgMin (pMin.m_z, tmp.m_z); + pMax.m_z = dgMax (pMax.m_z, tmp.m_z); + } + + dgStackcylinder (m_points.m_vertex.m_count); + dgBigVector scale (dgFloat64 (1.0f)/ (pMax.m_x - pMin.m_x), dgFloat64 (1.0f)/ (pMax.m_y - pMin.m_y), dgFloat64 (1.0f)/ (pMax.m_z - pMin.m_z), dgFloat64 (0.0f)); + for (dgInt32 i = 0; i < m_points.m_vertex.m_count; i ++) { + //dgBigVector point (uvAligment.RotateVector (m_points.m_vertex[i] - origin)); + dgBigVector point (buffer[i]); + dgFloat64 u = (point.m_x - pMin.m_x) * scale.m_x; + + dgAssert(point.m_w == dgFloat32(0.0f)); + dgAssert(point.DotProduct(point).GetScalar() > dgFloat32 (0.0f)); + point = point.Normalize(); + dgFloat64 v = dgAtan2 (point.m_y, point.m_z); + + v = v + dgPi; + cylinder[i].m_x = u; + cylinder[i].m_y = v; + } + + UnpackAttibuteData(); + m_attrib.m_uv0Channel.Reserve(m_attrib.m_pointChannel.m_count); + m_attrib.m_materialChannel.Reserve(m_attrib.m_pointChannel.m_count); + + dgPolyhedra::Iterator iter (*this); + for(iter.Begin(); iter; iter ++){ + dgEdge* const edge = &(*iter); + dgAttibutFormat::dgUV uv; + uv.m_u = dgFloat32(cylinder[edge->m_incidentVertex].m_x); + uv.m_v = dgFloat32(cylinder[edge->m_incidentVertex].m_y); + m_attrib.m_uv0Channel[dgInt32(edge->m_userData)] = uv; + m_attrib.m_materialChannel[dgInt32(edge->m_userData)] = cylinderMaterial; + } + + dgInt32 mark = IncLRU (); + for(iter.Begin(); iter; iter ++){ + dgEdge* const edge = &(*iter); + if ((edge->m_incidentFace > 0) && (edge->m_mark != mark)) { + dgAttibutFormat::dgUV uvRef(m_attrib.m_uv0Channel[dgInt32(edge->m_userData)]); + dgFloat32 UVrefSin = dgSin(uvRef.m_v); + dgFloat32 UVrefCos = dgCos(uvRef.m_v); + dgEdge* ptr = edge; + do { + ptr->m_mark = mark; + dgAttibutFormat::dgUV uv(m_attrib.m_uv0Channel[dgInt32(ptr->m_userData)]); + dgFloat32 sinAngle = UVrefCos * dgSin(uv.m_v) - UVrefSin * dgCos(uv.m_v); + dgFloat32 cosAngle = UVrefCos * dgCos(uv.m_v) + UVrefSin * dgSin(uv.m_v); + dgFloat32 deltaAngle = dgAtan2(sinAngle, cosAngle); + uv.m_v = (uvRef.m_v + deltaAngle) / dgPI2; + m_attrib.m_uv0Channel[dgInt32(ptr->m_userData)] = uv; + ptr = ptr->m_next; + } while (ptr != edge); + } + } + + // apply cap mapping + mark = IncLRU (); + for(iter.Begin(); iter; iter ++){ + dgEdge* const edge = &(*iter); + if ((edge->m_incidentFace > 0) && (edge->m_mark != mark)) { + //dgVector p0(uvAligment.RotateVector(m_points.m_vertex[edge->m_incidentVertex] - origin)); + //dgVector p1(uvAligment.RotateVector(m_points.m_vertex[edge->m_next->m_incidentVertex] - origin)); + dgVector p0(buffer[edge->m_incidentVertex]); + dgVector p1(buffer[edge->m_next->m_incidentVertex]); + + dgVector e1(p1 - p0); + dgBigVector normal(dgFloat32(0.0f)); + for (dgEdge* ptr = edge->m_next; ptr != edge; ptr = ptr->m_next) { + //dgVector p2(uvAligment.RotateVector(m_points.m_vertex[ptr->m_next->m_incidentVertex] - origin)); + dgVector p2(buffer[ptr->m_next->m_incidentVertex]); + dgBigVector e2(p2 - p0); + normal += e1.CrossProduct(e2); + e1 = e2; + } + normal = normal.Normalize(); + if (dgAbs(normal.m_x) > dgFloat32 (0.99f)) { + dgEdge* ptr = edge; + do { + dgAttibutFormat::dgUV uv; + //dgVector p(uvAligment.RotateVector(m_points.m_vertex[ptr->m_incidentVertex] - origin)); + dgVector p(buffer[ptr->m_incidentVertex]); + uv.m_u = dgFloat32((p.m_y - pMin.m_y) * scale.m_y); + uv.m_v = dgFloat32((p.m_z - pMin.m_z) * scale.m_z); + m_attrib.m_uv0Channel[dgInt32(ptr->m_userData)] = uv; + m_attrib.m_materialChannel[dgInt32(ptr->m_userData)] = capMaterial; + ptr = ptr->m_next; + } while (ptr != edge); + } + + dgEdge* ptr = edge; + do { + ptr->m_mark = mark; + ptr = ptr->m_next; + } while (ptr != edge); + + } + } + PackAttibuteData(); +} + +void dgMeshEffect::BoxMapping (dgInt32 front, dgInt32 side, dgInt32 top, const dgMatrix& uvAligment) +{ +/* + dgBigVector minVal; + dgBigVector maxVal; + + dgGetMinMax (minVal, maxVal, &m_points.m_vertex[0][0], m_points.m_vertex.m_count, sizeof (dgBigVector)); + dgBigVector dist (maxVal - minVal); + dist[0] = dgMax (dgFloat64 (1.0e-3f), dist[0]); + dist[1] = dgMax (dgFloat64 (1.0e-3f), dist[1]); + dist[2] = dgMax (dgFloat64 (1.0e-3f), dist[2]); + dgBigVector scale (dgFloat64 (1.0f)/ dist[0], dgFloat64 (1.0f)/ dist[1], dgFloat64 (1.0f)/ dist[2], dgFloat64 (0.0f)); +*/ + + dgBigVector origin(GetOrigin()); + dgStack buffer(m_points.m_vertex.m_count); + dgBigVector pMin(dgFloat64(1.0e10f), dgFloat64(1.0e10f), dgFloat64(1.0e10f), dgFloat64(0.0f)); + dgBigVector pMax(dgFloat64(-1.0e10f), dgFloat64(-1.0e10f), dgFloat64(-1.0e10f), dgFloat64(0.0f)); + + for (dgInt32 i = 0; i < m_points.m_vertex.m_count; i++) { + buffer[i] = uvAligment.RotateVector(m_points.m_vertex[i] - origin); + const dgBigVector& tmp = buffer[i]; + pMin.m_x = dgMin(pMin.m_x, tmp.m_x); + pMax.m_x = dgMax(pMax.m_x, tmp.m_x); + pMin.m_y = dgMin(pMin.m_y, tmp.m_y); + pMax.m_y = dgMax(pMax.m_y, tmp.m_y); + pMin.m_z = dgMin(pMin.m_z, tmp.m_z); + pMax.m_z = dgMax(pMax.m_z, tmp.m_z); + } + dgInt32 materialArray[3]; + + dgBigVector dist (pMax); + dist[0] = dgMax(dgFloat64(1.0e-3f), dist[0]); + dist[1] = dgMax(dgFloat64(1.0e-3f), dist[1]); + dist[2] = dgMax(dgFloat64(1.0e-3f), dist[2]); + dgBigVector scale (dgFloat64 (0.5f)/ dist[0], dgFloat64 (0.5f)/ dist[1], dgFloat64 (0.5f)/ dist[2], dgFloat64 (0.0f)); + + UnpackAttibuteData(); + m_attrib.m_uv0Channel.Reserve(m_attrib.m_pointChannel.m_count); + m_attrib.m_materialChannel.Reserve(m_attrib.m_pointChannel.m_count); + + materialArray[0] = front; + materialArray[1] = side; + materialArray[2] = top; + + dgInt32 mark = IncLRU(); + dgPolyhedra::Iterator iter (*this); + for(iter.Begin(); iter; iter ++){ + dgEdge* const edge = &(*iter); + if ((edge->m_mark < mark) && (edge->m_incidentFace > 0)) { + const dgBigVector& p0 = buffer[edge->m_incidentVertex]; + const dgBigVector& p1 = buffer[edge->m_next->m_incidentVertex]; + const dgBigVector& p2 = buffer[edge->m_prev->m_incidentVertex]; + + edge->m_mark = mark; + edge->m_next->m_mark = mark; + edge->m_prev->m_mark = mark; + + dgBigVector e0 (p1 - p0); + dgBigVector e1 (p2 - p0); + dgBigVector n (e0.CrossProduct(e1)); + + dgInt32 index = 0; + dgFloat64 maxProjection = dgFloat32 (0.0f); + + for (dgInt32 i = 0; i < 3; i ++) { + dgFloat64 proj = fabs (n[i]); + if (proj > maxProjection) { + index = i; + maxProjection = proj; + } + } + + dgInt32 u = (index + 1) % 3; + dgInt32 v = (u + 1) % 3; + if (index == 1) { + dgSwap (u, v); + } + dgEdge* ptr = edge; + do { + dgAttibutFormat::dgUV uv; + dgBigVector p(scale * buffer[ptr->m_incidentVertex] - dgFloat32 (0.5f)); + uv.m_u = dgFloat32 (p[u]); + uv.m_v = dgFloat32 (p[v]); + m_attrib.m_uv0Channel[dgInt32(ptr->m_userData)] = uv; + m_attrib.m_materialChannel[dgInt32(ptr->m_userData)] = materialArray[index]; + ptr = ptr->m_next; + } while (ptr != edge); + } + } + PackAttibuteData(); +} + + +void dgMeshEffect::UniformBoxMapping (dgInt32 material, const dgMatrix& textureMatrix) +{ + UnpackAttibuteData(); + m_attrib.m_uv0Channel.Reserve(m_attrib.m_pointChannel.m_count); + m_attrib.m_materialChannel.Reserve(m_attrib.m_pointChannel.m_count); + + dgInt32 mark = IncLRU(); + for (dgInt32 i = 0; i < 3; i ++) { + dgMatrix rotationMatrix (dgGetIdentityMatrix()); + if (i == 1) { + rotationMatrix = dgYawMatrix(dgFloat32 (90.0f * dgDegreeToRad)); + } else if (i == 2) { + rotationMatrix = dgPitchMatrix(dgFloat32 (90.0f * dgDegreeToRad)); + } + + dgPolyhedra::Iterator iter (*this); + + for(iter.Begin(); iter; iter ++){ + dgEdge* const edge = &(*iter); + if ((edge->m_mark < mark) && (edge->m_incidentFace > 0)) { + dgBigVector n (FaceNormal(edge, &m_points.m_vertex[0].m_x, sizeof (dgBigVector))); + dgVector normal (rotationMatrix.RotateVector(dgVector (n.Normalize()))); + normal.m_x = dgAbs (normal.m_x); + normal.m_y = dgAbs (normal.m_y); + normal.m_z = dgAbs (normal.m_z); + if ((normal.m_z >= (normal.m_x - dgFloat32 (1.0e-4f))) && (normal.m_z >= (normal.m_y - dgFloat32 (1.0e-4f)))) { + dgEdge* ptr = edge; + do { + ptr->m_mark = mark; + dgAttibutFormat::dgUV uv; + dgVector p (textureMatrix.TransformVector(rotationMatrix.RotateVector(m_points.m_vertex[ptr->m_incidentVertex]))); + uv.m_u = p.m_x; + uv.m_v = p.m_y; + m_attrib.m_uv0Channel[dgInt32(ptr->m_userData)] = uv; + m_attrib.m_materialChannel[dgInt32(ptr->m_userData)] = material; + + ptr = ptr->m_next; + }while (ptr != edge); + } + } + } + } + + PackAttibuteData(); +} + + +void dgMeshEffect::AngleBaseFlatteningMapping (dgInt32 material, dgReportProgress progressReportCallback, void* const userData) +{ + dgSetPrecisionDouble presicion; + + dgMeshEffect tmp (*this); + + dgBigVector minBox; + dgBigVector maxBox; + tmp.CalculateAABB(minBox, maxBox); + + dgBigVector size (maxBox - minBox); + dgFloat32 scale = dgFloat32 (1.0 / dgMax (size.m_x, size.m_y, size.m_z)); + + dgMatrix matrix (dgGetIdentityMatrix()); + matrix[0][0] = scale; + matrix[1][1] = scale; + matrix[2][2] = scale; + tmp.ApplyTransform(matrix); + + dgAngleBasedFlatteningMapping angleBadedFlattening (&tmp, material, progressReportCallback, userData); +} diff --git a/thirdparty/src/newton/dgNewton/Newton.cpp b/thirdparty/src/newton/dgNewton/Newton.cpp new file mode 100644 index 000000000..162597887 --- /dev/null +++ b/thirdparty/src/newton/dgNewton/Newton.cpp @@ -0,0 +1,8681 @@ +/* Copyright (c) <2003-2019> +* +* 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 "NewtonStdAfx.h" +#include "Newton.h" +#include "NewtonClass.h" + + +#ifdef _NEWTON_BUILD_DLL + #if (defined (__MINGW32__) || defined (__MINGW64__)) + int main(int argc, char* argv[]) + { + return 0; + } + #endif + + #ifdef _MSC_VER + BOOL APIENTRY DllMain( HMODULE hModule, DWORD ul_reason_for_call, LPVOID lpReserved) + { + switch (ul_reason_for_call) + { + case DLL_THREAD_ATTACH: + case DLL_PROCESS_ATTACH: + // check for memory leaks + #ifdef _DEBUG + // 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 +#endif + + + +/*! @defgroup Misc Misc +Misc +@{ +*/ + +//#define SAVE_COLLISION + +#ifdef SAVE_COLLISION +void SerializeFile (void* serializeHandle, const void* buffer, size_t size) +{ + fwrite (buffer, size, 1, (FILE*) serializeHandle); +} + +void DeSerializeFile (void* serializeHandle, void* buffer, size_t size) +{ + fread (buffer, size, 1, (FILE*) serializeHandle); +} + + +void SaveCollision (const NewtonCollision* const collisionPtr) +{ + FILE* file; + // save the collision file + file = fopen ("collisiontest.bin", "wb"); + //SerializeFile (file, MAGIC_NUMBER, strlen (MAGIC_NUMBER) + 1); + NewtonCollisionSerialize (collisionPtr, SerializeFile, file); + fclose (file); +} +#endif + + +/*! + Return the exact amount of memory (in Bytes) use by the engine at any given time. + + @return total memory use by the engine. + + Applications can use this function to ascertain that the memory use by the + engine is balanced at all times. + + See also: ::NewtonCreate +*/ +int NewtonGetMemoryUsed() +{ + TRACE_FUNCTION(__FUNCTION__); + return dgMemoryAllocator::GetGlobalMemoryUsed(); +} + +// fixme: needs docu +// @param mallocFnt is a pointer to the memory allocator callback function. If this parameter is NULL the standard *malloc* function is used. +// @param mfreeFnt is a pointer to the memory release callback function. If this parameter is NULL the standard *free* function is used. +// +void NewtonSetMemorySystem (NewtonAllocMemory mallocFnt, NewtonFreeMemory mfreeFnt) +{ + dgMemFree _free; + dgMemAlloc _malloc; + + TRACE_FUNCTION(__FUNCTION__); + + if (mallocFnt && mfreeFnt) { + _malloc = (dgMemAlloc) mallocFnt; + _free = (dgMemFree) mfreeFnt; + } else { + _malloc = (dgMemAlloc) Newton::DefaultAllocMemory; + _free = (dgMemFree) Newton::DefaultFreeMemory; + } + + dgMemoryAllocator::SetGlobalAllocators (_malloc, _free); +} + + +void* NewtonAlloc (int sizeInBytes) +{ + return dgMallocStack(sizeInBytes); +} + +void NewtonFree (void* const ptr) +{ + dgFreeStack(ptr); +} + +/*! @} */ // end of group Misc + + +/*! @defgroup World World +World interface +@{ +*/ + +/*! + Create an instance of the Newton world. + + @return Pointer to new Newton world. + + This function must be called before any of the other API functions. + + See also: ::NewtonDestroy, ::NewtonDestroyAllBodies +*/ +NewtonWorld* NewtonCreate() +{ + TRACE_FUNCTION(__FUNCTION__); + dgMemoryAllocator* const allocator = new dgMemoryAllocator(); + + NewtonWorld* const world = (NewtonWorld*) new (allocator) Newton (allocator); + return world; +} + + +/*! + Destroy an instance of the Newton world. + + @param *newtonWorld Pointer to the Newton world. + @return Nothing. + + This function will destroy the entire Newton world. + + See also: ::NewtonCreate, ::NewtonDestroyAllBodies +*/ +void NewtonDestroy(const NewtonWorld* const newtonWorld) +{ + TRACE_FUNCTION(__FUNCTION__); + + Newton* const world = (Newton *) newtonWorld; + dgMemoryAllocator* const allocator = world->dgWorld::GetAllocator(); + + delete world; + delete allocator; +} + +void NewtonSetPostUpdateCallback(const NewtonWorld* const newtonWorld, NewtonPostUpdateCallback callback) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *) newtonWorld; + world->SetPostUpdateCallback((OnPostUpdateCallback) callback); +} + +NewtonPostUpdateCallback NewtonGetPostUpdateCallback(const NewtonWorld* const newtonWorld) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *)newtonWorld; + return (NewtonPostUpdateCallback)world->GetPostUpdateCallback(); +} + +int NewtonGetBroadphaseAlgorithm (const NewtonWorld* const newtonWorld) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *) newtonWorld; + return world->GetBroadPhaseType(); +} + +void NewtonSelectBroadphaseAlgorithm (const NewtonWorld* const newtonWorld, int algorithmType) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *) newtonWorld; + world->SetBroadPhaseType(algorithmType); +} + +void NewtonResetBroadphase(const NewtonWorld* const newtonWorld) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *) newtonWorld; + return world->ResetBroadPhase(); +} + + +dFloat NewtonGetContactMergeTolerance (const NewtonWorld* const newtonWorld) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *) newtonWorld; + return world->GetContactMergeTolerance(); +} + +void NewtonSetContactMergeTolerance (const NewtonWorld* const newtonWorld, dFloat tolerance) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *) newtonWorld; + world->SetContactMergeTolerance(tolerance); +} + + +/*! + Reset all internal engine states. + + @param *newtonWorld Pointer to the Newton world. + + Call this function whenever you want to create a reproducible simulation from + a pre-defined initial condition. + + It does *not* suffice to merely reset the position and velocity of + objects. This is because Newton takes advantage of frame-to-frame coherence for + performance reasons. + + This function must be called outside of a Newton Update. + + Note: this kind of synchronization incurs a heavy performance penalty if + called during each update. + + See also: ::NewtonUpdate +*/ +void NewtonInvalidateCache(const NewtonWorld* const newtonWorld) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *)newtonWorld; + world->FlushCache(); +} + +void NewtonSetJointSerializationCallbacks (const NewtonWorld* const newtonWorld, NewtonOnJointSerializationCallback serializeJoint, NewtonOnJointDeserializationCallback deserializeJoint) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *) newtonWorld; + world->SetJointSerializationCallbacks (dgWorld::OnJointSerializationCallback(serializeJoint), dgWorld::OnJointDeserializationCallback(deserializeJoint)); +} + +void NewtonGetJointSerializationCallbacks (const NewtonWorld* const newtonWorld, NewtonOnJointSerializationCallback* const serializeJoint, NewtonOnJointDeserializationCallback* const deserializeJoint) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *) newtonWorld; + world->GetJointSerializationCallbacks ((dgWorld::OnJointSerializationCallback*)serializeJoint, (dgWorld::OnJointDeserializationCallback*)deserializeJoint); +} + + +void NewtonSerializeScene(const NewtonWorld* const newtonWorld, NewtonOnBodySerializationCallback bodyCallback, void* const bodyUserData, + NewtonSerializeCallback serializeCallback, void* const serializeHandle) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *) newtonWorld; + world->SerializeScene(bodyUserData, dgWorld::OnBodySerialize(bodyCallback), (dgSerialize) serializeCallback, serializeHandle); +} + +void NewtonDeserializeScene(const NewtonWorld* const newtonWorld, NewtonOnBodyDeserializationCallback bodyCallback, void* const bodyUserData, + NewtonDeserializeCallback deserializeCallback, void* const serializeHandle) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *) newtonWorld; + world->DeserializeScene(bodyUserData, (dgWorld::OnBodyDeserialize)bodyCallback, (dgDeserialize) deserializeCallback, serializeHandle); +} + + +void NewtonSerializeToFile (const NewtonWorld* const newtonWorld, const char* const filename, NewtonOnBodySerializationCallback bodyCallback, void* const bodyUserData) +{ + TRACE_FUNCTION(__FUNCTION__); + FILE* const file = fopen(filename, "wb"); + if (file) { + NewtonSerializeScene(newtonWorld, bodyCallback, bodyUserData, dgWorld::OnSerializeToFile, file); + fclose (file); + } +} + +void NewtonDeserializeFromFile (const NewtonWorld* const newtonWorld, const char* const filename, NewtonOnBodyDeserializationCallback bodyCallback, void* const bodyUserData) +{ + TRACE_FUNCTION(__FUNCTION__); + FILE* const file = fopen(filename, "rb"); + if (file) { + NewtonDeserializeScene(newtonWorld, bodyCallback, bodyUserData, dgWorld::OnDeserializeFromFile, file); + fclose (file); + } +} + +NewtonBody* NewtonFindSerializedBody(const NewtonWorld* const newtonWorld, int bodySerializedID) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *) newtonWorld; + dgAssert (0); + return (NewtonBody*) world->FindBodyFromSerializedID(bodySerializedID); +} + +void NewtonLoadPlugins(const NewtonWorld* const newtonWorld, const char* const plugInPath) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *)newtonWorld; + world->LoadPlugins(plugInPath); +} + +void NewtonUnloadPlugins(const NewtonWorld* const newtonWorld) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *)newtonWorld; + world->UnloadPlugins(); +} + +void* NewtonCurrentPlugin(const NewtonWorld* const newtonWorld) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *)newtonWorld; + return world->GetCurrentPlugin(); +} + +void* NewtonGetFirstPlugin(const NewtonWorld* const newtonWorld) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *)newtonWorld; + return world->GetFirstPlugin(); +} + +void* NewtonGetNextPlugin(const NewtonWorld* const newtonWorld, const void* const plugin) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *)newtonWorld; + dgWorldPluginList::dgListNode* const node = (dgWorldPluginList::dgListNode*) plugin; + return world->GetNextPlugin(node); +} + +const char* NewtonGetPluginString(const NewtonWorld* const newtonWorld, const void* const plugin) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *)newtonWorld; + dgWorldPluginList::dgListNode* const node = (dgWorldPluginList::dgListNode*) plugin; + return world->GetPluginId (node); +} + +void NewtonSelectPlugin(const NewtonWorld* const newtonWorld, const void* const plugin) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *)newtonWorld; + dgWorldPluginList::dgListNode* const node = (dgWorldPluginList::dgListNode*) plugin; + return world->SelectPlugin(node); +} + +void* NewtonGetPreferedPlugin(const NewtonWorld* const newtonWorld) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *)newtonWorld; + return world->GetpreferedPlugin(); +} + + +/*! + this function block all other threads from executing the same subsequent code simultaneously. + + @param *newtonWorld Pointer to the Newton world. + @param threadIndex thread index from whe thsi function is called, zero if call form outsize a newton update + + this function should use to present racing conditions when when a call back ins executed form a mutithreaded loop. + In general most call back are thread safe when they do not write to object outside the scope of the call back. + this means for example that the application can modify values of object pointed by the arguments and or call that function + that are allowed to be call for such callback. + There are cases, however, when the application need to collect data for the client logic, example of such case are collecting + information to display debug information, of collecting data for feedback. + In these situations it is possible the the same critical code could be execute at the same time but several thread causing unpredictable side effect. + so it is necessary to block all of the thread from executing any pieces of critical code. + + Not calling function *NewtonWorldCriticalSectionUnlock* will result on the engine going into an infinite loop. + + it is important that the critical section wrapped by functions *NewtonWorldCriticalSectionLock* and + *NewtonWorldCriticalSectionUnlock* be keep small if the application is using the multi threaded functionality of the engine + no doing so will lead to serialization of the parallel treads since only one thread can run the a critical section at a time. + + @return Nothing. + + See also: ::NewtonWorldCriticalSectionUnlock +*/ +void NewtonWorldCriticalSectionLock (const NewtonWorld* const newtonWorld, int threadIndex) +{ + TRACE_FUNCTION(__FUNCTION__); + + Newton* const world = (Newton *)newtonWorld; + world->GlobalLock(); +} + + +int NewtonAtomicSwap (int* const ptr, int value) +{ + TRACE_FUNCTION(__FUNCTION__); + return dgInterlockedExchange(ptr, value); +} + +int NewtonAtomicAdd (int* const ptr, int value) +{ + TRACE_FUNCTION(__FUNCTION__); + return dgAtomicExchangeAndAdd (ptr, value); +} + +void NewtonYield () +{ + TRACE_FUNCTION(__FUNCTION__); + dgThreadYield(); +} + + +/*! + this function block all other threads from executing the same subsequent code simultaneously. + + @param *newtonWorld Pointer to the Newton world. + + + this function should use to present racing conditions when when a call back ins executed form a multi threaded loop. + In general most call back are thread safe when they do not write to object outside the scope of the call back. + this means for example that the application can modify values of object pointed by the arguments and or call that function + that are allowed to be call for such callback. + There are cases, however, when the application need to collect data for the client logic, example of such case are collecting + information to display debug information, of collecting data for feedback. + In these situations it is possible the the same critical code could be execute at the same time but several thread causing unpredictable side effect. + so it is necessary to block all of the thread from executing any pieces of critical code. + + it is important that the critical section wrapped by functions *NewtonWorldCriticalSectionLock* and + *NewtonWorldCriticalSectionUnlock* be keep small if the application is using the multi threaded functionality of the engine + no doing so will lead to serialization of the parallel treads since only one thread can run the a critical section at a time. + + @return Nothing. + + See also: ::NewtonWorldCriticalSectionLock +*/ +void NewtonWorldCriticalSectionUnlock(const NewtonWorld* const newtonWorld) +{ + TRACE_FUNCTION(__FUNCTION__); + + Newton* const world = (Newton *)newtonWorld; + world->GlobalUnlock(); +} + + + +/*! + Set the maximum number of threads the engine can use. + + @param *newtonWorld Pointer to the Newton world. + @param threads Maximum number of allowed threads. + + @return Nothing + + The maximum number of threaded is set on initialization to the maximum number + of CPU in the system. + fixme: this appears to be wrong. It is set to 1. + + See also: ::NewtonGetThreadsCount +*/ +void NewtonSetThreadsCount(const NewtonWorld* const newtonWorld, int threads) +{ + TRACE_FUNCTION(__FUNCTION__); + + Newton* const world = (Newton *)newtonWorld; + world->SetThreadsCount(threads); +} + + +/*! + Return the number of threads currently used by the engine. + + @param *newtonWorld Pointer to the Newton world. + + @return Number threads + + See also: ::NewtonSetThreadsCount, ::NewtonSetParallelSolverOnLargeIsland +*/ +int NewtonGetThreadsCount(const NewtonWorld* const newtonWorld) +{ + TRACE_FUNCTION(__FUNCTION__); + + Newton* const world = (Newton *)newtonWorld; + return world->GetThreadCount(); +} + + +/*! + Return the maximum number of threads supported on this platform. + + @param *newtonWorld Pointer to the Newton world. + + @return Number threads. + + This function will return 1 on single core version of the library. + // fixme; what is a single core version? + + See also: ::NewtonSetThreadsCount, ::NewtonSetParallelSolverOnLargeIsland +*/ +int NewtonGetMaxThreadsCount(const NewtonWorld* const newtonWorld) +{ + TRACE_FUNCTION(__FUNCTION__); + + Newton* const world = (Newton *)newtonWorld; + return world->GetMaxThreadCount(); +} + + +/*! + Enable/disable multi-threaded constraint resolution for large islands + (disabled by default). + + @param *newtonWorld Pointer to the Newton world. + @param mode 1: enabled 0: disabled (default) + + @return Nothing + + Multi threaded mode is not always faster. Among the reasons are + + 1 - Significant software cost to set up threads, as well as instruction overhead. + 2 - Different systems have different cost for running separate threads in a shared memory environment. + 3 - Parallel algorithms often have decreased converge rate. This can be as + high as half of the of the sequential version. Consequently, the parallel + solver requires a higher number of interactions to achieve similar convergence. + + It is recommended this option is enabled on system with more than two cores, + since the performance gain in a dual core system are marginally better. Your + mileage may vary. + + At the very least the application must test the option to verify the performance gains. + + This option has no impact on other subsystems of the engine. + + See also: ::NewtonGetThreadsCount, ::NewtonSetThreadsCount +*/ +void NewtonSetParallelSolverOnLargeIsland(const NewtonWorld* const newtonWorld, int mode) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *)newtonWorld; + world->EnableParallelSolverOnLargeIsland (mode); +} + + +void NewtonDispachThreadJob(const NewtonWorld* const newtonWorld, NewtonJobTask task, void* const usedData, const char* const functionName) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *)newtonWorld; + world->ExecuteUserJob (dgWorkerThreadTaskCallback (task), usedData, functionName); +} + +void NewtonSyncThreadJobs(const NewtonWorld* const newtonWorld) +{ + Newton* const world = (Newton *)newtonWorld; + world->SynchronizationBarrier(); +} + +int NewtonGetParallelSolverOnLargeIsland(const NewtonWorld* const newtonWorld) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *)newtonWorld; + return world->GetParallelSolverOnLargeIsland(); +} + +/*! + Set the solver precision mode. + + @param *newtonWorld is the pointer to the Newton world + @param model model of operation n = number of iteration default value is 4. + + @return Nothing + + n: the solve will execute a maximum of n iteration per cluster of connected joints and will terminate regardless of the + of the joint residual acceleration. + If it happen that the joints residual acceleration fall below the minimum tolerance 1.0e-5 + then the solve will terminate before the number of iteration reach N. +*/ +void NewtonSetSolverIterations(const NewtonWorld* const newtonWorld, int model) +{ + Newton* const world = (Newton *)newtonWorld; + + TRACE_FUNCTION(__FUNCTION__); + world->SetSolverIterations (model); +} + +/*! +Get the solver precision mode. +*/ +int NewtonGetSolverIterations(const NewtonWorld* const newtonWorld) +{ + Newton* const world = (Newton *)newtonWorld; + + TRACE_FUNCTION(__FUNCTION__); + return world->GetSolverIterations(); +} + + +/*! + Advance the simulation by a user defined amount of time. + + @param *newtonWorld is the pointer to the Newton world + @param timestep time step in seconds. + + @return Nothing + + This function will advance the simulation by the specified amount of time. + + The Newton Engine does not perform sub-steps, nor does it need + tuning parameters. As a consequence, the application is responsible for + requesting sane time steps. + + See also: ::NewtonInvalidateCache +*/ +void NewtonUpdate(const NewtonWorld* const newtonWorld, dFloat timestep) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *)newtonWorld; + +//NewtonSerializeToFile (newtonWorld, "xxx.bin", NULL, NULL); + world->UpdatePhysics (timestep); +} + +void NewtonUpdateAsync (const NewtonWorld* const newtonWorld, dFloat timestep) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *)newtonWorld; + + world->UpdatePhysicsAsync(timestep); +} + +void NewtonWaitForUpdateToFinish (const NewtonWorld* const newtonWorld) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *)newtonWorld; + world->Sync (); +} + +dFloat NewtonGetLastUpdateTime (const NewtonWorld* const newtonWorld) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *)newtonWorld; + return world->GetUpdateTime(); +} + + +void NewtonSetNumberOfSubsteps (const NewtonWorld* const newtonWorld, int subSteps) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *) newtonWorld; + world->SetSubsteps (subSteps); +} + +int NewtonGetNumberOfSubsteps (const NewtonWorld* const newtonWorld) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *) newtonWorld; + return world->GetSubsteps (); +} + + + +/*! + Remove all bodies and joints from the Newton world. + + @param *newtonWorld Pointer to the Newton world. + + @return Nothing + + This function will destroy all bodies and all joints in the Newton world, but + will retain group IDs. + + Use this function for when you want to clear the world but preserve all the + group IDs and material pairs. + + See also: ::NewtonMaterialDestroyAllGroupID +*/ +void NewtonDestroyAllBodies(const NewtonWorld* const newtonWorld) +{ + TRACE_FUNCTION(__FUNCTION__); + + Newton* const world = (Newton *) newtonWorld; + world->DestroyAllBodies (); +} + +/*! + Set a function callback to be call on each island update. + + @param *newtonWorld Pointer to the Newton world. + @param islandUpdate callback function. + + @return Nothing. + + The application can set a function callback to be called just after the array + of all bodies making an island of connected bodies are collected. This + function will be called just before the array is accepted for contact + resolution and integration. + + The callback function must return an integer 0 or 1 to either skip or process + the bodies in that particular island. + + Applications can leverage this function to implement an game physics LOD. For + example the application can determine the AABB of the island and check it + against the view frustum. If the entire island AABB is invisible, then the + application can suspend its simulation, even if it is not in equilibrium. + + Other possible applications are to implement of a visual debugger, or freeze + entire islands for application specific reasons. + + The application must not create, modify, or destroy bodies inside the callback + or risk putting the engine into an undefined state (ie it will crash, if you + are lucky). + + See also: ::NewtonIslandGetBody +*/ +void NewtonSetIslandUpdateEvent(const NewtonWorld* const newtonWorld, NewtonIslandUpdate islandUpdate) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *) newtonWorld; + world->SetIslandUpdateCallback((dgWorld::OnClusterUpdate) islandUpdate); +} + + + + +/*! + Get the first body in the body in the world body list. + + @param *newtonWorld Pointer to the Newton world. + + @return nothing + + The application can call this function to iterate thought every body in the world. + + The application call this function for debugging purpose + See also: ::NewtonWorldGetNextBody, ::NewtonWorldForEachBodyInAABBDo, ::NewtonWorldForEachJointDo +*/ +NewtonBody* NewtonWorldGetFirstBody(const NewtonWorld* const newtonWorld) +{ + Newton* const world = (Newton *) newtonWorld; + dgBodyMasterList& masterList = *world; + + TRACE_FUNCTION(__FUNCTION__); + dgAssert (masterList.GetFirst()->GetInfo().GetBody() == world->GetSentinelBody()); + dgBodyMasterList::dgListNode* const node = masterList.GetFirst()->GetNext(); +// body = node->GetInfo().GetBody(); +// node = node->GetNext(); +// callback ((const NewtonBody*) body); +// } + if (node) { + return (NewtonBody*)node->GetInfo().GetBody(); + } else { + return NULL; + } +} + + +/*! + Get the first body in the general body. + + @param *newtonWorld Pointer to the Newton world. + @param curBody fixme + + @return nothing + + The application can call this function to iterate through every body in the world. + + The application call this function for debugging purpose + + See also: ::NewtonWorldGetFirstBody, ::NewtonWorldForEachBodyInAABBDo, ::NewtonWorldForEachJointDo +*/ +NewtonBody* NewtonWorldGetNextBody(const NewtonWorld* const newtonWorld, const NewtonBody* const curBody) +{ + dgBody* const body = (dgBody*) curBody; + + TRACE_FUNCTION(__FUNCTION__); + + dgBodyMasterList::dgListNode* const node = body->GetMasterList()->GetNext(); + if (node) { + return (NewtonBody*)node->GetInfo().GetBody(); + } else { + return NULL; + } + +} + + +/*! + Trigger callback function for each joint in the world. + + @param *newtonWorld Pointer to the Newton world. + @param callback The callback function to invoke for each joint. + @param *userData User data to pass into the callback. + + @return nothing + + The application should provide the function *NewtonJointIterator callback* to + be called by Newton for every joint in the world. + + Note that this function is primarily for debugging. The performance penalty + for calling it is high. + + See also: ::NewtonWorldForEachBodyInAABBDo, ::NewtonWorldGetFirstBody +*/ +void NewtonWorldForEachJointDo(const NewtonWorld* const newtonWorld, NewtonJointIterator callback, void* const userData) +{ + Newton* const world = (Newton *) newtonWorld; + dgBodyMasterList& masterList = *world; + + TRACE_FUNCTION(__FUNCTION__); + dgTree jointMap(world->dgWorld::GetAllocator()); + for (dgBodyMasterList::dgListNode* node = masterList.GetFirst()->GetNext(); node; node = node->GetNext()) { + dgBodyMasterListRow& row = node->GetInfo(); + for (dgBodyMasterListRow::dgListNode* jointNode = row.GetFirst(); jointNode; jointNode = jointNode->GetNext()) { + const dgBodyMasterListCell& cell = jointNode->GetInfo(); + if (cell.m_joint->GetId() != dgConstraint::m_contactConstraint) { + if (!jointMap.Find(cell.m_joint)) { + jointMap.Insert(cell.m_joint, cell.m_joint); + callback ((const NewtonJoint*) cell.m_joint, userData); + } + } + } + } +} + + +/*! + Trigger a callback for every body that intersects the specified AABB. + + @param *newtonWorld Pointer to the Newton world. + @param *p0 - pointer to an array of at least three floats to hold minimum value for the AABB. + @param *p1 - pointer to an array of at least three floats to hold maximum value for the AABB. + @param callback application defined callback + @param *userData pointer to the user defined user data value. + + @return nothing + + The application should provide the function *NewtonBodyIterator callback* to + be called by Newton for every body in the world. + + For small AABB volumes this function is much more inefficients (fixme: more or + less efficient?) than NewtonWorldGetFirstBody. However, if the AABB contains + the majority of objects in the scene, the overhead of scanning the internal + Broadphase collision plus the AABB test make this function more expensive. + + See also: ::NewtonWorldGetFirstBody +*/ +void NewtonWorldForEachBodyInAABBDo(const NewtonWorld* const newtonWorld, const dFloat* const p0, const dFloat* const p1, NewtonBodyIterator callback, void* const userData) +{ + TRACE_FUNCTION(__FUNCTION__); + + Newton* const world = (Newton *) newtonWorld; + dgVector q0 (dgMin (p0[0], p1[0]), dgMin (p0[1], p1[1]), dgMin (p0[2], p1[2]), dgFloat32 (0.0f)); + dgVector q1 (dgMax (p0[0], p1[0]), dgMax (p0[1], p1[1]), dgMax (p0[2], p1[2]), dgFloat32 (0.0f)); + + world->GetBroadPhase()->ForEachBodyInAABB (q0, q1, (OnBodiesInAABB) callback, userData); +} + + +/*! + Return the current library version number. + + @return version number as an integer, eg 314. + + The version number is a three-digit integer. + + First digit: major version (interface changes among other things) + Second digit: major patch number (new features, and bug fixes) + Third Digit: minor bug fixed patch. +*/ +int NewtonWorldGetVersion() +{ + TRACE_FUNCTION(__FUNCTION__); + return NEWTON_MAJOR_VERSION * 100 + NEWTON_MINOR_VERSION; +} + + +/*! + Return the size of a Newton dFloat in bytes. + + @return sizeof(dFloat) +*/ +int NewtonWorldFloatSize () +{ + TRACE_FUNCTION(__FUNCTION__); + return sizeof (dFloat); +} + + +/*! + Store a user defined data value with the world. + + @param *newtonWorld is the pointer to the newton world. + @param *userData pointer to the user defined user data value. + + @return Nothing. + + The application can attach custom data to the Newton world. Newton will never + look at this data. + + The user data is useful for application developing object oriented classes + based on the Newton API. + + See also: ::NewtonBodyGetUserData, ::NewtonWorldSetUserData, ::NewtonWorldGetUserData +*/ +void NewtonWorldSetUserData(const NewtonWorld* const newtonWorld, void* const userData) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *) newtonWorld; + world->SetUserData (userData); +} + +/*! + Retrieve the user data attached to the world. + + @param *newtonWorld Pointer to the Newton world. + + @return Pointer to user data. + + See also: ::NewtonBodySetUserData, ::NewtonWorldSetUserData, ::NewtonWorldGetUserData + */ +void* NewtonWorldGetUserData(const NewtonWorld* const newtonWorld) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *) newtonWorld; + return world->GetUserData(); +} + +/*! + Specify a custom destructor callback for destroying the world. + + @param *newtonWorld Pointer to the Newton world. + @param destructor function poiter callback + + The application may specify its own world destructor. + + See also: ::NewtonWorldSetDestructorCallback, ::NewtonWorldGetUserData +*/ +void NewtonWorldSetDestructorCallback(const NewtonWorld* const newtonWorld, NewtonWorldDestructorCallback destructor) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *) newtonWorld; + world->m_destructor = destructor; +} + + +/*! + Return pointer to destructor call back function. + + @param *newtonWorld Pointer to the Newton world. + + See also: ::NewtonWorldGetUserData, ::NewtonWorldSetDestructorCallback +*/ +NewtonWorldDestructorCallback NewtonWorldGetDestructorCallback(const NewtonWorld* const newtonWorld) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *) newtonWorld; + return world->m_destructor; +} + +void NewtonWorldSetCreateDestroyContactCallback(const NewtonWorld* const newtonWorld, NewtonCreateContactCallback createContact, NewtonDestroyContactCallback destroyContact) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *)newtonWorld; + world->SetCreateDestroyContactCallback((dgWorld::OnCreateContact) createContact, (dgWorld::OnDestroyContact) destroyContact); +} + +void NewtonWorldSetCollisionConstructorDestructorCallback (const NewtonWorld* const newtonWorld, NewtonCollisionCopyConstructionCallback constructor, NewtonCollisionDestructorCallback destructor) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *) newtonWorld; + world->SetCollisionInstanceConstructorDestructor((dgWorld::OnCollisionInstanceDuplicate) constructor, (dgWorld::OnCollisionInstanceDestroy)destructor); +} + +void* NewtonWorldAddListener(const NewtonWorld* const newtonWorld, const char* const nameId, void* const listenerUserData) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *)newtonWorld; + return world->AddListener(nameId, listenerUserData); +} + +void* NewtonWorldGetListener(const NewtonWorld* const newtonWorld, const char* const nameId) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *)newtonWorld; + return world->FindListener(nameId); +} + +void* NewtonWorldGetListenerUserData (const NewtonWorld* const newtonWorld, void* const listener) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *) newtonWorld; + return world->GetListenerUserData (listener); +} + +NewtonWorldListenerBodyDestroyCallback NewtonWorldListenerGetBodyDestroyCallback (const NewtonWorld* const newtonWorld, void* const listener) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *) newtonWorld; + return (NewtonWorldListenerBodyDestroyCallback) world->GetListenerBodyDestroyCallback (listener); +} + +void NewtonWorldListenerSetBodyDestroyCallback (const NewtonWorld* const newtonWorld, void* const listener, NewtonWorldListenerBodyDestroyCallback callback) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *) newtonWorld; + world->SetListenerBodyDestroyCallback (listener, (dgWorld::OnListenerBodyDestroyCallback) callback); +} + +void NewtonWorldListenerSetDestructorCallback(const NewtonWorld* const newtonWorld, void* const listener, NewtonWorldDestroyListenerCallback callback) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *)newtonWorld; + return world->ListenerSetDestroyCallback(listener, (dgWorld::OnListenerDestroyCallback) callback); +} + +void NewtonWorldListenerSetPreUpdateCallback(const NewtonWorld* const newtonWorld, void* const listener, NewtonWorldUpdateListenerCallback callback) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *)newtonWorld; + return world->ListenerSetPreUpdate(listener, (dgWorld::OnListenerUpdateCallback) callback); +} + +void NewtonWorldListenerSetPostUpdateCallback(const NewtonWorld* const newtonWorld, void* const listener, NewtonWorldUpdateListenerCallback callback) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *)newtonWorld; + return world->ListenerSetPostUpdate(listener, (dgWorld::OnListenerUpdateCallback) callback); +} + +void NewtonWorldListenerSetPostStepCallback (const NewtonWorld* const newtonWorld, void* const listener, NewtonWorldUpdateListenerCallback callback) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *)newtonWorld; + return world->ListenerSetPostStep(listener, (dgWorld::OnListenerUpdateCallback) callback); +} + +void NewtonWorldListenerSetDebugCallback (const NewtonWorld* const newtonWorld, void* const listener, NewtonWorldListenerDebugCallback callback) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *)newtonWorld; + return world->SetListenerBodyDebugCallback (listener, (dgWorld::OnListenerDebugCallback) callback); +} + +void NewtonWorldListenerDebug(const NewtonWorld* const newtonWorld, void* const context) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *)newtonWorld; + return world->ListenersDebug(context); +} + +/*! + Return the total number of rigid bodies in the world. + + @param *newtonWorld Pointer to the Newton world. + + @return Number of rigid bodies in the world. + +*/ +int NewtonWorldGetBodyCount(const NewtonWorld* const newtonWorld) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *) newtonWorld; + return world->GetBodiesCount(); +} + +/*! + Return the total number of constraints in the world. + + @param *newtonWorld pointer to the Newton world. + + @return number of constraints. + +*/ +int NewtonWorldGetConstraintCount(const NewtonWorld* const newtonWorld) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *) newtonWorld; + return world->GetConstraintsCount(); +} + + +/*! + Shoot ray from point p0 to p1 and trigger callback for each body on that line. + + @param *newtonWorld Pointer to the Newton world. + @param *p0 - pointer to an array of at least three floats containing the beginning of the ray in global space. + @param *p1 - pointer to an array of at least three floats containing the end of the ray in global space. + @param filter Callback function for each hit during the ray scan. + @param *userData user data to pass along to the filter callback. + @param prefilter user defined function to be called for each body before intersection. + @param threadIndex Index of thread that called this function (zero if called form outsize a newton update). + + @return nothing + + The ray cast function will trigger the callback for every intersection between + the line segment (from p0 to p1) and a body in the world. + + By writing the callback filter function in different ways the application can + implement different flavors of ray casting. For example an all body ray cast + can be easily implemented by having the filter function always returning 1.0, + and copying each rigid body into an array of pointers; a closest hit ray cast + can be implemented by saving the body with the smaller intersection parameter + and returning the parameter t; and a report the first body hit can be + implemented by having the filter function returning zero after the first call + and saving the pointer to the rigid body. + + The most common use for the ray cast function is the closest body hit, In this + case it is important, for performance reasons, that the filter function + returns the intersection parameter. If the filter function returns a value of + zero the ray cast will terminate immediately. + + if prefilter is not NULL, Newton will call the application right before + executing the intersections between the ray and the primitive. if the function + returns zero the Newton will not ray cast the primitive. passing a NULL + pointer will ray cast the. The application can use this implement faster or + smarter filters when implementing complex logic, otherwise for normal all ray + cast this parameter could be NULL. + + The ray cast function is provided as an utility function, this means that even + thought the function is very high performance by function standards, it can + not by batched and therefore it can not be an incremental function. For + example the cost of calling 1000 ray cast is 1000 times the cost of calling + one ray cast. This is much different than the collision system where the cost + of calculating collision for 1000 pairs in much, much less that the 1000 times + the cost of one pair. Therefore this function must be used with care, as + excessive use of it can degrade performance. + + See also: ::NewtonWorldConvexCast +*/ +void NewtonWorldRayCast(const NewtonWorld* const newtonWorld, const dFloat* const p0, const dFloat* const p1, NewtonWorldRayFilterCallback filter, void* const userData, NewtonWorldRayPrefilterCallback prefilter, int threadIndex) +{ + TRACE_FUNCTION(__FUNCTION__); + if (filter) { + dgVector pp0 (p0[0], p0[1], p0[2], dgFloat32 (0.0f)); + dgVector pp1 (p1[0], p1[1], p1[2], dgFloat32 (0.0f)); + Newton* const world = (Newton *) newtonWorld; + world->GetBroadPhase()->RayCast (pp0, pp1, (OnRayCastAction) filter, (OnRayPrecastAction) prefilter, userData); + } +} + + +/*! + cast a simple convex shape along the ray that goes for the matrix position to the destination and get the firsts contacts of collision. + + @param *newtonWorld Pointer to the Newton world. + @param *matrix pointer to an array of at least three floats containing the beginning and orienetaion of the shape in global space. + @param *target pointer to an array of at least three floats containing the end of the ray in global space. + @param shape collision shap[e use to cat the ray. + @param param pointe to a variable the will contart the time to closet aproah to the collision. + @param *userData user data to be passed to the prefilter callback. + @param prefilter user define function to be called for each body before intersection. + @param *info pointer to an array of contacts at the point of intesections. + @param maxContactsCount maximun number of contacts to be conclaculated, the variable sould be initialized to the capaciaty of *info* + @param threadIndex thread index from whe thsi function is called, zero if call form outsize a newton update + + @return the number of contact at the intesection point (a value equal o lower than maxContactsCount. + variable *hitParam* will be set the uintesation parameter an the momen of impact. + + passing and value of NULL in *info* an dzero in maxContactsCount will turn thos function into a spcial Ray cast + where the function will only calculate the *hitParam* at the momenet of contacts. tshi si one of the most effiecnet way to use thsio function. + + these function is similar to *NewtonWorldRayCast* but instead of casting a point it cast a simple convex shape along a ray for maoprix.m_poit + to target position. the shape is global orientation and position is set to matrix and then is swept along the segment to target and it will stop at the very first intersession contact. + + for case where the application need to cast solid short to medium rays, it is better to use this function instead of casting and array of parallel rays segments. + examples of these are: implementation of ray cast cars with cylindrical tires, foot placement of character controllers, kinematic motion of objects, user controlled continuous collision, etc. + this function may not be as efficient as sampling ray for long segment, for these cases try using parallel ray cast. + + The most common use for the ray cast function is the closest body hit, In this case it is important, for performance reasons, + that the filter function returns the intersection parameter. If the filter function returns a value of zero the ray cast will terminate + immediately. + + if prefilter is not NULL, Newton will call the application right before executing the intersections between the ray and the primitive. + if the function returns zero the Newton will not ray cast the primitive. + The application can use this callback to implement faster or smarter filters when implementing complex logic, otherwise for normal all ray cast + this parameter could be NULL. + + See also: ::NewtonWorldRayCast +*/ +int NewtonWorldConvexCast(const NewtonWorld* const newtonWorld, const dFloat* const matrix, const dFloat* const target, const NewtonCollision* const shape, + dFloat* const param, void* const userData, NewtonWorldRayPrefilterCallback prefilter, NewtonWorldConvexCastReturnInfo* const info, + int maxContactsCount, int threadIndex) +{ + TRACE_FUNCTION(__FUNCTION__); + dgVector destination (target[0], target[1], target[2], dgFloat32 (0.0f)); + Newton* const world = (Newton *) newtonWorld; + return world->GetBroadPhase()->ConvexCast ((dgCollisionInstance*) shape, dgMatrix (matrix), destination, param, (OnRayPrecastAction) prefilter, userData, (dgConvexCastReturnInfo*)info, maxContactsCount, threadIndex); +} + +int NewtonWorldCollide (const NewtonWorld* const newtonWorld, const dFloat* const matrix, const NewtonCollision* const shape, void* const userData, + NewtonWorldRayPrefilterCallback prefilter, NewtonWorldConvexCastReturnInfo* const info, int maxContactsCount, int threadIndex) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *)newtonWorld; + return world->GetBroadPhase()->Collide((dgCollisionInstance*)shape, dgMatrix(matrix), (OnRayPrecastAction)prefilter, userData, (dgConvexCastReturnInfo*)info, maxContactsCount, threadIndex); +} + +NewtonJoint* NewtonWorldFindJoint(const NewtonBody* const body0, const NewtonBody* const body1) +{ + for (NewtonJoint* joint = NewtonBodyGetFirstJoint(body0); joint; joint = NewtonBodyGetNextJoint(body0, joint)) { + if (((body0 == NewtonJointGetBody0(joint)) && (body1 == NewtonJointGetBody1(joint))) || + ((body1 == NewtonJointGetBody0(joint)) && (body0 == NewtonJointGetBody1(joint)))) { + return joint; + } + } + return NULL; +} + +/*! + Retrieve body by index from island. + + @param island Pointer to simulation island. + @param bodyIndex Index of body on current island. + + @return requested body. fixme: does it return NULL on error? + + This function can only be called from an island update callback. + + See also: ::NewtonSetIslandUpdateEvent +*/ +NewtonBody* NewtonIslandGetBody(const void* const island, int bodyIndex) +{ + TRACE_FUNCTION(__FUNCTION__); + dgWorld* const world = *(dgWorld**)island; + return (NewtonBody*)world->GetClusterBody (island, bodyIndex); +} + + +/*! + Return the AABB of the body on this island + + @param island Pointer to simulation island. + @param bodyIndex index to the body in current island. + @param p0 - fixme + @param p1 - fixme + + This function can only be called from an island update callback. + + See also: ::NewtonSetIslandUpdateEvent +*/ +void NewtonIslandGetBodyAABB(const void* const island, int bodyIndex, dFloat* const p0, dFloat* const p1) +{ + TRACE_FUNCTION(__FUNCTION__); + dgBody* const body = (dgBody*) NewtonIslandGetBody(island, bodyIndex); + if (body) { + body->GetAABB ((dgVector&) *p0, (dgVector&) *p1); + } +} + +/*! @} */ // end of group World + +/*! @defgroup GroupID GroupID +GroupID interface +@{ +*/ + + +/*! + Get the value of the default MaterialGroupID. + + @param *newtonWorld pointer to the Newton world. + + @return The ID number for the default Group ID. + + Group IDs can be interpreted as the nodes of a dense graph. The edges of the graph are the physics materials. + When the Newton world is created, the default Group ID is created by the engine. + When bodies are created the application assigns a group ID to the body. +*/ +int NewtonMaterialGetDefaultGroupID(const NewtonWorld* const newtonWorld) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *)newtonWorld; + return int (world->GetDefualtBodyGroupID()); +} + + +/*! + Create a new MaterialGroupID. + + @param *newtonWorld pointer to the Newton world. + + @return The ID of a new GroupID. + + Group IDs can be interpreted as the nodes of a dense graph. The edges of the graph are the physics materials. + When the Newton world is created, the default Group ID is created by the engine. + When bodies are created the application assigns a group ID to the body. + + Note: The only way to destroy a Group ID after its creation is by destroying all the bodies and calling the function *NewtonMaterialDestroyAllGroupID*. + + See also: ::NewtonMaterialDestroyAllGroupID +*/ +int NewtonMaterialCreateGroupID(const NewtonWorld* const newtonWorld) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *)newtonWorld; + return int (world->CreateBodyGroupID()); +} + +/*! + Remove all groups ID from the Newton world. + + @param *newtonWorld pointer to the Newton world. + + @return Nothing. + + This function removes all groups ID from the Newton world. + This function must be called after there are no more rigid bodies in the word. + + See also: ::NewtonDestroyAllBodies +*/ +void NewtonMaterialDestroyAllGroupID(const NewtonWorld* const newtonWorld) +{ +// dgAssert (0); + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *)newtonWorld; + world-> RemoveAllGroupID(); +} + +/*! @} */ // end of GroupID + +/*! @defgroup MaterialSetup MaterialSetup +Material setup interface +@{ +*/ + + +/*! + Set the material interaction between two physics materials to be collidable or non-collidable by default. + + @param *newtonWorld pointer to the Newton world. + @param id0 - group id0 + @param id1 - group id1 + @param state state for this material: 1 = collidable; 0 = non collidable + + @return Nothing. +*/ +void NewtonMaterialSetDefaultCollidable(const NewtonWorld* const newtonWorld, int id0, int id1, int state) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *)newtonWorld; + dgContactMaterial* const material = world->GetMaterial (dgUnsigned32 (id0), dgUnsigned32 (id1)); + if (state) { + material->m_flags |= dgContactMaterial::m_collisionEnable; + } else { + material->m_flags &= ~dgContactMaterial::m_collisionEnable; + } +} + +/*! + Set an imaginary thickness between the collision geometry of two colliding bodies whose physics + properties are defined by this material pair + + @param *newtonWorld pointer to the Newton world. + @param id0 - group id0 + @param id1 - group id1 + @param thickness material thickness a value form 0.0 to 0.125; the default surface value is 0.0 + + @return Nothing. + + when two bodies collide the engine resolve contact inter penetration by applying a small restoring + velocity at each contact point. By default this restoring velocity will stop when the two contacts are + at zero inter penetration distance. However by setting a non zero thickness the restoring velocity will + continue separating the contacts until the distance between the two point of the collision geometry is equal + to the surface thickness. + + Surfaces thickness can improve the behaviors of rolling objects on flat surfaces. + + Surface thickness does not alter the performance of contact calculation. +*/ +void NewtonMaterialSetSurfaceThickness(const NewtonWorld* const newtonWorld, int id0, int id1, dFloat thickness) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *)newtonWorld; + dgContactMaterial* const material = world->GetMaterial (dgUnsigned32 (id0), dgUnsigned32 (id1)); + material->m_skinThickness = dgClamp (thickness, dgFloat32 (0.0f), DG_MAX_COLLISION_AABB_PADDING * dgFloat32 (0.5f)); +} + + +/*! + Set the default coefficients of friction for the material interaction between two physics materials . + + @param *newtonWorld pointer to the Newton world. + @param id0 - group id0 + @param id1 - group id1 + @param staticFriction static friction coefficients + @param kineticFriction dynamic coefficient of friction + + @return Nothing. + + *staticFriction* and *kineticFriction* must be positive values. *kineticFriction* must be lower than *staticFriction*. + It is recommended that *staticFriction* and *kineticFriction* be set to a value lower or equal to 1.0, however because some synthetic materials + can have higher than one coefficient of friction Newton allows for the coefficient of friction to be as high as 2.0. +*/ +void NewtonMaterialSetDefaultFriction(const NewtonWorld* const newtonWorld, int id0, int id1, dFloat staticFriction, dFloat kineticFriction) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *)newtonWorld; + dgContactMaterial* const material = world->GetMaterial (dgUnsigned32 (id0), dgUnsigned32 (id1)); + + staticFriction = dgAbs (staticFriction); + kineticFriction = dgAbs (kineticFriction); + + if (material) { + if (staticFriction >= dgFloat32 (1.0e-2f)) { + dFloat stat0 = dgClamp (staticFriction, dFloat(0.01f), dFloat(2.0f)); + dFloat kine0 = dgClamp (kineticFriction, dFloat(0.01f), dFloat(2.0f)); + dFloat stat = dgMax (stat0, kine0); + dFloat kine = dgMin (stat0, kine0); + material->m_staticFriction0 = stat; + material->m_staticFriction1 = stat; + material->m_dynamicFriction0 = kine; + material->m_dynamicFriction1 = kine; + material->m_flags |= (dgContactMaterial::m_friction0Enable | dgContactMaterial::m_friction1Enable); + } else { + material->m_flags &= ~(dgContactMaterial::m_friction0Enable | dgContactMaterial::m_friction1Enable); + } + } +} + + +/*! + Set the default coefficients of restitution (elasticity) for the material interaction between two physics materials . + + @param *newtonWorld pointer to the Newton world. + @param id0 - group id0 + @param id1 - group id1 + @param elasticCoef static friction coefficients + + @return Nothing. + + *elasticCoef* must be a positive value. + It is recommended that *elasticCoef* be set to a value lower or equal to 1.0 +*/ +void NewtonMaterialSetDefaultElasticity(const NewtonWorld* const newtonWorld, int id0, int id1, dFloat elasticCoef) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *)newtonWorld; + dgContactMaterial* const material = world->GetMaterial (dgUnsigned32 (id0), dgUnsigned32 (id1)); + + material->m_restitution = dgClamp (elasticCoef, dFloat(0.0f), dFloat(2.0f)); +} + + +/*! + Set the default softness coefficients for the material interaction between two physics materials . + + @param *newtonWorld pointer to the Newton world. + @param id0 - group id0 + @param id1 - group id1 + @param softnessCoef softness coefficient + + @return Nothing. + + *softnessCoef* must be a positive value. + It is recommended that *softnessCoef* be set to value lower or equal to 1.0 + A low value for *softnessCoef* will make the material soft. A typical value for *softnessCoef* is 0.15 +*/ +void NewtonMaterialSetDefaultSoftness(const NewtonWorld* const newtonWorld, int id0, int id1, dFloat softnessCoef) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *)newtonWorld; + dgContactMaterial* const material = world->GetMaterial (dgUnsigned32 (id0), dgUnsigned32 (id1)); + + material->m_softness = dgClamp (softnessCoef, dFloat(0.01f), dFloat(dgFloat32(1.0f))); +} + +void NewtonMaterialSetCallbackUserData (const NewtonWorld* const newtonWorld, int id0, int id1, void* const userData) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *)newtonWorld; + dgContactMaterial* const material = world->GetMaterial(dgUnsigned32(id0), dgUnsigned32(id1)); + material->SetUserData(userData); +} + +void NewtonMaterialJointResetIntraJointCollision(const NewtonWorld* const newtonWorld, int id0, int id1) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *)newtonWorld; + dgContactMaterial* const material = world->GetMaterial(dgUnsigned32(id0), dgUnsigned32(id1)); + material->m_flags |= dgContactMaterial::m_resetSkeletonIntraCollision; +} + +void NewtonMaterialJointResetSelftJointCollision(const NewtonWorld* const newtonWorld, int id0, int id1) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *)newtonWorld; + dgContactMaterial* const material = world->GetMaterial(dgUnsigned32(id0), dgUnsigned32(id1)); + material->m_flags |= dgContactMaterial::m_resetSkeletonSelfCollision; +} + + +/*! + Set userData and the functions event handlers for the material interaction between two physics materials . + + @param *newtonWorld Pointer to the Newton world. + @param id0 - group id0. + @param id1 - group id1. + @param *aabbOverlap address of the event function called when the AABB of tow bodyes overlap. This parameter can be NULL. + @param *processCallback address of the event function called for every contact resulting from contact calculation. This parameter can be NULL. + + @return Nothing. + + When the AABB extend of the collision geometry of two bodies overlap, Newton collision system retrieves the material + interaction that defines the behavior between the pair of bodies. The material interaction is collected from a database of materials, + indexed by the material gruopID assigned to the bodies. If the material is tagged as non collidable, + then no action is taken and the simulation continues. + If the material is tagged as collidable, and a *aabbOverlap* was set for this material, then the *aabbOverlap* function is called. + If the function *aabbOverlap* returns 0, no further action is taken for this material (this can be use to ignore the interaction under + certain conditions). If the function *aabbOverlap* returns 1, Newton proceeds to calculate the array of contacts for the pair of + colliding bodies. If the function *processCallback* was set, the application receives a callback for every contact found between the + two colliding bodies. Here the application can perform fine grain control over the behavior of the collision system. For example, + rejecting the contact, making the contact frictionless, applying special effects to the surface etc. + After all contacts are processed and if the function *endCallback* was set, Newton calls *endCallback*. + Here the application can collect information gathered during the contact-processing phase and provide some feedback to the player. + A typical use for the material callback is to play sound effects. The application passes the address of structure in the *userData* along with + three event function callbacks. When the function *aabbOverlap* is called by Newton, the application resets a variable say *maximumImpactSpeed*. + Then for every call to the function *processCallback*, the application compares the impact speed for this contact with the value of + *maximumImpactSpeed*, if the value is larger, then the application stores the new value along with the position, and any other quantity desired. + When the application receives the call to *endCallback* the application plays a 3d sound based in the position and strength of the contact. +*/ +void NewtonMaterialSetCollisionCallback(const NewtonWorld* const newtonWorld, int id0, int id1, NewtonOnAABBOverlap aabbOverlap, NewtonContactsProcess processCallback) +{ + Newton* const world = (Newton *)newtonWorld; + + TRACE_FUNCTION(__FUNCTION__); + dgContactMaterial* const material = world->GetMaterial (dgUnsigned32 (id0), dgUnsigned32 (id1)); + material->SetCollisionCallback ((dgContactMaterial::OnAABBOverlap) aabbOverlap, (dgContactMaterial::OnContactCallback) processCallback); +} + +void NewtonMaterialSetContactGenerationCallback (const NewtonWorld* const newtonWorld, int id0, int id1, NewtonOnContactGeneration contactGeneration) +{ + Newton* const world = (Newton *)newtonWorld; + + TRACE_FUNCTION(__FUNCTION__); + dgContactMaterial* const material = world->GetMaterial(dgUnsigned32(id0), dgUnsigned32(id1)); + material->SetCollisionGenerationCallback ((dgContactMaterial::OnContactGeneration) contactGeneration); +} + +/*! + Set userData and the functions event handlers for the material interaction between two physics materials . + + @param *newtonWorld Pointer to the Newton world. + @param id0 - group id0. + @param id1 - group id1. + @param *compoundAabbOverlap: fixme (can this be NULL?) + + @return Nothing. + + When the AABB extents of the collision geometry of two bodies overlap, the Newton collision system retrieves the material + interaction that defines the behavior between the pair of bodies. The material interaction is collected from a database of materials, + indexed by the material gruopID assigned to the bodies. If the material is tagged as non collidable, + then no action is taken and the simulation continues. + If the material is tagged as collidable, and a *aabbOverlap* was set for this material, then the *aabbOverlap* function is called. + If the function *aabbOverlap* returns 0, no further action is taken for this material (this can be use to ignore the interaction under + certain conditions). If the function *aabbOverlap* returns 1, Newton proceeds to calculate the array of contacts for the pair of + colliding bodies. If the function *processCallback* was set, the application receives a callback for every contact found between the + two colliding bodies. Here the application can perform fine grain control over the behavior of the collision system. For example, + rejecting the contact, making the contact frictionless, applying special effects to the surface etc. + After all contacts are processed and if the function *endCallback* was set, Newton calls *endCallback*. + Here the application can collect information gathered during the contact-processing phase and provide some feedback to the player. + A typical use for the material callback is to play sound effects. The application passes the address of structure in the *userData* along with + three event function callbacks. When the function *aabbOverlap* is called by Newton, the application resets a variable say *maximumImpactSpeed*. + Then for every call to the function *processCallback*, the application compares the impact speed for this contact with the value of + *maximumImpactSpeed*, if the value is larger, then the application stores the new value along with the position, and any other quantity desired. + When the application receives the call to *endCallback* the application plays a 3d sound based in the position and strength of the contact. +*/ +void NewtonMaterialSetCompoundCollisionCallback(const NewtonWorld* const newtonWorld, int id0, int id1, NewtonOnCompoundSubCollisionAABBOverlap compoundAabbOverlap) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *)newtonWorld; + dgContactMaterial* const material = world->GetMaterial (dgUnsigned32 (id0), dgUnsigned32 (id1)); + + material->SetCompoundCollisionCallback ((dgContactMaterial::OnCompoundCollisionPrefilter) compoundAabbOverlap); +} + + +/*! + Get userData associated with this material. + + @param *newtonWorld Pointer to the Newton world. + @param id0 - group id0. + @param id1 - group id1. + + @return Nothing. +*/ +void* NewtonMaterialGetUserData (const NewtonWorld* const newtonWorld, int id0, int id1) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *)newtonWorld; + dgContactMaterial* const material = world->GetMaterial (dgUnsigned32 (id0), dgUnsigned32 (id1)); + + return material->GetUserData(); +} + + +/*! + Get the first Material pair from the material array. + + @param *newtonWorld Pointer to the Newton world. + + @return the first material. + + See also: ::NewtonWorldGetNextMaterial +*/ +NewtonMaterial* NewtonWorldGetFirstMaterial(const NewtonWorld* const newtonWorld) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *)newtonWorld; + return (NewtonMaterial*) world->GetFirstMaterial (); +} + +/*! + Get the next Material pair from the material array. + + @param *newtonWorld Pointer to the Newton world. + @param *material corrent material + + @return next material in material array or NULL if material is the last material in the list. + + See also: ::NewtonWorldGetFirstMaterial +*/ +NewtonMaterial* NewtonWorldGetNextMaterial(const NewtonWorld* const newtonWorld, const NewtonMaterial* const material) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *)newtonWorld; + + return (NewtonMaterial*)world->GetNextMaterial ((dgContactMaterial*) material); +} + +/*! @} */ // end of MaterialSetup + + +/*! @defgroup ContactBehaviour ContactBehaviour +Contact behavior control interface +@{ +*/ + +/*! + Get the userData set by the application when it created this material pair. + + @param materialHandle pointer to a material pair + + @return Application user data. + + This function can only be called from a material callback event handler. + + See also: ::NewtonMaterialSetCollisionCallback +*/ +void* NewtonMaterialGetMaterialPairUserData(const NewtonMaterial* const materialHandle) +{ + TRACE_FUNCTION(__FUNCTION__); + dgContactMaterial* const material = (dgContactMaterial*) materialHandle; + return material->GetUserData(); +} + +/*! + Return the face attribute assigned to this face when for a user defined collision or a Newton collision tree. + + @param materialHandle pointer to a material pair + + @return face attribute for collision trees. Zero if the contact was generated by two convex collisions. + + This function can only be called from a material callback event handler. + + this function can be used by the application to retrieve the face id of a polygon for a collision tree. + + See also: ::NewtonMaterialSetCollisionCallback +*/ +unsigned NewtonMaterialGetContactFaceAttribute(const NewtonMaterial* const materialHandle) +{ + TRACE_FUNCTION(__FUNCTION__); + dgContactMaterial* const material = (dgContactMaterial*) materialHandle; + return (unsigned) (material->m_shapeId1); +} + + +/*! + Calculate the speed of this contact along the normal vector of the contact. + + @param materialHandle pointer to a material pair + + @return Contact speed. A positive value means the contact is repulsive. + + This function can only be called from a material callback event handler. + + See also: ::NewtonMaterialSetCollisionCallback +*/ +dFloat NewtonMaterialGetContactNormalSpeed(const NewtonMaterial* const materialHandle) +{ +// dgAssert (0); + + TRACE_FUNCTION(__FUNCTION__); + dgContactMaterial* const material = (dgContactMaterial*) materialHandle; +// contact = (dgContact*) contactlHandle; + + const dgBody* const body0 = material->m_body0; + const dgBody* const body1 = material->m_body1; + + dgVector p0 (material->m_point - body0->GetPosition()); + dgVector p1 (material->m_point - body1->GetPosition()); + + dgVector v0 (body0->GetVelocity() + body0->GetOmega().CrossProduct(p0)); + dgVector v1 (body1->GetVelocity() + body1->GetOmega().CrossProduct(p1)); + + dgVector dv (v1 - v0); + + dgAssert (material->m_normal.m_w == dgFloat32 (0.0f)); + dFloat speed = dv.DotProduct(material->m_normal).GetScalar(); + return speed; +} + +/*! + Calculate the speed of this contact along the tangent vector of the contact. + + @param materialHandle pointer to a material pair. + @param index index to the tangent vector. This value can be 0 for primary tangent direction or 1 for the secondary tangent direction. + + @return Contact tangent speed. + + This function can only be called from a material callback event handler. + + See also: ::NewtonMaterialSetCollisionCallback +*/ +dFloat NewtonMaterialGetContactTangentSpeed(const NewtonMaterial* const materialHandle, int index) +{ + TRACE_FUNCTION(__FUNCTION__); + dgContactMaterial* const material = (dgContactMaterial*) materialHandle; + + const dgBody* const body0 = material->m_body0; + const dgBody* const body1 = material->m_body1; + + dgVector p0 (material->m_point - body0->GetPosition()); + dgVector p1 (material->m_point - body1->GetPosition()); + + dgVector v0 (body0->GetVelocity() + body0->GetOmega().CrossProduct(p0)); + dgVector v1 (body1->GetVelocity() + body1->GetOmega().CrossProduct(p1)); + + dgVector dv (v1 - v0); + dgVector dir (index ? material->m_dir1 : material->m_dir0); + dgAssert (dir.m_w == dgFloat32 (0.0f)); + dFloat speed = dv.DotProduct(dir).GetScalar(); + return - speed; +} + + +dFloat NewtonMaterialGetContactMaxNormalImpact(const NewtonMaterial* const materialHandle) +{ + TRACE_FUNCTION(__FUNCTION__); + dgContactMaterial* const material = (dgContactMaterial*) materialHandle; + return material->m_normal_Force.m_impact; +} + +dFloat NewtonMaterialGetContactMaxTangentImpact(const NewtonMaterial* const materialHandle, int index) +{ + TRACE_FUNCTION(__FUNCTION__); + + dgContactMaterial* const material = (dgContactMaterial*) materialHandle; + return index ? material->m_dir1_Force.m_impact: material->m_dir0_Force.m_impact; +} + + +/*! + Get the contact position and normal in global space. + + @param materialHandle pointer to a material pair. + @param *body pointer to body + @param *positPtr pointer to an array of at least three floats to hold the contact position. + @param *normalPtr pointer to an array of at least three floats to hold the contact normal. + + @return Nothing. + + This function can only be called from a material callback event handle. + + See also: ::NewtonMaterialSetCollisionCallback +*/ +void NewtonMaterialGetContactPositionAndNormal(const NewtonMaterial* const materialHandle, const NewtonBody* const body, dFloat* const positPtr, dFloat* const normalPtr) +{ + TRACE_FUNCTION(__FUNCTION__); + dgContactMaterial* const material = (dgContactMaterial*) materialHandle; + + positPtr[0] = material->m_point.m_x; + positPtr[1] = material->m_point.m_y; + positPtr[2] = material->m_point.m_z; + + normalPtr[0] = material->m_normal.m_x; + normalPtr[1] = material->m_normal.m_y; + normalPtr[2] = material->m_normal.m_z; + + if ((dgBody*)body != material->m_body0) { + normalPtr[0] *= dgFloat32 (-1.0f); + normalPtr[1] *= dgFloat32 (-1.0f); + normalPtr[2] *= dgFloat32 (-1.0f); + } +} + + + +/*! + Get the contact force vector in global space. + + @param materialHandle pointer to a material pair. + @param *body pointer to body + @param *forcePtr pointer to an array of at least three floats to hold the force vector in global space. + + @return Nothing. + + The contact force value is only valid when calculating resting contacts. This means if two bodies collide with + non zero relative velocity, the reaction force will be an impulse, which is not a reaction force, this will return zero vector. + this function will only return meaningful values when the colliding bodies are at rest. + + This function can only be called from a material callback event handler. + + See also: ::NewtonMaterialSetCollisionCallback +*/ +void NewtonMaterialGetContactForce(const NewtonMaterial* const materialHandle, const NewtonBody* const body, dFloat* const forcePtr) +{ + + TRACE_FUNCTION(__FUNCTION__); + dgContactMaterial* const material = (dgContactMaterial*) materialHandle; + + dgVector force (material->m_normal.Scale(material->m_normal_Force.m_force) + material->m_dir0.Scale (material->m_dir0_Force.m_force) + material->m_dir1.Scale (material->m_dir1_Force.m_force)); + + forcePtr[0] = force.m_x; + forcePtr[1] = force.m_y; + forcePtr[2] = force.m_z; + + if ((dgBody*)body != material->m_body0) { + forcePtr[0] *= dgFloat32 (-1.0f); + forcePtr[1] *= dgFloat32 (-1.0f); + forcePtr[2] *= dgFloat32 (-1.0f); + } +} + + + +/*! + Get the contact tangent vector to the contact point. + + @param materialHandle pointer to a material pair. + @param *body pointer to body + @param *dir0 - pointer to an array of at least three floats to hold the contact primary tangent vector. + @param *dir1 - pointer to an array of at least three floats to hold the contact secondary tangent vector. + + @return Nothing. + + This function can only be called from a material callback event handler. + + See also: ::NewtonMaterialSetCollisionCallback +*/ +void NewtonMaterialGetContactTangentDirections(const NewtonMaterial* const materialHandle, const NewtonBody* const body, dFloat* const dir0, dFloat* const dir1) +{ + TRACE_FUNCTION(__FUNCTION__); + dgContactMaterial* const material = (dgContactMaterial*) materialHandle; + + dir0[0] = material->m_dir0.m_x; + dir0[1] = material->m_dir0.m_y; + dir0[2] = material->m_dir0.m_z; + + dir1[0] = material->m_dir1.m_x; + dir1[1] = material->m_dir1.m_y; + dir1[2] = material->m_dir1.m_z; + + if ((dgBody*)body != material->m_body0) { + dir0[0] *= dgFloat32 (-1.0f); + dir0[1] *= dgFloat32 (-1.0f); + dir0[2] *= dgFloat32 (-1.0f); + + dir1[0] *= dgFloat32 (-1.0f); + dir1[1] *= dgFloat32 (-1.0f); + dir1[2] *= dgFloat32 (-1.0f); + } +} + +dFloat NewtonMaterialGetContactPenetration (const NewtonMaterial* const materialHandle) +{ + TRACE_FUNCTION(__FUNCTION__); + dgContactMaterial* const material = (dgContactMaterial*) materialHandle; + return material->m_penetration; +} + +NewtonCollision* NewtonMaterialGetBodyCollidingShape(const NewtonMaterial* const materialHandle, const NewtonBody* const body) +{ + TRACE_FUNCTION(__FUNCTION__); + dgBody* const bodyPtr = (dgBody*) body; + dgContactMaterial* const material = (dgContactMaterial*) materialHandle; + + const dgCollisionInstance* collision = material->m_collision0; + if (bodyPtr == material->m_body1) { + collision = material->m_collision1; + } + return (NewtonCollision*) collision; +} + + +//dFloat NewtonMaterialGetContactPruningTolerance(const NewtonBody* const body0, const NewtonBody* const body1) +dFloat NewtonMaterialGetContactPruningTolerance(const NewtonJoint* const contactJointPtr) +{ + TRACE_FUNCTION(__FUNCTION__); + dgContact* const contact = (dgContact*) contactJointPtr; + return contact->GetPruningTolerance (); +} + +void NewtonMaterialSetContactPruningTolerance(const NewtonJoint* const contactJointPtr, dFloat tolerance) +{ + TRACE_FUNCTION(__FUNCTION__); + TRACE_FUNCTION(__FUNCTION__); + dgContact* const contact = (dgContact*) contactJointPtr; + dgAssert(contact); + contact->SetPruningTolerance (dgMax (tolerance, dFloat (1.0e-3f))); +} + + +/*! + Override the default softness value for the contact. + + @param materialHandle pointer to a material pair. + @param softness softness value, must be positive. + + @return Nothing. + + This function can only be called from a material callback event handler. + + See also: ::NewtonMaterialSetCollisionCallback, ::NewtonMaterialSetDefaultSoftness +*/ +void NewtonMaterialSetContactSoftness(const NewtonMaterial* const materialHandle, dFloat softness) +{ + TRACE_FUNCTION(__FUNCTION__); + dgContactMaterial* const material = (dgContactMaterial*) materialHandle; + material->m_softness = dgClamp (softness, dFloat(0.01f), dFloat(0.7f)); +} + +/*! + Override the default contact skin thickness value for the contact. + + @param materialHandle pointer to a material pair. + @param thickness skin thickness value, must be positive. + + @return Nothing. + + This function can only be called from a material callback event handler. + + See also: ::NewtonMaterialSetCollisionCallback, ::NewtonMaterialSetDefaultSoftness +*/ +void NewtonMaterialSetContactThickness (const NewtonMaterial* const materialHandle, dFloat thickness) +{ + TRACE_FUNCTION(__FUNCTION__); + dgAssert (thickness >= dgFloat32 (0.0f)); + dgContactMaterial* const material = (dgContactMaterial*)materialHandle; + material->m_skinThickness = thickness; +} + +/*! + Override the default elasticity (coefficient of restitution) value for the contact. + + @param materialHandle pointer to a material pair. + @param restitution elasticity value, must be positive. + + @return Nothing. + + This function can only be called from a material callback event handler. + + See also: ::NewtonMaterialSetCollisionCallback, ::NewtonMaterialSetDefaultElasticity +*/ +void NewtonMaterialSetContactElasticity(const NewtonMaterial* const materialHandle, dFloat restitution) +{ + TRACE_FUNCTION(__FUNCTION__); + dgContactMaterial* const material = (dgContactMaterial*) materialHandle; + material->m_restitution = dgClamp (restitution, dFloat(0.0f), dFloat(2.0f)); +} + + +/*! + Enable or disable friction calculation for this contact. + + @param materialHandle pointer to a material pair. + @param state* new state. 0 makes the contact frictionless along the index tangent vector. + @param index index to the tangent vector. 0 for primary tangent vector or 1 for the secondary tangent vector. + + @return Nothing. + + This function can only be called from a material callback event handler. + + See also: ::NewtonMaterialSetCollisionCallback +*/ +void NewtonMaterialSetContactFrictionState(const NewtonMaterial* const materialHandle, int state, int index) +{ + TRACE_FUNCTION(__FUNCTION__); + dgContactMaterial* const material = (dgContactMaterial*) materialHandle; + + if (index) { + if (state) { + material->m_flags |= dgContactMaterial::m_friction1Enable; + } else { + material->m_flags &= ~dgContactMaterial::m_friction1Enable; + } + } else { + if (state) { + material->m_flags |= dgContactMaterial::m_friction0Enable; + } else { + material->m_flags &= ~dgContactMaterial::m_friction0Enable; + } + } +} + + + +/*! + Override the default value of the kinetic and static coefficient of friction for this contact. + + @param materialHandle pointer to a material pair. + @param staticFrictionCoef static friction coefficient. Must be positive. + @param kineticFrictionCoef static friction coefficient. Must be positive. + @param index index to the tangent vector. 0 for primary tangent vector or 1 for the secondary tangent vector. + + @return Nothing. + + This function can only be called from a material callback event handler. + + It is recommended that *coef* be set to a value lower or equal to 1.0, however because some synthetic materials + can have hight than one coefficient of friction Newton allows for the coefficient of friction to be as high as 2.0. + + the value *staticFrictionCoef* and *kineticFrictionCoef* will be clamped between 0.01f and 2.0. + If the application wants to set a kinetic friction higher than the current static friction it must increase the static friction first. + + See also: ::NewtonMaterialSetCollisionCallback, ::NewtonMaterialSetDefaultFriction +*/ +void NewtonMaterialSetContactFrictionCoef(const NewtonMaterial* const materialHandle, dFloat staticFrictionCoef, dFloat kineticFrictionCoef, int index) +{ + TRACE_FUNCTION(__FUNCTION__); + dgContactMaterial* const material = (dgContactMaterial*) materialHandle; + + if (staticFrictionCoef < kineticFrictionCoef) { + staticFrictionCoef = kineticFrictionCoef; + } + + if (index) { + material->m_staticFriction1 = dgClamp (staticFrictionCoef, dFloat(0.01f), dFloat(2.0f)); + material->m_dynamicFriction1 = dgClamp (kineticFrictionCoef, dFloat(0.01f), dFloat(2.0f)); + } else { + material->m_staticFriction0 = dgClamp (staticFrictionCoef, dFloat(0.01f), dFloat(2.0f)); + material->m_dynamicFriction0 = dgClamp (kineticFrictionCoef, dFloat(0.01f), dFloat(2.0f)); + } +} + +/*! + Force the contact point to have a non-zero acceleration aligned this the contact normal. + + @param materialHandle pointer to a material pair. + @param accel desired contact acceleration, Must be a positive value + + @return Nothing. + + This function can only be called from a material callback event handler. + + This function can be used for spacial effects like implementing jump, of explosive contact in a call back. + + See also: ::NewtonMaterialSetCollisionCallback +*/ +void NewtonMaterialSetContactNormalAcceleration(const NewtonMaterial* const materialHandle, dFloat accel) +{ + TRACE_FUNCTION(__FUNCTION__); + dgContactMaterial* const material = (dgContactMaterial*) materialHandle; + material->m_normal_Force.m_force = accel; + material->m_flags |= dgContactMaterial::m_overrideNormalAccel; +} + +void NewtonMaterialSetAsSoftContact (const NewtonMaterial* const materialHandle, dFloat relaxation) +{ + TRACE_FUNCTION(__FUNCTION__); + dgContactMaterial* const material = (dgContactMaterial*) materialHandle; + material->SetAsSoftContact (relaxation); +} + +/*! + Force the contact point to have a non-zero acceleration along the surface plane. + + @param materialHandle pointer to a material pair. + @param accel desired contact acceleration. + @param index index to the tangent vector. 0 for primary tangent vector or 1 for the secondary tangent vector. + + @return Nothing. + + This function can only be called from a material callback event handler. + + See also: ::NewtonMaterialSetCollisionCallback, ::NewtonMaterialContactRotateTangentDirections +*/ +void NewtonMaterialSetContactTangentAcceleration(const NewtonMaterial* const materialHandle, dFloat accel, int index) +{ + TRACE_FUNCTION(__FUNCTION__); + dgContactMaterial* const material = (dgContactMaterial*) materialHandle; + if (index) { + material->m_dir1_Force.m_force = accel; + material->m_flags |= dgContactMaterial::m_override1Accel; + } else { + material->m_dir0_Force.m_force = accel; + material->m_flags |= dgContactMaterial::m_override0Accel; + } +} + +void NewtonMaterialSetContactTangentFriction (const NewtonMaterial* const materialHandle, dFloat friction, int index) +{ + TRACE_FUNCTION(__FUNCTION__); + dgContactMaterial* const material = (dgContactMaterial*) materialHandle; + friction = dgMax (dFloat(0.01f), dgAbs (friction)); + if (index) { + material->m_flags |= dgContactMaterial::m_override1Friction; + dgAssert (index == 1); + material->m_staticFriction1 = friction; + material->m_dynamicFriction1 = friction; + } else { + material->m_flags |= dgContactMaterial::m_override0Friction; + material->m_staticFriction0 = friction; + material->m_dynamicFriction0 = friction; + } +} + +/*! + Set the new direction of the for this contact point. + + @param materialHandle pointer to a material pair. + @param *direction pointer to an array of at least three floats holding the direction vector. + + @return Nothing. + + This function can only be called from a material callback event handler. + This function changes the basis of the contact point to one where the contact normal is aligned to the new direction vector + and the tangent direction are recalculated to be perpendicular to the new contact normal. + + In 99.9% of the cases the collision system can calculates a very good contact normal. + however this algorithm that calculate the contact normal use as criteria the normal direction + that will resolve the inter penetration with the least amount on motion. + There are situations however when this solution is not the best. Take for example a rolling + ball over a tessellated floor, when the ball is over a flat polygon, the contact normal is always + perpendicular to the floor and pass by the origin of the sphere, however when the sphere is going + across two adjacent polygons, the contact normal is now perpendicular to the polygons edge and this does + not guarantee they it will pass bay the origin of the sphere, but we know that the best normal is always + the one passing by the origin of the sphere. + + See also: ::NewtonMaterialSetCollisionCallback, ::NewtonMaterialContactRotateTangentDirections +*/ +void NewtonMaterialSetContactNormalDirection(const NewtonMaterial* const materialHandle, const dFloat* const direction) +{ + TRACE_FUNCTION(__FUNCTION__); + dgContactMaterial* const material = (dgContactMaterial*) materialHandle; + dgVector normal (direction[0], direction[1], direction[2], dgFloat32 (0.0f)); + + //dgAssert (normal.DotProduct3(material->m_normal) > dgFloat32 (0.01f)); + if (normal.DotProduct(material->m_normal).GetScalar() < dgFloat32 (0.0f)) { + normal = normal * dgVector::m_negOne; + } + material->m_normal = normal; + + dgMatrix matrix (normal); + material->m_dir1 = matrix.m_up; + material->m_dir0 = matrix.m_right; +} + +void NewtonMaterialSetContactPosition(const NewtonMaterial* const materialHandle, const dFloat* const position) +{ + TRACE_FUNCTION(__FUNCTION__); + dgContactMaterial* const material = (dgContactMaterial*)materialHandle; + dgVector point(position[0], position[1], position[2], dgFloat32(1.0f)); + material->m_point = point; +} + + +/*! + Rotate the tangent direction of the contacts until the primary direction is aligned with the alignVector. + + @param *materialHandle pointer to a material pair. + @param *alignVector pointer to an array of at least three floats holding the aligning vector. + + @return Nothing. + + This function can only be called from a material callback event handler. + This function rotates the tangent vectors of the contact point until the primary tangent vector and the align vector + are perpendicular (ex. when the dot product between the primary tangent vector and the alignVector is 1.0). This + function can be used in conjunction with NewtonMaterialSetContactTangentAcceleration in order to + create special effects. For example, conveyor belts, cheap low LOD vehicles, slippery surfaces, etc. + + See also: ::NewtonMaterialSetCollisionCallback, ::NewtonMaterialSetContactNormalDirection +*/ +void NewtonMaterialContactRotateTangentDirections(const NewtonMaterial* const materialHandle, const dFloat* const alignVector) +{ + TRACE_FUNCTION(__FUNCTION__); + dgContactMaterial* const material = (dgContactMaterial*) materialHandle; + + const dgVector dir0 (alignVector[0], alignVector[1], alignVector[2], dgFloat32 (0.0f)); + + dgVector dir1 (material->m_normal.CrossProduct(dir0)); + dgAssert(dir1.m_w == dgFloat32(0.0f)); + dFloat mag2 = dir1.DotProduct(dir1).GetScalar(); + if (mag2 > dgFloat32(1.0e-6f)) { + material->m_dir1 = dir1.Normalize(); + material->m_dir0 = material->m_dir1.CrossProduct(material->m_normal); + } +} + +/*! @} */ // end of ContactBehaviour + +/*! @defgroup CshapesConvexSimple CshapesConvexSimple +Convex collision primitives interface +@{ +*/ + + +/*! + Create a transparent collision primitive. + + @param *newtonWorld Pointer to the Newton world. + + @return Pointer to the collision object. + + Some times the application needs to create helper rigid bodies that will never collide with other bodies, + for example the neck of a rag doll, or an internal part of an articulated structure. This can be done by using the material system + but it too much work and it will increase unnecessarily the material count, and therefore the project complexity. The Null collision + is a collision object that satisfy all this conditions without having to change the engine philosophy. + +*/ +NewtonCollision* NewtonCreateNull(const NewtonWorld* const newtonWorld) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *)newtonWorld; + return (NewtonCollision*) world->CreateNull(); +} + + +/*! + Create a box primitive for collision. + + @param *newtonWorld Pointer to the Newton world. + @param dx box side one x dimension. + @param dy box side one y dimension. + @param dz box side one z dimension. + @param shapeID fixme + @param *offsetMatrix pointer to an array of 16 floats containing the offset matrix of the box relative to the body. If this parameter is NULL, then the primitive is centered at the origin of the body. + + @return Pointer to the box + +*/ +NewtonCollision* NewtonCreateBox(const NewtonWorld* const newtonWorld, dFloat dx, dFloat dy, dFloat dz, int shapeID, const dFloat* const offsetMatrix) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *)newtonWorld; + dgMatrix matrix (dgGetIdentityMatrix()); + if (offsetMatrix) { + matrix = dgMatrix (offsetMatrix); + } + return (NewtonCollision*) world->CreateBox (dx, dy, dz, shapeID, matrix); +} + +/*! + Create a generalized ellipsoid primitive.. + + @param *newtonWorld Pointer to the Newton world. + @param radius sphere radius + @param shapeID user specified collision index that can be use for multi material collision. + @param *offsetMatrix pointer to an array of 16 floats containing the offset matrix of the sphere relative to the body. If this parameter is NULL then the sphere is centered at the origin of the body. + + @return Pointer to the generalized sphere. + + Sphere collision are generalized ellipsoids, the application can create many different kind of objects by just playing with dimensions of the radius. + for example to make a sphere set all tree radius to the same value, to make a ellipse of revolution just set two of the tree radius to the same value. + + General ellipsoids are very good hull geometries to represent the outer shell of avatars in a game. + +*/ +NewtonCollision* NewtonCreateSphere(const NewtonWorld* const newtonWorld, dFloat radius, int shapeID, const dFloat* const offsetMatrix) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *)newtonWorld; + dgMatrix matrix (dgGetIdentityMatrix()); + if (offsetMatrix) { + matrix = dgMatrix (offsetMatrix); + } + + return (NewtonCollision*) world->CreateSphere(dgAbs(radius), shapeID, matrix); +} + + +/*! + Create a cone primitive for collision. + + @param *newtonWorld Pointer to the Newton world. + @param radius cone radius at the base. + @param height cone height along the x local axis from base to tip. + @param shapeID user specified collision index that can be use for multi material collision. + @param *offsetMatrix pointer to an array of 16 floats containing the offset matrix of the box relative to the body. If this parameter is NULL, then the primitive is centered at the origin of the body. + + @return Pointer to the box + +*/ +NewtonCollision* NewtonCreateCone(const NewtonWorld* const newtonWorld, dFloat radius, dFloat height, int shapeID, const dFloat* const offsetMatrix) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *)newtonWorld; + dgMatrix matrix (dgGetIdentityMatrix()); + if (offsetMatrix) { + matrix = dgMatrix (offsetMatrix); + } + return (NewtonCollision*) world->CreateCone (radius, height, shapeID, matrix); +} + + +/*! + Create a capsule primitive for collision. + + @param *newtonWorld Pointer to the Newton world. + @param radio0 - fixme + @param radio1 - fixme + @param height capsule height along the x local axis from tip to tip. + @param shapeID fixme + @param *offsetMatrix pointer to an array of 16 floats containing the offset matrix of the box relative to the body. If this parameter is NULL, then the primitive is centered at the origin of the body. + + @return Pointer to the box + + the capsule height must equal of larger than the sum of the cap radius. If this is not the case the height will be clamped the 2 * radius. + +*/ +NewtonCollision* NewtonCreateCapsule(const NewtonWorld* const newtonWorld, dFloat radio0, dFloat radio1, dFloat height, int shapeID, const dFloat* const offsetMatrix) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *)newtonWorld; + dgMatrix matrix(dgGetIdentityMatrix()); + if (offsetMatrix) { + matrix = dgMatrix(offsetMatrix); + } + return (NewtonCollision*)world->CreateCapsule(radio0, radio1, height, shapeID, matrix); +} + + +/*! + Create a cylinder primitive for collision. + + @param *newtonWorld Pointer to the Newton world. + @param radio0 - fixme + @param radio1 - fixme + @param height cylinder height along the x local axis. + @param shapeID fixme + @param *offsetMatrix pointer to an array of 16 floats containing the offset matrix of the box relative to the body. If this parameter is NULL, then the primitive is centered at the origin of the body. + + @return Pointer to the box + +*/ +NewtonCollision* NewtonCreateCylinder(const NewtonWorld* const newtonWorld, dFloat radio0, dFloat radio1, dFloat height, int shapeID, const dFloat* const offsetMatrix) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *)newtonWorld; + dgMatrix matrix(dgGetIdentityMatrix()); + if (offsetMatrix) { + matrix = dgMatrix(offsetMatrix); + } + return (NewtonCollision*)world->CreateCylinder(radio0, radio1, height, shapeID, matrix); +} + + +/*! + Create a ChamferCylinder primitive for collision. + + @param *newtonWorld Pointer to the Newton world. + @param radius ChamferCylinder radius at the base. + @param height ChamferCylinder height along the x local axis. + @param shapeID fixme + @param *offsetMatrix pointer to an array of 16 floats containing the offset matrix of the box relative to the body. If this parameter is NULL, then the primitive is centered at the origin of the body. + + @return Pointer to the box + +*/ +NewtonCollision* NewtonCreateChamferCylinder(const NewtonWorld* const newtonWorld, dFloat radius, dFloat height, int shapeID, const dFloat* const offsetMatrix) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *)newtonWorld; + dgMatrix matrix (dgGetIdentityMatrix()); + if (offsetMatrix) { + matrix = dgMatrix (offsetMatrix); + } + return (NewtonCollision*) world->CreateChamferCylinder (radius, height, shapeID, matrix); +} + + +/*! + Create a ConvexHull primitive from collision from a cloud of points. + + @param *newtonWorld Pointer to the Newton world. + @param count number of consecutive point to follow must be at least 4. + @param *vertexCloud pointer to and array of point. + @param strideInBytes vertex size in bytes, must be at least 12. + @param tolerance tolerance value for the hull generation. + @param shapeID fixme + @param *offsetMatrix pointer to an array of 16 floats containing the offset matrix of the box relative to the body. If this parameter is NULL, then the primitive is centered at the origin of the body. + + @return Pointer to the collision mesh, NULL if the function fail to generate convex shape + + Convex hulls are the solution to collision primitive that can not be easily represented by an implicit solid. + The implicit solid primitives (spheres, cubes, cylinders, capsules, cones, etc.), have constant time complexity for contact calculation + and are also extremely efficient on memory usage, therefore the application get perfect smooth behavior. + However for cases where the shape is too difficult or a polygonal representation is desired, convex hulls come closest to the to the model shape. + For example it is a mistake to model a 10000 point sphere as a convex hull when the perfect sphere is available, but it is better to represent a + pyramid by a convex hull than with a sphere or a box. + + There is not upper limit as to how many vertex the application can pass to make a hull shape, + however for performance and memory usage concern it is the application responsibility to keep the max vertex at the possible minimum. + The minimum number of vertex should be equal or larger than 4 and it is the application responsibility that the points are part of a solid geometry. + Unpredictable results will occur if all points happen to be collinear or coplanar. + + The performance of collision with convex hull proxies is sensitive to the vertex count of the hull. Since a the convex hull + of a visual geometry is already an approximation of the mesh, for visual purpose there is not significant difference between the + appeal of a exact hull and one close to the exact hull but with but with a smaller vertex count. + It just happens that sometime complex meshes lead to generation of convex hulls with lots of small detail that play not + roll of the quality of the simulation but that have a significant impact on the performance because of a large vertex count. + For this reason the application have the option to set a *tolerance* parameter. + *tolerance* is use to post process the final geometry in the following faction, a point on the surface of the hull can + be remove if the distance of all of the surrounding vertex immediately adjacent to the average plane equation formed the + faces adjacent to that point, is smaller than the tolerance. A value of zero in *tolerance* will generate an exact hull and a value langer that zero + will generate a loosely fitting hull and it willbe faster to generate. + +*/ +NewtonCollision* NewtonCreateConvexHull(const NewtonWorld* const newtonWorld, int count, const dFloat* const vertexCloud, int strideInBytes, dgFloat32 tolerance, int shapeID, const dFloat* const offsetMatrix) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *)newtonWorld; + dgMatrix matrix (dgGetIdentityMatrix()); + if (offsetMatrix) { + matrix = dgMatrix (offsetMatrix); + } + tolerance = dgClamp (tolerance, dgFloat32 (0.0f), dgFloat32 (0.125f)); + return (NewtonCollision*) world->CreateConvexHull (count, vertexCloud, strideInBytes, tolerance, shapeID, matrix); +} + + +/*! + Create a ConvexHull primitive from a special effect mesh. + + @param *newtonWorld Pointer to the Newton world. + @param *mesh special effect mesh + @param tolerance tolerance value for the hull generation. + @param shapeID fixme + + @return Pointer to the collision mesh, NULL if the function fail to generate convex shape + + Because the in general this function is used for runtime special effect like debris and or solid particles + it is recommended that the source mesh complexity is kept small. + + See also: ::NewtonCreateConvexHull, ::NewtonMeshCreate +*/ +NewtonCollision* NewtonCreateConvexHullFromMesh(const NewtonWorld* const newtonWorld, const NewtonMesh* const mesh, dFloat tolerance, int shapeID) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *)newtonWorld; + dgMeshEffect* const meshEffect = (dgMeshEffect*) mesh; + return (NewtonCollision*) meshEffect->CreateConvexCollision(world, tolerance, shapeID); +} + + +/*! + Create a container to hold an array of convex collision primitives. + + @param *newtonWorld Pointer to the Newton world. + @param shapeID: fixme + + @return Pointer to the compound collision. + + Compound collision primitives can only be made of convex collision primitives and they can not contain compound collision. Therefore they are treated as convex primitives. + + Compound collision primitives are treated as instance collision objects that can not shared by multiples rigid bodies. + +*/ +NewtonCollision* NewtonCreateCompoundCollision(const NewtonWorld* const newtonWorld, int shapeID) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *)newtonWorld; + dgCollisionInstance* const collision = world->CreateCompound (); + collision->SetUserDataID(dgUnsigned32 (shapeID)); + return (NewtonCollision*) collision; +} + +void* NewtonCompoundCollisionAddSubCollision (NewtonCollision* const compoundCollision, const NewtonCollision* const convexCollision) +{ + TRACE_FUNCTION(__FUNCTION__); + + dgCollisionInstance* const compoundInstance = (dgCollisionInstance*) compoundCollision; + dgCollisionInstance* const compoundInstanceChild = (dgCollisionInstance*) convexCollision; + if (compoundInstance->IsType (dgCollision::dgCollisionCompound_RTTI) && compoundInstanceChild->IsType(dgCollision::dgCollisionConvexShape_RTTI)) { + dgCollisionCompound* const collision = (dgCollisionCompound*) compoundInstance->GetChildShape(); + return collision->AddCollision (compoundInstanceChild); + } + return NULL; +} + + +void NewtonCompoundCollisionRemoveSubCollision (NewtonCollision* const compoundCollision, const void* const collisionNode) +{ + TRACE_FUNCTION(__FUNCTION__); + dgCollisionInstance* const instance = (dgCollisionInstance*) compoundCollision; + if (instance->IsType (dgCollision::dgCollisionCompound_RTTI)) { + dgCollisionCompound* const collision = (dgCollisionCompound*) instance->GetChildShape(); + dgCollisionInstance* const childCollision = collision->GetCollisionFromNode((dgCollisionCompound::dgTreeArray::dgTreeNode*)collisionNode); + if (childCollision && childCollision->IsType(dgCollision::dgCollisionConvexShape_RTTI)) { + collision->RemoveCollision ((dgCollisionCompound::dgTreeArray::dgTreeNode*)collisionNode); + } + } +} + +void NewtonCompoundCollisionRemoveSubCollisionByIndex (NewtonCollision* const compoundCollision, int nodeIndex) +{ + TRACE_FUNCTION(__FUNCTION__); + dgCollisionInstance* const instance = (dgCollisionInstance*) compoundCollision; + if (instance->IsType (dgCollision::dgCollisionCompound_RTTI)) { + dgCollisionCompound* const collision = (dgCollisionCompound*) instance->GetChildShape(); + NewtonCompoundCollisionRemoveSubCollision (compoundCollision, collision->FindNodeByIndex(nodeIndex)); + } +} + + +void NewtonCompoundCollisionSetSubCollisionMatrix (NewtonCollision* const compoundCollision, const void* const collisionNode, const dFloat* const matrix) +{ + TRACE_FUNCTION(__FUNCTION__); + dgCollisionInstance* const compoundInstance = (dgCollisionInstance*) compoundCollision; + if (compoundInstance->IsType (dgCollision::dgCollisionCompound_RTTI)) { + dgCollisionCompound* const collision = (dgCollisionCompound*) compoundInstance->GetChildShape(); + collision->SetCollisionMatrix((dgCollisionCompound::dgTreeArray::dgTreeNode*)collisionNode, dgMatrix(matrix)); + } +} + + +void NewtonCompoundCollisionBeginAddRemove (NewtonCollision* const compoundCollision) +{ + TRACE_FUNCTION(__FUNCTION__); + dgCollisionInstance* const instance = (dgCollisionInstance*) compoundCollision; + if (instance->IsType (dgCollision::dgCollisionCompound_RTTI)) { + dgCollisionCompound* const collision = (dgCollisionCompound*) instance->GetChildShape(); + collision->BeginAddRemove(); + } +} + +void NewtonCompoundCollisionEndAddRemove (NewtonCollision* const compoundCollision) +{ + TRACE_FUNCTION(__FUNCTION__); + dgCollisionInstance* const instance = (dgCollisionInstance*) compoundCollision; + if (instance->IsType (dgCollision::dgCollisionCompound_RTTI)) { + dgCollisionCompound* const collision = (dgCollisionCompound*) instance->GetChildShape(); + collision->EndAddRemove(); + } +} + + +void* NewtonCompoundCollisionGetFirstNode (NewtonCollision* const compoundCollision) +{ + TRACE_FUNCTION(__FUNCTION__); + dgCollisionInstance* const instance = (dgCollisionInstance*) compoundCollision; + if (instance->IsType (dgCollision::dgCollisionCompound_RTTI)) { + dgCollisionCompound* const collision = (dgCollisionCompound*) instance->GetChildShape(); + return collision->GetFirstNode(); + } + return NULL; +} + +void* NewtonCompoundCollisionGetNextNode (NewtonCollision* const compoundCollision, const void* const node) +{ + TRACE_FUNCTION(__FUNCTION__); + dgCollisionInstance* const instance = (dgCollisionInstance*) compoundCollision; + if (instance->IsType (dgCollision::dgCollisionCompound_RTTI)) { + dgCollisionCompound* const collision = (dgCollisionCompound*) instance->GetChildShape(); + return collision->GetNextNode((dgCollisionCompound::dgTreeArray::dgTreeNode*)node); + } + return NULL; +} + +void* NewtonCompoundCollisionGetNodeByIndex (NewtonCollision* const compoundCollision, int index) +{ + TRACE_FUNCTION(__FUNCTION__); + dgCollisionInstance* const instance = (dgCollisionInstance*) compoundCollision; + if (instance->IsType (dgCollision::dgCollisionCompound_RTTI)) { + dgCollisionCompound* const collision = (dgCollisionCompound*) instance->GetChildShape(); + return collision->FindNodeByIndex(index); + } + return NULL; +} + +int NewtonCompoundCollisionGetNodeIndex (NewtonCollision* const compoundCollision, const void* const node) +{ + TRACE_FUNCTION(__FUNCTION__); + dgCollisionInstance* const instance = (dgCollisionInstance*) compoundCollision; + if (instance->IsType (dgCollision::dgCollisionCompound_RTTI)) { + dgCollisionCompound* const collision = (dgCollisionCompound*) instance->GetChildShape(); + return collision->GetNodeIndex((dgCollisionCompound::dgTreeArray::dgTreeNode*)node); + } + return -1; +} + + +NewtonCollision* NewtonCompoundCollisionGetCollisionFromNode (NewtonCollision* const compoundCollision, const void* const node) +{ + TRACE_FUNCTION(__FUNCTION__); + dgCollisionInstance* const compoundInstance = (dgCollisionInstance*) compoundCollision; + if (compoundInstance->IsType (dgCollision::dgCollisionCompound_RTTI)) { + dgCollisionCompound* const collision = (dgCollisionCompound*) compoundInstance->GetChildShape(); + return (NewtonCollision*) collision->GetCollisionFromNode((dgCollisionCompound::dgTreeArray::dgTreeNode*)node); + } + return NULL; +} + + +/*! + Create a compound collision from a concave mesh by an approximate convex partition + + @param *newtonWorld Pointer to the Newton world. + @param *convexAproximation fixme + @param hullTolerance fixme + @param shapeID fixme + @param subShapeID fixme + + + @return Pointer to the compound collision. + + The algorithm will separated the the original mesh into a series of sub meshes until either + the worse concave point is smaller than the specified min concavity or the max number convex shapes is reached. + + is is recommended that convex approximation are made by person with a graphics toll by physically overlaying collision primitives over the concave mesh. + but for quit test of maybe for simple meshes and algorithm approximations can be used. + + is is recommended that for best performance this function is used in an off line toll and serialize the output. + + Compound collision primitives are treated as instanced collision objects that cannot be shared by multiples rigid bodies. + +*/ +NewtonCollision* NewtonCreateCompoundCollisionFromMesh (const NewtonWorld* const newtonWorld, const NewtonMesh* const convexAproximation, dFloat hullTolerance, int shapeID, int subShapeID) +{ + TRACE_FUNCTION(__FUNCTION__); + NewtonCollision* const compound = NewtonCreateCompoundCollision(newtonWorld, shapeID); + NewtonCompoundCollisionBeginAddRemove(compound); + + NewtonMesh* nextSegment = NULL; + for (NewtonMesh* segment = NewtonMeshCreateFirstSingleSegment (convexAproximation); segment; segment = nextSegment) { + nextSegment = NewtonMeshCreateNextSingleSegment (convexAproximation, segment); + + NewtonCollision* const convexHull = NewtonCreateConvexHullFromMesh (newtonWorld, segment, hullTolerance, subShapeID); + if (convexHull) { + NewtonCompoundCollisionAddSubCollision (compound, convexHull); + NewtonDestroyCollision(convexHull); + } + NewtonMeshDestroy(segment); + } + + NewtonCompoundCollisionEndAddRemove(compound); + + return compound; +} + + +NewtonCollision* NewtonCreateFracturedCompoundCollision (const NewtonWorld* const newtonWorld, const NewtonMesh* const solidMesh, int shapeID, int fracturePhysicsMaterialID, int pointcloudCount, const dFloat* const vertexCloud, int strideInBytes, int materialID, const dFloat* const textureMatrix, + NewtonFractureCompoundCollisionReconstructMainMeshCallBack regenerateMainMeshCallback, + NewtonFractureCompoundCollisionOnEmitCompoundFractured emitFracturedCompound, NewtonFractureCompoundCollisionOnEmitChunk emitFracfuredChunk) +{ + TRACE_FUNCTION(__FUNCTION__); + + Newton* const world = (Newton *)newtonWorld; + dgMeshEffect* const mesh = (dgMeshEffect*) solidMesh; + + dgMatrix textMatrix (textureMatrix); + dgCollisionInstance* const collision = world->CreateFracturedCompound (mesh, shapeID, fracturePhysicsMaterialID, pointcloudCount, vertexCloud, strideInBytes, materialID, textMatrix, + (dgCollisionCompoundFractured::OnEmitFractureChunkCallBack) emitFracfuredChunk, + (dgCollisionCompoundFractured::OnEmitNewCompundFractureCallBack) emitFracturedCompound, + (dgCollisionCompoundFractured::OnReconstructFractureMainMeshCallBack) regenerateMainMeshCallback); + return (NewtonCollision*) collision; +} + +NewtonCollision* NewtonFracturedCompoundPlaneClip (const NewtonCollision* const fracturedCompound, const dFloat* const plane) +{ + TRACE_FUNCTION(__FUNCTION__); + dgCollisionInstance* const collision = (dgCollisionInstance*) fracturedCompound; + + if (collision->IsType (dgCollision::dgCollisionCompoundBreakable_RTTI)) { + dgCollisionCompoundFractured* const compound = (dgCollisionCompoundFractured*) collision->GetChildShape(); + dgWorld* const world = (dgWorld*)collision->GetWorld(); + dgCollisionCompoundFractured* const newCompound = compound->PlaneClip(dgVector (plane[0], plane[1], plane[2], plane[3])); + if (newCompound) { + dgCollisionInstance* const newCollision = world->CreateInstance (newCompound, int (collision->GetUserDataID()), dgGetIdentityMatrix()); + newCompound->Release(); + return (NewtonCollision*)newCollision; + } + } + return NULL; +} + +void NewtonFracturedCompoundSetCallbacks (const NewtonCollision* const fracturedCompound, + NewtonFractureCompoundCollisionReconstructMainMeshCallBack regenerateMainMeshCallback, + NewtonFractureCompoundCollisionOnEmitCompoundFractured emitFracturedCompound, NewtonFractureCompoundCollisionOnEmitChunk emitFracfuredChunk) +{ + TRACE_FUNCTION(__FUNCTION__); + dgCollisionInstance* const collision = (dgCollisionInstance*) fracturedCompound; + + if (collision->IsType (dgCollision::dgCollisionCompoundBreakable_RTTI)) { + dgCollisionCompoundFractured* const compound = (dgCollisionCompoundFractured*) collision->GetChildShape(); + compound->SetCallbacks ((dgCollisionCompoundFractured::OnEmitFractureChunkCallBack) emitFracfuredChunk, (dgCollisionCompoundFractured::OnEmitNewCompundFractureCallBack) emitFracturedCompound, (dgCollisionCompoundFractured::OnReconstructFractureMainMeshCallBack) regenerateMainMeshCallback); + } +} + + +int NewtonFracturedCompoundNeighborNodeList (const NewtonCollision* const fracturedCompound, void* const collisionNode, void** const nodesArray, int maxCount) +{ + TRACE_FUNCTION(__FUNCTION__); + dgCollisionInstance* const collision = (dgCollisionInstance*) fracturedCompound; + if (collision->IsType (dgCollision::dgCollisionCompoundBreakable_RTTI)) { + dgCollisionCompoundFractured* const compound = (dgCollisionCompoundFractured*) collision->GetChildShape(); + return compound->GetFirstNiegborghArray ((dgCollisionCompound::dgTreeArray::dgTreeNode*)collisionNode, (dgCollisionCompound::dgTreeArray::dgTreeNode**) nodesArray, maxCount); + } + return 0; +} + + + +int NewtonFracturedCompoundIsNodeFreeToDetach (const NewtonCollision* const fracturedCompound, void* const collisionNode) +{ + TRACE_FUNCTION(__FUNCTION__); + dgCollisionInstance* const collision = (dgCollisionInstance*) fracturedCompound; + + if (collision->IsType (dgCollision::dgCollisionCompoundBreakable_RTTI)) { + dgCollisionCompoundFractured* const compound = (dgCollisionCompoundFractured*) collision->GetChildShape(); + return compound->IsNodeSaseToDetach((dgCollisionCompound::dgTreeArray::dgTreeNode*)collisionNode) ? 1 : 0; + } + return 0; +} + +NewtonFracturedCompoundMeshPart* NewtonFracturedCompoundGetFirstSubMesh(const NewtonCollision* const fracturedCompound) +{ + TRACE_FUNCTION(__FUNCTION__); + dgCollisionInstance* const collision = (dgCollisionInstance*) fracturedCompound; + + NewtonFracturedCompoundMeshPart* mesh = NULL; + if (collision->IsType (dgCollision::dgCollisionCompoundBreakable_RTTI)) { + dgCollisionCompoundFractured* const compound = (dgCollisionCompoundFractured*) collision->GetChildShape(); + mesh = (NewtonFracturedCompoundMeshPart*) compound->GetFirstMesh(); + } + return mesh; +} + +NewtonFracturedCompoundMeshPart* NewtonFracturedCompoundGetNextSubMesh(const NewtonCollision* const fracturedCompound, NewtonFracturedCompoundMeshPart* const subMesh) +{ + TRACE_FUNCTION(__FUNCTION__); + dgCollisionInstance* const collision = (dgCollisionInstance*) fracturedCompound; + + NewtonFracturedCompoundMeshPart* mesh = NULL; + if (collision->IsType (dgCollision::dgCollisionCompoundBreakable_RTTI)) { + dgCollisionCompoundFractured* const compound = (dgCollisionCompoundFractured*) collision->GetChildShape(); + mesh = (NewtonFracturedCompoundMeshPart*) compound->GetNextMesh((dgCollisionCompoundFractured::dgConectivityGraph::dgListNode*) subMesh); + } + return mesh; +} + +NewtonFracturedCompoundMeshPart* NewtonFracturedCompoundGetMainMesh (const NewtonCollision* const fracturedCompound) +{ + TRACE_FUNCTION(__FUNCTION__); + dgCollisionInstance* const collision = (dgCollisionInstance*) fracturedCompound; + + NewtonFracturedCompoundMeshPart* mesh = NULL; + if (collision->IsType (dgCollision::dgCollisionCompoundBreakable_RTTI)) { + dgCollisionCompoundFractured* const compound = (dgCollisionCompoundFractured*) collision->GetChildShape(); + mesh = (NewtonFracturedCompoundMeshPart*) compound->GetMainMesh(); + } + return mesh; +} + + +int NewtonFracturedCompoundCollisionGetVertexCount (const NewtonCollision* const fracturedCompound, const NewtonFracturedCompoundMeshPart* const meshOwner) +{ + TRACE_FUNCTION(__FUNCTION__); + dgCollisionInstance* const collision = (dgCollisionInstance*) fracturedCompound; + + dgInt32 count = 0; + if (collision->IsType (dgCollision::dgCollisionCompoundBreakable_RTTI)) { + dgCollisionCompoundFractured* const compound = (dgCollisionCompoundFractured*) collision->GetChildShape(); + count = compound->GetVertecCount((dgCollisionCompoundFractured::dgConectivityGraph::dgListNode*) meshOwner); + } + return count; +} + + +const dFloat* NewtonFracturedCompoundCollisionGetVertexPositions (const NewtonCollision* const fracturedCompound, const NewtonFracturedCompoundMeshPart* const meshOwner) +{ + TRACE_FUNCTION(__FUNCTION__); + dgCollisionInstance* const collision = (dgCollisionInstance*) fracturedCompound; + + const dgFloat32* points = NULL; + if (collision->IsType (dgCollision::dgCollisionCompoundBreakable_RTTI)) { + dgCollisionCompoundFractured* const compound = (dgCollisionCompoundFractured*) collision->GetChildShape(); + points = compound->GetVertexPositions((dgCollisionCompoundFractured::dgConectivityGraph::dgListNode*) meshOwner); + } + return points; +} + + +const dFloat* NewtonFracturedCompoundCollisionGetVertexNormals (const NewtonCollision* const fracturedCompound, const NewtonFracturedCompoundMeshPart* const meshOwner) +{ + TRACE_FUNCTION(__FUNCTION__); + dgCollisionInstance* const collision = (dgCollisionInstance*) fracturedCompound; + + const dgFloat32* points = NULL; + if (collision->IsType (dgCollision::dgCollisionCompoundBreakable_RTTI)) { + dgCollisionCompoundFractured* const compound = (dgCollisionCompoundFractured*) collision->GetChildShape(); + points = compound->GetVertexNormal((dgCollisionCompoundFractured::dgConectivityGraph::dgListNode*) meshOwner); + } + return points; +} + +const dFloat* NewtonFracturedCompoundCollisionGetVertexUVs (const NewtonCollision* const fracturedCompound, const NewtonFracturedCompoundMeshPart* const meshOwner) +{ + TRACE_FUNCTION(__FUNCTION__); + dgCollisionInstance* const collision = (dgCollisionInstance*) fracturedCompound; + + const dgFloat32* points = NULL; + if (collision->IsType (dgCollision::dgCollisionCompoundBreakable_RTTI)) { + dgCollisionCompoundFractured* const compound = (dgCollisionCompoundFractured*) collision->GetChildShape(); + points = compound->GetVertexUVs((dgCollisionCompoundFractured::dgConectivityGraph::dgListNode*) meshOwner); + } + return points; +} + +int NewtonFracturedCompoundMeshPartGetIndexStream (const NewtonCollision* const fracturedCompound, const NewtonFracturedCompoundMeshPart* const meshOwner, const void* const segment, int* const index) +{ + TRACE_FUNCTION(__FUNCTION__); + + dgInt32 count = 0; + dgCollisionInstance* const collision = (dgCollisionInstance*) fracturedCompound; + if (collision->IsType (dgCollision::dgCollisionCompoundBreakable_RTTI)) { + dgCollisionCompoundFractured* const compound = (dgCollisionCompoundFractured*) collision; + count = compound->GetSegmentIndexStream ((dgCollisionCompoundFractured::dgConectivityGraph::dgListNode*) meshOwner, (dgCollisionCompoundFractured::dgMesh::dgListNode*) segment, index); + } + return count; +} + + +void* NewtonFracturedCompoundMeshPartGetFirstSegment (const NewtonFracturedCompoundMeshPart* const breakableComponentMesh) +{ + TRACE_FUNCTION(__FUNCTION__); + dgCollisionCompoundFractured::dgConectivityGraph::dgListNode* const node = (dgCollisionCompoundFractured::dgConectivityGraph::dgListNode*) breakableComponentMesh; + return node->GetInfo().m_nodeData.m_mesh->GetFirst(); +} + +void* NewtonFracturedCompoundMeshPartGetNextSegment (const void* const breakableComponentSegment) +{ + TRACE_FUNCTION(__FUNCTION__); + dgCollisionCompoundFractured::dgMesh::dgListNode* const node = (dgCollisionCompoundFractured::dgMesh::dgListNode*) breakableComponentSegment; + return node->GetNext(); +} + +int NewtonFracturedCompoundMeshPartGetMaterial (const void* const segment) +{ + TRACE_FUNCTION(__FUNCTION__); + + dgCollisionCompoundFractured::dgMesh::dgListNode* const node = (dgCollisionCompoundFractured::dgMesh::dgListNode*) segment; + return node->GetInfo().m_material; +} + + +int NewtonFracturedCompoundMeshPartGetIndexCount (const void* const segment) +{ + TRACE_FUNCTION(__FUNCTION__); + + dgCollisionCompoundFractured::dgMesh::dgListNode* const node = (dgCollisionCompoundFractured::dgMesh::dgListNode*) segment; + return node->GetInfo().m_faceCount * 3; +} + + + + + +// Return the trigger volume flag of this shape. +// +// @param convexCollision is the pointer to a convex collision primitive. +// +// @return 0 if collision shape is solid, non zero is collision shape is a trigger volume. +// +// this function can be used to place collision triggers in the scene. +// Setting this flag is not really a necessary to place a collision trigger however this option hint the engine that +// this particular shape is a trigger volume and no contact calculation is desired. +int NewtonCollisionGetMode(const NewtonCollision* const convexCollision) +{ + dgCollisionInstance* const collision = (dgCollisionInstance*) convexCollision; + TRACE_FUNCTION(__FUNCTION__); + return collision->GetCollisionMode() ? 1 : 0; +} + + +// Set a flag on a convex collision shape to indicate that no contacts should calculated for this shape. +// +// @param convexCollision is the pointer to a convex collision primitive. +// @param triggerMode 1 disable contact calculation, 0 enable contact calculation. +// +// @return nothing +// +// this function can be used to place collision triggers in the scene. +// Setting this flag is not really a necessary to place a collision trigger however this option hint the engine that +// this particular shape is a trigger volume and no contact calculation is desired. +// +void NewtonCollisionSetMode (const NewtonCollision* const convexCollision, int mode) +{ + TRACE_FUNCTION(__FUNCTION__); + dgCollisionInstance* const collision = (dgCollisionInstance*) convexCollision; + collision->SetCollisionMode(mode ? true : false); +} + +/* +void NewtonCollisionSetMaxBreakImpactImpulse(const NewtonCollision* const convexHullCollision, dFloat maxImpactImpulse) +{ + TRACE_FUNCTION(__FUNCTION__); + dgCollisionInstance* const collision = (dgCollisionInstance*) convexHullCollision; + collision->SetBreakImpulse(dgFloat32 (maxImpactImpulse)); +} + +dFloat NewtonCollisionGetMaxBreakImpactImpulse(const NewtonCollision* const convexHullCollision) +{ + TRACE_FUNCTION(__FUNCTION__); + dgCollisionInstance* const collision = (dgCollisionInstance*) convexHullCollision; + return dgFloat32 (collision->GetBreakImpulse()); +} +*/ + +int NewtonConvexHullGetVertexData (const NewtonCollision* const convexHullCollision, dFloat** const vertexData, int* strideInBytes) +{ + dgAssert (0); + return 0; +} + + +/*! + Return the number of vertices of face and copy each index into array faceIndices. + + @param convexHullCollision is the pointer to a convex collision hull primitive. + @param face fixme + @param faceIndices fixme + + @return user face count of face. + + this function will return zero on all shapes other than a convex full collision shape. + + To get the number of faces of a convex hull shape see function *NewtonCollisionGetInfo* + + See also: ::NewtonCollisionGetInfo, ::NewtonCreateConvexHull +*/ +int NewtonConvexHullGetFaceIndices(const NewtonCollision* const convexHullCollision, int face, int* const faceIndices) +{ + TRACE_FUNCTION(__FUNCTION__); + dgCollisionInstance* const coll = (dgCollisionInstance*) convexHullCollision; + + if (coll->IsType (dgCollision::dgCollisionConvexHull_RTTI)) { + //return ((dgCollisionConvexHull*)coll)->GetFaceIndices (face, faceIndices); + return ((dgCollisionConvexHull*)coll->GetChildShape())->GetFaceIndices (face, faceIndices); + } else { + return 0; + } +} + +/*! + calculate the total volume defined by a convex collision geometry. + + @param *convexCollision pointer to the collision. + + @return collision geometry volume. This function will return zero if the body collision geometry is no convex. + + The total volume calculated by the function is only an approximation of the ideal volume. This is not an error, it is a fact resulting from the polygonal representation of convex solids. + + This function can be used to assist the application in calibrating features like fluid density weigh factor when calibrating buoyancy forces for more realistic result. +*/ +dFloat NewtonConvexCollisionCalculateVolume(const NewtonCollision* const convexCollision) +{ + TRACE_FUNCTION(__FUNCTION__); + dgCollisionInstance* const collision = (dgCollisionInstance*)convexCollision; + return collision->GetVolume(); +} + + +/*! + Calculate the three principal axis and the the values of the inertia matrix of a convex collision objects. + + @param convexCollision is the pointer to a convex collision primitive. + @param *inertia pointer to and array of a least 3 floats to hold the values of the principal inertia. + @param *origin pointer to and array of a least 3 floats to hold the values of the center of mass for the principal inertia. + + This function calculate a general inertial matrix for arbitrary convex collision including compound collisions. + + See also: ::NewtonBodySetMassMatrix, ::NewtonBodyGetMass, ::NewtonBodySetCentreOfMass, ::NewtonBodyGetCentreOfMass +*/ +void NewtonConvexCollisionCalculateInertialMatrix(const NewtonCollision* convexCollision, dFloat* const inertia, dFloat* const origin) +{ + TRACE_FUNCTION(__FUNCTION__); + dgCollisionInstance* const collision = (dgCollisionInstance*)convexCollision; + +// dgVector tmpInertia; +// dgVector tmpOringin; +// collision->CalculateInertia(tmpInertia, tmpOringin); + dgMatrix tmpInertia (collision->CalculateInertia()); + + inertia[0] = tmpInertia[0][0]; + inertia[1] = tmpInertia[1][1]; + inertia[2] = tmpInertia[2][2]; + origin[0] = tmpInertia[3][0]; + origin[1] = tmpInertia[3][1]; + origin[2] = tmpInertia[3][2]; +} + + +/*! + Add buoyancy force and torque for bodies immersed in a fluid. + + @param convexCollision fixme + @param matrix fixme + @param fluidPlane fixme + @param centerOfBuoyancy fixme + + @return Nothing. + + This function is only effective when called from *NewtonApplyForceAndTorque callback* + + This function adds buoyancy force and torque to a body when it is immersed in a fluid. + The force is calculated according to Archimedes Buoyancy Principle. When the parameter *buoyancyPlane* is set to NULL, the body is considered + to completely immersed in the fluid. This can be used to simulate boats and lighter than air vehicles etc.. + + If *buoyancyPlane* return 0 buoyancy calculation for this collision primitive is ignored, this could be used to filter buoyancy calculation + of compound collision geometry with different IDs. + + See also: ::NewtonConvexCollisionCalculateVolume +*/ +//void NewtonConvexCollisionCalculateBuoyancyAcceleration (const NewtonCollision* const convexCollision, const dFloat* const matrix, const dFloat* const shapeOrigin, const dFloat* const gravityVector, const dFloat* const fluidPlane, dFloat fluidDensity, dFloat* const accel, dFloat* const alpha) +dFloat NewtonConvexCollisionCalculateBuoyancyVolume (const NewtonCollision* const convexCollision, const dFloat* const matrix, const dFloat* const fluidPlane, dFloat* const centerOfBuoyancy) +{ + TRACE_FUNCTION(__FUNCTION__); + dgCollisionInstance* const instance = (dgCollisionInstance*)convexCollision; + dgVector plane (fluidPlane[0], fluidPlane[1], fluidPlane[2], fluidPlane[3]); + + dgVector com (instance->CalculateBuoyancyVolume (dgMatrix (matrix), plane)); + centerOfBuoyancy[0] = com[0]; + centerOfBuoyancy[1] = com[1]; + centerOfBuoyancy[2] = com[2]; + return com.m_w; +} + +const void* NewtonCollisionDataPointer (const NewtonCollision* const convexCollision) +{ + dgCollisionInstance* const coll = (dgCollisionInstance*) convexCollision; + return coll->GetChildShape(); +} + +/*! @} */ // end of CshapesConvexSimple + +/*! @defgroup CshapesConvexComplex CshapesConvexComplex +Complex collision primitives interface +@{ +*/ + + +/*! + Create a complex collision geometry to be controlled by the application. + + @param *newtonWorld Pointer to the Newton world. + @param *minBox pointer to an array of at least three floats to hold minimum value for the box relative to the collision. + @param *maxBox pointer to an array of at least three floats to hold maximum value for the box relative to the collision. + @param *userData pointer to user data to be used as context for event callback. + @param collideCallback pointer to an event function for providing Newton with the polygon inside a given box region. + @param rayHitCallback pointer to an event function for providing Newton with ray intersection information. + @param destroyCallback pointer to an event function for destroying any data allocated for use by the application. + @param getInfoCallback fixme + @param getAABBOverlapTestCallback fixme + @param facesInAABBCallback fixme + @param serializeCallback fixme + @param shapeID fixme + + @return Pointer to the user collision. + + *UserMeshCollision* provides the application with a method of overloading the built-in collision system for background objects. + UserMeshCollision can be used for implementing collisions with height maps, collisions with BSP, and any other collision structure the application + supports and wishes to preserve. + However, *UserMeshCollision* can not take advantage of the efficient and sophisticated algorithms and data structures of the + built-in *TreeCollision*. We suggest you experiment with both methods and use the method best suited to your situation. + + When a *UserMeshCollision* is assigned to a body, the mass of the body is ignored in all dynamics calculations. + This make the body behave as a static body. + +*/ +NewtonCollision* NewtonCreateUserMeshCollision( + const NewtonWorld* const newtonWorld, + const dFloat* const minBox, + const dFloat* const maxBox, + void* const userData, + NewtonUserMeshCollisionCollideCallback collideCallback, + NewtonUserMeshCollisionRayHitCallback rayHitCallback, + NewtonUserMeshCollisionDestroyCallback destroyCallback, + NewtonUserMeshCollisionGetCollisionInfo getInfoCallback, + NewtonUserMeshCollisionAABBTest getAABBOverlapTestCallback, + NewtonUserMeshCollisionGetFacesInAABB facesInAABBCallback, + NewtonOnUserCollisionSerializationCallback serializeCallback, + int shapeID) +{ + TRACE_FUNCTION(__FUNCTION__); + dgVector p0 (minBox[0], minBox[1], minBox[2], dgFloat32(1.0f)); + dgVector p1 (maxBox[0], maxBox[1], maxBox[2], dgFloat32(1.0f)); + + Newton* const world = (Newton *)newtonWorld; + + dgUserMeshCreation data; + data.m_userData = userData; + data.m_collideCallback = (dgCollisionUserMesh::OnUserMeshCollideCallback) collideCallback; + data.m_rayHitCallback = (dgCollisionUserMesh::OnUserMeshRayHitCallback) rayHitCallback; + data.m_destroyCallback = (dgCollisionUserMesh::OnUserMeshDestroyCallback) destroyCallback; + data.m_getInfoCallback = (dgCollisionUserMesh::OnUserMeshCollisionInfo)getInfoCallback; + data.m_getAABBOvelapTestCallback = (dgCollisionUserMesh::OnUserMeshAABBOverlapTest) getAABBOverlapTestCallback; + data.m_faceInAABBCallback = (dgCollisionUserMesh::OnUserMeshFacesInAABB) facesInAABBCallback; + data.m_serializeCallback = (dgCollisionUserMesh::OnUserMeshSerialize) serializeCallback; + + + dgCollisionInstance* const collision = world->CreateStaticUserMesh (p0, p1, data); + collision->SetUserDataID(dgUnsigned32 (shapeID)); + return (NewtonCollision*)collision; +} + + + +int NewtonUserMeshCollisionContinuousOverlapTest (const NewtonUserMeshCollisionCollideDesc* const collideDescData, const void* const rayHandle, const dFloat* const minAabb, const dFloat* const maxAabb) +{ + const dgFastRayTest* const ray = (dgFastRayTest*) rayHandle; + + dgVector p0 (minAabb); + dgVector p1 (maxAabb); + + dgVector q0 (collideDescData->m_boxP0); + dgVector q1 (collideDescData->m_boxP1); + + p0 = p0 & dgVector::m_triplexMask; + p1 = p1 & dgVector::m_triplexMask; + q0 = q0 & dgVector::m_triplexMask; + q1 = q1 & dgVector::m_triplexMask; + + dgVector box0 (p0 - q1); + dgVector box1 (p1 - q0); + + dgFloat32 dist = ray->BoxIntersect(box0, box1); + return (dist < dgFloat32 (1.0f)) ? 1 : 0; +} + + + +/*! + Create an empty complex collision geometry tree. + + @param *newtonWorld Pointer to the Newton world. + @param shapeID fixme + + @return Pointer to the collision tree. + + *TreeCollision* is the preferred method within Newton for collision with polygonal meshes of arbitrary complexity. + The mesh must be made of flat non-intersecting polygons, but they do not explicitly need to be triangles. + *TreeCollision* can be serialized by the application to/from an arbitrary storage device. + + When a *TreeCollision* is assigned to a body the mass of the body is ignored in all dynamics calculations. + This makes the body behave as a static body. + + See also: ::NewtonTreeCollisionBeginBuild, ::NewtonTreeCollisionAddFace, ::NewtonTreeCollisionEndBuild, ::NewtonStaticCollisionSetDebugCallback, ::NewtonTreeCollisionGetFaceAttribute, ::NewtonTreeCollisionSetFaceAttribute +*/ +NewtonCollision* NewtonCreateTreeCollision(const NewtonWorld* const newtonWorld, int shapeID) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *)newtonWorld; + dgCollisionInstance* const collision = world->CreateBVH (); + collision->SetUserDataID(dgUnsigned32 (shapeID)); + return (NewtonCollision*) collision; +} + + +NewtonCollision* NewtonCreateTreeCollisionFromMesh (const NewtonWorld* const newtonWorld, const NewtonMesh* const mesh, int shapeID) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *)newtonWorld; + dgMeshEffect* const meshEffect = (dgMeshEffect*) mesh; + dgCollisionInstance* const collision = meshEffect->CreateCollisionTree(world, shapeID); + return (NewtonCollision*) collision; +} + + +/*! + set a function call back to be call during the face query of a collision tree. + + @param *staticCollision is the pointer to the static collision (a CollisionTree of a HeightFieldCollision) + @param *userCallback pointer to an event function to call before Newton evaluates the polygons colliding with a body. This parameter can be NULL. + + because debug display display report all the faces of a collision primitive, it could get slow on very large static collision. + this function can be used for debugging purpose to just report only faces intersection the collision AABB of the collision shape colliding with the polyginal mesh collision. + + this function is not recommended to use for production code only for debug purpose. + + See also: ::NewtonTreeCollisionGetFaceAttribute, ::NewtonTreeCollisionSetFaceAttribute +*/ +void NewtonStaticCollisionSetDebugCallback(const NewtonCollision* const staticCollision, NewtonTreeCollisionCallback userCallback) +{ + TRACE_FUNCTION(__FUNCTION__); + dgCollisionInstance* const collision = (dgCollisionInstance*)staticCollision; + if (collision->IsType (dgCollision::dgCollisionMesh_RTTI)) { + dgCollisionMesh* const mesh = (dgCollisionMesh*) collision->GetChildShape(); + mesh->SetDebugCollisionCallback ((dgCollisionMeshCollisionCallback) userCallback); + } + +} + +/*! + set a function call back to be called during the face query of a collision tree. + + @param *treeCollision is the pointer to the collision tree. + @param rayHitCallback pointer to an event function for providing Newton with ray intersection information. + + In general a ray cast on a collision tree will stops at the first intersections with the closest face in the tree + that was hit by the ray. In some cases the application may be interested in the intesation with faces other than the fiorst hit. + In this cases the application can set this alternate callback and the ray scanner will notify the application of each face hit by the ray scan. + + since this function faces the ray scanner to visit all of the potential faces intersected by the ray, + setting the function call back make the ray casting on collision tree less efficient than the default behavior. + So it is this functionality is only recommended for cases were the application is using especial effects like transparencies, or other effects + + calling this function with *rayHitCallback* = NULL will rest the collision tree to it default raycast mode, which is return with the closest hit. + + when *rayHitCallback* is not null then the callback is dalled with the follwing arguments + *const NewtonCollisio* collision - pointer to the collision tree + interseption - inetstion parameters of the ray + *normal - unnormalized face mormal in the space fo eth parent of the collision. + faceId - id of this face in the collision tree. + + See also: ::NewtonTreeCollisionGetFaceAttribute, ::NewtonTreeCollisionSetFaceAttribute +*/ +void NewtonTreeCollisionSetUserRayCastCallback(const NewtonCollision* const treeCollision, NewtonCollisionTreeRayCastCallback rayHitCallback) +{ + TRACE_FUNCTION(__FUNCTION__); + dgCollisionInstance* const collision = (dgCollisionInstance*)treeCollision; +// dgCollisionBVH* const collision = (dgCollisionBVH*) treeCollision; + if (collision->IsType (dgCollision::dgCollisionBVH_RTTI)) { + dgCollisionBVH* const shape = (dgCollisionBVH*) collision->GetChildShape(); + shape->SetCollisionRayCastCallback ((dgCollisionBVHUserRayCastCallback) rayHitCallback); + } +} + + +void NewtonHeightFieldSetUserRayCastCallback (const NewtonCollision* const heightField, NewtonHeightFieldRayCastCallback rayHitCallback) +{ + TRACE_FUNCTION(__FUNCTION__); + dgCollisionInstance* const collision = (dgCollisionInstance*)heightField; + if (collision->IsType (dgCollision::dgCollisionHeightField_RTTI)) { + dgCollisionHeightField* const shape = (dgCollisionHeightField*) collision->GetChildShape(); + shape->SetCollisionRayCastCallback ((dgCollisionHeightFieldRayCastCallback) rayHitCallback); + } +} + +/*! + Prepare a *TreeCollision* to begin to accept the polygons that comprise the collision mesh. + + @param *treeCollision is the pointer to the collision tree. + + @return Nothing. + + See also: ::NewtonTreeCollisionAddFace, ::NewtonTreeCollisionEndBuild +*/ +void NewtonTreeCollisionBeginBuild(const NewtonCollision* const treeCollision) +{ + TRACE_FUNCTION(__FUNCTION__); + dgCollisionBVH* const collision = (dgCollisionBVH*) ((dgCollisionInstance*)treeCollision)->GetChildShape(); + dgAssert (collision->IsType (dgCollision::dgCollisionBVH_RTTI)); + + collision->BeginBuild(); +} + +/*! + Add an individual polygon to a *TreeCollision*. + + @param *treeCollision is the pointer to the collision tree. + @param vertexCount number of vertex in *vertexPtr* + @param *vertexPtr pointer to an array of vertex. The vertex should consist of at least 3 floats each. + @param strideInBytes size of each vertex in bytes. This value should be 12 or larger. + @param faceAttribute id that identifies the polygon. The application can use this value to customize the behavior of the collision geometry. + + @return Nothing. + + After the call to *NewtonTreeCollisionBeginBuild* the *TreeCollision* is ready to accept polygons. The application should iterate + through the application's mesh, adding the mesh polygons to the *TreeCollision* one at a time. + The polygons must be flat and non-self intersecting. + + See also: ::NewtonTreeCollisionAddFace, ::NewtonTreeCollisionEndBuild +*/ +void NewtonTreeCollisionAddFace(const NewtonCollision* const treeCollision, int vertexCount, const dFloat* const vertexPtr, int strideInBytes, int faceAttribute) +{ + TRACE_FUNCTION(__FUNCTION__); + dgCollisionBVH* const collision = (dgCollisionBVH*) ((dgCollisionInstance*)treeCollision)->GetChildShape(); + dgAssert (collision->IsType (dgCollision::dgCollisionBVH_RTTI)); + collision->AddFace(vertexCount, vertexPtr, strideInBytes, faceAttribute); +} + +/*! + Finalize the construction of the polygonal mesh. + + @param *treeCollision is the pointer to the collision tree. + @param optimize flag that indicates to Newton whether it should optimize this mesh. Set to 1 to optimize the mesh, otherwise 0. + + @return Nothing. + + + After the application has finished adding polygons to the *TreeCollision*, it must call this function to finalize the construction of the collision mesh. + If concave polygons are added to the *TreeCollision*, the application must call this function with the parameter *optimize* set to 1. + With the *optimize* parameter set to 1, Newton will optimize the collision mesh by removing non essential edges from adjacent flat polygons. + Newton will not change the topology of the mesh but significantly reduces the number of polygons in the mesh. The reduction factor of the number of polygons in the mesh depends upon the irregularity of the mesh topology. + A reduction factor of 1.5 to 2.0 is common. + Calling this function with the parameter *optimize* set to zero, will leave the mesh geometry unaltered. + + See also: ::NewtonTreeCollisionAddFace, ::NewtonTreeCollisionEndBuild +*/ +void NewtonTreeCollisionEndBuild(const NewtonCollision* const treeCollision, int optimize) +{ + TRACE_FUNCTION(__FUNCTION__); + dgCollisionBVH* const collision = (dgCollisionBVH*) ((dgCollisionInstance*)treeCollision)->GetChildShape(); + dgAssert (collision->IsType (dgCollision::dgCollisionBVH_RTTI)); + collision->EndBuild(optimize); +} + + +/*! + Get the user defined collision attributes stored with each face of the collision mesh. + + @param treeCollision fixme + @param *faceIndexArray pointer to the face index list passed to the function *NewtonTreeCollisionCallback userCallback + @param indexCount fixme + + @return User id of the face. + + This function is used to obtain the user data stored in faces of the collision geometry. + The application can use this user data to achieve per polygon material behavior in large static collision meshes. + + See also: ::NewtonTreeCollisionSetFaceAttribute, ::NewtonCreateTreeCollision +*/ +int NewtonTreeCollisionGetFaceAttribute(const NewtonCollision* const treeCollision, const int* const faceIndexArray, int indexCount) +{ + TRACE_FUNCTION(__FUNCTION__); + dgCollisionBVH* const collision = (dgCollisionBVH*) ((dgCollisionInstance*)treeCollision)->GetChildShape(); + dgAssert (collision->IsType (dgCollision::dgCollisionBVH_RTTI)); + + return int (collision->GetTagId (faceIndexArray, indexCount)); +} + +/*! + Change the user defined collision attribute stored with faces of the collision mesh. + + @param *treeCollision fixme + @param *faceIndexArray pointer to the face index list passed to the NewtonTreeCollisionCallback function + @param indexCount fixme + @param attribute value of the user defined attribute to be stored with the face. + + @return User id of the face. + + This function is used to obtain the user data stored in faces of the collision geometry. + The application can use this user data to achieve per polygon material behavior in large static collision meshes. + By changing the value of this user data the application can achieve modifiable surface behavior with the collision geometry. + For example, in a driving game, the surface of a polygon that represents the street can changed from pavement to oily or wet after + some collision event occurs. + + See also: ::NewtonTreeCollisionGetFaceAttribute, ::NewtonCreateTreeCollision +*/ +void NewtonTreeCollisionSetFaceAttribute(const NewtonCollision* const treeCollision, const int* const faceIndexArray, int indexCount, int attribute) +{ + TRACE_FUNCTION(__FUNCTION__); + dgCollisionBVH* const collision = (dgCollisionBVH*) ((dgCollisionInstance*)treeCollision)->GetChildShape(); + dgAssert (collision->IsType (dgCollision::dgCollisionBVH_RTTI)); + + collision->SetTagId (faceIndexArray, indexCount, dgUnsigned32 (attribute)); +} + +void NewtonTreeCollisionForEachFace (const NewtonCollision* const treeCollision, NewtonTreeCollisionFaceCallback forEachFaceCallback, void* const context) +{ + TRACE_FUNCTION(__FUNCTION__); + dgCollisionBVH* const collision = (dgCollisionBVH*) ((dgCollisionInstance*)treeCollision)->GetChildShape(); + dgAssert (collision->IsType (dgCollision::dgCollisionBVH_RTTI)); + + collision->ForEachFace ((dgAABBIntersectCallback) forEachFaceCallback, context); +} + + + +/*! + collect the vertex list index list mesh intersecting the AABB in collision mesh. + + @param *treeCollision fixme + @param *p0 - pointer to an array of at least three floats representing the ray origin in the local space of the geometry. + @param *p1 - pointer to an array of at least three floats representing the ray end in the local space of the geometry. + @param **vertexArray pointer to a the vertex array of vertex. + @param *vertexCount pointer int to return the number of vertex in vertexArray. + @param *vertexStrideInBytes pointer to int to return the size of each vertex in vertexArray. + @param *indexList pointer to array on integers containing the triangles intersection the aabb. + @param maxIndexCount maximum number of indices the function will copy to indexList. + @param *faceAttribute pointer to array on integers top contain the face containing the . + + @return the number of triangles in indexList. + + indexList should be a list 3 * maxIndexCount the number of elements. + + faceAttributet should be a list maxIndexCount the number of elements. + + this function could be used by the application for many purposes. + for example it can be used to draw the collision geometry intersecting a collision primitive instead + of drawing the entire collision tree in debug mode. + Another use for this function is to to efficient draw projective texture shadows. +*/ +int NewtonTreeCollisionGetVertexListTriangleListInAABB(const NewtonCollision* const treeCollision, const dFloat* const p0, const dFloat* const p1, + const dFloat** const vertexArray, int* const vertexCount, int* const vertexStrideInBytes, + const int* const indexList, int maxIndexCount, const int* const faceAttribute) +{ + TRACE_FUNCTION(__FUNCTION__); + + dgInt32 count = 0; + dgCollisionInstance* meshColl = (dgCollisionInstance*) treeCollision; + if (meshColl->IsType (dgCollision::dgCollisionMesh_RTTI)) { + dgCollisionMesh* const collision = (dgCollisionMesh*) ((dgCollisionInstance*)treeCollision)->GetChildShape(); + + dgVector pmin (p0[0], p0[1], p0[2], dgFloat32 (0.0f)); + dgVector pmax (p1[0], p1[1], p1[2], dgFloat32 (0.0f)); + + dgCollisionMesh::dgMeshVertexListIndexList data; + data.m_indexList = (dgInt32 *)indexList; + data.m_userDataList = (dgInt32 *)faceAttribute; + data.m_maxIndexCount = maxIndexCount; + data.m_triangleCount = 0; + collision->GetVertexListIndexList (pmin, pmax, data); + + count = data.m_triangleCount; + *vertexArray = data.m_veterxArray; + *vertexCount = data.m_vertexCount; + *vertexStrideInBytes = data.m_vertexStrideInBytes; + } + return count; +} + + +/*! + Create a height field collision geometry. + + @param *newtonWorld Pointer to the Newton world. + @param width the number of sample points in the x direction (fixme) + @param height the number of sample points in the y direction (fixme) + @param gridsDiagonals fixme + @param elevationdatType fixme + @param elevationMap array holding elevation data of size = width*height (fixme) + @param attributeMap array holding attribute data of size = width*height (fixme) + @param verticalScale scale of the elevation (fixme) + @param horizontalScale scale in the xy direction. (fixme) + @param shapeID fixme + + @return Pointer to the collision. + + NewtonCollision* NewtonCreateHeightFieldCollision(const NewtonWorld* const newtonWorld, int width, int height, int cellsDiagonals, + const dFloat* const elevationMap, const char* const atributeMap, + dFloat horizontalScale, int shapeID) +*/ + NewtonCollision* NewtonCreateHeightFieldCollision (const NewtonWorld* const newtonWorld, int width, int height, int gridsDiagonals, dgInt32 elevationdatType, + const void* const elevationMap, const char* const attributeMap, dFloat verticalScale, dFloat horizontalScale_x, dFloat horizontalScale_z, int shapeID) +{ + Newton* const world = (Newton *)newtonWorld; + + TRACE_FUNCTION(__FUNCTION__); + dgCollisionInstance* const collision = world->CreateHeightField(width, height, gridsDiagonals, elevationdatType, elevationMap, (const dgInt8* const) attributeMap, verticalScale, horizontalScale_x, horizontalScale_z); + collision->SetUserDataID(dgUnsigned32 (shapeID)); + return (NewtonCollision*) collision; +} + + + +/*! + Create a height field collision geometry. + + @param *newtonWorld Pointer to the Newton world. + @param shapeID fixme + + @return Pointer to the collision. + +*/ +NewtonCollision* NewtonCreateSceneCollision (const NewtonWorld* const newtonWorld, int shapeID) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *)newtonWorld; + + dgCollisionInstance* const collision = world->CreateScene (); + + collision->SetUserDataID(dgUnsigned32 (shapeID)); + return (NewtonCollision*) collision; +} + + +void NewtonSceneCollisionBeginAddRemove (NewtonCollision* const sceneCollision) +{ + TRACE_FUNCTION(__FUNCTION__); + NewtonCompoundCollisionBeginAddRemove (sceneCollision); +} + +void NewtonSceneCollisionEndAddRemove (NewtonCollision* const sceneCollision) +{ + TRACE_FUNCTION(__FUNCTION__); + NewtonCompoundCollisionEndAddRemove (sceneCollision); +} + + +void NewtonSceneCollisionSetSubCollisionMatrix (NewtonCollision* const sceneCollision, const void* const collisionNode, const dFloat* const matrix) +{ + TRACE_FUNCTION(__FUNCTION__); + NewtonCompoundCollisionSetSubCollisionMatrix (sceneCollision, collisionNode, matrix); +} + + +void* NewtonSceneCollisionAddSubCollision (NewtonCollision* const sceneCollision, const NewtonCollision* const collision) +{ + TRACE_FUNCTION(__FUNCTION__); + + dgCollisionInstance* const sceneInstance = (dgCollisionInstance*) sceneCollision; + dgCollisionInstance* const sceneInstanceChild = (dgCollisionInstance*) collision; + if (sceneInstance->IsType (dgCollision::dgCollisionScene_RTTI) && !sceneInstanceChild->IsType(dgCollision::dgCollisionCompound_RTTI)) { + dgCollisionScene* const collision1 = (dgCollisionScene*) sceneInstance->GetChildShape(); + return collision1->AddCollision (sceneInstanceChild); + } + return NULL; +} + +void NewtonSceneCollisionRemoveSubCollision (NewtonCollision* const sceneCollision, const void* const collisionNode) +{ + TRACE_FUNCTION(__FUNCTION__); + dgCollisionInstance* const sceneInstance = (dgCollisionInstance*) sceneCollision; + if (sceneInstance->IsType (dgCollision::dgCollisionScene_RTTI)) { + dgCollisionScene* const collision = (dgCollisionScene*) sceneInstance->GetChildShape(); + dgCollisionInstance* const childCollision = collision->GetCollisionFromNode((dgCollisionCompound::dgTreeArray::dgTreeNode*)collisionNode); + if (childCollision) { + collision->RemoveCollision ((dgCollisionCompound::dgTreeArray::dgTreeNode*)collisionNode); + } + } +} + +void NewtonSceneCollisionRemoveSubCollisionByIndex (NewtonCollision* const sceneCollision, int nodeIndex) +{ + TRACE_FUNCTION(__FUNCTION__); + dgCollisionInstance* const instance = (dgCollisionInstance*) sceneCollision; + if (instance->IsType (dgCollision::dgCollisionCompound_RTTI)) { + dgCollisionCompound* const collision = (dgCollisionCompound*) instance->GetChildShape(); + NewtonSceneCollisionRemoveSubCollision (sceneCollision, collision->FindNodeByIndex(nodeIndex)); + } +} + + +void* NewtonSceneCollisionGetNodeByIndex (NewtonCollision* const sceneCollision, int index) +{ + TRACE_FUNCTION(__FUNCTION__); + return NewtonCompoundCollisionGetNodeByIndex (sceneCollision, index); +} + +int NewtonSceneCollisionGetNodeIndex (NewtonCollision* const sceneCollision, const void* const collisionNode) +{ + TRACE_FUNCTION(__FUNCTION__); + return NewtonCompoundCollisionGetNodeIndex (sceneCollision, collisionNode); +} + + +NewtonCollision* NewtonSceneCollisionGetCollisionFromNode (NewtonCollision* const sceneCollision, const void* const node) +{ + TRACE_FUNCTION(__FUNCTION__); + return NewtonCompoundCollisionGetCollisionFromNode (sceneCollision, node); +} + +void* NewtonSceneCollisionGetFirstNode (NewtonCollision* const sceneCollision) +{ + TRACE_FUNCTION(__FUNCTION__); + return NewtonCompoundCollisionGetFirstNode (sceneCollision); +} + +void* NewtonSceneCollisionGetNextNode (NewtonCollision* const sceneCollision, const void* const node) +{ + TRACE_FUNCTION(__FUNCTION__); + return NewtonCompoundCollisionGetNextNode (sceneCollision, node); +} + + +/*! @} */ // end of CshapesConvexComples + + +/*! @defgroup CollisionLibraryGeneric CollisionLibraryGeneric +Generic collision library functions +@{ +*/ + +/*! + Calculate the closest point between a point and convex collision primitive. + + @param *newtonWorld Pointer to the Newton world. + @param *point pointer to and array of a least 3 floats representing the origin. + @param *collision pointer to collision primitive. + @param *matrix pointer to an array of 16 floats containing the offset matrix of collision primitiveA. + @param *contact pointer to and array of a least 3 floats to contain the closest point to collisioA. + @param *normal pointer to and array of a least 3 floats to contain the separating vector normal. + @param threadIndex -Thread index form where the call is made from, zeor otherwize + + @return one if the two bodies are disjoint and the closest point could be found, + zero if the point is inside the convex primitive. + + This function can be used as a low-level building block for a stand-alone collision system. + Applications that have already there own physics system, and only want and quick and fast collision solution, + can use Newton advanced collision engine as the low level collision detection part. + To do this the application only needs to initialize Newton, create the collision primitives at application discretion, + and just call this function when the objects are in close proximity. Applications using Newton as a collision system + only, are responsible for implementing their own broad phase collision determination, based on any high level tree structure. + Also the application should implement their own trivial aabb test, before calling this function . + + the current implementation of this function do work on collision trees, or user define collision. + + See also: ::NewtonCollisionCollideContinue, ::NewtonCollisionClosestPoint, ::NewtonCollisionCollide, ::NewtonCollisionRayCast, ::NewtonCollisionCalculateAABB +*/ +int NewtonCollisionPointDistance(const NewtonWorld* const newtonWorld, const dFloat* const point, + const NewtonCollision* const collision, const dFloat* const matrix, + dFloat* const contact, dFloat* const normal, int threadIndex) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *)newtonWorld; + return world->ClosestPoint (*((dgTriplex*) point), (dgCollisionInstance*)collision, dgMatrix (matrix), *((dgTriplex*) contact), *((dgTriplex*) normal), threadIndex); +} + + +/*! + Calculate the closest points between two disjoint convex collision primitive. + + @param *newtonWorld Pointer to the Newton world. + @param *collisionA pointer to collision primitive A. + @param *matrixA pointer to an array of 16 floats containing the offset matrix of collision primitiveA. + @param *collisionB pointer to collision primitive B. + @param *matrixB pointer to an array of 16 floats containing the offset matrix of collision primitiveB. + @param *contactA pointer to and array of a least 3 floats to contain the closest point to collisionA. + @param *contactB pointer to and array of a least 3 floats to contain the closest point to collisionB. + @param *normalAB pointer to and array of a least 3 floats to contain the separating vector normal. + @param threadIndex -Thread index form where the call is made from, zeor otherwize + + @return one if the tow bodies are disjoint and he closest point could be found, + zero if the two collision primitives are intersecting. + + This function can be used as a low-level building block for a stand-alone collision system. + Applications that have already there own physics system, and only want and quick and fast collision solution, + can use Newton advanced collision engine as the low level collision detection part. + To do this the application only needs to initialize Newton, create the collision primitives at application discretion, + and just call this function when the objects are in close proximity. Applications using Newton as a collision system + only, are responsible for implementing their own broad phase collision determination, based on any high level tree structure. + Also the application should implement their own trivial aabb test, before calling this function . + + the current implementation of this function does not work on collision trees, or user define collision. + + See also: ::NewtonCollisionCollideContinue, ::NewtonCollisionPointDistance, ::NewtonCollisionCollide, ::NewtonCollisionRayCast, ::NewtonCollisionCalculateAABB +*/ +int NewtonCollisionClosestPoint(const NewtonWorld* const newtonWorld, + const NewtonCollision* const collisionA, const dFloat* const matrixA, + const NewtonCollision* const collisionB, const dFloat* const matrixB, + dFloat* const contactA, dFloat* const contactB, dFloat* const normalAB, int threadIndex) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *)newtonWorld; + return world->ClosestPoint ((dgCollisionInstance*)collisionA, dgMatrix (matrixA), + (dgCollisionInstance*)collisionB, dgMatrix (matrixB), + *((dgTriplex*) contactA), *((dgTriplex*) contactB), *((dgTriplex*) normalAB), threadIndex); +} + + +int NewtonCollisionIntersectionTest (const NewtonWorld* const newtonWorld, const NewtonCollision* const collisionA, const dFloat* const matrixA, const NewtonCollision* const collisionB, const dFloat* const matrixB, int threadIndex) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *)newtonWorld; + return world->IntersectionTest ((dgCollisionInstance*)collisionA, dgMatrix (matrixA), + (dgCollisionInstance*)collisionB, dgMatrix (matrixB), + threadIndex) ? 1 : 0; +} + + +/*! + Calculate contact points between two collision primitive. + + @param *newtonWorld Pointer to the Newton world. + @param maxSize size of maximum number of elements in contacts, normals, and penetration. + @param *collisionA pointer to collision primitive A. + @param *matrixA pointer to an array of 16 floats containing the offset matrix of collision primitiveA. + @param *collisionB pointer to collision primitive B. + @param *matrixB pointer to an array of 16 floats containing the offset matrix of collision primitiveB. + @param *contacts pointer to and array of a least 3 times maxSize floats to contain the collision contact points. + @param *normals pointer to and array of a least 3 times maxSize floats to contain the collision contact normals. + @param *penetration pointer to and array of a least maxSize floats to contain the collision penetration at each contact. + @param attributeA fixme + @param attributeB fixme + @param threadIndex Thread index form where the call is made from, zeor otherwize + + @return the number of contact points. + + This function can be used as a low-level building block for a stand-alone collision system. + Applications that have already there own physics system, and only want and quick and fast collision solution, + can use Newton advanced collision engine as the low level collision detection part. + To do this the application only needs to initialize Newton, create the collision primitives at application discretion, + and just call this function when the objects are in close proximity. Applications using Newton as a collision system + only, are responsible for implementing their own broad phase collision determination, based on any high level tree structure. + Also the application should implement their own trivial aabb test, before calling this function . + + See also: ::NewtonCollisionCollideContinue, ::NewtonCollisionClosestPoint, ::NewtonCollisionPointDistance, ::NewtonCollisionRayCast, ::NewtonCollisionCalculateAABB +*/ +int NewtonCollisionCollide (const NewtonWorld* const newtonWorld, int maxSize, + const NewtonCollision* const collisionA, const dFloat* const matrixA, + const NewtonCollision* const collisionB, const dFloat* const matrixB, + dFloat* const contacts, dFloat* const normals, dFloat* const penetration, + dLong* const attributeA, dLong* const attributeB, int threadIndex) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *)newtonWorld; + return world->Collide ((dgCollisionInstance*)collisionA, dgMatrix (matrixA), + (dgCollisionInstance*)collisionB, dgMatrix (matrixB), + (dgTriplex*) contacts, (dgTriplex*) normals, penetration, attributeA, attributeB, maxSize, threadIndex); +} + + + +/*! + Calculate time of impact of impact and contact points between two collision primitive. + + @param *newtonWorld Pointer to the Newton world. + @param maxSize size of maximum number of elements in contacts, normals, and penetration. + @param timestep maximum time interval considered for the continuous collision calculation. + @param *collisionA pointer to collision primitive A. + @param *matrixA pointer to an array of 16 floats containing the offset matrix of collision primitiveA. + @param *velocA pointer to and array of a least 3 times maxSize floats containing the linear velocity of collision primitiveA. + @param *omegaA pointer to and array of a least 3 times maxSize floats containing the angular velocity of collision primitiveA. + @param *collisionB pointer to collision primitive B. + @param *matrixB pointer to an array of 16 floats containing the offset matrix of collision primitiveB. + @param *velocB pointer to and array of a least 3 times maxSize floats containing the linear velocity of collision primitiveB. + @param *omegaB pointer to and array of a least 3 times maxSize floats containing the angular velocity of collision primitiveB. + @param *timeOfImpact pointer to least 1 float variable to contain the time of the intersection. + @param *contacts pointer to and array of a least 3 times maxSize floats to contain the collision contact points. + @param *normals pointer to and array of a least 3 times maxSize floats to contain the collision contact normals. + @param *penetration pointer to and array of a least maxSize floats to contain the collision penetration at each contact. + @param attributeA fixme + @param attributeB fixme + @param threadIndex -Thread index form where the call is made from, zeor otherwize + + @return the number of contact points. + + by passing zero as *maxSize* not contact will be calculated and the function will just determine the time of impact is any. + + if the body are inter penetrating the time of impact will be zero. + + if the bodies do not collide time of impact will be set to *timestep* + + This function can be used as a low-level building block for a stand-alone collision system. + Applications that have already there own physics system, and only want and quick and fast collision solution, + can use Newton advanced collision engine as the low level collision detection part. + To do this the application only needs to initialize Newton, create the collision primitives at application discretion, + and just call this function when the objects are in close proximity. Applications using Newton as a collision system + only, are responsible for implementing their own broad phase collision determination, based on any high level tree structure. + Also the application should implement their own trivial aabb test, before calling this function . + + See also: ::NewtonCollisionCollide, ::NewtonCollisionClosestPoint, ::NewtonCollisionPointDistance, ::NewtonCollisionRayCast, ::NewtonCollisionCalculateAABB +*/ +int NewtonCollisionCollideContinue(const NewtonWorld* const newtonWorld, int maxSize, dFloat timestep, + const NewtonCollision* const collisionA, const dFloat* const matrixA, const dFloat* const velocA, const dFloat* const omegaA, + const NewtonCollision* const collisionB, const dFloat* const matrixB, const dFloat* const velocB, const dFloat* const omegaB, + dFloat* const timeOfImpact, dFloat* const contacts, dFloat* const normals, dFloat* const penetration, + dLong* const attributeA, dLong* const attributeB, int threadIndex) +{ + Newton* const world = (Newton *)newtonWorld; + + *timeOfImpact = timestep; + + TRACE_FUNCTION(__FUNCTION__); + return world->CollideContinue ((dgCollisionInstance*)collisionA, dgMatrix (matrixA), *((dgVector*) velocA), *((dgVector*) omegaA), + (dgCollisionInstance*)collisionB, dgMatrix (matrixB), *((dgVector*) velocB), *((dgVector*) omegaB), + *timeOfImpact, (dgTriplex*) contacts, (dgTriplex*) normals, penetration, attributeA, attributeB, maxSize, threadIndex); +} + + +/*! + Calculate the most extreme point of a convex collision shape along the given direction. + + @param *collisionPtr pointer to the collision object. + @param *dir pointer to an array of at least three floats representing the search direction. + @param *vertex pointer to an array of at least three floats to hold the collision most extreme vertex along the search direction. + + @return nothing. + + the search direction must be in the space of the collision shape. + + See also: ::NewtonCollisionRayCast, ::NewtonCollisionClosestPoint, ::NewtonCollisionPointDistance +*/ +void NewtonCollisionSupportVertex(const NewtonCollision* const collisionPtr, const dFloat* const dir, dFloat* const vertex) +{ + TRACE_FUNCTION(__FUNCTION__); + + dgCollisionInstance* const collision = (dgCollisionInstance*) collisionPtr; + + const dgMatrix& matrix = collision->GetLocalMatrix (); + dgVector searchDir (matrix.UnrotateVector(dgVector (dir[0], dir[1], dir[2], dgFloat32 (0.0f)))); + searchDir = searchDir.Normalize(); + dgVector vertexOut (matrix.TransformVector(collision->SupportVertex(searchDir))); + + vertex[0] = vertexOut[0]; + vertex[1] = vertexOut[1]; + vertex[2] = vertexOut[2]; +} + + +/*! + Ray cast specific collision object. + + @param *collisionPtr pointer to the collision object. + @param *p0 - pointer to an array of at least three floats representing the ray origin in the local space of the geometry. + @param *p1 - pointer to an array of at least three floats representing the ray end in the local space of the geometry. + @param *normal pointer to an array of at least three floats to hold the normal at the intersection point. + @param *attribute pointer to an array of at least one floats to hold the ID of the face hit by the ray. + + @return the parametric value of the intersection, between 0.0 and 1.0, an value larger than 1.0 if the ray miss. + + This function is intended for applications using newton collision system separate from the dynamics system, also for applications + implementing any king of special purpose logic like sensing distance to another object. + + the ray most be local to the collisions geometry, for example and application ray casting the collision geometry of + of a rigid body, must first take the points p0, and p1 to the local space of the rigid body by multiplying the points by the + inverse of he rigid body transformation matrix. + + See also: ::NewtonCollisionClosestPoint, ::NewtonCollisionSupportVertex, ::NewtonCollisionPointDistance, ::NewtonCollisionCollide, ::NewtonCollisionCalculateAABB +*/ +dFloat NewtonCollisionRayCast(const NewtonCollision* const collisionPtr, const dFloat* const p0, const dFloat* const p1, dFloat* const normal, dLong* const attribute) +{ + dgCollisionInstance* const collision = (dgCollisionInstance*) collisionPtr; + + TRACE_FUNCTION(__FUNCTION__); + const dgMatrix& matrix = collision->GetLocalMatrix (); + + dgVector q0 (matrix.UntransformVector (dgVector (p0[0], p0[1], p0[2], dgFloat32 (0.0f)))); + dgVector q1 (matrix.UntransformVector (dgVector (p1[0], p1[1], p1[2], dgFloat32 (0.0f)))); + dgContactPoint contact; + dgKinematicBody dommyBody; + dommyBody.SetCollision (collision); + dFloat t = collision->RayCast (q0, q1, dgFloat32 (1.0f), contact, NULL, &dommyBody, NULL); + dommyBody.SetCollision (NULL); + + if (t >= dFloat (0.0f) && t <= dFloat (dgFloat32(1.0f))) { + attribute[0] = (dLong) contact.m_shapeId0; + + dgVector n (matrix.RotateVector (contact.m_normal)); + normal[0] = n[0]; + normal[1] = n[1]; + normal[2] = n[2]; + } + return t; +} + +/*! + Calculate an axis-aligned bounding box for this collision, the box is calculated relative to *offsetMatrix*. + + @param *collisionPtr pointer to the collision object. + @param *offsetMatrix pointer to an array of 16 floats containing the offset matrix used as the coordinate system and center of the AABB. + @param *p0 - pointer to an array of at least three floats to hold minimum value for the AABB. + @param *p1 - pointer to an array of at least three floats to hold maximum value for the AABB. + + @return Nothing. + + See also: ::NewtonCollisionClosestPoint, ::NewtonCollisionPointDistance, ::NewtonCollisionCollide, ::NewtonCollisionRayCast +*/ +void NewtonCollisionCalculateAABB(const NewtonCollision* const collisionPtr, const dFloat* const offsetMatrix, dFloat* const p0, dFloat* const p1) +{ + TRACE_FUNCTION(__FUNCTION__); + dgCollisionInstance* const collision = (dgCollisionInstance*) collisionPtr; + dgMatrix matrix (collision->GetLocalMatrix () * dgMatrix (offsetMatrix)); + + dgVector q0; + dgVector q1; + + collision->CalcAABB (matrix, q0, q1); + p0[0] = q0.m_x; + p0[1] = q0.m_y; + p0[2] = q0.m_z; + + p1[0] = q1.m_x; + p1[1] = q1.m_y; + p1[2] = q1.m_z; +} + +/*! + Iterate thought polygon of the collision geometry of a body calling the function callback. + + @param *collisionPtr is the pointer to the collision objects. + @param *matrixPtr is the pointer to the collision objects. + @param callback application define callback + @param *userDataPtr pointer to the user defined user data value. + + @return nothing + + This function used to be a member of the rigid body, but to making it a member of the collision object provides better + low lever display capabilities. The application can still call this function to show the collision of a rigid body by + getting the collision and the transformation matrix from the rigid, and then calling this functions. + + This function can be called by the application in order to show the collision geometry. The application should provide a pointer to the function *NewtonCollisionIterator*, + Newton will convert the collision geometry into a polygonal mesh, and will call *callback* for every polygon of the mesh + + this function affect severely the performance of Newton. The application should call this function only for debugging purpose + + This function will ignore user define collision mesh + See also: ::NewtonWorldGetFirstBody, ::NewtonWorldForEachBodyInAABBDo +*/ +void NewtonCollisionForEachPolygonDo(const NewtonCollision* const collisionPtr, const dFloat* const matrixPtr, NewtonCollisionIterator callback, void* const userDataPtr) +{ + TRACE_FUNCTION(__FUNCTION__); + dgCollisionInstance* const collision = (dgCollisionInstance*) (collisionPtr); + collision->DebugCollision (dgMatrix (matrixPtr), (dgCollision::OnDebugCollisionMeshCallback) callback, userDataPtr); +} + +int NewtonCollisionGetType(const NewtonCollision* const collision) +{ + TRACE_FUNCTION(__FUNCTION__); + dgCollisionInstance* const instance = (dgCollisionInstance*) collision; + return instance->GetCollisionPrimityType(); +} + +int NewtonCollisionIsConvexShape(const NewtonCollision* const collision) +{ + TRACE_FUNCTION(__FUNCTION__); + dgCollisionInstance* const instance = (dgCollisionInstance*)collision; + return instance->IsType (dgCollision::dgCollisionConvexShape_RTTI) ? 1 : 0; +} + +int NewtonCollisionIsStaticShape (const NewtonCollision* const collision) +{ + TRACE_FUNCTION(__FUNCTION__); + dgCollisionInstance* const instance = (dgCollisionInstance*)collision; + return (instance->IsType(dgCollision::dgCollisionMesh_RTTI) || instance->IsType(dgCollision::dgCollisionScene_RTTI)) ? 1 : 0; +} + +/*! + Store a user defined value with a convex collision primitive. + + @param collision is the pointer to a collision primitive. + @param id value to store with the collision primitive. + + @return nothing + + the application can store an id with any collision primitive. This id can be used to identify what type of collision primitive generated a contact. + + See also: ::NewtonCollisionGetUserID, ::NewtonCreateBox, ::NewtonCreateSphere +*/ +void NewtonCollisionSetUserID(const NewtonCollision* const collision, dLong id) +{ + TRACE_FUNCTION(__FUNCTION__); + dgCollisionInstance* const instance = (dgCollisionInstance*) collision; + instance->SetUserDataID (id); +} + +/*! + Return a user define value with a convex collision primitive. + + @param collision is the pointer to a convex collision primitive. + + @return user id + + the application can store an id with any collision primitive. This id can be used to identify what type of collision primitive generated a contact. + + See also: ::NewtonCreateBox, ::NewtonCreateSphere +*/ +dLong NewtonCollisionGetUserID(const NewtonCollision* const collision) +{ + TRACE_FUNCTION(__FUNCTION__); + dgCollisionInstance* const instance = (dgCollisionInstance*) collision; + return instance->GetUserDataID(); +} + +void NewtonCollisionSetUserData (const NewtonCollision* const collision, void* const userData) +{ + TRACE_FUNCTION(__FUNCTION__); + dgCollisionInstance* const instance = (dgCollisionInstance*) collision; + instance->SetUserData(userData); +} + +void* NewtonCollisionGetUserData (const NewtonCollision* const collision) +{ + TRACE_FUNCTION(__FUNCTION__); + dgCollisionInstance* const instance = (dgCollisionInstance*) collision; + return instance->GetUserData(); +} + +void NewtonCollisionSetMaterial (const NewtonCollision* const collision, const NewtonCollisionMaterial* const userData) +{ + TRACE_FUNCTION(__FUNCTION__); + dgCollisionInstance* const instance = (dgCollisionInstance*) collision; + dgCollisionInfo::dgInstanceMaterial& data = instance->m_material; + data.m_alignPad = userData->m_userData.m_int; + data.m_userId = userData->m_userId; + memcpy (data.m_userParam, userData->m_userParam, sizeof (data.m_userParam)); + instance->SetMaterial (data); +} + +void NewtonCollisionGetMaterial (const NewtonCollision* const collision, NewtonCollisionMaterial* const userData) +{ + TRACE_FUNCTION(__FUNCTION__); + dgCollisionInstance* const instance = (dgCollisionInstance*) collision; + const dgCollisionInfo::dgInstanceMaterial& data = instance->GetMaterial(); + userData->m_userId = data.m_userId; + userData->m_userData.m_int = data.m_alignPad; + memcpy (userData->m_userParam, data.m_userParam, sizeof (data.m_userParam)); +} + +void* NewtonCollisionGetSubCollisionHandle (const NewtonCollision* const collision) +{ + TRACE_FUNCTION(__FUNCTION__); + dgCollisionInstance* const instance = (dgCollisionInstance*) collision; + return (void*)instance->GetCollisionHandle (); +} + +NewtonCollision* NewtonCollisionGetParentInstance (const NewtonCollision* const collision) +{ + TRACE_FUNCTION(__FUNCTION__); + dgCollisionInstance* const instance = (dgCollisionInstance*) collision; + return (NewtonCollision*)instance->GetParent(); +} + + +void NewtonCollisionSetMatrix (const NewtonCollision* collision, const dFloat* const matrix) +{ + TRACE_FUNCTION(__FUNCTION__); + dgCollisionInstance* const instance = (dgCollisionInstance*) collision; + instance->SetLocalMatrix(dgMatrix (matrix)); +} + +void NewtonCollisionGetMatrix (const NewtonCollision* const collision, dFloat* const matrix) +{ + TRACE_FUNCTION(__FUNCTION__); + dgCollisionInstance* const instance = (dgCollisionInstance*) collision; + const dgMatrix& instanceMatrix = instance->GetLocalMatrix(); + memcpy (matrix, &instanceMatrix[0][0], sizeof (dgMatrix)); +} + + +void NewtonCollisionSetScale (const NewtonCollision* const collision, dFloat scaleX, dFloat scaleY, dFloat scaleZ) +{ + TRACE_FUNCTION(__FUNCTION__); + dgCollisionInstance* const instance = (dgCollisionInstance*) collision; + instance->SetScale(dgVector (scaleX, scaleY, scaleZ, dgFloat32 (0.0f))); +} + + +void NewtonCollisionGetScale (const NewtonCollision* const collision, dFloat* const scaleX, dFloat* const scaleY, dFloat* const scaleZ) +{ + TRACE_FUNCTION(__FUNCTION__); + dgCollisionInstance* const instance = (dgCollisionInstance*) collision; + + dgVector scale (instance->GetScale()); + *scaleX = scale.m_x; + *scaleY = scale.m_y; + *scaleZ = scale.m_z; +} + + + + +/*! + Release a reference from this collision object returning control to Newton. + + @param *collisionPtr pointer to the collision object + + @return Nothing. + + to get the correct reference count of a collision primitive the application can call function *NewtonCollisionGetInfo* + +*/ +void NewtonDestroyCollision(const NewtonCollision* const collisionPtr) +{ + TRACE_FUNCTION(__FUNCTION__); + dgCollisionInstance* const collision = (dgCollisionInstance*) collisionPtr; + collision->Release(); +} + +dFloat NewtonCollisionGetSkinThickness(const NewtonCollision* const collisionPtr) +{ + TRACE_FUNCTION(__FUNCTION__); + dgCollisionInstance* const collision = (dgCollisionInstance*)collisionPtr; + return collision->GetSkinThickness(); +} + +void NewtonCollisionSetSkinThickness(const NewtonCollision* const collisionPtr, dFloat thickness) +{ + TRACE_FUNCTION(__FUNCTION__); + dgCollisionInstance* const collision = (dgCollisionInstance*)collisionPtr; + collision->SetSkinThickness(thickness); +} + +NewtonCollision* NewtonCollisionCreateInstance (const NewtonCollision* const collision) +{ + TRACE_FUNCTION(__FUNCTION__); + dgCollisionInstance* const instance = (dgCollisionInstance*) collision; + return (NewtonCollision*) new (instance->GetAllocator()) dgCollisionInstance (*instance); +} + + + +/*! + Serialize a general collision shape. + + @param *newtonWorld Pointer to the Newton world. + @param *collision is the pointer to the collision tree shape. + @param serializeFunction pointer to the event function that will do the serialization. + @param *serializeHandle - user data that will be passed to the _NewtonSerialize_ callback. + + @return Nothing. + + Small and medium collision shapes like *TreeCollision* (under 50000 polygons) small convex hulls or compude collision can be constructed at application + startup without significant processing overhead. + + + See also: ::NewtonCollisionGetInfo +*/ +void NewtonCollisionSerialize(const NewtonWorld* const newtonWorld, const NewtonCollision* const collision, NewtonSerializeCallback serializeFunction, void* const serializeHandle) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *)newtonWorld; + world->SerializeCollision((dgCollisionInstance*) collision, (dgSerialize) serializeFunction, serializeHandle); +} + + +/*! + Create a collision shape via a serialization function. + + @param *newtonWorld Pointer to the Newton world. + @param deserializeFunction pointer to the event function that will do the deserialization. + @param *serializeHandle user data that will be passed to the _NewtonSerialize_ callback. + + @return Nothing. + + this function is useful to to load collision primitive for and archive file. In the case of complex shapes like convex hull and compound collision the + it save a significant amount of construction time. + + if this function is called to load a serialized tree collision, the tree collision will be loaded, but the function pointer callback will be set to NULL. + for this operation see function *NewtonCreateTreeCollisionFromSerialization* + + See also: ::NewtonCollisionSerialize, ::NewtonCollisionGetInfo +*/ +NewtonCollision* NewtonCreateCollisionFromSerialization(const NewtonWorld* const newtonWorld, NewtonDeserializeCallback deserializeFunction, void* const serializeHandle) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *)newtonWorld; + return (NewtonCollision*) world->CreateCollisionFromSerialization ((dgDeserialize) deserializeFunction, serializeHandle); +} + + +/*! + Get creation parameters for this collision objects. + + @param collision is the pointer to a convex collision primitive. + @param *collisionInfo pointer to a collision information record. + + This function can be used by the application for writing file format and for serialization. + + See also: ::NewtonCollisionGetInfo, ::NewtonCollisionSerialize +*/ +void NewtonCollisionGetInfo(const NewtonCollision* const collision, NewtonCollisionInfoRecord* const collisionInfo) +{ + TRACE_FUNCTION(__FUNCTION__); + dgCollisionInstance* const coll = (dgCollisionInstance*)collision; + + dgAssert ( dgInt32 (sizeof (dgCollisionInfo)) <= dgInt32 (sizeof (NewtonCollisionInfoRecord))); + dgCollisionInfo info; + coll->GetCollisionInfo (&info); + memcpy (collisionInfo, &info, sizeof (dgCollisionInfo)); +} + + +/*! @} */ // end of CollisionLibraryGeneric + + +/*! @defgroup TransUtil TransUtil +Transform utility functions +@{ +*/ + + +/*! + Get the three Euler angles from a 4x4 rotation matrix arranged in row-major order. + + @param matrix pointer to the 4x4 rotation matrix. + @param angles0 - fixme + @param angles1 - pointer to an array of at least three floats to hold the Euler angles. + + @return Nothing. + + The motivation for this function is that many graphics engines still use Euler angles to represent the orientation + of graphics entities. + The angles are expressed in radians and represent: + *angle[0]* - rotation about first matrix row + *angle[1]* - rotation about second matrix row + *angle[2]* - rotation about third matrix row + + See also: ::NewtonSetEulerAngle +*/ +void NewtonGetEulerAngle(const dFloat* const matrix, dFloat* const angles0, dFloat* const angles1) +{ + dgMatrix mat (matrix); + + TRACE_FUNCTION(__FUNCTION__); + dgVector euler0; + dgVector euler1; + mat.CalcPitchYawRoll (euler0, euler1); + + angles0[0] = euler0.m_x; + angles0[1] = euler0.m_y; + angles0[2] = euler0.m_z; + + angles1[0] = euler1.m_x; + angles1[1] = euler1.m_y; + angles1[2] = euler1.m_z; + +} + + +/*! + Build a rotation matrix from the Euler angles in radians. + + @param matrix pointer to the 4x4 rotation matrix. + @param angles pointer to an array of at least three floats to hold the Euler angles. + + @return Nothing. + + The motivation for this function is that many graphics engines still use Euler angles to represent the orientation + of graphics entities. + The angles are expressed in radians and represent: + *angle[0]* - rotation about first matrix row + *angle[1]* - rotation about second matrix row + *angle[2]* - rotation about third matrix row + + See also: ::NewtonGetEulerAngle +*/ +void NewtonSetEulerAngle(const dFloat* const angles, dFloat* const matrix) +{ + TRACE_FUNCTION(__FUNCTION__); + dgMatrix mat (dgPitchMatrix (angles[0]) * dgYawMatrix(angles[1]) * dgRollMatrix(angles[2])); + //dgMatrix retMatrix (matrix); + dgMatrix& retMatrix = *((dgMatrix*) matrix); + + for (dgInt32 i = 0; i < 3; i ++) { + retMatrix[3][i] = 0.0f; + for (dgInt32 j = 0; j < 4; j ++) { + retMatrix[i][j] = mat[i][j]; + } + } + retMatrix[3][3] = dgFloat32(1.0f); +} + + +/*! + Calculates the acceleration to satisfy the specified the spring damper system. + + @param dt integration time step. + @param ks spring stiffness, it must be a positive value. + @param x spring position. + @param kd desired spring damper, it must be a positive value. + @param s spring velocity. + + return: the spring acceleration. + + the acceleration calculated by this function represent the mass, spring system of the form + a = -ks * x - kd * v. +*/ +dFloat NewtonCalculateSpringDamperAcceleration(dFloat dt, dFloat ks, dFloat x, dFloat kd, dFloat v) +{ + TRACE_FUNCTION(__FUNCTION__); + //at = - (ks * x + kd * v); + //at = [- ks (x2 - x1) - kd * (v2 - v1) - dt * ks * (v2 - v1)] / [1 + dt * kd + dt * dt * ks] + dgFloat32 ksd = dt * ks; + dgFloat32 num = ks * x + kd * v + ksd * v; + dgFloat32 den = dgFloat32 (1.0f) + dt * kd + dt * ksd; + dgAssert (den > 0.0f); + dFloat accel = - num / den; + return accel; +} + +/*! @} */ // end of TransUtil + +/*! @defgroup RigidBodyInterface RigidBodyInterface +Rigid Body Interface +@{ +*/ + + +/*! + Create a rigid body. + + @param *newtonWorld Pointer to the Newton world. + @param *collisionPtr pointer to the collision object. + @param *matrixPtr fixme + + @return Pointer to the rigid body. + + This function creates a Newton rigid body and assigns a *collisionPtr* as the collision geometry representing the rigid body. + This function increments the reference count of the collision geometry. + All event functions are set to NULL and the material gruopID of the body is set to the default GroupID. + + See also: ::NewtonDestroyBody +*/ +NewtonBody* NewtonCreateDynamicBody(const NewtonWorld* const newtonWorld, const NewtonCollision* const collisionPtr, const dFloat* const matrixPtr) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *)newtonWorld; + dgCollisionInstance* collision = (dgCollisionInstance*)collisionPtr; + if (!collisionPtr) { + collision = (dgCollisionInstance*) NewtonCreateNull(newtonWorld); + } + + #ifdef SAVE_COLLISION + SaveCollision (collisionPtr); + #endif + + dgMatrix matrix (matrixPtr); + matrix.m_front.m_w = dgFloat32 (0.0f); + matrix.m_up.m_w = dgFloat32 (0.0f); + matrix.m_right.m_w = dgFloat32 (0.0f); + matrix.m_posit.m_w = dgFloat32 (1.0f); + + NewtonBody* const body = (NewtonBody*)world->CreateDynamicBody (collision, matrix); + if (!collisionPtr) { + NewtonDestroyCollision((NewtonCollision*)collision); + } + return body; +} + +NewtonBody* NewtonCreateAsymetricDynamicBody(const NewtonWorld* const newtonWorld, const NewtonCollision* const collisionPtr, const dFloat* const matrixPtr) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *)newtonWorld; + dgCollisionInstance* collision = (dgCollisionInstance*)collisionPtr; + if (!collisionPtr) { + collision = (dgCollisionInstance*)NewtonCreateNull(newtonWorld); + } + +#ifdef SAVE_COLLISION + SaveCollision(collisionPtr); +#endif + + dgMatrix matrix(matrixPtr); + matrix.m_front.m_w = dgFloat32(0.0f); + matrix.m_up.m_w = dgFloat32(0.0f); + matrix.m_right.m_w = dgFloat32(0.0f); + matrix.m_posit.m_w = dgFloat32(1.0f); + + NewtonBody* const body = (NewtonBody*)world->CreateDynamicBodyAsymetric(collision, matrix); + if (!collisionPtr) { + NewtonDestroyCollision((NewtonCollision*)collision); + } + return body; +} + +NewtonBody* NewtonCreateKinematicBody(const NewtonWorld* const newtonWorld, const NewtonCollision* const collisionPtr, const dFloat* const matrixPtr) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *)newtonWorld; + dgCollisionInstance* collision = (dgCollisionInstance*)collisionPtr; + if (!collisionPtr) { + collision = (dgCollisionInstance*)NewtonCreateNull(newtonWorld); + } + +#ifdef SAVE_COLLISION + SaveCollision (collisionPtr); +#endif + + dgMatrix matrix (matrixPtr); + matrix.m_front.m_w = dgFloat32 (0.0f); + matrix.m_up.m_w = dgFloat32 (0.0f); + matrix.m_right.m_w = dgFloat32 (0.0f); + matrix.m_posit.m_w = dgFloat32 (1.0f); + + NewtonBody* const body = (NewtonBody*) world->CreateKinematicBody(collision, matrix); + if (!collisionPtr) { + NewtonDestroyCollision((NewtonCollision*)collision); + } + return body; +} + + +/*! + Destroy a rigid body. + + @param *bodyPtr pointer to the body to be destroyed. + + @return Nothing. + + If this function is called from inside a simulation step the destruction of the body will be delayed until end of the time step. + This function will decrease the reference count of the collision geometry by one. If the reference count reaches zero, then the collision + geometry will be destroyed. This function will destroy all joints associated with this body. + + See also: ::NewtonCreateDynamicBody +*/ +void NewtonDestroyBody (const NewtonBody* const bodyPtr) +{ + TRACE_FUNCTION(__FUNCTION__); + dgBody* const body = (dgBody *)bodyPtr; + dgWorld* const world = body->GetWorld(); + world->DestroyBody(body); +} + +/* +void NewtonBodyEnableSimulation(const NewtonBody* const bodyPtr) +{ + TRACE_FUNCTION(__FUNCTION__); + dgBody* const body = (dgBody *)bodyPtr; + dgWorld* const world = body->GetWorld(); + world->BodyEnableSimulation (body); +} + +void NewtonBodyDisableSimulation(const NewtonBody* const bodyPtr) +{ + TRACE_FUNCTION(__FUNCTION__); + dgBody* const body = (dgBody *)bodyPtr; + dgWorld* const world = body->GetWorld(); + world->BodyDisableSimulation (body); +} +*/ + +/*! + Gets the current simulation state of the specified body. + + @param *bodyPtr pointer to the body to be inspected. + + @return the current simulation state 0: disabled 1: active. + + See also: ::NewtonBodySetSimulationState +*/ +int NewtonBodyGetSimulationState(const NewtonBody* const bodyPtr) +{ + TRACE_FUNCTION(__FUNCTION__); + dgBody* const body = (dgBody *)bodyPtr; + dgWorld* const world = body->GetWorld(); + return world->GetBodyEnableDisableSimulationState(body) ? 1 : 0; +} + +/*! + Sets the current simulation state of the specified body. + + @param *bodyPtr pointer to the body to be changed. + @param state the new simulation state 0: disabled 1: active + + @return Nothing. + + See also: ::NewtonBodyGetSimulationState +*/ +void NewtonBodySetSimulationState(const NewtonBody* const bodyPtr, const int state) +{ + TRACE_FUNCTION(__FUNCTION__); + dgBody* const body = (dgBody *)bodyPtr; + dgWorld* const world = body->GetWorld(); + + if (state) { + world->BodyEnableSimulation(body); + } else { + world->BodyDisableSimulation(body); + } +} + +int NewtonBodyGetCollidable (const NewtonBody* const bodyPtr) +{ + TRACE_FUNCTION(__FUNCTION__); + dgBody* const body = (dgBody *)bodyPtr; + return body->IsCollidable() ? 1 : 0; +} + +void NewtonBodySetCollidable (const NewtonBody* const bodyPtr, int collidable) +{ + TRACE_FUNCTION(__FUNCTION__); + dgBody* const body = (dgBody *)bodyPtr; + body->SetCollidable(collidable ? true : false); +} + +int NewtonBodyGetType (const NewtonBody* const bodyPtr) +{ + TRACE_FUNCTION(__FUNCTION__); + dgBody* const body = (dgBody *)bodyPtr; + if (body->IsRTTIType(dgBody::m_dynamicBodyRTTI)) { + return NEWTON_DYNAMIC_BODY; + } else if (body->IsRTTIType(dgBody::m_kinematicBodyRTTI)) { + return NEWTON_KINEMATIC_BODY; + } else if (body->IsRTTIType(dgBody::m_dynamicBodyAsymentricRTTI)) { + return NEWTON_DYNAMIC_ASYMETRIC_BODY; +// } else if (body->IsRTTIType(dgBody::m_deformableBodyRTTI)) { +// return NEWTON_DEFORMABLE_BODY; + } + dgAssert (0); + return 0; +} + +int NewtonBodyGetID (const NewtonBody* const bodyPtr) +{ + TRACE_FUNCTION(__FUNCTION__); + dgBody* const body = (dgBody *)bodyPtr; + return body->GetUniqueID(); +} + +/*! + Store a user defined data value with the body. + + @param *bodyPtr pointer to the body. + @param *userDataPtr pointer to the user defined user data value. + + @return Nothing. + + The application can store a user defined value with the Body. This value can be the pointer to a structure containing some application data for special effect. + if the application allocate some resource to store the user data, the application can register a joint destructor to get rid of the allocated resource when the body is destroyed + + See also: ::NewtonBodyGetUserData +*/ +void NewtonBodySetUserData(const NewtonBody* const bodyPtr, void* const userDataPtr) +{ + TRACE_FUNCTION(__FUNCTION__); + dgBody* const body = (dgBody *)bodyPtr; + body->SetUserData (userDataPtr); +} + +/*! + Retrieve a user defined data value stored with the body. + + @param *bodyPtr pointer to the body. + + @return The user defined data. + + The application can store a user defined value with a rigid body. This value can be the pointer + to a structure which is the graphical representation of the rigid body. + + See also: ::NewtonBodySetUserData, +*/ +void* NewtonBodyGetUserData(const NewtonBody* const bodyPtr) +{ + TRACE_FUNCTION(__FUNCTION__); + dgBody* const body = (dgBody *)bodyPtr; + return body->GetUserData (); +} + + +/*! + Return pointer to the Newton world of the specified body. + + @param *bodyPtr Pointer to the body. + + @return World that owns this body. + + The application can also determine the world from a joint, if it queries one + of the bodies attached to that joint. +*/ +NewtonWorld* NewtonBodyGetWorld(const NewtonBody* const bodyPtr) +{ + TRACE_FUNCTION(__FUNCTION__); + dgBody* const body = (dgBody *)bodyPtr; + return (NewtonWorld*) body->GetWorld(); +} + + +/*! + Assign a transformation event function to the body. + + @param *bodyPtr pointer to the body. + @param callback pointer to a function callback in used to update the transformation matrix of the visual object that represents the rigid body. + + @return Nothing. + + The function *NewtonSetTransform callback* is called by the Newton engine every time a visual object that represents the rigid body has changed. + The application can obtain the pointer user data value that points to the visual object. + The Newton engine does not call the *NewtonSetTransform callback* function for bodies that are inactive or have reached a state of stable equilibrium. + + The matrix should be organized in row-major order (this is the way directX and OpenGL stores matrices). + + See also: NewtonBodyGetTransformCallback +*/ +void NewtonBodySetTransformCallback(const NewtonBody* const bodyPtr, NewtonSetTransform callback) +{ + TRACE_FUNCTION(__FUNCTION__); + dgBody* const body = (dgBody *)bodyPtr; + body->SetMatrixUpdateCallback ((dgBody::OnMatrixUpdateCallback) callback); +} + + +/*! + Assign a transformation event function to the body. + + @param *bodyPtr pointer to the body. + + @return Nothing. + + The function *NewtonSetTransform callback* is called by the Newton engine every time a visual object that represents the rigid body has changed. + The application can obtain the pointer user data value that points to the visual object. + The Newton engine does not call the *NewtonSetTransform callback* function for bodies that are inactive or have reached a state of stable equilibrium. + + The matrix should be organized in row-major order (this is the way directX and OpenGL stores matrices). + + See also: ::NewtonBodySetTransformCallback +*/ +NewtonSetTransform NewtonBodyGetTransformCallback (const NewtonBody* const bodyPtr) +{ + TRACE_FUNCTION(__FUNCTION__); + dgBody* const body = (dgBody *)bodyPtr; + return (NewtonSetTransform) body->GetMatrixUpdateCallback(); +} + + +/*! + Assign an event function for applying external force and torque to a rigid body. + + @param *bodyPtr pointer to the body. + @param callback pointer to a function callback used to apply force and torque to a rigid body. + + @return Nothing. + + Before the *NewtonApplyForceAndTorque callback* is called for a body, Newton first clears the net force and net torque for the body. + + The function *NewtonApplyForceAndTorque callback* is called by the Newton Engine every time an active body is going to be simulated. + The Newton Engine does not call the *NewtonApplyForceAndTorque callback* function for bodies that are inactive or have reached a state of stable equilibrium. + + See also: ::NewtonBodyGetUserData, ::NewtonBodyGetUserData, ::NewtonBodyGetForceAndTorqueCallback +*/ +void NewtonBodySetForceAndTorqueCallback(const NewtonBody* const bodyPtr, NewtonApplyForceAndTorque callback) +{ + TRACE_FUNCTION(__FUNCTION__); + dgBody* const body = (dgBody *)bodyPtr; + body->SetExtForceAndTorqueCallback ((dgBody::OnApplyExtForceAndTorque) callback); +} + + +/*! + Return the pointer to the current force and torque call back function. + + @param *bodyPtr pointer to the body. + + @return pointer to the force call back. + + This function can be used to concatenate different force calculation components making more modular the + design of function components dedicated to apply special effect. For example a body may have a basic force a force that + only apply the effect of gravity, but that application can place a region in where there can be a fluid volume, or another gravity field. + we this function the application can read the correct function and save into a local variable, and set a new one. + this new function will firs call the save function pointer and upon return apply the correct effect. + this similar to the concept of virtual methods on objected oriented languages. + + The function *NewtonApplyForceAndTorque callback* is called by the Newton Engine every time an active body is going to be simulated. + The Newton Engine does not call the *NewtonApplyForceAndTorque callback* function for bodies that are inactive or have reached a state of stable equilibrium. + + See also: ::NewtonBodyGetUserData, ::NewtonBodyGetUserData, ::NewtonBodySetForceAndTorqueCallback +*/ +NewtonApplyForceAndTorque NewtonBodyGetForceAndTorqueCallback(const NewtonBody* const bodyPtr) +{ + TRACE_FUNCTION(__FUNCTION__); + dgBody* const body = (dgBody *)bodyPtr; + return (NewtonApplyForceAndTorque) body->GetExtForceAndTorqueCallback (); +} + + +/*! + Assign an event function to be called when this body is about to be destroyed. + + @param *bodyPtr pointer to the body to be destroyed. + @param callback pointer to a function callback. + + @return Nothing. + + + This function *NewtonBodyDestructor callback* acts like a destruction function in CPP. This function + is called when the body and all data joints associated with the body are about to be destroyed. + The application could use this function to destroy or release any resource associated with this body. + The application should not make reference to this body after this function returns. + + + The destruction of a body will destroy all joints associated with the body. + + See also: ::NewtonBodyGetUserData, ::NewtonBodyGetUserData +*/ +void NewtonBodySetDestructorCallback(const NewtonBody* const bodyPtr, NewtonBodyDestructor callback) +{ + TRACE_FUNCTION(__FUNCTION__); + dgBody* const body = (dgBody *)bodyPtr; + body->SetDestructorCallback (dgBody::OnBodyDestroy (callback)); +} + +NewtonBodyDestructor NewtonBodyGetDestructorCallback (const NewtonBody* const bodyPtr) +{ + TRACE_FUNCTION(__FUNCTION__); + dgBody* const body = (dgBody *)bodyPtr; + return NewtonBodyDestructor (body->GetDestructorCallback ()); +} + +/*! + Set the mass matrix of a rigid body. + + @param *bodyPtr pointer to the body. + @param mass mass value. + @param inertiaMatrix fixme + + @return Nothing. + + Newton algorithms have no restriction on the values for the mass, but due to floating point dynamic + range (24 bit precision) it is best if the ratio between the heaviest and the lightest body in the scene is limited to 200. + There are no special utility functions in Newton to calculate the moment of inertia of common primitives. + The application should specify the inertial values, keeping in mind that realistic inertia values are necessary for + realistic physics behavior. + + See also: ::NewtonConvexCollisionCalculateInertialMatrix, ::NewtonBodyGetMass, ::NewtonBodyGetInvMass +*/ +void NewtonBodySetFullMassMatrix(const NewtonBody* const bodyPtr, dFloat mass, const dFloat* const inertiaMatrix) +{ + TRACE_FUNCTION(__FUNCTION__); + dgBody* const body = (dgBody *)bodyPtr; + dgMatrix inertia(inertiaMatrix); + body->SetMassMatrix (mass, inertia); +} + + +void NewtonBodySetMassMatrix(const NewtonBody* const bodyPtr, dFloat mass, dFloat Ixx, dFloat Iyy, dFloat Izz) +{ + TRACE_FUNCTION(__FUNCTION__); + dgMatrix inertia (dgGetIdentityMatrix()); + inertia[0][0] = Ixx; + inertia[1][1] = Iyy; + inertia[2][2] = Izz; + NewtonBodySetFullMassMatrix(bodyPtr, mass, &inertia[0][0]); +} + +void NewtonBodySetMassProperties (const NewtonBody* const bodyPtr, dFloat mass, const NewtonCollision* const collisionPtr) +{ + TRACE_FUNCTION(__FUNCTION__); + dgBody* const body = (dgBody *)bodyPtr; + dgCollisionInstance* const collision = (dgCollisionInstance*) collisionPtr; + body->SetMassProperties (mass, collision); +} + +/*! + Get the mass matrix of a rigid body. + + @param *bodyPtr pointer to the body. + @param *mass pointer to a variable that will hold the mass value of the body. + @param *Ixx pointer to a variable that will hold the moment of inertia of the first principal axis of inertia of the body. + @param *Iyy pointer to a variable that will hold the moment of inertia of the first principal axis of inertia of the body. + @param *Izz pointer to a variable that will hold the moment of inertia of the first principal axis of inertia of the body. + + @return Nothing. + + See also: ::NewtonBodySetMassMatrix, ::NewtonBodyGetInvMass +*/ +void NewtonBodyGetMass(const NewtonBody* const bodyPtr, dFloat* const mass, dFloat* const Ixx, dFloat* const Iyy, dFloat* const Izz) +{ + TRACE_FUNCTION(__FUNCTION__); + dgBody* const body = (dgBody *)bodyPtr; + +// dgVector vector (body->GetApparentMass()); + dgVector vector (body->GetMass()); + Ixx[0] = vector.m_x; + Iyy[0] = vector.m_y; + Izz[0] = vector.m_z; + mass[0] = vector.m_w; + if (vector.m_w > DG_INFINITE_MASS * 0.5f) { + Ixx[0] = 0.0f; + Iyy[0] = 0.0f; + Izz[0] = 0.0f; + mass[0] = 0.0f; + } +} + +/*! + Get the inverse mass matrix of a rigid body. + + @param *bodyPtr pointer to the body. + @param *invMass pointer to a variable that will hold the mass inverse value of the body. + @param *invIxx pointer to a variable that will hold the moment of inertia inverse of the first principal axis of inertia of the body. + @param *invIyy pointer to a variable that will hold the moment of inertia inverse of the first principal axis of inertia of the body. + @param *invIzz pointer to a variable that will hold the moment of inertia inverse of the first principal axis of inertia of the body. + + @return Nothing. + + See also: ::NewtonBodySetMassMatrix, ::NewtonBodyGetMass +*/ +void NewtonBodyGetInvMass(const NewtonBody* const bodyPtr, dFloat* const invMass, dFloat* const invIxx, dFloat* const invIyy, dFloat* const invIzz) +{ + TRACE_FUNCTION(__FUNCTION__); + dgBody* const body = (dgBody *)bodyPtr; + +// dgVector vector1 (body->GetMass()); +// invIxx[0] = dgFloat32 (1.0f) / (vector1.m_x + dgFloat32 (1.0e-8f)); +// invIyy[0] = dgFloat32 (1.0f) / (vector1.m_y + dgFloat32 (1.0e-8f)); +// invIzz[0] = dgFloat32 (1.0f) / (vector1.m_z + dgFloat32 (1.0e-8f)); +// invMass[0] = dgFloat32 (1.0f) / (vector1.m_w + dgFloat32 (1.0e-8f)); + dgVector inverseMass (body->GetInvMass()); + invIxx[0] = inverseMass.m_x; + invIyy[0] = inverseMass.m_y; + invIzz[0] = inverseMass.m_z; + invMass[0] = inverseMass.m_w; +} + + +void NewtonBodyGetInertiaMatrix(const NewtonBody* const bodyPtr, dFloat* const inertiaMatrix) +{ + TRACE_FUNCTION(__FUNCTION__); + dgBody* const body = (dgBody *)bodyPtr; + + dgMatrix matrix (body->CalculateInertiaMatrix()); + memcpy (inertiaMatrix, &matrix[0][0], sizeof (dgMatrix)); +} + +void NewtonBodyGetInvInertiaMatrix(const NewtonBody* const bodyPtr, dFloat* const invInertiaMatrix) +{ + TRACE_FUNCTION(__FUNCTION__); + dgBody* const body = (dgBody *)bodyPtr; + + dgMatrix matrix (body->CalculateInvInertiaMatrix ()); + memcpy (invInertiaMatrix, &matrix[0][0], sizeof (dgMatrix)); +} + + + +/*! + Set the transformation matrix of a rigid body. + + @param *bodyPtr pointer to the body. + @param *matrixPtr pointer to an array of 16 floats containing the global matrix of the rigid body. + + @return Nothing. + + The matrix should be arranged in row-major order. + If you are using OpenGL matrices (column-major) you will need to transpose you matrices into a local array, before + passing them to Newton. + + That application should make sure the transformation matrix has not scale, otherwise unpredictable result will occur. + + See also: ::NewtonBodyGetMatrix +*/ +void NewtonBodySetMatrix(const NewtonBody* const bodyPtr, const dFloat* const matrixPtr) +{ + TRACE_FUNCTION(__FUNCTION__); + dgBody* const body = (dgBody *)bodyPtr; + dgMatrix matrix (matrixPtr); + + matrix.m_front.m_w = dgFloat32 (0.0f); + matrix.m_up.m_w = dgFloat32 (0.0f); + matrix.m_right.m_w = dgFloat32 (0.0f); + matrix.m_posit.m_w = dgFloat32 (1.0f); + body->SetMatrixResetSleep (matrix); +} + +void NewtonBodySetMatrixNoSleep (const NewtonBody* const bodyPtr, const dFloat* const matrixPtr) +{ + TRACE_FUNCTION(__FUNCTION__); + dgBody* const body = (dgBody *)bodyPtr; + dgMatrix matrix(matrixPtr); + + matrix.m_front.m_w = dgFloat32(0.0f); + matrix.m_up.m_w = dgFloat32(0.0f); + matrix.m_right.m_w = dgFloat32(0.0f); + matrix.m_posit.m_w = dgFloat32(1.0f); + body->SetMatrixNoSleep(matrix); +} + +/*! + Apply hierarchical transformation to a body. + + @param *bodyPtr pointer to the body. + @param *matrixPtr pointer to an array of 16 floats containing the global matrix of the rigid body. + + @return Nothing. + + This function applies the transformation matrix to the *body* and also applies the appropriate transformation matrix to + set of articulated bodies. If the body is in contact with another body the other body is not transformed. + + this function should not be used to transform set of articulated bodies that are connected to a static body. + doing so will result in unpredictables results. Think for example moving a chain attached to a ceiling from one place to another, + to do that in real life a person first need to disconnect the chain (destroy the joint), move the chain (apply the transformation to the + entire chain), the reconnect it in the new position (recreate the joint again). + + this function will set to zero the linear and angular velocity of all bodies that are part of the set of articulated body array. + + The matrix should be arranged in row-major order (this is the way direct x stores matrices). + If you are using OpenGL matrices (column-major) you will need to transpose you matrices into a local array, before + passing them to Newton. + + See also: ::NewtonBodySetMatrix +*/ +void NewtonBodySetMatrixRecursive(const NewtonBody* const bodyPtr, const dFloat* const matrixPtr) +{ + TRACE_FUNCTION(__FUNCTION__); + dgBody* const body = (dgBody *)bodyPtr; + Newton* const world = (Newton *)body->GetWorld(); + + world->BodySetMatrix (body, dgMatrix (matrixPtr)); +} + + +/*! + Get the transformation matrix of a rigid body. + + @param *bodyPtr pointer to the body. + @param *matrixPtr pointer to an array of 16 floats that will hold the global matrix of the rigid body. + + @return Nothing. + + The matrix should be arranged in row-major order (this is the way direct x stores matrices). + If you are using OpenGL matrices (column-major) you will need to transpose you matrices into a local array, before + passing them to Newton. + + See also: ::NewtonBodySetMatrix, ::NewtonBodyGetRotation +*/ +void NewtonBodyGetMatrix(const NewtonBody* const bodyPtr, dFloat* const matrixPtr) +{ + TRACE_FUNCTION(__FUNCTION__); + dgBody* const body = (dgBody *)bodyPtr; + const dgMatrix& matrix = body->GetMatrix(); + memcpy (matrixPtr, &matrix[0][0], sizeof (dgMatrix)); +} + +void NewtonBodyGetPosition(const NewtonBody* const bodyPtr, dFloat* const posPtr) +{ + TRACE_FUNCTION(__FUNCTION__); + dgBody* const body = (dgBody *)bodyPtr; + const dgVector& rot = body->GetPosition(); + posPtr[0] = rot.m_x; + posPtr[1] = rot.m_y; + posPtr[2] = rot.m_z; +} + +/*! + Get the rotation part of the transformation matrix of a body, in form of a unit quaternion. + + @param *bodyPtr pointer to the body. + @param *rotPtr pointer to an array of 4 floats that will hold the global rotation of the rigid body. + + @return Nothing. + + The rotation matrix is written set in the form of a unit quaternion in the format Rot (q0, q1, q1, q3) + + The rotation quaternion is the same as what the application would get by using at function to extract a quaternion form a matrix. + however since the rigid body already contained the rotation in it, it is more efficient to just call this function avoiding expensive conversion. + + this function could be very useful for the implementation of pseudo frame rate independent simulation. + by running the simulation at a fix rate and using linear interpolation between the last two simulation frames. + to determine the exact fraction of the render step. + + See also: ::NewtonBodySetMatrix, ::NewtonBodyGetMatrix +*/ +void NewtonBodyGetRotation(const NewtonBody* const bodyPtr, dFloat* const rotPtr) +{ + TRACE_FUNCTION(__FUNCTION__); + dgBody* const body = (dgBody *)bodyPtr; + const dgQuaternion& rot = body->GetRotation(); + rotPtr[0] = rot.m_x; + rotPtr[1] = rot.m_y; + rotPtr[2] = rot.m_z; + rotPtr[3] = rot.m_w; +} + + +/*! + Set the net force applied to a rigid body. + + @param *bodyPtr pointer to the body. + @param *vectorPtr pointer to an array of 3 floats containing the net force to be applied to the body. + + @return Nothing. + + This function is only effective when called from *NewtonApplyForceAndTorque callback* + + See also: ::NewtonBodyAddForce, ::NewtonBodyGetForce +*/ +void NewtonBodySetForce(const NewtonBody* const bodyPtr, const dFloat* const vectorPtr) +{ + TRACE_FUNCTION(__FUNCTION__); + dgBody* const body = (dgBody *)bodyPtr; + dgVector vector (vectorPtr[0], vectorPtr[1], vectorPtr[2], dgFloat32 (0.0f)); + body->SetForce (vector); +} + +/*! + Add the net force applied to a rigid body. + + @param *bodyPtr pointer to the body to be destroyed. + @param *vectorPtr pointer to an array of 3 floats containing the net force to be applied to the body. + + @return Nothing. + + This function is only effective when called from *NewtonApplyForceAndTorque callback* + + See also: ::NewtonBodySetForce, ::NewtonBodyGetForce +*/ +void NewtonBodyAddForce(const NewtonBody* const bodyPtr, const dFloat* const vectorPtr) +{ + TRACE_FUNCTION(__FUNCTION__); + dgBody* const body = (dgBody *)bodyPtr; + dgVector vector (vectorPtr[0], vectorPtr[1], vectorPtr[2], dgFloat32 (0.0f)); + + body->AddForce (vector); +} + +/*! + Get the net force applied to a rigid body after the last NewtonUpdate. + + @param *bodyPtr pointer to the body. + @param *vectorPtr pointer to an array of 3 floats to hold the net force of the body. + + @return Nothing. + + See also: ::NewtonBodyAddForce, ::NewtonBodyGetForce +*/ +void NewtonBodyGetForce(const NewtonBody* const bodyPtr, dFloat* const vectorPtr) +{ + TRACE_FUNCTION(__FUNCTION__); + dgBody* const body = (dgBody *)bodyPtr; + dgVector vector (body->GetForce()); + vectorPtr[0] = vector.m_x; + vectorPtr[1] = vector.m_y; + vectorPtr[2] = vector.m_z; +} + +/*! + Set the net torque applied to a rigid body. + + @param *bodyPtr pointer to the body. + @param *vectorPtr pointer to an array of 3 floats containing the net torque to be applied to the body. + + @return Nothing. + + This function is only effective when called from *NewtonApplyForceAndTorque callback* + + See also: ::NewtonBodyAddTorque, ::NewtonBodyGetTorque +*/ +void NewtonBodySetTorque(const NewtonBody* const bodyPtr, const dFloat* const vectorPtr) +{ + TRACE_FUNCTION(__FUNCTION__); + dgBody* const body = (dgBody *)bodyPtr; + dgVector vector (vectorPtr[0], vectorPtr[1], vectorPtr[2], dgFloat32 (0.0f)); + body->SetTorque (vector); +} + + +/*! + Add the net torque applied to a rigid body. + + @param *bodyPtr pointer to the body. + @param *vectorPtr pointer to an array of 3 floats containing the net torque to be applied to the body. + + @return Nothing. + + This function is only effective when called from *NewtonApplyForceAndTorque callback* + + See also: ::NewtonBodySetTorque, ::NewtonBodyGetTorque +*/ +void NewtonBodyAddTorque(const NewtonBody* const bodyPtr, const dFloat* const vectorPtr) +{ + TRACE_FUNCTION(__FUNCTION__); + dgBody* const body = (dgBody *)bodyPtr; + dgVector vector (vectorPtr[0], vectorPtr[1], vectorPtr[2], dgFloat32 (0.0f)); + body->AddTorque (vector); +} + +/*! + Get the net torque applied to a rigid body after the last NewtonUpdate. + + @param *bodyPtr pointer to the body. + @param *vectorPtr pointer to an array of 3 floats to hold the net torque of the body. + + @return Nothing. + + See also: ::NewtonBodyAddTorque, ::NewtonBodyGetTorque +*/ +void NewtonBodyGetTorque(const NewtonBody* const bodyPtr, dFloat* const vectorPtr) +{ + TRACE_FUNCTION(__FUNCTION__); + dgBody* const body = (dgBody *)bodyPtr; + dgVector vector (body->GetTorque()); + vectorPtr[0] = vector.m_x; + vectorPtr[1] = vector.m_y; + vectorPtr[2] = vector.m_z; +} + + + +/*! + Set the relative position of the center of mass of a rigid body. + + @param *bodyPtr pointer to the body. + @param *comPtr pointer to an array of 3 floats containing the relative offset of the center of mass of the body. + + @return Nothing. + + This function can be used to set the relative offset of the center of mass of a rigid body. + when a rigid body is created the center of mass is set the the point c(0, 0, 0), and normally this is + the best setting for a rigid body. However the are situations in which and object does not have symmetry or + simple some kind of special effect is desired, and this origin need to be changed. + + Care must be taken when offsetting the center of mass of a body. + The application must make sure that the external torques resulting from forces applied at at point + relative to the center of mass are calculated appropriately. + this could be done Transform and Torque callback function as the follow pseudo code fragment shows: + + Matrix matrix; + Vector center; + + NewtonGetMatrix(body, matrix) + NewtonGetCentreOfMass(body, center); + + //for global space torque. + Vector localForce (fx, fy, fz); + Vector localPosition (x, y, z); + Vector localTorque (crossproduct ((localPosition - center). localForce); + Vector globalTorque (matrix.RotateVector (localTorque)); + + //for global space torque. + Vector globalCentre (matrix.TranformVector (center)); + Vector globalPosition (x, y, z); + Vector globalForce (fx, fy, fz); + Vector globalTorque (crossproduct ((globalPosition - globalCentre). globalForce); + + See also: ::NewtonConvexCollisionCalculateInertialMatrix, ::NewtonBodyGetCentreOfMass +*/ +void NewtonBodySetCentreOfMass(const NewtonBody* const bodyPtr, const dFloat* const comPtr) +{ + TRACE_FUNCTION(__FUNCTION__); + dgBody* const body = (dgBody *)bodyPtr; + dgVector vector (comPtr[0], comPtr[1], comPtr[2], dgFloat32 (1.0f)); + body->SetCentreOfMass (vector); +} + +/*! + Get the relative position of the center of mass of a rigid body. + + @param *bodyPtr pointer to the body. + @param *comPtr pointer to an array of 3 floats to hold the relative offset of the center of mass of the body. + + @return Nothing. + + This function can be used to set the relative offset of the center of mass of a rigid body. + when a rigid body is created the center of mass is set the the point c(0, 0, 0), and normally this is + the best setting for a rigid body. However the are situations in which and object does not have symmetry or + simple some kind of special effect is desired, and this origin need to be changed. + + This function can be used in conjunction with *NewtonConvexCollisionCalculateInertialMatrix* + + See also: ::NewtonConvexCollisionCalculateInertialMatrix, ::NewtonBodySetCentreOfMass +*/ +void NewtonBodyGetCentreOfMass(const NewtonBody* const bodyPtr, dFloat* const comPtr) +{ + TRACE_FUNCTION(__FUNCTION__); + dgBody* const body = (dgBody *)bodyPtr; + dgVector vector (body->GetCentreOfMass ()); + comPtr[0] = vector.m_x; + comPtr[1] = vector.m_y; + comPtr[2] = vector.m_z; +} + + +/*! + Return a pointer to the first joint attached to this rigid body. + + @param *bodyPtr pointer to the body. + + @return Joint if at least one is attached to the body, NULL if not joint is attached + + this function will only return the pointer to user defined joints, older build in constraints will be skipped by this function. + + this function can be used to implement recursive walk of complex articulated arrangement of rodid bodies. + + See also: ::NewtonBodyGetNextJoint +*/ +NewtonJoint* NewtonBodyGetFirstJoint(const NewtonBody* const bodyPtr) +{ + TRACE_FUNCTION(__FUNCTION__); + dgBody* const body = (dgBody *)bodyPtr; + return (NewtonJoint*)body->GetFirstJoint(); +} + +/*! + Return a pointer to the next joint attached to this body. + + @param *bodyPtr pointer to the body. + @param *jointPtr pointer to current joint. + + @return Joint is at least one joint is attached to the body, NULL if not joint is attached + + this function will only return the pointer to User defined joint, older build in constraints will be skipped by this function. + + this function can be used to implement recursive walk of complex articulated arrangement of rodid bodies. + + See also: ::NewtonBodyGetFirstJoint +*/ +NewtonJoint* NewtonBodyGetNextJoint(const NewtonBody* const bodyPtr, const NewtonJoint* const jointPtr) +{ + TRACE_FUNCTION(__FUNCTION__); + dgBody* const body = (dgBody *)bodyPtr; + return (NewtonJoint*)body->GetNextJoint((dgConstraint*)jointPtr); +} + + +/*! + Return a pointer to the first contact joint attached to this rigid body. + + @param *bodyPtr pointer to the body. + + @return Contact if the body is colliding with anther body, NULL otherwise + + See also: ::NewtonBodyGetNextContactJoint, ::NewtonContactJointGetFirstContact, ::NewtonContactJointGetNextContact, ::NewtonContactJointRemoveContact +*/ +NewtonJoint* NewtonBodyGetFirstContactJoint(const NewtonBody* const bodyPtr) +{ + TRACE_FUNCTION(__FUNCTION__); + dgBody* const body = (dgBody *)bodyPtr; + return (NewtonJoint*)body->GetFirstContact(); +} + +/*! + Return a pointer to the next contact joint attached to this rigid body. + + @param *bodyPtr pointer to the body. + @param *contactPtr pointer to corrent contact joint. + + @return Contact if the body is colliding with anther body, NULL otherwise + + See also: ::NewtonBodyGetFirstContactJoint, ::NewtonContactJointGetFirstContact, ::NewtonContactJointGetNextContact, ::NewtonContactJointRemoveContact +*/ +NewtonJoint* NewtonBodyGetNextContactJoint(const NewtonBody* const bodyPtr, const NewtonJoint* const contactPtr) +{ + TRACE_FUNCTION(__FUNCTION__); + dgBody* const body = (dgBody *)bodyPtr; + return (NewtonJoint*)body->GetNextContact((dgConstraint*)contactPtr); +} + + +/*! + Return a pointer to the next contact joint the connect these two bodies, if the are colliding + + @param *body0 pointer to one body. + @param *body1 pointer to secund body. + + @return Contact if the body is colliding with anther body, NULL otherwise + + See also: ::NewtonBodyGetFirstContactJoint, ::NewtonContactJointGetFirstContact, ::NewtonContactJointGetNextContact, ::NewtonContactJointRemoveContact +*/ +NewtonJoint* NewtonBodyFindContact(const NewtonBody* const body0, const NewtonBody* const body1) +{ + TRACE_FUNCTION(__FUNCTION__); + const dgBody* const bodyPtr0 = (dgBody*)body0; + const dgBody* const bodyPtr1 = (dgBody*)body1; + return (NewtonJoint*)bodyPtr0->GetWorld()->FindContactJoint(bodyPtr0, bodyPtr1); +} + +int NewtonJointIsActive(const NewtonJoint* const jointPtr) +{ + TRACE_FUNCTION(__FUNCTION__); + dgConstraint* const joint = (dgConstraint*) jointPtr; + return joint->IsActive() ? 1 : 0; +} + +/*! + Return to number of contact in this contact joint. + + @param *contactJoint pointer to corrent contact joint. + + @return number of contacts. + + See also: ::NewtonContactJointGetFirstContact, ::NewtonContactJointGetNextContact, ::NewtonContactJointRemoveContact +*/ +int NewtonContactJointGetContactCount(const NewtonJoint* const contactJoint) +{ + TRACE_FUNCTION(__FUNCTION__); + dgContact* const joint = (dgContact *)contactJoint; + + if ((joint->GetId() == dgConstraint::m_contactConstraint) && joint->GetCount()){ + return joint->GetCount(); + } else { + return 0; + } +} + + +/*! + Return to pointer to the first contact from the contact array of the contact joint. + + @param *contactJoint pointer to a contact joint. + + @return a pointer to the first contact from the contact array, NULL if no contacts exist + + See also: ::NewtonContactJointGetNextContact, ::NewtonContactGetMaterial, ::NewtonContactJointRemoveContact +*/ +void* NewtonContactJointGetFirstContact(const NewtonJoint* const contactJoint) +{ + TRACE_FUNCTION(__FUNCTION__); + dgContact* const joint = (dgContact *)contactJoint; + if ((joint->GetId() == dgConstraint::m_contactConstraint) && joint->GetCount() && joint->GetMaxDOF()){ + return joint->GetFirst(); + } else { + return NULL; + } +} + +/*! + Return a pointer to the next contact from the contact array of the contact joint. + + @param *contactJoint pointer to a contact joint. + @param *contact pointer to current contact. + + @return a pointer to the next contact in the contact array, NULL if no contacts exist. + + See also: ::NewtonContactJointGetFirstContact, ::NewtonContactGetMaterial, ::NewtonContactJointRemoveContact +*/ +void* NewtonContactJointGetNextContact(const NewtonJoint* const contactJoint, void* const contact) +{ + TRACE_FUNCTION(__FUNCTION__); + dgContact* const joint = (dgContact *)contactJoint; + + if ((joint->GetId() == dgConstraint::m_contactConstraint) && joint->GetCount()){ + dgList::dgListNode* const node = (dgList::dgListNode*) contact; + return node->GetNext(); + } else { + return NULL; + } +} + + +/*! + Return to the next contact from the contact array of the contact joint. + + @param *contactJoint pointer to corrent contact joint. + @param *contact pointer to current contact. + + @return first contact contact array of the joint contact exist, NULL otherwise + + See also: ::NewtonBodyGetFirstContactJoint, ::NewtonBodyGetNextContactJoint, ::NewtonContactJointGetFirstContact, ::NewtonContactJointGetNextContact +*/ +void NewtonContactJointRemoveContact(const NewtonJoint* const contactJoint, void* const contact) +{ + TRACE_FUNCTION(__FUNCTION__); + dgContact* const joint = (dgContact *)contactJoint; + + if ((joint->GetId() == dgConstraint::m_contactConstraint) && joint->GetCount()){ + dgList::dgListNode* const node = (dgList::dgListNode*) contact; + + dgAssert (joint->GetBody0()); + dgAssert (joint->GetBody1()); + dgWorld* const world = joint->GetBody0()->GetWorld(); + world->GlobalLock(); + joint->Remove(node); + joint->GetBody0()->SetSleepState(false); + joint->GetBody1()->SetSleepState(false); + world->GlobalUnlock(); + } +} + +dFloat NewtonContactJointGetClosestDistance(const NewtonJoint* const contactJoint) +{ + TRACE_FUNCTION(__FUNCTION__); + dgContact* const joint = (dgContact *)contactJoint; + return joint->GetClosestDistance(); +} + +void NewtonContactJointResetSelftJointCollision(const NewtonJoint* const contactJoint) +{ + TRACE_FUNCTION(__FUNCTION__); + dgContact* const joint = (dgContact *)contactJoint; + joint->ResetSkeletonSelftCollision(); +} + +void NewtonContactJointResetIntraJointCollision(const NewtonJoint* const contactJoint) +{ + TRACE_FUNCTION(__FUNCTION__); + dgContact* const joint = (dgContact *)contactJoint; + joint->ResetSkeletonIntraCollision(); +} + +/*! + Return to the next contact from the contact array of the contact joint. + + @param *contact pointer to current contact. + + @return first contact contact array of the joint contact exist, NULL otherwise + + See also: ::NewtonContactJointGetFirstContact, ::NewtonContactJointGetNextContact +*/ +NewtonMaterial* NewtonContactGetMaterial(const void* const contact) +{ + TRACE_FUNCTION(__FUNCTION__); + + dgList::dgListNode* const node = (dgList::dgListNode*) contact; + dgContactMaterial& contactMaterial = node->GetInfo(); + return (NewtonMaterial*) &contactMaterial; +} + +NewtonCollision* NewtonContactGetCollision0(const void* const contact) +{ + TRACE_FUNCTION(__FUNCTION__); + + dgList::dgListNode* const node = (dgList::dgListNode*) contact; + dgContactMaterial& contactMaterial = node->GetInfo(); + return (NewtonCollision*) contactMaterial.m_collision0; +} + +NewtonCollision* NewtonContactGetCollision1(const void* const contact) +{ + TRACE_FUNCTION(__FUNCTION__); + + dgList::dgListNode* const node = (dgList::dgListNode*) contact; + dgContactMaterial& contactMaterial = node->GetInfo(); + return (NewtonCollision*) contactMaterial.m_collision1; +} + +void* NewtonContactGetCollisionID0(const void* const contact) +{ + TRACE_FUNCTION(__FUNCTION__); + + dgList::dgListNode* const node = (dgList::dgListNode*) contact; + dgContactMaterial& contactMaterial = node->GetInfo(); + return (void*) contactMaterial.m_shapeId0; +} + +void* NewtonContactGetCollisionID1(const void* const contact) +{ + TRACE_FUNCTION(__FUNCTION__); + + dgList::dgListNode* const node = (dgList::dgListNode*) contact; + dgContactMaterial& contactMaterial = node->GetInfo(); + return (NewtonCollision*) contactMaterial.m_shapeId1; +} + + + + +/*! + Assign a collision primitive to the body. + + @param *bodyPtr pointer to the body. + @param *collisionPtr pointer to the new collision geometry. + + @return Nothing. + + This function replaces a collision geometry of a body with the new collision geometry. + This function increments the reference count of the collision geometry and decrements the reference count + of the old collision geometry. If the reference count of the old collision geometry reaches zero, the old collision geometry is destroyed. + This function can be used to swap the collision geometry of bodies at runtime. + + See also: ::NewtonCreateDynamicBody, ::NewtonBodyGetCollision +*/ +void NewtonBodySetCollision(const NewtonBody* const bodyPtr, const NewtonCollision* const collisionPtr) +{ + TRACE_FUNCTION(__FUNCTION__); + dgBody* const body = (dgBody *)bodyPtr; + dgCollisionInstance* const collision = (dgCollisionInstance*) collisionPtr; + body->AttachCollision (collision); + body->UpdateCollisionMatrix(dgFloat32(0.0f), 0); +} + +void NewtonBodySetCollisionScale (const NewtonBody* const bodyPtr, dFloat scaleX, dFloat scaleY, dFloat scaleZ) +{ + TRACE_FUNCTION(__FUNCTION__); + dgBody* const body = (dgBody *)bodyPtr; + dgWorld* const world = body->GetWorld(); + NewtonCollision* const collision = NewtonBodyGetCollision(bodyPtr); + + dgFloat32 mass = body->GetInvMass().m_w > dgFloat32 (0.0f) ? body->GetMass().m_w : dgFloat32 (0.0f); + NewtonCollisionSetScale (collision, scaleX, scaleY, scaleZ); + + NewtonJoint* nextJoint; + for (NewtonJoint* joint = NewtonBodyGetFirstContactJoint(bodyPtr); joint; joint = nextJoint) { + dgConstraint* const contactJoint = (dgConstraint*)joint; + nextJoint = NewtonBodyGetNextContactJoint(bodyPtr, joint); + //world->DestroyConstraint (contactJoint); + contactJoint->ResetMaxDOF(); + } + NewtonBodySetMassProperties (bodyPtr, mass, collision); + body->UpdateCollisionMatrix(dgFloat32(0.0f), 0); + world->GetBroadPhase()->ResetEntropy (); +} + + +/*! + Get the collision primitive of a body. + + @param *bodyPtr pointer to the body. + + @return Pointer to body collision geometry. + + This function does not increment the reference count of the collision geometry. + + See also: ::NewtonCreateDynamicBody, ::NewtonBodySetCollision +*/ +NewtonCollision* NewtonBodyGetCollision(const NewtonBody* const bodyPtr) +{ + TRACE_FUNCTION(__FUNCTION__); + dgBody* const body = (dgBody *)bodyPtr; + return (NewtonCollision*) body->GetCollision(); +} + + +/*! + Assign a material group id to the body. + + @param *bodyPtr pointer to the body. + @param id id of a previously created material group. + + @return Nothing. + + When the application creates a body, the default material group, *defaultGroupId*, is applied by default. + + See also: ::NewtonBodyGetMaterialGroupID, ::NewtonMaterialCreateGroupID, ::NewtonMaterialGetDefaultGroupID +*/ +void NewtonBodySetMaterialGroupID(const NewtonBody* const bodyPtr, int id) +{ + dgBody* const body = (dgBody *)bodyPtr; + + TRACE_FUNCTION(__FUNCTION__); + body->SetGroupID (dgUnsigned32 (id)); +} + + +/*! + Get the material group id of the body. + + @param *bodyPtr pointer to the body. + + @return Nothing. + + See also: ::NewtonBodySetMaterialGroupID +*/ +int NewtonBodyGetMaterialGroupID(const NewtonBody* const bodyPtr) +{ + TRACE_FUNCTION(__FUNCTION__); + dgBody* const body = (dgBody *)bodyPtr; + return int (body->GetGroupID ()); +} + +/*! + Set the continuous collision state mode for this rigid body. + continuous collision flag is off by default in when bodies are created. + + @param *bodyPtr pointer to the body. + @param state collision state. 1 indicates this body may tunnel through other objects while moving at high speed. 0 ignore high speed collision checks. + + @return Nothing. + + continuous collision mode enable allow the engine to predict colliding contact on rigid bodies + Moving at high speed of subject to strong forces. + + continuous collision mode does not prevent rigid bodies from inter penetration instead it prevent bodies from + passing trough each others by extrapolating contact points when the bodies normal contact calculation determine the bodies are not colliding. + + for performance reason the bodies angular velocities is only use on the broad face of the collision, + but not on the contact calculation. + + continuous collision does not perform back tracking to determine time of contact, instead it extrapolate contact by incrementally + extruding the collision geometries of the two colliding bodies along the linear velocity of the bodies during the time step, + if during the extrusion colliding contact are found, a collision is declared and the normal contact resolution is called. + + for continuous collision to be active the continuous collision mode must on the material pair of the colliding bodies as well as on at least one of the two colliding bodies. + + Because there is penalty of about 40% to 80% depending of the shape complexity of the collision geometry, this feature is set + off by default. It is the job of the application to determine what bodies need this feature on. Good guidelines are: very small objects, + and bodies that move a height speed. + + See also: ::NewtonBodyGetContinuousCollisionMode, ::NewtonBodySetContinuousCollisionMode +*/ +void NewtonBodySetContinuousCollisionMode(const NewtonBody* const bodyPtr, unsigned state) +{ + TRACE_FUNCTION(__FUNCTION__); + dgBody* const body = (dgBody *)bodyPtr; + body->SetContinueCollisionMode (state ? true : false); +} + +int NewtonBodyGetSerializedID(const NewtonBody* const bodyPtr) +{ + TRACE_FUNCTION(__FUNCTION__); + dgBody* const body = (dgBody *)bodyPtr; + return body->GetSerializedID(); +} + +/*! + Get the continuous collision state mode for this rigid body. + + @param *bodyPtr pointer to the body. + + @return Nothing. + + + Because there is there is penalty of about 3 to 5 depending of the shape complexity of the collision geometry, this feature is set + off by default. It is the job of the application to determine what bodies need this feature on. Good guidelines are: very small objects, + and bodies that move a height speed. + + this feature is currently disabled: + + See also: ::NewtonBodySetContinuousCollisionMode, ::NewtonBodySetContinuousCollisionMode +*/ +int NewtonBodyGetContinuousCollisionMode (const NewtonBody* const bodyPtr) +{ + TRACE_FUNCTION(__FUNCTION__); + dgBody* const body = (dgBody *)bodyPtr; + return body->GetContinueCollisionMode () ? 1 : false; +} + + + +/*! + Set the collision state flag of this body when the body is connected to another body by a hierarchy of joints. + + @param *bodyPtr pointer to the body. + @param state collision state. 1 indicates this body will collide with any linked body. 0 disable collision with body connected to this one by joints. + + @return Nothing. + + sometimes when making complicated arrangements of linked bodies it is possible the collision geometry of these bodies is in the way of the + joints work space. This could be a problem for the normal operation of the joints. When this situation happens the application can determine which bodies + are the problem and disable collision for those bodies while they are linked by joints. For the collision to be disable for a pair of body, + both bodies must have the collision disabled. If the joints connecting the bodies are destroyed these bodies become collidable automatically. + This feature can also be achieved by making special material for the whole configuration of jointed bodies, however it is a lot easier just to set collision disable + for jointed bodies. + + See also: ::NewtonBodySetMaterialGroupID +*/ +void NewtonBodySetJointRecursiveCollision(const NewtonBody* const bodyPtr, unsigned state) +{ + TRACE_FUNCTION(__FUNCTION__); + dgBody* const body = (dgBody *)bodyPtr; + body->SetCollisionWithLinkedBodies (state ? true : false); +} + +/*! + Get the collision state flag when the body is joint. + + @param *bodyPtr pointer to the body. + + @return return the collision state flag for this body. + + See also: ::NewtonBodySetMaterialGroupID +*/ +int NewtonBodyGetJointRecursiveCollision (const NewtonBody* const bodyPtr) +{ + TRACE_FUNCTION(__FUNCTION__); + dgBody* const body = (dgBody *)bodyPtr; + + return body->GetCollisionWithLinkedBodies () ? 1 : 0; +} + +/*! + get the freeze state of this body + + @param *bodyPtr is the pointer to the body to be frozen + + @return 1 id the bode is frozen, 0 if bode is unfrozen. + + When a body is created it is automatically placed in the active simulation list. As an optimization + for large scenes, you may use this function to put background bodies in an inactive equilibrium state. + + This function tells Newton that this body does not currently need to be simulated. + However, if the body is part of a larger configuration it may be affected indirectly by the reaction forces + of objects that it is connected to. + + See also: ::NewtonBodySetAutoSleep, ::NewtonBodyGetAutoSleep +*/ +int NewtonBodyGetFreezeState(const NewtonBody* const bodyPtr) +{ + TRACE_FUNCTION(__FUNCTION__); + dgBody* const body = (dgBody *)bodyPtr; + return body->GetFreeze() ? 1 : 0; +} + + +/*! + This function tells Newton to simulate or suspend simulation of this body and all other bodies in contact with it + + @param *bodyPtr is the pointer to the body to be activated + @param state 1 teels newton to freeze the bode and allconceted bodiesm, 0 to unfreze it + + @return Nothing + + This function to no activate the body, is just lock or unlock the body for physics simulation. + + See also: ::NewtonBodyGetFreezeState, ::NewtonBodySetAutoSleep, ::NewtonBodyGetAutoSleep +*/ +void NewtonBodySetFreezeState(const NewtonBody* const bodyPtr, int state) +{ + TRACE_FUNCTION(__FUNCTION__); + dgBody* const body = (dgBody *)bodyPtr; + body->SetFreeze(state ? true : false); +} + +int NewtonBodyGetGyroscopicTorque(const NewtonBody* const bodyPtr) +{ + TRACE_FUNCTION(__FUNCTION__); + dgBody* const body = (dgBody *)bodyPtr; + return body->GetGyroMode() ? 1 : 0; +} + +void NewtonBodySetGyroscopicTorque(const NewtonBody* const bodyPtr, int state) +{ + TRACE_FUNCTION(__FUNCTION__); + dgBody* const body = (dgBody *)bodyPtr; + body->SetGyroMode(state ? true : false); +} + +/*! + Set the auto-activation mode for this body. + + @param *bodyPtr is the pointer to the body. + @param state active mode: 1 = auto-activation on (controlled by Newton). 0 = auto-activation off and body is active all the time. + + @return Nothing. + + Bodies are created with auto-activation on by default. + + Auto activation enabled is the default state for the majority of bodies in a large scene. + However, for player control, ai control or some other special circumstance, the application may want to control + the activation/deactivation of the body. + In that case, the application may call NewtonBodySetAutoSleep (body, 0) followed by + NewtonBodySetFreezeState(body), this will make the body active forever. + + See also: ::NewtonBodyGetFreezeState, ::NewtonBodySetFreezeState, ::NewtonBodyGetAutoSleep +*/ +void NewtonBodySetAutoSleep(const NewtonBody* const bodyPtr, int state) +{ + TRACE_FUNCTION(__FUNCTION__); + dgBody* const body = (dgBody *)bodyPtr; + body->SetAutoSleep (state ? true : false); +} + +/*! + Get the auto-activation state of the body. + + @param *bodyPtr is the pointer to the body. + + @return Auto activation state: 1 = auto-activation on. 0 = auto-activation off. + + See also: ::NewtonBodySetAutoSleep, ::NewtonBodyGetSleepState +*/ +int NewtonBodyGetAutoSleep(const NewtonBody* const bodyPtr) +{ + TRACE_FUNCTION(__FUNCTION__); + dgBody* const body = (dgBody *)bodyPtr; + + return body->GetAutoSleep () ? 1 : 0; +} + + +/*! + Return the sleep mode of a rigid body. + + @param *bodyPtr is the pointer to the body. + + @return Sleep state: 0 = active. 1 = sleeping. + + See also: ::NewtonBodySetAutoSleep +*/ +int NewtonBodyGetSleepState(const NewtonBody* const bodyPtr) +{ + TRACE_FUNCTION(__FUNCTION__); + dgBody* const body = (dgBody *)bodyPtr; + return body->GetSleepState() ? 1 : 0; +} + +void NewtonBodySetSleepState(const NewtonBody* const bodyPtr, int state) +{ + TRACE_FUNCTION(__FUNCTION__); + dgBody* const body = (dgBody *)bodyPtr; + body->SetSleepState(state ? true : false); +} + + +/*! + Get the world axis aligned bounding box (AABB) of the body. + + @param *bodyPtr is the pointer to the body. + @param *p0 - pointer to an array of at least three floats to hold minimum value for the AABB. + @param *p1 - pointer to an array of at least three floats to hold maximum value for the AABB. + +*/ +void NewtonBodyGetAABB(const NewtonBody* const bodyPtr, dFloat* const p0, dFloat* const p1) +{ + TRACE_FUNCTION(__FUNCTION__); + + dgVector vector0; + dgVector vector1; + + TRACE_FUNCTION(__FUNCTION__); + dgBody* const body = (dgBody *)bodyPtr; + body->GetAABB (vector0, vector1); + + p0[0] = vector0.m_x; + p0[1] = vector0.m_y; + p0[2] = vector0.m_z; + + p1[0] = vector1.m_x; + p1[1] = vector1.m_y; + p1[2] = vector1.m_z; +} + +/*! + Set the global linear velocity of the body. + + @param *bodyPtr is the pointer to the body. + @param *velocity pointer to an array of at least three floats containing the velocity vector. + + See also: ::NewtonBodyGetVelocity +*/ +void NewtonBodySetVelocity(const NewtonBody* const bodyPtr, const dFloat* const velocity) +{ + TRACE_FUNCTION(__FUNCTION__); + dgBody* const body = (dgBody *)bodyPtr; + + dgVector vector (velocity[0], velocity[1], velocity[2], dgFloat32 (0.0f)); + body->SetVelocity (vector); +} + +void NewtonBodySetVelocityNoSleep(const NewtonBody* const bodyPtr, const dFloat* const velocity) +{ + TRACE_FUNCTION(__FUNCTION__); + dgBody* const body = (dgBody *)bodyPtr; + + dgVector vector(velocity[0], velocity[1], velocity[2], dgFloat32(0.0f)); + body->SetVelocityNoSleep(vector); +} + + +/*! + Get the global linear velocity of the body. + + @param *bodyPtr is the pointer to the body. + @param *velocity pointer to an array of at least three floats to hold the velocity vector. + + See also: ::NewtonBodySetVelocity +*/ +void NewtonBodyGetVelocity(const NewtonBody* const bodyPtr, dFloat* const velocity) +{ + TRACE_FUNCTION(__FUNCTION__); + + dgBody* const body = (dgBody *)bodyPtr; + + dgVector vector (body->GetVelocity()); + velocity[0] = vector.m_x; + velocity[1] = vector.m_y; + velocity[2] = vector.m_z; +} + +/*! + Set the global angular velocity of the body. + + @param *bodyPtr is the pointer to the body. + @param *omega pointer to an array of at least three floats containing the angular velocity vector. + + See also: ::NewtonBodyGetOmega +*/ +void NewtonBodySetOmega(const NewtonBody* const bodyPtr, const dFloat* const omega) +{ + TRACE_FUNCTION(__FUNCTION__); + dgBody* const body = (dgBody *)bodyPtr; + + dgVector vector (omega[0], omega[1], omega[2], dgFloat32 (0.0f)); + body->SetOmega (vector); +} + +void NewtonBodySetOmegaNoSleep(const NewtonBody* const bodyPtr, const dFloat* const omega) +{ + TRACE_FUNCTION(__FUNCTION__); + dgBody* const body = (dgBody *)bodyPtr; + + dgVector vector(omega[0], omega[1], omega[2], dgFloat32(0.0f)); + body->SetOmegaNoSleep(vector); +} + +/*! + Get the global angular velocity of the body. + + @param *bodyPtr is the pointer to the body + @param *omega pointer to an array of at least three floats to hold the angular velocity vector. + + See also: ::NewtonBodySetOmega +*/ +void NewtonBodyGetOmega(const NewtonBody* const bodyPtr, dFloat* const omega) +{ + TRACE_FUNCTION(__FUNCTION__); + + dgBody* const body = (dgBody *)bodyPtr; + + dgVector vector (body->GetOmega()); + omega[0] = vector.m_x; + omega[1] = vector.m_y; + omega[2] = vector.m_z; +} + + +/*! +Get the global angular accelration of the body. + +@param *bodyPtr is the pointer to the body +@param *omega pointer to an array of at least three floats to hold the angular acceleration vector. +*/ +void NewtonBodyGetAlpha(const NewtonBody* const bodyPtr, dFloat* const alpha) +{ + TRACE_FUNCTION(__FUNCTION__); + dgBody* const body = (dgBody *)bodyPtr; + + const dgVector vector(body->GetAlpha()); + alpha[0] = vector.m_x; + alpha[1] = vector.m_y; + alpha[2] = vector.m_z; +} + + +/*! +Get the global linear Acceleration of the body. + +@param *bodyPtr is the pointer to the body. +@param *acceleration pointer to an array of at least three floats to hold the acceleration vector. + +See also: ::NewtonBodySetVelocity +*/ +void NewtonBodyGetAcceleration(const NewtonBody* const bodyPtr, dFloat* const acceleration) +{ + TRACE_FUNCTION(__FUNCTION__); + dgBody* const body = (dgBody *)bodyPtr; + + dgVector vector(body->GetAccel()); + acceleration[0] = vector.m_x; + acceleration[1] = vector.m_y; + acceleration[2] = vector.m_z; +} + +/*! + Apply the linear viscous damping coefficient to the body. + + @param *bodyPtr is the pointer to the body. + @param linearDamp linear damping coefficient. + + the default value of *linearDamp* is clamped to a value between 0.0 and 1.0; the default value is 0.1, + There is a non zero implicit attenuation value of 0.0001 assume by the integrator. + + The dampening viscous friction force is added to the external force applied to the body every frame before going to the solver-integrator. + This force is proportional to the square of the magnitude of the velocity to the body in the opposite direction of the velocity of the body. + An application can set *linearDamp* to zero when the application takes control of the external forces and torque applied to the body, should the application + desire to have absolute control of the forces over that body. However, it is recommended that the *linearDamp* coefficient is set to a non-zero + value for the majority of background bodies. This saves the application from having to control these forces and also prevents the integrator from + adding very large velocities to a body. + + See also: ::NewtonBodyGetLinearDamping +*/ +void NewtonBodySetLinearDamping(const NewtonBody* const bodyPtr, dFloat linearDamp) +{ + TRACE_FUNCTION(__FUNCTION__); + dgBody* const body = (dgBody *)bodyPtr; + + body->SetLinearDamping (linearDamp); +} + +/*! + Get the linear viscous damping of the body. + + @param *bodyPtr is the pointer to the body. + + @return The linear damping coefficient. + + See also: ::NewtonBodySetLinearDamping +*/ +dFloat NewtonBodyGetLinearDamping(const NewtonBody* const bodyPtr) +{ + TRACE_FUNCTION(__FUNCTION__); + dgBody* const body = (dgBody *)bodyPtr; + + return body->GetLinearDamping(); +} + + +/*! + Apply the angular viscous damping coefficient to the body. + + @param *bodyPtr is the pointer to the body. + @param *angularDamp pointer to an array of at least three floats containing the angular damping coefficients for the principal axis of the body. + + the default value of *angularDamp* is clamped to a value between 0.0 and 1.0; the default value is 0.1, + There is a non zero implicit attenuation value of 0.0001 assumed by the integrator. + + The dampening viscous friction torque is added to the external torque applied to the body every frame before going to the solver-integrator. + This torque is proportional to the square of the magnitude of the angular velocity to the body in the opposite direction of the angular velocity of the body. + An application can set *angularDamp* to zero when the to take control of the external forces and torque applied to the body, should the application + desire to have absolute control of the forces over that body. However, it is recommended that the *linearDamp* coefficient be set to a non-zero + value for the majority of background bodies. This saves the application from needing to control these forces and also prevents the integrator from + adding very large velocities to a body. + + See also: ::NewtonBodyGetAngularDamping +*/ +void NewtonBodySetAngularDamping(const NewtonBody* const bodyPtr, const dFloat* angularDamp) +{ + TRACE_FUNCTION(__FUNCTION__); + dgBody* const body = (dgBody *)bodyPtr; + + dgVector vector (angularDamp[0], angularDamp[1], angularDamp[2], dgFloat32 (0.0f)); + body->SetAngularDamping (vector); +} + + +/*! + Get the linear viscous damping of the body. + + @param *bodyPtr is the pointer to the body. + @param *angularDamp pointer to an array of at least three floats to hold the angular damping coefficient for the principal axis of the body. + + See also: ::NewtonBodySetAngularDamping +*/ +void NewtonBodyGetAngularDamping(const NewtonBody* const bodyPtr, dFloat* angularDamp) +{ + TRACE_FUNCTION(__FUNCTION__); + dgBody* const body = (dgBody *)bodyPtr; + +// dgVector& vector = *((dgVector*) angularDamp); +// vector = body->GetAngularDamping(); + + dgVector vector (body->GetAngularDamping()); + angularDamp[0] = vector.m_x; + angularDamp[1] = vector.m_y; + angularDamp[2] = vector.m_z; +} + + + +void NewtonBodyGetPointVelocity (const NewtonBody* const bodyPtr, const dFloat* const point, dFloat* const velocOut) +{ + TRACE_FUNCTION(__FUNCTION__); + dgBody* const body = (dgBody *)bodyPtr; + dgVector veloc (body->GetVelocityAtPoint (dgVector (point[0], point[1], point[2], dgFloat32 (0.0f)))); + velocOut[0] = veloc[0]; + velocOut[1] = veloc[1]; + velocOut[2] = veloc[2]; +} + +/*! + Add an impulse to a specific point on a body. + + @param *bodyPtr is the pointer to the body. + @param pointDeltaVeloc pointer to an array of at least three floats containing the desired change in velocity to point pointPosit. + @param pointPosit - pointer to an array of at least three floats containing the center of the impulse in global space. + @param timestep - the update rate time step. + + @return Nothing. + + This function will activate the body. + + *pointPosit* and *pointDeltaVeloc* must be specified in global space. + + *pointDeltaVeloc* represent a change in velocity. For example, a value of *pointDeltaVeloc* of (1, 0, 0) changes the velocity + of *bodyPtr* in such a way that the velocity of point *pointDeltaVeloc* will increase by (1, 0, 0) + + *the calculate impulse will be applied to the body on next frame update + + Because *pointDeltaVeloc* represents a change in velocity, this function must be used with care. Repeated calls + to this function will result in an increase of the velocity of the body and may cause to integrator to lose stability. +*/ +void NewtonBodyAddImpulse(const NewtonBody* const bodyPtr, const dFloat* const pointDeltaVeloc, const dFloat* const pointPosit, dFloat timestep) +{ + TRACE_FUNCTION(__FUNCTION__); + dgBody* const body = (dgBody *)bodyPtr; + + if (body->GetInvMass().m_w > dgFloat32 (0.0f)) { + dgVector p (pointPosit[0], pointPosit[1], pointPosit[2], dgFloat32 (0.0f)); + dgVector v (pointDeltaVeloc[0], pointDeltaVeloc[1], pointDeltaVeloc[2], dgFloat32 (0.0f)); + body->AddImpulse (v, p, timestep); + } +} + + +/*! + Add an train of impulses to a specific point on a body. + + @param *bodyPtr is the pointer to the body. + @param impulseCount - number of impulses and distances in the array distance + @param strideInByte - sized in bytes of vector impulse and + @param impulseArray pointer to an array containing the desired impulse to apply ate position point array. + @param pointArray pointer to an array of at least three floats containing the center of the impulse in global space. + @param timestep - the update rate time step. + + @return Nothing. + + This function will activate the body. + + *pointPosit* and *pointDeltaVeloc* must be specified in global space. + + *the calculate impulse will be applied to the body on next frame update + + this function apply at general impulse to a body a oppose to a desired change on velocity + this mean that the body mass, and Inertia will determine the gain on velocity. +*/ +void NewtonBodyApplyImpulseArray (const NewtonBody* const bodyPtr, int impulseCount, int strideInByte, const dFloat* const impulseArray, const dFloat* const pointArray, dFloat timestep) +{ + TRACE_FUNCTION(__FUNCTION__); + dgBody* const body = (dgBody *)bodyPtr; + + if (body->GetInvMass().m_w > dgFloat32 (0.0f)) { + body->ApplyImpulsesAtPoint (impulseCount, strideInByte, impulseArray, pointArray, timestep); + } +} + +void NewtonBodyApplyImpulsePair (const NewtonBody* const bodyPtr, dFloat* const linearImpulse, dFloat* const angularImpulse, dFloat timestep) +{ + TRACE_FUNCTION(__FUNCTION__); + dgBody* const body = (dgBody *)bodyPtr; + + if (body->GetInvMass().m_w > dgFloat32 (0.0f)) { + dgVector l (linearImpulse[0], linearImpulse[1], linearImpulse[2], dgFloat32 (0.0f)); + dgVector a (angularImpulse[0], angularImpulse[1], angularImpulse[2], dgFloat32 (0.0f)); + body->ApplyImpulsePair (l, a, timestep); + } +} + +void NewtonBodyIntegrateVelocity (const NewtonBody* const bodyPtr, dFloat timestep) +{ + TRACE_FUNCTION(__FUNCTION__); + dgBody* const body = (dgBody *)bodyPtr; + + if (body->IsRTTIType(dgBody::m_kinematicBody) || (body->GetInvMass().m_w > dgFloat32 (0.0f))) { + body->IntegrateVelocity(timestep); + } +} + +/*! @} */ // end of RigidBodyInterface + +/*! @defgroup ConstraintBall ConstraintBall +Ball and Socket joint interface +@{ +*/ + + +/*! + Create a ball an socket joint. + + @param *newtonWorld Pointer to the Newton world. + @param *pivotPoint is origin of ball and socket in global space. + @param *childBody is the pointer to the attached rigid body, this body can not be NULL or it can not have an infinity (zero) mass. + @param *parentBody is the pointer to the parent rigid body, this body can be NULL or any kind of rigid body. + + @return Pointer to the ball and socket joint. + + This function creates a ball and socket and add it to the world. By default joint disables collision with the linked bodies. +*/ +NewtonJoint* NewtonConstraintCreateBall(const NewtonWorld* const newtonWorld, + const dFloat* pivotPoint, + const NewtonBody* const childBody, + const NewtonBody* const parentBody) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *)newtonWorld; + dgBody* const body0 = (dgBody *)childBody; + dgBody* const body1 = (dgBody *)parentBody; + dgVector pivot (pivotPoint[0], pivotPoint[1], pivotPoint[2], dgFloat32 (0.0f)); + return (NewtonJoint*) world->CreateBallConstraint (pivot, body0, body1); +} + +/*! + Set the ball and socket cone and twist limits. + + @param *ball is the pointer to a ball and socket joint. + @param *pin pointer to a unit vector defining the cone axis in global space. + @param maxConeAngle max angle in radians the attached body is allow to swing relative to the pin axis, a value of zero will disable this limits. + @param maxTwistAngle max angle in radians the attached body is allow to twist relative to the pin axis, a value of zero will disable this limits. + + limits are disabled at creation time. A value of zero for *maxConeAngle* disable the cone limit, a value of zero for *maxTwistAngle* disable the twist limit + all non-zero value for *maxConeAngle* are clamped between 5 degree and 175 degrees + + See also: ::NewtonConstraintCreateBall +*/ +void NewtonBallSetConeLimits(const NewtonJoint* const ball, const dFloat* pin, dFloat maxConeAngle, dFloat maxTwistAngle) +{ + TRACE_FUNCTION(__FUNCTION__); + dgBallConstraint* const joint = (dgBallConstraint*) ball; + + dgVector coneAxis (pin[0], pin[1], pin[2], dgFloat32 (0.0f)); + + if (coneAxis.DotProduct(coneAxis).GetScalar() < 1.0e-3f) { + coneAxis.m_x = dgFloat32(1.0f); + } + dgVector tmp (dgFloat32 (1.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f)); + if (dgAbs (tmp.DotProduct(coneAxis).GetScalar()) > dgFloat32 (0.999f)) { + tmp = dgVector (dgFloat32 (0.0f), dgFloat32(1.0f), dgFloat32 (0.0f), dgFloat32 (0.0f)); + if (dgAbs (tmp.DotProduct(coneAxis).GetScalar()) > dgFloat32 (0.999f)) { + tmp = dgVector (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32(1.0f), dgFloat32 (0.0f)); + dgAssert (dgAbs (tmp.DotProduct(coneAxis).GetScalar()) < dgFloat32 (0.999f)); + } + } + dgVector lateral (tmp.CrossProduct(coneAxis)); + dgAssert(lateral.m_w = dgFloat32(0.0f)); + dgAssert(coneAxis.m_w = dgFloat32(0.0f)); + lateral = lateral.Normalize(); + coneAxis = coneAxis.Normalize(); + + maxConeAngle = dgAbs (maxConeAngle); + maxTwistAngle = dgAbs (maxTwistAngle); + joint->SetConeLimitState ((maxConeAngle > dgDegreeToRad) ? true : false); + joint->SetTwistLimitState ((maxTwistAngle > dgDegreeToRad) ? true : false); + joint->SetLatealLimitState (false); + joint->SetLimits (coneAxis, -maxConeAngle, maxConeAngle, maxTwistAngle, lateral, 0.0f, 0.0f); +} + + +/*! + Set an update call back to be called when either of the two bodies linked by the joint is active. + + @param *ball pointer to the joint. + @param callback pointer to the joint function call back. + + @return nothing. + + if the application wants to have some feedback from the joint simulation, the application can register a function + update callback to be called every time any of the bodies linked by this joint is active. This is useful to provide special + effects like particles, sound or even to simulate breakable moving parts. + + See also: ::NewtonJointSetUserData +*/ +void NewtonBallSetUserCallback(const NewtonJoint* const ball, NewtonBallCallback callback) +{ + dgBallConstraint* contraint; + + TRACE_FUNCTION(__FUNCTION__); + contraint = (dgBallConstraint*) ball; + contraint->SetJointParameterCallback ((dgBallJointFriction)callback); +} + + +/*! + Get the relative joint angle between the two bodies. + + @param *ball pointer to the joint. + @param *angle pointer to an array of a least three floats to hold the joint relative Euler angles. + + @return nothing. + + this function can be used during a function update call back to provide the application with some special effect. + for example the application can play a bell sound when the joint angle passes some max value. + + See also: ::NewtonBallSetUserCallback +*/ +void NewtonBallGetJointAngle (const NewtonJoint* const ball, dFloat* angle) +{ + dgBallConstraint* contraint; + + contraint = (dgBallConstraint*) ball; + dgVector angleVector (contraint->GetJointAngle ()); + + TRACE_FUNCTION(__FUNCTION__); + angle[0] = angleVector.m_x; + angle[1] = angleVector.m_y; + angle[2] = angleVector.m_z; +} + +/*! + Get the relative joint angular velocity between the two bodies. + + @param *ball pointer to the joint. + @param *omega pointer to an array of a least three floats to hold the joint relative angular velocity. + + @return nothing. + + this function can be used during a function update call back to provide the application with some special effect. + for example the application can play the creaky noise of a hanging lamp. + + See also: ::NewtonBallSetUserCallback +*/ +void NewtonBallGetJointOmega(const NewtonJoint* const ball, dFloat* omega) +{ + dgBallConstraint* contraint; + + TRACE_FUNCTION(__FUNCTION__); + contraint = (dgBallConstraint*) ball; + dgVector omegaVector (contraint->GetJointOmega ()); + omega[0] = omegaVector.m_x; + omega[1] = omegaVector.m_y; + omega[2] = omegaVector.m_z; +} + +/*! + Get the total force asserted over the joint pivot point, to maintain the constraint. + + @param *ball pointer to the joint. + @param *force pointer to an array of a least three floats to hold the force value of the joint. + + @return nothing. + + this function can be used during a function update call back to provide the application with some special effect. + for example the application can destroy the joint if the force exceeds some predefined value. + + See also: ::NewtonBallSetUserCallback +*/ +void NewtonBallGetJointForce(const NewtonJoint* const ball, dFloat* const force) +{ + // fixme: type? "constraint" instead of "contraint"? + dgBallConstraint* contraint; + + TRACE_FUNCTION(__FUNCTION__); + contraint = (dgBallConstraint*) ball; + dgVector forceVector (contraint->GetJointForce ()); + force[0] = forceVector.m_x; + force[1] = forceVector.m_y; + force[2] = forceVector.m_z; +} + + +/*! @} */ // end of ConstraintBall + +/*! @defgroup JointSlider JointSlider +Slider joint interface +@{ +*/ + +/*! + Create a slider joint. + + @param *newtonWorld Pointer to the Newton world. + @param *pivotPoint is origin of the slider in global space. + @param *pinDir is the line of action of the slider in global space. + @param *childBody is the pointer to the attached rigid body, this body can not be NULL or it can not have an infinity (zero) mass. + @param *parentBody is the pointer to the parent rigid body, this body can be NULL or any kind of rigid body. + + @return Pointer to the slider joint. + + This function creates a slider and add it to the world. By default joint disables collision with the linked bodies. +*/ +NewtonJoint* NewtonConstraintCreateSlider(const NewtonWorld* const newtonWorld, const dFloat* pivotPoint, const dFloat* pinDir, const NewtonBody* const childBody, const NewtonBody* const parentBody) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *)newtonWorld; + dgBody* const body0 = (dgBody *)childBody; + dgBody* const body1 = (dgBody *)parentBody; + dgVector pin (pinDir[0], pinDir[1], pinDir[2], dgFloat32 (0.0f)); + dgVector pivot (pivotPoint[0], pivotPoint[1], pivotPoint[2], dgFloat32 (0.0f)); + return (NewtonJoint*) world->CreateSlidingConstraint (pivot, pin, body0, body1); +} + + +/*! + Set an update call back to be called when either of the two body linked by the joint is active. + + @param *slider pointer to the joint. + @param callback pointer to the joint function call back. + + @return nothing. + + if the application wants to have some feedback from the joint simulation, the application can register a function + update callback to be call every time any of the bodies linked by this joint is active. This is useful to provide special + effects like particles, sound or even to simulate breakable moving parts. + + See also: ::NewtonJointGetUserData, ::NewtonJointSetUserData +*/ +void NewtonSliderSetUserCallback(const NewtonJoint* const slider, NewtonSliderCallback callback) +{ + dgSlidingConstraint* contraint; + + TRACE_FUNCTION(__FUNCTION__); + contraint = (dgSlidingConstraint*) slider; + contraint->SetJointParameterCallback ((dgSlidingJointAcceleration)callback); +} + +/*! + Get the relative joint angle between the two bodies. + + @param *Slider pointer to the joint. + + @return the joint angle relative to the hinge pin. + + this function can be used during a function update call back to provide the application with some special effect. + for example the application can play a bell sound when the joint angle passes some max value. + + See also: ::NewtonSliderSetUserCallback +*/ +dFloat NewtonSliderGetJointPosit (const NewtonJoint* Slider) +{ + dgSlidingConstraint* contraint; + + TRACE_FUNCTION(__FUNCTION__); + contraint = (dgSlidingConstraint*) Slider; + return contraint->GetJointPosit (); +} + +/*! + Get the relative joint angular velocity between the two bodies. + + @param *Slider pointer to the joint. + + @return the joint angular velocity relative to the pin axis. + + this function can be used during a function update call back to provide the application with some special effect. + for example the application can play the creaky noise of a hanging lamp. + + See also: ::NewtonSliderSetUserCallback +*/ +dFloat NewtonSliderGetJointVeloc(const NewtonJoint* Slider) +{ + dgSlidingConstraint* contraint; + + TRACE_FUNCTION(__FUNCTION__); + contraint = (dgSlidingConstraint*) Slider; + return contraint->GetJointVeloc (); +} + + +/*! + Calculate the angular acceleration needed to stop the slider at the desired angle. + + @param *slider pointer to the joint. + @param *desc is the pointer to the Slider or slide structure. + @param distance desired stop distance relative to the pivot point + + fixme: inconsistent variable capitalisation; some functions use "slider", others "Slider". + + @return the relative linear acceleration needed to stop the slider. + + this function can only be called from a *NewtonSliderCallback* and it can be used by the application to implement slider limits. + + See also: ::NewtonSliderSetUserCallback +*/ +dFloat NewtonSliderCalculateStopAccel(const NewtonJoint* const slider, const NewtonHingeSliderUpdateDesc* const desc, dFloat distance) +{ + dgSlidingConstraint* contraint; + + TRACE_FUNCTION(__FUNCTION__); + contraint = (dgSlidingConstraint*) slider; + return contraint->CalculateStopAccel (distance, (dgJointCallbackParam*) desc); +} + +/*! + Get the total force asserted over the joint pivot point, to maintain the constraint. + + @param *slider pointer to the joint. + @param *force pointer to an array of a least three floats to hold the force value of the joint. + + @return nothing. + + this function can be used during a function update call back to provide the application with some special effect. + for example the application can destroy the joint if the force exceeds some predefined value. + + See also: ::NewtonSliderSetUserCallback +*/ +void NewtonSliderGetJointForce(const NewtonJoint* const slider, dFloat* const force) +{ + dgSlidingConstraint* contraint; + + TRACE_FUNCTION(__FUNCTION__); + contraint = (dgSlidingConstraint*) slider; + dgVector forceVector (contraint->GetJointForce ()); + force[0] = forceVector.m_x; + force[1] = forceVector.m_y; + force[2] = forceVector.m_z; +} + + +/*! @} */ // end of JointSlider + +/*! @defgroup JointCorkscrew JointCorkscrew +Corkscrew joint interface +@{ +*/ + +/*! + Create a corkscrew joint. + + @param *newtonWorld Pointer to the Newton world. + @param *pivotPoint is origin of the corkscrew in global space. + @param *pinDir is the line of action of the corkscrew in global space. + @param *childBody is the pointer to the attached rigid body, this body can not be NULL or it can not have an infinity (zero) mass. + @param *parentBody is the pointer to the parent rigid body, this body can be NULL or any kind of rigid body. + + @return Pointer to the corkscrew joint. + + This function creates a corkscrew and add it to the world. By default joint disables collision with the linked bodies. +*/ +NewtonJoint* NewtonConstraintCreateCorkscrew(const NewtonWorld* const newtonWorld, const dFloat* pivotPoint, const dFloat* pinDir, const NewtonBody* const childBody, const NewtonBody* const parentBody) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *)newtonWorld; + dgBody* const body0 = (dgBody *)childBody; + dgBody* const body1 = (dgBody *)parentBody; + dgVector pin (pinDir[0], pinDir[1], pinDir[2], dgFloat32 (0.0f)); + dgVector pivot (pivotPoint[0], pivotPoint[1], pivotPoint[2], dgFloat32 (0.0f)); + return (NewtonJoint*) world->CreateCorkscrewConstraint (pivot, pin, body0, body1); +} + +/*! + Set an update call back to be called when either of the two body linked by the joint is active. + + @param *corkscrew pointer to the joint. + @param callback pointer to the joint function call back. + + @return nothing. + + if the application wants to have some feedback from the joint simulation, the application can register a function + update callback to be call every time any of the bodies linked by this joint is active. This is useful to provide special + effects like particles, sound or even to simulate breakable moving parts. + + the function *NewtonCorkscrewCallback callback* should return a bit field code. + if the application does not want to set the joint acceleration the return code is zero + if the application only wants to change the joint linear acceleration the return code is 1 + if the application only wants to change the joint angular acceleration the return code is 2 + if the application only wants to change the joint angular and linear acceleration the return code is 3 + + See also: ::NewtonJointGetUserData, ::NewtonJointSetUserData +*/ +void NewtonCorkscrewSetUserCallback(const NewtonJoint* const corkscrew, NewtonCorkscrewCallback callback) +{ + dgCorkscrewConstraint* contraint; + + TRACE_FUNCTION(__FUNCTION__); + contraint = (dgCorkscrewConstraint*) corkscrew; + contraint->SetJointParameterCallback ((dgCorkscrewJointAcceleration)callback); +} + +/*! + Get the relative joint angle between the two bodies. + + @param *corkscrew pointer to the joint. + + @return the joint angle relative to the hinge pin. + + this function can be used during a function update call back to provide the application with some special effect. + for example the application can play a bell sound when the joint angle passes some max value. + + See also: ::NewtonCorkscrewSetUserCallback +*/ +dFloat NewtonCorkscrewGetJointPosit (const NewtonJoint* const corkscrew) +{ + dgCorkscrewConstraint* contraint; + + TRACE_FUNCTION(__FUNCTION__); + contraint = (dgCorkscrewConstraint*) corkscrew; + return contraint->GetJointPosit (); +} + +/*! + Get the relative joint angular velocity between the two bodies. + + @param *corkscrew pointer to the joint. + + @return the joint angular velocity relative to the pin axis. + + this function can be used during a function update call back to provide the application with some special effect. + for example the application can play the creaky noise of a hanging lamp. + + See also: ::NewtonCorkscrewSetUserCallback +*/ +dFloat NewtonCorkscrewGetJointVeloc(const NewtonJoint* const corkscrew) +{ + dgCorkscrewConstraint* contraint; + + TRACE_FUNCTION(__FUNCTION__); + contraint = (dgCorkscrewConstraint*) corkscrew; + return contraint->GetJointVeloc (); +} + +/*! + Get the relative joint angle between the two bodies. + + @param *corkscrew pointer to the joint. + + @return the joint angle relative to the corkscrew pin. + + this function can be used during a function update call back to provide the application with some special effect. + for example the application can play a bell sound when the joint angle passes some max value. + + See also: ::NewtonCorkscrewSetUserCallback +*/ +dFloat NewtonCorkscrewGetJointAngle (const NewtonJoint* const corkscrew) +{ + dgCorkscrewConstraint* contraint; + + TRACE_FUNCTION(__FUNCTION__); + contraint = (dgCorkscrewConstraint*) corkscrew; + return contraint->GetJointAngle (); + +} + +/*! + Get the relative joint angular velocity between the two bodies. + + @param *corkscrew pointer to the joint. + + @return the joint angular velocity relative to the pin axis. + + this function can be used during a function update call back to provide the application with some special effect. + for example the application can play the creaky noise of a hanging lamp. + + See also: ::NewtonCorkscrewSetUserCallback +*/ +dFloat NewtonCorkscrewGetJointOmega(const NewtonJoint* const corkscrew) +{ + dgCorkscrewConstraint* contraint; + + TRACE_FUNCTION(__FUNCTION__); + contraint = (dgCorkscrewConstraint*) corkscrew; + return contraint->GetJointOmega (); +} + + +/*! + Calculate the angular acceleration needed to stop the corkscrew at the desired angle. + + @param *corkscrew pointer to the joint. + @param *desc is the pointer to the Corkscrew or slide structure. + @param angle is the desired corkscrew stop angle + + @return the relative angular acceleration needed to stop the corkscrew. + + this function can only be called from a *NewtonCorkscrewCallback* and it can be used by the application to implement corkscrew limits. + + See also: ::NewtonCorkscrewSetUserCallback +*/ +dFloat NewtonCorkscrewCalculateStopAlpha (const NewtonJoint* const corkscrew, const NewtonHingeSliderUpdateDesc* const desc, dFloat angle) +{ + dgCorkscrewConstraint* contraint; + + TRACE_FUNCTION(__FUNCTION__); + contraint = (dgCorkscrewConstraint*) corkscrew; + return contraint->CalculateStopAlpha (angle, (dgJointCallbackParam*) desc); +} + + +/*! + Calculate the angular acceleration needed to stop the corkscrew at the desired angle. + + @param *corkscrew pointer to the joint. + @param *desc is the pointer to the Corkscrew or slide structure. + @param distance desired stop distance relative to the pivot point + + @return the relative linear acceleration needed to stop the corkscrew. + + this function can only be called from a *NewtonCorkscrewCallback* and it can be used by the application to implement corkscrew limits. + + See also: ::NewtonCorkscrewSetUserCallback +*/ +dFloat NewtonCorkscrewCalculateStopAccel(const NewtonJoint* const corkscrew, const NewtonHingeSliderUpdateDesc* const desc, dFloat distance) +{ + dgCorkscrewConstraint* contraint; + contraint = (dgCorkscrewConstraint*) corkscrew; + return contraint->CalculateStopAccel (distance, (dgJointCallbackParam*) desc); +} + +/*! + Get the total force asserted over the joint pivot point, to maintain the constraint. + + @param *corkscrew pointer to the joint. + @param *force pointer to an array of a least three floats to hold the force value of the joint. + + @return nothing. + + this function can be used during a function update call back to provide the application with some special effect. + for example the application can destroy the joint if the force exceeds some predefined value. + + See also: ::NewtonCorkscrewSetUserCallback +*/ +void NewtonCorkscrewGetJointForce(const NewtonJoint* const corkscrew, dFloat* const force) +{ + dgCorkscrewConstraint* contraint; + + TRACE_FUNCTION(__FUNCTION__); + contraint = (dgCorkscrewConstraint*) corkscrew; + dgVector forceVector (contraint->GetJointForce ()); + force[0] = forceVector.m_x; + force[1] = forceVector.m_y; + force[2] = forceVector.m_z; +} + + +/*! @} */ // end of JointSlider + +/*! @defgroup JointUniversal JointUniversal +Universal joint interface +@{ +*/ + +/*! + Create a universal joint. + + @param *newtonWorld Pointer to the Newton world. + @param *pivotPoint is origin of the universal joint in global space. + @param *pinDir0 - first axis of rotation fixed on childBody body and perpendicular to pinDir1. + @param *pinDir1 - second axis of rotation fixed on parentBody body and perpendicular to pinDir0. + @param *childBody is the pointer to the attached rigid body, this body can not be NULL or it can not have an infinity (zero) mass. + @param *parentBody is the pointer to the parent rigid body, this body can be NULL or any kind of rigid body. + + @return Pointer to the universal joint. + + This function creates a universal joint and add it to the world. By default joint disables collision with the linked bodies. + + a universal joint is a constraint that restricts twp rigid bodies to be connected to a point fixed on both bodies, + while and allowing one body to spin around a fix axis in is own frame, and the other body to spin around another axis fixes on + it own frame. Both axis must be mutually perpendicular. +*/ +NewtonJoint* NewtonConstraintCreateUniversal(const NewtonWorld* const newtonWorld, const dFloat* pivotPoint, + const dFloat* pinDir0, const dFloat* pinDir1, const NewtonBody* const childBody, const NewtonBody* const parentBody) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *)newtonWorld; + dgBody* const body0 = (dgBody *)childBody; + dgBody* const body1 = (dgBody *)parentBody; + dgVector pin0 (pinDir0[0], pinDir0[1], pinDir0[2], dgFloat32 (0.0f)); + dgVector pin1 (pinDir1[0], pinDir1[1], pinDir1[2], dgFloat32 (0.0f)); + dgVector pivot (pivotPoint[0], pivotPoint[1], pivotPoint[2], dgFloat32 (0.0f)); + return (NewtonJoint*) world->CreateUniversalConstraint (pivot, pin0, pin1, body0, body1); +} + + +/*! + Set an update call back to be called when either of the two body linked by the joint is active. + + @param *universal pointer to the joint. + @param callback pointer to the joint function call back. + + @return nothing. + + if the application wants to have some feedback from the joint simulation, the application can register a function + update callback to be called every time any of the bodies linked by this joint is active. This is useful to provide special + effects like particles, sound or even to simulate breakable moving parts. + + the function *NewtonUniversalCallback callback* should return a bit field code. + if the application does not want to set the joint acceleration the return code is zero + if the application only wants to change the joint linear acceleration the return code is 1 + if the application only wants to change the joint angular acceleration the return code is 2 + if the application only wants to change the joint angular and linear acceleration the return code is 3 + + See also: ::NewtonJointGetUserData, ::NewtonJointSetUserData +*/ +void NewtonUniversalSetUserCallback(const NewtonJoint* const universal, NewtonUniversalCallback callback) +{ + dgUniversalConstraint* contraint; + + TRACE_FUNCTION(__FUNCTION__); + contraint = (dgUniversalConstraint*) universal; + contraint->SetJointParameterCallback ((dgUniversalJointAcceleration)callback); +} + + +/*! + Get the relative joint angle between the two bodies. + + @param *universal pointer to the joint. + + @return the joint angle relative to the universal pin0. + + this function can be used during a function update call back to provide the application with some special effect. + for example the application can play a bell sound when the joint angle passes some max value. + + See also: ::NewtonUniversalSetUserCallback +*/ +dFloat NewtonUniversalGetJointAngle0(const NewtonJoint* const universal) +{ + dgUniversalConstraint* contraint; + + TRACE_FUNCTION(__FUNCTION__); + contraint = (dgUniversalConstraint*) universal; + return contraint->GetJointAngle0 (); +} + +/*! + Get the relative joint angle between the two bodies. + + @param *universal pointer to the joint. + + @return the joint angle relative to the universal pin1. + + this function can be used during a function update call back to provide the application with some special effect. + for example the application can play a bell sound when the joint angle passes some max value. + + See also: ::NewtonUniversalSetUserCallback +*/ +dFloat NewtonUniversalGetJointAngle1(const NewtonJoint* const universal) +{ + dgUniversalConstraint* contraint; + + TRACE_FUNCTION(__FUNCTION__); + contraint = (dgUniversalConstraint*) universal; + return contraint->GetJointAngle1 (); +} + + +/*! + Get the relative joint angular velocity between the two bodies. + + @param *universal pointer to the joint. + + @return the joint angular velocity relative to the pin0 axis. + + this function can be used during a function update call back to provide the application with some special effect. + for example the application can play the creaky noise of a hanging lamp. + + See also: ::NewtonUniversalSetUserCallback +*/ +dFloat NewtonUniversalGetJointOmega0(const NewtonJoint* const universal) +{ + dgUniversalConstraint* contraint; + + TRACE_FUNCTION(__FUNCTION__); + contraint = (dgUniversalConstraint*) universal; + return contraint->GetJointOmega0 (); +} + + +/*! + Get the relative joint angular velocity between the two bodies. + + @param *universal pointer to the joint. + + @return the joint angular velocity relative to the pin1 axis. + + this function can be used during a function update call back to provide the application with some special effect. + for example the application can play the creaky noise of a hanging lamp. + + See also: ::NewtonUniversalSetUserCallback +*/ +dFloat NewtonUniversalGetJointOmega1(const NewtonJoint* const universal) +{ + dgUniversalConstraint* contraint; + + TRACE_FUNCTION(__FUNCTION__); + contraint = (dgUniversalConstraint*) universal; + return contraint->GetJointOmega1 (); +} + + + +/*! + Calculate the angular acceleration needed to stop the universal at the desired angle. + + @param *universal pointer to the joint. + @param *desc is the pointer to the Universal or slide structure. + @param angle is the desired universal stop angle rotation around pin0 + + @return the relative angular acceleration needed to stop the universal. + + this function can only be called from a *NewtonUniversalCallback* and it can be used by the application to implement universal limits. + + See also: ::NewtonUniversalSetUserCallback +*/ +dFloat NewtonUniversalCalculateStopAlpha0(const NewtonJoint* const universal, const NewtonHingeSliderUpdateDesc* const desc, dFloat angle) +{ + dgUniversalConstraint* contraint; + + TRACE_FUNCTION(__FUNCTION__); + contraint = (dgUniversalConstraint*) universal; + return contraint->CalculateStopAlpha0 (angle, (dgJointCallbackParam*) desc); +} + +/*! + Calculate the angular acceleration needed to stop the universal at the desired angle. + + @param *universal pointer to the joint. + @param *desc is the pointer to and the Universal or slide structure. + @param angle is the desired universal stop angle rotation around pin1 + + @return the relative angular acceleration needed to stop the universal. + + this function can only be called from a *NewtonUniversalCallback* and it can be used by the application to implement universal limits. + + See also: ::NewtonUniversalSetUserCallback +*/ +dFloat NewtonUniversalCalculateStopAlpha1(const NewtonJoint* const universal, const NewtonHingeSliderUpdateDesc* const desc, dFloat angle) +{ + dgUniversalConstraint* contraint; + + TRACE_FUNCTION(__FUNCTION__); + contraint = (dgUniversalConstraint*) universal; + return contraint->CalculateStopAlpha1 (angle, (dgJointCallbackParam*) desc); +} + + + +/*! + Get the total force asserted over the joint pivot point, to maintain the constraint. + + @param *universal pointer to the joint. + @param *force pointer to an array of a least three floats to hold the force value of the joint. + + @return nothing. + + this function can be used during a function update call back to provide the application with some special effect. + for example the application can destroy the joint if the force exceeds some predefined value. + + See also: ::NewtonUniversalSetUserCallback +*/ +void NewtonUniversalGetJointForce(const NewtonJoint* const universal, dFloat* const force) +{ + dgUniversalConstraint* contraint; + + TRACE_FUNCTION(__FUNCTION__); + contraint = (dgUniversalConstraint*) universal; + dgVector forceVector (contraint->GetJointForce ()); + force[0] = forceVector.m_x; + force[1] = forceVector.m_y; + force[2] = forceVector.m_z; +} + + +/*! @} */ // end of JointUniversal + +/*! @defgroup JointUpVector JointUpVector +UpVector joint Interface +@{ +*/ + +/*! + Create a UpVector joint. + + @param *newtonWorld Pointer to the Newton world. + @param *pinDir is the aligning vector. + @param *body is the pointer to the attached rigid body, this body can not be NULL or it can not have an infinity (zero) mass. + + @return Pointer to the up vector joint. + + This function creates an up vector joint. An up vector joint is a constraint that allows a body to translate freely in 3d space, + but it only allows the body to rotate around the pin direction vector. This could be use by the application to control a character + with physics and collision. + + Since the UpVector joint is a unary constraint, there is not need to have user callback or user data assigned to it. + The application can simple hold to the joint handle and update the pin on the force callback function of the rigid body owning the joint. +*/ +NewtonJoint* NewtonConstraintCreateUpVector (const NewtonWorld* const newtonWorld, const dFloat* pinDir, const NewtonBody* const body) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *)newtonWorld; + dgBody* const body0 = (dgBody *)body; + dgVector pin (pinDir[0], pinDir[1], pinDir[2], dgFloat32 (0.0f)); + return (NewtonJoint*) world->CreateUpVectorConstraint(pin, body0); +} + + +/*! + Get the up vector pin of this joint in global space. + + @param *upVector pointer to the joint. + @param *pin pointer to an array of a least three floats to hold the up vector direction in global space. + + @return nothing. + + the application ca call this function to read the up vector, this is useful to animate the up vector. + if the application is going to animated the up vector, it must do so by applying only small rotation, + too large rotation can cause vibration of the joint. + + See also: ::NewtonUpVectorSetPin +*/ +void NewtonUpVectorGetPin(const NewtonJoint* const upVector, dFloat *pin) +{ + dgUpVectorConstraint* contraint; + + TRACE_FUNCTION(__FUNCTION__); + contraint = (dgUpVectorConstraint*) upVector; + + dgVector pinVector (contraint ->GetPinDir ()); + pin[0] = pinVector.m_x; + pin[1] = pinVector.m_y; + pin[2] = pinVector.m_z; +} + + +/*! + Set the up vector pin of this joint in global space. + + @param *upVector pointer to the joint. + @param *pin pointer to an array of a least three floats containing the up vector direction in global space. + + @return nothing. + + the application ca call this function to change the joint up vector, this is useful to animate the up vector. + if the application is going to animated the up vector, it must do so by applying only small rotation, + too large rotation can cause vibration of the joint. + + See also: ::NewtonUpVectorGetPin +*/ +void NewtonUpVectorSetPin(const NewtonJoint* const upVector, const dFloat *pin) +{ + dgUpVectorConstraint* contraint; + + TRACE_FUNCTION(__FUNCTION__); + contraint = (dgUpVectorConstraint*) upVector; + + dgVector pinVector (pin[0], pin[1], pin[2], dgFloat32 (0.0f)); + contraint->SetPinDir (pinVector); +} + +/*! @} */ // end of JointUpVector + +/*! @defgroup JointUser JointUser +User defined joint interface +@{ +*/ + +/*! + Create a user define bilateral joint. + + @param *newtonWorld Pointer to the Newton world. + @param maxDOF is the maximum number of degree of freedom controlled by this joint. + @param submitConstraints pointer to the joint constraint definition function call back. + @param getInfo pointer to callback for collecting joint information. + @param *childBody is the pointer to the attached rigid body, this body can not be NULL or it can not have an infinity (zero) mass. + @param *parentBody is the pointer to the parent rigid body, this body can be NULL or any kind of rigid body. + + Bilateral joint are constraints that can have up to 6 degree of freedoms, 3 linear and 3 angular. + By restricting the motion along any number of these degree of freedom a very large number of useful joint between + two rigid bodies can be accomplished. Some of the degree of freedoms restriction makes no sense, and also some + combinations are so rare that only make sense to a very specific application, the Newton engine implements the more + commons combinations like, hinges, ball and socket, etc. However if and application is in the situation that any of + the provided joints can achieve the desired effect, then the application can design it own joint. + + User defined joint is a very advance feature that should be look at, only for very especial situations. + The designer must be a person with a very good understanding of constrained dynamics, and it may be the case + that many trial have to be made before a good result can be accomplished. + + function *submitConstraints* is called before the solver state to get the jacobian derivatives and the righ hand acceleration + for the definition of the constraint. + + maxDOF is and upper bound as to how many degrees of freedoms the joint can control, usually this value + can be 6 for bilateral joints, but it can be higher for special joints like vehicles where by the used of friction clamping + the number of rows can be higher. + In general the application should determine maxDof correctly, passing an unnecessary excessive value will lead to performance decreased. + + See also: ::NewtonUserJointSetFeedbackCollectorCallback +*/ +NewtonJoint* NewtonConstraintCreateUserJoint(const NewtonWorld* const newtonWorld, int maxDOF, + NewtonUserBilateralCallback submitConstraints, + const NewtonBody* const childBody, const NewtonBody* const parentBody) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *)newtonWorld; + dgBody* const body0 = (dgBody *)childBody; + dgBody* const body1 = (dgBody *)parentBody; + dgAssert(body0); + dgAssert(body0 != body1); + return (NewtonJoint*) new (world->dgWorld::GetAllocator()) NewtonUserJoint (world, maxDOF, submitConstraints, body0, body1); +} + +/*! + Set the solver algorithm use to calculation the constraint forces. + + @param *joint pointer to the joint. + @param *model - solve model to choose. + + model = 0 zero is the default value and tells the solver to use the best possible algorithm + model = 1 to signal the engine that is two joints form a kinematic loop + model = 2 to signal the engine this joint can be solved with a less accurate algorithm. + In case multiple joints form a kinematic loop, joints with a lower model are preffered towards an exact solution. + + See also: NewtonUserJointGetSolverModel +*/ +void NewtonUserJointSetSolverModel(const NewtonJoint* const joint, int model) +{ + TRACE_FUNCTION(__FUNCTION__); + dgConstraint* const contraint = (dgConstraint*)joint; + contraint->SetSolverModel(model); +} + +/*! +Get the solver algorithm use to calculation the constraint forces. +@param *joint pointer to the joint. + +See also: NewtonUserJointGetSolverModel +*/ +int NewtonUserJointGetSolverModel(const NewtonJoint* const joint) +{ + TRACE_FUNCTION(__FUNCTION__); + dgConstraint* const contraint = (dgConstraint*)joint; + return contraint->GetSolverModel(); +} + +void NewtonUserJointMassScale(const NewtonJoint* const joint, dFloat scaleBody0, dFloat scaleBody1) +{ + NewtonUserJoint* const contraint = (NewtonUserJoint*)joint; + dgAssert (contraint->IsBilateral()); + contraint->SetMassScale(scaleBody0, scaleBody1); +} + +/*! + Add a linear restricted degree of freedom. + + @param *joint pointer to the joint. + @param *pivot0 - pointer of a vector in global space fixed on body zero. + @param *pivot1 - pointer of a vector in global space fixed on body one. + @param *dir pointer of a unit vector in global space along which the relative position, velocity and acceleration between the bodies will be driven to zero. + + A linear constraint row calculates the Jacobian derivatives and relative acceleration required to enforce the constraint condition at + the attachment point and the pin direction considered fixed to both bodies. + + The acceleration is calculated such that the relative linear motion between the two points is zero, the application can + afterward override this value to create motors. + + after this function is call and internal DOF index will point to the current row entry in the constraint matrix. + + This function call only be called from inside a *NewtonUserBilateralCallback* callback. + + See also: ::NewtonUserJointAddAngularRow, +*/ +void NewtonUserJointAddLinearRow(const NewtonJoint* const joint, const dFloat* const pivot0, const dFloat* const pivot1, const dFloat* const dir) +{ + NewtonUserJoint* const userJoint = (NewtonUserJoint*) joint; + + TRACE_FUNCTION(__FUNCTION__); + dgVector direction (dir[0], dir[1], dir[2], dgFloat32 (0.0f)); + direction = direction.Normalize(); + dgAssert (dgAbs (direction.DotProduct(direction).GetScalar() - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f)); + dgVector pivotPoint0 (pivot0[0], pivot0[1], pivot0[2], dgFloat32 (0.0f)); + dgVector pivotPoint1 (pivot1[0], pivot1[1], pivot1[2], dgFloat32 (0.0f)); + + userJoint->AddLinearRowJacobian (pivotPoint0, pivotPoint1, direction); +} + + +/*! + Add an angular restricted degree of freedom. + + @param *joint pointer to the joint. + @param relativeAngleError relative angle error between both bodies around pin axis. + @param *pin pointer of a unit vector in global space along which the relative position, velocity and acceleration between the bodies will be driven to zero. + + An angular constraint row calculates the Jacobian derivatives and relative acceleration required to enforce the constraint condition at + pin direction considered fixed to both bodies. + + The acceleration is calculated such that the relative angular motion between the two points is zero, The application can + afterward override this value to create motors. + + After this function is called and internal DOF index will point to the current row entry in the constraint matrix. + + This function call only be called from inside a *NewtonUserBilateralCallback* callback. + + This function is of not practical to enforce hard constraints, but it is very useful for making angular motors. + + See also: ::NewtonUserJointAddLinearRow +*/ +void NewtonUserJointAddAngularRow(const NewtonJoint* const joint, dFloat relativeAngleError, const dFloat* const pin) +{ + TRACE_FUNCTION(__FUNCTION__); + NewtonUserJoint* const userJoint = (NewtonUserJoint*) joint; + dgVector direction (pin[0], pin[1], pin[2], dgFloat32 (0.0f)); + direction = direction.Normalize(); + dgAssert (dgAbs (direction.DotProduct(direction).GetScalar() - dgFloat32 (1.0f)) < dgFloat32 (1.0e-3f)); + + userJoint->AddAngularRowJacobian (direction, relativeAngleError); +} + +/*! + set the general linear and angular Jacobian for the desired degree of freedom + + @param *joint pointer to the joint. + @param *jacobian0 - pointer of a set of six values defining the linear and angular Jacobian for body0. + @param *jacobian1 - pointer of a set of six values defining the linear and angular Jacobian for body1. + + In general this function must be used for very special effects and in combination with other joints. + it is expected that the user have a knowledge of Constrained dynamics to make a good used of this function. + Must typical application of this function are the creation of synchronization or control joints like gears, pulleys, + worm gear and some other mechanical control. + + this function set the relative acceleration for this degree of freedom to zero. It is the + application responsibility to set the relative acceleration after a call to this function + + See also: ::NewtonUserJointAddLinearRow, ::NewtonUserJointAddAngularRow +*/ +void NewtonUserJointAddGeneralRow(const NewtonJoint* const joint, const dFloat* const jacobian0, const dFloat* const jacobian1) +{ + TRACE_FUNCTION(__FUNCTION__); + NewtonUserJoint* const userJoint = (NewtonUserJoint*) joint; + userJoint->AddGeneralRowJacobian (jacobian0, jacobian1); +} + +int NewtonUserJoinRowsCount(const NewtonJoint* const joint) +{ + TRACE_FUNCTION(__FUNCTION__); + NewtonUserJoint* const userJoint = (NewtonUserJoint*)joint; + return userJoint->GetJacobianCount(); +} + +void NewtonUserJointGetGeneralRow(const NewtonJoint* const joint, int index, dFloat* const jacobian0, dFloat* const jacobian1) +{ + TRACE_FUNCTION(__FUNCTION__); + NewtonUserJoint* const userJoint = (NewtonUserJoint*)joint; + userJoint->GetJacobianAt(index, jacobian0, jacobian1); +} + +/*! + Set the maximum friction value the solver is allow to apply to the joint row. + + @param *joint pointer to the joint. + @param friction maximum friction value for this row. It must be a positive value between 0.0 and INFINITY. + + This function will override the default friction values set after a call to NewtonUserJointAddLinearRow or NewtonUserJointAddAngularRow. + friction value is context sensitive, if for linear constraint friction is a Max friction force, for angular constraint friction is a + max friction is a Max friction torque. + + See also: ::NewtonUserJointSetRowMinimumFriction, ::NewtonUserJointAddLinearRow, ::NewtonUserJointAddAngularRow +*/ +void NewtonUserJointSetRowMaximumFriction(const NewtonJoint* const joint, dFloat friction) +{ + TRACE_FUNCTION(__FUNCTION__); + NewtonUserJoint* const userJoint = (NewtonUserJoint*) joint; + userJoint->SetHighFriction (friction); +} + +/*! + Set the minimum friction value the solver is allow to apply to the joint row. + + @param *joint pointer to the joint. + @param friction friction value for this row. It must be a negative value between 0.0 and -INFINITY. + + This function will override the default friction values set after a call to NewtonUserJointAddLinearRow or NewtonUserJointAddAngularRow. + friction value is context sensitive, if for linear constraint friction is a Min friction force, for angular constraint friction is a + friction is a Min friction torque. + + See also: ::NewtonUserJointSetRowMaximumFriction, ::NewtonUserJointAddLinearRow, ::NewtonUserJointAddAngularRow +*/ +void NewtonUserJointSetRowMinimumFriction(const NewtonJoint* const joint, dFloat friction) +{ + TRACE_FUNCTION(__FUNCTION__); + NewtonUserJoint* const userJoint = (NewtonUserJoint*) joint; + userJoint->SetLowerFriction (friction); +} + +/*! + Set the value for the desired acceleration for the current constraint row. + + @param *joint pointer to the joint. + @param acceleration desired acceleration value for this row. + + This function will override the default acceleration values set after a call to NewtonUserJointAddLinearRow or NewtonUserJointAddAngularRow. + friction value is context sensitive, if for linear constraint acceleration is a linear acceleration, for angular constraint acceleration is an + angular acceleration. + + See also: ::NewtonUserJointAddLinearRow, ::NewtonUserJointAddAngularRow +*/ +void NewtonUserJointSetRowAcceleration(const NewtonJoint* const joint, dFloat acceleration) +{ + TRACE_FUNCTION(__FUNCTION__); + NewtonUserJoint* const userJoint = (NewtonUserJoint*) joint; + userJoint->SetAcceleration (acceleration); +} + +dFloat NewtonUserJointGetRowAcceleration (const NewtonJoint* const joint) +{ + TRACE_FUNCTION(__FUNCTION__); + NewtonUserJoint* const userJoint = (NewtonUserJoint*) joint; + return userJoint->GetAcceleration(); +} + +void NewtonUserJointGetRowJacobian(const NewtonJoint* const joint, dFloat* const linear0, dFloat* const angular0, dFloat* const linear1, dFloat* const angular1) +{ + TRACE_FUNCTION(__FUNCTION__); + NewtonUserJoint* const userJoint = (NewtonUserJoint*)joint; + dgJacobian jacobian0; + dgJacobian jacobian1; + userJoint->GetJacobian(jacobian0, jacobian1); + for (dgInt32 i = 0; i < 3; i++) { + linear0[i] = jacobian0.m_linear[i]; + angular0[i] = jacobian0.m_angular[i]; + linear1[i] = jacobian1.m_linear[i]; + angular1[i] = jacobian1.m_angular[i]; + } +} + +dFloat NewtonUserJointCalculateRowZeroAcceleration (const NewtonJoint* const joint) +{ + TRACE_FUNCTION(__FUNCTION__); + NewtonUserJoint* const userJoint = (NewtonUserJoint*)joint; + return userJoint->CalculateZeroMotorAcceleration(); +} + +/*! + Calculates the row acceleration to satisfy the specified the spring damper system. + + @param *joint pointer to the joint. + @param rowStiffness fraction of the row reaction forces used a sspring damper penalty. + @param spring desired spring stiffness, it must be a positive value. + @param damper desired damper coefficient, it must be a positive value. + + This function will override the default acceleration values set after a call to NewtonUserJointAddLinearRow or NewtonUserJointAddAngularRow. + friction value is context sensitive, if for linear constraint acceleration is a linear acceleration, for angular constraint acceleration is an + angular acceleration. + + the acceleration calculated by this function represent the mass, spring system of the form + a = -ks * x - kd * v. + + for this function to take place the joint stiffness must be set to a values lower than 1.0 + + See also: ::NewtonUserJointSetRowAcceleration, ::NewtonUserJointSetRowStiffness +*/ +void NewtonUserJointSetRowSpringDamperAcceleration(const NewtonJoint* const joint, dFloat rowStiffness, dFloat spring, dFloat damper) +{ + TRACE_FUNCTION(__FUNCTION__); + NewtonUserJoint* const userJoint = (NewtonUserJoint*) joint; + userJoint->SetSpringDamperAcceleration (rowStiffness, spring, damper); +} + +/*! + Set the maximum percentage of the constraint force that will be applied to the constraint row. + + @param *joint pointer to the joint. + @param stiffness row stiffness, it must be a values between 0.0 and 1.0, the default is 0.9. + + This function will override the default stiffness value set after a call to NewtonUserJointAddLinearRow or NewtonUserJointAddAngularRow. + the row stiffness is the percentage of the constraint force that will be applied to the rigid bodies. Ideally the value should be + 1.0 (100% stiff) but dues to numerical integration error this could be the joint a little unstable, and lower values are preferred. + + See also: ::NewtonUserJointAddLinearRow, ::NewtonUserJointAddAngularRow, ::NewtonUserJointSetRowSpringDamperAcceleration +*/ +void NewtonUserJointSetRowStiffness(const NewtonJoint* const joint, dFloat stiffness) +{ + TRACE_FUNCTION(__FUNCTION__); + NewtonUserJoint* const userJoint = (NewtonUserJoint*) joint; + userJoint->SetRowStiffness (dgFloat32 (1.0f) - stiffness); +} + +/*! + Return the magnitude previews force or torque value calculated by the solver for this constraint row. + + @param *joint pointer to the joint. + @param row index to the constraint row. + + This function can be call for any of the previews row for this particular joint, The application must keep track of the meaning of the row. + + This function can be used to produce special effects like breakable or malleable joints, fro example a hinge can turn into ball and socket + after the force in some of the row exceed certain high value. +*/ +dFloat NewtonUserJointGetRowForce(const NewtonJoint* const joint, int row) +{ + TRACE_FUNCTION(__FUNCTION__); + NewtonUserJoint* const userJoint = (NewtonUserJoint*) joint; + return userJoint->GetRowForce (row); +} + + +/*! + Set a constrain callback to collect the force calculated by the solver to enforce this constraint + + @param *joint pointer to the joint. + @param getFeedback pointer to the joint constraint definition function call back. + + See also: ::NewtonUserJointGetRowForce +*/ +void NewtonUserJointSetFeedbackCollectorCallback(const NewtonJoint* const joint, NewtonUserBilateralCallback getFeedback) +{ + TRACE_FUNCTION(__FUNCTION__); + NewtonUserJoint* const userJoint = (NewtonUserJoint*) joint; + return userJoint->SetUpdateFeedbackFunction (getFeedback); +} + + +/*! @} */ // end of JointUser + +/*! @defgroup JointCommon JointCommon +Joint common function s +@{ +*/ + +/*! + Store a user defined data value with the joint. + + @param *joint pointer to the joint. + @param *userData pointer to the user defined user data value. + + @return Nothing. + + The application can store a user defined value with the Joint. This value can be the pointer to a structure containing some application data for special effect. + if the application allocate some resource to store the user data, the application can register a joint destructor to get rid of the allocated resource when the Joint is destroyed + + See also: ::NewtonJointSetDestructor +*/ +void NewtonJointSetUserData(const NewtonJoint* const joint, void* const userData) +{ + TRACE_FUNCTION(__FUNCTION__); + dgConstraint* const contraint = (dgConstraint*) joint; + contraint->SetUserData (userData); +} + +/*! + Retrieve a user defined data value stored with the joint. + + @param *joint pointer to the joint. + + @return The user defined data. + + The application can store a user defined value with a joint. This value can be the pointer + to a structure to store some game play data for special effect. + + See also: ::NewtonJointSetUserData +*/ +void* NewtonJointGetUserData(const NewtonJoint* const joint) +{ + TRACE_FUNCTION(__FUNCTION__); + dgConstraint* const contraint = (dgConstraint*) joint; + return contraint->GetUserData(); +} + +/*! + Get creation parameters for this joint. + + @param joint is the pointer to a convex collision primitive. + @param *jointInfo pointer to a collision information record. + + This function can be used by the application for writing file format and for serialization. + + See also: ::// See also: +*/ +void NewtonJointGetInfo(const NewtonJoint* const joint, NewtonJointRecord* const jointInfo) +{ + TRACE_FUNCTION(__FUNCTION__); + dgConstraint* const contraint = (dgConstraint*) joint; + contraint->GetInfo ((dgConstraintInfo*) jointInfo); +} + +/*! + Get the first body connected by this joint. + + @param *joint is the pointer to a convex collision primitive. + + + See also: ::// See also: +*/ +NewtonBody* NewtonJointGetBody0(const NewtonJoint* const joint) +{ + TRACE_FUNCTION(__FUNCTION__); + dgConstraint* const contraint = (dgConstraint*) joint; + dgBody* const body = contraint->GetBody0(); + dgWorld* const world = body->GetWorld(); + return (world->GetSentinelBody() != body) ? (NewtonBody*) body : NULL; +} + + +/*! + Get the second body connected by this joint. + + @param *joint is the pointer to a convex collision primitive. + + See also: ::// See also: +*/ +NewtonBody* NewtonJointGetBody1(const NewtonJoint* const joint) +{ + TRACE_FUNCTION(__FUNCTION__); + dgConstraint* const contraint = (dgConstraint*) joint; + dgBody* const body = contraint->GetBody1(); + dgWorld* const world = body->GetWorld(); + return (world->GetSentinelBody() != body) ? (NewtonBody*) body : NULL; +} + + +/*! + Enable or disable collision between the two bodies linked by this joint. The default state is collision disable when the joint is created. + + @param *joint pointer to the joint. + @param state collision state, zero mean disable collision, non zero enable collision between linked bodies. + + @return nothing. + + usually when two bodies are linked by a joint, the application wants collision between this two bodies to be disabled. + This is the default behavior of joints when they are created, however when this behavior is not desired the application can change + it by setting collision on. If the application decides to enable collision between jointed bodies, the application should make sure the + collision geometry do not collide in the work space of the joint. + + if the joint is destroyed the collision state of the two bodies linked by this joint is determined by the material pair assigned to each body. + + See also: ::NewtonJointGetCollisionState, ::NewtonBodySetJointRecursiveCollision +*/ +void NewtonJointSetCollisionState(const NewtonJoint* const joint, int state) +{ + TRACE_FUNCTION(__FUNCTION__); + dgConstraint* const contraint = (dgConstraint*) joint; + return contraint->SetCollidable (state ? true : false); +} + +/*! + Get the collision state of the two bodies linked by the joint. + + @param *joint pointer to the joint. + + @return the collision state. + + usually when two bodies are linked by a joint, the application wants collision between this two bodies to be disabled. + This is the default behavior of joints when they are created, however when this behavior is not desired the application can change + it by setting collision on. If the application decides to enable collision between jointed bodies, the application should make sure the + collision geometry do not collide in the work space of the joint. + + See also: ::NewtonJointSetCollisionState +*/ +int NewtonJointGetCollisionState(const NewtonJoint* const joint) +{ + TRACE_FUNCTION(__FUNCTION__); + dgConstraint* const contraint = (dgConstraint*) joint; + return contraint->IsCollidable () ? 1 : 0; +} + + +/*! + Set the strength coefficient to be applied to the joint reaction forces. + + @param *joint pointer to the joint. + @param stiffness stiffness coefficient, a value between 0, and 1.0, the default value for most joint is 0.9 + + @return nothing. + + Constraint keep bodies together by calculating the exact force necessary to cancel the relative acceleration between one or + more common points fixed in the two bodies. The problem is that when the bodies drift apart due to numerical integration inaccuracies, + the reaction force work to pull eliminated the error but at the expense of adding extra energy to the system, does violating the rule + that constraint forces must be work less. This is a inevitable situation and the only think we can do is to minimize the effect of the + extra energy by dampening the force by some amount. In essence the stiffness coefficient tell Newton calculate the precise reaction force + by only apply a fraction of it to the joint point. And value of 1.0 will apply the exact force, and a value of zero will apply only + 10 percent. + + The stiffness is set to a all around value that work well for most situation, however the application can play with these + parameter to make finals adjustment. A high value will make the joint stronger but more prompt to vibration of instability; a low + value will make the joint more stable but weaker. + + See also: ::NewtonJointGetStiffness +*/ +void NewtonJointSetStiffness(const NewtonJoint* const joint, dFloat stiffness) +{ + TRACE_FUNCTION(__FUNCTION__); + dgConstraint* const contraint = (dgConstraint*) joint; + contraint->SetStiffness(dgFloat32 (1.0f) - stiffness); +} + +/*! + Get the strength coefficient bing applied to the joint reaction forces. + + @param *joint pointer to the joint. + + @return stiffness coefficient. + + Constraint keep bodies together by calculating the exact force necessary to cancel the relative acceleration between one or + more common points fixed in the two bodies. The problem is that when the bodies drift apart due to numerical integration inaccuracies, + the reaction force work to pull eliminated the error but at the expense of adding extra energy to the system, does violating the rule + that constraint forces must be work less. This is a inevitable situation and the only think we can do is to minimize the effect of the + extra energy by dampening the force by some amount. In essence the stiffness coefficient tell Newton calculate the precise reaction force + by only apply a fraction of it to the joint point. And value of 1.0 will apply the exact force, and a value of zero will apply only + 10 percent. + + The stiffness is set to a all around value that work well for most situation, however the application can play with these + parameter to make finals adjustment. A high value will make the joint stronger but more prompt to vibration of instability; a low + value will make the joint more stable but weaker. + + See also: ::NewtonJointSetStiffness +*/ +dFloat NewtonJointGetStiffness(const NewtonJoint* const joint) +{ + TRACE_FUNCTION(__FUNCTION__); + dgConstraint* const contraint = (dgConstraint*) joint; + return dgFloat32 (1.0f) - contraint->GetStiffness(); +} + +/*! + Register a destructor callback to be called when the joint is about to be destroyed. + + @param *joint pointer to the joint. + @param destructor pointer to the joint destructor callback. + + @return nothing. + + If application stores any resource with the joint, or the application wants to be notified when the + joint is about to be destroyed. The application can register a destructor call back with the joint. + + See also: ::NewtonJointSetUserData +*/ +void NewtonJointSetDestructor(const NewtonJoint* const joint, NewtonConstraintDestructor destructor) +{ + TRACE_FUNCTION(__FUNCTION__); + dgConstraint* const contraint = (dgConstraint*) joint; + contraint->SetDestructorCallback ((OnConstraintDestroy) destructor); +} + + +/*! + destroy a joint. + + @param *newtonWorld is the pointer to the body. + @param *joint pointer to joint to be destroyed + + @return nothing + + The application can call this function when it wants to destroy a joint. This function can be used by the application to simulate + breakable joints + + See also: ::NewtonConstraintCreateSlider +*/ +void NewtonDestroyJoint(const NewtonWorld* const newtonWorld, const NewtonJoint* const joint) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *) newtonWorld; + world->DestroyJoint ((dgConstraint*) joint); +} + +/*! @} */ // end of JointCommon + +/*! @defgroup SpecialEffectMesh SpecialEffectMesh +Special effect mesh interface +@{ +*/ + +NewtonMesh* NewtonMeshCreate(const NewtonWorld* const newtonWorld) +{ + TRACE_FUNCTION(__FUNCTION__); + + Newton* const world = (Newton *) newtonWorld; + dgMeshEffect* const mesh = new (world->dgWorld::GetAllocator()) dgMeshEffect (world->dgWorld::GetAllocator()); + return (NewtonMesh*) mesh; +} + +NewtonMesh* NewtonMeshCreateFromMesh(const NewtonMesh* const mesh) +{ + TRACE_FUNCTION(__FUNCTION__); + dgMeshEffect* const srcMesh = (dgMeshEffect*) mesh; + + dgMeshEffect* const clone = new (srcMesh->GetAllocator()) dgMeshEffect (*srcMesh); + return (NewtonMesh*) clone; +} + +NewtonMesh* NewtonMeshCreateFromCollision(const NewtonCollision* const collision) +{ + TRACE_FUNCTION(__FUNCTION__); + + dgCollisionInstance* const shape = (dgCollisionInstance*) collision; + dgMeshEffect* const mesh = new (shape->GetAllocator()) dgMeshEffect (shape); + return (NewtonMesh*) mesh; +} + +NewtonMesh* NewtonMeshCreateConvexHull (const NewtonWorld* const newtonWorld, int count, const dFloat* const vertexCloud, int strideInBytes, dFloat tolerance) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *) newtonWorld; + dgStack pool (count); + + dgInt32 stride = strideInBytes / sizeof (dgFloat32); + for (dgInt32 i = 0; i < count; i ++) { + pool[i].m_x = vertexCloud[i * stride + 0]; + pool[i].m_y = vertexCloud[i * stride + 1]; + pool[i].m_z = vertexCloud[i * stride + 2]; + pool[i].m_w = dgFloat64 (0.0); + } + dgMeshEffect* const mesh = new (world->dgWorld::GetAllocator()) dgMeshEffect (world->dgWorld::GetAllocator(), &pool[0].m_x, count, sizeof (dgBigVector), tolerance); + return (NewtonMesh*) mesh; +} + +NewtonMesh* NewtonMeshCreateTetrahedraIsoSurface(const NewtonMesh* const closeManifoldMesh) +{ + TRACE_FUNCTION(__FUNCTION__); + dgMeshEffect* const meshEffect = (dgMeshEffect*) closeManifoldMesh; + return (NewtonMesh*)meshEffect->CreateTetrahedraIsoSurface(); +} + +void NewtonCreateTetrahedraLinearBlendSkinWeightsChannel(const NewtonMesh* const tetrahedraMesh, NewtonMesh* const skinMesh) +{ + dgAssert(0); + TRACE_FUNCTION(__FUNCTION__); + dgMeshEffect* const meshEffect = (dgMeshEffect*)skinMesh; + meshEffect->CreateTetrahedraLinearBlendSkinWeightsChannel((const dgMeshEffect*)tetrahedraMesh); +} + +NewtonMesh* NewtonMeshCreateVoronoiConvexDecomposition (const NewtonWorld* const newtonWorld, int pointCount, const dFloat* const vertexCloud, int strideInBytes, int materialID, const dFloat* const textureMatrix) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *) newtonWorld; + return (NewtonMesh*) dgMeshEffect::CreateVoronoiConvexDecomposition (world->dgWorld::GetAllocator(), pointCount, strideInBytes, vertexCloud, materialID, dgMatrix (textureMatrix)); +} + +NewtonMesh* NewtonMeshCreateFromSerialization (const NewtonWorld* const newtonWorld, NewtonDeserializeCallback deserializeFunction, void* const serializeHandle) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *) newtonWorld; + return (NewtonMesh*) dgMeshEffect::CreateFromSerialization (world->dgWorld::GetAllocator(), (dgDeserialize) deserializeFunction, serializeHandle); +} + +void NewtonMeshDestroy(const NewtonMesh* const mesh) +{ + TRACE_FUNCTION(__FUNCTION__); + dgMeshEffect* const meshEffect = (dgMeshEffect*) mesh; + delete meshEffect; +} + +void NewtonMeshSerialize (const NewtonMesh* const mesh, NewtonSerializeCallback serializeFunction, void* const serializeHandle) +{ + TRACE_FUNCTION(__FUNCTION__); + dgMeshEffect* const meshEffect = (dgMeshEffect*) mesh; + meshEffect->Serialize ((dgSerialize) serializeFunction, serializeHandle); +} + +void NewtonMeshSaveOFF(const NewtonMesh* const mesh, const char* const filename) +{ + TRACE_FUNCTION(__FUNCTION__); + dgAssert(0); + //dgMeshEffect* const meshEffect = (dgMeshEffect*) mesh; + //meshEffect->SaveOFF(filename); +} + +NewtonMesh* NewtonMeshLoadOFF(const NewtonWorld* const newtonWorld, const char* const filename) +{ + TRACE_FUNCTION(__FUNCTION__); + //Newton* const world = (Newton *) newtonWorld; + //dgMemoryAllocator* const allocator = world->dgWorld::GetAllocator(); + //dgMeshEffect* const mesh = new (allocator) dgMeshEffect (allocator); + //mesh->LoadOffMesh(filename); + //return (NewtonMesh*) mesh; + dgAssert(0); + return NULL; +} + +NewtonMesh* NewtonMeshLoadTetrahedraMesh(const NewtonWorld* const newtonWorld, const char* const filename) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *)newtonWorld; + dgMemoryAllocator* const allocator = world->dgWorld::GetAllocator(); + dgMeshEffect* const mesh = new (allocator) dgMeshEffect(allocator); + mesh->LoadTetraMesh (filename); + return (NewtonMesh*)mesh; +} + +void NewtonMeshApplyTransform (const NewtonMesh* const mesh, const dFloat* const matrix) +{ + TRACE_FUNCTION(__FUNCTION__); + dgMeshEffect* const meshEffect = (dgMeshEffect*) mesh; + + meshEffect->ApplyTransform(dgMatrix (matrix)); +} + +void NewtonMeshFlipWinding(const NewtonMesh* const mesh) +{ + TRACE_FUNCTION(__FUNCTION__); + dgMeshEffect* const meshEffect = (dgMeshEffect*)mesh; + meshEffect->FlipWinding(); +} + +void NewtonMeshCalculateOOBB(const NewtonMesh* const mesh, dFloat* const matrix, dFloat* const x, dFloat* const y, dFloat* const z) +{ + TRACE_FUNCTION(__FUNCTION__); + dgMeshEffect* const meshEffect = (dgMeshEffect*) mesh; + + dgBigVector size; + dgMatrix alignMatrix (meshEffect->CalculateOOBB (size)); + +// *((dgMatrix *)matrix) = alignMatrix; + memcpy (matrix, &alignMatrix[0][0], sizeof (dgMatrix)); + *x = dgFloat32 (size.m_x); + *y = dgFloat32 (size.m_y); + *z = dgFloat32 (size.m_z); +} + +void NewtonMeshCalculateVertexNormals(const NewtonMesh* const mesh, dFloat angleInRadians) +{ + TRACE_FUNCTION(__FUNCTION__); + dgMeshEffect* const meshEffect = (dgMeshEffect*) mesh; + meshEffect->CalculateNormals (angleInRadians); +} + +void NewtonMeshApplyAngleBasedMapping(const NewtonMesh* const mesh, int material, NewtonReportProgress reportPrograssCallback, void* const reportPrgressUserData, dFloat* const aligmentMatrix) +{ + TRACE_FUNCTION(__FUNCTION__); + dgMatrix matrix(aligmentMatrix); + dgMeshEffect* const meshEffect = (dgMeshEffect*) mesh; + meshEffect->AngleBaseFlatteningMapping(material, (dgReportProgress) reportPrograssCallback, reportPrgressUserData); +} + +void NewtonMeshApplySphericalMapping(const NewtonMesh* const mesh, int material, const dFloat* const aligmentMatrix) +{ + TRACE_FUNCTION(__FUNCTION__); + dgMatrix matrix(aligmentMatrix); + dgMeshEffect* const meshEffect = (dgMeshEffect*) mesh; + meshEffect->SphericalMapping (material, matrix); +} + +void NewtonMeshApplyBoxMapping(const NewtonMesh* const mesh, int front, int side, int top, const dFloat* const aligmentMatrix) +{ + TRACE_FUNCTION(__FUNCTION__); + dgMatrix matrix(aligmentMatrix); + dgMeshEffect* const meshEffect = (dgMeshEffect*) mesh; + meshEffect->BoxMapping (front, side, top, matrix); +} + +void NewtonMeshApplyCylindricalMapping(const NewtonMesh* const mesh, int cylinderMaterial, int capMaterial, const dFloat* const aligmentMatrix) +{ + TRACE_FUNCTION(__FUNCTION__); + dgMatrix matrix(aligmentMatrix); + dgMeshEffect* const meshEffect = (dgMeshEffect*) mesh; + meshEffect->CylindricalMapping (cylinderMaterial, capMaterial, matrix); +} + +void NewtonMeshTriangulate (const NewtonMesh* const mesh) +{ + TRACE_FUNCTION(__FUNCTION__); + ((dgMeshEffect*) mesh)->Triangulate (); +} + +void NewtonMeshPolygonize (const NewtonMesh* const mesh) +{ + TRACE_FUNCTION(__FUNCTION__); + ((dgMeshEffect*) mesh)->ConvertToPolygons (); +} + + +int NewtonMeshIsOpenMesh (const NewtonMesh* const mesh) +{ + TRACE_FUNCTION(__FUNCTION__); + + return ((dgMeshEffect*) mesh)->HasOpenEdges () ? 1 : 0; +} + +void NewtonMeshFixTJoints (const NewtonMesh* const mesh) +{ + TRACE_FUNCTION(__FUNCTION__); + + return ((dgMeshEffect*) mesh)->RepairTJoints(); +} + + +void NewtonMeshClip (const NewtonMesh* const mesh, const NewtonMesh* const clipper, const dFloat* const clipperMatrix, NewtonMesh** const topMesh, NewtonMesh** const bottomMesh) +{ + TRACE_FUNCTION(__FUNCTION__); + + *topMesh = NULL; + *bottomMesh = NULL; + ((dgMeshEffect*) mesh)->ClipMesh (dgMatrix (clipperMatrix), (dgMeshEffect*)clipper, (dgMeshEffect**) topMesh, (dgMeshEffect**) bottomMesh); +} + + +NewtonMesh* NewtonMeshSimplify (const NewtonMesh* const mesh, int maxVertexCount, NewtonReportProgress progressReportCallback, void* const reportPrgressUserData) +{ + TRACE_FUNCTION(__FUNCTION__); + return (NewtonMesh*) ((dgMeshEffect*) mesh)->CreateSimplification (maxVertexCount, (dgReportProgress) progressReportCallback, reportPrgressUserData); +} + +NewtonMesh* NewtonMeshApproximateConvexDecomposition (const NewtonMesh* const mesh, dFloat maxConcavity, dFloat backFaceDistanceFactor, int maxCount, int maxVertexPerHull, NewtonReportProgress progressReportCallback, void* const reportProgressUserData) +{ + TRACE_FUNCTION(__FUNCTION__); + return (NewtonMesh*) ((dgMeshEffect*) mesh)->CreateConvexApproximation (maxConcavity, backFaceDistanceFactor, maxCount, maxVertexPerHull, (dgReportProgress) progressReportCallback, reportProgressUserData); +} + + + +NewtonMesh* NewtonMeshUnion (const NewtonMesh* const mesh, const NewtonMesh* const clipper, const dFloat* const clipperMatrix) +{ + TRACE_FUNCTION(__FUNCTION__); + return (NewtonMesh*) ((dgMeshEffect*) mesh)->Union (dgMatrix (clipperMatrix), (dgMeshEffect*)clipper); +} + + +NewtonMesh* NewtonMeshDifference (const NewtonMesh* const mesh, const NewtonMesh* const clipper, const dFloat* const clipperMatrix) +{ + TRACE_FUNCTION(__FUNCTION__); + return (NewtonMesh*) ((dgMeshEffect*) mesh)->Difference (dgMatrix (clipperMatrix), (dgMeshEffect*)clipper); +} + +NewtonMesh* NewtonMeshIntersection (const NewtonMesh* const mesh, const NewtonMesh* const clipper, const dFloat* const clipperMatrix) +{ + TRACE_FUNCTION(__FUNCTION__); + return (NewtonMesh*) ((dgMeshEffect*) mesh)->Intersection (dgMatrix (clipperMatrix), (dgMeshEffect*)clipper); +} + +NewtonMesh* NewtonMeshConvexMeshIntersection (const NewtonMesh* const mesh, const NewtonMesh* const convexMesh) +{ + TRACE_FUNCTION(__FUNCTION__); + return (NewtonMesh*) ((dgMeshEffect*) mesh)->ConvexMeshIntersection ((dgMeshEffect*)convexMesh); +} + +void NewtonRemoveUnusedVertices(const NewtonMesh* const mesh, int* const vertexRemapTable) +{ + TRACE_FUNCTION(__FUNCTION__); + ((dgMeshEffect*) mesh)->RemoveUnusedVertices (vertexRemapTable); +} + + +void NewtonMeshBeginBuild(const NewtonMesh* const mesh) +{ + TRACE_FUNCTION(__FUNCTION__); + dgMeshEffect* const meshEffect = (dgMeshEffect*) mesh; + meshEffect->BeginBuild(); +} + +void NewtonMeshBeginFace (const NewtonMesh* const mesh) +{ + TRACE_FUNCTION(__FUNCTION__); + dgMeshEffect* const meshEffect = (dgMeshEffect*)mesh; + meshEffect->BeginBuildFace(); +} + +void NewtonMeshEndFace(const NewtonMesh* const mesh) +{ + TRACE_FUNCTION(__FUNCTION__); + dgMeshEffect* const meshEffect = (dgMeshEffect*)mesh; + meshEffect->EndBuildFace(); +} + +void NewtonMeshAddPoint(const NewtonMesh* const mesh, dFloat64 x, dFloat64 y, dFloat64 z) +{ + TRACE_FUNCTION(__FUNCTION__); + dgMeshEffect* const meshEffect = (dgMeshEffect*)mesh; + meshEffect->AddPoint (x, y, z); +} + +void NewtonMeshAddMaterial(const NewtonMesh* const mesh, int materialIndex) +{ + TRACE_FUNCTION(__FUNCTION__); + dgMeshEffect* const meshEffect = (dgMeshEffect*)mesh; + meshEffect->AddMaterial(materialIndex); +} + +void NewtonMeshAddLayer(const NewtonMesh* const mesh, int layer) +{ + TRACE_FUNCTION(__FUNCTION__); + dgMeshEffect* const meshEffect = (dgMeshEffect*)mesh; + meshEffect->AddLayer(layer); +} + +void NewtonMeshAddNormal(const NewtonMesh* const mesh, dFloat x, dFloat y, dFloat z) +{ + TRACE_FUNCTION(__FUNCTION__); + dgMeshEffect* const meshEffect = (dgMeshEffect*)mesh; + meshEffect->AddNormal(x, y, z); +} + +void NewtonMeshAddBinormal(const NewtonMesh* const mesh, dFloat x, dFloat y, dFloat z) +{ + TRACE_FUNCTION(__FUNCTION__); + dgMeshEffect* const meshEffect = (dgMeshEffect*)mesh; + meshEffect->AddBinormal(x, y, z); +} + +void NewtonMeshAddUV0(const NewtonMesh* const mesh, dFloat u, dFloat v) +{ + TRACE_FUNCTION(__FUNCTION__); + dgMeshEffect* const meshEffect = (dgMeshEffect*)mesh; + meshEffect->AddUV0(u, v); +} + +void NewtonMeshAddUV1(const NewtonMesh* const mesh, dFloat u, dFloat v) +{ + TRACE_FUNCTION(__FUNCTION__); + dgMeshEffect* const meshEffect = (dgMeshEffect*)mesh; + meshEffect->AddUV1(u, v); +} + +void NewtonMeshAddVertexColor(const NewtonMesh* const mesh, dFloat32 r, dFloat32 g, dFloat32 b, dFloat32 a) +{ + TRACE_FUNCTION(__FUNCTION__); + dgMeshEffect* const meshEffect = (dgMeshEffect*)mesh; + meshEffect->AddVertexColor(r, g, b, a); +} + +void NewtonMeshEndBuild(const NewtonMesh* const mesh) +{ + TRACE_FUNCTION(__FUNCTION__); + dgMeshEffect* const meshEffect = (dgMeshEffect*) mesh; + + meshEffect->EndBuild(dgFloat64 (1.0e-8f)); +} + +void NewtonMeshClearVertexFormat (NewtonMeshVertexFormat* const format) +{ + TRACE_FUNCTION(__FUNCTION__); + dgMeshEffect::dgMeshVertexFormat* const vertexFormat = (dgMeshEffect::dgMeshVertexFormat*) format; + vertexFormat->Clear (); +} + +void NewtonMeshBuildFromVertexListIndexList (const NewtonMesh* const mesh, const NewtonMeshVertexFormat* const format) +{ + TRACE_FUNCTION(__FUNCTION__); + dgMeshEffect* const meshEffect = (dgMeshEffect*) mesh; + meshEffect->BuildFromIndexList((dgMeshEffect::dgMeshVertexFormat*) format); +} + +void NewtonMeshOptimizePoints(const NewtonMesh* const mesh) +{ + TRACE_FUNCTION(__FUNCTION__); + dgMeshEffect* const meshEffect = (dgMeshEffect*)mesh; + meshEffect->OptimizePoints(); +} + +void NewtonMeshOptimizeVertex(const NewtonMesh* const mesh) +{ + TRACE_FUNCTION(__FUNCTION__); + dgMeshEffect* const meshEffect = (dgMeshEffect*)mesh; + meshEffect->OptimizeAttibutes(); +} + +void NewtonMeshOptimize(const NewtonMesh* const mesh) +{ + TRACE_FUNCTION(__FUNCTION__); + NewtonMeshOptimizePoints(mesh); + NewtonMeshOptimizeVertex(mesh); +} + +int NewtonMeshGetVertexCount(const NewtonMesh* const mesh) +{ + TRACE_FUNCTION(__FUNCTION__); + dgMeshEffect* const meshEffect = (dgMeshEffect*) mesh; + return meshEffect->GetVertexCount(); +} + +int NewtonMeshGetVertexBaseCount(const NewtonMesh* const mesh) +{ + TRACE_FUNCTION(__FUNCTION__); + dgMeshEffect* const meshEffect = (dgMeshEffect*)mesh; + return meshEffect->GetVertexBaseCount(); +} + +void NewtonMeshSetVertexBaseCount(const NewtonMesh* const mesh, int baseCount) +{ + TRACE_FUNCTION(__FUNCTION__); + dgMeshEffect* const meshEffect = (dgMeshEffect*)mesh; + meshEffect->SetVertexBaseCount(baseCount); +} + +int NewtonMeshGetVertexStrideInByte(const NewtonMesh* const mesh) +{ + TRACE_FUNCTION(__FUNCTION__); + dgMeshEffect* const meshEffect = (dgMeshEffect*) mesh; + + return meshEffect->GetVertexStrideInByte(); +} + +const dFloat64* NewtonMeshGetVertexArray (const NewtonMesh* const mesh) +{ + TRACE_FUNCTION(__FUNCTION__); + dgMeshEffect* const meshEffect = (dgMeshEffect*) mesh; + + return meshEffect->GetVertexPool (); +} + +int NewtonMeshGetPointCount (const NewtonMesh* const mesh) +{ + TRACE_FUNCTION(__FUNCTION__); + dgMeshEffect* const meshEffect = (dgMeshEffect*) mesh; + return meshEffect->GetPropertiesCount(); +} + +const int* NewtonMeshGetIndexToVertexMap(const NewtonMesh* const mesh) +{ + TRACE_FUNCTION(__FUNCTION__); + dgMeshEffect* const meshEffect = (dgMeshEffect*)mesh; + return meshEffect->GetIndexToVertexMap(); +} + +int NewtonMeshHasNormalChannel(const NewtonMesh* const mesh) +{ + TRACE_FUNCTION(__FUNCTION__); + dgMeshEffect* const meshEffect = (dgMeshEffect*)mesh; + return meshEffect->HasNormalChannel() ? 1 : 0; +} + +int NewtonMeshHasBinormalChannel(const NewtonMesh* const mesh) +{ + TRACE_FUNCTION(__FUNCTION__); + dgMeshEffect* const meshEffect = (dgMeshEffect*)mesh; + return meshEffect->HasBinormalChannel() ? 1 : 0; +} + +int NewtonMeshHasUV0Channel(const NewtonMesh* const mesh) +{ + TRACE_FUNCTION(__FUNCTION__); + dgMeshEffect* const meshEffect = (dgMeshEffect*)mesh; + return meshEffect->HasUV0Channel() ? 1 : 0; +} + +int NewtonMeshHasUV1Channel(const NewtonMesh* const mesh) +{ + TRACE_FUNCTION(__FUNCTION__); + dgMeshEffect* const meshEffect = (dgMeshEffect*)mesh; + return meshEffect->HasUV1Channel() ? 1 : 0; +} + +int NewtonMeshHasVertexColorChannel(const NewtonMesh* const mesh) +{ + TRACE_FUNCTION(__FUNCTION__); + dgMeshEffect* const meshEffect = (dgMeshEffect*)mesh; + return meshEffect->HasVertexColorChannel() ? 1 : 0; +} + +void NewtonMeshGetVertexDoubleChannel (const NewtonMesh* const mesh, int vertexStrideInByte, dFloat64* const outBuffer) +{ + TRACE_FUNCTION(__FUNCTION__); + dgMeshEffect* const meshEffect = (dgMeshEffect*)mesh; + meshEffect->GetVertexChannel64(vertexStrideInByte, (dgFloat64*)outBuffer); +} + +void NewtonMeshGetVertexChannel (const NewtonMesh* const mesh, int vertexStrideInByte, dFloat* const outBuffer) +{ + TRACE_FUNCTION(__FUNCTION__); + dgMeshEffect* const meshEffect = (dgMeshEffect*)mesh; + meshEffect->GetVertexChannel(vertexStrideInByte, (dgFloat32*)outBuffer); +} + +void NewtonMeshGetNormalChannel(const NewtonMesh* const mesh, int vertexStrideInByte, dFloat* const outBuffer) +{ + TRACE_FUNCTION(__FUNCTION__); + dgMeshEffect* const meshEffect = (dgMeshEffect*)mesh; + meshEffect->GetNormalChannel(vertexStrideInByte, (dgFloat32*)outBuffer); +} + +void NewtonMeshGetBinormalChannel(const NewtonMesh* const mesh, int vertexStrideInByte, dFloat* const outBuffer) +{ + TRACE_FUNCTION(__FUNCTION__); + dgMeshEffect* const meshEffect = (dgMeshEffect*)mesh; + meshEffect->GetBinormalChannel(vertexStrideInByte, (dgFloat32*)outBuffer); +} + +void NewtonMeshGetUV0Channel(const NewtonMesh* const mesh, int vertexStrideInByte, dFloat* const outBuffer) +{ + TRACE_FUNCTION(__FUNCTION__); + dgMeshEffect* const meshEffect = (dgMeshEffect*)mesh; + meshEffect->GetUV0Channel(vertexStrideInByte, (dgFloat32*)outBuffer); +} + +void NewtonMeshGetUV1Channel(const NewtonMesh* const mesh, int vertexStrideInByte, dFloat* const outBuffer) +{ + TRACE_FUNCTION(__FUNCTION__); + dgMeshEffect* const meshEffect = (dgMeshEffect*)mesh; + meshEffect->GetUV1Channel(vertexStrideInByte, (dgFloat32*)outBuffer); +} + +void NewtonMeshGetVertexColorChannel(const NewtonMesh* const mesh, int vertexStrideInByte, dFloat* const outBuffer) +{ + TRACE_FUNCTION(__FUNCTION__); + dgMeshEffect* const meshEffect = (dgMeshEffect*)mesh; + meshEffect->GetVertexColorChannel(vertexStrideInByte, (dgFloat32*)outBuffer); +} + + +void* NewtonMeshBeginHandle (const NewtonMesh* const mesh) +{ + TRACE_FUNCTION(__FUNCTION__); + dgMeshEffect* const meshEffect = (dgMeshEffect*) mesh; + + return meshEffect->MaterialGeometryBegin(); +} + +void NewtonMeshEndHandle (const NewtonMesh* const mesh, void* const handle) +{ + TRACE_FUNCTION(__FUNCTION__); + dgMeshEffect* const meshEffect = (dgMeshEffect*) mesh; + meshEffect->MaterialGeomteryEnd((dgMeshEffect::dgIndexArray*) handle); +} + + + +int NewtonMeshFirstMaterial (const NewtonMesh* const mesh, void* const handle) +{ + TRACE_FUNCTION(__FUNCTION__); + dgMeshEffect* const meshEffect = (dgMeshEffect*) mesh; + + return meshEffect->GetFirstMaterial((dgMeshEffect::dgIndexArray*) handle); +} + +int NewtonMeshNextMaterial (const NewtonMesh* const mesh, void* const handle, int materialId) +{ + TRACE_FUNCTION(__FUNCTION__); + dgMeshEffect* const meshEffect = (dgMeshEffect*) mesh; + + return meshEffect->GetNextMaterial((dgMeshEffect::dgIndexArray*) handle, materialId); +} + +int NewtonMeshMaterialGetMaterial (const NewtonMesh* const mesh, void* const handle, int materialId) +{ + TRACE_FUNCTION(__FUNCTION__); + dgMeshEffect* const meshEffect = (dgMeshEffect*) mesh; + + return meshEffect->GetMaterialID ((dgMeshEffect::dgIndexArray*) handle, materialId); +} + +int NewtonMeshMaterialGetIndexCount (const NewtonMesh* const mesh, void* const handle, int materialId) +{ + TRACE_FUNCTION(__FUNCTION__); + dgMeshEffect* const meshEffect = (dgMeshEffect*) mesh; + + return meshEffect->GetMaterialIndexCount ((dgMeshEffect::dgIndexArray*) handle, materialId); +} + +void NewtonMeshMaterialGetIndexStream (const NewtonMesh* const mesh, void* const handle, int materialId, int* const index) +{ + TRACE_FUNCTION(__FUNCTION__); + dgMeshEffect* const meshEffect = (dgMeshEffect*) mesh; + + meshEffect->GetMaterialGetIndexStream ((dgMeshEffect::dgIndexArray*) handle, materialId, index); +} + +void NewtonMeshMaterialGetIndexStreamShort (const NewtonMesh* const mesh, void* const handle, int materialId, short int* const index) +{ + TRACE_FUNCTION(__FUNCTION__); + dgMeshEffect* const meshEffect = (dgMeshEffect*) mesh; + + meshEffect->GetMaterialGetIndexStreamShort ((dgMeshEffect::dgIndexArray*) handle, materialId, index); +} + + +NewtonMesh* NewtonMeshCreateFirstSingleSegment (const NewtonMesh* const mesh) +{ + TRACE_FUNCTION(__FUNCTION__); + + dgMeshEffect* const effectMesh = (dgMeshEffect*)mesh; + dgPolyhedra segment(effectMesh->GetAllocator()); + + effectMesh->BeginConectedSurface(); + if (effectMesh->GetConectedSurface (segment)) { + dgMeshEffect* const solid = new (effectMesh->GetAllocator()) dgMeshEffect(segment, *((dgMeshEffect*)mesh)); + return (NewtonMesh*)solid; + } else { + return NULL; + } +} + +NewtonMesh* NewtonMeshCreateNextSingleSegment (const NewtonMesh* const mesh, const NewtonMesh* const segment) +{ + TRACE_FUNCTION(__FUNCTION__); + + dgMeshEffect* const effectMesh = (dgMeshEffect*)mesh; + dgPolyhedra nextSegment(effectMesh->GetAllocator()); + + dgAssert (segment); + dgInt32 moreSegments = effectMesh->GetConectedSurface (nextSegment); + + dgMeshEffect* solid; + if (moreSegments) { + solid = new (effectMesh->GetAllocator()) dgMeshEffect(nextSegment, *effectMesh); + } else { + solid = NULL; + effectMesh->EndConectedSurface(); + } + + return (NewtonMesh*)solid; +} + +NewtonMesh* NewtonMeshCreateFirstLayer (const NewtonMesh* const mesh) +{ + TRACE_FUNCTION(__FUNCTION__); + + dgMeshEffect* const effectMesh = (dgMeshEffect*)mesh; + return (NewtonMesh*) effectMesh->GetFirstLayer (); +} + +NewtonMesh* NewtonMeshCreateNextLayer (const NewtonMesh* const mesh, const NewtonMesh* const segment) +{ + TRACE_FUNCTION(__FUNCTION__); + + dgMeshEffect* const effectMesh = (dgMeshEffect*)mesh; + return (NewtonMesh*) effectMesh->GetNextLayer ((dgMeshEffect*)segment); +} + + + +int NewtonMeshGetTotalFaceCount (const NewtonMesh* const mesh) +{ + TRACE_FUNCTION(__FUNCTION__); + return ((dgMeshEffect*)mesh)->GetTotalFaceCount(); +} + +int NewtonMeshGetTotalIndexCount (const NewtonMesh* const mesh) +{ + TRACE_FUNCTION(__FUNCTION__); + return ((dgMeshEffect*)mesh)->GetTotalIndexCount(); +} + +void NewtonMeshGetFaces (const NewtonMesh* const mesh, int* const faceIndexCount, int* const faceMaterial, void** const faceIndices) +{ + TRACE_FUNCTION(__FUNCTION__); + ((dgMeshEffect*)mesh)->GetFaces (faceIndexCount, faceMaterial, faceIndices); +} + + +void* NewtonMeshGetFirstVertex (const NewtonMesh* const mesh) +{ + TRACE_FUNCTION(__FUNCTION__); + return ((dgMeshEffect*)mesh)->GetFirstVertex (); +} + +void* NewtonMeshGetNextVertex (const NewtonMesh* const mesh, const void* const vertex) +{ + TRACE_FUNCTION(__FUNCTION__); + return ((dgMeshEffect*)mesh)->GetNextVertex (vertex); +} + +int NewtonMeshGetVertexIndex (const NewtonMesh* const mesh, const void* const vertex) +{ + TRACE_FUNCTION(__FUNCTION__); + return ((dgMeshEffect*)mesh)->GetVertexIndex (vertex); +} + +void* NewtonMeshGetFirstPoint (const NewtonMesh* const mesh) +{ + TRACE_FUNCTION(__FUNCTION__); + return ((dgMeshEffect*)mesh)->GetFirstPoint (); +} +void* NewtonMeshGetNextPoint (const NewtonMesh* const mesh, const void* const point) +{ + TRACE_FUNCTION(__FUNCTION__); + return ((dgMeshEffect*)mesh)->GetNextPoint (point); +} + +int NewtonMeshGetPointIndex (const NewtonMesh* const mesh, const void* const point) +{ + TRACE_FUNCTION(__FUNCTION__); + return ((dgMeshEffect*)mesh)->GetPointIndex (point); +} + +int NewtonMeshGetVertexIndexFromPoint (const NewtonMesh* const mesh, const void* const point) +{ + TRACE_FUNCTION(__FUNCTION__); + return ((dgMeshEffect*)mesh)->GetVertexIndexFromPoint (point); +} + +void* NewtonMeshGetFirstEdge (const NewtonMesh* const mesh) +{ + TRACE_FUNCTION(__FUNCTION__); + return ((dgMeshEffect*)mesh)->GetFirstEdge (); +} + +void* NewtonMeshGetNextEdge (const NewtonMesh* const mesh, const void* const edge) +{ + TRACE_FUNCTION(__FUNCTION__); + return ((dgMeshEffect*)mesh)->GetNextEdge (edge); +} + +void NewtonMeshGetEdgeIndices (const NewtonMesh* const mesh, const void* const edge, int* const v0, int* const v1) +{ + TRACE_FUNCTION(__FUNCTION__); + return ((dgMeshEffect*)mesh)->GetEdgeIndex (edge, *v0, *v1); +} + + +//void NewtonMeshGetEdgePointIndices (const NewtonMesh* const mesh, const void* const edge, int* const v0, int* const v1) +//{ +// return ((dgMeshEffect*)mesh)->GetEdgeAttributeIndex (edge, *v0, *v1); +//} + +void* NewtonMeshGetFirstFace (const NewtonMesh* const mesh) +{ + TRACE_FUNCTION(__FUNCTION__); + return ((dgMeshEffect*)mesh)->GetFirstFace (); +} + +void* NewtonMeshGetNextFace (const NewtonMesh* const mesh, const void* const face) +{ + TRACE_FUNCTION(__FUNCTION__); + return ((dgMeshEffect*)mesh)->GetNextFace (face); +} + +int NewtonMeshIsFaceOpen (const NewtonMesh* const mesh, const void* const face) +{ + TRACE_FUNCTION(__FUNCTION__); + return ((dgMeshEffect*)mesh)->IsFaceOpen (face); +} + +int NewtonMeshGetFaceIndexCount (const NewtonMesh* const mesh, const void* const face) +{ + TRACE_FUNCTION(__FUNCTION__); + return ((dgMeshEffect*)mesh)->GetFaceIndexCount (face); +} + +int NewtonMeshGetFaceMaterial (const NewtonMesh* const mesh, const void* const face) +{ + TRACE_FUNCTION(__FUNCTION__); + return ((dgMeshEffect*)mesh)->GetFaceMaterial (face); +} + +void NewtonMeshSetFaceMaterial (const NewtonMesh* const mesh, const void* const face, int matId) +{ + TRACE_FUNCTION(__FUNCTION__); + return ((dgMeshEffect*)mesh)->SetFaceMaterial (face, matId); +} + +void NewtonMeshGetFaceIndices (const NewtonMesh* const mesh, const void* const face, int* const indices) +{ + TRACE_FUNCTION(__FUNCTION__); + ((dgMeshEffect*)mesh)->GetFaceIndex (face, indices); +} + +void NewtonMeshGetFacePointIndices (const NewtonMesh* const mesh, const void* const face, int* const indices) +{ + TRACE_FUNCTION(__FUNCTION__); + ((dgMeshEffect*)mesh)->GetFaceAttributeIndex (face, indices); +} + +void NewtonMeshCalculateFaceNormal (const NewtonMesh* const mesh, const void* const face, dFloat64* const normal) +{ + TRACE_FUNCTION(__FUNCTION__); + dgBigVector n (((dgMeshEffect*)mesh)->CalculateFaceNormal (face)); + normal[0] = n.m_x; + normal[1] = n.m_y; + normal[2] = n.m_z; +} + +NewtonCollision* NewtonCreateDeformableSolid(const NewtonWorld* const newtonWorld, const NewtonMesh* const mesh, int shapeID) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *)newtonWorld; + return (NewtonCollision*) world->CreateDeformableSolid ((dgMeshEffect*)mesh, shapeID); +} + +NewtonCollision* NewtonCreateMassSpringDamperSystem (const NewtonWorld* const newtonWorld, int shapeID, + const dFloat* const points, int pointCount, int strideInBytes, const dFloat* const pointMass, + const int* const links, int linksCount, const dFloat* const linksSpring, const dFloat* const linksDamper) +{ + TRACE_FUNCTION(__FUNCTION__); + Newton* const world = (Newton *)newtonWorld; + return (NewtonCollision*)world->CreateMassSpringDamperSystem (shapeID, pointCount, points, strideInBytes, pointMass, linksCount, links, linksSpring, linksDamper); +} + +int NewtonDeformableMeshGetParticleCount(const NewtonCollision* const deformableMesh) +{ + TRACE_FUNCTION(__FUNCTION__); + dgCollisionInstance* const collision = (dgCollisionInstance*)deformableMesh; + if (collision->IsType(dgCollision::dgCollisionLumpedMass_RTTI)) { + dgCollisionLumpedMassParticles* const deformableShape = (dgCollisionLumpedMassParticles*)collision->GetChildShape(); + return deformableShape->GetCount(); + } + return 0; +} + + +const dFloat* NewtonDeformableMeshGetParticleArray(const NewtonCollision* const deformableMesh) +{ + TRACE_FUNCTION(__FUNCTION__); + dgCollisionInstance* const collision = (dgCollisionInstance*)deformableMesh; + if (collision->IsType(dgCollision::dgCollisionLumpedMass_RTTI)) { + dgCollisionLumpedMassParticles* const deformableShape = (dgCollisionLumpedMassParticles*)collision->GetChildShape(); + const dgVector* const posit = deformableShape->GetPositions(); + return &posit[0].m_x; + } + return NULL; +} + + +int NewtonDeformableMeshGetParticleStrideInBytes(const NewtonCollision* const deformableMesh) +{ + TRACE_FUNCTION(__FUNCTION__); + dgCollisionInstance* const collision = (dgCollisionInstance*)deformableMesh; + if (collision->IsType(dgCollision::dgCollisionLumpedMass_RTTI)) { + dgCollisionLumpedMassParticles* const deformableShape = (dgCollisionLumpedMassParticles*)collision->GetChildShape(); + return deformableShape->GetStrideInByte(); + } + return 0; +} + + + +/* +void NewtonDeformableMeshConstraintParticle(NewtonCollision* const deformableMesh, int particleIndex, const dFloat* const posit, const NewtonBody* const body) +{ + TRACE_FUNCTION(__FUNCTION__); + dgCollisionInstance* const collision = (dgCollisionInstance*)deformableMesh; + if (collision->IsType(dgCollision::dgCollisionDeformableMesh_RTTI)) { + dgCollisionDeformableMesh* const deformableShape = (dgCollisionDeformableMesh*)collision->GetChildShape(); + dgVector position(posit[0], posit[1], posit[2], dgFloat32(0.0f)); + deformableShape->ConstraintParticle(particleIndex, position, (dgBody*)body); + } +} + + + +void NewtonDeformableMeshCreateClusters (NewtonCollision* const deformableMesh, int clunsterCount, dFloat overlapingWidth) +{ + dgAssert(0); + + TRACE_FUNCTION(__FUNCTION__); + dgCollisionInstance* const collision = (dgCollisionInstance*) deformableMesh; + if (collision->IsType(dgCollision::dgCollisionDeformableMesh_RTTI)) { + dgCollisionDeformableMesh* const deformableShape = (dgCollisionDeformableMesh*) collision->GetChildShape(); + deformableShape->CreateClusters(clunsterCount, overlapingWidth); + } + +} + +void NewtonDeformableMeshSetDebugCallback (NewtonCollision* const deformableMesh, NewtonCollisionIterator callback) +{ + TRACE_FUNCTION(__FUNCTION__); + dgCollisionInstance* const collision = (dgCollisionInstance*) deformableMesh; + if (collision->IsType(dgCollision::dgCollisionDeformableMesh_RTTI)) { + dgCollisionDeformableMesh* const deformableShape = (dgCollisionDeformableMesh*) collision->GetChildShape(); + deformableShape->SetOnDebugDisplay((dgCollision::OnDebugCollisionMeshCallback)callback); + } +} + +void NewtonDeformableMeshGetParticlePosition (NewtonCollision* const deformableMesh, int particleIndex, dFloat* const posit) +{ + TRACE_FUNCTION(__FUNCTION__); + dgCollisionInstance* const collision = (dgCollisionInstance*) deformableMesh; + if (collision->IsType(dgCollision::dgCollisionDeformableMesh_RTTI)) { + dgCollisionDeformableMesh* const deformableShape = (dgCollisionDeformableMesh*) collision->GetChildShape(); + dgVector p (deformableShape->GetParticlePosition(particleIndex)); + posit[0] = p[0]; + posit[1] = p[1]; + posit[2] = p[2]; + } +} + +void NewtonDeformableMeshBeginConfiguration (const NewtonCollision* const deformableMesh) +{ +} + +void NewtonDeformableMeshEndConfiguration (const NewtonCollision* const deformableMesh) +{ + TRACE_FUNCTION(__FUNCTION__); + dgCollisionInstance* const collision = (dgCollisionInstance*) deformableMesh; + if (collision->IsType(dgCollision::dgCollisionDeformableMesh_RTTI)) { + dgCollisionDeformableMesh* const deformableShape = (dgCollisionDeformableMesh*) collision->GetChildShape(); + deformableShape->EndConfiguration(); + } +} + +void NewtonDeformableMeshUnconstraintParticle (NewtonCollision* const deformableMesh, int partivleIndex) +{ +} + + + +void NewtonDeformableMeshSetSkinThickness (NewtonCollision* const deformableMesh, dFloat skinThickness) +{ + TRACE_FUNCTION(__FUNCTION__); + dgCollisionInstance* const collision = (dgCollisionInstance*) deformableMesh; + if (collision->IsType(dgCollision::dgCollisionDeformableMesh_RTTI)) { + dgCollisionDeformableMesh* const deformableShape = (dgCollisionDeformableMesh*) collision->GetChildShape(); + deformableShape->SetSkinThickness(skinThickness); + } +} + +void NewtonDeformableMeshSetPlasticity (NewtonCollision* const deformableMesh, dFloat plasticity) +{ + TRACE_FUNCTION(__FUNCTION__); + dgAssert (0); + + dgCollisionInstance* const collision = (dgCollisionInstance*) deformableMesh; + if (collision->IsType(dgCollision::dgCollisionDeformableMesh_RTTI)) { + dgCollisionDeformableMesh* const deformable = (dgCollisionDeformableMesh*) collision; + deformable->SetPlasticity (plasticity); + } +} + +void NewtonDeformableMeshSetStiffness (NewtonCollision* const deformableMesh, dFloat stiffness) +{ + TRACE_FUNCTION(__FUNCTION__); + dgAssert (0); + + dgCollisionInstance* const collision = (dgCollisionInstance*) deformableMesh; + if (collision->IsType(dgCollision::dgCollisionDeformableMesh_RTTI)) { + dgCollisionDeformableMesh* const deformable = (dgCollisionDeformableMesh*) collision; + deformable->SetStiffness(stiffness); + } +} + + +int NewtonDeformableMeshGetVertexCount (const NewtonCollision* const deformableMesh) +{ + TRACE_FUNCTION(__FUNCTION__); + dgCollisionInstance* const collision = (dgCollisionInstance*) deformableMesh; + if (collision->IsType(dgCollision::dgCollisionDeformableMesh_RTTI)) { + dgCollisionDeformableMesh* const deformableShape = (dgCollisionDeformableMesh*) collision->GetChildShape(); + return deformableShape->GetVisualPointsCount(); + } + return 0; +} + +void NewtonDeformableMeshUpdateRenderNormals (const NewtonCollision* const deformableMesh) +{ + TRACE_FUNCTION(__FUNCTION__); + dgCollisionInstance* const collision = (dgCollisionInstance*) deformableMesh; + if (collision->IsType(dgCollision::dgCollisionDeformableMesh_RTTI)) { + dgCollisionDeformableMesh* const deformableShape = (dgCollisionDeformableMesh*) collision->GetChildShape(); + deformableShape->UpdateVisualNormals(); + } +} + +void NewtonDeformableMeshGetVertexStreams (const NewtonCollision* const deformableMesh, int vertexStrideInByte, dFloat* const vertex, int normalStrideInByte, dFloat* const normal, int uvStrideInByte0, dFloat* const uv0) +{ + TRACE_FUNCTION(__FUNCTION__); + dgCollisionInstance* const collision = (dgCollisionInstance*) deformableMesh; + if (collision->IsType(dgCollision::dgCollisionDeformableMesh_RTTI)) { + dgCollisionDeformableMesh* const deformableShape = (dgCollisionDeformableMesh*) collision->GetChildShape(); + deformableShape->GetVisualVertexData(vertexStrideInByte, vertex, normalStrideInByte, normal, uvStrideInByte0, uv0); + } +} + +NewtonDeformableMeshSegment* NewtonDeformableMeshGetFirstSegment (const NewtonCollision* const deformableMesh) +{ + TRACE_FUNCTION(__FUNCTION__); + dgCollisionInstance* const collision = (dgCollisionInstance*) deformableMesh; + if (collision->IsType(dgCollision::dgCollisionDeformableMesh_RTTI)) { + dgCollisionDeformableMesh* const deformableShape = (dgCollisionDeformableMesh*) collision->GetChildShape(); + return (NewtonDeformableMeshSegment*) deformableShape->GetFirtVisualSegment(); + } + return NULL; +} + +NewtonDeformableMeshSegment* NewtonDeformableMeshGetNextSegment (const NewtonCollision* const deformableMesh, const NewtonDeformableMeshSegment* const segment) +{ + TRACE_FUNCTION(__FUNCTION__); + dgCollisionInstance* const collision = (dgCollisionInstance*) deformableMesh; + if (collision->IsType(dgCollision::dgCollisionDeformableMesh_RTTI)) { + dgCollisionDeformableMesh* const deformableShape = (dgCollisionDeformableMesh*) collision->GetChildShape(); + return (NewtonDeformableMeshSegment*) deformableShape->GetNextVisualSegment((void*)segment); + } + return NULL; +} + +int NewtonDeformableMeshSegmentGetMaterialID (const NewtonCollision* const deformableMesh, const NewtonDeformableMeshSegment* const segment) +{ + TRACE_FUNCTION(__FUNCTION__); + dgCollisionInstance* const collision = (dgCollisionInstance*) deformableMesh; + if (collision->IsType(dgCollision::dgCollisionDeformableMesh_RTTI)) { + dgCollisionDeformableMesh* const deformableShape = (dgCollisionDeformableMesh*) collision->GetChildShape(); + return deformableShape->GetSegmentMaterial((void*)segment); + } + return 0; +} + +int NewtonDeformableMeshSegmentGetIndexCount (const NewtonCollision* const deformableMesh, const NewtonDeformableMeshSegment* const segment) +{ + TRACE_FUNCTION(__FUNCTION__); + dgCollisionInstance* const collision = (dgCollisionInstance*) deformableMesh; + if (collision->IsType(dgCollision::dgCollisionDeformableMesh_RTTI)) { + dgCollisionDeformableMesh* const deformableShape = (dgCollisionDeformableMesh*) collision->GetChildShape(); + return deformableShape->GetSegmentIndexCount((void*)segment); + } + return 0; +} + +const int* NewtonDeformableMeshSegmentGetIndexList (const NewtonCollision* const deformableMesh, const NewtonDeformableMeshSegment* const segment) +{ + TRACE_FUNCTION(__FUNCTION__); + dgCollisionInstance* const collision = (dgCollisionInstance*) deformableMesh; + if (collision->IsType(dgCollision::dgCollisionDeformableMesh_RTTI)) { + dgCollisionDeformableMesh* const deformableShape = (dgCollisionDeformableMesh*) collision->GetChildShape(); + return deformableShape->GetSegmentIndexList((void*)segment); + } + return NULL; +} +*/ + +/*! @} */ // end of + + +void* NewtonCollisionAggregateCreate(NewtonWorld* const worldPtr) +{ + TRACE_FUNCTION(__FUNCTION__); + dgWorld* const world = (dgWorld*) worldPtr; + return world->CreateAggreGate(); +} + +void NewtonCollisionAggregateDestroy(void* const aggregatePtr) +{ + TRACE_FUNCTION(__FUNCTION__); + dgBroadPhaseAggregate* const aggregate = (dgBroadPhaseAggregate*) aggregatePtr; + aggregate->m_broadPhase->GetWorld()->DestroyAggregate(aggregate); +} + +void NewtonCollisionAggregateAddBody(void* const aggregatePtr, const NewtonBody* const body) +{ + TRACE_FUNCTION(__FUNCTION__); + dgBroadPhaseAggregate* const aggregate = (dgBroadPhaseAggregate*) aggregatePtr; + aggregate->AddBody((dgBody*)body); +} + +void NewtonCollisionAggregateRemoveBody(void* const aggregatePtr, const NewtonBody* const body) +{ + TRACE_FUNCTION(__FUNCTION__); + dgBroadPhaseAggregate* const aggregate = (dgBroadPhaseAggregate*) aggregatePtr; + aggregate->RemoveBody((dgBody*)body); +} + +int NewtonCollisionAggregateGetSelfCollision(void* const aggregatePtr) +{ + TRACE_FUNCTION(__FUNCTION__); + dgBroadPhaseAggregate* const aggregate = (dgBroadPhaseAggregate*) aggregatePtr; + return aggregate->GetSelfCollision() ? true : false; +} + +void NewtonCollisionAggregateSetSelfCollision(void* const aggregatePtr, int state) +{ + TRACE_FUNCTION(__FUNCTION__); + dgBroadPhaseAggregate* const aggregate = (dgBroadPhaseAggregate*) aggregatePtr; + aggregate->SetSelfCollision(state ? true : false); +} +/*! @} */ // end of + + diff --git a/thirdparty/src/newton/dgNewton/Newton.h b/thirdparty/src/newton/dgNewton/Newton.h new file mode 100644 index 000000000..1a7f3a039 --- /dev/null +++ b/thirdparty/src/newton/dgNewton/Newton.h @@ -0,0 +1,1322 @@ +/* Copyright (c) <2003-2019> +* +* 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 __NEWTON_H__ +#define __NEWTON_H__ + + +#define NEWTON_MAJOR_VERSION 3 +#define NEWTON_MINOR_VERSION 14 + +#include + +#ifdef _NEWTON_STATIC_LIB + #define NEWTON_API DG_LIBRARY_STATIC +#elif defined(_NEWTON_BUILD_DLL) + #define NEWTON_API DG_LIBRARY_EXPORT +#else + #define NEWTON_API DG_LIBRARY_IMPORT +#endif + + +#ifndef dLong + #define dLong long long +#endif + +#ifndef dFloat + #ifdef _NEWTON_USE_DOUBLE + #define dFloat double + #else + #define dFloat float + #endif +#endif + +#ifndef dFloat32 + #define dFloat32 float +#endif + +#ifndef dFloat64 + #define dFloat64 double +#endif + +#ifdef __cplusplus +extern "C" { +#endif + + #define NEWTON_BROADPHASE_DEFAULT 0 + #define NEWTON_BROADPHASE_PERSINTENT 1 + + #define NEWTON_DYNAMIC_BODY 0 + #define NEWTON_KINEMATIC_BODY 1 + #define NEWTON_DYNAMIC_ASYMETRIC_BODY 2 +// #define NEWTON_DEFORMABLE_BODY 2 + + #define SERIALIZE_ID_SPHERE 0 + #define SERIALIZE_ID_CAPSULE 1 + #define SERIALIZE_ID_CYLINDER 2 + #define SERIALIZE_ID_CHAMFERCYLINDER 3 + #define SERIALIZE_ID_BOX 4 + #define SERIALIZE_ID_CONE 5 + #define SERIALIZE_ID_CONVEXHULL 6 + #define SERIALIZE_ID_NULL 7 + #define SERIALIZE_ID_COMPOUND 8 + #define SERIALIZE_ID_TREE 9 + #define SERIALIZE_ID_HEIGHTFIELD 10 + #define SERIALIZE_ID_CLOTH_PATCH 11 + #define SERIALIZE_ID_DEFORMABLE_SOLID 12 + #define SERIALIZE_ID_USERMESH 13 + #define SERIALIZE_ID_SCENE 14 + #define SERIALIZE_ID_FRACTURED_COMPOUND 15 + +#ifdef __cplusplus + class NewtonMesh; + class NewtonBody; + class NewtonWorld; + class NewtonJoint; + class NewtonMaterial; + class NewtonCollision; + class NewtonDeformableMeshSegment; + class NewtonFracturedCompoundMeshPart; +#else + typedef struct NewtonMesh{} NewtonMesh; + typedef struct NewtonBody{} NewtonBody; + typedef struct NewtonWorld{} NewtonWorld; + typedef struct NewtonJoint{} NewtonJoint; + typedef struct NewtonMaterial{} NewtonMaterial; + typedef struct NewtonCollision{} NewtonCollision; + typedef struct NewtonDeformableMeshSegment{} NewtonDeformableMeshSegment; + typedef struct NewtonFracturedCompoundMeshPart{} NewtonFracturedCompoundMeshPart; +#endif + + typedef union + { + void* m_ptr; + dLong m_int; + dFloat m_float; + } NewtonMaterialData; + + typedef struct NewtonCollisionMaterial + { + dLong m_userId; + NewtonMaterialData m_userData; + NewtonMaterialData m_userParam[6]; + } NewtonCollisionMaterial; + + typedef struct NewtonBoxParam + { + dFloat m_x; + dFloat m_y; + dFloat m_z; + } NewtonBoxParam; + + typedef struct NewtonSphereParam + { + dFloat m_radio; + } NewtonSphereParam; + + + typedef struct NewtonCapsuleParam + { + dFloat m_radio0; + dFloat m_radio1; + dFloat m_height; + } NewtonCapsuleParam; + + typedef struct NewtonCylinderParam + { + dFloat m_radio0; + dFloat m_radio1; + dFloat m_height; + } NewtonCylinderParam; + + typedef struct NewtonConeParam + { + dFloat m_radio; + dFloat m_height; + } NewtonConeParam; + + typedef struct NewtonChamferCylinderParam + { + dFloat m_radio; + dFloat m_height; + } NewtonChamferCylinderParam; + + typedef struct NewtonConvexHullParam + { + int m_vertexCount; + int m_vertexStrideInBytes; + int m_faceCount; + dFloat* m_vertex; + } NewtonConvexHullParam; + + typedef struct NewtonCompoundCollisionParam + { + int m_chidrenCount; + } NewtonCompoundCollisionParam; + + typedef struct NewtonCollisionTreeParam + { + int m_vertexCount; + int m_indexCount; + } NewtonCollisionTreeParam; + + typedef struct NewtonDeformableMeshParam + { + int m_vertexCount; + int m_triangleCount; + int m_vrtexStrideInBytes; + unsigned short *m_indexList; + dFloat *m_vertexList; + } NewtonDeformableMeshParam; + + typedef struct NewtonHeightFieldCollisionParam + { + int m_width; + int m_height; + int m_gridsDiagonals; + int m_elevationDataType; // 0 = 32 bit floats, 1 = unsigned 16 bit integers + dFloat m_verticalScale; + dFloat m_horizonalScale_x; + dFloat m_horizonalScale_z; + void* m_vertialElevation; + char* m_atributes; + } NewtonHeightFieldCollisionParam; + + typedef struct NewtonSceneCollisionParam + { + int m_childrenProxyCount; + } NewtonSceneCollisionParam; + + typedef struct NewtonCollisionInfoRecord + { + dFloat m_offsetMatrix[4][4]; + NewtonCollisionMaterial m_collisionMaterial; + int m_collisionType; // tag id to identify the collision primitive + union { + NewtonBoxParam m_box; + NewtonConeParam m_cone; + NewtonSphereParam m_sphere; + NewtonCapsuleParam m_capsule; + NewtonCylinderParam m_cylinder; + NewtonChamferCylinderParam m_chamferCylinder; + NewtonConvexHullParam m_convexHull; + NewtonDeformableMeshParam m_deformableMesh; + NewtonCompoundCollisionParam m_compoundCollision; + NewtonCollisionTreeParam m_collisionTree; + NewtonHeightFieldCollisionParam m_heightField; + NewtonSceneCollisionParam m_sceneCollision; + dFloat m_paramArray[64]; // user define collision can use this to store information + }; + } NewtonCollisionInfoRecord; + + typedef struct NewtonJointRecord + { + dFloat m_attachmenMatrix_0[4][4]; + dFloat m_attachmenMatrix_1[4][4]; + dFloat m_minLinearDof[3]; + dFloat m_maxLinearDof[3]; + dFloat m_minAngularDof[3]; + dFloat m_maxAngularDof[3]; + const NewtonBody* m_attachBody_0; + const NewtonBody* m_attachBody_1; + dFloat m_extraParameters[64]; + int m_bodiesCollisionOn; + char m_descriptionType[128]; + } NewtonJointRecord; + + typedef struct NewtonUserMeshCollisionCollideDesc + { + dFloat m_boxP0[4]; // lower bounding box of intersection query in local space + dFloat m_boxP1[4]; // upper bounding box of intersection query in local space + dFloat m_boxDistanceTravel[4]; // max distance that box bpxP0 and boxP1 can travel on this timestep, used this for continue collision mode. + int m_threadNumber; // current thread executing this query + int m_faceCount; // the application should set here how many polygons intersect the query box + int m_vertexStrideInBytes; // the application should set here the size of each vertex + dFloat m_skinThickness; // this is the minimum skin separation specified by the material between these two colliding shapes + void* m_userData; // user data passed to the collision geometry at creation time + + NewtonBody* m_objBody; // pointer to the colliding body + NewtonBody* m_polySoupBody; // pointer to the rigid body owner of this collision tree + NewtonCollision* m_objCollision; // collision shape of the colliding body, (no necessarily the collision of m_objBody) + NewtonCollision* m_polySoupCollision; // collision shape of the collision tree, (no necessarily the collision of m_polySoupBody) + + dFloat* m_vertex; // the application should set here the pointer to the global vertex of the mesh. + int* m_faceIndexCount; // the application should set here the pointer to the vertex count of each face. + int* m_faceVertexIndex; // the application should set here the pointer index array for each vertex on a face. + // the format of a face is I0, I1, I2, I3, ..., M, N, E0, E1, E2, ..., A + // I0, I1, I2, .. are the indices to the vertex, relative to m_vertex pointer + // M is the index to the material sub shape id + // N in the index to the vertex normal relative to m_vertex pointer + // E0, E1, E2, ... are the indices of the the face normal that is shared to that face edge, when the edge does not share a face normal then the edge index is set to index N, which the index to the face normal + // A is and estimate of the largest diagonal of the face, this used internally as a hint to improve floating point accuracy and algorithm performance. + } NewtonUserMeshCollisionCollideDesc; + + typedef struct NewtonWorldConvexCastReturnInfo + { + dFloat m_point[4]; // collision point in global space + dFloat m_normal[4]; // surface normal at collision point in global space + //dFloat m_normalOnHitPoint[4]; // surface normal at the surface of the hit body, + // is the same as the normal calculated by a ray cast hitting the body at the hit point + dLong m_contactID; // collision ID at contact point + const NewtonBody* m_hitBody; // body hit at contact point + dFloat m_penetration; // contact penetration at collision point + } NewtonWorldConvexCastReturnInfo; + + typedef struct NewtonUserMeshCollisionRayHitDesc + { + dFloat m_p0[4]; // ray origin in collision local space + dFloat m_p1[4]; // ray destination in collision local space + dFloat m_normalOut[4]; // copy here the normal at the ray intersection + dLong m_userIdOut; // copy here a user defined id for further feedback + void* m_userData; // user data passed to the collision geometry at creation time + } NewtonUserMeshCollisionRayHitDesc; + + typedef struct NewtonHingeSliderUpdateDesc + { + dFloat m_accel; + dFloat m_minFriction; + dFloat m_maxFriction; + dFloat m_timestep; + } NewtonHingeSliderUpdateDesc; + + typedef struct NewtonUserContactPoint + { + dFloat m_point[4]; + dFloat m_normal[4]; + dLong m_shapeId0; + dLong m_shapeId1; + dFloat m_penetration; + int m_unused[3]; + } NewtonUserContactPoint; + + + typedef struct NewtonImmediateModeConstraint + { + dFloat m_jacobian01[8][6]; + dFloat m_jacobian10[8][6]; + dFloat m_minFriction[8]; + dFloat m_maxFriction[8]; + dFloat m_jointAccel[8]; + dFloat m_jointStiffness[8]; + } NewtonConstraintDescriptor; + + + // data structure for interfacing with NewtonMesh + typedef struct NewtonMeshDoubleData + { + dFloat64* m_data; + int* m_indexList; + int m_strideInBytes; + } NewtonMeshDoubleData; + + typedef struct NewtonMeshFloatData + { + dFloat* m_data; + int* m_indexList; + int m_strideInBytes; + } NewtonMeshFloatData; + + typedef struct NewtonMeshVertexFormat + { + int m_faceCount; + int* m_faceIndexCount; + int* m_faceMaterial; + NewtonMeshDoubleData m_vertex; + NewtonMeshFloatData m_normal; + NewtonMeshFloatData m_binormal; + NewtonMeshFloatData m_uv0; + NewtonMeshFloatData m_uv1; + NewtonMeshFloatData m_vertexColor; + } NewtonMeshVertexFormat; + + // Newton callback functions + typedef void* (*NewtonAllocMemory) (int sizeInBytes); + typedef void (*NewtonFreeMemory) (void* const ptr, int sizeInBytes); + + typedef void (*NewtonWorldDestructorCallback) (const NewtonWorld* const world); + typedef void (*NewtonPostUpdateCallback) (const NewtonWorld* const world, dFloat timestep); + + typedef void(*NewtonCreateContactCallback) (const NewtonWorld* const newtonWorld, NewtonJoint* const contact); + typedef void(*NewtonDestroyContactCallback) (const NewtonWorld* const newtonWorld, NewtonJoint* const contact); + + typedef void (*NewtonWorldListenerDebugCallback) (const NewtonWorld* const world, void* const listener, void* const debugContext); + typedef void (*NewtonWorldListenerBodyDestroyCallback) (const NewtonWorld* const world, void* const listenerUserData, NewtonBody* const body); + typedef void (*NewtonWorldUpdateListenerCallback) (const NewtonWorld* const world, void* const listenerUserData, dFloat timestep); + typedef void (*NewtonWorldDestroyListenerCallback) (const NewtonWorld* const world, void* const listenerUserData); + + typedef dLong (*NewtonGetTimeInMicrosencondsCallback) (); + + typedef void (*NewtonSerializeCallback) (void* const serializeHandle, const void* const buffer, int size); + typedef void (*NewtonDeserializeCallback) (void* const serializeHandle, void* const buffer, int size); + + typedef void (*NewtonOnBodySerializationCallback) (NewtonBody* const body, void* const userData, NewtonSerializeCallback function, void* const serializeHandle); + typedef void (*NewtonOnBodyDeserializationCallback) (NewtonBody* const body, void* const userData, NewtonDeserializeCallback function, void* const serializeHandle); + + typedef void (*NewtonOnJointSerializationCallback) (const NewtonJoint* const joint, NewtonSerializeCallback function, void* const serializeHandle); + typedef void (*NewtonOnJointDeserializationCallback) (NewtonBody* const body0, NewtonBody* const body1, NewtonDeserializeCallback function, void* const serializeHandle); + + typedef void (*NewtonOnUserCollisionSerializationCallback) (void* const userData, NewtonSerializeCallback function, void* const serializeHandle); + + // user collision callbacks + typedef void (*NewtonUserMeshCollisionDestroyCallback) (void* const userData); + typedef dFloat (*NewtonUserMeshCollisionRayHitCallback) (NewtonUserMeshCollisionRayHitDesc* const lineDescData); + typedef void (*NewtonUserMeshCollisionGetCollisionInfo) (void* const userData, NewtonCollisionInfoRecord* const infoRecord); + typedef int (*NewtonUserMeshCollisionAABBTest) (void* const userData, const dFloat* const boxP0, const dFloat* const boxP1); + typedef int (*NewtonUserMeshCollisionGetFacesInAABB) (void* const userData, const dFloat* const p0, const dFloat* const p1, + const dFloat** const vertexArray, int* const vertexCount, int* const vertexStrideInBytes, + const int* const indexList, int maxIndexCount, const int* const userDataList); + typedef void (*NewtonUserMeshCollisionCollideCallback) (NewtonUserMeshCollisionCollideDesc* const collideDescData, const void* const continueCollisionHandle); + + typedef int (*NewtonTreeCollisionFaceCallback) (void* const context, const dFloat* const polygon, int strideInBytes, const int* const indexArray, int indexCount); + + typedef dFloat (*NewtonCollisionTreeRayCastCallback) (const NewtonBody* const body, const NewtonCollision* const treeCollision, dFloat intersection, dFloat* const normal, int faceId, void* const usedData); + typedef dFloat (*NewtonHeightFieldRayCastCallback) (const NewtonBody* const body, const NewtonCollision* const heightFieldCollision, dFloat intersection, int row, int col, dFloat* const normal, int faceId, void* const usedData); + + typedef void (*NewtonCollisionCopyConstructionCallback) (const NewtonWorld* const newtonWorld, NewtonCollision* const collision, const NewtonCollision* const sourceCollision); + typedef void (*NewtonCollisionDestructorCallback) (const NewtonWorld* const newtonWorld, const NewtonCollision* const collision); + + // collision tree call back (obsoleted no recommended) + typedef void (*NewtonTreeCollisionCallback) (const NewtonBody* const bodyWithTreeCollision, const NewtonBody* const body, int faceID, + int vertexCount, const dFloat* const vertex, int vertexStrideInBytes); + + typedef void (*NewtonBodyDestructor) (const NewtonBody* const body); + typedef void (*NewtonApplyForceAndTorque) (const NewtonBody* const body, dFloat timestep, int threadIndex); + typedef void (*NewtonSetTransform) (const NewtonBody* const body, const dFloat* const matrix, int threadIndex); + + typedef int (*NewtonIslandUpdate) (const NewtonWorld* const newtonWorld, const void* islandHandle, int bodyCount); + + typedef void (*NewtonFractureCompoundCollisionOnEmitCompoundFractured) (NewtonBody* const fracturedBody); + typedef void (*NewtonFractureCompoundCollisionOnEmitChunk) (NewtonBody* const chunkBody, NewtonFracturedCompoundMeshPart* const fracturexChunkMesh, const NewtonCollision* const fracturedCompountCollision); + typedef void (*NewtonFractureCompoundCollisionReconstructMainMeshCallBack) (NewtonBody* const body, NewtonFracturedCompoundMeshPart* const mainMesh, const NewtonCollision* const fracturedCompountCollision); + + typedef unsigned (*NewtonWorldRayPrefilterCallback)(const NewtonBody* const body, const NewtonCollision* const collision, void* const userData); + typedef dFloat (*NewtonWorldRayFilterCallback)(const NewtonBody* const body, const NewtonCollision* const shapeHit, const dFloat* const hitContact, const dFloat* const hitNormal, dLong collisionID, void* const userData, dFloat intersectParam); + + typedef int (*NewtonOnAABBOverlap) (const NewtonJoint* const contact, dFloat timestep, int threadIndex); + typedef void (*NewtonContactsProcess) (const NewtonJoint* const contact, dFloat timestep, int threadIndex); + typedef int (*NewtonOnCompoundSubCollisionAABBOverlap) (const NewtonJoint* const contact, dFloat timestep, const NewtonBody* const body0, const void* const collisionNode0, const NewtonBody* const body1, const void* const collisionNode1, int threadIndex); + typedef int (*NewtonOnContactGeneration) (const NewtonMaterial* const material, const NewtonBody* const body0, const NewtonCollision* const collision0, const NewtonBody* const body1, const NewtonCollision* const collision1, NewtonUserContactPoint* const contactBuffer, int maxCount, int threadIndex); + + typedef int (*NewtonBodyIterator) (const NewtonBody* const body, void* const userData); + typedef void (*NewtonJointIterator) (const NewtonJoint* const joint, void* const userData); + typedef void (*NewtonCollisionIterator) (void* const userData, int vertexCount, const dFloat* const faceArray, int faceId); + + typedef void (*NewtonBallCallback) (const NewtonJoint* const ball, dFloat timestep); + typedef unsigned (*NewtonHingeCallback) (const NewtonJoint* const hinge, NewtonHingeSliderUpdateDesc* const desc); + typedef unsigned (*NewtonSliderCallback) (const NewtonJoint* const slider, NewtonHingeSliderUpdateDesc* const desc); + typedef unsigned (*NewtonUniversalCallback) (const NewtonJoint* const universal, NewtonHingeSliderUpdateDesc* const desc); + typedef unsigned (*NewtonCorkscrewCallback) (const NewtonJoint* const corkscrew, NewtonHingeSliderUpdateDesc* const desc); + + typedef void (*NewtonUserBilateralCallback) (const NewtonJoint* const userJoint, dFloat timestep, int threadIndex); + typedef void (*NewtonUserBilateralGetInfoCallback) (const NewtonJoint* const userJoint, NewtonJointRecord* const info); + + typedef void (*NewtonConstraintDestructor) (const NewtonJoint* const me); + + typedef void (*NewtonJobTask) (NewtonWorld* const world, void* const userData, int threadIndex); + typedef int (*NewtonReportProgress) (dFloat normalizedProgressPercent, void* const userData); + + // ********************************************************************************************** + // + // world control functions + // + // ********************************************************************************************** + NEWTON_API int NewtonWorldGetVersion (); + NEWTON_API int NewtonWorldFloatSize (); + + NEWTON_API int NewtonGetMemoryUsed (); + NEWTON_API void NewtonSetMemorySystem (NewtonAllocMemory malloc, NewtonFreeMemory free); + + NEWTON_API NewtonWorld* NewtonCreate (); + NEWTON_API void NewtonDestroy (const NewtonWorld* const newtonWorld); + NEWTON_API void NewtonDestroyAllBodies (const NewtonWorld* const newtonWorld); + + NEWTON_API NewtonPostUpdateCallback NewtonGetPostUpdateCallback(const NewtonWorld* const newtonWorld); + NEWTON_API void NewtonSetPostUpdateCallback (const NewtonWorld* const newtonWorld, NewtonPostUpdateCallback callback); + + NEWTON_API void* NewtonAlloc (int sizeInBytes); + NEWTON_API void NewtonFree (void* const ptr); + + NEWTON_API void NewtonLoadPlugins(const NewtonWorld* const newtonWorld, const char* const plugInPath); + NEWTON_API void NewtonUnloadPlugins(const NewtonWorld* const newtonWorld); + NEWTON_API void* NewtonCurrentPlugin(const NewtonWorld* const newtonWorld); + NEWTON_API void* NewtonGetFirstPlugin(const NewtonWorld* const newtonWorld); + NEWTON_API void* NewtonGetPreferedPlugin(const NewtonWorld* const newtonWorld); + NEWTON_API void* NewtonGetNextPlugin(const NewtonWorld* const newtonWorld, const void* const plugin); + NEWTON_API const char* NewtonGetPluginString(const NewtonWorld* const newtonWorld, const void* const plugin); + NEWTON_API void NewtonSelectPlugin(const NewtonWorld* const newtonWorld, const void* const plugin); + + NEWTON_API dFloat NewtonGetContactMergeTolerance (const NewtonWorld* const newtonWorld); + NEWTON_API void NewtonSetContactMergeTolerance (const NewtonWorld* const newtonWorld, dFloat tolerance); + + NEWTON_API void NewtonInvalidateCache (const NewtonWorld* const newtonWorld); + + NEWTON_API void NewtonSetSolverIterations (const NewtonWorld* const newtonWorld, int model); + NEWTON_API int NewtonGetSolverIterations(const NewtonWorld* const newtonWorld); + + NEWTON_API void NewtonSetParallelSolverOnLargeIsland (const NewtonWorld* const newtonWorld, int mode); + NEWTON_API int NewtonGetParallelSolverOnLargeIsland (const NewtonWorld* const newtonWorld); + + NEWTON_API int NewtonGetBroadphaseAlgorithm (const NewtonWorld* const newtonWorld); + NEWTON_API void NewtonSelectBroadphaseAlgorithm (const NewtonWorld* const newtonWorld, int algorithmType); + NEWTON_API void NewtonResetBroadphase(const NewtonWorld* const newtonWorld); + + NEWTON_API void NewtonUpdate (const NewtonWorld* const newtonWorld, dFloat timestep); + NEWTON_API void NewtonUpdateAsync (const NewtonWorld* const newtonWorld, dFloat timestep); + NEWTON_API void NewtonWaitForUpdateToFinish (const NewtonWorld* const newtonWorld); + + NEWTON_API int NewtonGetNumberOfSubsteps (const NewtonWorld* const newtonWorld); + NEWTON_API void NewtonSetNumberOfSubsteps (const NewtonWorld* const newtonWorld, int subSteps); + NEWTON_API dFloat NewtonGetLastUpdateTime (const NewtonWorld* const newtonWorld); + + NEWTON_API void NewtonSerializeToFile (const NewtonWorld* const newtonWorld, const char* const filename, NewtonOnBodySerializationCallback bodyCallback, void* const bodyUserData); + NEWTON_API void NewtonDeserializeFromFile (const NewtonWorld* const newtonWorld, const char* const filename, NewtonOnBodyDeserializationCallback bodyCallback, void* const bodyUserData); + + NEWTON_API void NewtonSerializeScene(const NewtonWorld* const newtonWorld, NewtonOnBodySerializationCallback bodyCallback, void* const bodyUserData, + NewtonSerializeCallback serializeCallback, void* const serializeHandle); + NEWTON_API void NewtonDeserializeScene(const NewtonWorld* const newtonWorld, NewtonOnBodyDeserializationCallback bodyCallback, void* const bodyUserData, + NewtonDeserializeCallback serializeCallback, void* const serializeHandle); + + NEWTON_API NewtonBody* NewtonFindSerializedBody(const NewtonWorld* const newtonWorld, int bodySerializedID); + NEWTON_API void NewtonSetJointSerializationCallbacks (const NewtonWorld* const newtonWorld, NewtonOnJointSerializationCallback serializeJoint, NewtonOnJointDeserializationCallback deserializeJoint); + NEWTON_API void NewtonGetJointSerializationCallbacks (const NewtonWorld* const newtonWorld, NewtonOnJointSerializationCallback* const serializeJoint, NewtonOnJointDeserializationCallback* const deserializeJoint); + + // multi threading interface + NEWTON_API void NewtonWorldCriticalSectionLock (const NewtonWorld* const newtonWorld, int threadIndex); + NEWTON_API void NewtonWorldCriticalSectionUnlock (const NewtonWorld* const newtonWorld); + NEWTON_API void NewtonSetThreadsCount (const NewtonWorld* const newtonWorld, int threads); + NEWTON_API int NewtonGetThreadsCount(const NewtonWorld* const newtonWorld); + NEWTON_API int NewtonGetMaxThreadsCount(const NewtonWorld* const newtonWorld); + NEWTON_API void NewtonDispachThreadJob(const NewtonWorld* const newtonWorld, NewtonJobTask task, void* const usedData, const char* const functionName); + NEWTON_API void NewtonSyncThreadJobs(const NewtonWorld* const newtonWorld); + + // atomic operations + NEWTON_API int NewtonAtomicAdd (int* const ptr, int value); + NEWTON_API int NewtonAtomicSwap (int* const ptr, int value); + NEWTON_API void NewtonYield (); + + NEWTON_API void NewtonSetIslandUpdateEvent (const NewtonWorld* const newtonWorld, NewtonIslandUpdate islandUpdate); + NEWTON_API void NewtonWorldForEachJointDo (const NewtonWorld* const newtonWorld, NewtonJointIterator callback, void* const userData); + NEWTON_API void NewtonWorldForEachBodyInAABBDo (const NewtonWorld* const newtonWorld, const dFloat* const p0, const dFloat* const p1, NewtonBodyIterator callback, void* const userData); + + NEWTON_API void NewtonWorldSetUserData (const NewtonWorld* const newtonWorld, void* const userData); + NEWTON_API void* NewtonWorldGetUserData (const NewtonWorld* const newtonWorld); + + NEWTON_API void* NewtonWorldAddListener (const NewtonWorld* const newtonWorld, const char* const nameId, void* const listenerUserData); + NEWTON_API void* NewtonWorldGetListener (const NewtonWorld* const newtonWorld, const char* const nameId); + + NEWTON_API void NewtonWorldListenerSetDebugCallback (const NewtonWorld* const newtonWorld, void* const listener, NewtonWorldListenerDebugCallback callback); + NEWTON_API void NewtonWorldListenerSetPostStepCallback (const NewtonWorld* const newtonWorld, void* const listener, NewtonWorldUpdateListenerCallback callback); + NEWTON_API void NewtonWorldListenerSetPreUpdateCallback (const NewtonWorld* const newtonWorld, void* const listener, NewtonWorldUpdateListenerCallback callback); + NEWTON_API void NewtonWorldListenerSetPostUpdateCallback (const NewtonWorld* const newtonWorld, void* const listener, NewtonWorldUpdateListenerCallback callback); + NEWTON_API void NewtonWorldListenerSetDestructorCallback (const NewtonWorld* const newtonWorld, void* const listener, NewtonWorldDestroyListenerCallback callback); + NEWTON_API void NewtonWorldListenerSetBodyDestroyCallback(const NewtonWorld* const newtonWorld, void* const listener, NewtonWorldListenerBodyDestroyCallback callback); + NEWTON_API void NewtonWorldListenerDebug(const NewtonWorld* const newtonWorld, void* const context); + NEWTON_API void* NewtonWorldGetListenerUserData(const NewtonWorld* const newtonWorld, void* const listener); + NEWTON_API NewtonWorldListenerBodyDestroyCallback NewtonWorldListenerGetBodyDestroyCallback (const NewtonWorld* const newtonWorld, void* const listener); + + NEWTON_API void NewtonWorldSetDestructorCallback (const NewtonWorld* const newtonWorld, NewtonWorldDestructorCallback destructor); + NEWTON_API NewtonWorldDestructorCallback NewtonWorldGetDestructorCallback (const NewtonWorld* const newtonWorld); + NEWTON_API void NewtonWorldSetCollisionConstructorDestructorCallback (const NewtonWorld* const newtonWorld, NewtonCollisionCopyConstructionCallback constructor, NewtonCollisionDestructorCallback destructor); + + NEWTON_API void NewtonWorldSetCreateDestroyContactCallback(const NewtonWorld* const newtonWorld, NewtonCreateContactCallback createContact, NewtonDestroyContactCallback destroyContact); + + NEWTON_API void NewtonWorldRayCast (const NewtonWorld* const newtonWorld, const dFloat* const p0, const dFloat* const p1, NewtonWorldRayFilterCallback filter, void* const userData, NewtonWorldRayPrefilterCallback prefilter, int threadIndex); + NEWTON_API int NewtonWorldConvexCast (const NewtonWorld* const newtonWorld, const dFloat* const matrix, const dFloat* const target, const NewtonCollision* const shape, dFloat* const param, void* const userData, NewtonWorldRayPrefilterCallback prefilter, NewtonWorldConvexCastReturnInfo* const info, int maxContactsCount, int threadIndex); + NEWTON_API int NewtonWorldCollide (const NewtonWorld* const newtonWorld, const dFloat* const matrix, const NewtonCollision* const shape, void* const userData, NewtonWorldRayPrefilterCallback prefilter, NewtonWorldConvexCastReturnInfo* const info, int maxContactsCount, int threadIndex); + + // world utility functions + NEWTON_API int NewtonWorldGetBodyCount(const NewtonWorld* const newtonWorld); + NEWTON_API int NewtonWorldGetConstraintCount(const NewtonWorld* const newtonWorld); + + NEWTON_API NewtonJoint* NewtonWorldFindJoint(const NewtonBody* const body0, const NewtonBody* const body1); + + // ********************************************************************************************** + // + // Simulation islands + // + // ********************************************************************************************** + NEWTON_API NewtonBody* NewtonIslandGetBody (const void* const island, int bodyIndex); + NEWTON_API void NewtonIslandGetBodyAABB (const void* const island, int bodyIndex, dFloat* const p0, dFloat* const p1); + + // ********************************************************************************************** + // + // Physics Material Section + // + // ********************************************************************************************** + NEWTON_API int NewtonMaterialCreateGroupID(const NewtonWorld* const newtonWorld); + NEWTON_API int NewtonMaterialGetDefaultGroupID(const NewtonWorld* const newtonWorld); + NEWTON_API void NewtonMaterialDestroyAllGroupID(const NewtonWorld* const newtonWorld); + + // material definitions that can not be overwritten in function callback + NEWTON_API void* NewtonMaterialGetUserData (const NewtonWorld* const newtonWorld, int id0, int id1); + NEWTON_API void NewtonMaterialSetSurfaceThickness (const NewtonWorld* const newtonWorld, int id0, int id1, dFloat thickness); + +// deprecated, not longer continue collision is set on the material +// NEWTON_API void NewtonMaterialSetContinuousCollisionMode (const NewtonWorld* const newtonWorld, int id0, int id1, int state); + + NEWTON_API void NewtonMaterialSetCallbackUserData (const NewtonWorld* const newtonWorld, int id0, int id1, void* const userData); + NEWTON_API void NewtonMaterialSetContactGenerationCallback (const NewtonWorld* const newtonWorld, int id0, int id1, NewtonOnContactGeneration contactGeneration); + NEWTON_API void NewtonMaterialSetCompoundCollisionCallback(const NewtonWorld* const newtonWorld, int id0, int id1, NewtonOnCompoundSubCollisionAABBOverlap compoundAabbOverlap); + NEWTON_API void NewtonMaterialSetCollisionCallback (const NewtonWorld* const newtonWorld, int id0, int id1, NewtonOnAABBOverlap aabbOverlap, NewtonContactsProcess process); + + NEWTON_API void NewtonMaterialSetDefaultSoftness (const NewtonWorld* const newtonWorld, int id0, int id1, dFloat value); + NEWTON_API void NewtonMaterialSetDefaultElasticity (const NewtonWorld* const newtonWorld, int id0, int id1, dFloat elasticCoef); + NEWTON_API void NewtonMaterialSetDefaultCollidable (const NewtonWorld* const newtonWorld, int id0, int id1, int state); + NEWTON_API void NewtonMaterialSetDefaultFriction (const NewtonWorld* const newtonWorld, int id0, int id1, dFloat staticFriction, dFloat kineticFriction); + + NEWTON_API void NewtonMaterialJointResetIntraJointCollision (const NewtonWorld* const newtonWorld, int id0, int id1); + NEWTON_API void NewtonMaterialJointResetSelftJointCollision (const NewtonWorld* const newtonWorld, int id0, int id1); + + NEWTON_API NewtonMaterial* NewtonWorldGetFirstMaterial (const NewtonWorld* const newtonWorld); + NEWTON_API NewtonMaterial* NewtonWorldGetNextMaterial (const NewtonWorld* const newtonWorld, const NewtonMaterial* const material); + + NEWTON_API NewtonBody* NewtonWorldGetFirstBody (const NewtonWorld* const newtonWorld); + NEWTON_API NewtonBody* NewtonWorldGetNextBody (const NewtonWorld* const newtonWorld, const NewtonBody* const curBody); + + + // ********************************************************************************************** + // + // Physics Contact control functions + // + // ********************************************************************************************** + NEWTON_API void *NewtonMaterialGetMaterialPairUserData (const NewtonMaterial* const material); + NEWTON_API unsigned NewtonMaterialGetContactFaceAttribute (const NewtonMaterial* const material); + NEWTON_API NewtonCollision* NewtonMaterialGetBodyCollidingShape (const NewtonMaterial* const material, const NewtonBody* const body); + NEWTON_API dFloat NewtonMaterialGetContactNormalSpeed (const NewtonMaterial* const material); + NEWTON_API void NewtonMaterialGetContactForce (const NewtonMaterial* const material, const NewtonBody* const body, dFloat* const force); + NEWTON_API void NewtonMaterialGetContactPositionAndNormal (const NewtonMaterial* const material, const NewtonBody* const body, dFloat* const posit, dFloat* const normal); + NEWTON_API void NewtonMaterialGetContactTangentDirections (const NewtonMaterial* const material, const NewtonBody* const body, dFloat* const dir0, dFloat* const dir1); + NEWTON_API dFloat NewtonMaterialGetContactTangentSpeed (const NewtonMaterial* const material, int index); + NEWTON_API dFloat NewtonMaterialGetContactMaxNormalImpact (const NewtonMaterial* const material); + NEWTON_API dFloat NewtonMaterialGetContactMaxTangentImpact (const NewtonMaterial* const material, int index); + NEWTON_API dFloat NewtonMaterialGetContactPenetration (const NewtonMaterial* const material); + NEWTON_API void NewtonMaterialSetAsSoftContact (const NewtonMaterial* const material, dFloat relaxation); + + NEWTON_API void NewtonMaterialSetContactSoftness (const NewtonMaterial* const material, dFloat softness); + NEWTON_API void NewtonMaterialSetContactThickness (const NewtonMaterial* const material, dFloat thickness); + NEWTON_API void NewtonMaterialSetContactElasticity (const NewtonMaterial* const material, dFloat restitution); + NEWTON_API void NewtonMaterialSetContactFrictionState (const NewtonMaterial* const material, int state, int index); + NEWTON_API void NewtonMaterialSetContactFrictionCoef (const NewtonMaterial* const material, dFloat staticFrictionCoef, dFloat kineticFrictionCoef, int index); + + NEWTON_API void NewtonMaterialSetContactNormalAcceleration (const NewtonMaterial* const material, dFloat accel); + NEWTON_API void NewtonMaterialSetContactNormalDirection (const NewtonMaterial* const material, const dFloat* const directionVector); + NEWTON_API void NewtonMaterialSetContactPosition (const NewtonMaterial* const material, const dFloat* const position); + + NEWTON_API void NewtonMaterialSetContactTangentFriction (const NewtonMaterial* const material, dFloat friction, int index); + NEWTON_API void NewtonMaterialSetContactTangentAcceleration (const NewtonMaterial* const material, dFloat accel, int index); + NEWTON_API void NewtonMaterialContactRotateTangentDirections (const NewtonMaterial* const material, const dFloat* const directionVector); + + //NEWTON_API dFloat NewtonMaterialGetContactPruningTolerance (const NewtonBody* const body0, const NewtonBody* const body1); + //NEWTON_API void NewtonMaterialSetContactPruningTolerance (const NewtonBody* const body0, const NewtonBody* const body1, dFloat tolerance); + NEWTON_API dFloat NewtonMaterialGetContactPruningTolerance(const NewtonJoint* const contactJoint); + NEWTON_API void NewtonMaterialSetContactPruningTolerance(const NewtonJoint* const contactJoint, dFloat tolerance); + + // ********************************************************************************************** + // + // convex collision primitives creation functions + // + // ********************************************************************************************** + NEWTON_API NewtonCollision* NewtonCreateNull (const NewtonWorld* const newtonWorld); + NEWTON_API NewtonCollision* NewtonCreateSphere (const NewtonWorld* const newtonWorld, dFloat radius, int shapeID, const dFloat* const offsetMatrix); + NEWTON_API NewtonCollision* NewtonCreateBox (const NewtonWorld* const newtonWorld, dFloat dx, dFloat dy, dFloat dz, int shapeID, const dFloat* const offsetMatrix); + NEWTON_API NewtonCollision* NewtonCreateCone (const NewtonWorld* const newtonWorld, dFloat radius, dFloat height, int shapeID, const dFloat* const offsetMatrix); + NEWTON_API NewtonCollision* NewtonCreateCapsule (const NewtonWorld* const newtonWorld, dFloat radius0, dFloat radius1, dFloat height, int shapeID, const dFloat* const offsetMatrix); + NEWTON_API NewtonCollision* NewtonCreateCylinder (const NewtonWorld* const newtonWorld, dFloat radio0, dFloat radio1, dFloat height, int shapeID, const dFloat* const offsetMatrix); + NEWTON_API NewtonCollision* NewtonCreateChamferCylinder (const NewtonWorld* const newtonWorld, dFloat radius, dFloat height, int shapeID, const dFloat* const offsetMatrix); + NEWTON_API NewtonCollision* NewtonCreateConvexHull (const NewtonWorld* const newtonWorld, int count, const dFloat* const vertexCloud, int strideInBytes, dFloat tolerance, int shapeID, const dFloat* const offsetMatrix); + NEWTON_API NewtonCollision* NewtonCreateConvexHullFromMesh (const NewtonWorld* const newtonWorld, const NewtonMesh* const mesh, dFloat tolerance, int shapeID); + + NEWTON_API int NewtonCollisionGetMode(const NewtonCollision* const convexCollision); + NEWTON_API void NewtonCollisionSetMode (const NewtonCollision* const convexCollision, int mode); + + NEWTON_API int NewtonConvexHullGetFaceIndices (const NewtonCollision* const convexHullCollision, int face, int* const faceIndices); + NEWTON_API int NewtonConvexHullGetVertexData (const NewtonCollision* const convexHullCollision, dFloat** const vertexData, int* strideInBytes); + + NEWTON_API dFloat NewtonConvexCollisionCalculateVolume (const NewtonCollision* const convexCollision); + NEWTON_API void NewtonConvexCollisionCalculateInertialMatrix (const NewtonCollision* convexCollision, dFloat* const inertia, dFloat* const origin); + NEWTON_API dFloat NewtonConvexCollisionCalculateBuoyancyVolume (const NewtonCollision* const convexCollision, const dFloat* const matrix, const dFloat* const fluidPlane, dFloat* const centerOfBuoyancy); + + NEWTON_API const void* NewtonCollisionDataPointer (const NewtonCollision* const convexCollision); + + // ********************************************************************************************** + // + // compound collision primitives creation functions + // + // ********************************************************************************************** + NEWTON_API NewtonCollision* NewtonCreateCompoundCollision (const NewtonWorld* const newtonWorld, int shapeID); + NEWTON_API NewtonCollision* NewtonCreateCompoundCollisionFromMesh (const NewtonWorld* const newtonWorld, const NewtonMesh* const mesh, dFloat hullTolerance, int shapeID, int subShapeID); + + NEWTON_API void NewtonCompoundCollisionBeginAddRemove (NewtonCollision* const compoundCollision); + NEWTON_API void* NewtonCompoundCollisionAddSubCollision (NewtonCollision* const compoundCollision, const NewtonCollision* const convexCollision); + NEWTON_API void NewtonCompoundCollisionRemoveSubCollision (NewtonCollision* const compoundCollision, const void* const collisionNode); + NEWTON_API void NewtonCompoundCollisionRemoveSubCollisionByIndex (NewtonCollision* const compoundCollision, int nodeIndex); + NEWTON_API void NewtonCompoundCollisionSetSubCollisionMatrix (NewtonCollision* const compoundCollision, const void* const collisionNode, const dFloat* const matrix); + NEWTON_API void NewtonCompoundCollisionEndAddRemove (NewtonCollision* const compoundCollision); + + NEWTON_API void* NewtonCompoundCollisionGetFirstNode (NewtonCollision* const compoundCollision); + NEWTON_API void* NewtonCompoundCollisionGetNextNode (NewtonCollision* const compoundCollision, const void* const collisionNode); + + NEWTON_API void* NewtonCompoundCollisionGetNodeByIndex (NewtonCollision* const compoundCollision, int index); + NEWTON_API int NewtonCompoundCollisionGetNodeIndex (NewtonCollision* const compoundCollision, const void* const collisionNode); + NEWTON_API NewtonCollision* NewtonCompoundCollisionGetCollisionFromNode (NewtonCollision* const compoundCollision, const void* const collisionNode); + + + // ********************************************************************************************** + // + // Fractured compound collision primitives interface + // + // ********************************************************************************************** + NEWTON_API NewtonCollision* NewtonCreateFracturedCompoundCollision (const NewtonWorld* const newtonWorld, const NewtonMesh* const solidMesh, int shapeID, int fracturePhysicsMaterialID, int pointcloudCount, const dFloat* const vertexCloud, int strideInBytes, int materialID, const dFloat* const textureMatrix, + NewtonFractureCompoundCollisionReconstructMainMeshCallBack regenerateMainMeshCallback, + NewtonFractureCompoundCollisionOnEmitCompoundFractured emitFracturedCompound, NewtonFractureCompoundCollisionOnEmitChunk emitFracfuredChunk); + NEWTON_API NewtonCollision* NewtonFracturedCompoundPlaneClip (const NewtonCollision* const fracturedCompound, const dFloat* const plane); + + NEWTON_API void NewtonFracturedCompoundSetCallbacks (const NewtonCollision* const fracturedCompound, NewtonFractureCompoundCollisionReconstructMainMeshCallBack regenerateMainMeshCallback, + NewtonFractureCompoundCollisionOnEmitCompoundFractured emitFracturedCompound, NewtonFractureCompoundCollisionOnEmitChunk emitFracfuredChunk); + + + NEWTON_API int NewtonFracturedCompoundIsNodeFreeToDetach (const NewtonCollision* const fracturedCompound, void* const collisionNode); + NEWTON_API int NewtonFracturedCompoundNeighborNodeList (const NewtonCollision* const fracturedCompound, void* const collisionNode, void** const list, int maxCount); + + + NEWTON_API NewtonFracturedCompoundMeshPart* NewtonFracturedCompoundGetMainMesh (const NewtonCollision* const fracturedCompound); + NEWTON_API NewtonFracturedCompoundMeshPart* NewtonFracturedCompoundGetFirstSubMesh(const NewtonCollision* const fracturedCompound); + NEWTON_API NewtonFracturedCompoundMeshPart* NewtonFracturedCompoundGetNextSubMesh(const NewtonCollision* const fracturedCompound, NewtonFracturedCompoundMeshPart* const subMesh); + + NEWTON_API int NewtonFracturedCompoundCollisionGetVertexCount (const NewtonCollision* const fracturedCompound, const NewtonFracturedCompoundMeshPart* const meshOwner); + NEWTON_API const dFloat* NewtonFracturedCompoundCollisionGetVertexPositions (const NewtonCollision* const fracturedCompound, const NewtonFracturedCompoundMeshPart* const meshOwner); + NEWTON_API const dFloat* NewtonFracturedCompoundCollisionGetVertexNormals (const NewtonCollision* const fracturedCompound, const NewtonFracturedCompoundMeshPart* const meshOwner); + NEWTON_API const dFloat* NewtonFracturedCompoundCollisionGetVertexUVs (const NewtonCollision* const fracturedCompound, const NewtonFracturedCompoundMeshPart* const meshOwner); + NEWTON_API int NewtonFracturedCompoundMeshPartGetIndexStream (const NewtonCollision* const fracturedCompound, const NewtonFracturedCompoundMeshPart* const meshOwner, const void* const segment, int* const index); + + NEWTON_API void* NewtonFracturedCompoundMeshPartGetFirstSegment (const NewtonFracturedCompoundMeshPart* const fractureCompoundMeshPart); + NEWTON_API void* NewtonFracturedCompoundMeshPartGetNextSegment (const void* const fractureCompoundMeshSegment); + NEWTON_API int NewtonFracturedCompoundMeshPartGetMaterial (const void* const fractureCompoundMeshSegment); + NEWTON_API int NewtonFracturedCompoundMeshPartGetIndexCount (const void* const fractureCompoundMeshSegment); + + + // ********************************************************************************************** + // + // scene collision are static compound collision that can take polygonal static collisions + // + // ********************************************************************************************** + NEWTON_API NewtonCollision* NewtonCreateSceneCollision (const NewtonWorld* const newtonWorld, int shapeID); + + NEWTON_API void NewtonSceneCollisionBeginAddRemove (NewtonCollision* const sceneCollision); + NEWTON_API void* NewtonSceneCollisionAddSubCollision (NewtonCollision* const sceneCollision, const NewtonCollision* const collision); + NEWTON_API void NewtonSceneCollisionRemoveSubCollision (NewtonCollision* const compoundCollision, const void* const collisionNode); + NEWTON_API void NewtonSceneCollisionRemoveSubCollisionByIndex (NewtonCollision* const sceneCollision, int nodeIndex); + NEWTON_API void NewtonSceneCollisionSetSubCollisionMatrix (NewtonCollision* const sceneCollision, const void* const collisionNode, const dFloat* const matrix); + NEWTON_API void NewtonSceneCollisionEndAddRemove (NewtonCollision* const sceneCollision); + + NEWTON_API void* NewtonSceneCollisionGetFirstNode (NewtonCollision* const sceneCollision); + NEWTON_API void* NewtonSceneCollisionGetNextNode (NewtonCollision* const sceneCollision, const void* const collisionNode); + + NEWTON_API void* NewtonSceneCollisionGetNodeByIndex (NewtonCollision* const sceneCollision, int index); + NEWTON_API int NewtonSceneCollisionGetNodeIndex (NewtonCollision* const sceneCollision, const void* const collisionNode); + NEWTON_API NewtonCollision* NewtonSceneCollisionGetCollisionFromNode (NewtonCollision* const sceneCollision, const void* const collisionNode); + + + // *********************************************************************************************************** + // + // User Static mesh collision interface + // + // *********************************************************************************************************** + NEWTON_API NewtonCollision* NewtonCreateUserMeshCollision (const NewtonWorld* const newtonWorld, const dFloat* const minBox, + const dFloat* const maxBox, void* const userData, NewtonUserMeshCollisionCollideCallback collideCallback, + NewtonUserMeshCollisionRayHitCallback rayHitCallback, NewtonUserMeshCollisionDestroyCallback destroyCallback, + NewtonUserMeshCollisionGetCollisionInfo getInfoCallback, NewtonUserMeshCollisionAABBTest getLocalAABBCallback, + NewtonUserMeshCollisionGetFacesInAABB facesInAABBCallback, NewtonOnUserCollisionSerializationCallback serializeCallback, int shapeID); + + NEWTON_API int NewtonUserMeshCollisionContinuousOverlapTest (const NewtonUserMeshCollisionCollideDesc* const collideDescData, const void* const continueCollisionHandle, const dFloat* const minAabb, const dFloat* const maxAabb); + + // *********************************************************************************************************** + // + // Collision serialization functions + // + // *********************************************************************************************************** + NEWTON_API NewtonCollision* NewtonCreateCollisionFromSerialization (const NewtonWorld* const newtonWorld, NewtonDeserializeCallback deserializeFunction, void* const serializeHandle); + NEWTON_API void NewtonCollisionSerialize (const NewtonWorld* const newtonWorld, const NewtonCollision* const collision, NewtonSerializeCallback serializeFunction, void* const serializeHandle); + NEWTON_API void NewtonCollisionGetInfo (const NewtonCollision* const collision, NewtonCollisionInfoRecord* const collisionInfo); + + // ********************************************************************************************** + // + // Static collision shapes functions + // + // ********************************************************************************************** + NEWTON_API NewtonCollision* NewtonCreateHeightFieldCollision (const NewtonWorld* const newtonWorld, int width, int height, int gridsDiagonals, int elevationdatType, const void* const elevationMap, const char* const attributeMap, dFloat verticalScale, dFloat horizontalScale_x, dFloat horizontalScale_z, int shapeID); + NEWTON_API void NewtonHeightFieldSetUserRayCastCallback (const NewtonCollision* const heightfieldCollision, NewtonHeightFieldRayCastCallback rayHitCallback); + + NEWTON_API NewtonCollision* NewtonCreateTreeCollision (const NewtonWorld* const newtonWorld, int shapeID); + NEWTON_API NewtonCollision* NewtonCreateTreeCollisionFromMesh (const NewtonWorld* const newtonWorld, const NewtonMesh* const mesh, int shapeID); + NEWTON_API void NewtonTreeCollisionSetUserRayCastCallback (const NewtonCollision* const treeCollision, NewtonCollisionTreeRayCastCallback rayHitCallback); + + NEWTON_API void NewtonTreeCollisionBeginBuild (const NewtonCollision* const treeCollision); + NEWTON_API void NewtonTreeCollisionAddFace (const NewtonCollision* const treeCollision, int vertexCount, const dFloat* const vertexPtr, int strideInBytes, int faceAttribute); + NEWTON_API void NewtonTreeCollisionEndBuild (const NewtonCollision* const treeCollision, int optimize); + + NEWTON_API int NewtonTreeCollisionGetFaceAttribute (const NewtonCollision* const treeCollision, const int* const faceIndexArray, int indexCount); + NEWTON_API void NewtonTreeCollisionSetFaceAttribute (const NewtonCollision* const treeCollision, const int* const faceIndexArray, int indexCount, int attribute); + + NEWTON_API void NewtonTreeCollisionForEachFace (const NewtonCollision* const treeCollision, NewtonTreeCollisionFaceCallback forEachFaceCallback, void* const context); + + NEWTON_API int NewtonTreeCollisionGetVertexListTriangleListInAABB (const NewtonCollision* const treeCollision, const dFloat* const p0, const dFloat* const p1, const dFloat** const vertexArray, int* const vertexCount, int* const vertexStrideInBytes, const int* const indexList, int maxIndexCount, const int* const faceAttribute); + + NEWTON_API void NewtonStaticCollisionSetDebugCallback (const NewtonCollision* const staticCollision, NewtonTreeCollisionCallback userCallback); + + // ********************************************************************************************** + // + // General purpose collision library functions + // + // ********************************************************************************************** + NEWTON_API NewtonCollision* NewtonCollisionCreateInstance (const NewtonCollision* const collision); + NEWTON_API int NewtonCollisionGetType (const NewtonCollision* const collision); + NEWTON_API int NewtonCollisionIsConvexShape (const NewtonCollision* const collision); + NEWTON_API int NewtonCollisionIsStaticShape (const NewtonCollision* const collision); + + // for the end user + NEWTON_API void NewtonCollisionSetUserData (const NewtonCollision* const collision, void* const userData); + NEWTON_API void* NewtonCollisionGetUserData (const NewtonCollision* const collision); + + NEWTON_API void NewtonCollisionSetUserID (const NewtonCollision* const collision, dLong id); + NEWTON_API dLong NewtonCollisionGetUserID (const NewtonCollision* const collision); + + NEWTON_API void NewtonCollisionGetMaterial (const NewtonCollision* const collision, NewtonCollisionMaterial* const userData); + NEWTON_API void NewtonCollisionSetMaterial (const NewtonCollision* const collision, const NewtonCollisionMaterial* const userData); + + NEWTON_API void* NewtonCollisionGetSubCollisionHandle (const NewtonCollision* const collision); + NEWTON_API NewtonCollision* NewtonCollisionGetParentInstance (const NewtonCollision* const collision); + + NEWTON_API void NewtonCollisionSetMatrix (const NewtonCollision* const collision, const dFloat* const matrix); + NEWTON_API void NewtonCollisionGetMatrix (const NewtonCollision* const collision, dFloat* const matrix); + + NEWTON_API void NewtonCollisionSetScale (const NewtonCollision* const collision, dFloat scaleX, dFloat scaleY, dFloat scaleZ); + NEWTON_API void NewtonCollisionGetScale (const NewtonCollision* const collision, dFloat* const scaleX, dFloat* const scaleY, dFloat* const scaleZ); + NEWTON_API void NewtonDestroyCollision (const NewtonCollision* const collision); + + NEWTON_API dFloat NewtonCollisionGetSkinThickness (const NewtonCollision* const collision); + NEWTON_API void NewtonCollisionSetSkinThickness(const NewtonCollision* const collision, dFloat thickness); + + NEWTON_API int NewtonCollisionIntersectionTest (const NewtonWorld* const newtonWorld, + const NewtonCollision* const collisionA, const dFloat* const matrixA, + const NewtonCollision* const collisionB, const dFloat* const matrixB, int threadIndex); + + NEWTON_API int NewtonCollisionPointDistance (const NewtonWorld* const newtonWorld, const dFloat* const point, + const NewtonCollision* const collision, const dFloat* const matrix, dFloat* const contact, dFloat* const normal, int threadIndex); + + NEWTON_API int NewtonCollisionClosestPoint (const NewtonWorld* const newtonWorld, + const NewtonCollision* const collisionA, const dFloat* const matrixA, + const NewtonCollision* const collisionB, const dFloat* const matrixB, + dFloat* const contactA, dFloat* const contactB, dFloat* const normalAB, int threadIndex); + + NEWTON_API int NewtonCollisionCollide (const NewtonWorld* const newtonWorld, int maxSize, + const NewtonCollision* const collisionA, const dFloat* const matrixA, + const NewtonCollision* const collisionB, const dFloat* const matrixB, + dFloat* const contacts, dFloat* const normals, dFloat* const penetration, + dLong* const attributeA, dLong* const attributeB, int threadIndex); + + NEWTON_API int NewtonCollisionCollideContinue (const NewtonWorld* const newtonWorld, int maxSize, dFloat timestep, + const NewtonCollision* const collisionA, const dFloat* const matrixA, const dFloat* const velocA, const dFloat* omegaA, + const NewtonCollision* const collisionB, const dFloat* const matrixB, const dFloat* const velocB, const dFloat* const omegaB, + dFloat* const timeOfImpact, dFloat* const contacts, dFloat* const normals, dFloat* const penetration, + dLong* const attributeA, dLong* const attributeB, int threadIndex); + + NEWTON_API void NewtonCollisionSupportVertex (const NewtonCollision* const collision, const dFloat* const dir, dFloat* const vertex); + NEWTON_API dFloat NewtonCollisionRayCast (const NewtonCollision* const collision, const dFloat* const p0, const dFloat* const p1, dFloat* const normal, dLong* const attribute); + NEWTON_API void NewtonCollisionCalculateAABB (const NewtonCollision* const collision, const dFloat* const matrix, dFloat* const p0, dFloat* const p1); + NEWTON_API void NewtonCollisionForEachPolygonDo (const NewtonCollision* const collision, const dFloat* const matrix, NewtonCollisionIterator callback, void* const userData); + + // ********************************************************************************************** + // + // collision aggregates, are a collision node on eh broad phase the serve as the root nod for a collection of rigid bodies + // that shared the property of being in close proximity all the time, they are similar to compound collision by the group bodies instead of collision instances + // These are good for speeding calculation calculation of rag doll, Vehicles or contractions of rigid bodied lined by joints. + // also for example if you know that many the life time of a group of bodies like the object on a house of a building will be localize to the confide of the building + // then warping the bodies under an aggregate will reduce collision calculation of almost an order of magnitude. + // + // ********************************************************************************************** + NEWTON_API void* NewtonCollisionAggregateCreate (NewtonWorld* const world); + NEWTON_API void NewtonCollisionAggregateDestroy (void* const aggregate); + NEWTON_API void NewtonCollisionAggregateAddBody (void* const aggregate, const NewtonBody* const body); + NEWTON_API void NewtonCollisionAggregateRemoveBody (void* const aggregate, const NewtonBody* const body); + + NEWTON_API int NewtonCollisionAggregateGetSelfCollision (void* const aggregate); + NEWTON_API void NewtonCollisionAggregateSetSelfCollision (void* const aggregate, int state); + + // ********************************************************************************************** + // + // transforms utility functions + // + // ********************************************************************************************** + NEWTON_API void NewtonSetEulerAngle (const dFloat* const eulersAngles, dFloat* const matrix); + NEWTON_API void NewtonGetEulerAngle (const dFloat* const matrix, dFloat* const eulersAngles0, dFloat* const eulersAngles1); + NEWTON_API dFloat NewtonCalculateSpringDamperAcceleration (dFloat dt, dFloat ks, dFloat x, dFloat kd, dFloat s); + + // ********************************************************************************************** + // + // body manipulation functions + // + // ********************************************************************************************** + NEWTON_API NewtonBody* NewtonCreateDynamicBody (const NewtonWorld* const newtonWorld, const NewtonCollision* const collision, const dFloat* const matrix); + NEWTON_API NewtonBody* NewtonCreateKinematicBody (const NewtonWorld* const newtonWorld, const NewtonCollision* const collision, const dFloat* const matrix); + NEWTON_API NewtonBody* NewtonCreateAsymetricDynamicBody(const NewtonWorld* const newtonWorld, const NewtonCollision* const collision, const dFloat* const matrix); + + NEWTON_API void NewtonDestroyBody(const NewtonBody* const body); + + NEWTON_API int NewtonBodyGetSimulationState(const NewtonBody* const body); + NEWTON_API void NewtonBodySetSimulationState(const NewtonBody* const bodyPtr, const int state); + + NEWTON_API int NewtonBodyGetType (const NewtonBody* const body); + NEWTON_API int NewtonBodyGetCollidable (const NewtonBody* const body); + NEWTON_API void NewtonBodySetCollidable (const NewtonBody* const body, int collidableState); + + NEWTON_API void NewtonBodyAddForce (const NewtonBody* const body, const dFloat* const force); + NEWTON_API void NewtonBodyAddTorque (const NewtonBody* const body, const dFloat* const torque); + + NEWTON_API void NewtonBodySetCentreOfMass (const NewtonBody* const body, const dFloat* const com); + NEWTON_API void NewtonBodySetMassMatrix (const NewtonBody* const body, dFloat mass, dFloat Ixx, dFloat Iyy, dFloat Izz); + NEWTON_API void NewtonBodySetFullMassMatrix (const NewtonBody* const body, dFloat mass, const dFloat* const inertiaMatrix); + + NEWTON_API void NewtonBodySetMassProperties (const NewtonBody* const body, dFloat mass, const NewtonCollision* const collision); + NEWTON_API void NewtonBodySetMatrix (const NewtonBody* const body, const dFloat* const matrix); + NEWTON_API void NewtonBodySetMatrixNoSleep (const NewtonBody* const body, const dFloat* const matrix); + NEWTON_API void NewtonBodySetMatrixRecursive (const NewtonBody* const body, const dFloat* const matrix); + + NEWTON_API void NewtonBodySetMaterialGroupID (const NewtonBody* const body, int id); + NEWTON_API void NewtonBodySetContinuousCollisionMode (const NewtonBody* const body, unsigned state); + NEWTON_API void NewtonBodySetJointRecursiveCollision (const NewtonBody* const body, unsigned state); + NEWTON_API void NewtonBodySetOmega (const NewtonBody* const body, const dFloat* const omega); + NEWTON_API void NewtonBodySetOmegaNoSleep (const NewtonBody* const body, const dFloat* const omega); + NEWTON_API void NewtonBodySetVelocity (const NewtonBody* const body, const dFloat* const velocity); + NEWTON_API void NewtonBodySetVelocityNoSleep (const NewtonBody* const body, const dFloat* const velocity); + NEWTON_API void NewtonBodySetForce (const NewtonBody* const body, const dFloat* const force); + NEWTON_API void NewtonBodySetTorque (const NewtonBody* const body, const dFloat* const torque); + + NEWTON_API void NewtonBodySetLinearDamping (const NewtonBody* const body, dFloat linearDamp); + NEWTON_API void NewtonBodySetAngularDamping (const NewtonBody* const body, const dFloat* const angularDamp); + NEWTON_API void NewtonBodySetCollision (const NewtonBody* const body, const NewtonCollision* const collision); + NEWTON_API void NewtonBodySetCollisionScale (const NewtonBody* const body, dFloat scaleX, dFloat scaleY, dFloat scaleZ); + + NEWTON_API int NewtonBodyGetSleepState (const NewtonBody* const body); + NEWTON_API void NewtonBodySetSleepState (const NewtonBody* const body, int state); + + NEWTON_API int NewtonBodyGetAutoSleep (const NewtonBody* const body); + NEWTON_API void NewtonBodySetAutoSleep (const NewtonBody* const body, int state); + + NEWTON_API int NewtonBodyGetFreezeState(const NewtonBody* const body); + NEWTON_API void NewtonBodySetFreezeState (const NewtonBody* const body, int state); + + NEWTON_API int NewtonBodyGetGyroscopicTorque(const NewtonBody* const body); + NEWTON_API void NewtonBodySetGyroscopicTorque(const NewtonBody* const body, int state); + + NEWTON_API void NewtonBodySetDestructorCallback (const NewtonBody* const body, NewtonBodyDestructor callback); + NEWTON_API NewtonBodyDestructor NewtonBodyGetDestructorCallback (const NewtonBody* const body); + + NEWTON_API void NewtonBodySetTransformCallback (const NewtonBody* const body, NewtonSetTransform callback); + NEWTON_API NewtonSetTransform NewtonBodyGetTransformCallback (const NewtonBody* const body); + + NEWTON_API void NewtonBodySetForceAndTorqueCallback (const NewtonBody* const body, NewtonApplyForceAndTorque callback); + NEWTON_API NewtonApplyForceAndTorque NewtonBodyGetForceAndTorqueCallback (const NewtonBody* const body); + + NEWTON_API int NewtonBodyGetID (const NewtonBody* const body); + + NEWTON_API void NewtonBodySetUserData (const NewtonBody* const body, void* const userData); + NEWTON_API void* NewtonBodyGetUserData (const NewtonBody* const body); + + NEWTON_API NewtonWorld* NewtonBodyGetWorld (const NewtonBody* const body); + NEWTON_API NewtonCollision* NewtonBodyGetCollision (const NewtonBody* const body); + NEWTON_API int NewtonBodyGetMaterialGroupID (const NewtonBody* const body); + + NEWTON_API int NewtonBodyGetSerializedID(const NewtonBody* const body); + NEWTON_API int NewtonBodyGetContinuousCollisionMode (const NewtonBody* const body); + NEWTON_API int NewtonBodyGetJointRecursiveCollision (const NewtonBody* const body); + + NEWTON_API void NewtonBodyGetPosition(const NewtonBody* const body, dFloat* const pos); + NEWTON_API void NewtonBodyGetMatrix(const NewtonBody* const body, dFloat* const matrix); + NEWTON_API void NewtonBodyGetRotation(const NewtonBody* const body, dFloat* const rotation); + NEWTON_API void NewtonBodyGetMass (const NewtonBody* const body, dFloat* mass, dFloat* const Ixx, dFloat* const Iyy, dFloat* const Izz); + NEWTON_API void NewtonBodyGetInvMass(const NewtonBody* const body, dFloat* const invMass, dFloat* const invIxx, dFloat* const invIyy, dFloat* const invIzz); + NEWTON_API void NewtonBodyGetInertiaMatrix(const NewtonBody* const body, dFloat* const inertiaMatrix); + NEWTON_API void NewtonBodyGetInvInertiaMatrix(const NewtonBody* const body, dFloat* const invInertiaMatrix); + NEWTON_API void NewtonBodyGetOmega(const NewtonBody* const body, dFloat* const vector); + NEWTON_API void NewtonBodyGetVelocity(const NewtonBody* const body, dFloat* const vector); + NEWTON_API void NewtonBodyGetAlpha(const NewtonBody* const body, dFloat* const vector); + NEWTON_API void NewtonBodyGetAcceleration(const NewtonBody* const body, dFloat* const vector); + NEWTON_API void NewtonBodyGetForce(const NewtonBody* const body, dFloat* const vector); + NEWTON_API void NewtonBodyGetTorque(const NewtonBody* const body, dFloat* const vector); + NEWTON_API void NewtonBodyGetCentreOfMass (const NewtonBody* const body, dFloat* const com); + NEWTON_API void NewtonBodyGetPointVelocity (const NewtonBody* const body, const dFloat* const point, dFloat* const velocOut); + + NEWTON_API void NewtonBodyApplyImpulsePair (const NewtonBody* const body, dFloat* const linearImpulse, dFloat* const angularImpulse, dFloat timestep); + NEWTON_API void NewtonBodyAddImpulse (const NewtonBody* const body, const dFloat* const pointDeltaVeloc, const dFloat* const pointPosit, dFloat timestep); + NEWTON_API void NewtonBodyApplyImpulseArray (const NewtonBody* const body, int impuleCount, int strideInByte, const dFloat* const impulseArray, const dFloat* const pointArray, dFloat timestep); + + NEWTON_API void NewtonBodyIntegrateVelocity (const NewtonBody* const body, dFloat timestep); + + NEWTON_API dFloat NewtonBodyGetLinearDamping (const NewtonBody* const body); + NEWTON_API void NewtonBodyGetAngularDamping (const NewtonBody* const body, dFloat* const vector); + NEWTON_API void NewtonBodyGetAABB (const NewtonBody* const body, dFloat* const p0, dFloat* const p1); + + NEWTON_API NewtonJoint* NewtonBodyGetFirstJoint (const NewtonBody* const body); + NEWTON_API NewtonJoint* NewtonBodyGetNextJoint (const NewtonBody* const body, const NewtonJoint* const joint); + + NEWTON_API NewtonJoint* NewtonBodyGetFirstContactJoint (const NewtonBody* const body); + NEWTON_API NewtonJoint* NewtonBodyGetNextContactJoint (const NewtonBody* const body, const NewtonJoint* const contactJoint); + NEWTON_API NewtonJoint* NewtonBodyFindContact (const NewtonBody* const body0, const NewtonBody* const body1); + + // ********************************************************************************************** + // + // contact joints interface + // + // ********************************************************************************************** + NEWTON_API void* NewtonContactJointGetFirstContact (const NewtonJoint* const contactJoint); + NEWTON_API void* NewtonContactJointGetNextContact (const NewtonJoint* const contactJoint, void* const contact); + + NEWTON_API int NewtonContactJointGetContactCount(const NewtonJoint* const contactJoint); + NEWTON_API void NewtonContactJointRemoveContact(const NewtonJoint* const contactJoint, void* const contact); + + NEWTON_API dFloat NewtonContactJointGetClosestDistance(const NewtonJoint* const contactJoint); + NEWTON_API void NewtonContactJointResetSelftJointCollision(const NewtonJoint* const contactJoint); + NEWTON_API void NewtonContactJointResetIntraJointCollision(const NewtonJoint* const contactJoint); + + NEWTON_API NewtonMaterial* NewtonContactGetMaterial (const void* const contact); + + NEWTON_API NewtonCollision* NewtonContactGetCollision0 (const void* const contact); + NEWTON_API NewtonCollision* NewtonContactGetCollision1 (const void* const contact); + + NEWTON_API void* NewtonContactGetCollisionID0 (const void* const contact); + NEWTON_API void* NewtonContactGetCollisionID1 (const void* const contact); + + + // ********************************************************************************************** + // + // Common joint functions + // + // ********************************************************************************************** + NEWTON_API void* NewtonJointGetUserData (const NewtonJoint* const joint); + NEWTON_API void NewtonJointSetUserData (const NewtonJoint* const joint, void* const userData); + + NEWTON_API NewtonBody* NewtonJointGetBody0 (const NewtonJoint* const joint); + NEWTON_API NewtonBody* NewtonJointGetBody1 (const NewtonJoint* const joint); + + NEWTON_API void NewtonJointGetInfo (const NewtonJoint* const joint, NewtonJointRecord* const info); + NEWTON_API int NewtonJointGetCollisionState (const NewtonJoint* const joint); + NEWTON_API void NewtonJointSetCollisionState (const NewtonJoint* const joint, int state); + + NEWTON_API dFloat NewtonJointGetStiffness (const NewtonJoint* const joint); + NEWTON_API void NewtonJointSetStiffness (const NewtonJoint* const joint, dFloat state); + + NEWTON_API void NewtonDestroyJoint(const NewtonWorld* const newtonWorld, const NewtonJoint* const joint); + NEWTON_API void NewtonJointSetDestructor (const NewtonJoint* const joint, NewtonConstraintDestructor destructor); + + NEWTON_API int NewtonJointIsActive (const NewtonJoint* const joint); + + // ********************************************************************************************** + // + // particle system interface (soft bodies, individual, pressure bodies and cloth) + // + // ********************************************************************************************** + NEWTON_API NewtonCollision* NewtonCreateMassSpringDamperSystem (const NewtonWorld* const newtonWorld, int shapeID, + const dFloat* const points, int pointCount, int strideInBytes, const dFloat* const pointMass, + const int* const links, int linksCount, const dFloat* const linksSpring, const dFloat* const linksDamper); + + NEWTON_API NewtonCollision* NewtonCreateDeformableSolid(const NewtonWorld* const newtonWorld, const NewtonMesh* const mesh, int shapeID); + + NEWTON_API int NewtonDeformableMeshGetParticleCount (const NewtonCollision* const deformableMesh); + NEWTON_API int NewtonDeformableMeshGetParticleStrideInBytes (const NewtonCollision* const deformableMesh); + NEWTON_API const dFloat* NewtonDeformableMeshGetParticleArray (const NewtonCollision* const deformableMesh); + +/* + NEWTON_API NewtonCollision* NewtonCreateClothPatch (const NewtonWorld* const newtonWorld, NewtonMesh* const mesh, int shapeID, NewtonClothPatchMaterial* const structuralMaterial, NewtonClothPatchMaterial* const bendMaterial); + NEWTON_API void NewtonDeformableMeshCreateClusters (NewtonCollision* const deformableMesh, int clusterCount, dFloat overlapingWidth); + NEWTON_API void NewtonDeformableMeshSetDebugCallback (NewtonCollision* const deformableMesh, NewtonCollisionIterator callback); + + + NEWTON_API void NewtonDeformableMeshGetParticlePosition (NewtonCollision* const deformableMesh, int particleIndex, dFloat* const posit); + + NEWTON_API void NewtonDeformableMeshBeginConfiguration (const NewtonCollision* const deformableMesh); + NEWTON_API void NewtonDeformableMeshUnconstraintParticle (NewtonCollision* const deformableMesh, int particleIndex); + NEWTON_API void NewtonDeformableMeshConstraintParticle (NewtonCollision* const deformableMesh, int particleIndex, const dFloat* const posit, const NewtonBody* const body); + NEWTON_API void NewtonDeformableMeshEndConfiguration (const NewtonCollision* const deformableMesh); + +// NEWTON_API void NewtonDeformableMeshSetPlasticity (NewtonCollision* const deformableMesh, dFloat plasticity); +// NEWTON_API void NewtonDeformableMeshSetStiffness (NewtonCollision* const deformableMesh, dFloat stiffness); + NEWTON_API void NewtonDeformableMeshSetSkinThickness (NewtonCollision* const deformableMesh, dFloat skinThickness); + + NEWTON_API void NewtonDeformableMeshUpdateRenderNormals (const NewtonCollision* const deformableMesh); + NEWTON_API int NewtonDeformableMeshGetVertexCount (const NewtonCollision* const deformableMesh); + NEWTON_API void NewtonDeformableMeshGetVertexStreams (const NewtonCollision* const deformableMesh, int vertexStrideInByte, dFloat* const vertex, int normalStrideInByte, dFloat* const normal, int uvStrideInByte0, dFloat* const uv0); + NEWTON_API NewtonDeformableMeshSegment* NewtonDeformableMeshGetFirstSegment (const NewtonCollision* const deformableMesh); + NEWTON_API NewtonDeformableMeshSegment* NewtonDeformableMeshGetNextSegment (const NewtonCollision* const deformableMesh, const NewtonDeformableMeshSegment* const segment); + + NEWTON_API int NewtonDeformableMeshSegmentGetMaterialID (const NewtonCollision* const deformableMesh, const NewtonDeformableMeshSegment* const segment); + NEWTON_API int NewtonDeformableMeshSegmentGetIndexCount (const NewtonCollision* const deformableMesh, const NewtonDeformableMeshSegment* const segment); + NEWTON_API const int* NewtonDeformableMeshSegmentGetIndexList (const NewtonCollision* const deformableMesh, const NewtonDeformableMeshSegment* const segment); +*/ + // ********************************************************************************************** + // + // Ball and Socket joint functions + // + // ********************************************************************************************** + NEWTON_API NewtonJoint* NewtonConstraintCreateBall (const NewtonWorld* const newtonWorld, const dFloat* pivotPoint, const NewtonBody* const childBody, const NewtonBody* const parentBody); + NEWTON_API void NewtonBallSetUserCallback (const NewtonJoint* const ball, NewtonBallCallback callback); + NEWTON_API void NewtonBallGetJointAngle (const NewtonJoint* const ball, dFloat* angle); + NEWTON_API void NewtonBallGetJointOmega (const NewtonJoint* const ball, dFloat* omega); + NEWTON_API void NewtonBallGetJointForce (const NewtonJoint* const ball, dFloat* const force); + NEWTON_API void NewtonBallSetConeLimits (const NewtonJoint* const ball, const dFloat* pin, dFloat maxConeAngle, dFloat maxTwistAngle); + + // ********************************************************************************************** + // + // Hinge joint functions + // + // ********************************************************************************************** + NEWTON_API NewtonJoint* NewtonConstraintCreateHinge (const NewtonWorld* const newtonWorld, const dFloat* pivotPoint, const dFloat* pinDir, const NewtonBody* const childBody, const NewtonBody* const parentBody); + NEWTON_API void NewtonHingeSetUserCallback (const NewtonJoint* const hinge, NewtonHingeCallback callback); + NEWTON_API dFloat NewtonHingeGetJointAngle (const NewtonJoint* const hinge); + NEWTON_API dFloat NewtonHingeGetJointOmega (const NewtonJoint* const hinge); + NEWTON_API void NewtonHingeGetJointForce (const NewtonJoint* const hinge, dFloat* const force); + NEWTON_API dFloat NewtonHingeCalculateStopAlpha (const NewtonJoint* const hinge, const NewtonHingeSliderUpdateDesc* const desc, dFloat angle); + + // ********************************************************************************************** + // + // Slider joint functions + // + // ********************************************************************************************** + NEWTON_API NewtonJoint* NewtonConstraintCreateSlider (const NewtonWorld* const newtonWorld, const dFloat* pivotPoint, const dFloat* pinDir, const NewtonBody* const childBody, const NewtonBody* const parentBody); + NEWTON_API void NewtonSliderSetUserCallback (const NewtonJoint* const slider, NewtonSliderCallback callback); + NEWTON_API dFloat NewtonSliderGetJointPosit (const NewtonJoint* slider); + NEWTON_API dFloat NewtonSliderGetJointVeloc (const NewtonJoint* slider); + NEWTON_API void NewtonSliderGetJointForce (const NewtonJoint* const slider, dFloat* const force); + NEWTON_API dFloat NewtonSliderCalculateStopAccel (const NewtonJoint* const slider, const NewtonHingeSliderUpdateDesc* const desc, dFloat position); + + + // ********************************************************************************************** + // + // Corkscrew joint functions + // + // ********************************************************************************************** + NEWTON_API NewtonJoint* NewtonConstraintCreateCorkscrew (const NewtonWorld* const newtonWorld, const dFloat* pivotPoint, const dFloat* pinDir, const NewtonBody* const childBody, const NewtonBody* const parentBody); + NEWTON_API void NewtonCorkscrewSetUserCallback (const NewtonJoint* const corkscrew, NewtonCorkscrewCallback callback); + NEWTON_API dFloat NewtonCorkscrewGetJointPosit (const NewtonJoint* const corkscrew); + NEWTON_API dFloat NewtonCorkscrewGetJointAngle (const NewtonJoint* const corkscrew); + NEWTON_API dFloat NewtonCorkscrewGetJointVeloc (const NewtonJoint* const corkscrew); + NEWTON_API dFloat NewtonCorkscrewGetJointOmega (const NewtonJoint* const corkscrew); + NEWTON_API void NewtonCorkscrewGetJointForce (const NewtonJoint* const corkscrew, dFloat* const force); + NEWTON_API dFloat NewtonCorkscrewCalculateStopAlpha (const NewtonJoint* const corkscrew, const NewtonHingeSliderUpdateDesc* const desc, dFloat angle); + NEWTON_API dFloat NewtonCorkscrewCalculateStopAccel (const NewtonJoint* const corkscrew, const NewtonHingeSliderUpdateDesc* const desc, dFloat position); + + + // ********************************************************************************************** + // + // Universal joint functions + // + // ********************************************************************************************** + NEWTON_API NewtonJoint* NewtonConstraintCreateUniversal (const NewtonWorld* const newtonWorld, const dFloat* pivotPoint, const dFloat* pinDir0, const dFloat* pinDir1, const NewtonBody* const childBody, const NewtonBody* const parentBody); + NEWTON_API void NewtonUniversalSetUserCallback (const NewtonJoint* const universal, NewtonUniversalCallback callback); + NEWTON_API dFloat NewtonUniversalGetJointAngle0 (const NewtonJoint* const universal); + NEWTON_API dFloat NewtonUniversalGetJointAngle1 (const NewtonJoint* const universal); + NEWTON_API dFloat NewtonUniversalGetJointOmega0 (const NewtonJoint* const universal); + NEWTON_API dFloat NewtonUniversalGetJointOmega1 (const NewtonJoint* const universal); + NEWTON_API void NewtonUniversalGetJointForce (const NewtonJoint* const universal, dFloat* const force); + NEWTON_API dFloat NewtonUniversalCalculateStopAlpha0 (const NewtonJoint* const universal, const NewtonHingeSliderUpdateDesc* const desc, dFloat angle); + NEWTON_API dFloat NewtonUniversalCalculateStopAlpha1 (const NewtonJoint* const universal, const NewtonHingeSliderUpdateDesc* const desc, dFloat angle); + + + // ********************************************************************************************** + // + // Up vector joint functions + // + // ********************************************************************************************** + NEWTON_API NewtonJoint* NewtonConstraintCreateUpVector (const NewtonWorld* const newtonWorld, const dFloat* pinDir, const NewtonBody* const body); + NEWTON_API void NewtonUpVectorGetPin (const NewtonJoint* const upVector, dFloat *pin); + NEWTON_API void NewtonUpVectorSetPin (const NewtonJoint* const upVector, const dFloat *pin); + + + // ********************************************************************************************** + // + // User defined bilateral Joint + // + // ********************************************************************************************** + NEWTON_API NewtonJoint* NewtonConstraintCreateUserJoint (const NewtonWorld* const newtonWorld, int maxDOF, NewtonUserBilateralCallback callback, const NewtonBody* const childBody, const NewtonBody* const parentBody) ; + NEWTON_API int NewtonUserJointGetSolverModel(const NewtonJoint* const joint); + NEWTON_API void NewtonUserJointSetSolverModel(const NewtonJoint* const joint, int model); + NEWTON_API void NewtonUserJointMassScale(const NewtonJoint* const joint, dFloat scaleBody0, dFloat scaleBody1); + + NEWTON_API void NewtonUserJointSetFeedbackCollectorCallback (const NewtonJoint* const joint, NewtonUserBilateralCallback getFeedback); + NEWTON_API void NewtonUserJointAddLinearRow (const NewtonJoint* const joint, const dFloat* const pivot0, const dFloat* const pivot1, const dFloat* const dir); + NEWTON_API void NewtonUserJointAddAngularRow (const NewtonJoint* const joint, dFloat relativeAngle, const dFloat* const dir); + NEWTON_API void NewtonUserJointAddGeneralRow (const NewtonJoint* const joint, const dFloat* const jacobian0, const dFloat* const jacobian1); + NEWTON_API void NewtonUserJointSetRowMinimumFriction (const NewtonJoint* const joint, dFloat friction); + NEWTON_API void NewtonUserJointSetRowMaximumFriction (const NewtonJoint* const joint, dFloat friction); + NEWTON_API dFloat NewtonUserJointCalculateRowZeroAcceleration (const NewtonJoint* const joint); + NEWTON_API dFloat NewtonUserJointGetRowAcceleration (const NewtonJoint* const joint); + NEWTON_API void NewtonUserJointGetRowJacobian(const NewtonJoint* const joint, dFloat* const linear0, dFloat* const angula0, dFloat* const linear1, dFloat* const angula1); + NEWTON_API void NewtonUserJointSetRowAcceleration (const NewtonJoint* const joint, dFloat acceleration); + NEWTON_API void NewtonUserJointSetRowSpringDamperAcceleration (const NewtonJoint* const joint, dFloat rowStiffness, dFloat spring, dFloat damper); + NEWTON_API void NewtonUserJointSetRowStiffness (const NewtonJoint* const joint, dFloat stiffness); + NEWTON_API int NewtonUserJoinRowsCount (const NewtonJoint* const joint); + NEWTON_API void NewtonUserJointGetGeneralRow (const NewtonJoint* const joint, int index, dFloat* const jacobian0, dFloat* const jacobian1); + NEWTON_API dFloat NewtonUserJointGetRowForce (const NewtonJoint* const joint, int row); + + // ********************************************************************************************** + // + // Mesh joint functions + // + // ********************************************************************************************** + NEWTON_API NewtonMesh* NewtonMeshCreate(const NewtonWorld* const newtonWorld); + NEWTON_API NewtonMesh* NewtonMeshCreateFromMesh(const NewtonMesh* const mesh); + NEWTON_API NewtonMesh* NewtonMeshCreateFromCollision(const NewtonCollision* const collision); + NEWTON_API NewtonMesh* NewtonMeshCreateTetrahedraIsoSurface(const NewtonMesh* const mesh); + NEWTON_API NewtonMesh* NewtonMeshCreateConvexHull (const NewtonWorld* const newtonWorld, int pointCount, const dFloat* const vertexCloud, int strideInBytes, dFloat tolerance); + NEWTON_API NewtonMesh* NewtonMeshCreateVoronoiConvexDecomposition (const NewtonWorld* const newtonWorld, int pointCount, const dFloat* const vertexCloud, int strideInBytes, int materialID, const dFloat* const textureMatrix); + NEWTON_API NewtonMesh* NewtonMeshCreateFromSerialization (const NewtonWorld* const newtonWorld, NewtonDeserializeCallback deserializeFunction, void* const serializeHandle); + NEWTON_API void NewtonMeshDestroy(const NewtonMesh* const mesh); + + NEWTON_API void NewtonMeshSerialize (const NewtonMesh* const mesh, NewtonSerializeCallback serializeFunction, void* const serializeHandle); + NEWTON_API void NewtonMeshSaveOFF(const NewtonMesh* const mesh, const char* const filename); + NEWTON_API NewtonMesh* NewtonMeshLoadOFF(const NewtonWorld* const newtonWorld, const char* const filename); + NEWTON_API NewtonMesh* NewtonMeshLoadTetrahedraMesh(const NewtonWorld* const newtonWorld, const char* const filename); + + NEWTON_API void NewtonMeshFlipWinding(const NewtonMesh* const mesh); + + NEWTON_API void NewtonMeshApplyTransform (const NewtonMesh* const mesh, const dFloat* const matrix); + NEWTON_API void NewtonMeshCalculateOOBB(const NewtonMesh* const mesh, dFloat* const matrix, dFloat* const x, dFloat* const y, dFloat* const z); + + NEWTON_API void NewtonMeshCalculateVertexNormals(const NewtonMesh* const mesh, dFloat angleInRadians); + NEWTON_API void NewtonMeshApplySphericalMapping(const NewtonMesh* const mesh, int material, const dFloat* const aligmentMatrix); + NEWTON_API void NewtonMeshApplyCylindricalMapping(const NewtonMesh* const mesh, int cylinderMaterial, int capMaterial, const dFloat* const aligmentMatrix); + NEWTON_API void NewtonMeshApplyBoxMapping(const NewtonMesh* const mesh, int frontMaterial, int sideMaterial, int topMaterial, const dFloat* const aligmentMatrix); + NEWTON_API void NewtonMeshApplyAngleBasedMapping(const NewtonMesh* const mesh, int material, NewtonReportProgress reportPrograssCallback, void* const reportPrgressUserData, dFloat* const aligmentMatrix); + + NEWTON_API void NewtonCreateTetrahedraLinearBlendSkinWeightsChannel(const NewtonMesh* const tetrahedraMesh, NewtonMesh* const skinMesh); + + NEWTON_API void NewtonMeshOptimize (const NewtonMesh* const mesh); + NEWTON_API void NewtonMeshOptimizePoints (const NewtonMesh* const mesh); + NEWTON_API void NewtonMeshOptimizeVertex (const NewtonMesh* const mesh); + NEWTON_API int NewtonMeshIsOpenMesh (const NewtonMesh* const mesh); + NEWTON_API void NewtonMeshFixTJoints (const NewtonMesh* const mesh); + + NEWTON_API void NewtonMeshPolygonize (const NewtonMesh* const mesh); + NEWTON_API void NewtonMeshTriangulate (const NewtonMesh* const mesh); + NEWTON_API NewtonMesh* NewtonMeshUnion (const NewtonMesh* const mesh, const NewtonMesh* const clipper, const dFloat* const clipperMatrix); + NEWTON_API NewtonMesh* NewtonMeshDifference (const NewtonMesh* const mesh, const NewtonMesh* const clipper, const dFloat* const clipperMatrix); + NEWTON_API NewtonMesh* NewtonMeshIntersection (const NewtonMesh* const mesh, const NewtonMesh* const clipper, const dFloat* const clipperMatrix); + NEWTON_API void NewtonMeshClip (const NewtonMesh* const mesh, const NewtonMesh* const clipper, const dFloat* const clipperMatrix, NewtonMesh** const topMesh, NewtonMesh** const bottomMesh); + + NEWTON_API NewtonMesh* NewtonMeshConvexMeshIntersection (const NewtonMesh* const mesh, const NewtonMesh* const convexMesh); + + NEWTON_API NewtonMesh* NewtonMeshSimplify (const NewtonMesh* const mesh, int maxVertexCount, NewtonReportProgress reportPrograssCallback, void* const reportPrgressUserData); + NEWTON_API NewtonMesh* NewtonMeshApproximateConvexDecomposition (const NewtonMesh* const mesh, dFloat maxConcavity, dFloat backFaceDistanceFactor, int maxCount, int maxVertexPerHull, NewtonReportProgress reportProgressCallback, void* const reportProgressUserData); + + NEWTON_API void NewtonRemoveUnusedVertices(const NewtonMesh* const mesh, int* const vertexRemapTable); + + NEWTON_API void NewtonMeshBeginBuild(const NewtonMesh* const mesh); + NEWTON_API void NewtonMeshBeginFace(const NewtonMesh* const mesh); + NEWTON_API void NewtonMeshAddPoint(const NewtonMesh* const mesh, dFloat64 x, dFloat64 y, dFloat64 z); + NEWTON_API void NewtonMeshAddLayer(const NewtonMesh* const mesh, int layerIndex); + NEWTON_API void NewtonMeshAddMaterial(const NewtonMesh* const mesh, int materialIndex); + NEWTON_API void NewtonMeshAddNormal(const NewtonMesh* const mesh, dFloat x, dFloat y, dFloat z); + NEWTON_API void NewtonMeshAddBinormal(const NewtonMesh* const mesh, dFloat x, dFloat y, dFloat z); + NEWTON_API void NewtonMeshAddUV0(const NewtonMesh* const mesh, dFloat u, dFloat v); + NEWTON_API void NewtonMeshAddUV1(const NewtonMesh* const mesh, dFloat u, dFloat v); + NEWTON_API void NewtonMeshAddVertexColor(const NewtonMesh* const mesh, dFloat32 r, dFloat32 g, dFloat32 b, dFloat32 a); + NEWTON_API void NewtonMeshEndFace(const NewtonMesh* const mesh); + NEWTON_API void NewtonMeshEndBuild(const NewtonMesh* const mesh); + + NEWTON_API void NewtonMeshClearVertexFormat (NewtonMeshVertexFormat* const format); + NEWTON_API void NewtonMeshBuildFromVertexListIndexList (const NewtonMesh* const mesh, const NewtonMeshVertexFormat* const format); + + NEWTON_API int NewtonMeshGetPointCount (const NewtonMesh* const mesh); + NEWTON_API const int* NewtonMeshGetIndexToVertexMap(const NewtonMesh* const mesh); + + NEWTON_API void NewtonMeshGetVertexDoubleChannel (const NewtonMesh* const mesh, int vertexStrideInByte, dFloat64* const outBuffer); + NEWTON_API void NewtonMeshGetVertexChannel (const NewtonMesh* const mesh, int vertexStrideInByte, dFloat* const outBuffer); + NEWTON_API void NewtonMeshGetNormalChannel (const NewtonMesh* const mesh, int vertexStrideInByte, dFloat* const outBuffer); + NEWTON_API void NewtonMeshGetBinormalChannel (const NewtonMesh* const mesh, int vertexStrideInByte, dFloat* const outBuffer); + NEWTON_API void NewtonMeshGetUV0Channel (const NewtonMesh* const mesh, int vertexStrideInByte, dFloat* const outBuffer); + NEWTON_API void NewtonMeshGetUV1Channel (const NewtonMesh* const mesh, int vertexStrideInByte, dFloat* const outBuffer); + NEWTON_API void NewtonMeshGetVertexColorChannel (const NewtonMesh* const mesh, int vertexStrideInByte, dFloat* const outBuffer); + + NEWTON_API int NewtonMeshHasNormalChannel(const NewtonMesh* const mesh); + NEWTON_API int NewtonMeshHasBinormalChannel(const NewtonMesh* const mesh); + NEWTON_API int NewtonMeshHasUV0Channel(const NewtonMesh* const mesh); + NEWTON_API int NewtonMeshHasUV1Channel(const NewtonMesh* const mesh); + NEWTON_API int NewtonMeshHasVertexColorChannel(const NewtonMesh* const mesh); + + NEWTON_API void* NewtonMeshBeginHandle (const NewtonMesh* const mesh); + NEWTON_API void NewtonMeshEndHandle (const NewtonMesh* const mesh, void* const handle); + NEWTON_API int NewtonMeshFirstMaterial (const NewtonMesh* const mesh, void* const handle); + NEWTON_API int NewtonMeshNextMaterial (const NewtonMesh* const mesh, void* const handle, int materialId); + NEWTON_API int NewtonMeshMaterialGetMaterial (const NewtonMesh* const mesh, void* const handle, int materialId); + NEWTON_API int NewtonMeshMaterialGetIndexCount (const NewtonMesh* const mesh, void* const handle, int materialId); + NEWTON_API void NewtonMeshMaterialGetIndexStream (const NewtonMesh* const mesh, void* const handle, int materialId, int* const index); + NEWTON_API void NewtonMeshMaterialGetIndexStreamShort (const NewtonMesh* const mesh, void* const handle, int materialId, short int* const index); + + NEWTON_API NewtonMesh* NewtonMeshCreateFirstSingleSegment (const NewtonMesh* const mesh); + NEWTON_API NewtonMesh* NewtonMeshCreateNextSingleSegment (const NewtonMesh* const mesh, const NewtonMesh* const segment); + + NEWTON_API NewtonMesh* NewtonMeshCreateFirstLayer (const NewtonMesh* const mesh); + NEWTON_API NewtonMesh* NewtonMeshCreateNextLayer (const NewtonMesh* const mesh, const NewtonMesh* const segment); + + NEWTON_API int NewtonMeshGetTotalFaceCount (const NewtonMesh* const mesh); + NEWTON_API int NewtonMeshGetTotalIndexCount (const NewtonMesh* const mesh); + NEWTON_API void NewtonMeshGetFaces (const NewtonMesh* const mesh, int* const faceIndexCount, int* const faceMaterial, void** const faceIndices); + + NEWTON_API int NewtonMeshGetVertexCount (const NewtonMesh* const mesh); + NEWTON_API int NewtonMeshGetVertexStrideInByte (const NewtonMesh* const mesh); + NEWTON_API const dFloat64* NewtonMeshGetVertexArray (const NewtonMesh* const mesh); + + NEWTON_API int NewtonMeshGetVertexBaseCount(const NewtonMesh* const mesh); + NEWTON_API void NewtonMeshSetVertexBaseCount(const NewtonMesh* const mesh, int baseCount); + + NEWTON_API void* NewtonMeshGetFirstVertex (const NewtonMesh* const mesh); + NEWTON_API void* NewtonMeshGetNextVertex (const NewtonMesh* const mesh, const void* const vertex); + NEWTON_API int NewtonMeshGetVertexIndex (const NewtonMesh* const mesh, const void* const vertex); + + NEWTON_API void* NewtonMeshGetFirstPoint (const NewtonMesh* const mesh); + NEWTON_API void* NewtonMeshGetNextPoint (const NewtonMesh* const mesh, const void* const point); + NEWTON_API int NewtonMeshGetPointIndex (const NewtonMesh* const mesh, const void* const point); + NEWTON_API int NewtonMeshGetVertexIndexFromPoint (const NewtonMesh* const mesh, const void* const point); + + NEWTON_API void* NewtonMeshGetFirstEdge (const NewtonMesh* const mesh); + NEWTON_API void* NewtonMeshGetNextEdge (const NewtonMesh* const mesh, const void* const edge); + NEWTON_API void NewtonMeshGetEdgeIndices (const NewtonMesh* const mesh, const void* const edge, int* const v0, int* const v1); + //NEWTON_API void NewtonMeshGetEdgePointIndices (const NewtonMesh* const mesh, const void* const edge, int* const v0, int* const v1); + + NEWTON_API void* NewtonMeshGetFirstFace (const NewtonMesh* const mesh); + NEWTON_API void* NewtonMeshGetNextFace (const NewtonMesh* const mesh, const void* const face); + NEWTON_API int NewtonMeshIsFaceOpen (const NewtonMesh* const mesh, const void* const face); + NEWTON_API int NewtonMeshGetFaceMaterial (const NewtonMesh* const mesh, const void* const face); + NEWTON_API int NewtonMeshGetFaceIndexCount (const NewtonMesh* const mesh, const void* const face); + NEWTON_API void NewtonMeshGetFaceIndices (const NewtonMesh* const mesh, const void* const face, int* const indices); + NEWTON_API void NewtonMeshGetFacePointIndices (const NewtonMesh* const mesh, const void* const face, int* const indices); + NEWTON_API void NewtonMeshCalculateFaceNormal (const NewtonMesh* const mesh, const void* const face, dFloat64* const normal); + + NEWTON_API void NewtonMeshSetFaceMaterial (const NewtonMesh* const mesh, const void* const face, int matId); + + +#ifdef __cplusplus +} +#endif +#endif + + + diff --git a/thirdparty/src/newton/dgNewton/NewtonClass.cpp b/thirdparty/src/newton/dgNewton/NewtonClass.cpp new file mode 100644 index 000000000..ef51615c7 --- /dev/null +++ b/thirdparty/src/newton/dgNewton/NewtonClass.cpp @@ -0,0 +1,275 @@ +/* Copyright (c) <2003-2019> +* +* 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 "NewtonStdAfx.h" +#include "NewtonClass.h" + + +void* Newton::DefaultAllocMemory (dgInt32 size) +{ + return malloc (size_t (size)); +} + + +void Newton::DefaultFreeMemory (void* const ptr, dgInt32 size) +{ + free (ptr); +} + + +Newton::Newton (dgMemoryAllocator* const allocator) + :dgWorld(allocator) + ,m_destructor(NULL) +{ +} + +Newton::~Newton () +{ + if (m_destructor) { + m_destructor ((NewtonWorld*)this); + } +} + +void Newton::UpdatePhysics (dgFloat32 timestep) +{ + Update (timestep); +} + +void Newton::UpdatePhysicsAsync (dgFloat32 timestep) +{ + UpdateAsync (timestep); +} + +NewtonUserJoint::NewtonUserJoint(NewtonUserBilateralCallback callback, dgBody* const body) + :dgUserConstraint(NULL, body, NULL, 1) + ,m_forceArray(m_jointForce) + ,m_param(NULL) + ,m_rows(0) +{ + m_maxDOF = 6; + m_jacobianFnt = callback; + m_body1 = body->GetWorld()->GetSentinelBody(); +} + +NewtonUserJoint::NewtonUserJoint (dgWorld* const world, dgInt32 maxDof, NewtonUserBilateralCallback callback, dgBody* const dyn0, dgBody* const dyn1) + :dgUserConstraint (world, dyn0, dyn1, 1) + ,m_forceArray(m_jointForce) + ,m_param(NULL) + ,m_rows(0) +{ + m_maxDOF = dgUnsigned8(maxDof); + m_jacobianFnt = callback; + + dgAssert (world); + if (m_maxDOF > DG_BILATERAL_CONTRAINT_DOF) { + m_forceArray = (dgForceImpactPair*) world->GetAllocator()->Malloc (dgInt32 (m_maxDOF * sizeof (dgForceImpactPair))); + } + memset (m_forceArray, 0, m_maxDOF * sizeof (dgFloat32)); +} + +NewtonUserJoint::~NewtonUserJoint () +{ + if (m_forceArray != m_jointForce) { + m_body0->GetWorld()->GetAllocator()->Free (m_forceArray); + } +} + +void NewtonUserJoint::SetMassScale (dgFloat32 scale0, dgFloat32 scale1) +{ + m_massScaleBody0 = scale0; + m_massScaleBody1 = scale1; + + dgWorld* const world = m_body0->GetWorld(); + dgSkeletonList& skelManager = *world; + skelManager.m_skelListIsDirty = true; +} + +dgUnsigned32 NewtonUserJoint::JacobianDerivative (dgContraintDescritor& params) +{ + m_rows = 0; + m_param = ¶ms; + m_jacobianFnt ((NewtonJoint*)this, params.m_timestep, params.m_threadIndex); + return dgUnsigned32 (m_rows); +} + +void NewtonUserJoint::Serialize (dgSerialize serializeCallback, void* const userData) +{ + dgWorld::OnJointSerializationCallback serializeJoint; + dgWorld::OnJointDeserializationCallback deserializeJoint; + + Newton* const world = m_body0 ? (Newton*)m_body0->GetWorld() : (Newton*)m_body1->GetWorld(); + world->GetJointSerializationCallbacks (&serializeJoint, &deserializeJoint); + if (serializeJoint) { + ((NewtonOnJointSerializationCallback)serializeJoint) ((NewtonJoint*)this, (NewtonSerializeCallback)serializeCallback, userData); + } +} + +void NewtonUserJoint::AddLinearRowJacobian (const dgVector& pivot0, const dgVector& pivot1, const dgVector& dir) +{ + dgPointParam pointData; + InitPointParam (pointData, m_defualtDiagonalRegularizer, pivot0, pivot1); + + CalculatePointDerivative (m_rows, *m_param, dir, pointData, &m_forceArray[m_rows]); + m_rows ++; + dgAssert (m_rows <= dgInt32 (m_maxDOF)); +} + +void NewtonUserJoint::AddAngularRowJacobian (const dgVector& dir, dgFloat32 relAngle) +{ + CalculateAngularDerivative (m_rows, *m_param, dir, m_defualtDiagonalRegularizer, relAngle, &m_forceArray[m_rows]); + m_rows ++; + dgAssert (m_rows <= dgInt32 (m_maxDOF)); +} + +void NewtonUserJoint::AddGeneralRowJacobian (const dgFloat32* const jacobian0, const dgFloat32* const jacobian1) +{ + SetJacobianDerivative (m_rows, *m_param, jacobian0, jacobian1, &m_forceArray[m_rows]); + m_rows ++; + dgAssert (m_rows <= dgInt32 (m_maxDOF)); +} + +dgInt32 NewtonUserJoint::GetJacobianCount() const +{ + return m_rows; +} + +void NewtonUserJoint::GetJacobianAt(dgInt32 index, dgFloat32* const jacobian0, dgFloat32* const jacobian1) const +{ + if (index < m_rows) { + for (dgInt32 i = 0; i < 3; i ++) { + jacobian0[i] = m_param->m_jacobian[index].m_jacobianM0.m_linear[i]; + jacobian1[i] = m_param->m_jacobian[index].m_jacobianM1.m_linear[i]; + jacobian0[i + 3] = m_param->m_jacobian[index].m_jacobianM0.m_angular[i]; + jacobian1[i + 3] = m_param->m_jacobian[index].m_jacobianM1.m_angular[i]; + } + } +} + +void NewtonUserJoint::GetJacobian(dgJacobian& jacobian0, dgJacobian& jacobian1) const +{ + dgInt32 index = m_rows - 1; + if ((index >= 0) && (index < dgInt32(m_maxDOF))) { + jacobian0.m_linear = m_param->m_jacobian[index].m_jacobianM0.m_linear; + jacobian0.m_angular = m_param->m_jacobian[index].m_jacobianM0.m_angular; + jacobian1.m_linear = m_param->m_jacobian[index].m_jacobianM1.m_linear; + jacobian1.m_angular = m_param->m_jacobian[index].m_jacobianM1.m_angular; + } +} + + +dFloat NewtonUserJoint::GetAcceleration () const +{ + dgInt32 index = m_rows - 1; + if ((index >= 0) && (index < dgInt32 (m_maxDOF))) { + return GetRowAcceleration (index, *m_param); + } + return 0.0f; +} + + +void NewtonUserJoint::SetAcceleration (dgFloat32 acceleration) +{ + dgInt32 index = m_rows - 1; + if ((index >= 0) && (index < dgInt32 (m_maxDOF))) { + SetMotorAcceleration (index, acceleration, *m_param); + } +} + +dgFloat32 NewtonUserJoint::CalculateZeroMotorAcceleration() const +{ + dgInt32 index = m_rows - 1; + dgFloat32 accel = dgFloat32 (0.0f); + if ((index >= 0) && (index < dgInt32(m_maxDOF))) { + accel = CalculateMotorAcceleration(index, *m_param); + } + return accel; +} + +void NewtonUserJoint::SetSpringDamperAcceleration (dgFloat32 rowStiffness, dFloat spring, dFloat damper) +{ + dgInt32 index = m_rows - 1; + if ((index >= 0) && (index < dgInt32 (m_maxDOF))) { + dgBilateralConstraint::SetSpringDamperAcceleration (index, *m_param, rowStiffness, spring, damper); + } +} + +void NewtonUserJoint::SetHighFriction (dgFloat32 friction) +{ + dgInt32 index = m_rows - 1; + if ((index >= 0) && (index < dgInt32 (m_maxDOF))) { + m_param->m_forceBounds[index].m_upper = dgClamp (friction, dgFloat32(0.001f), dgFloat32(DG_MAX_BOUND)); + dgAssert (m_param->m_forceBounds[index].m_normalIndex == DG_INDEPENDENT_ROW); + + #ifdef _DEBUG + dgInt32 i0 = 0; + dgInt32 i1 = m_rows - 1; + while ((i0 <= i1) && (m_param->m_forceBounds[i0].m_normalIndex == DG_INDEPENDENT_ROW)) i0 ++; + while ((i1 >= i0) && (m_param->m_forceBounds[i1].m_normalIndex != DG_INDEPENDENT_ROW)) i1 --; + dgAssert((i0 - i1) == 1); + if ((i0 - i1) != 1) { + dgTrace(("make sure that friction joint are issue at last\n")); + } + #endif + } +} + +void NewtonUserJoint::SetLowerFriction (dgFloat32 friction) +{ + dgInt32 index = m_rows - 1; + if ((index >= 0) && (index < dgInt32 (m_maxDOF))) { + m_param->m_forceBounds[index].m_low = dgClamp (friction, dgFloat32(DG_MIN_BOUND), dgFloat32(-0.001f)); + dgAssert (m_param->m_forceBounds[index].m_normalIndex == DG_INDEPENDENT_ROW); + + #ifdef _DEBUG + dgInt32 i0 = 0; + dgInt32 i1 = m_rows - 1; + while ((i0 <= i1) && (m_param->m_forceBounds[i0].m_normalIndex == DG_INDEPENDENT_ROW)) i0++; + while ((i1 >= i0) && (m_param->m_forceBounds[i1].m_normalIndex != DG_INDEPENDENT_ROW)) i1--; + dgAssert((i0 - i1) == 1); + if ((i0 - i1) != 1) { + dgTrace(("make sure that friction joint are issue at last\n")); + } + #endif + } +} + +void NewtonUserJoint::SetRowStiffness (dgFloat32 stiffness) +{ + dgInt32 index = m_rows - 1; + if ((index >= 0) && (index < dgInt32 (m_maxDOF))) { + stiffness = dgClamp (stiffness, dgFloat32(0.0f), dgFloat32(1.0f)); + m_param->m_diagonalRegularizer[index] = stiffness; + } +} + +dgFloat32 NewtonUserJoint::GetRowForce (dgInt32 row) const +{ + dgFloat32 force = 0.0f; + if ((row >= 0) && (row < dgInt32 (m_maxDOF))) { + force = m_forceArray[row].m_force; + } + return force; +} + +void NewtonUserJoint::SetUpdateFeedbackFunction (NewtonUserBilateralCallback getFeedback) +{ + dgUserConstraint::SetUpdateFeedbackFunction ((ConstraintsForceFeeback) getFeedback); +} diff --git a/thirdparty/src/newton/dgNewton/NewtonClass.h b/thirdparty/src/newton/dgNewton/NewtonClass.h new file mode 100644 index 000000000..3e7a3eae5 --- /dev/null +++ b/thirdparty/src/newton/dgNewton/NewtonClass.h @@ -0,0 +1,90 @@ +/* Copyright (c) <2003-2019> +* +* 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 __NewotnClass_H__ +#define __NewotnClass_H__ + +#include "NewtonStdAfx.h" +#include "Newton.h" + +class Newton; + + +class Newton: public dgWorld +{ + public: + DG_CLASS_ALLOCATOR(allocator) + + Newton (dgMemoryAllocator* const allocator); + ~Newton (); + + void UpdatePhysics (dgFloat32 timestep); + void UpdatePhysicsAsync (dgFloat32 timestep); + static void* DefaultAllocMemory (dgInt32 size); + static void DefaultFreeMemory (void* const ptr, dgInt32 size); + + NewtonWorldDestructorCallback m_destructor; +}; + + +class NewtonUserJoint: public dgUserConstraint +{ + public: + NewtonUserJoint (dgWorld* const world, dgInt32 maxDof, NewtonUserBilateralCallback callback, dgBody* const dyn0, dgBody* const dyn1); + ~NewtonUserJoint (); + + dgUnsigned32 JacobianDerivative (dgContraintDescritor& params); + void Serialize (dgSerialize serializeCallback, void* const userData); + + void AddAngularRowJacobian (const dgVector& dir, dgFloat32 relAngle); + void AddGeneralRowJacobian (const dgFloat32* const jacobian0, const dgFloat32* const jacobian1); + void AddLinearRowJacobian (const dgVector& pivot0, const dgVector& pivot1, const dgVector& dir); + + dgInt32 GetJacobianCount () const; + void GetJacobianAt (dgInt32 index, dgFloat32* const jacobian0, dgFloat32* const jacobian1) const; + + dgFloat32 GetRowForce (dgInt32 row) const; + void GetJacobian(dgJacobian& jacobian0, dgJacobian& jacobian1) const; + + void SetHighFriction (dgFloat32 friction); + void SetLowerFriction (dgFloat32 friction); + void SetRowStiffness (dgFloat32 stiffness); + void SetAcceleration (dgFloat32 acceleration); + dgFloat32 GetAcceleration () const; + dgFloat32 CalculateZeroMotorAcceleration() const; + + void SetSpringDamperAcceleration (dgFloat32 rowStiffness, dgFloat32 springK, dgFloat32 springD); + void SetUpdateFeedbackFunction (NewtonUserBilateralCallback getFeedback); + + void SetMassScale (dgFloat32 scale0, dgFloat32 scale1); + + protected: + NewtonUserJoint(NewtonUserBilateralCallback callback, dgBody* const body); + + private: + NewtonUserBilateralCallback m_jacobianFnt; + + dgForceImpactPair* m_forceArray; + dgContraintDescritor* m_param; + dgInt32 m_rows; +}; + +#endif diff --git a/thirdparty/src/newton/dgNewton/NewtonStdAfx.h b/thirdparty/src/newton/dgNewton/NewtonStdAfx.h new file mode 100644 index 000000000..e25f61616 --- /dev/null +++ b/thirdparty/src/newton/dgNewton/NewtonStdAfx.h @@ -0,0 +1,34 @@ +/* Copyright (c) <2003-2019> +* +* 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 __NEWTON_STDAFX_H__ +#define __NEWTON_STDAFX_H__ + + +#ifndef WIN32_LEAN_AND_MEAN + #define WIN32_LEAN_AND_MEAN // Exclude rarely-used stuff from Windows headers +#endif + +#include "dg.h" +#include "dgPhysics.h" + +#endif // !defined(AFX_STDAFX_H__7B02FF2D_C0C3_4291_9676_333105355F57_H) + diff --git a/thirdparty/src/newton/dgPhysics/CMakeLists.txt b/thirdparty/src/newton/dgPhysics/CMakeLists.txt new file mode 100644 index 000000000..d214a06c1 --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/CMakeLists.txt @@ -0,0 +1,61 @@ +# Copyright (c) <2014-2017> +# +# 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 "dgPhysics") +message (${projectName}) + +if(NEWTON_USE_PLUGIN_DLL) + add_definitions(-DDG_USE_PLUGINS) +endif() + + +#source and header files +file(GLOB CPP_SOURCE + *.h + *.cpp + ../dgMeshUtil/*.h + ../dgMeshUtil/*.cpp +) +#source_group(TREE "${CMAKE_CURRENT_SOURCE_DIR}/" FILES ${CPP_SOURCE}) + +if (MSVC) + set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} /fp:fast") + set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} /fp:fast") +endif(MSVC) + +if(NEWTON_BUILD_SHARED_LIBS) + if(MSVC) + add_library(${projectName} STATIC ${CPP_SOURCE}) + else() + add_library(${projectName} SHARED ${CPP_SOURCE}) + endif() +else() + add_library(${projectName} STATIC ${CPP_SOURCE}) +endif() + +if (MSVC) + if(CMAKE_VS_MSBUILD_COMMAND OR CMAKE_VS_DEVENV_COMMAND) + set_target_properties(${projectName} PROPERTIES COMPILE_FLAGS "/YudgPhysicsStdafx.h") + set_source_files_properties(dgWorld.cpp PROPERTIES COMPILE_FLAGS "/YcdgPhysicsStdafx.h") + endif() +endif(MSVC) + +target_include_directories(${projectName} PUBLIC . ../dgMeshUtil) +target_link_libraries(${projectName} dgCore) + +install(TARGETS ${projectName} + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin) + +install(FILES ${HEADERS} DESTINATION include/${projectName}) + diff --git a/thirdparty/src/newton/dgPhysics/dgBallConstraint.cpp b/thirdparty/src/newton/dgPhysics/dgBallConstraint.cpp new file mode 100644 index 000000000..01b76bcd6 --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgBallConstraint.cpp @@ -0,0 +1,304 @@ +/* Copyright (c) <2003-2019> +* +* 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 "dgPhysicsStdafx.h" +#include "dgBody.h" +#include "dgWorld.h" +#include "dgBallConstraint.h" + + +////////////////////////////////////////////////////////////////////// +// Construction/Destruction +////////////////////////////////////////////////////////////////////// +dgBallConstraint::dgBallConstraint () + :dgBilateralConstraint() +{ + dgAssert ((((dgUnsigned64) &m_localMatrix0) & 15) == 0); + + m_localMatrix0 = dgGetIdentityMatrix(); + m_localMatrix1 = dgGetIdentityMatrix(); + + m_maxDOF = 6; + m_jointUserCallback = NULL; + m_constId = m_ballConstraint; + m_ballLimits = 0; + m_angles = dgVector (dgFloat32 (0.0f)); +} + +dgBallConstraint::~dgBallConstraint () +{ +} + +void dgBallConstraint::SetJointParameterCallback (dgBallJointFriction callback) +{ + m_jointUserCallback = callback; +} + +dgVector dgBallConstraint::GetJointAngle ()const +{ + return m_angles; +} + +dgVector dgBallConstraint::GetJointOmega () const +{ + dgAssert (m_body0); + dgAssert (m_body1); + const dgMatrix& matrix = m_body0->GetMatrix(); + + dgVector dir0 (matrix.RotateVector (m_localMatrix0[0])); + dgVector dir1 (matrix.RotateVector (m_localMatrix0[1])); + dgVector dir2 (matrix.RotateVector (m_localMatrix0[2])); + + const dgVector& omega0 = m_body0->GetOmega(); + const dgVector& omega1 = m_body1->GetOmega(); + +// dgVector omega1 (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f)); +// if (m_body1) { +// omega1 = m_body1->GetOmega(); +// } + + dgVector relOmega (omega0 - omega1); + return dgVector (relOmega.DotProduct(dir0).GetScalar(), relOmega.DotProduct(dir1).GetScalar(), relOmega.DotProduct(dir2).GetScalar(), dgFloat32 (0.0f)); +} + +dgVector dgBallConstraint::GetJointForce () const +{ + dgMatrix matrix0; + dgMatrix matrix1; + + CalculateGlobalMatrixAndAngle (m_localMatrix0, m_localMatrix1, matrix0, matrix1); + return dgVector (matrix0.m_front.Scale (m_jointForce[0].m_force) + matrix0.m_up.Scale (m_jointForce[1].m_force) + matrix0.m_right.Scale (m_jointForce[2].m_force)); +} + +bool dgBallConstraint::GetTwistLimitState () const +{ + return m_twistLimit; +} + +bool dgBallConstraint::GetConeLimitState () const +{ + return m_coneLimit; +} + +bool dgBallConstraint::GetLatealLimitState () const +{ + return m_lateralLimit; +} + +void dgBallConstraint::SetTwistLimitState (bool state) +{ + m_twistLimit = dgUnsigned32 (state); +} + +void dgBallConstraint::SetConeLimitState (bool state) +{ + m_coneLimit = dgUnsigned32 (state); +} + +void dgBallConstraint::SetLatealLimitState (bool state) +{ + m_lateralLimit = dgUnsigned32 (state); +} + +void dgBallConstraint::SetPivotPoint(const dgVector &pivot) +{ + dgAssert (m_body0); + dgAssert (m_body1); + const dgMatrix& matrix = m_body0->GetMatrix(); + + dgVector pin (dgVector::m_triplexMask & (pivot - matrix.m_posit)); + dgAssert (pin.m_w == dgFloat32 (0.0f)); + if (pin.DotProduct(pin).GetScalar() < dgFloat32 (1.0e-3f)) { + pin = matrix.m_front; + } + + SetPivotAndPinDir (pivot, pin, m_localMatrix0, m_localMatrix1); + + dgMatrix matrix0; + dgMatrix matrix1; + CalculateGlobalMatrixAndAngle (m_localMatrix0, m_localMatrix1, matrix0, matrix1); + SetLimits (matrix0.m_front, -dgPi * dgFloat32 (0.5f), dgPi * dgFloat32 (0.5f), dgPi * dgFloat32 (0.5f), matrix0.m_right, dgFloat32 (0.0f), dgFloat32 (0.0f)); +} + +void dgBallConstraint::SetLimits ( + const dgVector& coneDir, + dgFloat32 minConeAngle, + dgFloat32 maxConeAngle, + dgFloat32 maxTwistAngle, + const dgVector& bilateralDir, + dgFloat32 negativeBilateralConeAngle__, + dgFloat32 positiveBilateralConeAngle__) +{ + dgMatrix matrix0; + dgMatrix matrix1; + CalculateGlobalMatrixAndAngle (m_localMatrix0, m_localMatrix1, matrix0, matrix1); + + dgAssert (m_body0); + dgAssert (m_body1); + const dgMatrix& body0_Matrix = m_body0->GetMatrix(); + + dgVector lateralDir (bilateralDir.CrossProduct(coneDir)); + if (lateralDir.DotProduct(lateralDir).GetScalar() < dgFloat32 (1.0e-3f)) { + dgMatrix tmp (coneDir); + lateralDir = tmp.m_up; + } + + + m_localMatrix0.m_front = body0_Matrix.UnrotateVector (coneDir); + m_localMatrix0.m_up = body0_Matrix.UnrotateVector (lateralDir); + m_localMatrix0.m_posit = body0_Matrix.UntransformVector (matrix1.m_posit); + + m_localMatrix0.m_front = m_localMatrix0.m_front.Normalize(); + m_localMatrix0.m_up = m_localMatrix0.m_up.Normalize(); + m_localMatrix0.m_right = m_localMatrix0.m_front.CrossProduct(m_localMatrix0.m_up); + + m_localMatrix0.m_front.m_w = dgFloat32 (0.0f); + m_localMatrix0.m_up.m_w = dgFloat32 (0.0f); + m_localMatrix0.m_right.m_w = dgFloat32 (0.0f); + m_localMatrix0.m_posit.m_w = dgFloat32 (1.0f); + + const dgMatrix& body1_Matrix = m_body1->GetMatrix(); + + m_twistAngle = dgClamp (maxTwistAngle, dgFloat32 (5.0f) * dgDegreeToRad, dgFloat32 (90.0f) * dgDegreeToRad); + m_coneAngle = dgClamp ((maxConeAngle - minConeAngle) * dgFloat32 (0.5f), dgFloat32 (5.0f) * dgDegreeToRad, 175.0f * dgDegreeToRad); + m_coneAngleCos = dgCos (m_coneAngle); + + dgMatrix coneMatrix (dgPitchMatrix((maxConeAngle + minConeAngle) * dgFloat32 (0.5f))); + + m_localMatrix0 = coneMatrix * m_localMatrix0; + + m_localMatrix1 = m_localMatrix0 * body0_Matrix * body1_Matrix.Inverse(); + +} + +dgUnsigned32 dgBallConstraint::JacobianDerivative (dgContraintDescritor& params) +{ + dgMatrix matrix0; + dgMatrix matrix1; + + if (m_jointUserCallback) { + m_jointUserCallback (*this, params.m_timestep); + } + + dgVector angle (CalculateGlobalMatrixAndAngle (m_localMatrix0, m_localMatrix1, matrix0, matrix1)); + m_angles = angle.Scale (-dgFloat32 (1.0f)); + + const dgVector& dir0 = matrix0.m_front; + const dgVector& dir1 = matrix0.m_up; + const dgVector& dir2 = matrix0.m_right; + const dgVector& p0 = matrix0.m_posit; + const dgVector& p1 = matrix1.m_posit; + + + dgPointParam pointData; + InitPointParam (pointData, m_defualtDiagonalRegularizer, p0, p1); + CalculatePointDerivative (0, params, dir0, pointData, &m_jointForce[0]); + CalculatePointDerivative (1, params, dir1, pointData, &m_jointForce[1]); + CalculatePointDerivative (2, params, dir2, pointData, &m_jointForce[2]); + dgInt32 ret = 3; + +// dgAssert (0); +/* + dgFloat32 relVelocErr; + dgFloat32 penetrationErr; + if (m_twistLimit) { + if (angle.m_x > m_twistAngle) { + dgVector q0 (matrix0.m_posit + matrix0.m_up.Scale(MIN_JOINT_PIN_LENGTH)); + InitPointParam (pointData, m_defualtDiagonalRegularizer, q0, q0); + + const dgVector& dir = matrix0.m_right; + CalculatePointDerivative (ret, params, dir, pointData, &m_jointForce[ret]); + + dgVector velocError (pointData.m_veloc1 - pointData.m_veloc0); + relVelocErr = velocError.DotProduct(dir).GetScalar(); + if (relVelocErr > dgFloat32 (1.0e-3f)) { + relVelocErr *= dgFloat32 (1.1f); + } + + penetrationErr = MIN_JOINT_PIN_LENGTH * (angle.m_x - m_twistAngle); + dgAssert (penetrationErr >= dgFloat32 (0.0f)); + + params.m_forceBounds[ret].m_low = dgFloat32 (0.0f); + params.m_forceBounds[ret].m_normalIndex = DG_INDEPENDENT_ROW; + params.m_forceBounds[ret].m_jointForce = &m_jointForce[ret]; + SetMotorAcceleration (ret, (relVelocErr + penetrationErr) * params.m_invTimestep, params); + ret ++; + } else if (angle.m_x < - m_twistAngle) { + dgVector q0 (matrix0.m_posit + matrix0.m_up.Scale(MIN_JOINT_PIN_LENGTH)); + InitPointParam (pointData, m_defualtDiagonalRegularizer, q0, q0); + //dgVector dir (matrix0.m_right.Scale (-dgFloat32 (1.0f))); + dgVector dir (matrix0.m_right * dgVector::m_negOne); + CalculatePointDerivative (ret, params, dir, pointData, &m_jointForce[ret]); + + dgVector velocError (pointData.m_veloc1 - pointData.m_veloc0); + relVelocErr = velocError.DotProduct(dir).GetScalar(); + if (relVelocErr > dgFloat32 (1.0e-3f)) { + relVelocErr *= dgFloat32 (1.1f); + } + + penetrationErr = MIN_JOINT_PIN_LENGTH * (- m_twistAngle - angle.m_x); + dgAssert (penetrationErr >= dgFloat32 (0.0f)); + + params.m_forceBounds[ret].m_low = dgFloat32 (0.0f); + params.m_forceBounds[ret].m_normalIndex = DG_INDEPENDENT_ROW; + params.m_forceBounds[ret].m_jointForce = &m_jointForce[ret]; + SetMotorAcceleration (ret, (relVelocErr + penetrationErr) * params.m_invTimestep, params); + ret ++; + } + } + + if (m_coneLimit) { + + dgFloat32 coneCos; + coneCos = matrix0.m_front.DotProduct(matrix1.m_front).GetScalar(); + if (coneCos < m_coneAngleCos) { + dgVector q0 (matrix0.m_posit + matrix0.m_front.Scale(MIN_JOINT_PIN_LENGTH)); + InitPointParam (pointData, m_defualtDiagonalRegularizer, q0, q0); + + dgVector tangentDir (matrix0.m_front.CrossProduct(matrix1.m_front)); + tangentDir = tangentDir.Normalize()); + CalculatePointDerivative (ret, params, tangentDir, pointData, &m_jointForce[ret]); + ret ++; + + dgVector normalDir (tangentDir.CrossProduct(matrix0.m_front)); + + dgVector velocError (pointData.m_veloc1 - pointData.m_veloc0); + //restitution = contact.m_restitution; + relVelocErr = velocError.DotProduct(normalDir).GetScalar(); + if (relVelocErr > dgFloat32 (1.0e-3f)) { + relVelocErr *= dgFloat32 (1.1f); + } + + penetrationErr = MIN_JOINT_PIN_LENGTH * (dgAcos (dgMax (coneCos, dgFloat32(-0.9999f))) - m_coneAngle); + dgAssert (penetrationErr >= dgFloat32 (0.0f)); + + CalculatePointDerivative (ret, params, normalDir, pointData, &m_jointForce[ret]); + params.m_forceBounds[ret].m_low = dgFloat32 (0.0f); + params.m_forceBounds[ret].m_normalIndex = DG_INDEPENDENT_ROW; + params.m_forceBounds[ret].m_jointForce = &m_jointForce[ret]; + SetMotorAcceleration (ret, (relVelocErr + penetrationErr) * params.m_invTimestep, params); + ret ++; + } + } +*/ + return dgUnsigned32 (ret); +} + diff --git a/thirdparty/src/newton/dgPhysics/dgBallConstraint.h b/thirdparty/src/newton/dgPhysics/dgBallConstraint.h new file mode 100644 index 000000000..a62d50fab --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgBallConstraint.h @@ -0,0 +1,96 @@ +/* Copyright (c) <2003-2019> +* +* 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_DGBALLCONSTRAINT_H__7C9E1F9A_5EC6_48BE_8C9F_FB90132C6619_H) +#define AFX_DGBALLCONSTRAINT_H__7C9E1F9A_5EC6_48BE_8C9F_FB90132C6619_H +#include "dgBilateralConstraint.h" + +#ifdef _WIN_32_VER + #pragma warning (disable: 4201) //nonstandard extension used : nameless struct/union +#endif + + +//template class dgPool; + +class dgBallConstraint; +typedef dgUnsigned32 (dgApi *dgBallJointFriction) (const dgBallConstraint& ball,dgFloat32 timestep); + +class dgBallConstraint: public dgBilateralConstraint +{ + public: + dgVector GetJointAngle ()const; + dgVector GetJointOmega ()const; + dgVector GetJointForce ()const; + void SetJointParameterCallback (dgBallJointFriction callback); + + bool GetTwistLimitState () const; + void SetTwistLimitState (bool state); + + bool GetConeLimitState () const; + void SetConeLimitState (bool state); + + bool GetLatealLimitState () const; + void SetLatealLimitState (bool state); + + + void SetLimits (const dgVector& coneDir, dgFloat32 minConeAngle, dgFloat32 maxConeAngle, dgFloat32 maxTwistAngle, + const dgVector& bilateralDir, dgFloat32 negativeBilateralConeAngle, dgFloat32 positiveBilateralConeAngle); + + + private: + dgBallConstraint (); + virtual ~dgBallConstraint (); + + void SetPivotPoint (const dgVector& pivot); + virtual dgUnsigned32 JacobianDerivative (dgContraintDescritor& params); + virtual void Serialize (dgSerialize serializeCallback, void* const userData) {dgAssert (0);} + + + dgVector m_angles; + union { + unsigned m_ballLimits; + struct { + dgUnsigned32 m_coneLimit : 1; + dgUnsigned32 m_twistLimit : 1; + dgUnsigned32 m_lateralLimit : 1; + }; + }; + + dgMatrix m_localMatrix0; + dgMatrix m_localMatrix1; + dgFloat32 m_coneAngle; + dgFloat32 m_twistAngle; + dgFloat32 m_coneAngleCos; + dgBallJointFriction m_jointUserCallback; + +// dgUnsigned32 m_reserve[3]; + + friend class dgWorld; +// friend class dgPool; +}; + +//class dgBallConstraintArray: public dgPoolContainer +//{ +//}; + + +#endif + diff --git a/thirdparty/src/newton/dgPhysics/dgBilateralConstraint.cpp b/thirdparty/src/newton/dgPhysics/dgBilateralConstraint.cpp new file mode 100644 index 000000000..236b49476 --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgBilateralConstraint.cpp @@ -0,0 +1,492 @@ +/* Copyright (c) <2003-2019> +* +* 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 "dgPhysicsStdafx.h" +#include "dgBody.h" +#include "dgWorld.h" +#include "dgConstraint.h" +#include "dgWorldDynamicUpdate.h" +#include "dgBilateralConstraint.h" + + +////////////////////////////////////////////////////////////////////// +// Construction/Destruction +////////////////////////////////////////////////////////////////////// + +#define DG_VEL_DAMP (dgFloat32(100.0f)) +#define DG_POS_DAMP (dgFloat32(1500.0f)) + + +dgBilateralConstraint::dgBilateralConstraint () + :dgConstraint () + ,m_destructor(NULL) + ,m_jointNode(NULL) +{ + m_maxDOF = 6; + m_isBilateral = true; + m_isActive = true; + m_solverModel = 0; + m_rowIsMotor = 0; + m_massScaleBody0 = dgFloat32 (1.0f); + m_massScaleBody1 = dgFloat32 (1.0f); + m_defualtDiagonalRegularizer = dgFloat32 (0.0f); + SetStiffness (dgFloat32 (0.0f)); + + memset (m_jointForce, 0, sizeof (m_jointForce)); + memset (m_motorAcceleration, 0, sizeof (m_motorAcceleration)); +} + +dgBilateralConstraint::~dgBilateralConstraint () +{ + if (m_destructor) { + m_destructor(*this); + } + + if (m_jointNode) { + dgAssert(m_body0); + dgBilateralConstraintList* const jointList = m_body0->m_world; + jointList->Remove(m_jointNode); + } +} + +void dgBilateralConstraint::AppendToJointList() +{ + dgAssert(m_body0); + dgAssert(!m_jointNode); + + dgBilateralConstraintList* const jointList = m_body0->m_world; + m_jointNode = jointList->Addtop(this); +} + +dgInt32 dgBilateralConstraint::GetSolverModel() const +{ + return m_solverModel; +} + +dgFloat32 dgBilateralConstraint::GetMassScaleBody0() const +{ + return m_massScaleBody0; +} + +dgFloat32 dgBilateralConstraint::GetMassScaleBody1() const +{ + return m_massScaleBody1; +} + +void dgBilateralConstraint::SetSolverModel(dgInt32 model) +{ + m_solverModel = dgClamp(model, 0, 3); +} + +dgFloat32 dgBilateralConstraint::GetStiffness() const +{ + return m_defualtDiagonalRegularizer; +} + +void dgBilateralConstraint::SetStiffness(dgFloat32 stiffness) +{ + m_defualtDiagonalRegularizer = dgClamp (stiffness, dgFloat32(0.0f), dgFloat32(1.0f)); +} + +void dgBilateralConstraint::SetDestructorCallback (OnConstraintDestroy destructor) +{ + m_destructor = destructor; +} + +void dgBilateralConstraint::CalculateMatrixOffset (const dgVector& pivot, const dgVector& dir, dgMatrix& matrix0, dgMatrix& matrix1) const +{ + dgFloat32 length; + dgAssert (m_body0); + dgAssert (m_body1); + dgAssert (dir.m_w == dgFloat32 (0.0f)); + const dgMatrix& body0_Matrix = m_body0->GetMatrix(); + + length = dir.DotProduct(dir).GetScalar(); + length = dgSqrt (length); + dgAssert (length > dgFloat32 (0.0f)); + matrix0 = dgMatrix (body0_Matrix.UnrotateVector (dir.Scale (dgFloat32 (1.0f) / length))); + matrix0.m_posit = body0_Matrix.UntransformVector (pivot); + + matrix0.m_front.m_w = dgFloat32 (0.0f); + matrix0.m_up.m_w = dgFloat32 (0.0f); + matrix0.m_right.m_w = dgFloat32 (0.0f); + matrix0.m_posit.m_w = dgFloat32 (1.0f); + + const dgMatrix& body1_Matrix = m_body1->GetMatrix(); + matrix1 = matrix0 * body0_Matrix * body1_Matrix.Inverse(); +} + + +void dgBilateralConstraint::SetPivotAndPinDir(const dgVector &pivot, const dgVector &pinDirection, dgMatrix& matrix0, dgMatrix& matrix1) const +{ + CalculateMatrixOffset (pivot, pinDirection, matrix0, matrix1); +} + +void dgBilateralConstraint::SetPivotAndPinDir (const dgVector& pivot, const dgVector& pinDirection0, const dgVector& pinDirection1, dgMatrix& matrix0, dgMatrix& matrix1) const +{ + dgAssert (m_body0); + dgAssert (m_body1); + + const dgMatrix& body0_Matrix = m_body0->GetMatrix(); + dgAssert (pinDirection0.m_w == dgFloat32 (0.0f)); + dgAssert ((pinDirection0.DotProduct(pinDirection0).GetScalar()) > dgFloat32 (0.0f)); + + matrix0.m_front = pinDirection0.Scale (dgRsqrt (pinDirection0.DotProduct(pinDirection0).GetScalar())); + matrix0.m_right = matrix0.m_front.CrossProduct(pinDirection1); + matrix0.m_right = matrix0.m_right.Scale (dgRsqrt (matrix0.m_right.DotProduct(matrix0.m_right).GetScalar())); + matrix0.m_up = matrix0.m_right.CrossProduct(matrix0.m_front); + matrix0.m_posit = pivot; + + matrix0.m_front.m_w = dgFloat32 (0.0f); + matrix0.m_up.m_w = dgFloat32 (0.0f); + matrix0.m_right.m_w = dgFloat32 (0.0f); + matrix0.m_posit.m_w = dgFloat32 (1.0f); + + const dgMatrix& body1_Matrix = m_body1->GetMatrix(); + + matrix1 = matrix0 * body1_Matrix.Inverse(); + matrix0 = matrix0 * body0_Matrix.Inverse(); +} + +dgVector dgBilateralConstraint::CalculateGlobalMatrixAndAngle (const dgMatrix& localMatrix0, const dgMatrix& localMatrix1, dgMatrix& globalMatrix0, dgMatrix& globalMatrix1) const +{ + dgAssert (m_body0); + dgAssert (m_body1); + const dgMatrix& body0Matrix = m_body0->GetMatrix(); + const dgMatrix& body1Matrix = m_body1->GetMatrix(); + + globalMatrix0 = localMatrix0 * body0Matrix; + globalMatrix1 = localMatrix1 * body1Matrix; + + dgMatrix relMatrix (globalMatrix1 * globalMatrix0.Inverse()); + + dgAssert (dgAbs (dgFloat32 (1.0f) - (relMatrix.m_front.DotProduct(relMatrix.m_front).GetScalar())) < 1.0e-5f); + dgAssert (dgAbs (dgFloat32 (1.0f) - (relMatrix.m_up.DotProduct(relMatrix.m_up).GetScalar())) < 1.0e-5f); + dgAssert (dgAbs (dgFloat32 (1.0f) - (relMatrix.m_right.DotProduct(relMatrix.m_right).GetScalar())) < 1.0e-5f); + + dgVector euler0; + dgVector euler1; + relMatrix.CalcPitchYawRoll (euler0, euler1); + return euler0; +} + +dgFloat32 dgBilateralConstraint::GetRowAcceleration (dgInt32 index, dgContraintDescritor& desc) const +{ + //return m_motorAcceleration[index]; + return desc.m_penetrationStiffness[index]; +} + +void dgBilateralConstraint::SetMotorAcceleration (dgInt32 index, dgFloat32 acceleration, dgContraintDescritor& desc) +{ + m_rowIsMotor |= (1 << index); + desc.m_flags[index] = 0; + m_motorAcceleration[index] = acceleration; + desc.m_jointAccel[index] = acceleration; + desc.m_penetrationStiffness[index] = acceleration; +} + +void dgBilateralConstraint::SetJacobianDerivative (dgInt32 index, dgContraintDescritor& desc, const dgFloat32* const jacobianA, const dgFloat32* const jacobianB, dgForceImpactPair* const jointForce) +{ + dgJacobian &jacobian0 = desc.m_jacobian[index].m_jacobianM0; + dgJacobian &jacobian1 = desc.m_jacobian[index].m_jacobianM1; + + m_r0[index] = dgVector::m_zero; + jacobian0.m_linear[0] = jacobianA[0]; + jacobian0.m_linear[1] = jacobianA[1]; + jacobian0.m_linear[2] = jacobianA[2]; + jacobian0.m_linear[3] = dgFloat32 (0.0f); + jacobian0.m_angular[0] = jacobianA[3]; + jacobian0.m_angular[1] = jacobianA[4]; + jacobian0.m_angular[2] = jacobianA[5]; + jacobian0.m_angular[3] = dgFloat32 (0.0f); + + m_r1[index] = dgVector::m_zero; + jacobian1.m_linear[0] = jacobianB[0]; + jacobian1.m_linear[1] = jacobianB[1]; + jacobian1.m_linear[2] = jacobianB[2]; + jacobian1.m_linear[3] = dgFloat32 (0.0f); + jacobian1.m_angular[0] = jacobianB[3]; + jacobian1.m_angular[1] = jacobianB[4]; + jacobian1.m_angular[2] = jacobianB[5]; + jacobian1.m_angular[3] = dgFloat32 (0.0f); + + m_rowIsMotor |= (1 << index); + m_motorAcceleration[index] = dgFloat32 (0.0f); + + desc.m_flags[index] = 0; + desc.m_restitution[index] = dgFloat32 (0.0f); + desc.m_jointAccel[index] = dgFloat32 (0.0f); + desc.m_penetration[index] = dgFloat32 (0.0f); + desc.m_penetrationStiffness[index] = dgFloat32 (0.0f); + desc.m_diagonalRegularizer[index] = m_defualtDiagonalRegularizer; + desc.m_forceBounds[index].m_jointForce = jointForce; +} + +void dgBilateralConstraint::SetSpringDamperAcceleration (dgInt32 index, dgContraintDescritor& desc, dgFloat32 rowStiffness, dgFloat32 spring, dgFloat32 damper) +{ + if (desc.m_timestep > dgFloat32 (0.0f)) { + + dgAssert (m_body1); + const dgJacobian &jacobian0 = desc.m_jacobian[index].m_jacobianM0; + const dgJacobian &jacobian1 = desc.m_jacobian[index].m_jacobianM1; + + const dgVector& veloc0 = m_body0->m_veloc; + const dgVector& omega0 = m_body0->m_omega; + const dgVector& veloc1 = m_body1->m_veloc; + const dgVector& omega1 = m_body1->m_omega; + + //dgFloat32 relPosit = (p1Global - p0Global) % jacobian0.m_linear + jointAngle; + dgFloat32 relPosit = desc.m_penetration[index]; + dgFloat32 relVeloc = - (veloc0.DotProduct(jacobian0.m_linear) + veloc1.DotProduct(jacobian1.m_linear) + omega0.DotProduct(jacobian0.m_angular) + omega1.DotProduct(jacobian1.m_angular)).GetScalar(); + + //at = [- ks (x2 - x1) - kd * (v2 - v1) - dt * ks * (v2 - v1)] / [1 + dt * kd + dt * dt * ks] + dgFloat32 dt = desc.m_timestep; + dgFloat32 ks = dgAbs (spring); + dgFloat32 kd = dgAbs (damper); + dgFloat32 ksd = dt * ks; + dgFloat32 num = ks * relPosit + kd * relVeloc + ksd * relVeloc; + dgFloat32 den = dt * kd + dt * ksd; + dgFloat32 accel = num / (dgFloat32 (1.0f) + den); + desc.m_diagonalRegularizer[index] = rowStiffness; + SetMotorAcceleration (index, accel, desc); + } +} + +dgFloat32 dgBilateralConstraint::CalculateMotorAcceleration (dgInt32 index, dgContraintDescritor& desc) const +{ + return desc.m_zeroRowAcceleration[index]; +} + +void dgBilateralConstraint::CalculateAngularDerivative (dgInt32 index, dgContraintDescritor& desc, const dgVector& dir, dgFloat32 stiffness, dgFloat32 jointAngle, dgForceImpactPair* const jointForce) +{ + dgAssert (jointForce); + dgAssert (m_body0); + dgAssert (dir.m_w == dgFloat32 (0.0f)); + + dgJacobian &jacobian0 = desc.m_jacobian[index].m_jacobianM0; + m_r0[index] = dgVector::m_zero; + jacobian0.m_linear = dgVector::m_zero; + jacobian0.m_angular = dir; + dgAssert(jacobian0.m_angular.m_w == dgFloat32(0.0f)); + + dgJacobian &jacobian1 = desc.m_jacobian[index].m_jacobianM1; + dgAssert (m_body1); + m_r1[index] = dgVector::m_zero; + jacobian1.m_linear = dgVector::m_zero; + jacobian1.m_angular = dir * dgVector::m_negOne; + dgAssert(jacobian1.m_angular.m_w == dgFloat32(0.0f)); + + const dgVector& omega0 = m_body0->GetOmega(); + const dgVector& omega1 = m_body1->GetOmega(); + const dgFloat32 relOmega = -(omega0 * jacobian0.m_angular + omega1 * jacobian1.m_angular).AddHorizontal().GetScalar(); + + m_rowIsMotor &= ~(1 << index); + m_motorAcceleration[index] = dgFloat32 (0.0f); + if (desc.m_timestep > dgFloat32 (0.0f)) { + #ifdef _DEBUG + const dgFloat32 relCentr = -(omega0 * omega0.CrossProduct(jacobian0.m_angular) + omega1 * omega1.CrossProduct(jacobian1.m_angular)).AddHorizontal().GetScalar(); + // allow for some large error since this is affected bu numerical precision a lot + dgAssert (dgAbs(relCentr) < dgFloat32 (4.0f)); + #endif + + const dgVector& gyroAlpha0 = m_body0->m_gyroAlpha; + const dgVector& gyroAlpha1 = m_body1->m_gyroAlpha; + const dgFloat32 relGyro = (jacobian0.m_angular * gyroAlpha0 + jacobian1.m_angular * gyroAlpha1).AddHorizontal().GetScalar(); + + //at = [- ks (x2 - x1) - kd * (v2 - v1) - dt * ks * (v2 - v1)] / [1 + dt * kd + dt * dt * ks] + dgFloat32 dt = desc.m_timestep; + dgFloat32 ks = DG_POS_DAMP; + dgFloat32 kd = DG_VEL_DAMP; + dgFloat32 ksd = dt * ks; + dgFloat32 num = ks * jointAngle + kd * relOmega + ksd * relOmega; + dgFloat32 den = dgFloat32 (1.0f) + dt * kd + dt * ksd; + dgFloat32 alphaError = num / den; + + desc.m_flags[index] = 0; + desc.m_penetration[index] = jointAngle; + desc.m_diagonalRegularizer[index] = stiffness; + desc.m_jointAccel[index] = alphaError + relGyro; + desc.m_penetrationStiffness[index] = alphaError + relGyro; + desc.m_restitution[index] = dgFloat32(0.0f); + desc.m_forceBounds[index].m_jointForce = jointForce; + desc.m_zeroRowAcceleration[index] = relOmega * desc.m_invTimestep + relGyro; + + } else { + desc.m_flags[index] = 0; + desc.m_penetration[index] = dgFloat32 (0.0f); + desc.m_restitution[index] = dgFloat32 (0.0f); + desc.m_diagonalRegularizer[index] = stiffness; + desc.m_jointAccel[index] = relOmega; + desc.m_penetrationStiffness[index] = relOmega;; + desc.m_zeroRowAcceleration[index] = dgFloat32 (0.0f); + desc.m_forceBounds[index].m_jointForce = jointForce; + } +} + +void dgBilateralConstraint::CalculatePointDerivative (dgInt32 index, dgContraintDescritor& desc, const dgVector& dir, const dgPointParam& param, dgForceImpactPair* const jointForce) +{ + dgAssert (jointForce); + dgAssert (m_body0); + dgAssert (m_body1); + dgAssert (dir.m_w == dgFloat32 (0.0f)); + + dgJacobian &jacobian0 = desc.m_jacobian[index].m_jacobianM0; + dgVector r0CrossDir (param.m_r0.CrossProduct(dir)); + m_r0[index] = param.m_r0; + jacobian0.m_linear = dir; + jacobian0.m_angular = r0CrossDir; + dgAssert(jacobian0.m_linear.m_w == dgFloat32(0.0f)); + dgAssert(jacobian0.m_angular.m_w == dgFloat32(0.0f)); + + dgJacobian &jacobian1 = desc.m_jacobian[index].m_jacobianM1; + dgVector r1CrossDir (dir.CrossProduct(param.m_r1)); + m_r1[index] = param.m_r1; + jacobian1.m_linear = dir * dgVector::m_negOne; + jacobian1.m_angular = r1CrossDir; + dgAssert(jacobian1.m_linear.m_w == dgFloat32(0.0f)); + dgAssert(jacobian1.m_angular.m_w == dgFloat32(0.0f)); + + m_rowIsMotor &= ~(1 << index); + m_motorAcceleration[index] = dgFloat32 (0.0f); + if (desc.m_timestep > dgFloat32 (0.0f)) { + dgVector positError (param.m_posit1 - param.m_posit0); + dgFloat32 relPosit = positError.DotProduct(dir).GetScalar(); + + const dgVector& veloc0 = m_body0->m_veloc; + const dgVector& veloc1 = m_body1->m_veloc; + const dgVector& omega0 = m_body0->m_omega; + const dgVector& omega1 = m_body1->m_omega; + const dgVector& gyroAlpha0 = m_body0->m_gyroAlpha; + const dgVector& gyroAlpha1 = m_body1->m_gyroAlpha; + const dgVector& centripetal0 (omega0.CrossProduct(omega0.CrossProduct(m_r0[index]))); + const dgVector& centripetal1 (omega1.CrossProduct(omega1.CrossProduct(m_r1[index]))); + + const dgFloat32 relGyro = (jacobian0.m_angular * gyroAlpha0 + jacobian1.m_angular * gyroAlpha1).AddHorizontal().GetScalar(); + const dgFloat32 relCentr = -(jacobian0.m_linear * centripetal0 + jacobian1.m_linear * centripetal1).AddHorizontal().GetScalar(); + const dgFloat32 relVeloc = -(jacobian0.m_linear * veloc0 + jacobian0.m_angular * omega0 + jacobian1.m_linear * veloc1 + jacobian1.m_angular * omega1).AddHorizontal().GetScalar(); + + //at = [- ks (x2 - x1) - kd * (v2 - v1) - dt * ks * (v2 - v1)] / [1 + dt * kd + dt * dt * ks] + const dgFloat32 dt = desc.m_timestep; + const dgFloat32 ks = DG_POS_DAMP; + const dgFloat32 kd = DG_VEL_DAMP; + const dgFloat32 ksd = dt * ks; + const dgFloat32 num = ks * relPosit + kd * relVeloc + ksd * relVeloc; + const dgFloat32 den = dgFloat32 (1.0f) + dt * kd + dt * ksd; + const dgFloat32 accelError = num / den; + + const dgFloat32 relAccel = accelError + relCentr + relGyro; + desc.m_flags[index] = 0; + desc.m_penetration[index] = relPosit; + desc.m_diagonalRegularizer[index] = param.m_defualtDiagonalRegularizer; + desc.m_jointAccel[index] = relAccel; + desc.m_penetrationStiffness[index] = relAccel; + desc.m_restitution[index] = dgFloat32 (0.0f); + desc.m_forceBounds[index].m_jointForce = jointForce; + desc.m_zeroRowAcceleration[index] = relVeloc * desc.m_invTimestep + relGyro; + + } else { + const dgVector& veloc0 = m_body0->m_veloc; + const dgVector& veloc1 = m_body1->m_veloc; + const dgVector& omega0 = m_body0->m_omega; + const dgVector& omega1 = m_body1->m_omega; + const dgFloat32 relVeloc = -(jacobian0.m_linear * veloc0 + jacobian0.m_angular * omega0 + jacobian1.m_linear * veloc1 + jacobian1.m_angular * omega1).AddHorizontal().GetScalar(); + + desc.m_flags[index] = 0; + desc.m_penetration[index] = dgFloat32 (0.0f); + desc.m_diagonalRegularizer[index] = param.m_defualtDiagonalRegularizer; + desc.m_jointAccel[index] = relVeloc; + desc.m_penetrationStiffness[index] = relVeloc; + desc.m_restitution[index] = dgFloat32 (0.0f); + desc.m_zeroRowAcceleration[index] = dgFloat32 (0.0f); + desc.m_forceBounds[index].m_jointForce = jointForce; + } +} + +void dgBilateralConstraint::JointAccelerations(dgJointAccelerationDecriptor* const params) +{ + const dgVector& bodyVeloc0 = m_body0->m_veloc; + const dgVector& bodyOmega0 = m_body0->m_omega; + const dgVector& bodyVeloc1 = m_body1->m_veloc; + const dgVector& bodyOmega1 = m_body1->m_omega; + const dgVector& gyroAlpha0 = m_body0->m_gyroAlpha; + const dgVector& gyroAlpha1 = m_body1->m_gyroAlpha; + + dgRightHandSide* const rhs = params->m_rightHandSide; + const dgLeftHandSide* const row = params->m_leftHandSide; + if (params->m_timeStep > dgFloat32 (0.0f)) { + const dgFloat32 ks = DG_POS_DAMP * dgFloat32 (0.5f); + const dgFloat32 kd = DG_VEL_DAMP * dgFloat32 (4.0f); + const dgFloat32 dt = params->m_timeStep; + for (dgInt32 k = 0; k < params->m_rowsCount; k ++) { + if (m_rowIsMotor & (1 << k)) { + rhs[k].m_coordenateAccel = m_motorAcceleration[k] + rhs[k].m_deltaAccel; + } else { + const dgJacobianPair& Jt = row[k].m_Jt; + + //calculate internal centripetal each sub step + const dgVector& centripetal0(bodyOmega0.CrossProduct(bodyOmega0.CrossProduct(m_r0[k]))); + const dgVector& centripetal1(bodyOmega1.CrossProduct(bodyOmega1.CrossProduct(m_r1[k]))); + + const dgVector relVeloc(Jt.m_jacobianM0.m_linear * bodyVeloc0 + Jt.m_jacobianM0.m_angular * bodyOmega0 + + Jt.m_jacobianM1.m_linear * bodyVeloc1 + Jt.m_jacobianM1.m_angular * bodyOmega1); + const dgFloat32 relGyro = (Jt.m_jacobianM0.m_angular * gyroAlpha0 + Jt.m_jacobianM1.m_angular * gyroAlpha1).AddHorizontal().GetScalar(); + const dgFloat32 relCentr = -(Jt.m_jacobianM0.m_linear * centripetal0 + Jt.m_jacobianM1.m_linear * centripetal1).AddHorizontal().GetScalar(); + + dgFloat32 vRel = relVeloc.AddHorizontal().GetScalar(); + + //at = [- ks (x2 - x1) - kd * (v2 - v1) - dt * ks * (v2 - v1)] / [1 + dt * kd + dt * dt * ks] + //alphaError = num / den; + //at = [- ks (x2 - x1) - kd * (v2 - v1) - dt * ks * (v2 - v1)] / [1 + dt * kd + dt * dt * ks] + //dgFloat32 dt = desc.m_timestep; + //dgFloat32 ks = DG_POS_DAMP; + //dgFloat32 kd = DG_VEL_DAMP; + //dgFloat32 ksd = dt * ks; + //dgFloat32 num = ks * relPosit + kd * relVeloc + ksd * relVeloc; + //dgFloat32 den = dgFloat32 (1.0f) + dt * kd + dt * ksd; + //accelError = num / den; + + dgFloat32 relPosit = rhs[k].m_penetration - vRel * dt * params->m_firstPassCoefFlag; + rhs[k].m_penetration = relPosit; + + dgFloat32 ksd = dt * ks; + dgFloat32 num = ks * relPosit - kd * vRel - ksd * vRel; + dgFloat32 den = dgFloat32(1.0f) + dt * kd + dt * ksd; + dgFloat32 aRelErr = num / den; + rhs[k].m_coordenateAccel = rhs[k].m_deltaAccel + aRelErr + relCentr + relGyro; + } + } + } else { + + for (dgInt32 k = 0; k < params->m_rowsCount; k ++) { + if (m_rowIsMotor & (1 << k)) { + rhs[k].m_coordenateAccel = m_motorAcceleration[k] + rhs[k].m_deltaAccel; + } else { + const dgJacobianPair& Jt = row[k].m_Jt; + dgVector relVeloc (Jt.m_jacobianM0.m_linear * bodyVeloc0 + Jt.m_jacobianM0.m_angular * bodyOmega0 + + Jt.m_jacobianM1.m_linear * bodyVeloc1 + Jt.m_jacobianM1.m_angular * bodyOmega1); + + dgFloat32 vRel = relVeloc.m_x + relVeloc.m_y + relVeloc.m_z; + rhs[k].m_coordenateAccel = rhs[k].m_deltaAccel - vRel; + } + } + } +} + diff --git a/thirdparty/src/newton/dgPhysics/dgBilateralConstraint.h b/thirdparty/src/newton/dgPhysics/dgBilateralConstraint.h new file mode 100644 index 000000000..d2ae8fee2 --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgBilateralConstraint.h @@ -0,0 +1,96 @@ +/* Copyright (c) <2003-2019> +* +* 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 __DGBILATERALCONSTRAINT_H__ +#define __DGBILATERALCONSTRAINT_H__ + +#include "dgConstraint.h" + +class dgBilateralConstraint; +#define DG_BILATERAL_CONTRAINT_DOF 8 + +class dgBilateralConstraintList: public dgList +{ + public: + dgBilateralConstraintList(dgMemoryAllocator* const allocator) + :dgList(allocator) + { + } +}; + +class dgBilateralConstraint: public dgConstraint +{ + public: + virtual void SetDestructorCallback (OnConstraintDestroy destructor); + virtual void Serialize (dgSerialize serializeCallback, void* const userData) = 0; + + bool IsRowMotor(dgInt32 index) const {return m_rowIsMotor & (1 << index) ? true : false; } + + protected: + dgBilateralConstraint (); + virtual ~dgBilateralConstraint (); + + virtual void Init (){dgAssert (0);} + virtual void Remove (dgWorld* world) {dgAssert (0);} + + virtual dgFloat32 GetStiffness() const; + virtual void SetStiffness(dgFloat32 stiffness); + + virtual dgInt32 GetSolverModel() const; + virtual void SetSolverModel(dgInt32 model); + + virtual dgFloat32 GetMassScaleBody0() const; + virtual dgFloat32 GetMassScaleBody1() const; + + void CalculateMatrixOffset (const dgVector& pivot, const dgVector& dir, dgMatrix& matrix0, dgMatrix& matrix1) const; + void SetPivotAndPinDir(const dgVector &pivot, const dgVector &pinDirection, dgMatrix& matrix0, dgMatrix& matrix1) const; + void SetPivotAndPinDir(const dgVector& pivot, const dgVector& pinDirection0, const dgVector& pinDirection1, dgMatrix& matrix0, dgMatrix& matrix1) const; + dgVector CalculateGlobalMatrixAndAngle (const dgMatrix& localMatrix0, const dgMatrix& localMatrix1, dgMatrix& globalMatrix0, dgMatrix& globalMatrix1) const; + + virtual void JointAccelerations(dgJointAccelerationDecriptor* const params); + + dgFloat32 GetRowAcceleration (dgInt32 index, dgContraintDescritor& desc) const; + dgFloat32 CalculateMotorAcceleration (dgInt32 index, dgContraintDescritor& desc) const; + void SetMotorAcceleration (dgInt32 index, dgFloat32 acceleration, dgContraintDescritor& desc); + void SetSpringDamperAcceleration (dgInt32 index, dgContraintDescritor& desc, dgFloat32 rowStiffness, dgFloat32 spring, dgFloat32 damper); + void SetJacobianDerivative (dgInt32 index, dgContraintDescritor& desc, const dgFloat32* const jacobianA, const dgFloat32* const jacobianB, dgForceImpactPair* const jointForce); + void CalculatePointDerivative (dgInt32 index, dgContraintDescritor& desc, const dgVector& normalGlobal, const dgPointParam& param, dgForceImpactPair* const jointForce); + void CalculateAngularDerivative (dgInt32 index, dgContraintDescritor& desc, const dgVector& normalGlobal, dgFloat32 stiffness, dgFloat32 jointAngle, dgForceImpactPair* const jointForce); + + void AppendToJointList(); + + dgVector m_r0[DG_BILATERAL_CONTRAINT_DOF]; + dgVector m_r1[DG_BILATERAL_CONTRAINT_DOF]; + dgForceImpactPair m_jointForce[DG_BILATERAL_CONTRAINT_DOF]; + dgFloat32 m_motorAcceleration[DG_BILATERAL_CONTRAINT_DOF]; + dgFloat32 m_massScaleBody0; + dgFloat32 m_massScaleBody1; + dgFloat32 m_defualtDiagonalRegularizer; + OnConstraintDestroy m_destructor; + dgBilateralConstraintList::dgListNode* m_jointNode; + dgInt8 m_rowIsMotor; + + friend class dgBodyMasterList; + friend class dgWorldDynamicUpdate; +}; + +#endif + diff --git a/thirdparty/src/newton/dgPhysics/dgBody.cpp b/thirdparty/src/newton/dgPhysics/dgBody.cpp new file mode 100644 index 000000000..c3e1aee73 --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgBody.cpp @@ -0,0 +1,727 @@ +/* Copyright (c) <2003-2019> +* +* 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 "dgPhysicsStdafx.h" +#include "dgBody.h" +#include "dgWorld.h" +#include "dgContact.h" +#include "dgCollision.h" +#include "dgCollisionInstance.h" +#include "dgCollisionCompound.h" +#include "dgWorldDynamicUpdate.h" +#include "dgCollisionDeformableMesh.h" +#include "dgCollisionCompoundFractured.h" + + +////////////////////////////////////////////////////////////////////// +// Construction/Destruction +////////////////////////////////////////////////////////////////////// + +dgBody::dgBody() + :m_matrix (dgGetIdentityMatrix()) + ,m_rotation() + ,m_invWorldInertiaMatrix(dgGetZeroMatrix()) + ,m_mass(dgFloat32 (DG_INFINITE_MASS * 2.0f), dgFloat32 (DG_INFINITE_MASS * 2.0f), dgFloat32 (DG_INFINITE_MASS * 2.0f), dgFloat32 (DG_INFINITE_MASS * 2.0f)) + ,m_invMass(dgVector::m_zero) + ,m_veloc(dgVector::m_zero) + ,m_omega(dgVector::m_zero) + ,m_accel(dgVector::m_zero) + ,m_alpha(dgVector::m_zero) + ,m_minAABB(dgVector::m_zero) + ,m_maxAABB(dgVector::m_zero) + ,m_localCentreOfMass(dgVector::m_zero) + ,m_globalCentreOfMass(dgVector::m_zero) + ,m_impulseForce(dgVector::m_zero) + ,m_impulseTorque(dgVector::m_zero) + ,m_gyroAlpha(dgVector::m_zero) + ,m_gyroTorque(dgVector::m_zero) + ,m_gyroRotation() + ,m_criticalSectionLock(0) + ,m_flags(0) + ,m_userData(NULL) + ,m_world(NULL) + ,m_collision(NULL) + ,m_broadPhaseNode(NULL) + ,m_masterNode(NULL) + ,m_broadPhaseaggregateNode(NULL) + ,m_destructor(NULL) + ,m_matrixUpdate(NULL) + ,m_index(0) + ,m_uniqueID(0) + ,m_bodyGroupId(0) + ,m_rtti(m_baseBodyRTTI) + ,m_type(0) + ,m_serializedEnum(-1) + ,m_dynamicsLru(0) + ,m_genericLRUMark(0) +{ + m_autoSleep = true; + m_collidable = true; +// m_gyroTorqueOn = true; + m_transformIsDirty = true; + m_collideWithLinkedBodies = true; + m_invWorldInertiaMatrix[3][3] = dgFloat32 (1.0f); + InitJointSet(); +} + +dgBody::dgBody (dgWorld* const world, const dgTree* const collisionCashe, dgDeserialize serializeCallback, void* const userData, dgInt32 revisionNumber) + :m_matrix (dgGetIdentityMatrix()) + ,m_rotation() + ,m_invWorldInertiaMatrix(dgGetZeroMatrix()) + ,m_mass(dgFloat32 (DG_INFINITE_MASS * 2.0f), dgFloat32 (DG_INFINITE_MASS * 2.0f), dgFloat32 (DG_INFINITE_MASS * 2.0f), dgFloat32 (DG_INFINITE_MASS * 2.0f)) + ,m_invMass(dgVector::m_zero) + ,m_veloc(dgVector::m_zero) + ,m_omega(dgVector::m_zero) + ,m_accel(dgVector::m_zero) + ,m_alpha(dgVector::m_zero) + ,m_minAABB(dgVector::m_zero) + ,m_maxAABB(dgVector::m_zero) + ,m_localCentreOfMass(dgVector::m_zero) + ,m_globalCentreOfMass(dgVector::m_zero) + ,m_impulseForce(dgVector::m_zero) + ,m_impulseTorque(dgVector::m_zero) + ,m_gyroAlpha(dgVector::m_zero) + ,m_gyroTorque(dgVector::m_zero) + ,m_gyroRotation() + ,m_criticalSectionLock(0) + ,m_flags(0) + ,m_userData(NULL) + ,m_world(world) + ,m_collision(NULL) + ,m_broadPhaseNode(NULL) + ,m_masterNode(NULL) + ,m_broadPhaseaggregateNode(NULL) + ,m_destructor(NULL) + ,m_matrixUpdate(NULL) + ,m_index(0) + ,m_uniqueID(0) + ,m_bodyGroupId(0) + ,m_rtti(m_baseBodyRTTI) + ,m_type(0) + ,m_serializedEnum(-1) + ,m_dynamicsLru(0) + ,m_genericLRUMark(0) +{ + m_autoSleep = true; + m_collidable = true; + m_transformIsDirty = true; + m_collideWithLinkedBodies = true; + m_invWorldInertiaMatrix[3][3] = dgFloat32 (1.0f); + + serializeCallback (userData, &m_rotation, sizeof (m_rotation)); + serializeCallback (userData, &m_matrix, sizeof (m_matrix)); + serializeCallback (userData, &m_veloc, sizeof (m_veloc)); + serializeCallback (userData, &m_omega, sizeof (m_omega)); + serializeCallback (userData, &m_accel, sizeof (m_veloc)); + serializeCallback (userData, &m_alpha, sizeof (m_omega)); + serializeCallback (userData, &m_localCentreOfMass, sizeof (m_localCentreOfMass)); + serializeCallback (userData, &m_mass, sizeof (m_mass)); + serializeCallback (userData, &m_flags, sizeof (m_flags)); + serializeCallback (userData, &m_serializedEnum, sizeof(dgInt32)); + + dgInt32 id; + serializeCallback (userData, &id, sizeof (id)); + + dgTree::dgTreeNode* const node = collisionCashe->Find(id); + dgAssert (node); + + const dgCollision* const collision = node->GetInfo(); + collision->AddRef(); + + dgCollisionInstance* const instance = new (world->GetAllocator()) dgCollisionInstance (world, serializeCallback, userData, revisionNumber); + instance->m_childShape = collision; + m_collision = instance; + + InitJointSet(); +} + +dgBody::~dgBody() +{ +} + +dgVector dgBody::GetAlpha() const +{ + return dgVector::m_zero; +} +dgVector dgBody::GetAccel() const +{ + return dgVector::m_zero; +} + +void dgBody::SetAlpha(const dgVector& alpha) +{ +} + +void dgBody::SetAccel(const dgVector& accel) +{ +} + +void dgBody::AttachCollision (dgCollisionInstance* const collisionSrc) +{ + dgCollisionInstance* const instance = new (m_world->GetAllocator()) dgCollisionInstance (*collisionSrc); + m_world->GetBroadPhase()->CollisionChange (this, instance); + if (m_collision) { + m_collision->Release(); + } + m_collision = instance; + m_equilibrium = 0; +} + +void dgBody::Serialize (const dgTree& collisionRemapId, dgSerialize serializeCallback, void* const userData) +{ + serializeCallback (userData, &m_rotation, sizeof (m_rotation)); + serializeCallback (userData, &m_matrix, sizeof (m_matrix)); + serializeCallback (userData, &m_veloc, sizeof (m_veloc)); + serializeCallback (userData, &m_omega, sizeof (m_omega)); + serializeCallback (userData, &m_accel, sizeof (m_veloc)); + serializeCallback (userData, &m_alpha, sizeof (m_omega)); + serializeCallback (userData, &m_localCentreOfMass, sizeof (m_localCentreOfMass)); + serializeCallback(userData, &m_mass, sizeof (m_mass)); + serializeCallback (userData, &m_flags, sizeof (m_flags)); + serializeCallback(userData, &m_serializedEnum, sizeof(dgInt32)); + + dgTree::dgTreeNode* const node = collisionRemapId.Find(m_collision->GetChildShape()); + dgAssert (node); + + dgInt32 id = node->GetInfo(); + serializeCallback (userData, &id, sizeof (id)); + m_collision->Serialize(serializeCallback, userData, false); +} + +void dgBody::UpdateLumpedMatrix() +{ + if (m_collision->IsType(dgCollision::dgCollisionLumpedMass_RTTI)) { + dgAssert(IsRTTIType(dgBody::m_dynamicBodyRTTI)); + dgCollisionLumpedMassParticles* const lumpedMass = (dgCollisionLumpedMassParticles*)m_collision->m_childShape; + lumpedMass->SetOwnerAndMassPraperties((dgDynamicBody*) this); + } +} + + +void dgBody::SetMatrix(const dgMatrix& matrix) +{ + SetMatrixOriginAndRotation(matrix); + + if (!m_inCallback) { + UpdateCollisionMatrix (dgFloat32 (0.0f), 0); + } +} + +void dgBody::SetMatrixNoSleep(const dgMatrix& matrix) +{ + SetMatrix(matrix); +} + +void dgBody::SetMatrixResetSleep(const dgMatrix& matrix) +{ + m_sleeping = 0; + m_equilibrium = 0; + SetMatrix(matrix); +} + +void dgBody::UpdateWorlCollisionMatrix() const +{ + m_collision->SetGlobalMatrix (m_collision->GetLocalMatrix() * m_matrix); +} + +void dgBody::UpdateCollisionMatrix (dgFloat32 timestep, dgInt32 threadIndex) +{ + m_transformIsDirty = true; + m_collision->SetGlobalMatrix (m_collision->GetLocalMatrix() * m_matrix); + m_collision->CalcAABB (m_collision->GetGlobalMatrix(), m_minAABB, m_maxAABB); + + if (m_continueCollisionMode) { + dgVector predictiveVeloc (PredictLinearVelocity(timestep)); + dgVector predictiveOmega (PredictAngularVelocity(timestep)); + dgMovingAABB (m_minAABB, m_maxAABB, predictiveVeloc, predictiveOmega, timestep, m_collision->GetBoxMaxRadius(), m_collision->GetBoxMinRadius()); + } + + if (m_broadPhaseNode) { + dgAssert (m_world); + if (!m_equilibrium) { + m_world->GetBroadPhase()->UpdateBody (this, threadIndex); + } + } +} + + +dgFloat32 dgBody::RayCast (const dgLineBox& line, OnRayCastAction filter, OnRayPrecastAction preFilter, void* const userData, dgFloat32 maxT) const +{ + dgAssert (filter); + dgVector l0 (line.m_l0); + dgVector l1 (line.m_l0 + (line.m_l1 - line.m_l0).Scale (dgMin(maxT, dgFloat32 (1.0f)))); + if (dgRayBoxClip (l0, l1, m_minAABB, m_maxAABB)) { +// if (1) { +//l0 = dgVector (-20.3125000f, 3.54991579f, 34.3441200f, 0.0f); +//l1 = dgVector (-19.6875000f, 3.54257250f, 35.2211456f, 0.0f); + + dgContactPoint contactOut; + const dgMatrix& globalMatrix = m_collision->GetGlobalMatrix(); + dgVector localP0 (globalMatrix.UntransformVector (l0)); + dgVector localP1 (globalMatrix.UntransformVector (l1)); + dgVector p1p0 (localP1 - localP0); + dgAssert (p1p0.m_w == dgFloat32 (0.0f)); + if (p1p0.DotProduct(p1p0).GetScalar() > dgFloat32 (1.0e-12f)) { + dgFloat32 t = m_collision->RayCast (localP0, localP1, dgFloat32 (1.0f), contactOut, preFilter, this, userData); + if (t < dgFloat32 (1.0f)) { + dgAssert (localP0.m_w == dgFloat32 (0.0f)); + dgAssert (localP1.m_w == dgFloat32 (0.0f)); + dgVector p (globalMatrix.TransformVector(localP0 + (localP1 - localP0).Scale(t))); + dgVector l1l0 (line.m_l1 - line.m_l0); + dgAssert (l1l0.m_w == dgFloat32 (0.0f)); + t = l1l0.DotProduct(p - line.m_l0).GetScalar() / l1l0.DotProduct(l1l0).GetScalar(); + if (t < maxT) { + dgAssert (t >= dgFloat32 (0.0f)); + dgAssert (t <= dgFloat32 (1.0f)); + contactOut.m_normal = globalMatrix.RotateVector (contactOut.m_normal); + maxT = filter (this, contactOut.m_collision0, p, contactOut.m_normal, contactOut.m_shapeId0, userData, t); + } + } + } + } + return maxT; +} + +void dgBody::IntegrateVelocity (dgFloat32 timestep) +{ +//if (m_uniqueID == 3) +//dgTrace(("%d v(%f %f %f)\n", m_uniqueID, m_veloc.m_x, m_veloc.m_y, m_veloc.m_z)); + + dgAssert (m_veloc.m_w == dgFloat32 (0.0f)); + dgAssert (m_omega.m_w == dgFloat32 (0.0f)); + m_globalCentreOfMass += m_veloc.Scale (timestep); + dgFloat32 omegaMag2 = m_omega.DotProduct(m_omega).GetScalar(); +#ifdef _DEBUG + const dgFloat32 err = dgFloat32(90.0f * dgDegreeToRad); + const dgFloat32 err2 = err * err; + const dgFloat32 step2 = omegaMag2 * timestep * timestep; + const dgFloat32 speed2 = m_veloc.DotProduct(m_veloc).GetScalar() * timestep * timestep;; + if ((step2 > err2) || (speed2 > 100.0f)) { + dgTrace (("warning bodies %d w(%f %f %f) v(%f %f %f) with very high velocity or angular velocity, may be unstable\n", m_uniqueID, + m_omega.m_x, m_omega.m_y, m_omega.m_z, m_veloc.m_x, m_veloc.m_y, m_veloc.m_z)); + //dgAssert(0); + } +#endif + + // this is correct + if (omegaMag2 > ((dgFloat32 (0.0125f) * dgDegreeToRad) * (dgFloat32 (0.0125f) * dgDegreeToRad))) { + dgFloat32 invOmegaMag = dgRsqrt (omegaMag2); + dgVector omegaAxis (m_omega.Scale (invOmegaMag)); + dgFloat32 omegaAngle = invOmegaMag * omegaMag2 * timestep; + dgQuaternion rotation (omegaAxis, omegaAngle); + m_rotation = m_rotation * rotation; + m_rotation.Scale(dgRsqrt (m_rotation.DotProduct (m_rotation))); + m_matrix = dgMatrix (m_rotation, m_matrix.m_posit); + } + + m_matrix.m_posit = m_globalCentreOfMass - m_matrix.RotateVector(m_localCentreOfMass); + dgAssert (m_matrix.TestOrthogonal()); +} + +dgConstraint* dgBody::GetFirstJoint() const +{ + if (m_masterNode) { + for (dgBodyMasterListRow::dgListNode* node = m_masterNode->GetInfo().GetFirst(); node; node = node->GetNext()) { + dgConstraint* const joint = node->GetInfo().m_joint; + if (joint && (joint->GetId() >= dgConstraint::m_unknownConstraint)) { + return joint; + } + } + } + return NULL; +} + +dgConstraint* dgBody::GetNextJoint(dgConstraint* const joint) const +{ + dgBodyMasterListRow::dgListNode* node = joint->GetLink0(); + if (joint->GetBody0() != this) { + node = joint->GetLink1(); + } + + if (node->GetInfo().m_joint == joint) { + for (node = node->GetNext(); node; node = node->GetNext()) { + dgConstraint* const joint1 = node->GetInfo().m_joint; + if (joint1->GetId() >= dgConstraint::m_unknownConstraint) { + return joint1; + } + } + } + + return NULL; +} + + +dgConstraint* dgBody::GetFirstContact() const +{ + if (m_masterNode) { + for (dgBodyMasterListRow::dgListNode* node = m_masterNode->GetInfo().GetFirst(); node; node = node->GetNext()) { + dgConstraint* const joint = node->GetInfo().m_joint; + dgAssert (joint); + if (joint && (joint->GetId() == dgConstraint::m_contactConstraint)) { + dgContact* const contactJoint = (dgContact*) joint; + if (contactJoint->m_isActive) { + return joint; + } + } + } + } + return NULL; +} + +dgConstraint* dgBody::GetNextContact(dgConstraint* const joint) const +{ + dgBodyMasterListRow::dgListNode* node = joint->GetLink0(); + if (joint->GetBody0() != this) { + node = joint->GetLink1(); + } + + if (node->GetInfo().m_joint == joint) { + for (node = node->GetNext(); node; node = node->GetNext()) { + dgConstraint* const joint1 = node->GetInfo().m_joint; + dgAssert (joint1); + if (joint1 && (joint1->GetId() == dgConstraint::m_contactConstraint)) { + dgContact* const contactJoint = (dgContact*) joint1; + if (contactJoint->m_isActive) { + return joint1; + } + } + } + } + + return NULL; +} + +void dgBody::SetFreeze (bool state) +{ + if (state){ + Freeze(); + } else { + Unfreeze(); + } +} + + +void dgBody::Freeze () +{ + if (GetInvMass().m_w > dgFloat32 (0.0f)) { + if (!m_freeze) { + m_freeze = true; + for (dgBodyMasterListRow::dgListNode* node = m_masterNode->GetInfo().GetFirst(); node; node = node->GetNext()) { + dgBody* const body = node->GetInfo().m_bodyNode; + body->Freeze (); + } + } + } +} + +void dgBody::Unfreeze () +{ + if (GetInvMass().m_w > dgFloat32 (0.0f)) { + if (m_freeze) { + m_freeze = false; + for (dgBodyMasterListRow::dgListNode* node = m_masterNode->GetInfo().GetFirst(); node; node = node->GetNext()) { + dgBody* const body = node->GetInfo().m_bodyNode; + body->Unfreeze (); + } + } + } +} + +void dgBody::SetMassMatrix(dgFloat32 mass, const dgMatrix& inertia) +{ + dgFloat32 Ixx = inertia[0][0]; + dgFloat32 Iyy = inertia[1][1]; + dgFloat32 Izz = inertia[2][2]; + mass = dgAbs (mass); + if (m_collision->IsType(dgCollision::dgCollisionMesh_RTTI) || m_collision->IsType(dgCollision::dgCollisionScene_RTTI)) { + mass = DG_INFINITE_MASS * 2.0f; + } + + if (m_collision->IsType(dgCollision::dgCollisionCompound_RTTI)) { + const dgCollision* const childShape = m_collision->GetChildShape(); + if ((childShape->m_inertia.m_x < dgFloat32 (1.0e-5f)) || (childShape->m_inertia.m_y < dgFloat32 (1.0e-5f)) || (childShape->m_inertia.m_z < dgFloat32 (1.0e-5f))){ + mass = DG_INFINITE_MASS * 2.0f; + } + } + + if (mass < DG_MINIMUM_MASS) { + mass = DG_INFINITE_MASS * 2.0f; + } + + //dgAssert (m_masterNode); + m_world->GetBroadPhase()->CheckStaticDynamic(this, mass); + + if (mass >= DG_INFINITE_MASS) { + if (m_masterNode) { + if (m_invMass.m_w != dgFloat32 (0.0f)) { + dgBodyMasterList& masterList (*m_world); + if (masterList.GetFirst() != m_masterNode) { + masterList.InsertAfter (masterList.GetFirst(), m_masterNode); + } + } + } + + m_mass.m_x = DG_INFINITE_MASS; + m_mass.m_y = DG_INFINITE_MASS; + m_mass.m_z = DG_INFINITE_MASS; + m_mass.m_w = DG_INFINITE_MASS; + m_invMass = dgVector::m_zero; + + } else { + Ixx = dgAbs (Ixx); + Iyy = dgAbs (Iyy); + Izz = dgAbs (Izz); + + dgFloat32 Ixx1 = dgClamp (Ixx, dgFloat32 (0.001f) * mass, dgFloat32 (1000.0f) * mass); + dgFloat32 Iyy1 = dgClamp (Iyy, dgFloat32 (0.001f) * mass, dgFloat32 (1000.0f) * mass); + dgFloat32 Izz1 = dgClamp (Izz, dgFloat32 (0.001f) * mass, dgFloat32 (1000.0f) * mass); + + dgAssert (Ixx > dgFloat32 (0.0f)); + dgAssert (Iyy > dgFloat32 (0.0f)); + dgAssert (Izz > dgFloat32 (0.0f)); + + if (m_masterNode) { + if (m_invMass.m_w == dgFloat32 (0.0f)) { + dgBodyMasterList& masterList(*m_world); + masterList.RotateToEnd(m_masterNode); + } + } + + m_mass.m_x = Ixx1; + m_mass.m_y = Iyy1; + m_mass.m_z = Izz1; + m_mass.m_w = mass; + + m_invMass.m_x = dgFloat32 (1.0f) / Ixx1; + m_invMass.m_y = dgFloat32 (1.0f) / Iyy1; + m_invMass.m_z = dgFloat32 (1.0f) / Izz1; + m_invMass.m_w = dgFloat32 (1.0f) / mass; + } + + +//#ifdef _DEBUG +#if 0 + dgBodyMasterList& me = *m_world; + for (dgBodyMasterList::dgListNode* refNode = me.GetFirst(); refNode; refNode = refNode->GetNext()) { + dgBody* const body0 = refNode->GetInfo().GetBody(); + dgVector invMass (body0->GetInvMass()); + if (invMass.m_w != 0.0f) { + for (; refNode; refNode = refNode->GetNext()) { + dgBody* const body1 = refNode->GetInfo().GetBody(); + dgVector invMass1 (body1->GetInvMass()); + dgAssert (invMass1.m_w != 0.0f); + } + break; + } + } +#endif +// UpdateLumpedMatrix(); +} + +void dgBody::SetMassProperties (dgFloat32 mass, const dgCollisionInstance* const collision) +{ + // using general central theorem, to extract the Inertia relative to the center of mass + dgMatrix inertia (collision->CalculateInertia()); + + dgVector origin (inertia.m_posit); + for (dgInt32 i = 0; i < 3; i ++) { + inertia[i] = inertia[i].Scale (mass); + //inertia[i][i] = (inertia[i][i] + origin[i] * origin[i]) * mass; + //for (dgInt32 j = i + 1; j < 3; j ++) { + // dgFloat32 crossIJ = origin[i] * origin[j]; + // inertia[i][j] = (inertia[i][j] + crossIJ) * mass; + // inertia[j][i] = (inertia[j][i] + crossIJ) * mass; + //} + } + + // although the engine fully supports asymmetric inertia, I will ignore cross inertia for now + //SetMassMatrix(mass, inertia[0][0], inertia[1][1], inertia[2][2]); + SetCentreOfMass(origin); + SetMassMatrix(mass, inertia); +} + +dgMatrix dgBody::CalculateLocalInertiaMatrix () const +{ + dgMatrix inertia (dgGetIdentityMatrix()); + inertia[0][0] = m_mass.m_x; + inertia[1][1] = m_mass.m_y; + inertia[2][2] = m_mass.m_z; + return inertia; +} + +dgMatrix dgBody::CalculateInertiaMatrix () const +{ + const dgVector Ixx(m_mass[0]); + const dgVector Iyy(m_mass[1]); + const dgVector Izz(m_mass[2]); + return dgMatrix (m_matrix.m_front.Scale(m_matrix.m_front[0]) * Ixx + + m_matrix.m_up.Scale(m_matrix.m_up[0]) * Iyy + + m_matrix.m_right.Scale(m_matrix.m_right[0]) * Izz, + + m_matrix.m_front.Scale(m_matrix.m_front[1]) * Ixx + + m_matrix.m_up.Scale(m_matrix.m_up[1]) * Iyy + + m_matrix.m_right.Scale(m_matrix.m_right[1]) * Izz, + + m_matrix.m_front.Scale(m_matrix.m_front[2]) * Ixx + + m_matrix.m_up.Scale(m_matrix.m_up[2]) * Iyy + + m_matrix.m_right.Scale(m_matrix.m_right[2]) * Izz, + dgVector::m_wOne); +} + +dgMatrix dgBody::CalculateInvInertiaMatrix () const +{ + const dgVector invIxx(m_invMass[0]); + const dgVector invIyy(m_invMass[1]); + const dgVector invIzz(m_invMass[2]); + return dgMatrix(m_matrix.m_front.Scale(m_matrix.m_front[0]) * invIxx + + m_matrix.m_up.Scale(m_matrix.m_up[0]) * invIyy + + m_matrix.m_right.Scale(m_matrix.m_right[0]) * invIzz, + + m_matrix.m_front.Scale(m_matrix.m_front[1]) * invIxx + + m_matrix.m_up.Scale(m_matrix.m_up[1]) * invIyy + + m_matrix.m_right.Scale(m_matrix.m_right[1]) * invIzz, + + m_matrix.m_front.Scale(m_matrix.m_front[2]) * invIxx + + m_matrix.m_up.Scale(m_matrix.m_up[2]) * invIyy + + m_matrix.m_right.Scale(m_matrix.m_right[2]) * invIzz, + dgVector::m_wOne); +} + +void dgBody::InvalidateCache () +{ + m_sleeping = false; + m_equilibrium = false; + m_genericLRUMark = 0; + dgMatrix matrix (m_matrix); + SetMatrixOriginAndRotation(matrix); +} + +void dgBody::AddImpulse (const dgVector& pointDeltaVeloc, const dgVector& pointPosit, dgFloat32 timestep) +{ + dgMatrix invInertia (CalculateInvInertiaMatrix()); + + // get contact matrix + dgMatrix tmp; + dgVector globalContact (pointPosit - m_globalCentreOfMass); + + tmp[0][0] = dgFloat32 (0.0f); + tmp[0][1] = + globalContact[2]; + tmp[0][2] = - globalContact[1]; + tmp[0][3] = dgFloat32 (0.0f); + + tmp[1][0] = -globalContact[2]; + tmp[1][1] = dgFloat32 (0.0f); + tmp[1][2] = +globalContact[0]; + tmp[1][3] = dgFloat32 (0.0f); + + tmp[2][0] = +globalContact[1]; + tmp[2][1] = -globalContact[0]; + tmp[2][2] = dgFloat32 (0.0f); + tmp[2][3] = dgFloat32 (0.0f); + + tmp[3][0] = dgFloat32 (0.0f); + tmp[3][1] = dgFloat32 (0.0f); + tmp[3][2] = dgFloat32 (0.0f); + tmp[3][3] = dgFloat32 (1.0f); + + dgMatrix contactMatrix (tmp * invInertia * tmp); + for (dgInt32 i = 0; i < 3; i ++) { + for (dgInt32 j = 0; j < 3; j ++) { + contactMatrix[i][j] *= -dgFloat32 (1.0f); + } + } + contactMatrix[0][0] += m_invMass.m_w; + contactMatrix[1][1] += m_invMass.m_w; + contactMatrix[2][2] += m_invMass.m_w; + + contactMatrix = contactMatrix.Inverse4x4 (); + + // change of momentum + dgVector changeOfMomentum (contactMatrix.RotateVector (pointDeltaVeloc)); + + if (changeOfMomentum.DotProduct(changeOfMomentum).GetScalar() > dgFloat32(1.0e-6f)) { + m_impulseForce += changeOfMomentum.Scale(1.0f / timestep); + m_impulseTorque += globalContact.CrossProduct(m_impulseForce); + + m_sleeping = false; + m_equilibrium = false; + Unfreeze(); + } +} + +void dgBody::ApplyImpulsePair (const dgVector& linearImpulseIn, const dgVector& angularImpulseIn, dgFloat32 timestep) +{ + dgAssert(linearImpulseIn.m_w == dgFloat32(0.0f)); + dgAssert(angularImpulseIn.m_w == dgFloat32(0.0f)); + if ((linearImpulseIn.DotProduct(linearImpulseIn).GetScalar() > dgFloat32(1.0e-6f)) || + (angularImpulseIn.DotProduct(angularImpulseIn).GetScalar() > dgFloat32(1.0e-6f))) { + + m_impulseForce += linearImpulseIn.Scale(1.0f / timestep); + m_impulseTorque += angularImpulseIn.Scale(1.0f / timestep); + + m_sleeping = false; + m_equilibrium = false; + Unfreeze(); + } +} + +void dgBody::ApplyImpulsesAtPoint (dgInt32 count, dgInt32 strideInBytes, const dgFloat32* const impulseArray, const dgFloat32* const pointArray, dgFloat32 timestep) +{ + dgInt32 stride = strideInBytes / sizeof (dgFloat32); + + dgVector impulse (dgVector::m_zero); + dgVector angularImpulse (dgVector::m_zero); + + dgVector com (m_globalCentreOfMass); + for (dgInt32 i = 0; i < count; i ++) { + dgInt32 index = i * stride; + dgVector r (pointArray[index], pointArray[index + 1], pointArray[index + 2], dgFloat32 (0.0f)); + dgVector L (impulseArray[index], impulseArray[index + 1], impulseArray[index + 2], dgFloat32 (0.0f)); + dgVector Q ((r - com).CrossProduct(L)); + + impulse += L; + angularImpulse += Q; + } + + if ((impulse.DotProduct(impulse).GetScalar() > dgFloat32(1.0e-6f)) || + (angularImpulse.DotProduct(angularImpulse).GetScalar() > dgFloat32(1.0e-6f))) { + m_impulseForce += impulse.Scale(1.0f / timestep); + m_impulseTorque += angularImpulse.Scale(1.0f / timestep); + + m_sleeping = false; + m_equilibrium = false; + Unfreeze(); + } +} + +void dgBody::SetSleepState(bool state) +{ + m_sleeping = state; + m_equilibrium = state; + if ((m_invMass.m_w > dgFloat32 (0.0f)) && (m_veloc.DotProduct(m_veloc).GetScalar() < dgFloat32(1.0e-10f)) && (m_omega.DotProduct(m_omega).GetScalar() < dgFloat32(1.0e-10f))) { + m_equilibrium = state; + for (dgConstraint* contact = GetFirstContact(); contact; contact = GetNextContact(contact)) { + dgAssert(contact->GetId() == dgConstraint::m_contactConstraint); + dgContact* const contactJoint = (dgContact*)contact; + contactJoint->m_positAcc = dgVector(dgFloat32(10.0f)); + } + } +} diff --git a/thirdparty/src/newton/dgPhysics/dgBody.h b/thirdparty/src/newton/dgPhysics/dgBody.h new file mode 100644 index 000000000..08c92c21e --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgBody.h @@ -0,0 +1,672 @@ +/* Copyright (c) <2003-2019> +* +* 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_BODY_H_ +#define _DG_BODY_H_ + +#include "dgPhysicsStdafx.h" +#include "dgCollision.h" +#include "dgBodyMasterList.h" + +class dgLink; +class dgBody; +class dgWorld; +class dgCollision; +class dgBroadPhaseNode; +class dgSkeletonContainer; +class dgCollisionInstance; +class dgBroadPhaseBodyNode; +class dgBroadPhaseAggregate; + + +#define DG_MINIMUM_MASS dgFloat32(1.0e-5f) +#define DG_INFINITE_MASS dgFloat32(1.0e15f) + +#define OverlapTest(body0,body1) dgOverlapTest ((body0)->m_minAABB, (body0)->m_maxAABB, (body1)->m_minAABB, (body1)->m_maxAABB) + + + +DG_MSC_VECTOR_ALIGNMENT +struct dgLineBox +{ + dgVector m_l0; + dgVector m_l1; + dgVector m_boxL0; + dgVector m_boxL1; +} DG_GCC_VECTOR_ALIGNMENT; + + +DG_MSC_VECTOR_ALIGNMENT +class dgBody +{ + public: + typedef void (dgApi *OnBodyDestroy) (dgBody& me); + typedef void (dgApi *OnApplyExtForceAndTorque) (dgBody& me, dgFloat32 timestep, dgInt32 threadIndex); + typedef void (dgApi *OnMatrixUpdateCallback) (const dgBody& me, const dgMatrix& matrix, dgInt32 threadIndex); + + enum dgRTTI + { + m_baseBodyRTTI = 1<<0, + m_dynamicBodyRTTI = 1<<1, + m_kinematicBodyRTTI = 1<<2, + m_dynamicBodyAsymentricRTTI = 1 << 3, + //m_deformableBodyRTTI = 1<<3, + }; + + enum dgType + { + m_dynamicBody = 0, + m_kinematicBody, + m_dynamicBodyAsymatric, + //m_deformableBody, + }; + + class dgSetInfo + { + public: + DG_INLINE dgSetInfo() + { + } + + DG_INLINE void Init(dgBody* const self) + { + m_parent = self; + m_rank = 0; + m_bodyCount = 1; + m_jointCount = 0; + m_rowCount = 0; + } + + dgBody* m_parent; + dgInt32 m_rank; + dgInt32 m_bodyCount; + dgInt32 m_jointCount; + dgInt32 m_rowCount; + }; + + DG_CLASS_ALLOCATOR(allocator) + + dgBody(); + dgBody (dgWorld* const world, const dgTree* const collisionNode, dgDeserialize serializeCallback, void* const userData, dgInt32 revisionNumber); + virtual ~dgBody(); + + dgType GetType () const; + dgInt32 IsRTTIType (dgUnsigned32 rtti) const; + + void* GetUserData() const; + void SetUserData (void* const userData); + dgWorld* GetWorld() const; + + const dgMatrix& GetMatrix() const; + const dgQuaternion& GetRotation() const; + const dgVector& GetPosition() const; + + void SetCentreOfMass (const dgVector& com); + const dgVector& GetCentreOfMass () const; + + void GetAABB (dgVector &p0, dgVector &p1) const; + + const dgVector& GetOmega() const; + const dgVector& GetVelocity() const; + + dgVector GetVelocityAtPoint (const dgVector& point) const; + + void SetOmega (const dgVector& omega); + void SetVelocity (const dgVector& velocity); + + void SetOmegaNoSleep(const dgVector& omega); + void SetVelocityNoSleep(const dgVector& velocity); + + dgUnsigned32 GetGroupID () const; + virtual void SetGroupID (dgUnsigned32 id); + dgInt32 GetUniqueID () const; + + bool GetContinueCollisionMode () const; + void SetContinueCollisionMode (bool mode); + bool GetCollisionWithLinkedBodies () const; + void SetCollisionWithLinkedBodies (bool state); + + void Freeze (); + void Unfreeze (); + bool GetFreeze () const; + void SetFreeze (bool state); + + bool GetSleepState () const; + void SetSleepState (bool state); + + bool GetAutoSleep () const; + void SetAutoSleep (bool state); + + bool GetGyroMode() const; + void SetGyroMode(bool state); + + dgCollisionInstance* GetCollision () const; + dgBodyMasterList::dgListNode* GetMasterList() const; + + OnBodyDestroy GetDestructorCallback () const; + void SetDestructorCallback (OnBodyDestroy destructor); + OnMatrixUpdateCallback GetMatrixUpdateCallback () const; + void SetMatrixUpdateCallback (OnMatrixUpdateCallback callback); + + dgVector GetMass() const; + dgVector GetInvMass() const; + void SetInvMass(const dgVector& invMass); + const dgMatrix& GetInvInertiaMatrix () const; + + bool IsCollidable() const; + void UpdateCollisionMatrix(dgFloat32 timestep, dgInt32 threadIndex); + + virtual dgMatrix CalculateInertiaMatrix () const; + virtual dgMatrix CalculateInvInertiaMatrix () const; + virtual dgMatrix CalculateLocalInertiaMatrix () const; + + virtual dgVector GetAlpha() const; + virtual dgVector GetAccel() const; + + virtual void SetAlpha(const dgVector& alpha); + virtual void SetAccel(const dgVector& accel); + + virtual void SetCollidable (bool state) = 0; + virtual bool IsInEquilibrium () const = 0; + + virtual const dgVector& GetForce() const = 0; + virtual const dgVector& GetTorque() const = 0; + virtual void AddForce (const dgVector& force) = 0; + virtual void AddTorque (const dgVector& torque) = 0; + virtual void SetForce (const dgVector& force) = 0; + virtual void SetTorque (const dgVector& torque) = 0; + + virtual dgFloat32 GetLinearDamping () const = 0; + virtual dgVector GetAngularDamping () const = 0; + virtual void SetLinearDamping (dgFloat32 linearDamp) = 0; + virtual void SetAngularDamping (const dgVector& angularDamp) = 0; + virtual void AddDampingAcceleration(dgFloat32 timestep) = 0; + + virtual void SetMassMatrix (dgFloat32 mass, const dgMatrix& inertia); + virtual void SetMassProperties (dgFloat32 mass, const dgCollisionInstance* const collision); + + virtual dgVector PredictLinearVelocity(dgFloat32 timestep) const = 0; + virtual dgVector PredictAngularVelocity(dgFloat32 timestep) const = 0; + virtual void InvalidateCache(); + + virtual void SetMatrix(const dgMatrix& matrix); + virtual void SetMatrixResetSleep(const dgMatrix& matrix); + virtual void SetMatrixNoSleep(const dgMatrix& matrix); + virtual void IntegrateVelocity (dgFloat32 timestep); + virtual void AttachCollision (dgCollisionInstance* const collision); + + virtual void ApplyExtenalForces (dgFloat32 timestep, dgInt32 threadIndex) = 0; + virtual OnApplyExtForceAndTorque GetExtForceAndTorqueCallback () const = 0; + virtual void SetExtForceAndTorqueCallback (OnApplyExtForceAndTorque callback) = 0; + + virtual dgFloat32 RayCast (const dgLineBox& line, OnRayCastAction filter, OnRayPrecastAction preFilter, void* const userData, dgFloat32 minT) const; + virtual void Serialize (const dgTree& collisionRemapId, dgSerialize serializeCallback, void* const userData); + + virtual dgConstraint* GetFirstJoint() const; + virtual dgConstraint* GetNextJoint(dgConstraint* const joint) const; + virtual dgConstraint* GetFirstContact() const; + virtual dgConstraint* GetNextContact(dgConstraint* const joint) const; + virtual dgSkeletonContainer* GetSkeleton() const; + + void SetMatrixOriginAndRotation(const dgMatrix& matrix); + + dgBroadPhaseBodyNode* GetBroadPhase () const; + void SetBroadPhase(dgBroadPhaseBodyNode* const node); + dgBroadPhaseAggregate* GetBroadPhaseAggregate() const; + void SetBroadPhaseAggregate(dgBroadPhaseAggregate* const aggregate); + + void AddImpulse (const dgVector& pointVeloc, const dgVector& pointPosit, dgFloat32 timestep); + void ApplyImpulsePair (const dgVector& linearImpulse, const dgVector& angularImpulse, dgFloat32 timestep); + void ApplyImpulsesAtPoint (dgInt32 count, dgInt32 strideInBytes, const dgFloat32* const impulseArray, const dgFloat32* const pointArray, dgFloat32 timestep); + + dgInt32 GetSerializedID() const; + void CalcInvInertiaMatrix(); + void UpdateGyroData(); + + void InitJointSet (); + + void SetCollision(dgCollisionInstance* const collision); + + protected: + void UpdateWorlCollisionMatrix() const; + void UpdateLumpedMatrix(); + + dgVector CalculateLinearMomentum() const; + dgVector CalculateAngularMomentum() const; + + dgMatrix m_matrix; + dgQuaternion m_rotation; + dgMatrix m_invWorldInertiaMatrix; + dgVector m_mass; + dgVector m_invMass; + dgVector m_veloc; + dgVector m_omega; + dgVector m_accel; + dgVector m_alpha; + dgVector m_minAABB; + dgVector m_maxAABB; + dgVector m_localCentreOfMass; + dgVector m_globalCentreOfMass; + dgVector m_impulseForce; + dgVector m_impulseTorque; + dgVector m_gyroAlpha; + dgVector m_gyroTorque; + dgQuaternion m_gyroRotation; + + mutable dgInt32 m_criticalSectionLock; + union + { + dgUnsigned32 m_flags; + struct { + dgUnsigned32 m_freeze : 1; + dgUnsigned32 m_resting : 1; + dgUnsigned32 m_sleeping : 1; + dgUnsigned32 m_autoSleep : 1; + dgUnsigned32 m_inCallback : 1; + dgUnsigned32 m_jointSet : 1; + dgUnsigned32 m_collidable : 1; + dgUnsigned32 m_equilibrium : 1; + dgUnsigned32 m_spawnnedFromCallback : 1; + dgUnsigned32 m_continueCollisionMode : 1; + dgUnsigned32 m_collideWithLinkedBodies : 1; + dgUnsigned32 m_transformIsDirty : 1; + dgUnsigned32 m_gyroTorqueOn : 1; + dgUnsigned32 m_isdead : 1; + }; + }; + + void* m_userData; + dgWorld* m_world; + dgCollisionInstance* m_collision; + dgBroadPhaseBodyNode* m_broadPhaseNode; + dgBodyMasterList::dgListNode* m_masterNode; + dgBroadPhaseAggregate* m_broadPhaseaggregateNode; + OnBodyDestroy m_destructor; + OnMatrixUpdateCallback m_matrixUpdate; + + dgSetInfo m_disjointInfo; + dgInt32 m_index; + dgInt32 m_uniqueID; + dgInt32 m_bodyGroupId; + dgInt32 m_rtti; + dgInt32 m_type; + dgInt32 m_serializedEnum; + dgUnsigned32 m_dynamicsLru; + dgUnsigned32 m_genericLRUMark; + + friend class dgWorld; + friend class dgSolver; + friend class dgContact; + friend class dgConstraint; + friend class dgDeadBodies; + friend class dgBroadPhase; + friend class dgCollisionBVH; + friend class dgBroadPhaseNode; + friend class dgBodyMasterList; + friend class dgCollisionScene; + friend class dgCollisionConvex; + friend class dgBroadPhaseMixed; + friend class dgCollisionCompound; + friend class dgCollisionUserMesh; + friend class dgBodyMasterListRow; + friend class dgParallelBodySolver; + friend class dgWorldDynamicUpdate; + friend class dgBroadPhaseBodyNode; + friend class dgBilateralConstraint; + friend class dgBroadPhaseAggregate; + friend class dgBroadPhaseSegregated; + friend class dgCollisionConvexPolygon; + friend class dgCollidingPairCollector; + friend class dgCollisionLumpedMassParticles; + +} DG_GCC_VECTOR_ALIGNMENT; + +// ***************************************************************************** +// +// Implementation +// +// ***************************************************************************** +DG_INLINE const dgMatrix& dgBody::GetInvInertiaMatrix () const +{ + return m_invWorldInertiaMatrix; +} + +DG_INLINE dgVector dgBody::GetInvMass() const +{ + return m_invMass; +} + +DG_INLINE void dgBody::SetInvMass(const dgVector& invMass) +{ + m_invMass = invMass; +} + +DG_INLINE dgVector dgBody::GetMass() const +{ + return m_mass; +} + + +DG_INLINE dgBody::dgType dgBody::GetType () const +{ + return dgType (m_type); +} + +DG_INLINE dgInt32 dgBody::IsRTTIType (dgUnsigned32 rtti) const +{ + return rtti & m_rtti; +} + +DG_INLINE void dgBody::SetUserData(void* const userData) +{ + m_userData = userData; +} + +DG_INLINE void* dgBody::GetUserData() const +{ + return m_userData; +} + +DG_INLINE dgWorld* dgBody::GetWorld() const +{ + return m_world; +} + + +DG_INLINE dgUnsigned32 dgBody::GetGroupID () const +{ + return dgUnsigned32 (m_bodyGroupId); +} + +DG_INLINE void dgBody::SetGroupID (dgUnsigned32 id) +{ + m_bodyGroupId = dgInt32 (id); +} + + + +DG_INLINE dgBodyMasterList::dgListNode* dgBody::GetMasterList() const +{ + return m_masterNode; +} + +DG_INLINE void dgBody::GetAABB (dgVector &p0, dgVector &p1) const +{ + p0.m_x = m_minAABB.m_x; + p0.m_y = m_minAABB.m_y; + p0.m_z = m_minAABB.m_z; + p1.m_x = m_maxAABB.m_x; + p1.m_y = m_maxAABB.m_y; + p1.m_z = m_maxAABB.m_z; +} + +DG_INLINE const dgVector& dgBody::GetOmega() const +{ + return m_omega; +} + +DG_INLINE const dgVector& dgBody::GetVelocity() const +{ + return m_veloc; +} + +DG_INLINE void dgBody::SetOmegaNoSleep(const dgVector& omega) +{ + m_omega = omega; +} + +DG_INLINE void dgBody::SetOmega (const dgVector& omega) +{ + SetOmegaNoSleep(omega); + m_equilibrium = false; +} + +DG_INLINE dgVector dgBody::GetVelocityAtPoint (const dgVector& point) const +{ + return m_veloc + m_omega.CrossProduct(point - m_globalCentreOfMass); +} + +DG_INLINE void dgBody::SetVelocityNoSleep(const dgVector& velocity) +{ + m_veloc = velocity; +} + +DG_INLINE void dgBody::SetVelocity (const dgVector& velocity) +{ + SetVelocityNoSleep(velocity); + m_equilibrium = false; +} + +DG_INLINE const dgMatrix& dgBody::GetMatrix() const +{ + return m_matrix; +} + +DG_INLINE const dgVector& dgBody::GetPosition() const +{ + return m_matrix.m_posit; +} + +DG_INLINE const dgQuaternion& dgBody::GetRotation() const +{ + return m_rotation; +} + +DG_INLINE const dgVector& dgBody::GetCentreOfMass () const +{ + return m_localCentreOfMass; +} + +DG_INLINE void dgBody::SetCentreOfMass (const dgVector& com) +{ + m_localCentreOfMass.m_x = com.m_x; + m_localCentreOfMass.m_y = com.m_y; + m_localCentreOfMass.m_z = com.m_z; + m_localCentreOfMass.m_w = dgFloat32 (1.0f); + m_globalCentreOfMass = m_matrix.TransformVector (m_localCentreOfMass); +} + + +DG_INLINE dgInt32 dgBody::GetUniqueID () const +{ + return m_uniqueID; +} + +DG_INLINE void dgBody::SetDestructorCallback (OnBodyDestroy destructor) +{ + m_destructor = destructor; +} + +DG_INLINE dgBody::OnBodyDestroy dgBody::GetDestructorCallback () const +{ + return m_destructor; +} + +DG_INLINE void dgBody::SetMatrixUpdateCallback (OnMatrixUpdateCallback callback) +{ + m_matrixUpdate = callback; +} + +DG_INLINE dgBody::OnMatrixUpdateCallback dgBody::GetMatrixUpdateCallback () const +{ + return m_matrixUpdate; +} + +DG_INLINE dgCollisionInstance* dgBody::GetCollision () const +{ + return m_collision; +} + +DG_INLINE void dgBody::SetContinueCollisionMode (bool mode) +{ + m_continueCollisionMode = dgUnsigned32 (mode); +} + +DG_INLINE bool dgBody::GetContinueCollisionMode () const +{ + return m_continueCollisionMode; +} + +DG_INLINE void dgBody::SetCollisionWithLinkedBodies (bool state) +{ + m_collideWithLinkedBodies = dgUnsigned32 (state); +} + +DG_INLINE bool dgBody::GetCollisionWithLinkedBodies () const +{ + return m_collideWithLinkedBodies; +} + +DG_INLINE bool dgBody::GetFreeze () const +{ + return m_freeze; +} + + +DG_INLINE void dgBody::SetAutoSleep (bool state) +{ + SetSleepState(false); + m_autoSleep = dgUnsigned32 (state); +} + +DG_INLINE bool dgBody::GetAutoSleep () const +{ + return m_autoSleep; +} + +DG_INLINE bool dgBody::GetSleepState () const +{ +// return m_equilibrium; + return m_sleeping; +} + +DG_INLINE bool dgBody::GetGyroMode() const +{ + return m_gyroTorqueOn; +} + +DG_INLINE void dgBody::SetGyroMode(bool state) +{ + m_gyroTorqueOn = state; +} + +DG_INLINE bool dgBody::IsCollidable() const +{ + return m_collidable; +} + + +DG_INLINE void dgBody::SetMatrixOriginAndRotation(const dgMatrix& matrix) +{ + m_matrix = matrix; + dgAssert (m_matrix.TestOrthogonal(dgFloat32 (1.0e-4f))); + + m_rotation = dgQuaternion (m_matrix); + m_globalCentreOfMass = m_matrix.TransformVector (m_localCentreOfMass); + UpdateLumpedMatrix(); +} + + +DG_INLINE void dgBody::SetBroadPhase(dgBroadPhaseBodyNode* const node) +{ + m_broadPhaseNode = node; +} + +DG_INLINE dgBroadPhaseBodyNode* dgBody::GetBroadPhase() const +{ + return m_broadPhaseNode; +} + +DG_INLINE dgBroadPhaseAggregate* dgBody::GetBroadPhaseAggregate() const +{ + return m_broadPhaseaggregateNode; +} + +DG_INLINE void dgBody::SetBroadPhaseAggregate(dgBroadPhaseAggregate* const aggregate) +{ + m_broadPhaseaggregateNode = aggregate; +} + +DG_INLINE dgVector dgBody::CalculateLinearMomentum() const +{ + return dgVector(m_veloc.Scale(m_mass.m_w)); +} + +DG_INLINE dgVector dgBody::CalculateAngularMomentum() const +{ + dgVector localOmega(m_matrix.UnrotateVector(m_omega)); + dgVector localAngularMomentum(m_mass * localOmega); + return m_matrix.RotateVector(localAngularMomentum); +} + +DG_INLINE void dgBody::UpdateGyroData() +{ + if (m_gyroTorqueOn) { + m_gyroTorque = m_omega.CrossProduct(CalculateAngularMomentum()); + m_gyroAlpha = m_invWorldInertiaMatrix.RotateVector(m_gyroTorque); + } else { + dgVector zero(0.0f); + m_gyroTorque = zero; + m_gyroAlpha = zero; + } +} + +DG_INLINE void dgBody::CalcInvInertiaMatrix () +{ + dgAssert (m_invWorldInertiaMatrix[0][3] == dgFloat32 (0.0f)); + dgAssert (m_invWorldInertiaMatrix[1][3] == dgFloat32 (0.0f)); + dgAssert (m_invWorldInertiaMatrix[2][3] == dgFloat32 (0.0f)); + dgAssert (m_invWorldInertiaMatrix[3][3] == dgFloat32 (1.0f)); + + m_invWorldInertiaMatrix = CalculateInvInertiaMatrix (); + + dgAssert (m_invWorldInertiaMatrix[0][3] == dgFloat32 (0.0f)); + dgAssert (m_invWorldInertiaMatrix[1][3] == dgFloat32 (0.0f)); + dgAssert (m_invWorldInertiaMatrix[2][3] == dgFloat32 (0.0f)); + dgAssert (m_invWorldInertiaMatrix[3][3] == dgFloat32 (1.0f)); + + UpdateGyroData(); +} + +DG_INLINE dgSkeletonContainer* dgBody::GetSkeleton() const +{ + return NULL; +} + +DG_INLINE dgInt32 dgBody::GetSerializedID() const +{ + return m_serializedEnum; +} + +DG_INLINE void dgBody::InitJointSet () +{ + m_index = -1; + m_jointSet = 1; + m_disjointInfo.Init (this); +} + +DG_INLINE void dgBody::SetCollision(dgCollisionInstance* const collision) +{ + m_collision = collision; +} + +#endif + diff --git a/thirdparty/src/newton/dgPhysics/dgBodyMasterList.cpp b/thirdparty/src/newton/dgPhysics/dgBodyMasterList.cpp new file mode 100644 index 000000000..55c80c091 --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgBodyMasterList.cpp @@ -0,0 +1,339 @@ +/* Copyright (c) <2003-2019> +* +* 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 "dgPhysicsStdafx.h" +#include "dgBody.h" +#include "dgWorld.h" +#include "dgConstraint.h" +#include "dgDynamicBody.h" +#include "dgBodyMasterList.h" +#include "dgSkeletonContainer.h" + +dgBodyMasterListRow::dgBodyMasterListRow () + :dgList(NULL) + ,m_body (NULL) +{ +} + +dgBodyMasterListRow::~dgBodyMasterListRow() +{ + dgAssert (GetCount() == 0); +} + +DG_INLINE dgBodyMasterListRow::dgListNode* dgBodyMasterListRow::AddContactJoint (dgContact* const joint, dgBody* const body) +{ + dgListNode* const node = Addtop(); + node->GetInfo().m_joint = joint; + node->GetInfo().m_bodyNode = body; + return node; +} + +dgBodyMasterListRow::dgListNode* dgBodyMasterListRow::AddBilateralJoint (dgConstraint* const joint, dgBody* const body) +{ + dgScopeSpinLock lock(&m_body->m_criticalSectionLock); + dgListNode* const node = Append(); + + node->GetInfo().m_joint = joint; + node->GetInfo().m_bodyNode = body; + return node; +} + +void dgBodyMasterListRow::RemoveAllJoints () +{ + dgWorld* const world = m_body->GetWorld(); + + for (dgListNode* node = GetFirst(); node; ) { + dgConstraint* const constraint = node->GetInfo().m_joint; + node = node->GetNext(); + world->DestroyConstraint (constraint); + } +} + +void dgBodyMasterListRow::RemoveContactJoint (dgListNode* const link) +{ + dgScopeSpinLock lock(&m_body->m_criticalSectionLock); + m_body->m_world->GlobalLock(); + Remove(link); + m_body->m_world->GlobalUnlock(); +} + +void dgBodyMasterListRow::RemoveBilateralJoint (dgListNode* const link) +{ + dgScopeSpinLock lock(&m_body->m_criticalSectionLock); + m_body->m_world->GlobalLock(); + Remove(link); + m_body->m_world->GlobalUnlock(); +} + +DG_INLINE dgBilateralConstraint* dgBodyMasterListRow::FindBilateralJoint (const dgBody* const otherBody) const +{ + for (dgBodyMasterListRow::dgListNode* link = GetLast(); link && (link->GetInfo().m_joint->GetId() != dgConstraint::m_contactConstraint); link = link->GetPrev()) { + if (link->GetInfo().m_bodyNode == otherBody) { + dgAssert (link->GetInfo().m_joint->IsBilateral()); + return (dgBilateralConstraint*) link->GetInfo().m_joint; + } + } + return NULL; +} + +void dgBodyMasterListRow::SortList() +{ + dgScopeSpinLock lock(&m_body->m_criticalSectionLock); + if (GetFirst()) { + dgAssert (GetFirst()->GetInfo().m_joint->GetId() != dgConstraint::m_contactConstraint); + dgListNode* nextKeyNode = NULL; + for (dgListNode* keyNode = GetFirst()->GetNext(); keyNode; keyNode = nextKeyNode) { + nextKeyNode = keyNode->GetNext(); + dgAssert (keyNode->GetInfo().m_joint->GetId() != dgConstraint::m_contactConstraint); + dgInt32 key = keyNode->GetInfo().m_bodyNode->GetUniqueID(); + + dgListNode* ptr0 = NULL; + for (dgListNode* ptr = GetFirst(); ptr != keyNode; ptr = ptr->GetNext()) { + dgInt32 key1 = ptr->GetInfo().m_bodyNode->GetUniqueID(); + if (key1 > key) { + break; + } + ptr0 = ptr; + } + dgAssert (ptr0 != keyNode); + if (!ptr0) { + RotateToBegin (keyNode); + } else { + InsertAfter(ptr0, keyNode); + } + } + } +} + +dgBodyMasterList::dgBodyMasterList (dgMemoryAllocator* const allocator) + :dgList(allocator) + ,m_disableBodies(allocator) + ,m_constraintCount (0) +{ +} + +dgBodyMasterList::~dgBodyMasterList(void) +{ +} + +void dgBodyMasterList::AddBody (dgBody* const body) +{ + dgListNode* const node = Append(); + body->m_masterNode = node; + node->GetInfo().SetAllocator (body->GetWorld()->GetAllocator()); + node->GetInfo().SetBody(body); + + if ((body->m_invMass.m_w == dgFloat32 (0.0f)) && (GetFirst() != node)) { + InsertAfter (GetFirst(), node); + } +} + +void dgBodyMasterList::RemoveBody (dgBody* const body) +{ + dgListNode* const node = body->m_masterNode; + dgAssert (node); + + node->GetInfo().RemoveAllJoints(); + dgAssert (node->GetInfo().GetCount() == 0); + + Remove (node); + body->m_masterNode = NULL; +} + +dgBodyMasterListRow::dgListNode* dgBodyMasterList::FindConstraintLink (const dgBody* const body0, const dgBody* const body1) const +{ + dgAssert (body0); + dgAssert (body1); + dgAssert (body0->m_masterNode); + + for (dgBodyMasterListRow::dgListNode* node = body0->m_masterNode->GetInfo().GetFirst(); node; node = node->GetNext()) { + if (node->GetInfo().m_bodyNode == body1) { + return node; + } + } + return NULL; +} + + +dgBilateralConstraint* dgBodyMasterList::FindBilateralJoint (const dgBody* body0, const dgBody* body1) const +{ + return body0->m_masterNode->GetInfo().FindBilateralJoint (body1); +} + +void dgBodyMasterList::AttachConstraint(dgConstraint* const constraint, dgBody* const body0, dgBody* const otherBody) +{ + dgAssert (body0); + dgBody* body1 = otherBody; + if (!body1) { + body1 = body0->GetWorld()->GetSentinelBody(); + } + dgAssert (body1); + + #ifdef _DEBUG + if (FindBilateralJoint(body0, body1)) + { + dgTrace (("warning!! bilateral joint duplication between bodied: %d and %d\n", body0->m_uniqueID, body1->m_uniqueID)); + } + #endif + + constraint->m_body0 = body0; + constraint->m_body1 = body1; + + dgAssert(constraint->GetId() != dgConstraint::m_contactConstraint); + + dgBilateralConstraint* const bilateralJoint = constraint->IsBilateral() ? (dgBilateralConstraint*)constraint : NULL; + dgAssert (bilateralJoint); + if (bilateralJoint) { + bilateralJoint->AppendToJointList(); + } + + dgWorld* const world = body0->GetWorld(); + world->m_skelListIsDirty = world->m_skelListIsDirty || (constraint->m_solverModel != 2); + + body0->m_equilibrium = body0->GetInvMass().m_w ? false : true; + body1->m_equilibrium = body1->GetInvMass().m_w ? false : true; + constraint->m_link0 = body0->m_masterNode->GetInfo().AddBilateralJoint (constraint, body1); + constraint->m_link1 = body1->m_masterNode->GetInfo().AddBilateralJoint (constraint, body0); + dgAtomicExchangeAndAdd((dgInt32*) &m_constraintCount, 1); +} + +void dgBodyMasterList::RemoveConstraint (dgConstraint* const constraint) +{ + dgAtomicExchangeAndAdd((dgInt32*) &m_constraintCount, -1); + dgAssert (((dgInt32)m_constraintCount) >= 0); + + dgBody* const body0 = constraint->m_body0; + dgBody* const body1 = constraint->m_body1; + dgAssert (body0); + dgAssert (body1); + dgAssert (body0 == constraint->m_link1->GetInfo().m_bodyNode); + dgAssert (body1 == constraint->m_link0->GetInfo().m_bodyNode); + + dgBodyMasterListRow& row0 = body0->m_masterNode->GetInfo(); + dgBodyMasterListRow& row1 = body1->m_masterNode->GetInfo(); + + if (body0->IsRTTIType(dgBody::m_dynamicBodyRTTI)) { + dgDynamicBody* const dynBody0 = (dgDynamicBody*)body0; + dynBody0->m_savedExternalForce = dgVector(dgFloat32(0.0f)); + dynBody0->m_savedExternalTorque = dgVector(dgFloat32(0.0f)); + } + + if (body1->IsRTTIType(dgBody::m_dynamicBodyRTTI)) { + dgDynamicBody* const dynBody1 = (dgDynamicBody*)body1; + dynBody1->m_savedExternalForce = dgVector(dgFloat32(0.0f)); + dynBody1->m_savedExternalTorque = dgVector(dgFloat32(0.0f)); + } + + if (constraint->GetId() == dgConstraint::m_contactConstraint) { + dgConstraint* const contact = (dgConstraint*) constraint; + if (contact->m_maxDOF) { + body0->m_equilibrium = body0->GetInvMass().m_w ? false : true; + body1->m_equilibrium = body1->GetInvMass().m_w ? false : true; + } + row0.RemoveContactJoint(constraint->m_link0); + row1.RemoveContactJoint(constraint->m_link1); + } else { + dgWorld* const world = body0->GetWorld(); + world->m_skelListIsDirty = true; + + if (body0->GetSkeleton()) { + world->DestroySkeletonContainer (body0->GetSkeleton()); + } + if (body1->GetSkeleton()) { + world->DestroySkeletonContainer(body1->GetSkeleton()); + } + + body0->m_equilibrium = body0->GetInvMass().m_w ? false : true; + body1->m_equilibrium = body1->GetInvMass().m_w ? false : true; + row0.RemoveBilateralJoint(constraint->m_link0); + row1.RemoveBilateralJoint(constraint->m_link1); + } +} + +void dgBodyMasterList::AttachContact(dgContact* const contact) +{ + dgBody* const body0 = contact->m_body0; + dgBody* const body1 = contact->m_body1; + contact->m_link0 = body0->m_masterNode->GetInfo().AddContactJoint(contact, body1); + contact->m_link1 = body1->m_masterNode->GetInfo().AddContactJoint(contact, body0); + m_constraintCount++; +} + +void dgBodyMasterList::RemoveContact(dgContact* const contact) +{ + m_constraintCount --; + dgAssert(((dgInt32)m_constraintCount) >= 0); + dgAssert(contact->GetId() == dgConstraint::m_contactConstraint); + //dgAssert(!contact->m_maxDOF); + + dgBody* const body0 = contact->m_body0; + dgBody* const body1 = contact->m_body1; + dgAssert(body0); + dgAssert(body1); + dgAssert(body0 == contact->m_link1->GetInfo().m_bodyNode); + dgAssert(body1 == contact->m_link0->GetInfo().m_bodyNode); + + dgBodyMasterListRow& row0 = body0->m_masterNode->GetInfo(); + dgBodyMasterListRow& row1 = body1->m_masterNode->GetInfo(); + + row0.Remove(contact->m_link0); + row1.Remove(contact->m_link1); +} + +DG_INLINE dgUnsigned32 dgBodyMasterList::MakeSortMask(const dgBody* const body) const +{ + dgUnsigned32 val0 = body->IsRTTIType(dgBody::m_dynamicBodyRTTI) ? (body->GetInvMass().m_w > 0.0f) << 30 : 0; + dgUnsigned32 val1 = body->IsRTTIType(dgBody::m_kinematicBodyRTTI) ? 1<<29 : 0; + return body->m_uniqueID | val0 | val1; +} + +void dgBodyMasterList::SortMasterList() +{ + GetFirst()->GetInfo().SortList(); + for (dgListNode* node = GetFirst()->GetNext(); node; ) { + node->GetInfo().SortList(); + dgBody* const body1 = node->GetInfo().GetBody(); + + dgAssert (GetFirst() != node); + + body1->InvalidateCache (); + + dgInt32 key1 = MakeSortMask (body1); + dgListNode* const entry = node; + node = node->GetNext(); + dgListNode* prev = entry->GetPrev(); + for (; prev != GetFirst(); prev = prev->GetPrev()) { + dgBody* const body0 = prev->GetInfo().GetBody(); + + dgInt32 key0 = MakeSortMask (body0); + if (key0 < key1) { + break; + } + } + + if (!prev) { + dgAssert (entry == GetFirst()); + RotateToBegin (entry); + } else { + InsertAfter (prev, entry); + } + } +} diff --git a/thirdparty/src/newton/dgPhysics/dgBodyMasterList.h b/thirdparty/src/newton/dgPhysics/dgBodyMasterList.h new file mode 100644 index 000000000..1f25142ac --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgBodyMasterList.h @@ -0,0 +1,85 @@ +/* Copyright (c) <2003-2019> +* +* 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 __DGBODYMASTER_LIST__ +#define __DGBODYMASTER_LIST__ + +class dgBody; +class dgContact; +class dgConstraint; +class dgBilateralConstraint; + +class dgBodyMasterListCell +{ + public: + dgConstraint* m_joint; + dgBody* m_bodyNode; +}; + +class dgBodyMasterListRow: public dgList +{ + public: + dgBodyMasterListRow (); + ~dgBodyMasterListRow (); + + dgBody* GetBody() const { return m_body;} + + private: + void SortList(); + void SetBody(dgBody* const body) {m_body = body;} + + dgBilateralConstraint* FindBilateralJoint (const dgBody* const body) const; + dgListNode* AddContactJoint (dgContact* const contact, dgBody* const body); + dgListNode* AddBilateralJoint (dgConstraint* const joint, dgBody* const body); + + void RemoveContactJoint (dgListNode* const link); + void RemoveBilateralJoint (dgListNode* const link); + void RemoveAllJoints (); + + dgBody* m_body; + friend class dgBodyMasterList; +}; + +class dgBodyMasterList: public dgList +{ + public: + dgBodyMasterList (dgMemoryAllocator* const allocator); + ~dgBodyMasterList (); + + void AddBody (dgBody* const body); + void RemoveBody (dgBody* const body); + void RemoveConstraint (dgConstraint* const constraint); + void AttachConstraint (dgConstraint* const constraint, dgBody* const body0, dgBody* const body1); + + void AttachContact(dgContact* const contact); + void RemoveContact(dgContact* const contact); + + dgBilateralConstraint* FindBilateralJoint (const dgBody* body0, const dgBody* body1) const; + dgBodyMasterListRow::dgListNode* FindConstraintLink (const dgBody* const body0, const dgBody* const body1) const; + dgUnsigned32 MakeSortMask(const dgBody* const body) const; + void SortMasterList(); + + public: + dgTree m_disableBodies; + dgUnsigned32 m_constraintCount; +}; + +#endif diff --git a/thirdparty/src/newton/dgPhysics/dgBroadPhase.cpp b/thirdparty/src/newton/dgPhysics/dgBroadPhase.cpp new file mode 100644 index 000000000..407959dd0 --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgBroadPhase.cpp @@ -0,0 +1,1776 @@ +/* Copyright (c) <2003-2019> +* +* 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 "dgPhysicsStdafx.h" +#include "dgBody.h" +#include "dgWorld.h" +#include "dgContact.h" +#include "dgBroadPhase.h" +#include "dgDynamicBody.h" +#include "dgCollisionConvex.h" +#include "dgCollisionInstance.h" +#include "dgWorldDynamicUpdate.h" +#include "dgBilateralConstraint.h" +#include "dgBroadPhaseAggregate.h" +#include "dgCollisionLumpedMassParticles.h" +//#include "dgCollisionLumpedMassParticles.h" + +#define DG_CONVEX_CAST_POOLSIZE 32 +#define DG_BROADPHASE_AABB_SCALE dgFloat32 (8.0f) +#define DG_BROADPHASE_AABB_INV_SCALE (dgFloat32 (1.0f) / DG_BROADPHASE_AABB_SCALE) +#define DG_CONTACT_TRANSLATION_ERROR dgFloat32 (1.0e-3f) +#define DG_CONTACT_ANGULAR_ERROR (dgFloat32 (0.25f * dgDegreeToRad)) +#define DG_NARROW_PHASE_DIST dgFloat32 (0.2f) +#define DG_CONTACT_DELAY_FRAMES 4 + +//#define DG_USE_OLD_SCANNER + +dgVector dgBroadPhase::m_velocTol(dgFloat32(1.0e-16f)); +dgVector dgBroadPhase::m_angularContactError2(DG_CONTACT_ANGULAR_ERROR * DG_CONTACT_ANGULAR_ERROR); +dgVector dgBroadPhase::m_linearContactError2(DG_CONTACT_TRANSLATION_ERROR * DG_CONTACT_TRANSLATION_ERROR); + +dgVector dgBroadPhaseNode::m_broadPhaseScale (DG_BROADPHASE_AABB_SCALE, DG_BROADPHASE_AABB_SCALE, DG_BROADPHASE_AABB_SCALE, dgFloat32 (0.0f)); +dgVector dgBroadPhaseNode::m_broadInvPhaseScale (DG_BROADPHASE_AABB_INV_SCALE, DG_BROADPHASE_AABB_INV_SCALE, DG_BROADPHASE_AABB_INV_SCALE, dgFloat32 (0.0f)); + + +class dgBroadPhase::dgSpliteInfo +{ + public: + dgSpliteInfo (dgBroadPhaseNode** const boxArray, dgInt32 boxCount) + { + dgVector minP ( dgFloat32 (1.0e15f)); + dgVector maxP (-dgFloat32 (1.0e15f)); + + if (boxCount == 2) { + m_axis = 1; + for (dgInt32 i = 0; i < boxCount; i ++) { + dgBroadPhaseNode* const node = boxArray[i]; + dgAssert (node->IsLeafNode()); + minP = minP.GetMin (node->m_minBox); + maxP = maxP.GetMax (node->m_maxBox); + } + } else { + dgVector median (dgFloat32 (0.0f)); + dgVector varian (dgFloat32 (0.0f)); + for (dgInt32 i = 0; i < boxCount; i ++) { + dgBroadPhaseNode* const node = boxArray[i]; + dgAssert (node->IsLeafNode()); + minP = minP.GetMin (node->m_minBox); + maxP = maxP.GetMax (node->m_maxBox); + dgVector p (dgVector::m_half * (node->m_minBox + node->m_maxBox)); + median += p; + varian += p * p; + } + + varian = varian.Scale (dgFloat32 (boxCount)) - median * median; + + dgInt32 index = 0; + dgFloat32 maxVarian = dgFloat32 (-1.0e10f); + for (dgInt32 i = 0; i < 3; i ++) { + if (varian[i] > maxVarian) { + index = i; + maxVarian = varian[i]; + } + } + + dgVector center = median.Scale (dgFloat32 (1.0f) / dgFloat32 (boxCount)); + + dgFloat32 test = center[index]; + + dgInt32 i0 = 0; + dgInt32 i1 = boxCount - 1; + do { + for (; i0 <= i1; i0 ++) { + dgBroadPhaseNode* const node = boxArray[i0]; + dgFloat32 val = (node->m_minBox[index] + node->m_maxBox[index]) * dgFloat32 (0.5f); + if (val > test) { + break; + } + } + + for (; i1 >= i0; i1 --) { + dgBroadPhaseNode* const node = boxArray[i1]; + dgFloat32 val = (node->m_minBox[index] + node->m_maxBox[index]) * dgFloat32 (0.5f); + if (val < test) { + break; + } + } + + if (i0 < i1) { + dgSwap(boxArray[i0], boxArray[i1]); + i0++; + i1--; + } + + } while (i0 <= i1); + + if (i0 > 0){ + i0 --; + } + if ((i0 + 1) >= boxCount) { + i0 = boxCount - 2; + } + m_axis = i0 + 1; + } + + dgAssert (maxP.m_x - minP.m_x >= dgFloat32 (0.0f)); + dgAssert (maxP.m_y - minP.m_y >= dgFloat32 (0.0f)); + dgAssert (maxP.m_z - minP.m_z >= dgFloat32 (0.0f)); + m_p0 = minP; + m_p1 = maxP; + } + + dgInt32 m_axis; + dgVector m_p0; + dgVector m_p1; +}; + + +dgBroadPhase::dgBroadPhase(dgWorld* const world) + :m_world(world) + ,m_rootNode(NULL) + ,m_generatedBodies(world->GetAllocator()) + ,m_updateList(world->GetAllocator()) + ,m_aggregateList(world->GetAllocator()) + ,m_lru(DG_CONTACT_DELAY_FRAMES) + ,m_contactCache(world->GetAllocator()) + ,m_pendingSoftBodyCollisions(world->GetAllocator(), 64) + ,m_pendingSoftBodyPairsCount(0) + ,m_criticalSectionLock(0) +{ +} + +dgBroadPhase::~dgBroadPhase() +{ +} + + +void dgBroadPhase::MoveNodes (dgBroadPhase* const dst) +{ + const dgBodyMasterList* const masterList = m_world; + for (dgBodyMasterList::dgListNode* node = masterList->GetFirst(); node; node = node->GetNext()) { + dgBody* const body = node->GetInfo().GetBody(); + if (body->GetBroadPhase() && !body->GetBroadPhaseAggregate()) { + Remove(body); + dst->Add(body); + } + } + + dgList::dgListNode* next; + for (dgList::dgListNode* ptr = m_aggregateList.GetFirst(); ptr; ptr = next) { + next = ptr->GetNext(); + dgBroadPhaseAggregate* const aggregate = ptr->GetInfo(); + m_aggregateList.Remove (aggregate->m_myAggregateNode); + m_updateList.Remove(aggregate->m_updateNode); + aggregate->m_updateNode = NULL; + aggregate->m_myAggregateNode = NULL; + UnlinkAggregate(aggregate); + dst->LinkAggregate(aggregate); + } +} + +dgBroadPhaseTreeNode* dgBroadPhase::InsertNode(dgBroadPhaseNode* const root, dgBroadPhaseNode* const node) +{ + dgVector p0; + dgVector p1; + + dgBroadPhaseNode* sibling = root; + dgFloat32 surfaceArea = CalculateSurfaceArea(node, sibling, p0, p1); + while (!sibling->IsLeafNode()) { + + if (surfaceArea > sibling->m_surfaceArea) { + break; + } + + sibling->m_minBox = p0; + sibling->m_maxBox = p1; + sibling->m_surfaceArea = surfaceArea; + + dgVector leftP0; + dgVector leftP1; + dgFloat32 leftSurfaceArea = CalculateSurfaceArea(node, sibling->GetLeft(), leftP0, leftP1); + + dgVector rightP0; + dgVector rightP1; + dgFloat32 rightSurfaceArea = CalculateSurfaceArea(node, sibling->GetRight(), rightP0, rightP1); + + if (leftSurfaceArea < rightSurfaceArea) { + sibling = sibling->GetLeft(); + p0 = leftP0; + p1 = leftP1; + surfaceArea = leftSurfaceArea; + } else { + sibling = sibling->GetRight(); + p0 = rightP0; + p1 = rightP1; + surfaceArea = rightSurfaceArea; + } + } + + dgBroadPhaseTreeNode* const parent = new (m_world->GetAllocator()) dgBroadPhaseTreeNode(sibling, node); + return parent; +} + +void dgBroadPhase::UpdateAggregateEntropyKernel(void* const context, void* const node, dgInt32 threadID) +{ + D_TRACKTIME(); + dgBroadphaseSyncDescriptor* const descriptor = (dgBroadphaseSyncDescriptor*)context; + dgWorld* const world = descriptor->m_world; + dgBroadPhase* const broadPhase = world->GetBroadPhase(); + broadPhase->UpdateAggregateEntropy(descriptor, (dgList::dgListNode*) node, threadID); +} + +void dgBroadPhase::ForceAndToqueKernel(void* const context, void* const node, dgInt32 threadID) +{ + D_TRACKTIME(); + dgBroadphaseSyncDescriptor* const descriptor = (dgBroadphaseSyncDescriptor*)context; + dgWorld* const world = descriptor->m_world; + dgBroadPhase* const broadPhase = world->GetBroadPhase(); + broadPhase->ApplyForceAndtorque(descriptor, (dgBodyMasterList::dgListNode*) node, threadID); +} + +void dgBroadPhase::SleepingStateKernel(void* const context, void* const node, dgInt32 threadID) +{ + D_TRACKTIME(); + dgBroadphaseSyncDescriptor* const descriptor = (dgBroadphaseSyncDescriptor*)context; + dgWorld* const world = descriptor->m_world; + dgBroadPhase* const broadPhase = world->GetBroadPhase(); + broadPhase->SleepingState(descriptor, (dgBodyMasterList::dgListNode*) node, threadID); +} + +bool dgBroadPhase::DoNeedUpdate(dgBodyMasterList::dgListNode* const node) const +{ + dgBody* const body = node->GetInfo().GetBody(); + + bool state = body->GetInvMass().m_w != dgFloat32 (0.0f); + state = state || !body->m_equilibrium || (body->GetExtForceAndTorqueCallback() != NULL); + return state; +} + +void dgBroadPhase::UpdateAggregateEntropy (dgBroadphaseSyncDescriptor* const descriptor, dgList::dgListNode* node, dgInt32 threadID) +{ + DG_TRACKTIME(); + const dgInt32 threadCount = m_world->GetThreadCount(); + while (node) { + node->GetInfo()->ImproveEntropy(); + for (dgInt32 i = 0; i < threadCount; i++) { + node = node ? node->GetNext() : NULL; + } + } +} + +void dgBroadPhase::ApplyForceAndtorque(dgBroadphaseSyncDescriptor* const descriptor, dgBodyMasterList::dgListNode* node, dgInt32 threadID) +{ + dgFloat32 timestep = descriptor->m_timestep; + + const dgInt32 threadCount = m_world->GetThreadCount(); + while (node) { + dgBody* const body = node->GetInfo().GetBody(); + body->InitJointSet(); + if (DoNeedUpdate(node)) { + if (body->IsRTTIType(dgBody::m_dynamicBodyRTTI)) { + dgDynamicBody* const dynamicBody = (dgDynamicBody*)body; + dynamicBody->ApplyExtenalForces(timestep, threadID); + } + } + + for (dgInt32 i = 0; i < threadCount; i++) { + node = node ? node->GetNext() : NULL; + } + } +} + +void dgBroadPhase::SleepingState(dgBroadphaseSyncDescriptor* const descriptor, dgBodyMasterList::dgListNode* node, dgInt32 threadID) +{ + DG_TRACKTIME(); + dgFloat32 timestep = descriptor->m_timestep; + + const dgInt32 threadCount = m_world->GetThreadCount(); + dgBodyInfo* const pendingBodies = &m_world->m_bodiesMemory[0]; + + dgInt32* const atomicBodiesCount = &descriptor->m_atomicDynamicsCount; + dgInt32* const atomicPendingBodiesCount = &descriptor->m_atomicPendingBodiesCount; + + while (node) { + if (DoNeedUpdate(node)) { + dgBody* const body = node->GetInfo().GetBody(); + + if (body->IsRTTIType(dgBody::m_dynamicBodyRTTI)) { + dgDynamicBody* const dynamicBody = (dgDynamicBody*)body; + + if (!dynamicBody->m_equilibrium && (dynamicBody->GetInvMass().m_w == dgFloat32(0.0f))) { + descriptor->m_fullScan = true; + } + if (dynamicBody->GetInvMass().m_w) { + dgAtomicExchangeAndAdd(atomicBodiesCount, 1); + } + + if (dynamicBody->GetInvMass().m_w == dgFloat32(0.0f) || body->m_collision->IsType(dgCollision::dgCollisionMesh_RTTI)) { + dynamicBody->m_sleeping = true; + dynamicBody->m_autoSleep = true; + dynamicBody->m_equilibrium = true; + } + + if (dynamicBody->IsInEquilibrium()) { + dynamicBody->m_equilibrium = true; + dynamicBody->m_sleeping = dynamicBody->m_autoSleep; + } else { + dynamicBody->m_sleeping = false; + dynamicBody->m_equilibrium = false; + if (dynamicBody->GetBroadPhase()) { + dynamicBody->UpdateCollisionMatrix(timestep, threadID); + dgInt32 pendingBodyIndex = dgAtomicExchangeAndAdd(atomicPendingBodiesCount, 1); + pendingBodies[pendingBodyIndex].m_body = dynamicBody; + } + } + + dynamicBody->m_savedExternalForce = dynamicBody->m_externalForce; + dynamicBody->m_savedExternalTorque = dynamicBody->m_externalTorque; + } else { + dgAssert(body->IsRTTIType(dgBody::m_kinematicBodyRTTI)); + + // kinematic bodies are always sleeping (skip collision with kinematic bodies) + bool isResting = (body->m_omega.DotProduct(body->m_omega).GetScalar() < dgFloat32 (1.0e-6f)) && (body->m_veloc.DotProduct(body->m_veloc).GetScalar() < dgFloat32(1.0e-4f)); + if (body->IsCollidable()) { + body->m_sleeping = false; + body->m_autoSleep = false; + } else { + body->m_autoSleep = true; + body->m_sleeping = isResting; + descriptor->m_fullScan = !isResting; + } + body->m_equilibrium = isResting; + + // update collision matrix by calling the transform callback for all kinematic bodies + if (body->GetBroadPhase()) { + body->UpdateCollisionMatrix(timestep, threadID); + } + } + } + + for (dgInt32 i = 0; i < threadCount; i++) { + node = node ? node->GetNext() : NULL; + } + } +} + + +void dgBroadPhase::ForEachBodyInAABB(const dgBroadPhaseNode** stackPool, dgInt32 stack, const dgVector& minBox, const dgVector& maxBox, OnBodiesInAABB callback, void* const userData) const +{ + while (stack) { + stack--; + const dgBroadPhaseNode* const rootNode = stackPool[stack]; + if (dgOverlapTest(rootNode->m_minBox, rootNode->m_maxBox, minBox, maxBox)) { + + dgBody* const body = rootNode->GetBody(); + if (body) { + if (!body->m_isdead && dgOverlapTest(body->m_minAABB, body->m_maxAABB, minBox, maxBox)) { + if (!callback(body, userData)) { + break; + } + } + } else if (rootNode->IsAggregate()) { + dgBroadPhaseAggregate* const aggregate = (dgBroadPhaseAggregate*)rootNode; + if (aggregate->m_root) { + stackPool[stack] = aggregate->m_root; + stack++; + dgAssert(stack < DG_BROADPHASE_MAX_STACK_DEPTH); + } + + } else { + dgAssert (!rootNode->IsLeafNode()); + dgBroadPhaseTreeNode* const node = (dgBroadPhaseTreeNode*)rootNode; + stackPool[stack] = node->m_left; + stack++; + dgAssert(stack < DG_BROADPHASE_MAX_STACK_DEPTH); + + stackPool[stack] = node->m_right; + stack++; + dgAssert(stack < DG_BROADPHASE_MAX_STACK_DEPTH); + } + } + } +} + +dgInt32 dgBroadPhase::ConvexCast(const dgBroadPhaseNode** stackPool, dgFloat32* const distance, dgInt32 stack, const dgVector& velocA, const dgVector& velocB, dgFastRayTest& ray, + dgCollisionInstance* const shape, const dgMatrix& matrix, const dgVector& target, dgFloat32* const param, OnRayPrecastAction prefilter, void* const userData, dgConvexCastReturnInfo* const info, dgInt32 maxContacts, dgInt32 threadIndex) const +{ + dgVector boxP0; + dgVector boxP1; + dgTriplex points[DG_CONVEX_CAST_POOLSIZE]; + dgTriplex normals[DG_CONVEX_CAST_POOLSIZE]; + dgFloat32 penetration[DG_CONVEX_CAST_POOLSIZE]; + dgInt64 attributeA[DG_CONVEX_CAST_POOLSIZE]; + dgInt64 attributeB[DG_CONVEX_CAST_POOLSIZE]; + dgInt32 totalCount = 0; + + dgAssert(matrix.TestOrthogonal()); + shape->CalcAABB(matrix, boxP0, boxP1); + + maxContacts = dgMin (maxContacts, DG_CONVEX_CAST_POOLSIZE); + dgAssert (!maxContacts || (maxContacts && info)); + dgFloat32 maxParam = *param; + dgFloat32 timeToImpact = *param; + while (stack) { + stack--; + + dgFloat32 dist = distance[stack]; + + if (dist > maxParam) { + break; + } else { + const dgBroadPhaseNode* const me = stackPool[stack]; + + dgBody* const body = me->GetBody(); + if (body) { + if (!body->m_isdead && !PREFILTER_RAYCAST(prefilter, body, body->m_collision, userData)) { + dgInt32 count = m_world->CollideContinue(shape, matrix, velocA, velocB, body->m_collision, body->m_matrix, velocB, velocB, timeToImpact, points, normals, penetration, attributeA, attributeB, maxContacts, threadIndex); + + if (timeToImpact < maxParam) { + if ((timeToImpact - maxParam) < dgFloat32(-1.0e-3f)) { + totalCount = 0; + } + maxParam = timeToImpact; + if (count >= (maxContacts - totalCount)) { + count = maxContacts - totalCount; + } + + for (dgInt32 i = 0; i < count; i++) { + info[totalCount].m_point[0] = points[i].m_x; + info[totalCount].m_point[1] = points[i].m_y; + info[totalCount].m_point[2] = points[i].m_z; + info[totalCount].m_point[3] = dgFloat32(0.0f); + info[totalCount].m_normal[0] = normals[i].m_x; + info[totalCount].m_normal[1] = normals[i].m_y; + info[totalCount].m_normal[2] = normals[i].m_z; + info[totalCount].m_normal[3] = dgFloat32(0.0f); + info[totalCount].m_penetration = penetration[i]; + info[totalCount].m_contaID = attributeB[i]; + + info[totalCount].m_hitBody = body; + totalCount++; + } + } + if (maxParam < 1.0e-8f) { + break; + } + } + } else if (me->IsAggregate()) { + dgBroadPhaseAggregate* const aggregate = (dgBroadPhaseAggregate*)me; + const dgBroadPhaseNode* const node = aggregate->m_root; + if (node) { + dgVector minBox(node->m_minBox - boxP1); + dgVector maxBox(node->m_maxBox - boxP0); + dgFloat32 dist1 = ray.BoxIntersect(minBox, maxBox); + if (dist1 < maxParam) { + dgInt32 j = stack; + for (; j && (dist1 > distance[j - 1]); j--) { + stackPool[j] = stackPool[j - 1]; + distance[j] = distance[j - 1]; + } + stackPool[j] = node; + distance[j] = dist1; + stack++; + dgAssert(stack < DG_BROADPHASE_MAX_STACK_DEPTH); + } + } + + } else { + dgBroadPhaseTreeNode* const node = (dgBroadPhaseTreeNode*)me; + const dgBroadPhaseNode* const left = node->m_left; + dgAssert(left); + dgVector minBox(left->m_minBox - boxP1); + dgVector maxBox(left->m_maxBox - boxP0); + dgFloat32 dist1 = ray.BoxIntersect(minBox, maxBox); + if (dist1 < maxParam) { + dgInt32 j = stack; + for (; j && (dist1 > distance[j - 1]); j--) { + stackPool[j] = stackPool[j - 1]; + distance[j] = distance[j - 1]; + } + stackPool[j] = left; + distance[j] = dist1; + stack++; + dgAssert(stack < DG_BROADPHASE_MAX_STACK_DEPTH); + } + + const dgBroadPhaseNode* const right = node->m_right; + dgAssert(right); + minBox = right->m_minBox - boxP1; + maxBox = right->m_maxBox - boxP0; + dist1 = ray.BoxIntersect(minBox, maxBox); + if (dist1 < maxParam) { + dgInt32 j = stack; + for (; j && (dist1 > distance[j - 1]); j--) { + stackPool[j] = stackPool[j - 1]; + distance[j] = distance[j - 1]; + } + stackPool[j] = right; + distance[j] = dist1; + stack++; + dgAssert(stack < DG_BROADPHASE_MAX_STACK_DEPTH); + } + } + } + } + *param = maxParam; + return totalCount; +} + +dgInt32 dgBroadPhase::Collide(const dgBroadPhaseNode** stackPool, dgInt32* const ovelapStack, dgInt32 stack, const dgVector& boxP0, const dgVector& boxP1, dgCollisionInstance* const shape, const dgMatrix& matrix, OnRayPrecastAction prefilter, void* const userData, dgConvexCastReturnInfo* const info, dgInt32 maxContacts, dgInt32 threadIndex) const +{ + dgTriplex points[DG_CONVEX_CAST_POOLSIZE]; + dgTriplex normals[DG_CONVEX_CAST_POOLSIZE]; + dgFloat32 penetration[DG_CONVEX_CAST_POOLSIZE]; + dgInt64 attributeA[DG_CONVEX_CAST_POOLSIZE]; + dgInt64 attributeB[DG_CONVEX_CAST_POOLSIZE]; + + dgInt32 totalCount = 0; + while (stack) { + stack--; + + dgInt32 test = ovelapStack[stack]; + if (test) { + const dgBroadPhaseNode* const me = stackPool[stack]; + + dgBody* const body = me->GetBody(); + if (body) { + if (!body->m_isdead && !PREFILTER_RAYCAST(prefilter, body, body->m_collision, userData)) { + dgInt32 count = m_world->Collide(shape, matrix, body->m_collision, body->m_matrix, points, normals, penetration, attributeA, attributeB, DG_CONVEX_CAST_POOLSIZE, threadIndex); + + if (count) { + bool teminate = false; + if (count >= (maxContacts - totalCount)) { + count = maxContacts - totalCount; + teminate = true; + } + + for (dgInt32 i = 0; i < count; i++) { + info[totalCount].m_point[0] = points[i].m_x; + info[totalCount].m_point[1] = points[i].m_y; + info[totalCount].m_point[2] = points[i].m_z; + info[totalCount].m_point[3] = dgFloat32(0.0f); + info[totalCount].m_normal[0] = normals[i].m_x; + info[totalCount].m_normal[1] = normals[i].m_y; + info[totalCount].m_normal[2] = normals[i].m_z; + info[totalCount].m_normal[3] = dgFloat32(0.0f); + info[totalCount].m_penetration = penetration[i]; + info[totalCount].m_contaID = attributeB[i]; + info[totalCount].m_hitBody = body; + totalCount++; + } + + if (teminate) { + break; + } + } + } + } else if (me->IsAggregate()) { + dgBroadPhaseAggregate* const aggregate = (dgBroadPhaseAggregate*)me; + const dgBroadPhaseNode* const node = aggregate->m_root; + if (node) { + stackPool[stack] = node; + ovelapStack[stack] = dgOverlapTest(node->m_minBox, node->m_maxBox, boxP0, boxP1); + stack++; + dgAssert(stack < DG_BROADPHASE_MAX_STACK_DEPTH); + } + + } else { + dgBroadPhaseTreeNode* const node = (dgBroadPhaseTreeNode*)me; + const dgBroadPhaseNode* const left = node->m_left; + stackPool[stack] = left; + ovelapStack[stack] = dgOverlapTest(left->m_minBox, left->m_maxBox, boxP0, boxP1); + stack ++; + dgAssert(stack < DG_BROADPHASE_MAX_STACK_DEPTH); + + const dgBroadPhaseNode* const right = node->m_right; + stackPool[stack] = right; + ovelapStack[stack] = dgOverlapTest(right->m_minBox, right->m_maxBox, boxP0, boxP1); + stack++; + dgAssert(stack < DG_BROADPHASE_MAX_STACK_DEPTH); + } + } + } + return totalCount; +} + +void dgBroadPhase::RayCast(const dgBroadPhaseNode** stackPool, dgFloat32* const distance, dgInt32 stack, const dgVector& l0, const dgVector& l1, dgFastRayTest& ray, OnRayCastAction filter, OnRayPrecastAction prefilter, void* const userData) const +{ + dgLineBox line; + line.m_l0 = l0; + line.m_l1 = l1; + dgVector test(line.m_l0 <= line.m_l1); + + dgFloat32 maxParam = dgFloat32 (1.2f); + + //line.m_boxL0 = (line.m_l0 & test) | line.m_l1.AndNot(test); + //line.m_boxL1 = (line.m_l1 & test) | line.m_l0.AndNot(test); + line.m_boxL0 = line.m_l1.Select(line.m_l0, test); + line.m_boxL1 = line.m_l0.Select(line.m_l1, test); + + while (stack) { + stack--; + dgFloat32 dist = distance[stack]; + if (dist > maxParam) { + break; + } else { + const dgBroadPhaseNode* const me = stackPool[stack]; + dgAssert(me); + dgBody* const body = me->GetBody(); + if (body) { + if (!body->m_isdead) { + dgAssert(!me->GetLeft()); + dgAssert(!me->GetRight()); + dgFloat32 param = body->RayCast(line, filter, prefilter, userData, maxParam); + if (param < maxParam) { + maxParam = param; + if (maxParam < dgFloat32(1.0e-8f)) { + break; + } + } + } + } else if (me->IsAggregate()) { + dgBroadPhaseAggregate* const aggregate = (dgBroadPhaseAggregate*) me; + if (aggregate->m_root) { + const dgBroadPhaseNode* const child = aggregate->m_root; + dgAssert(child); + dgFloat32 dist1 = ray.BoxIntersect(child->m_minBox, child->m_maxBox); + if (dist1 < maxParam) { + dgInt32 j = stack; + for (; j && (dist1 > distance[j - 1]); j--) { + stackPool[j] = stackPool[j - 1]; + distance[j] = distance[j - 1]; + } + stackPool[j] = child; + distance[j] = dist1; + stack++; + dgAssert(stack < DG_BROADPHASE_MAX_STACK_DEPTH); + } + } + } else { + const dgBroadPhaseNode* const left = me->GetLeft(); + dgAssert(left); + dgFloat32 dist1 = ray.BoxIntersect(left->m_minBox, left->m_maxBox); + if (dist1 < maxParam) { + dgInt32 j = stack; + for (; j && (dist1 > distance[j - 1]); j--) { + stackPool[j] = stackPool[j - 1]; + distance[j] = distance[j - 1]; + } + stackPool[j] = left; + distance[j] = dist1; + stack++; + dgAssert(stack < DG_BROADPHASE_MAX_STACK_DEPTH); + } + + const dgBroadPhaseNode* const right = me->GetRight(); + dgAssert(right); + dist1 = ray.BoxIntersect(right->m_minBox, right->m_maxBox); + if (dist1 < maxParam) { + dgInt32 j = stack; + for (; j && (dist1 > distance[j - 1]); j--) { + stackPool[j] = stackPool[j - 1]; + distance[j] = distance[j - 1]; + } + stackPool[j] = right; + distance[j] = dist1; + stack++; + dgAssert(stack < DG_BROADPHASE_MAX_STACK_DEPTH); + } + } + } + } +} + +void dgBroadPhase::CollisionChange (dgBody* const body, dgCollisionInstance* const collision) +{ + dgCollisionInstance* const bodyCollision = body->GetCollision(); + if (bodyCollision) { + if (bodyCollision->IsType (dgCollision::dgCollisionNull_RTTI) && !collision->IsType (dgCollision::dgCollisionNull_RTTI)) { + dgAssert (!body->GetBroadPhase()); + body->m_collision = m_world->m_pointCollision; + Add (body); + body->m_collision = bodyCollision; + } else if (!bodyCollision->IsType (dgCollision::dgCollisionNull_RTTI) && collision->IsType (dgCollision::dgCollisionNull_RTTI)) { + Remove(body); + } + } +} + +void dgBroadPhase::UpdateBody(dgBody* const body, dgInt32 threadIndex) +{ + if (m_rootNode && body->m_masterNode) { + dgBroadPhaseBodyNode* const node = body->GetBroadPhase(); + dgBody* const body1 = node->GetBody(); + dgAssert(body1 == body); + dgAssert(!body1->m_equilibrium); + dgAssert(!node->GetLeft()); + dgAssert(!node->GetRight()); + dgAssert(!body1->GetCollision()->IsType(dgCollision::dgCollisionNull_RTTI)); + + if (body1->GetBroadPhaseAggregate()) { + dgBroadPhaseAggregate* const aggregate = body1->GetBroadPhaseAggregate(); + dgScopeSpinPause lock(&aggregate->m_criticalSectionLock); + aggregate->m_isInEquilibrium = body1->m_equilibrium; + } + + if (!dgBoxInclusionTest(body1->m_minAABB, body1->m_maxAABB, node->m_minBox, node->m_maxBox)) { + dgAssert(!node->IsAggregate()); + node->SetAABB(body1->m_minAABB, body1->m_maxAABB); + + if (!m_rootNode->IsLeafNode()) { + const dgBroadPhaseNode* const root = (m_rootNode->GetLeft() && m_rootNode->GetRight()) ? NULL : m_rootNode; + for (dgBroadPhaseNode* parent = node->m_parent; parent != root; parent = parent->m_parent) { + dgScopeSpinPause lock(&parent->m_criticalSectionLock); + if (!parent->IsAggregate()) { + dgVector minBox; + dgVector maxBox; + dgFloat32 area = CalculateSurfaceArea(parent->GetLeft(), parent->GetRight(), minBox, maxBox); + if (dgBoxInclusionTest(minBox, maxBox, parent->m_minBox, parent->m_maxBox)) { + break; + } + parent->m_minBox = minBox; + parent->m_maxBox = maxBox; + parent->m_surfaceArea = area; + } else { + dgBroadPhaseAggregate* const aggregate = (dgBroadPhaseAggregate*)parent; + aggregate->m_minBox = aggregate->m_root->m_minBox; + aggregate->m_maxBox = aggregate->m_root->m_maxBox; + aggregate->m_surfaceArea = aggregate->m_root->m_surfaceArea; + } + } + } + } + } +} + +dgBroadPhaseNode* dgBroadPhase::BuildTopDown(dgBroadPhaseNode** const leafArray, dgInt32 firstBox, dgInt32 lastBox, dgFitnessList::dgListNode** const nextNode) +{ + dgAssert(firstBox >= 0); + dgAssert(lastBox >= 0); + + if (lastBox == firstBox) { + return leafArray[firstBox]; + } else { + dgSpliteInfo info(&leafArray[firstBox], lastBox - firstBox + 1); + + dgBroadPhaseTreeNode* const parent = (*nextNode)->GetInfo(); + parent->m_parent = NULL; + *nextNode = (*nextNode)->GetNext(); + + parent->SetAABB(info.m_p0, info.m_p1); + + parent->m_left = BuildTopDown(leafArray, firstBox, firstBox + info.m_axis - 1, nextNode); + parent->m_left->m_parent = parent; + + parent->m_right = BuildTopDown(leafArray, firstBox + info.m_axis, lastBox, nextNode); + parent->m_right->m_parent = parent; + return parent; + } +} + + +dgBroadPhaseNode* dgBroadPhase::BuildTopDownBig(dgBroadPhaseNode** const leafArray, dgInt32 firstBox, dgInt32 lastBox, dgFitnessList::dgListNode** const nextNode) +{ + if (lastBox == firstBox) { + return BuildTopDown(leafArray, firstBox, lastBox, nextNode); + } + + dgInt32 midPoint = -1; + const dgFloat32 scale = dgFloat32 (1.0f / 64.0f); + const dgBroadPhaseNode* const node0 = leafArray[firstBox]; + const dgInt32 count = lastBox - firstBox; + dgFloat32 area0 = scale * node0->m_surfaceArea; + for (dgInt32 i = 1; i <= count; i++) { + const dgBroadPhaseNode* const node1 = leafArray[firstBox + i]; + dgFloat32 area1 = node1->m_surfaceArea; + if (area0 > area1) { + midPoint = i - 1; + break; + } + } + + if (midPoint == -1) { + return BuildTopDown(leafArray, firstBox, lastBox, nextNode); + } else { + dgBroadPhaseTreeNode* const parent = (*nextNode)->GetInfo(); + + parent->m_parent = NULL; + *nextNode = (*nextNode)->GetNext(); + + parent->m_right = BuildTopDown(leafArray, firstBox, firstBox + midPoint, nextNode); + parent->m_right->m_parent = parent; + + parent->m_left = BuildTopDownBig(leafArray, firstBox + midPoint + 1, lastBox, nextNode); + parent->m_left->m_parent = parent; + + dgVector minP (parent->m_left->m_minBox.GetMin(parent->m_right->m_minBox)); + dgVector maxP (parent->m_left->m_maxBox.GetMax(parent->m_right->m_maxBox)); + parent->SetAABB(minP, maxP); + + return parent; + } +} + + +dgInt32 dgBroadPhase::CompareNodes(const dgBroadPhaseNode* const nodeA, const dgBroadPhaseNode* const nodeB, void* const) +{ + dgFloat32 areaA = nodeA->m_surfaceArea; + dgFloat32 areaB = nodeB->m_surfaceArea; + if (areaA < areaB) { + return 1; + } + if (areaA > areaB) { + return -1; + } + return 0; +} + + +void dgBroadPhase::ImproveFitness(dgFitnessList& fitness, dgFloat64& oldEntropy, dgBroadPhaseNode** const root) +{ + if (*root) { + DG_TRACKTIME(); + dgBroadPhaseNode* const parent = (*root)->m_parent; + (*root)->m_parent = NULL; + dgFloat64 entropy = CalculateEntropy(fitness, root); + + if ((entropy > oldEntropy * dgFloat32(1.5f)) || (entropy < oldEntropy * dgFloat32(0.75f))) { + if (fitness.GetFirst()) { + m_world->m_solverJacobiansMemory.ResizeIfNecessary ((fitness.GetCount() * 2 + 16) * sizeof (dgBroadPhaseNode*)); + dgBroadPhaseNode** const leafArray = (dgBroadPhaseNode**)&m_world->m_solverJacobiansMemory[0]; + + dgInt32 leafNodesCount = 0; + for (dgFitnessList::dgListNode* nodePtr = fitness.GetFirst(); nodePtr; nodePtr = nodePtr->GetNext()) { + dgBroadPhaseNode* const node = nodePtr->GetInfo(); + dgBroadPhaseNode* const leftNode = node->GetLeft(); + dgBody* const leftBody = leftNode->GetBody(); + if (leftBody) { + node->SetAABB(leftBody->m_minAABB, leftBody->m_maxAABB); + leafArray[leafNodesCount] = leftNode; + leafNodesCount++; + } else if (leftNode->IsAggregate()) { + leafArray[leafNodesCount] = leftNode; + leafNodesCount++; + } + dgBroadPhaseNode* const rightNode = node->GetRight(); + dgBody* const rightBody = rightNode->GetBody(); + if (rightBody) { + rightNode->SetAABB(rightBody->m_minAABB, rightBody->m_maxAABB); + leafArray[leafNodesCount] = rightNode; + leafNodesCount++; + } else if (rightNode->IsAggregate()) { + leafArray[leafNodesCount] = rightNode; + leafNodesCount++; + } + } + + dgFitnessList::dgListNode* nodePtr = fitness.GetFirst(); + + dgSortIndirect(leafArray, leafNodesCount, CompareNodes); + *root = BuildTopDownBig(leafArray, 0, leafNodesCount - 1, &nodePtr); + dgAssert(!(*root)->m_parent); + //entropy = CalculateEntropy(fitness, root); + entropy = fitness.TotalCost(); + fitness.m_prevCost = entropy; + } + oldEntropy = entropy; + } + (*root)->m_parent = parent; + } +} + + +void dgBroadPhase::RotateLeft (dgBroadPhaseTreeNode* const node, dgBroadPhaseNode** const root) +{ + dgVector cost1P0; + dgVector cost1P1; + + dgBroadPhaseTreeNode* const parent = (dgBroadPhaseTreeNode*)node->m_parent; + dgAssert(parent && !parent->IsLeafNode()); + dgFloat32 cost1 = CalculateSurfaceArea(node->m_left, parent->m_left, cost1P0, cost1P1); + + dgVector cost2P0; + dgVector cost2P1; + dgFloat32 cost2 = CalculateSurfaceArea(node->m_right, parent->m_left, cost2P0, cost2P1); + + dgFloat32 cost0 = node->m_surfaceArea; + if ((cost1 <= cost0) && (cost1 <= cost2)) { + //dgBroadPhaseNode* const parent = node->m_parent; + node->m_minBox = parent->m_minBox; + node->m_maxBox = parent->m_maxBox; + node->m_surfaceArea = parent->m_surfaceArea; + + dgBroadPhaseTreeNode* const grandParent = (dgBroadPhaseTreeNode*) parent->m_parent; + if (grandParent) { + if (grandParent->m_left == parent) { + grandParent->m_left = node; + } else { + dgAssert(grandParent->m_right == parent); + grandParent->m_right = node; + } + } else { + (*root) = node; + } + + node->m_parent = parent->m_parent; + parent->m_parent = node; + node->m_left->m_parent = parent; + parent->m_right = node->m_left; + node->m_left = parent; + + parent->m_minBox = cost1P0; + parent->m_maxBox = cost1P1; + parent->m_surfaceArea = cost1; + + } else if ((cost2 <= cost0) && (cost2 <= cost1)) { + //dgBroadPhaseNode* const parent = node->m_parent; + node->m_minBox = parent->m_minBox; + node->m_maxBox = parent->m_maxBox; + node->m_surfaceArea = parent->m_surfaceArea; + + dgBroadPhaseTreeNode* const grandParent = (dgBroadPhaseTreeNode*) parent->m_parent; + if (grandParent) { + if (grandParent->m_left == parent) { + grandParent->m_left = node; + } else { + dgAssert(grandParent->m_right == parent); + grandParent->m_right = node; + } + } else { + (*root) = node; + } + + node->m_parent = parent->m_parent; + parent->m_parent = node; + node->m_right->m_parent = parent; + parent->m_right = node->m_right; + node->m_right = parent; + + parent->m_minBox = cost2P0; + parent->m_maxBox = cost2P1; + parent->m_surfaceArea = cost2; + } +} + +void dgBroadPhase::RotateRight (dgBroadPhaseTreeNode* const node, dgBroadPhaseNode** const root) +{ + dgVector cost1P0; + dgVector cost1P1; + + dgBroadPhaseTreeNode* const parent = (dgBroadPhaseTreeNode*) node->m_parent; + dgAssert (parent && !parent->IsLeafNode()); + + dgFloat32 cost1 = CalculateSurfaceArea(node->m_right, parent->m_right, cost1P0, cost1P1); + + dgVector cost2P0; + dgVector cost2P1; + dgFloat32 cost2 = CalculateSurfaceArea(node->m_left, parent->m_right, cost2P0, cost2P1); + + dgFloat32 cost0 = node->m_surfaceArea; + if ((cost1 <= cost0) && (cost1 <= cost2)) { + //dgBroadPhaseNode* const parent = node->m_parent; + node->m_minBox = parent->m_minBox; + node->m_maxBox = parent->m_maxBox; + node->m_surfaceArea = parent->m_surfaceArea; + + dgBroadPhaseTreeNode* const grandParent = (dgBroadPhaseTreeNode*) parent->m_parent; + if (grandParent) { + dgAssert (!grandParent->IsLeafNode()); + if (grandParent->m_left == parent) { + grandParent->m_left = node; + } else { + dgAssert(grandParent->m_right == parent); + grandParent->m_right = node; + } + } else { + (*root) = node; + } + + node->m_parent = parent->m_parent; + parent->m_parent = node; + node->m_right->m_parent = parent; + parent->m_left = node->m_right; + node->m_right = parent; + parent->m_minBox = cost1P0; + parent->m_maxBox = cost1P1; + parent->m_surfaceArea = cost1; + + } else if ((cost2 <= cost0) && (cost2 <= cost1)) { + //dgBroadPhaseNode* const parent = node->m_parent; + node->m_minBox = parent->m_minBox; + node->m_maxBox = parent->m_maxBox; + node->m_surfaceArea = parent->m_surfaceArea; + + dgBroadPhaseTreeNode* const grandParent = (dgBroadPhaseTreeNode*) parent->m_parent; + if (parent->m_parent) { + if (grandParent->m_left == parent) { + grandParent->m_left = node; + } else { + dgAssert(grandParent->m_right == parent); + grandParent->m_right = node; + } + } else { + (*root) = node; + } + + node->m_parent = parent->m_parent; + parent->m_parent = node; + node->m_left->m_parent = parent; + parent->m_left = node->m_left; + node->m_left = parent; + + parent->m_minBox = cost2P0; + parent->m_maxBox = cost2P1; + parent->m_surfaceArea = cost2; + } +} + +DG_INLINE bool dgBroadPhase::ValidateContactCache(dgContact* const contact, const dgVector& timestep) const +{ + dgAssert(contact && (contact->GetId() == dgConstraint::m_contactConstraint)); + + dgBody* const body0 = contact->GetBody0(); + dgBody* const body1 = contact->GetBody1(); + if (!contact->m_material->m_contactGeneration) { + dgVector positStep(timestep * (body0->m_veloc - body1->m_veloc)); + positStep = ((positStep.DotProduct(positStep)) > m_velocTol) & positStep; + contact->m_positAcc += positStep; + + dgVector positError2(contact->m_positAcc.DotProduct(contact->m_positAcc)); + if ((positError2 < m_linearContactError2).GetSignMask()) { + dgVector rotationStep(timestep * (body0->m_omega - body1->m_omega)); + rotationStep = ((rotationStep.DotProduct(rotationStep)) > m_velocTol) & rotationStep; + contact->m_rotationAcc = contact->m_rotationAcc * dgQuaternion(dgFloat32(1.0f), rotationStep.m_x, rotationStep.m_y, rotationStep.m_z); + + dgVector angle(contact->m_rotationAcc.m_x, contact->m_rotationAcc.m_y, contact->m_rotationAcc.m_z, dgFloat32(0.0f)); + dgVector rotatError2(angle.DotProduct(angle)); + if ((rotatError2 < m_angularContactError2).GetSignMask()) { + return true; + } + } + } + return false; +} + +void dgBroadPhase::CalculatePairContacts (dgPair* const pair, dgInt32 threadID) +{ + dgContactPoint contacts[DG_MAX_CONTATCS]; + + pair->m_cacheIsValid = false; + pair->m_contactBuffer = contacts; + m_world->CalculateContacts(pair, threadID, false, false); + + if (pair->m_contactCount) { + dgAssert(pair->m_contactCount <= (DG_CONSTRAINT_MAX_ROWS / 3)); + m_world->ProcessContacts(pair, threadID); + KinematicBodyActivation(pair->m_contact); + } else { + if (pair->m_cacheIsValid) { + KinematicBodyActivation(pair->m_contact); + } else { + pair->m_contact->m_maxDOF = 0; + } + } +} + +void dgBroadPhase::AddPair (dgContact* const contact, dgFloat32 timestep, dgInt32 threadIndex) +{ + //DG_TRACKTIME(); + dgWorld* const world = (dgWorld*) m_world; + dgBody* const body0 = contact->m_body0; + dgBody* const body1 = contact->m_body1; + + dgAssert (body0 != m_world->m_sentinelBody); + dgAssert (body1 != m_world->m_sentinelBody); + dgAssert (contact->GetId() == dgConstraint::m_contactConstraint); + dgAssert (body0->GetWorld()); + dgAssert (body1->GetWorld()); + dgAssert (body0->GetWorld() == world); + dgAssert (body1->GetWorld() == world); + if (!(body0->m_collideWithLinkedBodies & body1->m_collideWithLinkedBodies)) { + if (world->AreBodyConnectedByJoints (body0, body1)) { + return; + } + } + + const dgContactMaterial* const material = contact->m_material; + if (material->m_flags & dgContactMaterial::m_collisionEnable) { + dgInt32 processContacts = 1; + if (material->m_aabbOverlap) { + processContacts = material->m_aabbOverlap(*contact, timestep, threadIndex); + } + if (processContacts) { + dgPair pair; + dgAssert (!body0->m_collision->IsType (dgCollision::dgCollisionNull_RTTI)); + dgAssert (!body1->m_collision->IsType (dgCollision::dgCollisionNull_RTTI)); + + pair.m_contact = contact; + pair.m_timestep = timestep; + CalculatePairContacts (&pair, threadIndex); + } + } +} + +bool dgBroadPhase::TestOverlaping(const dgBody* const body0, const dgBody* const body1, dgFloat32 timestep) const +{ + bool mass0 = (body0->m_invMass.m_w != dgFloat32(0.0f)); + bool mass1 = (body1->m_invMass.m_w != dgFloat32(0.0f)); + bool isDynamic0 = body0->IsRTTIType(dgBody::m_dynamicBodyRTTI) != 0; + bool isDynamic1 = body1->IsRTTIType(dgBody::m_dynamicBodyRTTI) != 0; + bool isKinematic0 = body0->IsRTTIType(dgBody::m_kinematicBodyRTTI) != 0; + bool isKinematic1 = body1->IsRTTIType(dgBody::m_kinematicBodyRTTI) != 0; + + dgAssert(!body0->GetCollision()->IsType(dgCollision::dgCollisionNull_RTTI)); + dgAssert(!body1->GetCollision()->IsType(dgCollision::dgCollisionNull_RTTI)); + + const dgBroadPhaseAggregate* const agreggate0 = body0->GetBroadPhaseAggregate(); + const dgBroadPhaseAggregate* const agreggate1 = body1->GetBroadPhaseAggregate(); + + bool tier1 = true; + bool tier2 = !(body0->m_sleeping & body1->m_sleeping); + bool tier3 = (agreggate0 != agreggate1) || !agreggate0 || (agreggate0 && agreggate0->GetSelfCollision()); + bool tier4 = isDynamic0 & mass0; + bool tier5 = isDynamic1 & mass1; + bool tier6 = isKinematic0 & mass1; + bool tier7 = isKinematic1 & mass0; + bool ret = tier1 & tier2 & tier3 & (tier4 | tier5 | tier6 | tier7); + + if (ret) { + const dgCollisionInstance* const instance0 = body0->GetCollision(); + const dgCollisionInstance* const instance1 = body1->GetCollision(); + + if (body0->m_continueCollisionMode | body1->m_continueCollisionMode) { + dgVector velRelative(body1->GetVelocity() - body0->GetVelocity()); + if (velRelative.DotProduct(velRelative).GetScalar() > dgFloat32(0.25f)) { + dgVector box0_p0; + dgVector box0_p1; + dgVector box1_p0; + dgVector box1_p1; + + instance0->CalcAABB(instance0->GetGlobalMatrix(), box0_p0, box0_p1); + instance1->CalcAABB(instance1->GetGlobalMatrix(), box1_p0, box1_p1); + + dgVector boxp0(box0_p0 - box1_p1); + dgVector boxp1(box0_p1 - box1_p0); + dgFastRayTest ray(dgVector::m_zero, velRelative.Scale(timestep * dgFloat32(4.0f))); + dgFloat32 distance = ray.BoxIntersect(boxp0, boxp1); + ret = (distance < dgFloat32(1.0f)); + } else { + ret = dgOverlapTest(body0->m_minAABB, body0->m_maxAABB, body1->m_minAABB, body1->m_maxAABB) ? 1 : 0; + } + } else { + ret = dgOverlapTest(body0->m_minAABB, body0->m_maxAABB, body1->m_minAABB, body1->m_maxAABB) ? 1 : 0; + } + } + return ret; +} + +void dgBroadPhase::AddPair (dgBody* const body0, dgBody* const body1, const dgFloat32 timestep, dgInt32 threadID) +{ + dgAssert(body0); + dgAssert(body1); + const bool test = TestOverlaping (body0, body1, timestep); + if (test) { + dgContact* contact = m_contactCache.FindContactJoint(body0, body1); + if (!contact) { + const dgBilateralConstraint* const bilateral = m_world->FindBilateralJoint (body0, body1); + const bool isCollidable = bilateral ? bilateral->IsCollidable() : true; + + if (isCollidable) { + dgUnsigned32 group0_ID = dgUnsigned32 (body0->m_bodyGroupId); + dgUnsigned32 group1_ID = dgUnsigned32 (body1->m_bodyGroupId); + + if (group1_ID < group0_ID) { + dgSwap (group0_ID, group1_ID); + } + + dgUnsigned32 key = (group1_ID << 16) + group0_ID; + const dgBodyMaterialList* const materialList = m_world; + dgAssert (materialList->Find (key)); + const dgContactMaterial* const material = &materialList->Find (key)->GetInfo(); + + if (material->m_flags & dgContactMaterial::m_collisionEnable) { + + dgInt32 isBody0Kinematic = body0->IsRTTIType(dgBody::m_kinematicBodyRTTI); + dgInt32 isBody1Kinematic = body1->IsRTTIType(dgBody::m_kinematicBodyRTTI); + + const dgInt32 kinematicTest = !((isBody0Kinematic && isBody1Kinematic) || ((isBody0Kinematic && body0->IsCollidable()) || (isBody1Kinematic && body1->IsCollidable()))); + const dgInt32 collisionTest = kinematicTest && !(body0->m_isdead | body1->m_isdead) && !(body0->m_equilibrium & body1->m_equilibrium); + if (collisionTest) { + const dgInt32 isSofBody0 = body0->m_collision->IsType(dgCollision::dgCollisionLumpedMass_RTTI); + const dgInt32 isSofBody1 = body1->m_collision->IsType(dgCollision::dgCollisionLumpedMass_RTTI); + + if (isSofBody0 || isSofBody1) { + m_pendingSoftBodyCollisions[m_pendingSoftBodyPairsCount].m_body0 = body0; + m_pendingSoftBodyCollisions[m_pendingSoftBodyPairsCount].m_body1 = body1; + m_pendingSoftBodyPairsCount++; + } else { + dgContactList& contactList = *m_world; + dgAtomicExchangeAndAdd(&contactList.m_contactCountReset, 1); + if (contactList.m_contactCount < contactList.GetElementsCapacity()) { + contact = new (m_world->m_allocator) dgContact(m_world, material, body0, body1); + dgAssert(contact); + contactList.Push(contact); + } + } + } + } + } + } + } +} + +void dgBroadPhase::FindGeneratedBodiesCollidingPairs(dgBroadphaseSyncDescriptor* const descriptor, dgInt32 threadID) +{ +dgAssert (0); +/* + dgList::dgListNode* node = NULL; + { + dgThreadHiveScopeLock lock(m_world, &m_criticalSectionLock, false); + node = descriptor->m_newBodiesNodes; + if (node) { + descriptor->m_newBodiesNodes = node->GetNext(); + } + } + + dgVector timestep2(descriptor->m_timestep * descriptor->m_timestep * dgFloat32(4.0f)); + while (node) { + dgBody* const body = node->GetInfo(); + dgBroadPhaseNode* const breadPhaseNode = body->GetBroadPhase(); + if (breadPhaseNode) { + if (!body->m_collision->IsType(dgCollision::dgCollisionNull_RTTI)) { + for (dgBroadPhaseNode* ptr = breadPhaseNode; ptr->m_parent; ptr = ptr->m_parent) { + dgBroadPhaseNode* const sibling = ptr->m_parent->m_right; + if (sibling != ptr) { + dgAssert(0); + //SubmitPairs (bodyNode, sibling, timestep2, threadID); + } + else { + dgAssert(0); + //dgNode* const sibling = ptr->m_parent->m_left; + //dgAssert (sibling); + //dgAssert (sibling != ptr); + //dgAssert (0); + //SubmitPairs (bodyNode, sibling, timestep2, threadID); + } + } + } + } + + dgThreadHiveScopeLock lock(m_world, &m_criticalSectionLock, false); + node = descriptor->m_newBodiesNodes; + if (node) { + descriptor->m_newBodiesNodes = node->GetNext(); + } + } +*/ +} + + +void dgBroadPhase::SubmitPairs(dgBroadPhaseNode* const leafNode, dgBroadPhaseNode* const node, dgFloat32 timestep, dgInt32 threadCount, dgInt32 threadID) +{ + dgBroadPhaseNode* pool[DG_BROADPHASE_MAX_STACK_DEPTH]; + pool[0] = node; + dgInt32 stack = 1; + + dgAssert (leafNode->IsLeafNode()); + dgBody* const body0 = leafNode->GetBody(); + + const dgVector boxP0 (body0 ? body0->m_minAABB : leafNode->m_minBox); + const dgVector boxP1 (body0 ? body0->m_maxAABB : leafNode->m_maxBox); + + const bool test0 = body0 ? (body0->GetInvMass().m_w != dgFloat32(0.0f)) : true; + + while (stack) { + stack--; + dgBroadPhaseNode* const rootNode = pool[stack]; + if (dgOverlapTest(rootNode->m_minBox, rootNode->m_maxBox, boxP0, boxP1)) { + if (rootNode->IsLeafNode()) { + dgAssert(!rootNode->GetRight()); + dgAssert(!rootNode->GetLeft()); + dgBody* const body1 = rootNode->GetBody(); + if (body0) { + if (body1) { + if (test0 || (body1->GetInvMass().m_w != dgFloat32(0.0f))) { + AddPair(body0, body1, timestep, threadID); + } + } else { + dgAssert (rootNode->IsAggregate()); + dgBroadPhaseAggregate* const aggregate = (dgBroadPhaseAggregate*) rootNode; + aggregate->SummitPairs(body0, timestep, threadID); + } + } else { + dgAssert (leafNode->IsAggregate()); + dgBroadPhaseAggregate* const aggregate = (dgBroadPhaseAggregate*) leafNode; + if (body1) { + aggregate->SummitPairs(body1, timestep, threadID); + } else { + dgAssert (rootNode->IsAggregate()); + aggregate->SummitPairs((dgBroadPhaseAggregate*) rootNode, timestep, threadID); + } + } + } else { + dgBroadPhaseTreeNode* const tmpNode = (dgBroadPhaseTreeNode*) rootNode; + dgAssert (tmpNode->m_left); + dgAssert (tmpNode->m_right); + + pool[stack] = tmpNode->m_left; + stack++; + dgAssert(stack < dgInt32(sizeof (pool) / sizeof (pool[0]))); + + pool[stack] = tmpNode->m_right; + stack++; + dgAssert(stack < dgInt32(sizeof (pool) / sizeof (pool[0]))); + } + } + } +} + +void dgBroadPhase::ImproveNodeFitness(dgBroadPhaseTreeNode* const node, dgBroadPhaseNode** const root) +{ + dgAssert(node->GetLeft()); + dgAssert(node->GetRight()); + + dgBroadPhaseNode* const parent = node->m_parent; + if (parent && parent->m_parent) { + dgAssert (!parent->IsLeafNode()); + if (parent->GetLeft() == node) { + RotateRight(node, root); + } else { + RotateLeft(node, root); + } + } + dgAssert(!m_rootNode->m_parent); +} + +dgFloat64 dgBroadPhase::CalculateEntropy (dgFitnessList& fitness, dgBroadPhaseNode** const root) +{ + DG_TRACKTIME(); +#if 0 + dgFloat64 cost0 = fitness.TotalCost(); + dgFloat64 cost1 = cost0; + do { + cost0 = cost1; + for (dgFitnessList::dgListNode* node = fitness.GetFirst(); node; node = node->GetNext()) { + ImproveNodeFitness(node->GetInfo(), root); + } + cost1 = fitness.TotalCost(); + } while (cost1 < (dgFloat32(0.99f)) * cost0); + return cost1; +#else + dgFloat64 cost = dgFloat32 (0.0f); + if (fitness.GetCount() < 32) { + for (dgFitnessList::dgListNode* node = fitness.GetFirst(); node; node = node->GetNext()) { + ImproveNodeFitness(node->GetInfo(), root); + } + cost = fitness.TotalCost(); + fitness.m_prevCost = cost; + } else { + const dgInt32 mod = 16; + cost = fitness.m_prevCost; + dgFitnessList::dgListNode* node = fitness.GetFirst(); + for (dgInt32 i = 0; i < fitness.m_index; i++) { + node = node->GetNext(); + } + + do { + ImproveNodeFitness(node->GetInfo(), root); + for (dgInt32 i = 0; i < mod; i++) { + node = node ? node->GetNext() : NULL; + } + } while (node); + + if (!fitness.m_index) { + cost = fitness.TotalCost(); + fitness.m_prevCost = cost; + } + fitness.m_index = (fitness.m_index + 1) % mod; + } + return cost; +#endif +} + +void dgBroadPhase::KinematicBodyActivation (dgContact* const contatJoint) const +{ + dgBody* const body0 = contatJoint->GetBody0(); + dgBody* const body1 = contatJoint->GetBody1(); + if (body0->IsCollidable() | body1->IsCollidable()) { + if (body0->IsRTTIType(dgBody::m_kinematicBodyRTTI)) { + if (body1->IsRTTIType(dgBody::m_dynamicBodyRTTI) && (body1->GetInvMass().m_w > dgFloat32 (0.0f))) { + if (body1->m_equilibrium) { + dgVector relVeloc (body0->m_veloc - body1->m_veloc); + dgVector relOmega (body0->m_omega - body1->m_omega); + dgVector mask2 ((relVeloc.DotProduct(relVeloc) < dgDynamicBody::m_equilibriumError2) & (relOmega.DotProduct(relOmega) < dgDynamicBody::m_equilibriumError2)); + + dgScopeSpinPause lock(&body1->m_criticalSectionLock); + body1->m_sleeping = false; + body1->m_equilibrium = mask2.GetSignMask() ? true : false; + } + } + } else if (body1->IsRTTIType(dgBody::m_kinematicBodyRTTI)) { + if (body0->IsRTTIType(dgBody::m_dynamicBodyRTTI) && (body0->GetInvMass().m_w > dgFloat32 (0.0f))) { + if (body0->m_equilibrium) { + dgVector relVeloc (body0->m_veloc - body1->m_veloc); + dgVector relOmega (body0->m_omega - body1->m_omega); + dgVector mask2 ((relVeloc.DotProduct(relVeloc) < dgDynamicBody::m_equilibriumError2) & (relOmega.DotProduct(relOmega) < dgDynamicBody::m_equilibriumError2)); + + dgScopeSpinPause lock(&body1->m_criticalSectionLock); + body0->m_sleeping = false; + body0->m_equilibrium = mask2.GetSignMask() ? true : false; + } + } + } + } +} + +void dgBroadPhase::CollidingPairsKernel(void* const context, void* const node, dgInt32 threadID) +{ + D_TRACKTIME(); + dgBroadphaseSyncDescriptor* const descriptor = (dgBroadphaseSyncDescriptor*)context; + dgWorld* const world = descriptor->m_world; + dgBroadPhase* const broadPhase = world->GetBroadPhase(); + broadPhase->FindCollidingPairs(descriptor, (dgList::dgListNode*) node, threadID); +} + +void dgBroadPhase::AddGeneratedBodiesContactsKernel (void* const context, void* const worldContext, dgInt32 threadID) +{ + D_TRACKTIME(); + dgBroadphaseSyncDescriptor* const descriptor = (dgBroadphaseSyncDescriptor*) context; + dgWorld* const world = (dgWorld*) worldContext; + dgBroadPhase* const broadPhase = world->GetBroadPhase(); + broadPhase->FindGeneratedBodiesCollidingPairs (descriptor, threadID); +} + +void dgBroadPhase::UpdateSoftBodyContactKernel(void* const context, void* const worldContext, dgInt32 threadID) +{ + D_TRACKTIME(); + dgBroadphaseSyncDescriptor* const descriptor = (dgBroadphaseSyncDescriptor*)context; + dgWorld* const world = descriptor->m_world; + dgBroadPhase* const broadPhase = world->GetBroadPhase(); + broadPhase->UpdateSoftBodyContacts(descriptor, descriptor->m_timestep, threadID); +} + +void dgBroadPhase::UpdateRigidBodyContactKernel(void* const context, void* const , dgInt32 threadID) +{ + D_TRACKTIME(); + dgBroadphaseSyncDescriptor* const descriptor = (dgBroadphaseSyncDescriptor*)context; + dgWorld* const world = descriptor->m_world; + dgBroadPhase* const broadPhase = world->GetBroadPhase(); + broadPhase->UpdateRigidBodyContacts(descriptor, descriptor->m_timestep, threadID); +} + +void dgBroadPhase::UpdateSoftBodyContacts(dgBroadphaseSyncDescriptor* const descriptor, dgFloat32 timeStep, dgInt32 threadID) +{ + dgAssert(0); +/* + const dgInt32 count = m_pendingSoftBodyPairsCount; + for (dgInt32 i = dgAtomicExchangeAndAdd(&descriptor->m_pairsAtomicCounter, 1); i < count; i = dgAtomicExchangeAndAdd(&descriptor->m_pairsAtomicCounter, 1)) { + dgPendingCollisionSoftBodies& pair = m_pendingSoftBodyCollisions[i]; + if (pair.m_body0->m_collision->IsType(dgCollision::dgCollisionLumpedMass_RTTI)) { + dgCollisionLumpedMassParticles* const lumpedMassShape = (dgCollisionLumpedMassParticles*)pair.m_body0->m_collision->GetChildShape(); + dgAssert(pair.m_body0->IsRTTIType(dgBody::m_dynamicBodyRTTI)); + dgAssert (pair.m_body0 == lumpedMassShape->GetOwner ()); + lumpedMassShape->RegisterCollision(pair.m_body1); + } else if (pair.m_body1->m_collision->IsType(dgCollision::dgCollisionLumpedMass_RTTI)) { + dgCollisionLumpedMassParticles* const lumpedMassShape = (dgCollisionLumpedMassParticles*)pair.m_body1->m_collision->GetChildShape(); + dgAssert(pair.m_body1->IsRTTIType(dgBody::m_dynamicBodyRTTI)); + dgAssert (pair.m_body1 == lumpedMassShape->GetOwner ()); + lumpedMassShape->RegisterCollision(pair.m_body0); + } + } +*/ +} + +void dgBroadPhase::UpdateRigidBodyContacts(dgBroadphaseSyncDescriptor* const descriptor, dgFloat32 timeStep, dgInt32 threadID) +{ + DG_TRACKTIME(); + + dgContactList& contactList = *m_world; + const dgFloat32 timestep = descriptor->m_timestep; + const dgInt32 threadCount = m_world->GetThreadCount(); + const dgUnsigned32 lru = m_lru - DG_CONTACT_DELAY_FRAMES; + + const dgInt32 contactCount = contactList.m_contactCount; + dgContact** const contactArray = &contactList[0]; + + dgVector deltaTime(timestep); + for (dgInt32 i = threadID; i < contactCount; i += threadCount) { + dgContact* const contact = contactArray[i]; + dgAssert (contact); + + dgBody* const body0 = contact->GetBody0(); + dgBody* const body1 = contact->GetBody1(); + + if (!(contact->m_killContact | (body0->m_equilibrium & body1->m_equilibrium))) { + dgAssert(!contact->m_killContact); + + bool isActive = contact->m_isActive; + if (ValidateContactCache(contact, deltaTime)) { + contact->m_broadphaseLru = m_lru; + contact->m_timeOfImpact = dgFloat32(1.0e10f); + } else { + contact->m_isActive = 0; + contact->m_positAcc = dgVector::m_zero; + contact->m_rotationAcc = dgQuaternion(); + + dgFloat32 distance = contact->m_separationDistance; + if (distance >= DG_NARROW_PHASE_DIST) { + const dgVector veloc0 (body0->GetVelocity()); + const dgVector veloc1 (body1->GetVelocity()); + + const dgVector veloc(veloc1 - veloc0); + const dgVector omega0 (body0->GetOmega()); + const dgVector omega1 (body1->GetOmega()); + const dgCollisionInstance* const collision0 = body0->GetCollision(); + const dgCollisionInstance* const collision1 = body1->GetCollision(); + const dgVector scale(dgFloat32(1.0f), dgFloat32(3.5f) * collision0->GetBoxMaxRadius(), dgFloat32(3.5f) * collision1->GetBoxMaxRadius(), dgFloat32(0.0f)); + const dgVector velocMag2(veloc.DotProduct(veloc).GetScalar(), omega0.DotProduct(omega0).GetScalar(), omega1.DotProduct(omega1).GetScalar(), dgFloat32(0.0f)); + const dgVector velocMag(velocMag2.GetMax(dgVector::m_epsilon).InvSqrt() * velocMag2 * scale); + const dgFloat32 speed = velocMag.AddHorizontal().GetScalar() + dgFloat32(0.5f); + + distance -= speed * timestep; + contact->m_separationDistance = distance; + } + if (distance < DG_NARROW_PHASE_DIST) { + AddPair(contact, timestep, threadID); + if (contact->m_maxDOF) { + contact->m_timeOfImpact = dgFloat32(1.0e10f); + } + contact->m_broadphaseLru = m_lru; + } else { + dgAssert (contact->m_maxDOF == 0); + const dgBroadPhaseNode* const bodyNode0 = contact->GetBody0()->m_broadPhaseNode; + const dgBroadPhaseNode* const bodyNode1 = contact->GetBody1()->m_broadPhaseNode; + if (dgOverlapTest(bodyNode0->m_minBox, bodyNode0->m_maxBox, bodyNode1->m_minBox, bodyNode1->m_maxBox)) { + contact->m_broadphaseLru = m_lru; + } else if (contact->m_broadphaseLru < lru) { + contact->m_killContact = 1; + } + } + } + + if (isActive ^ contact->m_isActive) { + if (body0->GetInvMass().m_w) { + body0->m_equilibrium = false; + } + if (body1->GetInvMass().m_w) { + body1->m_equilibrium = false; + } + } + + } else { + contact->m_broadphaseLru = m_lru; + } + + contact->m_killContact = contact->m_killContact | (body0->m_equilibrium & body1->m_equilibrium & !contact->m_isActive); + } +} + +bool dgBroadPhase::SanityCheck() const +{ +#ifdef _DEBUG + class dgKey + { + public: + dgKey (dgContact* const contact) + :m_low(dgMin (contact->GetBody0()->m_uniqueID, contact->GetBody1()->m_uniqueID)) + ,m_high(dgMax (contact->GetBody0()->m_uniqueID, contact->GetBody1()->m_uniqueID)) + { + } + + bool operator> (const dgKey& key) const + { + return m_key > key.m_key; + } + + bool operator< (const dgKey& key) const + { + return m_key < key.m_key; + } + + union + { + dgUnsigned64 m_key; + struct + { + dgInt32 m_low; + dgInt32 m_high; + }; + }; + }; + + dgTree filter (m_world->GetAllocator()); + + const dgContactList& contactList = *m_world; + for (dgInt32 i = contactList.m_contactCount - 1; i >= 0; i--) { + dgContact* const contact = contactList[i]; + dgAssert (!contact->m_killContact); + dgAssert (filter.Insert(0, dgKey(contact))); + } +#endif + return true; +} + +void dgBroadPhase::AttachNewContact(dgInt32 startCount) +{ + DG_TRACKTIME(); + dgContactList& contactList = *m_world; + if (contactList.m_contactCountReset > contactList.m_contactCount) { + contactList.Resize(contactList.GetElementsCapacity() * 2); + } + + dgContact** const contactArray = &contactList[0]; + for (dgInt32 i = contactList.m_contactCount - 1; i >= startCount; i--) { + dgContact* const contact = contactArray[i]; + if (m_contactCache.AddContactJoint(contact)) { + m_world->AttachContact(contact); + } else { + contactList.m_contactCount--; + contactArray[i] = contactList[contactList.m_contactCount]; + delete contact; + } + } +} + +void dgBroadPhase::DeleteDeadContact(dgFloat32 timestep) +{ + DG_TRACKTIME(); + dgInt32 activeCount = 0; + dgContactList& contactList = *m_world; + dgContact** const contactArray = &contactList[0]; + dgArray& constraintArray = m_world->m_jointsMemory; + for (dgInt32 i = contactList.m_contactCount - 1; i >= 0; i--) { + dgContact* const contact = contactArray[i]; + if (contact->m_killContact) { + m_contactCache.RemoveContactJoint(contact); + m_world->RemoveContact(contact); + contactList.m_contactCount--; + contactArray[i] = contactList[contactList.m_contactCount]; + delete contact; + } else if (contact->m_isActive && contact->m_maxDOF){ + constraintArray[activeCount].m_joint = contact; + activeCount++; + } else if (contact->m_body0->m_continueCollisionMode | contact->m_body1->m_continueCollisionMode){ + if (contact->EstimateCCD(timestep)) { + constraintArray[activeCount].m_joint = contact; + activeCount++; + } + } + } + dgAssert(SanityCheck()); + contactList.m_activeContactCount = activeCount; + //dgTrace (("%d %d\n", contactList.m_activeContactCount, contactList.m_contactCount)); +} + +void dgBroadPhase::UpdateContacts(dgFloat32 timestep) +{ + D_TRACKTIME(); + m_lru = m_lru + 1; + m_pendingSoftBodyPairsCount = 0; + + const dgInt32 threadsCount = m_world->GetThreadCount(); + + const dgBodyMasterList* const masterList = m_world; + + m_world->m_bodiesMemory.ResizeIfNecessary(masterList->GetCount()); + dgBroadphaseSyncDescriptor syncPoints(timestep, m_world); + + dgBodyMasterList::dgListNode* node = masterList->GetFirst()->GetNext(); + for (dgInt32 i = 0; i < threadsCount; i++) { + m_world->QueueJob(ForceAndToqueKernel, &syncPoints, node, "dgBroadPhase::ForceAndToque"); + node = node ? node->GetNext() : NULL; + } + m_world->SynchronizationBarrier(); + + // update pre-listeners after the force and torque are applied + if (m_world->m_listeners.GetCount()) { + for (dgWorld::dgListenerList::dgListNode* node1 = m_world->m_listeners.GetFirst(); node1; node1 = node1->GetNext()) { + dgWorld::dgListener& listener = node1->GetInfo(); + if (listener.m_onPreUpdate) { + listener.m_onPreUpdate(m_world, listener.m_userData, timestep); + } + } + } + + // check for sleeping bodies states + node = masterList->GetFirst()->GetNext(); + for (dgInt32 i = 0; i < threadsCount; i++) { + m_world->QueueJob(SleepingStateKernel, &syncPoints, node, "dgBroadPhase::SleepingState"); + node = node ? node->GetNext() : NULL; + } + m_world->SynchronizationBarrier(); + + // this will move to an asynchronous thread + dgList::dgListNode* aggregateNode = m_aggregateList.GetFirst(); + for (dgInt32 i = 0; i < threadsCount; i++) { + m_world->QueueJob(UpdateAggregateEntropyKernel, &syncPoints, aggregateNode, "dgBroadPhase::UpdateAggregateEntropy"); + aggregateNode = aggregateNode ? aggregateNode->GetNext() : NULL; + } + m_world->SynchronizationBarrier(); + + UpdateFitness(); + + dgContactList& contactList = *m_world; + contactList.m_contactCountReset = contactList.m_contactCount; + syncPoints.m_contactStart = contactList.m_contactCount; + + syncPoints.m_fullScan = syncPoints.m_fullScan || (syncPoints.m_atomicPendingBodiesCount >= (syncPoints.m_atomicDynamicsCount / 2)); + dgList::dgListNode* broadPhaseNode = m_updateList.GetFirst(); + for (dgInt32 i = 0; i < threadsCount; i++) { + m_world->QueueJob(CollidingPairsKernel, &syncPoints, broadPhaseNode, "dgBroadPhase::CollidingPairs"); + broadPhaseNode = broadPhaseNode ? broadPhaseNode->GetNext() : NULL; + } + m_world->SynchronizationBarrier(); + + AttachNewContact(syncPoints.m_contactStart); + for (dgInt32 i = 0; i < threadsCount; i++) { + m_world->QueueJob(UpdateRigidBodyContactKernel, &syncPoints, NULL, "dgBroadPhase::UpdateRigidBodyContact"); + } + m_world->SynchronizationBarrier(); + + if (m_pendingSoftBodyPairsCount) { + dgAssert (0); + //for (dgInt32 i = 0; i < threadsCount; i++) { + // m_world->QueueJob(UpdateSoftBodyContactKernel, &syncPoints, contactListNode, "dgBroadPhase::UpdateSoftBodyContact"); + //} + //m_world->SynchronizationBarrier(); + } + + // m_recursiveChunks = false; + if (m_generatedBodies.GetCount()) { + dgAssert(0); + //syncPoints.m_newBodiesNodes = m_generatedBodies.GetFirst(); + //for (dgInt32 i = 0; i < threadsCount; i++) { + // m_world->QueueJob(AddGeneratedBodiesContactsKernel, &syncPoints, m_world); + //} + //m_world->SynchronizationBarrier(); + // + //for (dgInt32 i = 0; i < threadsCount; i++) { + // m_world->QueueJob(UpdateContactsKernel, &syncPoints, m_world); + //} + //m_world->SynchronizationBarrier(); + // + //m_generatedBodies.RemoveAll(); + } + + DeleteDeadContact(timestep); +} diff --git a/thirdparty/src/newton/dgPhysics/dgBroadPhase.h b/thirdparty/src/newton/dgPhysics/dgBroadPhase.h new file mode 100644 index 000000000..31e95404e --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgBroadPhase.h @@ -0,0 +1,603 @@ +/* Copyright (c) <2003-2019> +* +* 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_BROADPHASE_H_ +#define __DG_BROADPHASE_H_ + +#include "dgPhysicsStdafx.h" +#include "dgBody.h" +#include "dgBodyMasterList.h" + +class dgBody; +class dgWorld; +class dgContact; +class dgCollision; +class dgDynamicBody; +class dgCollisionInstance; +class dgBroadPhaseAggregate; + + +#define DG_CACHE_DIST_TOL dgFloat32 (1.0e-3f) +#define DG_BROADPHASE_MAX_STACK_DEPTH 256 + +class dgConvexCastReturnInfo +{ + public: + dgFloat32 m_point[4]; // collision point in global space + dgFloat32 m_normal[4]; // surface normal at collision point in global space + //dgFloat32 m_normalOnHitPoint[4]; // surface normal at the surface of the hit body, + // is the same as the normal calculate by a raycast passing by the hit point in the direction of the cast + dgInt64 m_contaID; // collision ID at contact point + const dgBody* m_hitBody; // body hit at contact point + dgFloat32 m_penetration; // contact penetration at collision point +}; + + +DG_MSC_VECTOR_ALIGNMENT +class dgBroadPhaseNode +{ + public: + DG_CLASS_ALLOCATOR(allocator) + dgBroadPhaseNode(dgBroadPhaseNode* const parent) + :m_minBox(dgFloat32(-1.0e15f)) + ,m_maxBox(dgFloat32(1.0e15f)) + ,m_parent(parent) + ,m_surfaceArea(dgFloat32(1.0e20f)) + ,m_criticalSectionLock(0) + { + } + + virtual ~dgBroadPhaseNode() + { + } + + virtual bool IsSegregatedRoot() const + { + return false; + } + + virtual bool IsLeafNode() const + { + return false; + } + + virtual bool IsAggregate() const + { + return false; + } + + void SetAABB(const dgVector& minBox, const dgVector& maxBox) + { + dgAssert(minBox.m_x <= maxBox.m_x); + dgAssert(minBox.m_y <= maxBox.m_y); + dgAssert(minBox.m_z <= maxBox.m_z); + + dgVector p0(minBox * m_broadPhaseScale); + dgVector p1(maxBox * m_broadPhaseScale + dgVector::m_one); + + m_minBox = p0.Floor() * m_broadInvPhaseScale; + m_maxBox = p1.Floor() * m_broadInvPhaseScale; + + dgAssert(m_minBox.m_w == dgFloat32(0.0f)); + dgAssert(m_maxBox.m_w == dgFloat32(0.0f)); + + dgVector side0(m_maxBox - m_minBox); + m_surfaceArea = side0.DotProduct(side0.ShiftTripleRight()).m_x; + } + + virtual dgBody* GetBody() const + { + return NULL; + } + + virtual dgBroadPhaseNode* GetLeft() const + { + return NULL; + } + + virtual dgBroadPhaseNode* GetRight() const + { + return NULL; + } + + dgVector m_minBox; + dgVector m_maxBox; + dgBroadPhaseNode* m_parent; + dgFloat32 m_surfaceArea; + dgInt32 m_criticalSectionLock; + + static dgVector m_broadPhaseScale; + static dgVector m_broadInvPhaseScale; +} DG_GCC_VECTOR_ALIGNMENT; + + +class dgBroadPhaseBodyNode: public dgBroadPhaseNode +{ + public: + dgBroadPhaseBodyNode(dgBody* const body) + :dgBroadPhaseNode(NULL) + ,m_body(body) + ,m_updateNode(NULL) + { + SetAABB(body->m_minAABB, body->m_maxAABB); + m_body->SetBroadPhase(this); + } + + virtual ~dgBroadPhaseBodyNode() + { + m_body->SetBroadPhase(NULL); + } + + virtual bool IsLeafNode() const + { + return true; + } + + virtual dgBody* GetBody() const + { + return m_body; + } + + dgBody* m_body; + dgList::dgListNode* m_updateNode; +}; + +class dgBroadPhaseTreeNode: public dgBroadPhaseNode +{ + public: + dgBroadPhaseTreeNode() + :dgBroadPhaseNode(NULL) + ,m_left(NULL) + ,m_right(NULL) + ,m_fitnessNode(NULL) + { + } + + dgBroadPhaseTreeNode(dgBroadPhaseNode* const sibling, dgBroadPhaseNode* const myNode) + :dgBroadPhaseNode(sibling->m_parent) + ,m_left(sibling) + ,m_right(myNode) + ,m_fitnessNode(NULL) + { + if (m_parent) { + dgBroadPhaseTreeNode* const myParent = (dgBroadPhaseTreeNode*)m_parent; + if (myParent->m_left == sibling) { + myParent->m_left = this; + } else { + dgAssert(myParent->m_right == sibling); + myParent->m_right = this; + } + } + + sibling->m_parent = this; + myNode->m_parent = this; + + dgBroadPhaseNode* const left = m_left; + dgBroadPhaseNode* const right = m_right; + + m_minBox = left->m_minBox.GetMin(right->m_minBox); + m_maxBox = left->m_maxBox.GetMax(right->m_maxBox); + dgVector side0(m_maxBox - m_minBox); + m_surfaceArea = side0.DotProduct(side0.ShiftTripleRight()).m_x; + } + + virtual ~dgBroadPhaseTreeNode() + { + if (m_left) { + delete m_left; + } + if (m_right) { + delete m_right; + } + } + + virtual dgBroadPhaseNode* GetLeft() const + { + return m_left; + } + + virtual dgBroadPhaseNode* GetRight() const + { + return m_right; + } + + dgBroadPhaseNode* m_left; + dgBroadPhaseNode* m_right; + dgList::dgListNode* m_fitnessNode; +} DG_GCC_VECTOR_ALIGNMENT; + +#define DG_CONTACT_CACHE_LINE_SIZE 4 + +class dgBroadPhase +{ + protected: + + class CacheEntryTag + { + public: + DG_INLINE CacheEntryTag() {} + DG_INLINE CacheEntryTag(dgUnsigned32 tag0, dgUnsigned32 tag1) + :m_tagLow(dgMin(tag0, tag1)) + ,m_tagHigh(dgMax(tag0, tag1)) + { + } + + DG_INLINE dgUnsigned32 GetHash() const + { + return m_tagHigh * 31415821u + m_tagLow; + } + + union + { + dgUnsigned64 m_tag; + struct + { + dgUnsigned32 m_tagLow; + dgUnsigned32 m_tagHigh; + }; + }; + }; + + class dgContactCacheLine + { + public: + DG_INLINE dgContactCacheLine() + { + } + + dgInt32 m_count; + dgUnsigned32 m_key; + dgContact* m_contact[DG_CONTACT_CACHE_LINE_SIZE]; + dgInt32 m_hashKey[DG_CONTACT_CACHE_LINE_SIZE]; + CacheEntryTag m_tags[DG_CONTACT_CACHE_LINE_SIZE]; + }; + + class dgContactCache: public dgArray + { + public: + dgContactCache (dgMemoryAllocator* const allocator) + :dgArray(allocator) + ,m_count(1<<10) + { + Init(); + } + + void Flush() + { + Clear(); + Init(); + } + + void Init() + { + m_count = 1 << 10; + ResizeIfNecessary(m_count); + dgContactCacheLine* const cache = &(*this)[0]; + memset(cache, 0, m_count * sizeof(dgContactCacheLine)); + } + + DG_INLINE dgContact* FindContactJoint(const dgBody* const body0, const dgBody* const body1) const + { + CacheEntryTag tag(body0->m_uniqueID, body1->m_uniqueID); + dgUnsigned32 hash = tag.GetHash(); + + dgInt32 entry = hash & (m_count - 1); + + const dgContactCacheLine& cacheLine = (*this)[entry]; + for (dgInt32 i = 0; i < cacheLine.m_count; i++) { + if (cacheLine.m_tags[i].m_tag == tag.m_tag) { + return cacheLine.m_contact[i]; + } + } + return NULL; + } + + DG_INLINE bool AddContactJoint(dgContact* const joint) + { + // note this function is not thread safe + CacheEntryTag tag(joint->GetBody0()->m_uniqueID, joint->GetBody1()->m_uniqueID); + dgUnsigned32 hash = tag.GetHash(); + + dgInt32 entry = hash & (m_count - 1); + dgContactCacheLine* cacheLine = &(*this)[entry]; + + for (dgInt32 i = cacheLine->m_count - 1; i >= 0; i--) { + if (cacheLine->m_tags[i].m_tag == tag.m_tag) { + return false; + } + } + + while (cacheLine->m_count == 4) { + Rehash(); + entry = hash & (m_count - 1); + cacheLine = &(*this)[entry]; + } + if (cacheLine->m_count == 0) { + cacheLine->m_key = hash; + } + + const dgInt32 index = cacheLine->m_count; + cacheLine->m_count++; + cacheLine->m_tags[index] = tag; + cacheLine->m_hashKey[index] = hash; + cacheLine->m_contact[index] = joint; + return true; + } + + DG_INLINE void RemoveContactJoint(dgContact* const joint) + { + CacheEntryTag tag(joint->GetBody0()->m_uniqueID, joint->GetBody1()->m_uniqueID); + dgUnsigned32 hash = tag.GetHash(); + + dgInt32 entry = hash & (m_count - 1); + dgContactCacheLine* const cacheLine = &(*this)[entry]; + for (dgInt32 i = cacheLine->m_count - 1; i >= 0 ; i--) { + if (cacheLine->m_tags[i].m_tag == tag.m_tag) { + cacheLine->m_count--; + const dgInt32 index = cacheLine->m_count; + cacheLine->m_tags[i] = cacheLine->m_tags[index]; + cacheLine->m_hashKey[i] = cacheLine->m_hashKey[index]; + cacheLine->m_contact[i] = cacheLine->m_contact[index]; + break; + } + } + } + + private: + void Rehash() + { + const dgInt32 newCount = m_count * 2; + ResizeIfNecessary(newCount); + dgContactCacheLine* const cache0 = &(*this)[0]; + dgContactCacheLine* const cache1 = &cache0[m_count]; + + const dgInt32 mask = newCount - 1; + for (dgInt32 i = 0; i < m_count; i++) { + dgContactCacheLine* const src = &cache0[i]; + dgContactCacheLine* const dst = &cache1[i]; + dst->m_count = 0; + for (dgInt32 j = src->m_count - 1; j >= 0; j--) { + dgInt32 entry = src->m_hashKey[j] & mask; + if (entry >= m_count) { + const dgInt32 dstIndex = dst->m_count; + dst->m_count++; + dst->m_tags[dstIndex] = src->m_tags[j]; + dst->m_hashKey[dstIndex] = src->m_hashKey[j]; + dst->m_contact[dstIndex] = src->m_contact[j]; + + src->m_count--; + const dgInt32 srcIndex = src->m_count; + src->m_tags[j] = src->m_tags[srcIndex]; + src->m_hashKey[j] = src->m_hashKey[srcIndex]; + src->m_contact[j] = src->m_contact[srcIndex]; + } + } + } + m_count = newCount; + } + + dgInt32 m_count; + }; + + class dgSpliteInfo; + class dgBroadphaseSyncDescriptor + { + public: + dgBroadphaseSyncDescriptor(dgFloat32 timestep, dgWorld* const world) + :m_world(world) + ,m_timestep(timestep) + ,m_atomicIndex(0) + ,m_contactStart(0) + ,m_atomicDynamicsCount(0) + ,m_atomicPendingBodiesCount(0) + ,m_fullScan(false) + { + } + + dgWorld* m_world; + dgFloat32 m_timestep; + dgInt32 m_atomicIndex; + dgInt32 m_contactStart; + dgInt32 m_atomicDynamicsCount; + dgInt32 m_atomicPendingBodiesCount; + bool m_fullScan; + }; + + class dgFitnessList: public dgList + { + public: + dgFitnessList(dgMemoryAllocator* const allocator) + :dgList (allocator) + ,m_index(0) + ,m_prevCost(dgFloat32 (0.0f)) + { + } + + dgFloat64 TotalCost() const + { + dgFloat64 cost = dgFloat32(0.0f); + for (dgListNode* node = GetFirst(); node; node = node->GetNext()) { + dgBroadPhaseNode* const box = node->GetInfo(); + cost += box->m_surfaceArea; + } + return cost; + } + + dgInt32 m_index; + dgFloat64 m_prevCost; + }; + + public: + enum dgContactCode + { + m_close, + m_persist, + m_separated, + }; + + class dgPair + { + public: + dgContact* m_contact; + dgContactPoint* m_contactBuffer; + dgFloat32 m_timestep; + dgInt32 m_contactCount : 16; + dgInt32 m_cacheIsValid : 1; + dgInt32 m_flipContacts : 1; + }; + + dgBroadPhase(dgWorld* const world); + virtual ~dgBroadPhase(); + + DG_INLINE dgUnsigned32 GetLRU() const + { + return m_lru; + } + + DG_INLINE dgFloat32 CalculateSurfaceArea(const dgBroadPhaseNode* const node0, const dgBroadPhaseNode* const node1, dgVector& minBox, dgVector& maxBox) const + { + minBox = node0->m_minBox.GetMin(node1->m_minBox); + maxBox = node0->m_maxBox.GetMax(node1->m_maxBox); + dgVector side0(maxBox - minBox); + return side0.DotProduct(side0.ShiftTripleRight()).GetScalar(); + } + + dgWorld* GetWorld() const { return m_world;} + + virtual dgInt32 GetType() const = 0; + + virtual void Add(dgBody* const body) = 0; + virtual void Remove(dgBody* const body) = 0; + + virtual void ResetEntropy() = 0; + virtual void UpdateFitness() = 0; + virtual void InvalidateCache() = 0; + virtual dgBroadPhaseAggregate* CreateAggregate() = 0; + virtual void DestroyAggregate(dgBroadPhaseAggregate* const aggregate) = 0; + + virtual void CheckStaticDynamic(dgBody* const body, dgFloat32 mass) = 0; + virtual void ForEachBodyInAABB (const dgVector& minBox, const dgVector& maxBox, OnBodiesInAABB callback, void* const userData) const = 0; + virtual void RayCast (const dgVector& p0, const dgVector& p1, OnRayCastAction filter, OnRayPrecastAction prefilter, void* const userData) const = 0; + virtual dgInt32 Collide(dgCollisionInstance* const shape, const dgMatrix& matrix, OnRayPrecastAction prefilter, void* const userData, dgConvexCastReturnInfo* const info, dgInt32 maxContacts, dgInt32 threadIndex) const = 0; + virtual dgInt32 ConvexCast (dgCollisionInstance* const shape, const dgMatrix& matrix, const dgVector& target, dgFloat32* const param, OnRayPrecastAction prefilter, void* const userData, dgConvexCastReturnInfo* const info, dgInt32 maxContacts, dgInt32 threadIndex) const = 0; + virtual void FindCollidingPairs (dgBroadphaseSyncDescriptor* const descriptor, dgList::dgListNode* const node, dgInt32 threadID) = 0; + + void UpdateBody(dgBody* const body, dgInt32 threadIndex); + void AddInternallyGeneratedBody(dgBody* const body) + { + m_generatedBodies.Append(body); + } + + void UpdateContacts(dgFloat32 timestep); + void CollisionChange (dgBody* const body, dgCollisionInstance* const collisionSrc); + + void MoveNodes (dgBroadPhase* const dest); + + protected: + virtual void LinkAggregate (dgBroadPhaseAggregate* const aggregate) = 0; + virtual void UnlinkAggregate (dgBroadPhaseAggregate* const aggregate) = 0; + + bool DoNeedUpdate(dgBodyMasterList::dgListNode* const node) const; + dgFloat64 CalculateEntropy (dgFitnessList& fitness, dgBroadPhaseNode** const root); + dgBroadPhaseTreeNode* InsertNode (dgBroadPhaseNode* const root, dgBroadPhaseNode* const node); + + void RotateLeft(dgBroadPhaseTreeNode* const node, dgBroadPhaseNode** const root); + void RotateRight(dgBroadPhaseTreeNode* const node, dgBroadPhaseNode** const root); + void ImproveNodeFitness(dgBroadPhaseTreeNode* const node, dgBroadPhaseNode** const root); + void ImproveFitness(dgFitnessList& fitness, dgFloat64& oldEntropy, dgBroadPhaseNode** const root); + + void CalculatePairContacts (dgPair* const pair, dgInt32 threadID); + void AddPair (dgContact* const contact, dgFloat32 timestep, dgInt32 threadIndex); + void AddPair (dgBody* const body0, dgBody* const body1, dgFloat32 timestep, dgInt32 threadID); + + bool TestOverlaping(const dgBody* const body0, const dgBody* const body1, dgFloat32 timestep) const; + + void ForEachBodyInAABB (const dgBroadPhaseNode** stackPool, dgInt32 stack, const dgVector& minBox, const dgVector& maxBox, OnBodiesInAABB callback, void* const userData) const; + void RayCast (const dgBroadPhaseNode** stackPool, dgFloat32* const distance, dgInt32 stack, const dgVector& l0, const dgVector& l1, dgFastRayTest& ray, OnRayCastAction filter, OnRayPrecastAction prefilter, void* const userData) const; + + dgInt32 ConvexCast (const dgBroadPhaseNode** stackPool, dgFloat32* const distance, dgInt32 stack, const dgVector& velocA, const dgVector& velocB, dgFastRayTest& ray, + dgCollisionInstance* const shape, const dgMatrix& matrix, const dgVector& target, dgFloat32* const param, OnRayPrecastAction prefilter, void* const userData, dgConvexCastReturnInfo* const info, dgInt32 maxContacts, dgInt32 threadIndex) const; + + dgInt32 Collide(const dgBroadPhaseNode** stackPool, dgInt32* const overlap, dgInt32 stack, const dgVector& p0, const dgVector& p1, + dgCollisionInstance* const shape, const dgMatrix& matrix, OnRayPrecastAction prefilter, void* const userData, dgConvexCastReturnInfo* const info, dgInt32 maxContacts, dgInt32 threadIndex) const; + + void SleepingState (dgBroadphaseSyncDescriptor* const descriptor, dgBodyMasterList::dgListNode* node, dgInt32 threadID); + void ApplyForceAndtorque (dgBroadphaseSyncDescriptor* const descriptor, dgBodyMasterList::dgListNode* node, dgInt32 threadID); + + void UpdateAggregateEntropy (dgBroadphaseSyncDescriptor* const descriptor, dgList::dgListNode* node, dgInt32 threadID); + + dgBroadPhaseNode* BuildTopDown(dgBroadPhaseNode** const leafArray, dgInt32 firstBox, dgInt32 lastBox, dgFitnessList::dgListNode** const nextNode); + dgBroadPhaseNode* BuildTopDownBig(dgBroadPhaseNode** const leafArray, dgInt32 firstBox, dgInt32 lastBox, dgFitnessList::dgListNode** const nextNode); + + void KinematicBodyActivation (dgContact* const contatJoint) const; + + void FindGeneratedBodiesCollidingPairs (dgBroadphaseSyncDescriptor* const descriptor, dgInt32 threadID); + void UpdateSoftBodyContacts(dgBroadphaseSyncDescriptor* const descriptor, dgFloat32 timeStep, dgInt32 threadID); + void UpdateRigidBodyContacts (dgBroadphaseSyncDescriptor* const descriptor, dgFloat32 timeStep, dgInt32 threadID); + void SubmitPairs (dgBroadPhaseNode* const body, dgBroadPhaseNode* const node, dgFloat32 timestep, dgInt32 threaCount, dgInt32 threadID); + + bool SanityCheck() const; + void AttachNewContact(dgInt32 startCount); + void DeleteDeadContact(dgFloat32 timestep); + + DG_INLINE bool ValidateContactCache(dgContact* const contact, const dgVector& timestep) const; + + static void SleepingStateKernel(void* const descriptor, void* const worldContext, dgInt32 threadID); + static void ForceAndToqueKernel(void* const descriptor, void* const worldContext, dgInt32 threadID); + static void CollidingPairsKernel(void* const descriptor, void* const worldContext, dgInt32 threadID); + static void UpdateAggregateEntropyKernel(void* const descriptor, void* const worldContext, dgInt32 threadID); + static void AddGeneratedBodiesContactsKernel(void* const descriptor, void* const worldContext, dgInt32 threadID); + static void UpdateRigidBodyContactKernel(void* const descriptor, void* const worldContext, dgInt32 threadID); + static void UpdateSoftBodyContactKernel(void* const descriptor, void* const worldContext, dgInt32 threadID); + static dgInt32 CompareNodes(const dgBroadPhaseNode* const nodeA, const dgBroadPhaseNode* const nodeB, void* const notUsed); + + class dgPendingCollisionSoftBodies + { + public: + dgBody* m_body0; + dgBody* m_body1; + }; + + dgWorld* m_world; + dgBroadPhaseNode* m_rootNode; + dgList m_generatedBodies; + dgList m_updateList; + dgList m_aggregateList; + dgUnsigned32 m_lru; + dgContactCache m_contactCache; + dgArray m_pendingSoftBodyCollisions; + dgInt32 m_pendingSoftBodyPairsCount; + dgInt32 m_criticalSectionLock; + + static dgVector m_velocTol; + static dgVector m_linearContactError2; + static dgVector m_angularContactError2; + + friend class dgBody; + friend class dgWorld; + friend class dgDeadBodies; + friend class dgWorldDynamicUpdate; + friend class dgBroadPhaseAggregate; + friend class dgCollisionCompoundFractured; +}; + + +#endif diff --git a/thirdparty/src/newton/dgPhysics/dgBroadPhaseAggregate.cpp b/thirdparty/src/newton/dgPhysics/dgBroadPhaseAggregate.cpp new file mode 100644 index 000000000..54fcc2d50 --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgBroadPhaseAggregate.cpp @@ -0,0 +1,374 @@ +/* Copyright (c) <2003-2019> +* +* 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 "dgPhysicsStdafx.h" +#include "dgBody.h" +#include "dgWorld.h" +#include "dgBroadPhaseAggregate.h" + + +dgBroadPhaseAggregate::dgBroadPhaseAggregate(dgBroadPhase* const broadPhase) + :dgBroadPhaseNode(NULL) + ,m_root(NULL) + ,m_broadPhase(broadPhase) + ,m_updateNode(NULL) + ,m_myAggregateNode(NULL) + ,m_fitnessList(broadPhase->m_world->GetAllocator()) + ,m_treeEntropy(dgFloat32(0.0f)) + ,m_isInEquilibrium(false) + ,m_isSelfCollidable(true) +{ + m_minBox = dgVector(dgFloat32(0.0f)); + m_maxBox = dgVector(dgFloat32(0.0f)); + m_surfaceArea = dgFloat32(0.0f); +} + +dgBroadPhaseAggregate::~dgBroadPhaseAggregate() +{ + if (m_root) { + dgBody* buffer[2040]; + + dgBroadPhaseNode* pool[DG_BROADPHASE_MAX_STACK_DEPTH]; + pool[0] = m_root; + dgInt32 stack = 1; + + dgInt32 count = 0; + while (stack) { + stack--; + dgBroadPhaseNode* const rootNode = pool[stack]; + if (rootNode->IsLeafNode()) { + buffer[count] = rootNode->GetBody();; + count ++; + } else { + dgBroadPhaseTreeNode* const tmpNode = (dgBroadPhaseTreeNode*)rootNode; + dgAssert(tmpNode->m_left); + dgAssert(tmpNode->m_right); + + pool[stack] = tmpNode->m_left; + stack++; + dgAssert(stack < dgInt32(sizeof (pool) / sizeof (pool[0]))); + pool[stack] = tmpNode->m_right; + stack++; + dgAssert(stack < dgInt32(sizeof (pool) / sizeof (pool[0]))); + } + } + + for (dgInt32 i = 0; i < count; i ++) { + RemoveBody(buffer[i]); + } + dgAssert (!m_root); + } +} + +void dgBroadPhaseAggregate::AddBody(dgBody* const body) +{ + dgAssert(body->GetBroadPhase()); + m_broadPhase->Remove(body); + + dgBroadPhaseBodyNode* const newNode = new (m_broadPhase->GetWorld()->GetAllocator()) dgBroadPhaseBodyNode(body); + if (!m_root) { + m_root = newNode; + newNode->m_parent = this; + } else { + dgBroadPhaseTreeNode* const tmp = m_broadPhase->InsertNode(m_root, newNode); + dgList::dgListNode* const link = m_fitnessList.Append(tmp); + tmp->m_fitnessNode = link; + } + body->m_broadPhaseaggregateNode = this; + SetAABB (m_root->m_minBox, m_root->m_maxBox); + for (dgBroadPhaseNode* ptr = this; ptr->m_parent; ptr = ptr->m_parent) { + if (dgBoxInclusionTest(ptr->m_minBox, ptr->m_maxBox, ptr->m_parent->m_minBox, ptr->m_parent->m_maxBox)) { + break; + } + dgVector minBox; + dgVector maxBox; + dgFloat32 area; + area = m_broadPhase->CalculateSurfaceArea(ptr->m_parent, ptr, minBox, maxBox); + ptr->m_parent->m_minBox = minBox; + ptr->m_parent->m_maxBox = maxBox; + ptr->m_parent->m_surfaceArea = area; + } +} + +void dgBroadPhaseAggregate::RemoveBody(dgBody* const body) +{ + dgAssert(body->GetBroadPhase()); + m_broadPhase->Remove(body); + m_broadPhase->Add(body); +} + + +void dgBroadPhaseAggregate::ImproveEntropy() +{ + if (m_root) { + if (m_root->IsLeafNode()) { + dgAssert (m_root->GetBody()); + m_isInEquilibrium = m_root->GetBody()->m_equilibrium; + } else if (!m_isInEquilibrium) { + + bool equlibrium = true; + dgFloat64 entropy = dgFloat32(0.0f); + for (dgList::dgListNode* ptr = m_fitnessList.GetFirst(); ptr; ptr = ptr->GetNext()) { + dgBroadPhaseTreeNode* const tmpNode = ptr->GetInfo(); + entropy += tmpNode->m_surfaceArea; + const dgBody* const leftBody = tmpNode->m_left->GetBody(); + const dgBody* const rightBody = tmpNode->m_right->GetBody(); + equlibrium &= (!leftBody || leftBody->m_equilibrium) ? true : false; + equlibrium &= (!rightBody ||rightBody->m_equilibrium) ? true : false; + } + + m_isInEquilibrium = equlibrium; + if (!m_isInEquilibrium && ((entropy > m_treeEntropy * dgFloat32(2.0f)) || (entropy < m_treeEntropy * dgFloat32(0.5f)))) { + m_root->m_parent = NULL; + dgFloat64 cost0 = entropy; + dgFloat64 cost1 = cost0; + do { + cost0 = cost1; + for (dgList::dgListNode* ptr = m_fitnessList.GetFirst(); ptr; ptr = ptr->GetNext()) { + dgBroadPhaseTreeNode* const tmpNode = ptr->GetInfo(); + m_broadPhase->ImproveNodeFitness(tmpNode, &m_root); + } + cost1 = dgFloat32(0.0f); + for (dgList::dgListNode* ptr = m_fitnessList.GetFirst(); ptr; ptr = ptr->GetNext()) { + dgBroadPhaseTreeNode* const tmpNode = ptr->GetInfo(); + cost1 += tmpNode->m_surfaceArea; + } + } while (cost1 < (dgFloat32(0.99f)) * cost0); + + m_treeEntropy = cost1; + m_root->m_parent = this; + m_minBox = m_root->m_minBox; + m_maxBox = m_root->m_maxBox; + m_surfaceArea = m_root->m_surfaceArea; + } + } + } +} + +void dgBroadPhaseAggregate::SummitPairs(dgBroadPhaseAggregate* const aggregate, dgFloat32 timestep, dgInt32 threadID) const +{ + if (m_root && aggregate->m_root && !(m_isInEquilibrium & aggregate->m_isInEquilibrium)) { + SubmitSelfPairs(m_root, aggregate->m_root, timestep, threadID); + } +} + +void dgBroadPhaseAggregate::SubmitSelfPairs(dgFloat32 timestep, dgInt32 threadID) const +{ + if (m_root && !m_root->IsLeafNode()) { + if (!m_isInEquilibrium & m_isSelfCollidable) { + SubmitSelfPairs(m_root->GetLeft(), m_root->GetRight(), timestep, threadID); + } + } +} + +void dgBroadPhaseAggregate::SummitPairs(dgBody* const body, dgFloat32 timestep, dgInt32 threadID) const +{ + if (m_root) { + if (m_root->IsLeafNode()) { + dgAssert (m_root->GetBody()); + m_broadPhase->AddPair(body, m_root->GetBody(), timestep, threadID); + } else if (!(m_isInEquilibrium & body->m_equilibrium)) { + dgBroadPhaseNode* pool[DG_BROADPHASE_MAX_STACK_DEPTH/2]; + pool[0] = m_root; + dgInt32 stack = 1; + + const dgVector& boxP0 = body->m_minAABB; + const dgVector& boxP1 = body->m_maxAABB; + + while (stack) { + stack--; + dgBroadPhaseNode* const rootNode = pool[stack]; + if (dgOverlapTest(rootNode->m_minBox, rootNode->m_maxBox, boxP0, boxP1)) { + if (rootNode->IsLeafNode()) { + dgBody* const body1 = rootNode->GetBody(); + dgAssert (body1); + m_broadPhase->AddPair(body, body1, timestep, threadID); + } else { + dgBroadPhaseTreeNode* const tmpNode = (dgBroadPhaseTreeNode*)rootNode; + dgAssert(tmpNode->m_left); + dgAssert(tmpNode->m_right); + + pool[stack] = tmpNode->m_left; + stack++; + dgAssert(stack < dgInt32(sizeof (pool) / sizeof (pool[0]))); + + pool[stack] = tmpNode->m_right; + stack++; + dgAssert(stack < dgInt32(sizeof (pool) / sizeof (pool[0]))); + } + } + } + } + } +} + +void dgBroadPhaseAggregate::SubmitSelfPairs(dgBroadPhaseNode* const node0, dgBroadPhaseNode* const node1, dgFloat32 timestep, dgInt32 threadID) const +{ +/* + dgInt32 stack = 1; + dgBroadPhaseNode* pool[DG_BROADPHASE_MAX_STACK_DEPTH][2]; + + pool[0][0] = node0; + pool[0][1] = node1; + while (stack) { + stack--; + dgBroadPhaseNode* const root0 = pool[stack][0]; + dgBroadPhaseNode* const root1 = pool[stack][1]; + if (dgOverlapTest(root0->m_minBox, root0->m_maxBox, root1->m_minBox, root1->m_maxBox)) { + if (root0->IsLeafNode()) { + if (root1->IsLeafNode()) { + dgBody* const body0 = root0->GetBody(); + dgBody* const body1 = root1->GetBody(); + dgAssert(body0); + dgAssert(body1); + m_broadPhase->AddPair(body0, body1, timestep, threadID); + } else { + dgBroadPhaseTreeNode* const tmpNode1 = (dgBroadPhaseTreeNode*)root1; + dgAssert(tmpNode1->m_left); + dgAssert(tmpNode1->m_right); + + pool[stack][0] = root0; + pool[stack][1] = tmpNode1->m_left; + stack++; + dgAssert(stack < dgInt32(sizeof (pool) / sizeof (pool[0]))); + + pool[stack][0] = root0; + pool[stack][1] = tmpNode1->m_right; + stack++; + dgAssert(stack < dgInt32(sizeof (pool) / sizeof (pool[0]))); + } + } else if (root1->IsLeafNode()) { + dgBroadPhaseTreeNode* const tmpNode0 = (dgBroadPhaseTreeNode*)root0; + dgAssert(tmpNode0->m_left); + dgAssert(tmpNode0->m_right); + + pool[stack][0] = root1; + pool[stack][1] = tmpNode0->m_left; + stack++; + dgAssert(stack < dgInt32(sizeof (pool) / sizeof (pool[0]))); + + pool[stack][0] = root1; + pool[stack][1] = tmpNode0->m_right; + stack++; + dgAssert(stack < dgInt32(sizeof (pool) / sizeof (pool[0]))); + } else { + + dgBroadPhaseTreeNode* const tmpNode0 = (dgBroadPhaseTreeNode*)root0; + dgBroadPhaseTreeNode* const tmpNode1 = (dgBroadPhaseTreeNode*)root1; + dgAssert(tmpNode0->m_left); + dgAssert(tmpNode0->m_right); + dgAssert(tmpNode1->m_left); + dgAssert(tmpNode1->m_right); + + pool[stack][0] = tmpNode0->m_left; + pool[stack][1] = tmpNode1->m_left; + stack++; + dgAssert(stack < dgInt32(sizeof (pool) / sizeof (pool[0]))); + + pool[stack][0] = tmpNode0->m_left; + pool[stack][1] = tmpNode1->m_right; + stack++; + dgAssert(stack < dgInt32(sizeof (pool) / sizeof (pool[0]))); + + pool[stack][0] = tmpNode0->m_right; + pool[stack][1] = tmpNode1->m_left; + stack++; + dgAssert(stack < dgInt32(sizeof (pool) / sizeof (pool[0]))); + + pool[stack][0] = tmpNode0->m_right; + pool[stack][1] = tmpNode1->m_right; + stack++; + dgAssert(stack < dgInt32(sizeof (pool) / sizeof (pool[0]))); + } + } + } +*/ + + const dgBroadPhaseNode* pool[DG_BROADPHASE_MAX_STACK_DEPTH][2]; + pool[0][0] = node0; + pool[0][1] = node1; + dgInt32 stack = 1; + + while (stack) { + stack--; + + const dgBroadPhaseNode* const left = pool[stack][0]; + const dgBroadPhaseNode* const right = pool[stack][1]; + + if (left->IsLeafNode() && right->IsLeafNode()) { + dgBody* const body0 = left->GetBody(); + dgBody* const body1 = right->GetBody(); + if (dgOverlapTest(body0->m_minAABB, body0->m_maxAABB, body1->m_minAABB, body1->m_maxAABB)) { + m_broadPhase->AddPair(body0, body1, timestep, threadID); + } + } else { + if (left->m_parent == right->m_parent) { + if (!left->IsLeafNode()) { + pool[stack][0] = left->GetLeft(); + pool[stack][1] = left->GetRight(); + stack++; + dgAssert(stack < sizeof(pool) / sizeof(pool[0])); + } + if (!right->IsLeafNode()) { + pool[stack][0] = right->GetLeft(); + pool[stack][1] = right->GetRight(); + stack++; + dgAssert(stack < sizeof(pool) / sizeof(pool[0])); + } + } + + const dgBroadPhaseNode* leftPool[2]; + const dgBroadPhaseNode* rightPool[2]; + dgInt32 leftCount = 2; + dgInt32 rightCount = 2; + if (left->IsLeafNode()) { + leftCount = 1; + dgAssert(!right->IsLeafNode()); + leftPool[0] = left; + rightPool[0] = right->GetLeft(); + rightPool[1] = right->GetRight(); + } else if (right->IsLeafNode()) { + rightCount = 1; + dgAssert(!left->IsLeafNode()); + leftPool[0] = left->GetLeft(); + leftPool[1] = left->GetRight(); + rightPool[0] = right; + } + else { + leftPool[0] = left->GetLeft(); + leftPool[1] = left->GetRight(); + rightPool[0] = right->GetLeft(); + rightPool[1] = right->GetRight(); + } + + for (dgInt32 i = 0; i < leftCount; i++) { + for (dgInt32 j = 0; j < rightCount; j++) { + if (dgOverlapTest(leftPool[i]->m_minBox, leftPool[i]->m_maxBox, rightPool[j]->m_minBox, rightPool[j]->m_maxBox)) { + pool[stack][0] = leftPool[i]; + pool[stack][1] = rightPool[j]; + stack++; + dgAssert(stack < sizeof (pool) / sizeof (pool[0])); + } + } + } + } + } +} diff --git a/thirdparty/src/newton/dgPhysics/dgBroadPhaseAggregate.h b/thirdparty/src/newton/dgPhysics/dgBroadPhaseAggregate.h new file mode 100644 index 000000000..0adfb87c7 --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgBroadPhaseAggregate.h @@ -0,0 +1,75 @@ +/* Copyright (c) <2003-2019> +* +* 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_BROADPHASE_AGGREGATE_H__ +#define __DG_BROADPHASE_AGGREGATE_H__ + +#include "dgPhysicsStdafx.h" +#include "dgBroadPhase.h" + +class dgBroadPhaseAggregate: public dgBroadPhaseNode +{ + public: + dgBroadPhaseAggregate (dgBroadPhase* const broadPhase); + virtual ~dgBroadPhaseAggregate();; + + virtual bool IsLeafNode() const + { + return true; + } + + virtual bool IsAggregate() const + { + return true; + } + + bool GetSelfCollision() const + { + return m_isSelfCollidable ? true : false; + } + + void SetSelfCollision(bool state) + { + m_isSelfCollidable = state; + } + + void AddBody (dgBody* const body); + void RemoveBody (dgBody* const body); + + void ImproveEntropy (); + void SubmitSelfPairs(dgFloat32 timestep, dgInt32 threadID) const; + void SummitPairs(dgBody* const body, dgFloat32 timestep, dgInt32 threadID) const; + void SummitPairs(dgBroadPhaseAggregate* const aggregate, dgFloat32 timestep, dgInt32 threadID) const; + private: + void SubmitSelfPairs(dgBroadPhaseNode* const node0, dgBroadPhaseNode* const node1, dgFloat32 timestep, dgInt32 threadID) const; + + public: + dgBroadPhaseNode* m_root; + dgBroadPhase* m_broadPhase; + dgList::dgListNode* m_updateNode; + dgList::dgListNode* m_myAggregateNode; + dgList m_fitnessList; + dgFloat64 m_treeEntropy; + bool m_isInEquilibrium; + bool m_isSelfCollidable; +}; + +#endif diff --git a/thirdparty/src/newton/dgPhysics/dgBroadPhaseMixed.cpp b/thirdparty/src/newton/dgPhysics/dgBroadPhaseMixed.cpp new file mode 100644 index 000000000..93dcd8c79 --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgBroadPhaseMixed.cpp @@ -0,0 +1,402 @@ +/* Copyright (c) <2003-2019> +* +* 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 "dgPhysicsStdafx.h" +#include "dgBody.h" +#include "dgWorld.h" +#include "dgCollisionInstance.h" +#include "dgBroadPhaseMixed.h" +#include "dgBroadPhaseAggregate.h" + + +dgBroadPhaseMixed::dgBroadPhaseMixed(dgWorld* const world) + :dgBroadPhase(world) + ,m_treeEntropy(dgFloat32(0.0f)) + ,m_fitness(world->GetAllocator()) +{ +} + +dgBroadPhaseMixed::~dgBroadPhaseMixed() +{ + if (m_rootNode) { + delete m_rootNode; + } + m_rootNode = NULL; +} + +dgInt32 dgBroadPhaseMixed::GetType() const +{ + return dgWorld::m_broadphaseMixed; +} + +void dgBroadPhaseMixed::ResetEntropy() +{ + m_treeEntropy = dgFloat32(0.0f); +} + +void dgBroadPhaseMixed::UpdateFitness() +{ + ImproveFitness(m_fitness, m_treeEntropy, &m_rootNode); +} + +void dgBroadPhaseMixed::InvalidateCache() +{ + ResetEntropy(); + ImproveFitness(m_fitness, m_treeEntropy, &m_rootNode); + m_contactCache.Flush(); +} + +void dgBroadPhaseMixed::ForEachBodyInAABB(const dgVector& minBox, const dgVector& maxBox, OnBodiesInAABB callback, void* const userData) const +{ + if (m_rootNode) { + const dgBroadPhaseNode* stackPool[DG_BROADPHASE_MAX_STACK_DEPTH]; + stackPool[0] = m_rootNode; + dgBroadPhase::ForEachBodyInAABB(stackPool, 1, minBox, maxBox, callback, userData); + } +} + + +void dgBroadPhaseMixed::RayCast(const dgVector& l0, const dgVector& l1, OnRayCastAction filter, OnRayPrecastAction prefilter, void* const userData) const +{ + if (filter && m_rootNode) { + dgVector segment(l1 - l0); + dgAssert (segment.m_w == dgFloat32 (0.0f)); + dgFloat32 dist2 = segment.DotProduct(segment).GetScalar(); + if (dist2 > dgFloat32(1.0e-8f)) { + + dgFloat32 distance[DG_BROADPHASE_MAX_STACK_DEPTH]; + const dgBroadPhaseNode* stackPool[DG_BROADPHASE_MAX_STACK_DEPTH]; + + dgFastRayTest ray(l0, l1); + + stackPool[0] = m_rootNode; + distance[0] = ray.BoxIntersect(m_rootNode->m_minBox, m_rootNode->m_maxBox); + dgBroadPhase::RayCast(stackPool, distance, 1, l0, l1, ray, filter, prefilter, userData); + } + } +} + +dgInt32 dgBroadPhaseMixed::ConvexCast(dgCollisionInstance* const shape, const dgMatrix& matrix, const dgVector& target, dgFloat32* const param, OnRayPrecastAction prefilter, void* const userData, dgConvexCastReturnInfo* const info, dgInt32 maxContacts, dgInt32 threadIndex) const +{ + dgInt32 totalCount = 0; + if (m_rootNode) { + dgVector boxP0; + dgVector boxP1; + dgAssert(matrix.TestOrthogonal()); + shape->CalcAABB(matrix, boxP0, boxP1); + + dgFloat32 distance[DG_BROADPHASE_MAX_STACK_DEPTH]; + const dgBroadPhaseNode* stackPool[DG_BROADPHASE_MAX_STACK_DEPTH]; + + dgVector velocA((target - matrix.m_posit) & dgVector::m_triplexMask); + dgVector velocB(dgFloat32(0.0f)); + dgFastRayTest ray(dgVector(dgFloat32(0.0f)), velocA); + + dgVector minBox(m_rootNode->m_minBox - boxP1); + dgVector maxBox(m_rootNode->m_maxBox - boxP0); + stackPool[0] = m_rootNode; + distance[0] = ray.BoxIntersect(minBox, maxBox); + + *param = dgFloat32 (1.0f); + totalCount = dgBroadPhase::ConvexCast(stackPool, distance, 1, velocA, velocB, ray, shape, matrix, target, param, prefilter, userData, info, maxContacts, threadIndex); + } + + return totalCount; +} + +dgInt32 dgBroadPhaseMixed::Collide(dgCollisionInstance* const shape, const dgMatrix& matrix, OnRayPrecastAction prefilter, void* const userData, dgConvexCastReturnInfo* const info, dgInt32 maxContacts, dgInt32 threadIndex) const +{ + dgInt32 totalCount = 0; + if (m_rootNode) { + dgVector boxP0; + dgVector boxP1; + dgAssert(matrix.TestOrthogonal()); + shape->CalcAABB(shape->GetLocalMatrix() * matrix, boxP0, boxP1); + + dgInt32 overlaped[DG_BROADPHASE_MAX_STACK_DEPTH]; + const dgBroadPhaseNode* stackPool[DG_BROADPHASE_MAX_STACK_DEPTH]; + + stackPool[0] = m_rootNode; + overlaped[0] = dgOverlapTest(m_rootNode->m_minBox, m_rootNode->m_maxBox, boxP0, boxP1); + + totalCount = dgBroadPhase::Collide(stackPool, overlaped, 1, boxP0, boxP1, shape, matrix, prefilter, userData, info, maxContacts, threadIndex); + } + + return totalCount; +} + +void dgBroadPhaseMixed::AddNode(dgBroadPhaseNode* const newNode) +{ + if (!m_rootNode) { + m_rootNode = newNode; + } else { + dgBroadPhaseTreeNode* const node = InsertNode(m_rootNode, newNode); + node->m_fitnessNode = m_fitness.Append(node); + if (!node->m_parent) { + m_rootNode = node; + } + } +} + +void dgBroadPhaseMixed::Add(dgBody* const body) +{ + // create a new leaf node; + dgAssert (!body->GetCollision()->IsType (dgCollision::dgCollisionNull_RTTI)); + dgBroadPhaseBodyNode* const bodyNode = new (m_world->GetAllocator()) dgBroadPhaseBodyNode(body); + bodyNode->m_updateNode = m_updateList.Append(bodyNode); + AddNode(bodyNode); +} + +dgBroadPhaseAggregate* dgBroadPhaseMixed::CreateAggregate() +{ + dgBroadPhaseAggregate* const aggregate = new (m_world->GetAllocator()) dgBroadPhaseAggregate(m_world->GetBroadPhase()); + LinkAggregate (aggregate); + return aggregate; +} + +void dgBroadPhaseMixed::LinkAggregate(dgBroadPhaseAggregate* const aggregate) +{ + AddNode(aggregate); + aggregate->m_broadPhase = this; + aggregate->m_updateNode = m_updateList.Append(aggregate); + aggregate->m_myAggregateNode = m_aggregateList.Append(aggregate); +} + +void dgBroadPhaseMixed::RemoveNode(dgBroadPhaseNode* const node) +{ + if (node->m_parent) { + if (!node->m_parent->IsAggregate()) { + dgBroadPhaseTreeNode* const parent = (dgBroadPhaseTreeNode*)node->m_parent; + if (parent->m_parent) { + if (parent->m_parent->IsAggregate()) { + dgBroadPhaseAggregate* const aggregate = (dgBroadPhaseAggregate*)parent->m_parent; + if (parent->m_left == node) { + dgAssert(parent->m_right); + aggregate->m_root = parent->m_right; + parent->m_right->m_parent = aggregate; + parent->m_right = NULL; + } else { + dgAssert(parent->m_right == node); + aggregate->m_root = parent->m_left; + parent->m_left->m_parent = aggregate; + parent->m_left = NULL; + } + parent->m_parent = NULL; + } else { + dgBroadPhaseTreeNode* const grandParent = (dgBroadPhaseTreeNode*)parent->m_parent; + if (grandParent->m_left == parent) { + if (parent->m_right == node) { + grandParent->m_left = parent->m_left; + parent->m_left->m_parent = grandParent; + parent->m_left = NULL; + parent->m_parent = NULL; + } else { + grandParent->m_left = parent->m_right; + parent->m_right->m_parent = grandParent; + parent->m_right = NULL; + parent->m_parent = NULL; + } + } else { + if (parent->m_right == node) { + grandParent->m_right = parent->m_left; + parent->m_left->m_parent = grandParent; + parent->m_left = NULL; + parent->m_parent = NULL; + } else { + grandParent->m_right = parent->m_right; + parent->m_right->m_parent = grandParent; + parent->m_right = NULL; + parent->m_parent = NULL; + } + } + } + } else { + dgAssert(!node->m_parent->IsLeafNode()); + dgBroadPhaseTreeNode* const parent1 = (dgBroadPhaseTreeNode*)node->m_parent; + if (parent1->m_right == node) { + m_rootNode = parent1->m_left; + m_rootNode->m_parent = NULL; + parent1->m_left = NULL; + } else { + m_rootNode = parent1->m_right; + m_rootNode->m_parent = NULL; + parent1->m_right = NULL; + } + } + + if (parent->m_fitnessNode) { + dgBody* const body = node->GetBody(); + if (body && body->GetBroadPhaseAggregate()) { + body->GetBroadPhaseAggregate()->m_fitnessList.Remove(parent->m_fitnessNode); + body->SetBroadPhaseAggregate(NULL); + } else { + m_fitness.Remove(parent->m_fitnessNode); + } + } + delete parent; + } else { + dgBroadPhaseAggregate* const aggregate = (dgBroadPhaseAggregate*)node->m_parent; + dgBody* const body = node->GetBody(); + dgAssert (body); + dgAssert(body->GetBroadPhaseAggregate() == aggregate); + body->SetBroadPhaseAggregate(NULL); + aggregate->m_root = NULL; + node->m_parent = NULL; + delete node; + } + } else { + delete node; + m_rootNode = NULL; + } +} + +void dgBroadPhaseMixed::UnlinkAggregate(dgBroadPhaseAggregate* const aggregate) +{ + dgAssert (m_rootNode); + if (m_rootNode == aggregate) { + m_rootNode = NULL; + } else if (aggregate->m_parent == m_rootNode) { + dgBroadPhaseTreeNode* const parent = (dgBroadPhaseTreeNode*)aggregate->m_parent; + if (parent->m_left == aggregate) { + m_rootNode = parent->m_right; + } else { + dgAssert(parent->m_right == aggregate); + m_rootNode = parent->m_left; + } + m_rootNode->m_parent = NULL; + + parent->m_left = NULL; + parent->m_right = NULL; + parent->m_parent = NULL; + delete parent; + } else { + dgBroadPhaseTreeNode* const parent = (dgBroadPhaseTreeNode*)aggregate->m_parent; + dgBroadPhaseTreeNode* const grandParent = (dgBroadPhaseTreeNode*)parent->m_parent; + if (grandParent->m_left == parent) { + if (parent->m_left == aggregate) { + grandParent->m_left = parent->m_right; + parent->m_right->m_parent = grandParent; + } else { + dgAssert (parent->m_right == aggregate); + grandParent->m_left = parent->m_left; + parent->m_left->m_parent = grandParent; + } + } else { + dgAssert (grandParent->m_right == parent); + if (parent->m_left == aggregate) { + grandParent->m_right = parent->m_right; + parent->m_right->m_parent = grandParent; + } else { + dgAssert(parent->m_right == aggregate); + grandParent->m_right = parent->m_left; + parent->m_left->m_parent = grandParent; + } + } + parent->m_left = NULL; + parent->m_right = NULL; + parent->m_parent = NULL; + delete parent; + } + aggregate->m_parent = NULL; +} + + +void dgBroadPhaseMixed::Remove(dgBody* const body) +{ + if (body->GetBroadPhase()) { + dgBroadPhaseBodyNode* const node = (dgBroadPhaseBodyNode*)body->GetBroadPhase(); + if (node->m_updateNode) { + m_updateList.Remove(node->m_updateNode); + } + RemoveNode(node); + } +} + + +void dgBroadPhaseMixed::DestroyAggregate(dgBroadPhaseAggregate* const aggregate) +{ + m_updateList.Remove(aggregate->m_updateNode); + m_aggregateList.Remove(aggregate->m_myAggregateNode); + RemoveNode(aggregate); +} + +void dgBroadPhaseMixed::FindCollidingPairs(dgBroadphaseSyncDescriptor* const descriptor, dgList::dgListNode* const nodePtr, dgInt32 threadID) +{ + DG_TRACKTIME(); + const dgFloat32 timestep = descriptor->m_timestep; + + dgList::dgListNode* node = nodePtr; + const dgInt32 threadCount = descriptor->m_world->GetThreadCount(); + + if (descriptor->m_fullScan) { + while (node) { + dgBroadPhaseNode* const broadPhaseNode = node->GetInfo(); + dgAssert(broadPhaseNode->IsLeafNode()); + + dgBody* const body = broadPhaseNode->GetBody(); + dgAssert(!body || (body->GetBroadPhase() == broadPhaseNode)); + + if (!(body && body->m_isdead)) { + if (broadPhaseNode->IsAggregate()) { + ((dgBroadPhaseAggregate*)broadPhaseNode)->SubmitSelfPairs(timestep, threadID); + } + + for (dgBroadPhaseNode* ptr = broadPhaseNode; ptr->m_parent; ptr = ptr->m_parent) { + dgBroadPhaseTreeNode* const parent = (dgBroadPhaseTreeNode*)ptr->m_parent; + dgAssert(!parent->IsLeafNode()); + dgBroadPhaseNode* const sibling = parent->m_right; + if (sibling != ptr) { + SubmitPairs(broadPhaseNode, sibling, timestep, 0, threadID); + } + } + } + + for (dgInt32 i = 0; i < threadCount; i++) { + node = node ? node->GetNext() : NULL; + } + } + + } else { + const dgBodyInfo* const bodyArray = &m_world->m_bodiesMemory[0]; + const dgInt32 bodyCount = descriptor->m_atomicPendingBodiesCount; + dgInt32* const atomicIndex = &descriptor->m_atomicIndex; + + for (dgInt32 i = dgAtomicExchangeAndAdd(atomicIndex, 1); i < bodyCount; i = dgAtomicExchangeAndAdd(atomicIndex, 1)) { + dgBroadPhaseNode* const broadPhaseNode = bodyArray[i].m_body->GetBroadPhase(); + dgAssert(broadPhaseNode->IsLeafNode()); + dgAssert(!broadPhaseNode->GetBody() || (broadPhaseNode->GetBody()->GetBroadPhase() == broadPhaseNode)); + + for (dgBroadPhaseNode* ptr = broadPhaseNode; ptr->m_parent; ptr = ptr->m_parent) { + dgBroadPhaseTreeNode* const parent = (dgBroadPhaseTreeNode*)ptr->m_parent; + if (!parent->IsAggregate()) { + dgAssert(!parent->IsLeafNode()); + dgBroadPhaseNode* const rightSibling = parent->m_right; + if (rightSibling != ptr) { + SubmitPairs(broadPhaseNode, rightSibling, timestep, threadCount, threadID); + } else { + SubmitPairs(broadPhaseNode, parent->m_left, timestep, threadCount, threadID); + } + } + } + } + } +} + diff --git a/thirdparty/src/newton/dgPhysics/dgBroadPhaseMixed.h b/thirdparty/src/newton/dgPhysics/dgBroadPhaseMixed.h new file mode 100644 index 000000000..4bdfbb83b --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgBroadPhaseMixed.h @@ -0,0 +1,68 @@ +/* Copyright (c) <2003-2019> +* +* 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 __AFX_BROADPHASE_DEFAULT_H_ +#define __AFX_BROADPHASE_DEFAULT_H_ + +#include "dgPhysicsStdafx.h" +#include "dgBroadPhase.h" + + +class dgBroadPhaseMixed: public dgBroadPhase +{ + public: + DG_CLASS_ALLOCATOR(allocator); + + dgBroadPhaseMixed(dgWorld* const world); + virtual ~dgBroadPhaseMixed(); + + protected: + virtual dgInt32 GetType() const; + virtual void Add(dgBody* const body); + virtual void Remove(dgBody* const body); + virtual void UpdateFitness(); + virtual void InvalidateCache(); + virtual dgBroadPhaseAggregate* CreateAggregate(); + virtual void DestroyAggregate(dgBroadPhaseAggregate* const aggregate); + + virtual void LinkAggregate (dgBroadPhaseAggregate* const aggregate); + virtual void UnlinkAggregate (dgBroadPhaseAggregate* const aggregate); + virtual void CheckStaticDynamic(dgBody* const body, dgFloat32 mass) {} + virtual void FindCollidingPairs (dgBroadphaseSyncDescriptor* const descriptor, dgList::dgListNode* const node, dgInt32 threadID); + + void RayCast (const dgVector& p0, const dgVector& p1, OnRayCastAction filter, OnRayPrecastAction prefilter, void* const userData) const; + dgInt32 Collide(dgCollisionInstance* const shape, const dgMatrix& matrix, OnRayPrecastAction prefilter, void* const userData, dgConvexCastReturnInfo* const info, dgInt32 maxContacts, dgInt32 threadIndex) const; + dgInt32 ConvexCast (dgCollisionInstance* const shape, const dgMatrix& p0, const dgVector& p1, dgFloat32* const param, OnRayPrecastAction prefilter, void* const userData, dgConvexCastReturnInfo* const info, dgInt32 maxContacts, dgInt32 threadIndex) const; + void ForEachBodyInAABB (const dgVector& q0, const dgVector& q1, OnBodiesInAABB callback, void* const userData) const; + + void ResetEntropy (); + void AddNode(dgBroadPhaseNode* const node); + void RemoveNode(dgBroadPhaseNode* const node); + + void ApplyDeformableForceAndtorque (dgBroadphaseSyncDescriptor* const descriptor, dgInt32 threadID); + + dgFloat64 m_treeEntropy; + dgFitnessList m_fitness; + +}; + + +#endif diff --git a/thirdparty/src/newton/dgPhysics/dgBroadPhaseSegregated.cpp b/thirdparty/src/newton/dgPhysics/dgBroadPhaseSegregated.cpp new file mode 100644 index 000000000..b155f013f --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgBroadPhaseSegregated.cpp @@ -0,0 +1,589 @@ +/* Copyright (c) <2003-2019> +* +* 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 "dgPhysicsStdafx.h" +#include "dgBody.h" +#include "dgWorld.h" +#include "dgCollisionInstance.h" +#include "dgBroadPhaseAggregate.h" +#include "dgBroadPhaseSegregated.h" + + +class dgBroadPhaseSegregatedRootNode: public dgBroadPhaseTreeNode +{ + public: + dgBroadPhaseSegregatedRootNode() + :dgBroadPhaseTreeNode() + { + } + + virtual bool IsSegregatedRoot() const + { + return true; + } + + void SetBox () + { + if (m_right && m_left) { + dgVector minBox (m_right->m_minBox.GetMin(m_left->m_minBox)); + dgVector maxBox (m_right->m_maxBox.GetMin(m_left->m_maxBox)); + SetAABB(minBox, maxBox); + } else if (m_right) { + SetAABB(m_right->m_minBox, m_right->m_maxBox); + } else if (m_left) { + SetAABB(m_left->m_minBox, m_left->m_maxBox); + } + } +}; + +dgBroadPhaseSegregated::dgBroadPhaseSegregated (dgWorld* const world) + :dgBroadPhase(world) + ,m_staticEntropy(dgFloat32 (0.0f)) + ,m_dynamicsEntropy(dgFloat32 (0.0f)) + ,m_staticFitness(world->GetAllocator()) + ,m_dynamicsFitness(world->GetAllocator()) + ,m_staticNeedsUpdate(true) +{ + m_rootNode = new (world->GetAllocator()) dgBroadPhaseSegregatedRootNode(); +} + +dgBroadPhaseSegregated::~dgBroadPhaseSegregated () +{ + delete m_rootNode; +} + +dgInt32 dgBroadPhaseSegregated::GetType() const +{ + return dgWorld::m_broadphaseSegregated; +} + +void dgBroadPhaseSegregated::CheckStaticDynamic(dgBody* const body, dgFloat32 mass1) +{ + dgBroadPhaseNode* const node = body->GetBroadPhase(); + if (node) { + const dgFloat32 mass0 = body->GetMass().m_w; + const bool m0 = (mass0 >= DG_INFINITE_MASS); + const bool m1 = (mass1 >= DG_INFINITE_MASS); + if (m0 ^ m1) { + Remove(body); + if (m1) { + AddStaticBody (body); + } else { + AddDynamicBody (body); + } + } + } +} + +void dgBroadPhaseSegregated::AddStaticBody(dgBody* const body) +{ + dgBroadPhaseSegregatedRootNode* const root = (dgBroadPhaseSegregatedRootNode*)m_rootNode; + dgAssert(m_rootNode->IsSegregatedRoot()); + + m_staticNeedsUpdate = true; + dgBroadPhaseBodyNode* const bodyNode = new (m_world->GetAllocator()) dgBroadPhaseBodyNode(body); + if (root->m_right) { + dgBroadPhaseTreeNode* const node = InsertNode(root->m_right, bodyNode); + node->m_fitnessNode = m_staticFitness.Append(node); + } else { + root->m_right = bodyNode; + root->m_right->m_parent = root; + } +} + +void dgBroadPhaseSegregated::AddDynamicBody(dgBody* const body) +{ + dgBroadPhaseSegregatedRootNode* const root = (dgBroadPhaseSegregatedRootNode*)m_rootNode; + dgAssert(m_rootNode->IsSegregatedRoot()); + + dgBroadPhaseBodyNode* const newNode = new (m_world->GetAllocator()) dgBroadPhaseBodyNode(body); + if (root->m_left) { + dgBroadPhaseTreeNode* const node = InsertNode(root->m_left, newNode); + node->m_fitnessNode = m_dynamicsFitness.Append(node); + } else { + root->m_left = newNode; + root->m_left->m_parent = root; + } + newNode->m_updateNode = m_updateList.Append(newNode); +} + + +void dgBroadPhaseSegregated::Add(dgBody* const body) +{ + dgAssert (!body->GetCollision()->IsType (dgCollision::dgCollisionNull_RTTI)); + dgAssert (((dgBroadPhaseSegregatedRootNode*)m_rootNode)->IsSegregatedRoot()); + + if (body->GetCollision()->IsType(dgCollision::dgCollisionMesh_RTTI) || (body->GetInvMass().m_w == dgFloat32(0.0f))) { + AddStaticBody(body); + } else { + AddDynamicBody(body); + } +} + +dgBroadPhaseAggregate* dgBroadPhaseSegregated::CreateAggregate() +{ + dgBroadPhaseAggregate* const aggregate = new (m_world->GetAllocator()) dgBroadPhaseAggregate(m_world->GetBroadPhase()); + LinkAggregate(aggregate); + return aggregate; +} + +void dgBroadPhaseSegregated::LinkAggregate(dgBroadPhaseAggregate* const aggregate) +{ + dgAssert(m_rootNode->IsSegregatedRoot()); + dgBroadPhaseSegregatedRootNode* const root = (dgBroadPhaseSegregatedRootNode*)m_rootNode; + + aggregate->m_broadPhase = this; + if (root->m_left) { + dgBroadPhaseTreeNode* const node = InsertNode(root->m_left, aggregate); + node->m_fitnessNode = m_dynamicsFitness.Append(node); + } else { + root->m_left = aggregate; + root->m_left->m_parent = m_rootNode; + } + aggregate->m_updateNode = m_updateList.Append(aggregate); + aggregate->m_myAggregateNode = m_aggregateList.Append(aggregate); +} + +void dgBroadPhaseSegregated::DestroyAggregate(dgBroadPhaseAggregate* const aggregate) +{ + m_updateList.Remove(aggregate->m_updateNode); + m_aggregateList.Remove(aggregate->m_myAggregateNode); + RemoveNode(aggregate); +} + +void dgBroadPhaseSegregated::RemoveNode(dgBroadPhaseNode* const node) +{ + dgAssert (node->m_parent); + + if (node->m_parent->IsSegregatedRoot()) { + dgBroadPhaseSegregatedRootNode* const parent = (dgBroadPhaseSegregatedRootNode*)m_rootNode; + dgAssert(parent == node->m_parent); + + if (parent->m_right == node) { + m_staticNeedsUpdate = true; + parent->m_right = NULL; + } else { + dgAssert(parent->m_left == node); + parent->m_left = NULL; + } + node->m_parent = NULL; + delete node; + } else if (node->m_parent->IsAggregate()) { + dgBroadPhaseAggregate* const aggregate = (dgBroadPhaseAggregate*)node->m_parent; + dgBody* const body = node->GetBody(); + dgAssert(body); + dgAssert(body->GetBroadPhaseAggregate() == aggregate); + body->SetBroadPhaseAggregate(NULL); + aggregate->m_root = NULL; + node->m_parent = NULL; + delete node; + } else { + dgBroadPhaseTreeNode* const parent = (dgBroadPhaseTreeNode*)node->m_parent; + if (parent->m_parent->IsAggregate()) { + dgBroadPhaseAggregate* const aggregate = (dgBroadPhaseAggregate*)parent->m_parent; + if (parent->m_left == node) { + dgAssert(parent->m_right); + aggregate->m_root = parent->m_right; + parent->m_right->m_parent = aggregate; + parent->m_right = NULL; + } else { + dgAssert(parent->m_right == node); + aggregate->m_root = parent->m_left; + parent->m_left->m_parent = aggregate; + parent->m_left = NULL; + } + parent->m_parent = NULL; + + if (parent->m_fitnessNode) { + dgBody* const body = node->GetBody(); + if (body && body->GetBroadPhaseAggregate()) { + body->GetBroadPhaseAggregate()->m_fitnessList.Remove(parent->m_fitnessNode); + body->SetBroadPhaseAggregate(NULL); + } else { + m_dynamicsFitness.Remove(parent->m_fitnessNode); + } + } + + delete parent; + + } else if (parent->m_parent->IsSegregatedRoot()) { + dgBroadPhaseSegregatedRootNode* const grandParent = (dgBroadPhaseSegregatedRootNode*) parent->m_parent; + if (grandParent->m_right == parent) { + m_staticNeedsUpdate = true; + if (parent->m_right == node) { + grandParent->m_right = parent->m_left; + parent->m_left->m_parent = grandParent; + parent->m_left = NULL; + parent->m_parent = NULL; + } else { + grandParent->m_right = parent->m_right; + parent->m_right->m_parent = grandParent; + parent->m_right = NULL; + parent->m_parent = NULL; + } + m_staticFitness.Remove(parent->m_fitnessNode); + delete parent; + } else { + dgAssert (grandParent->m_left == parent); + if (parent->m_right == node) { + grandParent->m_left = parent->m_left; + parent->m_left->m_parent = grandParent; + parent->m_left = NULL; + parent->m_parent = NULL; + } else { + grandParent->m_left = parent->m_right; + parent->m_right->m_parent = grandParent; + parent->m_right = NULL; + parent->m_parent = NULL; + } + m_dynamicsFitness.Remove(parent->m_fitnessNode); + delete parent; + } + } else { + dgBroadPhaseTreeNode* const grandParent = (dgBroadPhaseTreeNode*)parent->m_parent; + dgAssert (grandParent->GetLeft()); + dgAssert (grandParent->GetRight()); + if (grandParent->m_left == parent) { + if (parent->m_right == node) { + grandParent->m_left = parent->m_left; + parent->m_left->m_parent = grandParent; + parent->m_left = NULL; + parent->m_parent = NULL; + } else { + grandParent->m_left = parent->m_right; + parent->m_right->m_parent = grandParent; + parent->m_right = NULL; + parent->m_parent = NULL; + } + } else { + if (parent->m_right == node) { + grandParent->m_right = parent->m_left; + parent->m_left->m_parent = grandParent; + parent->m_left = NULL; + parent->m_parent = NULL; + } else { + grandParent->m_right = parent->m_right; + parent->m_right->m_parent = grandParent; + parent->m_right = NULL; + parent->m_parent = NULL; + } + } + + dgBody* const body = node->GetBody(); + if (body) { + if (body->GetInvMass().m_w == dgFloat32(0.0f)) { + m_staticNeedsUpdate = true; + m_staticFitness.Remove(parent->m_fitnessNode); + } else if (body->GetBroadPhaseAggregate()) { + body->GetBroadPhaseAggregate()->m_fitnessList.Remove(parent->m_fitnessNode); + body->SetBroadPhaseAggregate(NULL); + } else { + m_dynamicsFitness.Remove(parent->m_fitnessNode); + } + } else { + dgAssert (node->IsAggregate()); + m_dynamicsFitness.Remove(parent->m_fitnessNode); + } + + delete parent; + } + } +} + +void dgBroadPhaseSegregated::UnlinkAggregate (dgBroadPhaseAggregate* const aggregate) +{ + dgBroadPhaseSegregatedRootNode* const root = (dgBroadPhaseSegregatedRootNode*)m_rootNode; + dgAssert (root && root->m_left); + if (aggregate->m_parent == root) { + root->m_left = NULL; + } else { + dgBroadPhaseTreeNode* const parent = (dgBroadPhaseTreeNode*)aggregate->m_parent; + dgBroadPhaseTreeNode* const grandParent = (dgBroadPhaseTreeNode*)parent->m_parent; + if (grandParent->m_left == parent) { + if (parent->m_left == aggregate) { + grandParent->m_left = parent->m_right; + parent->m_right->m_parent = grandParent; + } else { + dgAssert(parent->m_right == aggregate); + grandParent->m_left = parent->m_left; + parent->m_left->m_parent = grandParent; + } + } else { + dgAssert(grandParent->m_right == parent); + if (parent->m_left == aggregate) { + grandParent->m_right = parent->m_right; + parent->m_right->m_parent = grandParent; + } else { + dgAssert(parent->m_right == aggregate); + grandParent->m_right = parent->m_left; + parent->m_left->m_parent = grandParent; + } + } + parent->m_left = NULL; + parent->m_right = NULL; + parent->m_parent = NULL; + delete parent; + } + aggregate->m_parent = NULL; +} + +void dgBroadPhaseSegregated::Remove(dgBody* const body) +{ + if (body->GetBroadPhase()) { + dgBroadPhaseBodyNode* const node = body->GetBroadPhase(); + if (node->m_updateNode) { + m_updateList.Remove(node->m_updateNode); + } + RemoveNode(node); + } +} + +void dgBroadPhaseSegregated::ResetEntropy() +{ + m_staticNeedsUpdate = true; + m_staticEntropy = dgFloat32(0.0f); + m_dynamicsEntropy = dgFloat32(0.0f); +} + + +void dgBroadPhaseSegregated::InvalidateCache() +{ + ResetEntropy(); + m_staticNeedsUpdate = false; + dgAssert (m_rootNode->IsSegregatedRoot()); + dgBroadPhaseSegregatedRootNode* const root = (dgBroadPhaseSegregatedRootNode*)m_rootNode; + ImproveFitness(m_staticFitness, m_staticEntropy, &root->m_right); + ImproveFitness(m_dynamicsFitness, m_dynamicsEntropy, &root->m_left); + root->SetBox (); + m_contactCache.Flush(); +} + +void dgBroadPhaseSegregated::UpdateFitness() +{ + dgBroadPhaseSegregatedRootNode* const root = (dgBroadPhaseSegregatedRootNode*)m_rootNode; + if (m_staticNeedsUpdate) { + m_staticNeedsUpdate = false; + ImproveFitness(m_staticFitness, m_staticEntropy, &root->m_right); + } + ImproveFitness(m_dynamicsFitness, m_dynamicsEntropy, &root->m_left); + root->SetBox (); +} + +void dgBroadPhaseSegregated::ForEachBodyInAABB(const dgVector& minBox, const dgVector& maxBox, OnBodiesInAABB callback, void* const userData) const +{ + dgBroadPhaseSegregatedRootNode* const root = (dgBroadPhaseSegregatedRootNode*)m_rootNode; + const dgBroadPhaseNode* stackPool[DG_BROADPHASE_MAX_STACK_DEPTH]; + + dgInt32 stack = 0; + if (root->m_left) { + stackPool[stack] = root->m_left; + stack++; + } + + if (root->m_right) { + stackPool[stack] = root->m_right; + stack++; + } + dgBroadPhase::ForEachBodyInAABB(stackPool, stack, minBox, maxBox, callback, userData); +} + +void dgBroadPhaseSegregated::RayCast(const dgVector& l0, const dgVector& l1, OnRayCastAction filter, OnRayPrecastAction prefilter, void* const userData) const +{ + dgBroadPhaseSegregatedRootNode* const root = (dgBroadPhaseSegregatedRootNode*)m_rootNode; + if (filter && (root->m_left || root->m_right)) { + dgVector segment(l1 - l0); + dgAssert (segment.m_w == dgFloat32 (0.0f)); + dgFloat32 dist2 = segment.DotProduct(segment).GetScalar(); + if (dist2 > dgFloat32(1.0e-8f)) { + + dgFloat32 distance[DG_BROADPHASE_MAX_STACK_DEPTH]; + const dgBroadPhaseNode* stackPool[DG_BROADPHASE_MAX_STACK_DEPTH]; + + dgFastRayTest ray(l0, l1); + + dgInt32 stack = 0; + if (root->m_left) { + stackPool[stack] = root->m_left; + distance[stack] = ray.BoxIntersect(root->m_left->m_minBox, root->m_left->m_maxBox); + stack++; + } + if (root->m_right) { + stackPool[stack] = root->m_right; + distance[stack] = ray.BoxIntersect(root->m_right->m_minBox, root->m_right->m_maxBox); + stack++; + } + if (stack == 2) { + if (distance[0] < distance[1]) { + dgSwap(distance[0], distance[1]); + dgSwap(stackPool[0], stackPool[1]); + } + } + + dgBroadPhase::RayCast(stackPool, distance, stack, l0, l1, ray, filter, prefilter, userData); + } + } +} + +dgInt32 dgBroadPhaseSegregated::ConvexCast(dgCollisionInstance* const shape, const dgMatrix& matrix, const dgVector& target, dgFloat32* const param, OnRayPrecastAction prefilter, void* const userData, dgConvexCastReturnInfo* const info, dgInt32 maxContacts, dgInt32 threadIndex) const +{ + dgInt32 totalCount = 0; + if (m_rootNode) { + dgVector boxP0; + dgVector boxP1; + dgAssert(matrix.TestOrthogonal()); + shape->CalcAABB(matrix, boxP0, boxP1); + + dgFloat32 distance[DG_BROADPHASE_MAX_STACK_DEPTH]; + const dgBroadPhaseNode* stackPool[DG_BROADPHASE_MAX_STACK_DEPTH]; + + dgVector velocA((target - matrix.m_posit) & dgVector::m_triplexMask); + dgVector velocB(dgFloat32(0.0f)); + dgFastRayTest ray(dgVector(dgFloat32(0.0f)), velocA); + + dgInt32 stack = 0; + dgBroadPhaseSegregatedRootNode* const root = (dgBroadPhaseSegregatedRootNode*)m_rootNode; + if (root->m_left) { + dgVector minBox(root->m_left->m_minBox - boxP1); + dgVector maxBox(root->m_left->m_maxBox - boxP0); + stackPool[stack] = root->m_left; + distance[stack] = ray.BoxIntersect(minBox, maxBox); + stack++; + } + if (root->m_right) { + dgVector minBox(root->m_right->m_minBox - boxP1); + dgVector maxBox(root->m_right->m_maxBox - boxP0); + + stackPool[stack] = root->m_right; + distance[stack] = ray.BoxIntersect(minBox, maxBox); + stack++; + } + if (stack == 2) { + if (distance[0] < distance[1]) { + dgSwap(distance[0], distance[1]); + dgSwap(stackPool[0], stackPool[1]); + } + } + + *param = dgFloat32 (1.0f); + totalCount = dgBroadPhase::ConvexCast(stackPool, distance, 2, velocA, velocB, ray, shape, matrix, target, param, prefilter, userData, info, maxContacts, threadIndex); + } + return totalCount; +} + +dgInt32 dgBroadPhaseSegregated::Collide(dgCollisionInstance* const shape, const dgMatrix& matrix, OnRayPrecastAction prefilter, void* const userData, dgConvexCastReturnInfo* const info, dgInt32 maxContacts, dgInt32 threadIndex) const +{ + dgInt32 totalCount = 0; + if (m_rootNode) { + dgVector boxP0; + dgVector boxP1; + dgAssert(matrix.TestOrthogonal()); + shape->CalcAABB(shape->GetLocalMatrix() * matrix, boxP0, boxP1); + + dgInt32 overlaped[DG_BROADPHASE_MAX_STACK_DEPTH]; + const dgBroadPhaseNode* stackPool[DG_BROADPHASE_MAX_STACK_DEPTH]; + + dgInt32 stack = 0; + dgBroadPhaseSegregatedRootNode* const root = (dgBroadPhaseSegregatedRootNode*)m_rootNode; + if (dgOverlapTest(m_rootNode->m_minBox, m_rootNode->m_maxBox, boxP0, boxP1)) { + if (root->m_left) { + stackPool[stack] = root->m_left; + overlaped[stack] = dgOverlapTest(root->m_left->m_minBox, root->m_left->m_maxBox, boxP0, boxP1); + stack ++; + } + if (root->m_right) { + stackPool[stack] = root->m_right; + overlaped[stack] = dgOverlapTest(root->m_right->m_minBox, root->m_right->m_maxBox, boxP0, boxP1); + stack++; + } + totalCount = dgBroadPhase::Collide(stackPool, overlaped, stack, boxP0, boxP1, shape, matrix, prefilter, userData, info, maxContacts, threadIndex); + } + } + + return totalCount; +} + +void dgBroadPhaseSegregated::FindCollidingPairs (dgBroadphaseSyncDescriptor* const descriptor, dgList::dgListNode* const nodePtr, dgInt32 threadID) +{ + DG_TRACKTIME(); + const dgFloat32 timestep = descriptor->m_timestep; + + dgList::dgListNode* node = nodePtr; + const dgInt32 threadCount = descriptor->m_world->GetThreadCount(); + + if (descriptor->m_fullScan) { + while (node) { + dgBroadPhaseNode* const broadPhaseNode = node->GetInfo(); + dgAssert(broadPhaseNode->IsLeafNode()); + + dgBody* const body = broadPhaseNode->GetBody(); + dgAssert(!body || (body->GetBroadPhase() == broadPhaseNode)); + + if (!(body && body->m_isdead)) { + if (broadPhaseNode->IsAggregate()) { + ((dgBroadPhaseAggregate*)broadPhaseNode)->SubmitSelfPairs(timestep, threadID); + } + + for (dgBroadPhaseNode* ptr = broadPhaseNode; ptr->m_parent; ptr = ptr->m_parent) { + dgBroadPhaseTreeNode* const parent = (dgBroadPhaseTreeNode*)ptr->m_parent; + dgAssert(!parent->IsLeafNode()); + dgBroadPhaseNode* const sibling = parent->m_right; + if (sibling && (sibling != ptr)) { + SubmitPairs(broadPhaseNode, sibling, timestep, 0, threadID); + } + } + } + + for (dgInt32 i = 0; i < threadCount; i++) { + dgBroadPhaseNode* const info = node ? node->GetInfo() : NULL; + node = (info && ((info->GetBody() && (info->GetBody()->GetInvMass().m_w != dgFloat32(0.0f))) || info->IsAggregate())) ? node->GetNext() : NULL; + } + } + + } else { + const dgBodyInfo* const bodyArray = &m_world->m_bodiesMemory[0]; + const dgInt32 bodyCount = descriptor->m_atomicPendingBodiesCount; + dgInt32* const atomicIndex = &descriptor->m_atomicIndex; + + for (dgInt32 i = dgAtomicExchangeAndAdd(atomicIndex, 1); i < bodyCount; i = dgAtomicExchangeAndAdd(atomicIndex, 1)) { + dgBroadPhaseNode* const broadPhaseNode = bodyArray[i].m_body->GetBroadPhase(); + + dgAssert(broadPhaseNode->IsLeafNode()); + dgAssert(!broadPhaseNode->GetBody() || (broadPhaseNode->GetBody()->GetBroadPhase() == broadPhaseNode)); + + for (dgBroadPhaseNode* ptr = broadPhaseNode; ptr->m_parent; ptr = ptr->m_parent) { + dgBroadPhaseTreeNode* const parent = (dgBroadPhaseTreeNode*)ptr->m_parent; + if (!parent->IsAggregate()) { + dgAssert(!parent->IsLeafNode()); + dgBroadPhaseNode* const rightSibling = parent->m_right; + if (rightSibling && (rightSibling != ptr)) { + SubmitPairs(broadPhaseNode, rightSibling, timestep, 0, threadID); + } + + dgBroadPhaseNode* const leftSibling = parent->m_left; + if (leftSibling && (leftSibling != ptr)) { + SubmitPairs(broadPhaseNode, leftSibling, timestep, threadCount, threadID); + } + } + } + } + } +} + diff --git a/thirdparty/src/newton/dgPhysics/dgBroadPhaseSegregated.h b/thirdparty/src/newton/dgPhysics/dgBroadPhaseSegregated.h new file mode 100644 index 000000000..fd8a4af7e --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgBroadPhaseSegregated.h @@ -0,0 +1,68 @@ +/* Copyright (c) <2003-2019> +* +* 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 __AFX_BROADPHASE_PERSINTENT_H_ +#define __AFX_BROADPHASE_PERSINTENT_H_ + +#include "dgPhysicsStdafx.h" +#include "dgBroadPhase.h" + + +class dgBroadPhaseSegregated : public dgBroadPhase +{ + public: + DG_CLASS_ALLOCATOR(allocator); + + dgBroadPhaseSegregated (dgWorld* const world); + virtual ~dgBroadPhaseSegregated (); + + protected: + virtual dgInt32 GetType() const; + virtual void Add(dgBody* const body); + virtual void Remove(dgBody* const body); + virtual void InvalidateCache(); + virtual dgBroadPhaseAggregate* CreateAggregate(); + virtual void DestroyAggregate(dgBroadPhaseAggregate* const aggregate); + + virtual void CheckStaticDynamic(dgBody* const body, dgFloat32 mass); + virtual void LinkAggregate(dgBroadPhaseAggregate* const aggregate); + virtual void UnlinkAggregate(dgBroadPhaseAggregate* const aggregate); + virtual void FindCollidingPairs (dgBroadphaseSyncDescriptor* const descriptor, dgList::dgListNode* const node, dgInt32 threadID); + + virtual void ResetEntropy(); + virtual void UpdateFitness(); + virtual void ForEachBodyInAABB(const dgVector& minBox, const dgVector& maxBox, OnBodiesInAABB callback, void* const userData) const; + virtual void RayCast(const dgVector& p0, const dgVector& p1, OnRayCastAction filter, OnRayPrecastAction prefilter, void* const userData) const; + virtual dgInt32 Collide(dgCollisionInstance* const shape, const dgMatrix& matrix, OnRayPrecastAction prefilter, void* const userData, dgConvexCastReturnInfo* const info, dgInt32 maxContacts, dgInt32 threadIndex) const; + virtual dgInt32 ConvexCast(dgCollisionInstance* const shape, const dgMatrix& matrix, const dgVector& target, dgFloat32* const param, OnRayPrecastAction prefilter, void* const userData, dgConvexCastReturnInfo* const info, dgInt32 maxContacts, dgInt32 threadIndex) const; + void RemoveNode(dgBroadPhaseNode* const node); + + private: + void AddStaticBody(dgBody* const body); + void AddDynamicBody(dgBody* const body); + + dgFloat64 m_staticEntropy; + dgFloat64 m_dynamicsEntropy; + dgFitnessList m_staticFitness; + dgFitnessList m_dynamicsFitness; + bool m_staticNeedsUpdate; +}; +#endif diff --git a/thirdparty/src/newton/dgPhysics/dgCollision.cpp b/thirdparty/src/newton/dgPhysics/dgCollision.cpp new file mode 100644 index 000000000..4f8353ce4 --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgCollision.cpp @@ -0,0 +1,152 @@ +/* Copyright (c) <2003-2019> +* +* 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 "dgPhysicsStdafx.h" +#include "dgBody.h" +#include "dgWorld.h" +#include "dgCollision.h" + +dgVector dgCollision::m_flushZero (dgFloat32 (1.0e-7f)); + +////////////////////////////////////////////////////////////////////// +// Construction/Destruction +////////////////////////////////////////////////////////////////////// +dgCollision::dgCollision(dgMemoryAllocator* const allocator, dgUnsigned32 signature, dgCollisionID id) + :m_inertia(dgVector::m_zero) + ,m_crossInertia(dgVector::m_zero) + ,m_centerOfMass(dgVector::m_zero) + ,m_boxSize (dgVector::m_zero) + ,m_boxOrigin (dgVector::m_zero) + ,m_rtti(0) + ,m_refCount(1) + ,m_signature(signature) + ,m_collisionId(id) + ,m_allocator(allocator) +{ +} + +dgCollision::dgCollision (const dgCollision& source) + :m_inertia(source.m_inertia) + ,m_crossInertia(source.m_crossInertia) + ,m_centerOfMass(source.m_centerOfMass) + ,m_boxSize(source.m_boxSize) + ,m_boxOrigin (source.m_boxOrigin) + ,m_rtti(source.m_rtti) + ,m_refCount(1) + ,m_signature(source.m_signature) + ,m_collisionId(source.m_collisionId) + ,m_allocator(source.m_allocator) +{ +} + +dgCollision::dgCollision (dgWorld* const world, dgDeserialize deserialization, void* const userData, dgInt32 revision) + :m_inertia(dgVector::m_zero) + ,m_crossInertia(dgVector::m_zero) + ,m_centerOfMass(dgVector::m_zero) + ,m_boxSize (dgVector::m_zero) + ,m_boxOrigin (dgVector::m_zero) + ,m_rtti(0) + ,m_refCount(1) + ,m_signature(0) + ,m_collisionId(dgCollisionID(0)) + ,m_allocator(world->GetAllocator()) +{ + dgInt32 collisionId; + deserialization (userData, &m_inertia, sizeof (m_inertia)); + deserialization (userData, &m_crossInertia, sizeof (m_crossInertia)); + deserialization (userData, &m_centerOfMass, sizeof (m_centerOfMass)); + deserialization (userData, &m_boxSize, sizeof (m_boxSize)); + deserialization (userData, &m_boxOrigin, sizeof (m_boxOrigin)); + deserialization (userData, &m_rtti, sizeof (m_rtti)); + deserialization (userData, &m_signature, sizeof (m_signature)); + deserialization (userData, &collisionId, sizeof (collisionId)); + m_collisionId = dgCollisionID(collisionId); +} + +dgCollision::~dgCollision() +{ +} + +dgUnsigned32 dgCollision::Quantize(dgFloat32 value) +{ + return dgUnsigned32 (value * 1024.0f); +} + +dgUnsigned32 dgCollision::Quantize(const void* const buffer, int size) +{ + dgUnsigned32 crc = dgCRC (buffer, size); + return crc; +} + +void dgCollision::MassProperties () +{ + // using general central theorem, to extract the Inertia relative to the center of mass + //IImatrix = IIorigin + unitmass * [(displacemnet % displacemnet) * identityMatrix - transpose(displacement) * displacement)]; + + dgMatrix inertia (dgGetIdentityMatrix()); + inertia[0][0] = m_inertia[0]; + inertia[1][1] = m_inertia[1]; + inertia[2][2] = m_inertia[2]; + inertia[0][1] = m_crossInertia[2]; + inertia[1][0] = m_crossInertia[2]; + inertia[0][2] = m_crossInertia[1]; + inertia[2][0] = m_crossInertia[1]; + inertia[1][2] = m_crossInertia[0]; + inertia[2][1] = m_crossInertia[0]; + + dgVector origin (m_centerOfMass); + dgFloat32 originMag2 = origin.DotProduct(origin & dgVector::m_triplexMask).GetScalar(); + + dgMatrix Covariance (origin, origin); + dgMatrix parallel (dgGetIdentityMatrix()); + for (dgInt32 i = 0; i < 3; i ++ ) { + parallel[i][i] = originMag2; + inertia[i] += (parallel[i] - Covariance[i]); + dgAssert (inertia[i][i] > dgFloat32 (0.0f)); + } + + m_inertia[0] = inertia[0][0]; + m_inertia[1] = inertia[1][1]; + m_inertia[2] = inertia[2][2]; + m_crossInertia[0] = inertia[2][1]; + m_crossInertia[1] = inertia[2][0]; + m_crossInertia[2] = inertia[1][0]; +} + + +void dgCollision::GetCollisionInfo(dgCollisionInfo* const info) const +{ + info->m_collisionType = m_collisionId; +} + +void dgCollision::SerializeLow (dgSerialize callback, void* const userData) const +{ + dgInt32 collisionId = m_collisionId; + callback (userData, &m_inertia, sizeof (m_inertia)); + callback (userData, &m_crossInertia, sizeof (m_crossInertia)); + callback (userData, &m_centerOfMass, sizeof (m_centerOfMass)); + callback (userData, &m_boxSize, sizeof (m_boxSize)); + callback (userData, &m_boxOrigin, sizeof (m_boxOrigin)); + callback (userData, &m_rtti, sizeof (m_rtti)); + callback (userData, &m_signature, sizeof (m_signature)); + callback (userData, &collisionId, sizeof (collisionId)); +} + diff --git a/thirdparty/src/newton/dgPhysics/dgCollision.h b/thirdparty/src/newton/dgPhysics/dgCollision.h new file mode 100644 index 000000000..d9cd8f2c0 --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgCollision.h @@ -0,0 +1,397 @@ +/* Copyright (c) <2003-2019> +* +* 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_COLLISION_H_ +#define _DG_COLLISION_H_ + + +class dgBody; +class dgWorld; +class dgCollision; +class dgMeshEffect; +class dgContactPoint; +class dgPolygonSoupDesc; +class dgCollisionConvex; +class dgPolygonMeshDesc; +class dgCollisionInstance; +class dgCollisionConvexHull; +class dgPolygonSoupRayHitDesc; + +#ifdef _DEBUG +// #define DG_DEBUG_AABB +#endif + +#define PREFILTER_RAYCAST(filter,body,collision,userData) (filter && !filter(body,collision,userData)) + +typedef dgInt32(dgApi *OnBodiesInAABB) (dgBody* body, void* const userData); +typedef dgUnsigned32(dgApi *OnRayPrecastAction) (const dgBody* const body, const dgCollisionInstance* const collision, void* const userData); +typedef dgFloat32(dgApi *OnRayCastAction) (const dgBody* const body, const dgCollisionInstance* const collision, const dgVector& contact, const dgVector& normal, dgInt64 collisionID, void* const userData, dgFloat32 intersetParam); + + +enum dgCollisionID +{ + // do not change the order of these enum + m_sphereCollision = 0, + m_capsuleCollision, + m_cylinderCollision, + m_chamferCylinderCollision, + m_boxCollision, + m_coneCollision, + m_convexHullCollision, + // this must be the last convex shape ID + m_nullCollision, + + m_compoundCollision, + m_boundingBoxHierachy, + m_heightField, + m_deformableClothPatch, + m_deformableSolidMesh, + m_userMesh, + m_sceneCollision, + m_compoundFracturedCollision, + + // these are for internal use only + m_contactCloud, + m_polygonCollision, + m_lumpedMassCollision +}; + + +DG_MSC_VECTOR_ALIGNMENT +class dgCollisionInfo +{ + public: + class dgInstanceMaterial + { + public: + dgInstanceMaterial() + { + memset(this, 0, sizeof (dgInstanceMaterial)); + } + + dgInt64 m_userId; + union { + void* m_userData; + dgUnsigned64 m_alignPad; + }; + union { + dgUnsigned64 m_intData; + dgFloat32 m_floatData; + } m_userParam[6]; + }; + + struct dgBoxData + { + dgFloat32 m_x; + dgFloat32 m_y; + dgFloat32 m_z; + } ; + + struct dgSphereData + { + dgFloat32 m_radius; + } ; + + struct dgCylinderData + { + dgFloat32 m_radio0; + dgFloat32 m_radio1; + dgFloat32 m_height; + }; + + struct dgCapsuleData + { + dgFloat32 m_radio0; + dgFloat32 m_radio1; + dgFloat32 m_height; + }; + + struct dgConeData + { + dgFloat32 m_r; + dgFloat32 m_height; + }; + + struct dgChamferCylinderData + { + dgFloat32 m_r; + dgFloat32 m_height; + }; + + struct dgConvexHullData + { + dgInt32 m_vertexCount; + dgInt32 m_strideInBytes; + dgInt32 m_faceCount; + dgVector* m_vertex; + }; + + struct dgConvexModifierData + { + dgCollision* m_child; + }; + + struct dgCoumpountCollisionData + { + dgInt32 m_chidrenCount; + //dgCollision** m_chidren; + }; + + struct dgCollisionBVHData + { + dgInt32 m_vertexCount; + dgInt32 m_indexCount; + }; + + struct dgDeformableMeshData + { + dgInt32 m_vertexCount; + dgInt32 m_triangleCount; + dgInt32 m_vertexStrideInBytes; + dgUnsigned16* m_indexList; + dgFloat32* m_vertexList; + }; + + struct dgHeightMapCollisionData + { + dgInt32 m_width; + dgInt32 m_height; + dgInt32 m_gridsDiagonals; + dgInt32 m_elevationDataType; // 0 = 32 bit floats, 1 = unsigned 16 bit intergers + dgFloat32 m_verticalScale; + dgFloat32 m_horizonalScale_x; + dgFloat32 m_horizonalScale_z; + void* m_elevation; + dgInt8* m_atributes; + }; + + struct dgSceneData + { + dgInt32 m_childrenProxyCount; + }; + + dgMatrix m_offsetMatrix; + dgInstanceMaterial m_collisionMaterial; + dgInt32 m_collisionType; + union + { + dgBoxData m_box; + dgConeData m_cone; + dgSphereData m_sphere; + dgCapsuleData m_capsule; + dgCylinderData m_cylinder; + dgChamferCylinderData m_chamferCylinder; + dgConvexHullData m_convexHull; + dgDeformableMeshData m_deformableMesh; + dgConvexModifierData m_convexModifierData; + dgCoumpountCollisionData m_compoundCollision; + dgCollisionBVHData m_bvhCollision; + dgHeightMapCollisionData m_heightFieldCollision; + dgSceneData m_sceneCollision; + dgFloat32 m_paramArray[32]; + }; +}DG_GCC_VECTOR_ALIGNMENT; + +DG_MSC_VECTOR_ALIGNMENT +class dgCollision +{ + public: + typedef void (dgApi *OnDebugCollisionMeshCallback) (void* userData, int vertexCount, const dgFloat32* faceArray, int faceId); + + enum dgRTTI { + dgCollisionNull_RTTI = 1<<0, + dgCollisionBox_RTTI = 1<<1, + dgCollisionCone_RTTI = 1<<2, + dgCollisionSphere_RTTI = 1<<3, + dgCollisionCapsule_RTTI = 1<<4, + dgCollisionCylinder_RTTI = 1<<5, + dgCollisionConvexHull_RTTI = 1<<6, + dgCollisionChamferCylinder_RTTI = 1<<7, + dgCollisionConvexPolygon_RTTI = 1<<8, + dgCollisionConvexShape_RTTI = 1<<9, + dgCollisionCompound_RTTI = 1<<10, + dgCollisionBVH_RTTI = 1<<11, + dgCollisionMesh_RTTI = 1<<12, + dgCollisionLumpedMass_RTTI = 1<<13, + dgCollisionDeformableMesh_RTTI = 1<<14, + dgCollisionDeformableSolidMesh_RTTI = 1<<15, + dgCollisionMassSpringDamperSystem_RTTI = 1<<16, + dgCollisionIncompressibleParticles_RTTI = 1<<17, + dgCollisionUserMesh_RTTI = 1<<18, + dgCollisionHeightField_RTTI = 1<<19, + dgCollisionScene_RTTI = 1<<20, + dgCollisionCompoundBreakable_RTTI = 1<<21, + }; + + DG_CLASS_ALLOCATOR(allocator) + static dgUnsigned32 Quantize (dgFloat32 value); + static dgUnsigned32 Quantize(const void* const buffer, int size); + + // these function should be be virtual + dgInt32 IsType (dgRTTI type) const; + dgUnsigned32 GetSignature () const; + dgCollisionID GetCollisionPrimityType () const; + + virtual dgVector SupportVertex (const dgVector& dir, dgInt32* const vertexIndex) const = 0; + virtual dgVector SupportVertexSpecialProjectPoint (const dgVector& point, const dgVector& dir) const = 0; + virtual dgVector SupportVertexSpecial (const dgVector& dir, dgFloat32 skinSkinThickness, dgInt32* const vertexIndex) const = 0; + + virtual dgInt32 CalculatePlaneIntersection (const dgVector& normal, const dgVector& point, dgVector* const contactsOut) const = 0; + + virtual void SetCollisionBBox (const dgVector& p0, const dgVector& p1) = 0; + virtual void CalcAABB (const dgMatrix& matrix, dgVector& p0, dgVector& p1) const = 0; + + virtual void DebugCollision (const dgMatrix& matrix, dgCollision::OnDebugCollisionMeshCallback callback, void* const userData) const = 0; + virtual dgFloat32 RayCast (const dgVector& localP0, const dgVector& localP1, dgFloat32 maxT, dgContactPoint& contactOut, const dgBody* const body, void* const userData, OnRayPrecastAction preFilter) const = 0; + + virtual dgFloat32 GetVolume () const = 0; + + virtual void MassProperties (); + virtual dgFloat32 CalculateMassProperties (const dgMatrix& offset, dgVector& inertia, dgVector& crossInertia, dgVector& centerOfMass) const {dgAssert (0); return 0;} + virtual dgMatrix CalculateInertiaAndCenterOfMass (const dgMatrix& m_alignMatrix, const dgVector& localScale, const dgMatrix& matrix) const {dgAssert (0); return dgGetZeroMatrix();} + + virtual dgFloat32 GetBoxMinRadius () const = 0; + virtual dgFloat32 GetBoxMaxRadius () const = 0; + + virtual dgVector CalculateVolumeIntegral (const dgMatrix& globalMatrix, const dgVector& globalPlane, const dgCollisionInstance& parentScale) const = 0; + virtual void Serialize(dgSerialize callback, void* const userData) const = 0; + + virtual void GetCollisionInfo(dgCollisionInfo* const info) const; + virtual void SerializeLow(dgSerialize callback, void* const userData) const; + + virtual void CalculateImplicitContacts(dgInt32 count, dgContactPoint* const contactPoints) const {dgAssert (0);} + + + virtual dgInt32 GetConvexVertexCount() const; + + const dgCollision* AddRef () const; + dgInt32 GetRefCount() const; + virtual dgInt32 Release () const; + + const dgVector& GetObbOrigin() const; + virtual dgVector GetObbSize() const; + + dgFloat32 GetUmbraClipSize () const; + dgMemoryAllocator* GetAllocator() const; + + protected: + dgCollision (const dgCollision& source); + dgCollision (dgMemoryAllocator* const allocator, dgUnsigned32 signature, dgCollisionID id); + dgCollision (dgWorld* const world, dgDeserialize deserialization, void* const userData, dgInt32 revision); + virtual ~dgCollision(); + + void SetSignature (dgInt32 signature); + virtual dgInt32 CalculateSignature () const = 0; + + dgVector m_inertia; + dgVector m_crossInertia; + dgVector m_centerOfMass; + dgVector m_boxSize; + dgVector m_boxOrigin; + dgInt32 m_rtti; + mutable dgInt32 m_refCount; + dgUnsigned32 m_signature; + dgCollisionID m_collisionId; + dgMemoryAllocator* m_allocator; + + static dgVector m_flushZero; + friend class dgBody; + friend class dgWorld; + friend class dgMinkowskiConv; + friend class dgCollisionInstance; + friend class dgCollisionCompound; +}DG_GCC_VECTOR_ALIGNMENT; + +DG_INLINE dgCollisionID dgCollision::GetCollisionPrimityType () const +{ + return m_collisionId; +} + +DG_INLINE dgUnsigned32 dgCollision::GetSignature () const +{ + return m_signature; +} + +DG_INLINE void dgCollision::SetSignature (dgInt32 signature) +{ + m_signature = dgUnsigned32 (signature); +} + + +DG_INLINE const dgCollision* dgCollision::AddRef () const +{ + dgAtomicExchangeAndAdd (&m_refCount, 1); + return this; +} + +DG_INLINE dgInt32 dgCollision::Release () const +{ + dgAtomicExchangeAndAdd (&m_refCount, -1); + if (m_refCount) { + return m_refCount; + } + delete this; + return 0; +} + +DG_INLINE dgInt32 dgCollision::GetRefCount() const +{ + dgAssert (0); + return m_refCount; +} + + +DG_INLINE dgMemoryAllocator* dgCollision::GetAllocator() const +{ + return m_allocator; +} + + +DG_INLINE dgFloat32 dgCollision::GetUmbraClipSize () const +{ +// return GetMax (GetBoxMaxRadius() * dgFloat32 (2.0f) + dgFloat32 (1.0f), dgFloat32 (16.0f)); + return dgFloat32 (3.0f) * GetBoxMaxRadius(); +} + +DG_INLINE dgInt32 dgCollision::GetConvexVertexCount() const +{ + return 0; +} + +DG_INLINE dgInt32 dgCollision::IsType (dgRTTI type) const +{ + return type & m_rtti; +} + +DG_INLINE dgVector dgCollision::GetObbSize() const +{ + return m_boxSize; +} + +DG_INLINE const dgVector& dgCollision::GetObbOrigin() const +{ + return m_boxOrigin; +} + + +#endif + + diff --git a/thirdparty/src/newton/dgPhysics/dgCollisionBVH.cpp b/thirdparty/src/newton/dgPhysics/dgCollisionBVH.cpp new file mode 100644 index 000000000..d772d6764 --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgCollisionBVH.cpp @@ -0,0 +1,422 @@ +/* Copyright (c) <2003-2019> +* +* 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 "dgPhysicsStdafx.h" +#include "dgBody.h" +#include "dgWorld.h" +#include "dgCollisionBVH.h" + + +dgCollisionBVH::dgCollisionBVH(dgWorld* const world) + :dgCollisionMesh (world, m_boundingBoxHierachy), dgAABBPolygonSoup() + ,m_trianglesCount(0) +{ + m_rtti |= dgCollisionBVH_RTTI; + m_builder = NULL; + m_userRayCastCallback = NULL; +} + +dgCollisionBVH::dgCollisionBVH (dgWorld* const world, dgDeserialize deserialization, void* const userData, dgInt32 revisionNumber) + :dgCollisionMesh (world, deserialization, userData, revisionNumber) + ,dgAABBPolygonSoup() + ,m_trianglesCount(0) +{ + dgAssert (m_rtti | dgCollisionBVH_RTTI); + m_builder = NULL;; + m_userRayCastCallback = NULL; + + dgAABBPolygonSoup::Deserialize (deserialization, userData, revisionNumber); + + dgVector p0; + dgVector p1; + GetAABB (p0, p1); + SetCollisionBBox(p0, p1); + + deserialization(userData, &m_trianglesCount, sizeof (dgInt32)); +} + +dgCollisionBVH::~dgCollisionBVH(void) +{ +} + +void dgCollisionBVH::Serialize(dgSerialize callback, void* const userData) const +{ + SerializeLow(callback, userData); + dgAABBPolygonSoup::Serialize ((dgSerialize) callback, userData); + callback(userData, &m_trianglesCount, sizeof (dgInt32)); +} + +void dgCollisionBVH::BeginBuild() +{ + m_builder = new (m_allocator) dgPolygonSoupDatabaseBuilder(m_allocator); + m_builder->Begin(); +} + +void dgCollisionBVH::AddFace(dgInt32 vertexCount, const dgFloat32* const vertexPtr, dgInt32 strideInBytes, dgInt32 faceAttribute) +{ + dgInt32 faceArray; + dgInt32 indexList[256]; + + faceArray = vertexCount; + dgAssert (vertexCount < dgInt32 (sizeof (indexList) / sizeof (indexList[0]))); + for (dgInt32 i = 0; i < vertexCount; i ++) { + indexList[i] = i; + } + m_builder->AddMesh (vertexPtr, vertexCount, strideInBytes, 1, &faceArray, indexList, &faceAttribute, dgGetIdentityMatrix()); +} + +void dgCollisionBVH::SetCollisionRayCastCallback (dgCollisionBVHUserRayCastCallback rayCastCallback) +{ + m_userRayCastCallback = rayCastCallback; +} + +void dgCollisionBVH::EndBuild(dgInt32 optimize) +{ + dgVector p0; + dgVector p1; + + bool state = optimize ? true : false; + +#ifdef _DEBUG + if (state && (optimize >> 1)) { + char debugMesh[256]; + sprintf (debugMesh, "debugMesh_%d.ply", optimize-1); + m_builder->SavePLY(debugMesh); + } +#endif + + m_builder->End(state); + Create (*m_builder, state); + CalculateAdjacendy(); + + GetAABB (p0, p1); + SetCollisionBBox (p0, p1); + + delete m_builder; + m_builder = NULL; + + dgMeshVertexListIndexList data; + data.m_indexList = NULL; + data.m_userDataList = NULL; + data.m_maxIndexCount = 1000000000; + data.m_triangleCount = 0; + dgVector zero (dgFloat32 (0.0f)); + dgFastAABBInfo box (dgGetIdentityMatrix(), dgVector (dgFloat32 (1.0e15f))); + ForAllSectors (box, zero, dgFloat32 (1.0f), GetTriangleCount, &data); + m_trianglesCount = data.m_triangleCount; +} + + +void dgCollisionBVH::GetCollisionInfo(dgCollisionInfo* const info) const +{ + dgCollision::GetCollisionInfo(info); + + info->m_bvhCollision.m_vertexCount = GetVertexCount(); + info->m_bvhCollision.m_indexCount = m_trianglesCount * 3; +} + +void dgCollisionBVH::ForEachFace (dgAABBIntersectCallback callback, void* const context) const +{ + dgVector p0 (-1.0e10f, -1.0e10f, -1.0e10f, 1.0f); + dgVector p1 ( 1.0e10f, 1.0e10f, 1.0e10f, 1.0f); + dgVector zero (dgFloat32 (0.0f)); + dgFastAABBInfo box (dgGetIdentityMatrix(), dgVector (dgFloat32 (1.0e15f))); + //ForAllSectors (p0, p1, zero, dgFloat32 (1.0f), callback, context); + ForAllSectors (box, zero, dgFloat32 (1.0f), callback, context); +} + + +dgIntersectStatus dgCollisionBVH::CollectVertexListIndexList (void* const context, const dgFloat32* const polygon, dgInt32 strideInBytes, const dgInt32* const indexArray, dgInt32 indexCount, dgFloat32 hitDistance) +{ + dgMeshVertexListIndexList& data = (*(dgMeshVertexListIndexList*) context); + + if ((data.m_triangleCount + indexCount - 2) * 3 > data.m_maxIndexCount) { + return t_StopSearh; + } + + dgInt32 k = data.m_triangleCount; + dgInt32 j = data.m_triangleCount * 3; + dgInt32 index0 = indexArray[0]; + dgInt32 index1 = indexArray[1]; + dgInt32 attribute = indexArray[indexCount]; + for (dgInt32 i = 2; i < indexCount; i ++) { + dgInt32 index2 = indexArray[i]; + data.m_indexList[j + 0] = index0; + data.m_indexList[j + 1] = index1; + data.m_indexList[j + 2] = index2; + data.m_userDataList[k] = attribute; + index1 = index2; + k ++; + j += 3; + } + + dgAssert (j <= data.m_maxIndexCount); + data.m_triangleCount = k; + dgAssert ((data.m_triangleCount * 3) <= data.m_maxIndexCount); + + return t_ContinueSearh; +} + + + +dgIntersectStatus dgCollisionBVH::GetTriangleCount (void* const context, const dgFloat32* const polygon, dgInt32 strideInBytes, const dgInt32* const indexArray, dgInt32 indexCount, dgFloat32 hitDistance) +{ + dgMeshVertexListIndexList& data = (*(dgMeshVertexListIndexList*) context); + + if ((data.m_triangleCount + indexCount - 2) * 3 > data.m_maxIndexCount) { + return t_StopSearh; + } + + data.m_triangleCount += (indexCount - 2); + dgAssert ((data.m_triangleCount * 3) <= data.m_maxIndexCount); + return t_ContinueSearh; +} + + +void dgCollisionBVH::GetVertexListIndexList (const dgVector& p0, const dgVector& p1, dgMeshVertexListIndexList &data) const +{ + dgFastAABBInfo box (p0, p1); + ForAllSectors (box, dgVector (dgFloat32 (0.0f)), dgFloat32 (1.0f), CollectVertexListIndexList, &data); + + data.m_veterxArray = GetLocalVertexPool(); + data.m_vertexCount = GetVertexCount(); + data.m_vertexStrideInBytes = GetStrideInBytes(); + +} + + + +dgFloat32 dgCollisionBVH::RayHit (void* const context, const dgFloat32* const polygon, dgInt32 strideInBytes, const dgInt32* const indexArray, dgInt32 indexCount) +{ + dgBVHRay& me = *((dgBVHRay*) context); + dgVector normal (&polygon[indexArray[indexCount + 1] * (strideInBytes / sizeof (dgFloat32))]); + normal = normal & dgVector::m_triplexMask; + dgFloat32 t = me.PolygonIntersect (normal, me.m_t, polygon, strideInBytes, indexArray, indexCount); + if (t <= (me.m_t * dgFloat32 (1.0001f))) { +// if ((t * dgFloat32 (1.0001f)) >= me.m_t) { +// dgFloat32 dist0; +// dgFloat32 dist1; +// dist0 = me.m_diff % normal; +// dist1 = me.m_diff % me.m_normal; +// if (dist0 < dist1) { +// me.m_t = t; +// me.m_normal = normal; +// me.m_id = me.m_me->GetTagId(indexArray, indexCount); +// } else { +// t = me.m_t; +// } +// } else { + me.m_t = t; + me.m_normal = normal; + me.m_id = me.m_me->GetTagId(indexArray, indexCount); +// } + } + return t; +} + +dgFloat32 dgCollisionBVH::RayHitUser (void* const context, const dgFloat32* const polygon, dgInt32 strideInBytes, const dgInt32* const indexArray, dgInt32 indexCount) +{ + dgAssert (0); + dgFloat32 t = dgFloat32 (1.2f); + dgBVHRay& me = *((dgBVHRay*) context); + dgVector normal (&polygon[indexArray[indexCount + 1] * (strideInBytes / sizeof (dgFloat32))]); + normal = normal & dgVector::m_triplexMask; +dgAssert (0); + t = me.PolygonIntersect (normal, me.m_t, polygon, strideInBytes, indexArray, indexCount); + if (t < dgFloat32 (1.0f)) { + if (t < me.m_t) { + me.m_t = t; + me.m_normal = normal; + me.m_id = me.m_me->GetTagId(indexArray, indexCount); + } + normal = me.m_matrix.RotateVector(normal); + t = me.m_me->GetDebugRayCastCallback() (me.m_myBody, me.m_me, t, &normal[0], dgInt32 (me.m_me->GetTagId(indexArray, indexCount)), me.m_userData); + } + return t; +} + + + +dgFloat32 dgCollisionBVH::RayCast (const dgVector& localP0, const dgVector& localP1, dgFloat32 maxT, dgContactPoint& contactOut, const dgBody* const body, void* const userData, OnRayPrecastAction preFilter) const +{ + dgBVHRay ray (localP0, localP1); + ray.m_t = dgMin(maxT, dgFloat32 (1.0f)); + ray.m_me = this; + ray.m_userData = userData; + if (!m_userRayCastCallback) { + ForAllSectorsRayHit (ray, maxT, RayHit, &ray); + if (ray.m_t < maxT) { + maxT = ray.m_t; + dgAssert (ray.m_normal.m_w == dgFloat32 (0.0f)); + dgAssert (ray.m_normal.DotProduct(ray.m_normal).GetScalar() > dgFloat32(0.0f)); + //contactOut.m_normal = ray.m_normal.Scale (dgRsqrt (ray.m_normal.DotProduct(ray.m_normal).GetScalar() + dgFloat32 (1.0e-8f))); + contactOut.m_normal = ray.m_normal.Normalize(); + //contactOut.m_userId = ray.m_id; + contactOut.m_shapeId0 = ray.m_id; + contactOut.m_shapeId1 = ray.m_id; + } + } else { + if (body) { + //ray.m_matrix = body->m_collisionWorldMatrix; + ray.m_matrix = body->m_collision->GetGlobalMatrix(); + } + + ForAllSectorsRayHit (ray, maxT, RayHitUser, &ray); + if (ray.m_t < dgFloat32 (1.0f)) { + maxT = ray.m_t; + dgAssert(ray.m_normal.m_w == dgFloat32(0.0f)); + dgAssert(ray.m_normal.DotProduct(ray.m_normal).GetScalar() > dgFloat32(0.0f)); + //contactOut.m_normal = ray.m_normal.Scale (dgRsqrt (ray.m_normal.DotProduct(ray.m_normal).GetScalar() + dgFloat32 (1.0e-8f))); + contactOut.m_normal = ray.m_normal.Normalize(); + //contactOut.m_userId = ray.m_id; + contactOut.m_shapeId0 = ray.m_id; + contactOut.m_shapeId1 = ray.m_id; + } + } + return maxT; +} + +dgIntersectStatus dgCollisionBVH::GetPolygon (void* const context, const dgFloat32* const polygon, dgInt32 strideInBytes, const dgInt32* const indexArray, dgInt32 indexCount, dgFloat32 hitDistance) +{ + dgPolygonMeshDesc& data = (*(dgPolygonMeshDesc*) context); + if (data.m_faceCount >= DG_MAX_COLLIDING_FACES) { + dgTrace (("buffer Over float, try using a lower resolution mesh for collision\n")); + return t_StopSearh; + } + if ((data.m_globalIndexCount + indexCount * 2 + 3) >= DG_MAX_COLLIDING_INDICES) { + dgTrace (("buffer Over float, try using a lower resolution mesh for collision\n")); + return t_StopSearh; + } + + if (data.m_me->GetDebugCollisionCallback()) { + dgTriplex triplex[128]; + dgInt32 stride = dgInt32 (strideInBytes / sizeof (dgFloat32)); + const dgVector scale = data.m_polySoupInstance->GetScale(); + dgMatrix matrix (data.m_polySoupInstance->GetLocalMatrix() * data.m_polySoupBody->GetMatrix()); + for (dgInt32 i = 0; i < indexCount; i ++ ) { + dgVector p (matrix.TransformVector(scale * (dgVector(&polygon[indexArray[i] * stride]) & dgVector::m_triplexMask))); + triplex[i].m_x = p.m_x; + triplex[i].m_y = p.m_y; + triplex[i].m_z = p.m_z; + } + if (data.m_polySoupBody) { + data.m_me->GetDebugCollisionCallback() (data.m_polySoupBody, data.m_objBody, indexArray[indexCount], indexCount, &triplex[0].m_x, sizeof (dgTriplex)); + } + } + + dgAssert (data.m_vertex == polygon); + dgInt32 count = indexCount * 2 + 3; + + data.m_faceIndexCount[data.m_faceCount] = indexCount; +// data.m_faceIndexStart[data.m_faceCount] = data.m_faceCount ? (data.m_faceIndexStart[data.m_faceCount - 1] + data.m_faceIndexCount[data.m_faceCount - 1]) : 0; + data.m_faceIndexStart[data.m_faceCount] = data.m_globalIndexCount; + data.m_hitDistance[data.m_faceCount] = hitDistance; + data.m_faceCount ++; + dgInt32* const dst = &data.m_faceVertexIndex[data.m_globalIndexCount]; + + //the docks say memcpy is an intrinsic function but as usual this is another Microsoft lied + //memcpy (dst, indexArray, sizeof (dgInt32) * count); + for (dgInt32 i = 0; i < count; i ++) { + dst[i] = indexArray[i]; + } + + data.m_globalIndexCount += count; + + return t_ContinueSearh; +} + +void dgCollisionBVH::GetCollidingFaces (dgPolygonMeshDesc* const data) const +{ + data->m_me = this; + data->m_vertex = GetLocalVertexPool(); + data->m_vertexStrideInBytes = GetStrideInBytes(); + + data->m_faceCount = 0; + data->m_globalIndexCount = 0; + data->m_faceIndexCount = data->m_meshData.m_globalFaceIndexCount; + data->m_faceIndexStart = data->m_meshData.m_globalFaceIndexStart; + data->m_faceVertexIndex = data->m_globalFaceVertexIndex; + data->m_hitDistance = data->m_meshData.m_globalHitDistance; + ForAllSectors (*data, data->m_boxDistanceTravelInMeshSpace, data->m_maxT, GetPolygon, data); +} + + +dgVector dgCollisionBVH::SupportVertex (const dgVector& dir) const +{ + return ForAllSectorsSupportVectex (dir); +} + +dgVector dgCollisionBVH::SupportVertex(const dgVector& dir, dgInt32* const vertexIndex) const +{ + return ForAllSectorsSupportVectex(dir); +} + +dgVector dgCollisionBVH::SupportVertexSpecial(const dgVector& dir, dgFloat32 skinThickness, dgInt32* const vertexIndex) const +{ + dgAssert(0); + return SupportVertex(dir, vertexIndex); +} + + + +void dgCollisionBVH::GetLocalAABB (const dgVector& p0, const dgVector& p1, dgVector& boxP0, dgVector& boxP1) const +{ + dgAssert (0); +} + +struct dgCollisionBVHShowPolyContext +{ + dgMatrix m_matrix; + void* m_userData; + dgCollision::OnDebugCollisionMeshCallback m_callback; +}; + +dgIntersectStatus dgCollisionBVH::ShowDebugPolygon (void* const context, const dgFloat32* const polygon, dgInt32 strideInBytes, const dgInt32* const indexArray, dgInt32 indexCount, dgFloat32 hitDistance) +{ + dgTriplex triplex[128]; + dgInt32 stride = dgInt32 (strideInBytes / sizeof (dgFloat32)); + + dgCollisionBVHShowPolyContext& data = *(dgCollisionBVHShowPolyContext*) context; + for (dgInt32 i = 0; i < indexCount; i ++ ) { + dgVector p (&polygon[indexArray[i] * stride]); + p = p & dgVector::m_triplexMask; + p = data.m_matrix.TransformVector(p); + triplex[i].m_x = p.m_x; + triplex[i].m_y = p.m_y; + triplex[i].m_z = p.m_z; + } +// data.m_callback (data.m_userData, indexCount, &triplex[0].m_x, indexArray[-1]); + data.m_callback (data.m_userData, indexCount, &triplex[0].m_x, indexArray[indexCount]); + + return t_ContinueSearh; +} + +void dgCollisionBVH::DebugCollision (const dgMatrix& matrixPtr, dgCollision::OnDebugCollisionMeshCallback callback, void* const userData) const +{ + dgCollisionBVHShowPolyContext context; + + context.m_matrix = matrixPtr; + context.m_userData = userData;; + context.m_callback = callback; + + dgFastAABBInfo box (dgGetIdentityMatrix(), dgVector (1.0e15f)); + ForAllSectors (box, dgVector(dgFloat32 (0.0f)), dgFloat32 (1.0f), ShowDebugPolygon, &context); +} diff --git a/thirdparty/src/newton/dgPhysics/dgCollisionBVH.h b/thirdparty/src/newton/dgPhysics/dgCollisionBVH.h new file mode 100644 index 000000000..755c8e494 --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgCollisionBVH.h @@ -0,0 +1,98 @@ +/* Copyright (c) <2003-2019> +* +* 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 __DGCOLLISION_BVH__ +#define __DGCOLLISION_BVH__ + +#include "dgCollision.h" +#include "dgCollisionMesh.h" + +class dgCollisionBVH; + +typedef dgFloat32 (*dgCollisionBVHUserRayCastCallback) (const dgBody* const body, const dgCollisionBVH* const heightFieldCollision, dgFloat32 interception, dgFloat32* normal, dgInt32 faceId, void* usedData); + +class dgCollisionBVH: public dgCollisionMesh, public dgAABBPolygonSoup +{ + public: + DG_MSC_VECTOR_ALIGNMENT + class dgBVHRay: public dgFastRayTest + { + public: + dgBVHRay(const dgVector& l0, const dgVector& l1) + :dgFastRayTest (l0, l1) + { + } + + dgMatrix m_matrix; + dgVector m_normal; + dgUnsigned32 m_id; + dgFloat32 m_t; + void* m_userData; + const dgBody* m_myBody; + const dgCollisionBVH* m_me; + } DG_GCC_VECTOR_ALIGNMENT; + + dgCollisionBVH(dgWorld* const world); + dgCollisionBVH (dgWorld* const world, dgDeserialize deserialization, void* const userData, dgInt32 revisionNumber); + virtual ~dgCollisionBVH(void); + + void BeginBuild(); + void AddFace (dgInt32 vertexCount, const dgFloat32* const vertexPtr, dgInt32 strideInBytes, dgInt32 faceAttribute); + void EndBuild(dgInt32 optimize); + + void SetCollisionRayCastCallback (dgCollisionBVHUserRayCastCallback rayCastCallback); + dgCollisionBVHUserRayCastCallback GetDebugRayCastCallback() const { return m_userRayCastCallback;} + void GetVertexListIndexList (const dgVector& p0, const dgVector& p1, dgMeshVertexListIndexList &data) const; + + void ForEachFace (dgAABBIntersectCallback callback, void* const context) const; + + private: + static dgFloat32 RayHit (void* const context, const dgFloat32* const polygon, dgInt32 strideInBytes, const dgInt32* const indexArray, dgInt32 indexCount); + static dgFloat32 RayHitUser (void* const context, const dgFloat32* const polygon, dgInt32 strideInBytes, const dgInt32* const indexArray, dgInt32 indexCount); + static dgIntersectStatus GetPolygon (void* const context, const dgFloat32* const polygon, dgInt32 strideInBytes, const dgInt32* const indexArray, dgInt32 indexCount, dgFloat32 hitDistance); + static dgIntersectStatus ShowDebugPolygon (void* const context, const dgFloat32* const polygon, dgInt32 strideInBytes, const dgInt32* const indexArray, dgInt32 indexCount, dgFloat32 hitDistance); + static dgIntersectStatus GetTriangleCount (void* const context, const dgFloat32* const polygon, dgInt32 strideInBytes, const dgInt32* const indexArray, dgInt32 indexCount, dgFloat32 hitDistance); + static dgIntersectStatus CollectVertexListIndexList (void* const context, const dgFloat32* const polygon, dgInt32 strideInBytes, const dgInt32* const indexArray, dgInt32 indexCount, dgFloat32 hitDistance); + + void Serialize(dgSerialize callback, void* const userData) const; + virtual dgVector SupportVertex (const dgVector& dir) const; + + virtual dgFloat32 RayCast (const dgVector& localP0, const dgVector& localP1, dgFloat32 maxT, dgContactPoint& contactOut, const dgBody* const body, void* const userData, OnRayPrecastAction preFilter) const; + virtual void GetCollidingFaces (dgPolygonMeshDesc* const data) const; + virtual void GetCollisionInfo(dgCollisionInfo* const info) const; + + virtual void GetLocalAABB (const dgVector& p0, const dgVector& p1, dgVector& boxP0, dgVector& boxP1) const; + virtual void DebugCollision (const dgMatrix& matrixPtr, dgCollision::OnDebugCollisionMeshCallback callback, void* const userData) const; + + virtual dgVector SupportVertex (const dgVector& dir, dgInt32* const vertexIndex) const; + virtual dgVector SupportVertexSpecial (const dgVector& dir, dgFloat32 skinThickness, dgInt32* const vertexIndex) const; + virtual dgVector SupportVertexSpecialProjectPoint (const dgVector& point, const dgVector& dir) const {return point;} + + dgPolygonSoupDatabaseBuilder* m_builder; + dgCollisionBVHUserRayCastCallback m_userRayCastCallback; + + dgInt32 m_trianglesCount; + friend class dgCollisionCompound; + friend class dgCollisionDeformableMesh; +}; + + +#endif diff --git a/thirdparty/src/newton/dgPhysics/dgCollisionBox.cpp b/thirdparty/src/newton/dgPhysics/dgCollisionBox.cpp new file mode 100644 index 000000000..581140cb7 --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgCollisionBox.cpp @@ -0,0 +1,471 @@ +/* Copyright (c) <2003-2019> +* +* 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 "dgPhysicsStdafx.h" +#include "dgBody.h" +#include "dgWorld.h" +#include "dgCollisionBox.h" + + +////////////////////////////////////////////////////////////////////// +// Construction/Destruction +////////////////////////////////////////////////////////////////////// +dgInt32 dgCollisionBox::m_initSimplex = 0; +dgCollisionConvex::dgConvexSimplexEdge dgCollisionBox::m_edgeArray[24]; +dgCollisionConvex::dgConvexSimplexEdge* dgCollisionBox::m_edgeEdgeMap[12]; +dgCollisionConvex::dgConvexSimplexEdge* dgCollisionBox::m_vertexToEdgeMap[8]; +dgInt32 dgCollisionBox::m_faces[][4] = +{ + {0, 1, 3, 2}, + {0, 4, 5, 1}, + {1, 5, 7, 3}, + {0, 2, 6, 4}, + {2, 3, 7, 6}, + {4, 6, 7, 5}, +}; + +dgVector dgCollisionBox::m_indexMark (dgFloat32 (1.0f), dgFloat32 (2.0f), dgFloat32 (4.0f), dgFloat32 (0.0f)); +dgVector dgCollisionBox::m_penetrationTol (DG_PENETRATION_TOL, DG_PENETRATION_TOL, DG_PENETRATION_TOL, dgFloat32 (0.0f)); + +dgCollisionBox::dgCollisionBox(dgMemoryAllocator* allocator, dgUnsigned32 signature, dgFloat32 size_x, dgFloat32 size_y, dgFloat32 size_z) + :dgCollisionConvex(allocator, signature, m_boxCollision) +{ + Init (size_x, size_y, size_z); +} + +dgCollisionBox::dgCollisionBox(dgWorld* const world, dgDeserialize deserialization, void* const userData, dgInt32 revisionNumber) + :dgCollisionConvex (world, deserialization, userData, revisionNumber) +{ + dgVector size; + deserialization (userData, &size, sizeof (dgVector)); + Init (size.m_x, size.m_y, size.m_z); +} + +void dgCollisionBox::Init (dgFloat32 size_x, dgFloat32 size_y, dgFloat32 size_z) +{ + m_rtti |= dgCollisionBox_RTTI; + m_size[0].m_x = dgMax (dgAbs (size_x) * dgFloat32 (0.5f), D_MIN_CONVEX_SHAPE_SIZE); + m_size[0].m_y = dgMax (dgAbs (size_y) * dgFloat32 (0.5f), D_MIN_CONVEX_SHAPE_SIZE); + m_size[0].m_z = dgMax (dgAbs (size_z) * dgFloat32 (0.5f), D_MIN_CONVEX_SHAPE_SIZE); + m_size[0].m_w = dgFloat32 (0.0f); + + m_size[1].m_x = - m_size[0].m_x; + m_size[1].m_y = - m_size[0].m_y; + m_size[1].m_z = - m_size[0].m_z; + m_size[1].m_w = dgFloat32 (0.0f); + + m_edgeCount = 24; + m_vertexCount = 8; + + m_vertex[0] = dgVector ( m_size[0].m_x, m_size[0].m_y, m_size[0].m_z, dgFloat32 (0.0f)); + m_vertex[1] = dgVector (-m_size[0].m_x, m_size[0].m_y, m_size[0].m_z, dgFloat32 (0.0f)); + m_vertex[2] = dgVector ( m_size[0].m_x, -m_size[0].m_y, m_size[0].m_z, dgFloat32 (0.0f)); + m_vertex[3] = dgVector (-m_size[0].m_x, -m_size[0].m_y, m_size[0].m_z, dgFloat32 (0.0f)); + + m_vertex[4] = dgVector ( m_size[0].m_x, m_size[0].m_y, -m_size[0].m_z, dgFloat32 (0.0f)); + m_vertex[5] = dgVector (-m_size[0].m_x, m_size[0].m_y, -m_size[0].m_z, dgFloat32 (0.0f)); + m_vertex[6] = dgVector ( m_size[0].m_x, -m_size[0].m_y, -m_size[0].m_z, dgFloat32 (0.0f)); + m_vertex[7] = dgVector (-m_size[0].m_x, -m_size[0].m_y, -m_size[0].m_z, dgFloat32 (0.0f)); + + dgCollisionConvex::m_vertex = m_vertex; + dgCollisionConvex::m_simplex = m_edgeArray; + + if (!m_initSimplex) { + dgPolyhedra polyhedra (GetAllocator()); + polyhedra.BeginFace(); + for (dgInt32 i = 0; i < 6; i ++) { + polyhedra.AddFace (4, &m_faces[i][0]); + } + polyhedra.EndFace(); + + int index = 0; + dgInt32 mark = polyhedra.IncLRU();; + dgPolyhedra::Iterator iter (polyhedra); + for (iter.Begin(); iter; iter ++) { + dgEdge* const edge = &iter.GetNode()->GetInfo(); + if (edge->m_mark != mark) { + dgEdge* ptr = edge; + do { + ptr->m_mark = mark; + ptr->m_userData = index; + index ++; + ptr = ptr->m_twin->m_next; + } while (ptr != edge) ; + } + } + dgAssert (index == 24); + + polyhedra.IncLRU(); + mark = polyhedra.IncLRU(); + for (iter.Begin(); iter; iter ++) { + dgEdge* const edge = &iter.GetNode()->GetInfo(); + dgEdge *ptr = edge; + do { + ptr->m_mark = mark; + dgConvexSimplexEdge* const simplexPtr = &m_simplex[ptr->m_userData]; + simplexPtr->m_vertex = ptr->m_incidentVertex; + simplexPtr->m_next = &m_simplex[ptr->m_next->m_userData]; + simplexPtr->m_prev = &m_simplex[ptr->m_prev->m_userData]; + simplexPtr->m_twin = &m_simplex[ptr->m_twin->m_userData]; + ptr = ptr->m_twin->m_next; + } while (ptr != edge) ; + } + + for (iter.Begin(); iter; iter ++) { + dgEdge* const edge = &iter.GetNode()->GetInfo(); + m_vertexToEdgeMap[edge->m_incidentVertex] = &m_simplex[edge->m_userData]; + } + + dgInt32 count = 0; + mark = polyhedra.IncLRU(); + for (iter.Begin(); iter; iter ++) { + dgEdge* const edge = &iter.GetNode()->GetInfo(); + if (edge->m_mark != mark) { + edge->m_mark = mark; + edge->m_twin->m_mark = mark; + m_edgeEdgeMap[count] = &m_simplex[edge->m_userData]; + count ++; + dgAssert (count <= 12); + } + } + + m_initSimplex = 1; + } + + SetVolumeAndCG (); +} + +dgCollisionBox::~dgCollisionBox() +{ + dgCollisionConvex::m_simplex = NULL; + dgCollisionConvex::m_vertex = NULL; +} + + +void dgCollisionBox::SetCollisionBBox (const dgVector& p0__, const dgVector& p1__) +{ + dgAssert (0); +} + +dgInt32 dgCollisionBox::CalculateSignature (dgFloat32 dx, dgFloat32 dy, dgFloat32 dz) +{ + dgUnsigned32 buffer[4]; + + dx = dgAbs (dx); + dy = dgAbs (dy); + dz = dgAbs (dz); + buffer[0] = m_boxCollision; + buffer[1] = Quantize (dx * dgFloat32 (0.5f)); + buffer[2] = Quantize (dy * dgFloat32 (0.5f)); + buffer[3] = Quantize (dz * dgFloat32 (0.5f)); + return Quantize(buffer, sizeof (buffer)); +} + +dgInt32 dgCollisionBox::CalculateSignature () const +{ + return CalculateSignature(m_size[0].m_x, m_size[0].m_y, m_size[0].m_z); +} + +dgVector dgCollisionBox::SupportVertex (const dgVector& dir0, dgInt32* const vertexIndex) const +{ + dgVector mask0(dir0.Abs() > m_flushZero); + dgVector dir(dir0 & mask0); + + dgAssert (dgAbs(dir.DotProduct(dir).GetScalar() - dgFloat32 (1.0f)) < dgFloat32 (1.0e-3f)); + dgAssert (dir.m_w == dgFloat32 (0.0f)); + dgVector mask(dir < dgVector::m_zero); + if (vertexIndex) { + dgVector index (m_indexMark * (mask & dgVector::m_one)); + index = (index.AddHorizontal()).GetInt(); + *vertexIndex = dgInt32 (index.m_ix); + } + return m_size[0].Select(m_size[1], mask); +} + +dgVector dgCollisionBox::SupportVertexSpecial(const dgVector& dir0, dgFloat32 skinThickness, dgInt32* const vertexIndex) const +{ + dgVector mask0(dir0.Abs() > m_flushZero); + dgVector dir(dir0 & mask0); + + dgAssert(dgAbs(dir.DotProduct(dir).GetScalar() - dgFloat32(1.0f)) < dgFloat32(1.0e-3f)); + dgAssert(dir.m_w == dgFloat32(0.0f)); +// dgVector mask(dir < dgVector(dgFloat32(0.0f))); + dgVector mask(dir < dgVector::m_zero); + if (vertexIndex) { + dgVector index(m_indexMark * (mask & dgVector::m_one)); + index = (index.AddHorizontal()).GetInt(); + *vertexIndex = dgInt32 (index.m_ix); + } + + //dgVector padd (DG_PENETRATION_TOL); + //padd = padd & dgVector::m_triplexMask; + dgVector size0 (m_size[0] - m_penetrationTol); + dgVector size1 (m_size[1] + m_penetrationTol); + return size0.Select(size1, mask); +} + +dgVector dgCollisionBox::SupportVertexSpecialProjectPoint(const dgVector& point, const dgVector& dir0) const +{ + dgVector mask0(dir0.Abs() > m_flushZero); + dgVector dir(dir0 & mask0); + dgAssert(dgAbs((dir.DotProduct(dir).GetScalar() - dgFloat32(1.0f))) < dgFloat32(1.0e-3f)); + return point + dir.Scale (DG_PENETRATION_TOL); +} + +void dgCollisionBox::CalcAABB (const dgMatrix& matrix, dgVector &p0, dgVector &p1) const +{ + dgVector size (matrix[0].Abs().Scale(m_size[0].m_x) + matrix[1].Abs().Scale(m_size[0].m_y) + matrix[2].Abs().Scale(m_size[0].m_z)); + p0 = (matrix[3] - size) & dgVector::m_triplexMask; + p1 = (matrix[3] + size) & dgVector::m_triplexMask; +} + + +dgFloat32 dgCollisionBox::RayCast (const dgVector& localP0, const dgVector& localP1, dgFloat32 maxT, dgContactPoint& contactOut, const dgBody* const body, void* const userData, OnRayPrecastAction preFilter) const +{ + dgAssert (localP0.m_w == dgFloat32 (0.0f)); + dgAssert (localP1.m_w == dgFloat32 (0.0f)); + + dgInt32 index = 0; + dgFloat32 signDir = dgFloat32 (0.0f); + dgFloat32 tmin = dgFloat32 (0.0f); + dgFloat32 tmax = dgFloat32 (1.0f); + for (dgInt32 i = 0; i < 3; i++) { + dgFloat32 dp = localP1[i] - localP0[i]; + if (dgAbs (dp) < dgFloat32 (1.0e-8f)) { + if (localP0[i] <= m_size[1][i] || localP0[i] >= m_size[0][i]) { + return dgFloat32 (1.2f); + } + } else { + dp = dgFloat32 (1.0f) / dp; + dgFloat32 t1 = (m_size[1][i] - localP0[i]) * dp; + dgFloat32 t2 = (m_size[0][i] - localP0[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); + contactOut.m_normal = dgVector (dgFloat32 (0.0f)); + contactOut.m_normal[index] = signDir; + //contactOut.m_userId = SetUserDataID(); + } else { + tmin = dgFloat32 (1.2f); + } + return tmin; +} + +void dgCollisionBox::MassProperties () +{ + m_centerOfMass = dgVector::m_zero; + m_crossInertia = dgVector::m_zero; + dgFloat32 volume = dgFloat32 (8.0f) * m_size[0].m_x * m_size[0].m_y * m_size[0].m_z; + m_inertia = dgVector (dgFloat32 (1.0f / 3.0f) * (m_size[0].m_y * m_size[0].m_y + m_size[0].m_z * m_size[0].m_z), + dgFloat32 (1.0f / 3.0f) * (m_size[0].m_x * m_size[0].m_x + m_size[0].m_z * m_size[0].m_z), + dgFloat32 (1.0f / 3.0f) * (m_size[0].m_x * m_size[0].m_x + m_size[0].m_y * m_size[0].m_y), + dgFloat32 (0.0f)); + m_centerOfMass.m_w = volume; +} + +void dgCollisionBox::GetCollisionInfo(dgCollisionInfo* const info) const +{ + dgCollisionConvex::GetCollisionInfo(info); + + info->m_box.m_x = m_size[0].m_x * dgFloat32 (2.0f); + info->m_box.m_y = m_size[0].m_y * dgFloat32 (2.0f); + info->m_box.m_z = m_size[0].m_z * dgFloat32 (2.0f); +} + +void dgCollisionBox::Serialize(dgSerialize callback, void* const userData) const +{ + SerializeLow(callback, userData); + dgVector size (m_size[0].Scale (dgFloat32 (2.0f))); + callback (userData, &size, sizeof (dgVector)); +} + +const dgCollisionConvex::dgConvexSimplexEdge** dgCollisionBox::GetVertexToEdgeMapping() const +{ + return (const dgConvexSimplexEdge**)&m_vertexToEdgeMap[0]; +} + + + +dgInt32 dgCollisionBox::CalculatePlaneIntersection (const dgVector& normal, const dgVector& point, dgVector* const contactsOut) const +{ + dgVector support[4]; + dgInt32 featureCount = 3; + + const dgConvexSimplexEdge** const vertToEdgeMapping = GetVertexToEdgeMapping(); + if (vertToEdgeMapping) { + dgInt32 edgeIndex; + support[0] = SupportVertex (normal, &edgeIndex); + + dgFloat32 dist = normal.DotProduct(support[0] - point).GetScalar(); + if (dist <= DG_PENETRATION_TOL) { + dgVector normalAlgin (normal.Abs()); + if (!((normalAlgin.m_x > dgFloat32 (0.9999f)) || (normalAlgin.m_y > dgFloat32 (0.9999f)) || (normalAlgin.m_z > dgFloat32 (0.9999f)))) { + // 0.25 degrees + const dgFloat32 tiltAngle = dgFloat32 (0.005f); + const dgFloat32 tiltAngle2 = tiltAngle * tiltAngle ; + dgPlane testPlane (normal, - (normal.DotProduct(support[0]).GetScalar())); + + featureCount = 1; + const dgConvexSimplexEdge* const edge = vertToEdgeMapping[edgeIndex]; + const dgConvexSimplexEdge* ptr = edge; + do { + const dgVector& p = m_vertex[ptr->m_twin->m_vertex]; + dgFloat32 test1 = testPlane.Evalue(p); + dgVector dist1 (p - support[0]); + dgFloat32 angle2 = test1 * test1 / (dist1.DotProduct(dist1).GetScalar()); + if (angle2 < tiltAngle2) { + support[featureCount] = p; + featureCount ++; + } + ptr = ptr->m_twin->m_next; + } while ((ptr != edge) && (featureCount < 3)); + } + } + } + + dgInt32 count = 0; + switch (featureCount) + { + case 1: + { + contactsOut[0] = support[0] - normal * normal.DotProduct(support[0] - point); + count = 1; + break; + } + + case 2: + { + contactsOut[0] = support[0] - normal * normal.DotProduct(support[0] - point); + contactsOut[1] = support[1] - normal * normal.DotProduct(support[1] - point); + count = 2; + break; + } + + default: + { + dgFloat32 test[8]; + dgAssert(normal.m_w == dgFloat32(0.0f)); + dgPlane plane(normal, -(normal.DotProduct(point).GetScalar())); + for (dgInt32 i = 0; i < 8; i++) { + dgAssert(m_vertex[i].m_w == dgFloat32(0.0f)); + test[i] = plane.DotProduct(m_vertex[i] | dgVector::m_wOne).m_x; + } + + dgConvexSimplexEdge* edge = NULL; + for (dgInt32 i = 0; i < dgInt32 (sizeof (m_edgeEdgeMap) / sizeof (m_edgeEdgeMap[0])); i ++) { + dgConvexSimplexEdge* const ptr = m_edgeEdgeMap[i]; + dgFloat32 side0 = test[ptr->m_vertex]; + dgFloat32 side1 = test[ptr->m_twin->m_vertex]; + if ((side0 * side1) < dgFloat32 (0.0f)) { + edge = ptr; + break; + } + } + + if (edge) { + if (test[edge->m_vertex] < dgFloat32 (0.0f)) { + edge = edge->m_twin; + } + dgAssert (test[edge->m_vertex] > dgFloat32 (0.0f)); + + dgConvexSimplexEdge* ptr = edge; + dgConvexSimplexEdge* firstEdge = NULL; + dgFloat32 side0 = test[edge->m_vertex]; + do { + dgAssert (m_vertex[ptr->m_twin->m_vertex].m_w == dgFloat32 (0.0f)); + dgFloat32 side1 = test[ptr->m_twin->m_vertex]; + if (side1 < side0) { + if (side1 < dgFloat32 (0.0f)) { + firstEdge = ptr; + break; + } + + side0 = side1; + edge = ptr->m_twin; + ptr = edge; + } + ptr = ptr->m_twin->m_next; + } while (ptr != edge); + + if (firstEdge) { + edge = firstEdge; + ptr = edge; + do { + dgVector dp (m_vertex[ptr->m_twin->m_vertex] - m_vertex[ptr->m_vertex]); + dgFloat32 t = plane.DotProduct(dp).m_x; + if (t >= dgFloat32 (-1.e-24f)) { + t = dgFloat32 (0.0f); + } else { + t = test[ptr->m_vertex] / t; + if (t > dgFloat32 (0.0f)) { + t = dgFloat32 (0.0f); + } + if (t < dgFloat32 (-1.0f)) { + t = dgFloat32 (-1.0f); + } + } + + dgAssert (t <= dgFloat32 (0.01f)); + dgAssert (t >= dgFloat32 (-1.05f)); + contactsOut[count] = m_vertex[ptr->m_vertex] - dp.Scale (t); + count ++; + + dgConvexSimplexEdge* ptr1 = ptr->m_next; + for (; ptr1 != ptr; ptr1 = ptr1->m_next) { + dgInt32 index0 = ptr1->m_twin->m_vertex; + if (test[index0] >= dgFloat32 (0.0f)) { + dgAssert (test[ptr1->m_vertex] <= dgFloat32 (0.0f)); + break; + } + } + dgAssert (ptr != ptr1); + ptr = ptr1->m_twin; + + } while ((ptr != edge) && (count < 8)); + } + } + } + } + + if (count > 2) { + count = RectifyConvexSlice (count, normal, contactsOut); + } + return count; +} diff --git a/thirdparty/src/newton/dgPhysics/dgCollisionBox.h b/thirdparty/src/newton/dgPhysics/dgCollisionBox.h new file mode 100644 index 000000000..0520d798b --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgCollisionBox.h @@ -0,0 +1,69 @@ +/* Copyright (c) <2003-2019> +* +* 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_BOX_H_ +#define _DG_BOX_H_ + + +#include "dgCollisionConvex.h" + + +class dgCollisionBox: public dgCollisionConvex +{ + public: + + dgCollisionBox(dgMemoryAllocator* const allocator, dgUnsigned32 signature, dgFloat32 size_x, dgFloat32 size_y, dgFloat32 size_z); + dgCollisionBox(dgWorld* const world, dgDeserialize deserialization, void* const userData, dgInt32 revisionNumber); + virtual ~dgCollisionBox(); + + protected: + void Init (dgFloat32 size_x, dgFloat32 size_y, dgFloat32 size_z); + virtual void CalcAABB (const dgMatrix& matrix, dgVector& p0, dgVector& p1) const; + virtual dgFloat32 RayCast (const dgVector& localP0, const dgVector& localP1, dgFloat32 maxT, dgContactPoint& contactOut, const dgBody* const body, void* const userData, OnRayPrecastAction preFilter) const; + virtual dgVector SupportVertex (const dgVector& dir, dgInt32* const vertexIndex) const; + virtual dgVector SupportVertexSpecial(const dgVector& dir, dgFloat32 skinThickness, dgInt32* const vertexIndex) const; + virtual dgVector SupportVertexSpecialProjectPoint(const dgVector& point, const dgVector& dir) const; + virtual dgInt32 CalculatePlaneIntersection (const dgVector& normal, const dgVector& point, dgVector* const contactsOut) const; + virtual const dgConvexSimplexEdge** GetVertexToEdgeMapping() const; + + virtual dgInt32 CalculateSignature () const; + virtual void SetCollisionBBox (const dgVector& p0, const dgVector& p1); + virtual void MassProperties (); + + virtual void GetCollisionInfo(dgCollisionInfo* const info) const; + virtual void Serialize(dgSerialize callback, void* const userData) const; + + static dgInt32 CalculateSignature (dgFloat32 dx, dgFloat32 dy, dgFloat32 dz); + + dgVector m_size[2]; + dgVector m_vertex[8]; + static dgInt32 m_initSimplex; + static dgInt32 m_faces[][4]; + static dgVector m_indexMark; + static dgVector m_penetrationTol; + static dgConvexSimplexEdge m_edgeArray[]; + static dgConvexSimplexEdge* m_edgeEdgeMap[]; + static dgConvexSimplexEdge* m_vertexToEdgeMap[]; + friend class dgWorld; +}; + +#endif + diff --git a/thirdparty/src/newton/dgPhysics/dgCollisionCapsule.cpp b/thirdparty/src/newton/dgPhysics/dgCollisionCapsule.cpp new file mode 100644 index 000000000..24e591acf --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgCollisionCapsule.cpp @@ -0,0 +1,532 @@ +/* Copyright (c) <2003-2019> +* +* 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 "dgPhysicsStdafx.h" +#include "dgBody.h" +#include "dgWorld.h" +#include "dgContact.h" +#include "dgCollisionCapsule.h" + +#define DG_CAPSULE_SEGMENTS 10 +#define DG_CAPSULE_CAP_SEGMENTS 12 + +////////////////////////////////////////////////////////////////////// +// Construction/Destruction +////////////////////////////////////////////////////////////////////// +dgCollisionCapsule::dgCollisionCapsule(dgMemoryAllocator* allocator, dgUnsigned32 signature, dgFloat32 radio0, dgFloat32 radio1, dgFloat32 height) + :dgCollisionConvex(allocator, signature, m_capsuleCollision) +{ + Init (radio0, radio1, height); +} + +dgCollisionCapsule::dgCollisionCapsule(dgWorld* const world, dgDeserialize deserialization, void* const userData, dgInt32 revisionNumber) + :dgCollisionConvex (world, deserialization, userData, revisionNumber) +{ + dgVector size; + deserialization(userData, &size, sizeof (dgVector)); + Init (size.m_x, size.m_y, size.m_z * dgFloat32 (2.0f)); +} + + +dgCollisionCapsule::~dgCollisionCapsule() +{ +} + +void dgCollisionCapsule::Init (dgFloat32 radio0, dgFloat32 radio1, dgFloat32 height) +{ + m_rtti |= dgCollisionCapsule_RTTI; + + radio0 = dgMax (dgAbs (radio0), D_MIN_CONVEX_SHAPE_SIZE); + radio1 = dgMax (dgAbs (radio1), D_MIN_CONVEX_SHAPE_SIZE); + height = dgMax (dgAbs (height), D_MIN_CONVEX_SHAPE_SIZE); + + m_transform = dgVector (dgFloat32 (1.0f), dgFloat32 (1.0f), dgFloat32 (1.0f), dgFloat32 (0.0f)); + if (radio0 > radio1) { + m_transform.m_x = dgFloat32 (-1.0f); + m_transform.m_y = dgFloat32 (-1.0f); + dgSwap(radio0, radio1); + } + + m_radio0 = radio0; + m_radio1 = radio1; + m_height = height * dgFloat32 (0.5f); + + m_p0 = dgVector (- m_height, m_radio0, dgFloat32 (0.0f), dgFloat32 (0.0f)); + m_p1 = dgVector ( m_height, m_radio1, dgFloat32 (0.0f), dgFloat32 (0.0f)); + m_normal = dgVector (dgFloat32 (0.0f), dgFloat32 (1.0f), dgFloat32 (0.0f), dgFloat32 (0.0f)); + dgVector side (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (1.0f), dgFloat32 (0.0f)); + + for (int i = 0; i < 16; i ++) { + dgVector p1p0 (m_p1 - m_p0); + //dgVector dir(side.CrossProduct(p1p0)); + m_normal = side.CrossProduct(p1p0).Normalize(); + dgVector support0(m_normal.Scale(m_radio0)); + dgVector support1(m_normal.Scale(m_radio1)); + support0.m_x -= m_height; + support1.m_x += m_height; + dgFloat32 distance0 = support0.DotProduct(m_normal).GetScalar(); + dgFloat32 distance1 = support1.DotProduct(m_normal).GetScalar(); + + if (distance1 > distance0) { + m_p1 = support1; + } else if (distance1 < distance0) { + m_p0 = support0; + } else { + i = 1000; + } + } + + dgVector tempVertex[4 * DG_CAPSULE_CAP_SEGMENTS * DG_CAPSULE_SEGMENTS + 100]; + dgInt32 index = 0; + dgInt32 dx0 = dgInt32(dgFloor(DG_CAPSULE_SEGMENTS * ((m_p0.m_x + m_height + m_radio0) / m_radio0)) + dgFloat32(1.0f)); + dgFloat32 step = m_radio0 / DG_CAPSULE_SEGMENTS; + dgFloat32 x0 = m_p0.m_x - step * dx0; + for (dgInt32 j = 0; j < dx0; j++) { + x0 += step; + dgFloat32 x = x0 + m_height; + dgFloat32 arg = dgMax (m_radio0 * m_radio0 - x * x, dgFloat32 (1.0e-3f)); + dgFloat32 r0 = dgSqrt (arg); + + dgFloat32 angle = dgFloat32(0.0f); + for (dgInt32 i = 0; i < DG_CAPSULE_CAP_SEGMENTS; i++) { + dgFloat32 z = dgSin(angle); + dgFloat32 y = dgCos(angle); + tempVertex[index] = dgVector(x0, y * r0, z * r0, dgFloat32(0.0f)); + index++; + angle += dgPI2 / DG_CAPSULE_CAP_SEGMENTS; + dgAssert(index < sizeof (tempVertex) / sizeof (tempVertex[0])); + } + } + + dgFloat32 x1 = m_p1.m_x; + dgInt32 dx1 = dgInt32 (dgFloor (DG_CAPSULE_SEGMENTS * ((m_height + m_radio1 - m_p1.m_x) / m_radio1)) + dgFloat32 (1.0f)); + step = m_radio1 / DG_CAPSULE_SEGMENTS; + for (dgInt32 j = 0; j < dx1; j ++) { + dgFloat32 x = x1 - m_height; + dgFloat32 arg = dgMax (m_radio1 * m_radio1 - x * x, dgFloat32 (1.0e-3f)); + dgFloat32 r1 = dgSqrt (arg); + dgFloat32 angle = dgFloat32 (0.0f); + for (dgInt32 i = 0; i < DG_CAPSULE_CAP_SEGMENTS; i ++) { + dgFloat32 z = dgSin (angle); + dgFloat32 y = dgCos (angle); + tempVertex[index] = dgVector ( x1, y * r1, z * r1, dgFloat32 (0.0f)); + index ++; + angle += dgPI2 / DG_CAPSULE_CAP_SEGMENTS; + dgAssert (index < sizeof (tempVertex) / sizeof (tempVertex[0])); + } + x1 += step; + } + + m_vertexCount = dgInt16 (index); + dgCollisionConvex::m_vertex = (dgVector*) m_allocator->Malloc (dgInt32 (m_vertexCount * sizeof (dgVector))); + memcpy (dgCollisionConvex::m_vertex, tempVertex, m_vertexCount * sizeof (dgVector)); + + dgPolyhedra polyhedra(m_allocator); + polyhedra.BeginFace (); + + dgInt32 wireframe[DG_CAPSULE_SEGMENTS + 10]; + + dgInt32 i1 = 0; + dgInt32 i0 = DG_CAPSULE_CAP_SEGMENTS - 1; + const dgInt32 n = index / DG_CAPSULE_CAP_SEGMENTS - 1; + for (dgInt32 j = 0; j < n; j ++) { + for (dgInt32 i = 0; i < DG_CAPSULE_CAP_SEGMENTS; i ++) { + wireframe[0] = i0; + wireframe[1] = i1; + wireframe[2] = i1 + DG_CAPSULE_CAP_SEGMENTS; + wireframe[3] = i0 + DG_CAPSULE_CAP_SEGMENTS; + i0 = i1; + i1 ++; + polyhedra.AddFace (4, wireframe); + } + i0 = i1 + DG_CAPSULE_CAP_SEGMENTS - 1; + } + + for (dgInt32 i = 0; i < DG_CAPSULE_CAP_SEGMENTS; i ++) { + wireframe[i] = DG_CAPSULE_CAP_SEGMENTS - i - 1; + } + polyhedra.AddFace (DG_CAPSULE_CAP_SEGMENTS, wireframe); + + for (dgInt32 i = 0; i < DG_CAPSULE_CAP_SEGMENTS; i ++) { + wireframe[i] = index - DG_CAPSULE_CAP_SEGMENTS + i; + } + polyhedra.AddFace (DG_CAPSULE_CAP_SEGMENTS, wireframe); + polyhedra.EndFace (); + + dgAssert (SanityCheck (polyhedra)); + + m_edgeCount = dgInt16 (polyhedra.GetEdgeCount()); + m_simplex = (dgConvexSimplexEdge*) m_allocator->Malloc (dgInt32 (m_edgeCount * sizeof (dgConvexSimplexEdge))); + + dgUnsigned64 i = 0; + dgPolyhedra::Iterator iter (polyhedra); + for (iter.Begin(); iter; iter ++) { + dgEdge* const edge = &(*iter); + edge->m_userData = i; + i ++; + } + + for (iter.Begin(); iter; iter ++) { + dgEdge* const edge = &(*iter); + + dgConvexSimplexEdge* const ptr = &m_simplex[edge->m_userData]; + + ptr->m_vertex = edge->m_incidentVertex; + ptr->m_next = &m_simplex[edge->m_next->m_userData]; + ptr->m_prev = &m_simplex[edge->m_prev->m_userData]; + ptr->m_twin = &m_simplex[edge->m_twin->m_userData]; + } + SetVolumeAndCG (); +} + + +dgInt32 dgCollisionCapsule::CalculateSignature (dgFloat32 radio0, dgFloat32 radio1, dgFloat32 height) +{ + dgUnsigned32 buffer[4]; + + buffer[0] = m_capsuleCollision; + buffer[1] = Quantize (radio0); + buffer[2] = Quantize (radio1); + buffer[3] = Quantize (height); + return Quantize(buffer, sizeof (buffer)); +} + +dgInt32 dgCollisionCapsule::CalculateSignature () const +{ + return CalculateSignature (m_radio0, m_radio1, m_height); +} + + +void dgCollisionCapsule::TesselateTriangle(dgInt32 level, const dgVector& p0, const dgVector& p1, const dgVector& p2, dgInt32& count, dgVector* ouput) const +{ + if (level) { + dgAssert(dgAbs(p0.DotProduct(p0).GetScalar() - dgFloat32(1.0f)) < dgFloat32(1.0e-4f)); + dgAssert(dgAbs(p1.DotProduct(p1).GetScalar() - dgFloat32(1.0f)) < dgFloat32(1.0e-4f)); + dgAssert(dgAbs(p2.DotProduct(p2).GetScalar() - dgFloat32(1.0f)) < dgFloat32(1.0e-4f)); + dgVector p01(p0 + p1); + dgVector p12(p1 + p2); + dgVector p20(p2 + p0); + + p01 = p01.Scale(dgRsqrt(p01.DotProduct(p01).GetScalar())); + p12 = p12.Scale(dgRsqrt(p12.DotProduct(p12).GetScalar())); + p20 = p20.Scale(dgRsqrt(p20.DotProduct(p20).GetScalar())); + + dgAssert(dgAbs(p01.DotProduct(p01).GetScalar() - dgFloat32(1.0f)) < dgFloat32(1.0e-4f)); + dgAssert(dgAbs(p12.DotProduct(p12).GetScalar() - dgFloat32(1.0f)) < dgFloat32(1.0e-4f)); + dgAssert(dgAbs(p20.DotProduct(p20).GetScalar() - dgFloat32(1.0f)) < dgFloat32(1.0e-4f)); + + TesselateTriangle(level - 1, p0, p01, p20, count, ouput); + TesselateTriangle(level - 1, p1, p12, p01, count, ouput); + TesselateTriangle(level - 1, p2, p20, p12, count, ouput); + TesselateTriangle(level - 1, p01, p12, p20, count, ouput); + + } else { + ouput[count + 0] = p0.Scale(m_radio0); + ouput[count + 1] = p1.Scale(m_radio0); + ouput[count + 2] = p2.Scale(m_radio0); + count += 3; + } +} + + +void dgCollisionCapsule::DebugCollision (const dgMatrix& matrix, dgCollision::OnDebugCollisionMeshCallback callback, void* const userData) const +{ + if (m_radio0 == m_radio1) { + #define POWER 2 + dgVector tmpVectex[512]; + + dgVector p0(dgFloat32(1.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f)); + dgVector p1(-dgFloat32(1.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f)); + dgVector p2(dgFloat32(0.0f), dgFloat32(1.0f), dgFloat32(0.0f), dgFloat32(0.0f)); + dgVector p3(dgFloat32(0.0f), -dgFloat32(1.0f), dgFloat32(0.0f), dgFloat32(0.0f)); + dgVector p4(dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(1.0f), dgFloat32(0.0f)); + dgVector p5(dgFloat32(0.0f), dgFloat32(0.0f), -dgFloat32(1.0f), dgFloat32(0.0f)); + + dgInt32 count = 0; + TesselateTriangle(POWER, p0, p2, p4, count, tmpVectex); + TesselateTriangle(POWER, p0, p4, p3, count, tmpVectex); + TesselateTriangle(POWER, p0, p3, p5, count, tmpVectex); + TesselateTriangle(POWER, p0, p5, p2, count, tmpVectex); + + TesselateTriangle(POWER, p1, p4, p2, count, tmpVectex); + TesselateTriangle(POWER, p1, p3, p4, count, tmpVectex); + TesselateTriangle(POWER, p1, p5, p3, count, tmpVectex); + TesselateTriangle(POWER, p1, p2, p5, count, tmpVectex); + + for (dgInt32 i = 0; i < count; i += 3) { + int positive = 0; + for (int j = 0; j < 3; j++) { + if (tmpVectex[i + j].m_x > dgFloat32(0.0f)) { + positive++; + } + } + + if (positive) { + dgVector face[4]; + face[0] = tmpVectex[i + 0]; + face[1] = tmpVectex[i + 1]; + face[2] = tmpVectex[i + 2]; + face[0].m_x += m_height; + face[1].m_x += m_height; + face[2].m_x += m_height; + matrix.TransformTriplex(&face[0].m_x, sizeof (dgTriplex), &face[0].m_x, sizeof (dgVector), 3); + callback(userData, 3, &face[0].m_x, 0); + } else { + dgVector face[4]; + face[0] = tmpVectex[i + 0]; + face[1] = tmpVectex[i + 1]; + face[2] = tmpVectex[i + 2]; + face[0].m_x -= m_height; + face[1].m_x -= m_height; + face[2].m_x -= m_height; + matrix.TransformTriplex(&face[0].m_x, sizeof (dgTriplex), &face[0].m_x, sizeof (dgVector), 3); + callback(userData, 3, &face[0].m_x, 0); + } + if (positive == 1) { + dgVector q0(tmpVectex[i + 0]); + dgVector q1(tmpVectex[i + 1]); + if ((tmpVectex[i + 1].m_x == dgFloat32(0.0f)) && (tmpVectex[i + 2].m_x == dgFloat32(0.0f))) { + q0 = tmpVectex[i + 1]; + q1 = tmpVectex[i + 2]; + } + else if ((tmpVectex[i + 2].m_x == dgFloat32(0.0f)) && (tmpVectex[i + 0].m_x == dgFloat32(0.0f))) { + q0 = tmpVectex[i + 2]; + q1 = tmpVectex[i + 0]; + } + + dgVector face[4]; + face[0] = q1; + face[1] = q0; + face[2] = q0; + face[3] = q1; + face[0].m_x += m_height; + face[1].m_x += m_height; + face[2].m_x -= m_height; + face[3].m_x -= m_height; + matrix.TransformTriplex(&face[0].m_x, sizeof (dgTriplex), &face[0].m_x, sizeof (dgVector), 4); + callback(userData, 4, &face[0].m_x, 0); + } + } + } else { + dgMatrix transform (matrix); + transform[0] = transform[0].Scale(m_transform.m_x); + transform[1] = transform[1].Scale(m_transform.m_y); + transform[2] = transform[2].Scale(m_transform.m_z); + dgCollisionConvex::DebugCollision (transform, callback, userData); + } +} + + +void dgCollisionCapsule::SetCollisionBBox (const dgVector& p0__, const dgVector& p1__) +{ + dgAssert (0); +} + + +dgVector dgCollisionCapsule::SupportVertex (const dgVector& direction, dgInt32* const vertexIndex) const +{ + dgVector dir (direction * m_transform); + dgAssert (dir.m_w == dgFloat32 (0.0f)); + dgAssert (dgAbs(dir.DotProduct(dir).GetScalar() - dgFloat32 (1.0f)) < dgFloat32 (1.0e-3f)); + + dgVector p0(dir.Scale (m_radio0)); + dgVector p1(dir.Scale (m_radio1)); + p0.m_x -= m_height; + p1.m_x += m_height; + dgFloat32 dir0 = p0.DotProduct(dir).GetScalar(); + dgFloat32 dir1 = p1.DotProduct(dir).GetScalar(); + if (dir1 > dir0) { + p0 = p1; + } + return p0 * m_transform; +} + +dgVector dgCollisionCapsule::SupportVertexSpecial(const dgVector& direction, dgFloat32 skinThickness, dgInt32* const vertexIndex) const +{ + dgVector dir(direction * m_transform); + dgAssert (dir.m_w == dgFloat32 (0.0f)); + dgAssert(dgAbs(dir.DotProduct(dir).GetScalar() - dgFloat32(1.0f)) < dgFloat32(1.0e-3f)); + + dgVector p0(dgVector::m_zero); + dgVector p1(dir.Scale(m_radio1 - m_radio0)); + p0.m_x -= m_height; + p1.m_x += m_height; + dgFloat32 dir0 = p0.DotProduct(dir).GetScalar(); + dgFloat32 dir1 = p1.DotProduct(dir).GetScalar(); + if (dir1 > dir0) { + p0 = p1; + } + return p0 * m_transform; +} + + +dgVector dgCollisionCapsule::SupportVertexSpecialProjectPoint (const dgVector& testPoint, const dgVector& direction) const +{ + dgVector dir(direction * m_transform); + dgVector point(testPoint * m_transform); + point += dir.Scale(m_radio0 - DG_PENETRATION_TOL); + return m_transform * point; +} + + +dgFloat32 dgCollisionCapsule::CalculateMassProperties (const dgMatrix& offset, dgVector& inertia, dgVector& crossInertia, dgVector& centerOfMass) const +{ + return dgCollisionConvex::CalculateMassProperties (offset, inertia, crossInertia, centerOfMass); +} + +void dgCollisionCapsule::GetCollisionInfo(dgCollisionInfo* const info) const +{ + dgCollisionConvex::GetCollisionInfo(info); + + info->m_capsule.m_radio0 = m_radio0; + info->m_capsule.m_radio1 = m_radio1; + info->m_capsule.m_height = dgFloat32 (2.0f) * m_height; + + if (m_transform.m_x < dgFloat32 (0.0f)) { + dgSwap(info->m_capsule.m_radio0, info->m_capsule.m_radio1); + } +} + +void dgCollisionCapsule::Serialize(dgSerialize callback, void* const userData) const +{ + SerializeLow(callback, userData); + dgVector size(m_radio0, m_radio1, m_height, dgFloat32 (0.0f)); + callback (userData, &size, sizeof (dgVector)); +} + + +dgInt32 dgCollisionCapsule::CalculatePlaneIntersection (const dgVector& direction, const dgVector& point, dgVector* const contactsOut) const +{ + dgVector normal(direction * m_transform); + dgVector origin(point * m_transform); + + dgInt32 count = 0; + dgVector p0 (-m_height, dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f)); + dgVector dir0 (p0 - origin); + dgFloat32 dist0 = dir0.DotProduct(normal).GetScalar(); + if ((dist0 * dist0 - dgFloat32 (5.0e-5f)) < (m_radio0 * m_radio0)) { + contactsOut[count] = m_transform * (p0 - normal.Scale (dist0)); + count ++; + } + + dgVector p1 (m_height, dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f)); + dgVector dir1 (p1 - origin); + dgFloat32 dist1 = dir1.DotProduct(normal).GetScalar(); + if ((dist1 * dist1 - dgFloat32 (5.0e-5f)) < (m_radio1 * m_radio1)) { + contactsOut[count] = m_transform * (p1 - normal.Scale (dist1)); + count ++; + } + return count; +} + +dgFloat32 dgCollisionCapsule::RayCast (const dgVector& r0, const dgVector& r1, dgFloat32 maxT, dgContactPoint& contactOut, const dgBody* const body, void* const userData, OnRayPrecastAction preFilter) const +{ + dgVector q0(r0 * m_transform); + dgVector q1(r1 * m_transform); + + dgVector origin0 (-m_height, dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f)); + dgVector origin1 ( m_height, dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f)); + dgFloat32 t0 = dgRayCastSphere (q0, q1, origin0, m_radio0); + dgFloat32 t1 = dgRayCastSphere (q0, q1, origin1, m_radio1); + if ((t0 < maxT) && (t1 < maxT)) { + if (t0 < t1) { + dgVector q (q0 + (q1 - q0).Scale (t0)); + dgVector n(q - origin0); + dgAssert(n.m_w == dgFloat32(0.0f)); + //contactOut.m_normal = m_transform * n * n.DotProduct(n).InvSqrt(); + contactOut.m_normal = m_transform * n.Normalize(); + return t0; + } else { + dgVector q (q0 + (q1 - q0).Scale (t1)); + dgVector n(q - origin1); + dgAssert(n.m_w == dgFloat32(0.0f)); + //contactOut.m_normal = m_transform * n * n.DotProduct(n).InvSqrt(); + contactOut.m_normal = m_transform * n.Normalize(); + return t1; + } + } else if (t1 < maxT) { + dgVector q (q0 + (q1 - q0).Scale (t1)); + if (q.m_x >= m_p1.m_x) { + dgVector n (q - origin1); + dgAssert (n.m_w == dgFloat32 (0.0f)); + //contactOut.m_normal = m_transform * n * n.DotProduct(n).InvSqrt(); + contactOut.m_normal = m_transform * n.Normalize(); + return t1; + } + } else if (t0 < maxT) { + dgVector q (q0 + (q1 - q0).Scale (t0)); + if (q.m_x <= m_p0.m_x) { + dgVector n (q - origin0); + dgAssert (n.m_w == dgFloat32 (0.0f)); + //contactOut.m_normal = m_transform * n * n.DotProduct(n).InvSqrt(); + contactOut.m_normal = m_transform * n.Normalize(); + return t0; + } + } + + dgFloat32 ret = dgCollisionConvex::RayCast (q0, q1, maxT, contactOut, body, NULL, NULL); + if (ret <= dgFloat32 (1.0f)) { + contactOut.m_normal = m_transform * contactOut.m_normal; + } + return ret; +} + + +void dgCollisionCapsule::CalculateImplicitContacts(dgInt32 count, dgContactPoint* const contactPoints) const +{ + for (dgInt32 i = 0; i < count; i++) { + contactPoints[i].m_point = contactPoints[i].m_point * m_transform; + if (contactPoints[i].m_point.m_x <= m_p0.m_x) { + dgVector normal(contactPoints[i].m_point); + normal.m_x += m_height; + dgAssert(normal.DotProduct(normal).GetScalar() > dgFloat32(0.0f)); + normal = normal.Normalize(); + contactPoints[i].m_normal = normal * dgVector::m_negOne; + contactPoints[i].m_point = normal.Scale(m_radio0); + contactPoints[i].m_point.m_x -= m_height; + } else if (contactPoints[i].m_point.m_x >= m_p1.m_x) { + dgVector normal(contactPoints[i].m_point); + normal.m_x -= m_height; + dgAssert(normal.DotProduct(normal).GetScalar() > dgFloat32(0.0f)); + normal = normal.Normalize(); + contactPoints[i].m_normal = normal * dgVector::m_negOne; + contactPoints[i].m_point = normal.Scale(m_radio1); + contactPoints[i].m_point.m_x += m_height; + } else { + dgVector normal(contactPoints[i].m_point); + normal.m_x = dgFloat32(0.0f); + normal.m_w = dgFloat32(0.0f); + dgAssert(normal.DotProduct(normal).GetScalar() > dgFloat32(0.0f)); + normal = normal.Normalize(); + + dgFloat32 h = m_p0.m_y + dgFloat32(0.5f) * (m_p1.m_y - m_p0.m_y) * contactPoints[i].m_point.m_x / m_height; + dgVector point(normal.Scale(h)); + contactPoints[i].m_point.m_y = point.m_y; + contactPoints[i].m_point.m_z = point.m_z; + + contactPoints[i].m_normal.m_x = -m_normal.m_x; + contactPoints[i].m_normal.m_y = -m_normal.m_y * normal.m_y; + contactPoints[i].m_normal.m_z = -m_normal.m_y * normal.m_z; + } + contactPoints[i].m_point = contactPoints[i].m_point * m_transform; + contactPoints[i].m_normal = contactPoints[i].m_normal * m_transform; + } +} diff --git a/thirdparty/src/newton/dgPhysics/dgCollisionCapsule.h b/thirdparty/src/newton/dgPhysics/dgCollisionCapsule.h new file mode 100644 index 000000000..de1a77aaf --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgCollisionCapsule.h @@ -0,0 +1,73 @@ +/* Copyright (c) <2003-2019> +* +* 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_COLLISION_CAPSULE_H_ +#define _DG_COLLISION_CAPSULE_H_ + + +#include "dgCollisionConvex.h" + +class dgCollisionCapsule: public dgCollisionConvex +{ + public: + dgCollisionCapsule (dgMemoryAllocator* const allocator, dgUnsigned32 signature, dgFloat32 radio0, dgFloat32 radio1, dgFloat32 height); + dgCollisionCapsule(dgWorld* const world, dgDeserialize deserialization, void* const userData, dgInt32 revisionNumber); + virtual ~dgCollisionCapsule(); + + private: + void Init (dgFloat32 radio0, dgFloat32 radio1, dgFloat32 height); + + virtual dgFloat32 RayCast (const dgVector& localP0, const dgVector& localP1, dgFloat32 maxT, dgContactPoint& contactOut, const dgBody* const body, void* const userData, OnRayPrecastAction preFilter) const; + + virtual dgVector SupportVertex (const dgVector& dir, dgInt32* const vertexIndex) const; + + virtual dgInt32 CalculatePlaneIntersection (const dgVector& normal, const dgVector& point, dgVector* const contactsOut) const; + + virtual void DebugCollision (const dgMatrix& matrix, dgCollision::OnDebugCollisionMeshCallback callback, void* const userData) const; + + virtual dgInt32 CalculateSignature () const; + virtual void SetCollisionBBox (const dgVector& p0, const dgVector& p1); + virtual dgFloat32 CalculateMassProperties (const dgMatrix& offset, dgVector& inertia, dgVector& crossInertia, dgVector& centerOfMass) const; + + virtual void GetCollisionInfo(dgCollisionInfo* const info) const; + virtual void Serialize(dgSerialize callback, void* const userData) const; + + static dgInt32 CalculateSignature(dgFloat32 radio0, dgFloat32 radio1, dgFloat32 height); + + virtual void CalculateImplicitContacts(dgInt32 count, dgContactPoint* const contactPoints) const; + virtual dgVector SupportVertexSpecialProjectPoint(const dgVector& point, const dgVector& dir) const; + virtual dgVector SupportVertexSpecial (const dgVector& dir, dgFloat32 skinThickness, dgInt32* const vertexIndex) const; + + void TesselateTriangle(dgInt32 level, const dgVector& p0, const dgVector& p1, const dgVector& p2, dgInt32& count, dgVector* ouput) const; + + dgVector m_p0; + dgVector m_p1; + dgVector m_normal; + dgVector m_transform; + dgFloat32 m_height; + dgFloat32 m_radio0; + dgFloat32 m_radio1; + + friend class dgWorld; +}; + +#endif + diff --git a/thirdparty/src/newton/dgPhysics/dgCollisionChamferCylinder.cpp b/thirdparty/src/newton/dgPhysics/dgCollisionChamferCylinder.cpp new file mode 100644 index 000000000..6691c4071 --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgCollisionChamferCylinder.cpp @@ -0,0 +1,414 @@ +/* Copyright (c) <2003-2019> +* +* 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 "dgPhysicsStdafx.h" +#include "dgBody.h" +#include "dgContact.h" +#include "dgCollisionChamferCylinder.h" + +////////////////////////////////////////////////////////////////////// +// Construction/Destruction +////////////////////////////////////////////////////////////////////// + + + +dgInt32 dgCollisionChamferCylinder::m_shapeRefCount = 0; +dgVector dgCollisionChamferCylinder::m_yzMask (0, 0xffffffff, 0xffffffff, 0); +dgVector dgCollisionChamferCylinder::m_shapesDirs[DG_MAX_CHAMFERCYLINDER_DIR_COUNT]; +dgCollisionConvex::dgConvexSimplexEdge dgCollisionChamferCylinder::m_edgeArray[(4 * DG_CHAMFERCYLINDER_SLICES + 2)* DG_CHAMFERCYLINDER_BRAKES]; + +dgCollisionChamferCylinder::dgCollisionChamferCylinder(dgMemoryAllocator* allocator, dgUnsigned32 signature, dgFloat32 radius, dgFloat32 height) + :dgCollisionConvex(allocator, signature, m_chamferCylinderCollision) +{ + Init (radius, height); +} + +dgCollisionChamferCylinder::dgCollisionChamferCylinder(dgWorld* const world, dgDeserialize deserialization, void* const userData, dgInt32 revisionNumber) + :dgCollisionConvex (world, deserialization, userData, revisionNumber) +{ + dgVector size; + deserialization (userData, &size, sizeof (dgVector)); + Init (size.m_x, size.m_y); +} + + +dgCollisionChamferCylinder::~dgCollisionChamferCylinder() +{ + m_shapeRefCount --; + dgAssert (m_shapeRefCount >= 0); + + dgCollisionConvex::m_simplex = NULL; + dgCollisionConvex::m_vertex = NULL; +} + + +void dgCollisionChamferCylinder::Init (dgFloat32 radius, dgFloat32 height) +{ + m_rtti |= dgCollisionChamferCylinder_RTTI; + + m_radius = dgMax (dgAbs (radius), D_MIN_CONVEX_SHAPE_SIZE); + m_height = dgMax (dgAbs (height * dgFloat32 (0.5f)), D_MIN_CONVEX_SHAPE_SIZE); + + dgFloat32 sliceAngle = dgFloat32 (0.0f); + dgFloat32 sliceStep = dgPi / DG_CHAMFERCYLINDER_SLICES; + dgFloat32 breakStep = dgPI2 / DG_CHAMFERCYLINDER_BRAKES; + + dgMatrix rot (dgPitchMatrix (breakStep)); + dgInt32 index = 0; + for (dgInt32 j = 0; j <= DG_CHAMFERCYLINDER_SLICES; j ++) { + dgVector p0 (-m_height * dgCos(sliceAngle), dgFloat32 (0.0f), m_radius + m_height * dgSin(sliceAngle), dgFloat32 (0.0f)); + sliceAngle += sliceStep; + for (dgInt32 i = 0; i < DG_CHAMFERCYLINDER_BRAKES; i ++) { + m_vertex[index] = p0; + index ++; + p0 = rot.UnrotateVector (p0); + } + } + + m_edgeCount = (4 * DG_CHAMFERCYLINDER_SLICES + 2)* DG_CHAMFERCYLINDER_BRAKES; + m_vertexCount = DG_CHAMFERCYLINDER_BRAKES * (DG_CHAMFERCYLINDER_SLICES + 1); + dgCollisionConvex::m_vertex = m_vertex; + + if (!m_shapeRefCount) { + dgPolyhedra polyhedra(m_allocator); + dgInt32 wireframe[DG_CHAMFERCYLINDER_SLICES + 10]; + + for (dgInt32 i = 0; i < DG_MAX_CHAMFERCYLINDER_DIR_COUNT; i ++) { + dgMatrix matrix (dgPitchMatrix (dgFloat32 (dgPI2 * i) / DG_MAX_CHAMFERCYLINDER_DIR_COUNT)); + m_shapesDirs[i] = matrix.RotateVector (dgVector (dgFloat32 (0.0f), dgFloat32 (1.0f), dgFloat32 (0.0f), dgFloat32 (0.0f))); + } + + + dgInt32 index0 = 0; + for (dgInt32 j = 0; j < DG_CHAMFERCYLINDER_SLICES; j ++) { + dgInt32 index1 = index0 + DG_CHAMFERCYLINDER_BRAKES - 1; + for (dgInt32 i = 0; i < DG_CHAMFERCYLINDER_BRAKES; i ++) { + wireframe[0] = index0; + wireframe[1] = index1; + wireframe[2] = index1 + DG_CHAMFERCYLINDER_BRAKES; + wireframe[3] = index0 + DG_CHAMFERCYLINDER_BRAKES; + + index1 = index0; + index0 ++; + polyhedra.AddFace (4, wireframe); + } + } + + for (dgInt32 i = 0; i < DG_CHAMFERCYLINDER_BRAKES; i ++) { + wireframe[i] = i; + } + polyhedra.AddFace (DG_CHAMFERCYLINDER_BRAKES, wireframe); + + for (dgInt32 i = 0; i < DG_CHAMFERCYLINDER_BRAKES; i ++) { + wireframe[i] = DG_CHAMFERCYLINDER_BRAKES * (DG_CHAMFERCYLINDER_SLICES + 1) - i - 1; + } + polyhedra.AddFace (DG_CHAMFERCYLINDER_BRAKES, wireframe); + polyhedra.EndFace (); + + dgAssert (SanityCheck (polyhedra)); + + dgUnsigned64 i = 0; + dgPolyhedra::Iterator iter (polyhedra); + for (iter.Begin(); iter; iter ++) { + dgEdge* const edge = &(*iter); + edge->m_userData = i; + i ++; + } + + for (iter.Begin(); iter; iter ++) { + dgEdge* const edge = &(*iter); + + dgConvexSimplexEdge* const ptr = &m_edgeArray[edge->m_userData]; + ptr->m_vertex = edge->m_incidentVertex; + ptr->m_next = &m_edgeArray[edge->m_next->m_userData]; + ptr->m_prev = &m_edgeArray[edge->m_prev->m_userData]; + ptr->m_twin = &m_edgeArray[edge->m_twin->m_userData]; + } + } + + m_shapeRefCount ++; + dgCollisionConvex::m_simplex = m_edgeArray; + + SetVolumeAndCG (); +} + + +void dgCollisionChamferCylinder::DebugCollision (const dgMatrix& matrix, dgCollision::OnDebugCollisionMeshCallback callback, void* const userData) const +{ + dgInt32 slices = 12; + dgInt32 brakes = 24; + dgFloat32 sliceAngle = dgFloat32 (0.0f); + dgFloat32 sliceStep = dgPi / slices; + dgFloat32 breakStep = dgPI2 / brakes; + + dgTriplex pool[24 * (12 + 1)]; + + dgMatrix rot (dgPitchMatrix (breakStep)); + dgInt32 index = 0; + for (dgInt32 j = 0; j <= slices; j ++) { + dgVector p0 (-m_height * dgCos(sliceAngle), dgFloat32 (0.0f), m_radius + m_height * dgSin(sliceAngle), dgFloat32 (0.0f)); + sliceAngle += sliceStep; + for (dgInt32 i = 0; i < brakes; i ++) { + pool[index].m_x = p0.m_x; + pool[index].m_y = p0.m_y; + pool[index].m_z = p0.m_z; + index ++; + p0 = rot.UnrotateVector (p0); + } + } + + matrix.TransformTriplex (&pool[0].m_x, sizeof (dgTriplex), &pool[0].m_x, sizeof (dgTriplex), 24 * (12 + 1)); + + dgTriplex face[32]; + + index = 0; + for (dgInt32 j = 0; j < slices; j ++) { + dgInt32 index0 = index + brakes - 1; + for (dgInt32 i = 0; i < brakes; i ++) { + face[0] = pool[index]; + face[1] = pool[index0]; + face[2] = pool[index0 + brakes]; + face[3] = pool[index + brakes]; + index0 = index; + index ++; + callback (userData, 4, &face[0].m_x, 0); + } + } + + for (dgInt32 i = 0; i < brakes; i ++) { + face[i] = pool[i]; + } + callback (userData, 24, &face[0].m_x, 0); + + for (dgInt32 i = 0; i < brakes; i ++) { + face[i] = pool[brakes * (slices + 1) - i - 1]; + } + callback (userData, 24, &face[0].m_x, 0); +} + + +dgInt32 dgCollisionChamferCylinder::CalculateSignature (dgFloat32 radius, dgFloat32 height) +{ + dgUnsigned32 buffer[3]; + + memset (buffer, 0, sizeof (buffer)); + buffer[0] = m_chamferCylinderCollision; + buffer[1] = dgCollision::Quantize (radius); + buffer[2] = dgCollision::Quantize (height); + return dgInt32 (dgCollision::Quantize(buffer, sizeof (buffer))); +} + +dgInt32 dgCollisionChamferCylinder::CalculateSignature () const +{ + return CalculateSignature (m_radius, m_height); +} + + +void dgCollisionChamferCylinder::SetCollisionBBox (const dgVector& p0__, const dgVector& p1__) +{ + dgAssert (0); +} + + +void dgCollisionChamferCylinder::GetCollisionInfo(dgCollisionInfo* const info) const +{ + dgCollisionConvex::GetCollisionInfo(info); + + info->m_chamferCylinder.m_r = m_radius; + info->m_chamferCylinder.m_height = m_height * dgFloat32 (2.0f); +} + +void dgCollisionChamferCylinder::Serialize(dgSerialize callback, void* const userData) const +{ + dgVector size (m_radius, m_height * dgFloat32 (2.0f), dgFloat32 (0.0f), dgFloat32 (0.0f)); + + SerializeLow(callback, userData); + callback (userData, &size, sizeof (dgVector)); +} + + + +dgFloat32 dgCollisionChamferCylinder::RayCast(const dgVector& q0, const dgVector& q1, dgFloat32 maxT, dgContactPoint& contactOut, const dgBody* const body, void* const userData, OnRayPrecastAction preFilter) const +{ + if (q0.m_x > m_height) { + if (q1.m_x < m_height) { + dgFloat32 t1 = (m_height - q0.m_x) / (q1.m_x - q0.m_x); + dgFloat32 y = q0.m_y + (q1.m_y - q0.m_y) * t1; + dgFloat32 z = q0.m_z + (q1.m_z - q0.m_z) * t1; + if ((y * y + z * z) < m_radius * m_radius) { + contactOut.m_normal = dgVector(dgFloat32(1.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f)); + return t1; + } + } + } + + if (q0.m_x < -m_height) { + if (q1.m_x > -m_height) { + dgFloat32 t1 = (-m_height - q0.m_x) / (q1.m_x - q0.m_x); + dgFloat32 y = q0.m_y + (q1.m_y - q0.m_y) * t1; + dgFloat32 z = q0.m_z + (q1.m_z - q0.m_z) * t1; + if ((y * y + z * z) < m_radius * m_radius) { + contactOut.m_normal = dgVector(dgFloat32(-1.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f)); + return t1; + } + } + } + + dgVector dq((q1 - q0) & dgVector::m_triplexMask); + + // avoid NaN as a result of a division by zero + if (dq.DotProduct(dq).GetScalar() <= 0.0f) { + return dgFloat32(1.2f); + } + + //dgVector dir(dq * dq.InvMagSqrt()); + dgVector dir(dq.Normalize()); + if (dgAbs(dir.m_x) > 0.9999f) { + return dgCollisionConvex::RayCast(q0, q1, maxT, contactOut, body, NULL, NULL); + } + + dgVector p0(q0 & dgVector::m_triplexMask); + dgVector p1(q1 & dgVector::m_triplexMask); + + p0.m_x = dgFloat32 (0.0f); + p1.m_x = dgFloat32 (0.0f); + + dgVector dp (p1 - p0); + dgFloat32 a = dp.DotProduct(dp).GetScalar(); + dgFloat32 b = dgFloat32 (2.0f) * dp.DotProduct(p0).GetScalar(); + dgFloat32 c = p0.DotProduct(p0).GetScalar() - m_radius * m_radius; + + dgFloat32 disc = b * b - dgFloat32 (4.0f) * a * c; + if (disc >= dgFloat32 (0.0f)) { + disc = dgSqrt (disc); + dgVector origin0(p0 + dp.Scale ((-b + disc) / (dgFloat32 (2.0f) * a))); + dgVector origin1(p0 + dp.Scale ((-b - disc) / (dgFloat32 (2.0f) * a))); + dgFloat32 t0 = dgRayCastSphere(q0, q1, origin0, m_height); + dgFloat32 t1 = dgRayCastSphere(q0, q1, origin1, m_height); + if(t1 < t0) { + t0 = t1; + origin0 = origin1; + } + + if ((t0 >= 0.0f) && (t0 <= 1.0f)) { + contactOut.m_normal = q0 + dq.Scale(t0) - origin0; + dgAssert(contactOut.m_normal.m_w == dgFloat32(0.0f)); + + //contactOut.m_normal = contactOut.m_normal * contactOut.m_normal.DotProduct(contactOut.m_normal).InvSqrt(); + contactOut.m_normal = contactOut.m_normal.Normalize(); + return t0; + } + } else { + dgVector origin0 (dgPointToRayDistance (dgVector::m_zero, p0, p1)); + origin0 = origin0.Scale(m_radius / dgSqrt(origin0.DotProduct(origin0).GetScalar())); + dgFloat32 t0 = dgRayCastSphere(q0, q1, origin0, m_height); + if ((t0 >= 0.0f) && (t0 <= 1.0f)) { + contactOut.m_normal = q0 + dq.Scale(t0) - origin0; + dgAssert(contactOut.m_normal.m_w == dgFloat32(0.0f)); + + //contactOut.m_normal = contactOut.m_normal * contactOut.m_normal.DotProduct(contactOut.m_normal).InvSqrt(); + contactOut.m_normal = contactOut.m_normal.Normalize(); + return t0; + } + } + return dgFloat32(1.2f); +} + +dgVector dgCollisionChamferCylinder::SupportVertex (const dgVector& dir, dgInt32* const vertexIndex) const +{ + dgAssert (dir.m_w == dgFloat32 (0.0f)); + dgAssert (dgAbs(dir.DotProduct(dir).GetScalar() - dgFloat32 (1.0f)) < dgFloat32 (1.0e-3f)); + + dgFloat32 x = dir.GetScalar(); + if (dgAbs (x) > dgFloat32 (0.9999f)) { + return dgVector (dgSign (x) * m_height, m_radius, dgFloat32 (0.0f), dgFloat32 (0.0f)); + } + + dgVector sideDir (m_yzMask & dir); + sideDir = sideDir.Normalize(); + return sideDir.Scale(m_radius) + dir.Scale (m_height); +} + +dgVector dgCollisionChamferCylinder::SupportVertexSpecial (const dgVector& dir, dgFloat32 skinThickness, dgInt32* const vertexIndex) const +{ + dgAssert (dir.m_w == dgFloat32 (0.0f)); + dgAssert (dgAbs(dir.DotProduct(dir).GetScalar() - dgFloat32 (1.0f)) < dgFloat32 (1.0e-3f)); + + dgFloat32 x = dir.GetScalar(); + if (dgAbs (x) > dgFloat32 (0.99995f)) { + return dgVector (dgFloat32 (0.0f), m_radius, dgFloat32 (0.0f), dgFloat32 (0.0f)); + } + + dgVector sideDir (m_yzMask & dir); + dgAssert (sideDir.DotProduct(sideDir).GetScalar() > dgFloat32 (0.0f)); + return sideDir.Normalize().Scale(m_radius); +} + +dgVector dgCollisionChamferCylinder::SupportVertexSpecialProjectPoint (const dgVector& point, const dgVector& dir) const +{ + dgAssert (dir.m_w == 0.0f); + return point + dir.Scale(m_height - DG_PENETRATION_TOL); +} + +dgInt32 dgCollisionChamferCylinder::CalculatePlaneIntersection (const dgVector& normal, const dgVector& origin, dgVector* const contactsOut) const +{ + dgInt32 count = 0; + const dgFloat32 inclination = dgFloat32 (0.9999f); + if (normal.m_x < -inclination) { + dgMatrix matrix(normal); + dgFloat32 x = dgSqrt (dgMax (m_height * m_height - origin.m_x * origin.m_x, dgFloat32 (0.0f))); + matrix.m_posit.m_x = origin.m_x; + count = BuildCylinderCapPoly (m_radius + x, matrix, contactsOut); + //count = RectifyConvexSlice(n, normal, contactsOut); + } else if (normal.m_x > inclination) { + dgMatrix matrix(normal); + dgFloat32 x = dgSqrt (dgMax (m_height * m_height - origin.m_x * origin.m_x, dgFloat32 (0.0f))); + matrix.m_posit.m_x = origin.m_x; + count = BuildCylinderCapPoly (m_radius + x, matrix, contactsOut); + //count = RectifyConvexSlice(n, normal, contactsOut); + } else { + count = 1; + contactsOut[0] = SupportVertex (normal, NULL); + } + return count; +} + +void dgCollisionChamferCylinder::CalculateImplicitContacts(dgInt32 count, dgContactPoint* const contactPoints) const +{ + for (dgInt32 i = 0; i < count; i ++) { + dgVector diskPoint (contactPoints[i].m_point); + diskPoint.m_x = dgFloat32 (0.0f); + diskPoint.m_w = dgFloat32 (0.0f); + dgAssert(diskPoint.DotProduct(diskPoint).GetScalar() > dgFloat32(0.0f)); + dgFloat32 r2 = diskPoint.DotProduct(diskPoint).GetScalar(); + if (r2 >= m_radius * m_radius) { + diskPoint = diskPoint.Normalize().Scale (m_radius); + dgVector normal (contactPoints[i].m_point - diskPoint); + normal = normal.Normalize(); + contactPoints[i].m_point = diskPoint + normal.Scale (m_height); + contactPoints[i].m_normal = normal * dgVector::m_negOne; + } else { + contactPoints[i].m_normal = dgVector::m_zero; + contactPoints[i].m_normal.m_x = dgSign(contactPoints[i].m_point.m_x); + contactPoints[i].m_point.m_x = dgSign(contactPoints[i].m_normal.m_x) * m_height; + } + } +} \ No newline at end of file diff --git a/thirdparty/src/newton/dgPhysics/dgCollisionChamferCylinder.h b/thirdparty/src/newton/dgPhysics/dgCollisionChamferCylinder.h new file mode 100644 index 000000000..b8f80befd --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgCollisionChamferCylinder.h @@ -0,0 +1,71 @@ +/* Copyright (c) <2003-2019> +* +* 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_DGCOLLISIONCHAMFERCYLINDER_H__AS235640FER_H) +#define AFX_DGCOLLISIONCHAMFERCYLINDER_H__AS235640FER_H + +#include "dgCollisionConvex.h" + +#define DG_CHAMFERCYLINDER_SLICES 4 +#define DG_CHAMFERCYLINDER_BRAKES 8 +#define DG_MAX_CHAMFERCYLINDER_DIR_COUNT 8 + + +class dgCollisionChamferCylinder: public dgCollisionConvex +{ + public: + dgCollisionChamferCylinder(dgMemoryAllocator* const allocator, dgUnsigned32 signature, dgFloat32 radius, dgFloat32 height); + dgCollisionChamferCylinder(dgWorld* const world, dgDeserialize deserialization, void* const userData, dgInt32 revisionNumber); + virtual ~dgCollisionChamferCylinder(); + + protected: + void Init (dgFloat32 radius, dgFloat32 height); + virtual dgVector SupportVertex (const dgVector& dir, dgInt32* const vertexIndex) const; + virtual dgInt32 CalculatePlaneIntersection (const dgVector& normal, const dgVector& origin, dgVector* const contactsOut) const; + virtual dgFloat32 RayCast (const dgVector& localP0, const dgVector& localP1, dgFloat32 maxT, dgContactPoint& contactOut, const dgBody* const body, void* const userData, OnRayPrecastAction preFilter) const; + + virtual void DebugCollision (const dgMatrix& matrix, dgCollision::OnDebugCollisionMeshCallback callback, void* const userData) const; + virtual dgInt32 CalculateSignature () const; + virtual void SetCollisionBBox (const dgVector& p0, const dgVector& p1); + virtual void GetCollisionInfo(dgCollisionInfo* const info) const; + virtual void Serialize(dgSerialize callback, void* const userData) const; + + static dgInt32 CalculateSignature (dgFloat32 radius, dgFloat32 height); + + virtual void CalculateImplicitContacts(dgInt32 count, dgContactPoint* const contactPoints) const; + virtual dgVector SupportVertexSpecial (const dgVector& dir, dgFloat32 skinThickness, dgInt32* const vertexIndex) const; + virtual dgVector SupportVertexSpecialProjectPoint (const dgVector& point, const dgVector& dir) const; + + private: + dgFloat32 m_height; + dgFloat32 m_radius; + + dgVector m_vertex[DG_CHAMFERCYLINDER_BRAKES * (DG_CHAMFERCYLINDER_SLICES + 1)]; + static dgInt32 m_shapeRefCount; + static dgConvexSimplexEdge m_edgeArray[]; + static dgVector m_shapesDirs[]; + static dgVector m_yzMask; + + friend class dgWorld; +}; + +#endif + diff --git a/thirdparty/src/newton/dgPhysics/dgCollisionCompound.cpp b/thirdparty/src/newton/dgPhysics/dgCollisionCompound.cpp new file mode 100644 index 000000000..0a5fcc943 --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgCollisionCompound.cpp @@ -0,0 +1,3065 @@ +/* Copyright (c) <2003-2019> +* +* 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 "dgPhysicsStdafx.h" +#include "dgWorld.h" +#include "dgCollisionBVH.h" +#include "dgCollisionConvex.h" +#include "dgCollisionCompound.h" +#include "dgCollisionInstance.h" +#include "dgCollisionUserMesh.h" +#include "dgCollisionHeightField.h" + + +////////////////////////////////////////////////////////////////////// +// Construction/Destruction +////////////////////////////////////////////////////////////////////// + +#define DG_MAX_MIN_VOLUME dgFloat32 (1.0e-3f) + + +dgVector dgCollisionCompound::m_padding (dgFloat32 (1.0e-3f)); +dgVector dgCollisionCompound::dgOOBBTestData::m_maxDist (dgFloat32 (1.0e10f)); + +class dgCollisionCompound::dgHeapNodePair +{ + public: + dgNodeBase* m_nodeA; + dgNodeBase* m_nodeB; +}; + + +dgCollisionCompound::dgTreeArray::dgTreeArray (dgMemoryAllocator* const allocator) + :dgTree(allocator) +{ +} + +void dgCollisionCompound::dgTreeArray::AddNode (dgNodeBase* const node, dgInt32 index, const dgCollisionInstance* const parent) +{ + dgTreeNode* const myNode = dgTree::Insert(node, index); + node->m_myNode = myNode; + node->m_shape->m_parent = parent; + node->m_shape->m_subCollisionHandle = myNode; +} + + +dgCollisionCompound::dgOOBBTestData::dgOOBBTestData (const dgMatrix& matrix) + :m_matrix (matrix) + ,m_separatingDistance(dgFloat32 (1.0e10f)) +{ + m_absMatrix[0] = m_matrix[0].Abs(); + m_absMatrix[1] = m_matrix[1].Abs(); + m_absMatrix[2] = m_matrix[2].Abs(); + m_absMatrix[3] = dgVector::m_wOne; + + dgInt32 index = 0; + for (dgInt32 i = 0; i < 3; i ++) { + dgVector dir(dgFloat32 (0.0f)); + dir[i] = dgFloat32 (1.0f); + for (dgInt32 j = 0; j < 3; j ++) { + dgVector axis (dir.CrossProduct(m_matrix[j])); + m_crossAxis[index] = axis; + m_crossAxisAbs[index] = axis.Abs(); + m_crossAxisDotAbs[index] = matrix.UnrotateVector (axis).Abs(); + index ++; + } + } + + dgVector tmp; + dgVector::Transpose4x4 (m_crossAxis[0], m_crossAxis[1], m_crossAxis[2], m_crossAxis[3], m_crossAxis[0], m_crossAxis[1], m_crossAxis[2], m_crossAxis[3]); + dgVector::Transpose4x4 (m_crossAxis[3], m_crossAxis[4], m_crossAxis[5], m_crossAxis[6], m_crossAxis[4], m_crossAxis[5], m_crossAxis[6], m_crossAxis[7]); + dgVector::Transpose4x4 (m_crossAxis[6], m_crossAxis[7], m_crossAxis[8], tmp, m_crossAxis[8], m_crossAxis[8], m_crossAxis[8], m_crossAxis[8]); + + dgVector::Transpose4x4 (m_crossAxisAbs[0], m_crossAxisAbs[1], m_crossAxisAbs[2], m_crossAxisAbs[3], m_crossAxisAbs[0], m_crossAxisAbs[1], m_crossAxisAbs[2], m_crossAxisAbs[3]); + dgVector::Transpose4x4 (m_crossAxisAbs[3], m_crossAxisAbs[4], m_crossAxisAbs[5], m_crossAxisAbs[6], m_crossAxisAbs[4], m_crossAxisAbs[5], m_crossAxisAbs[6], m_crossAxisAbs[7]); + dgVector::Transpose4x4 (m_crossAxisAbs[6], m_crossAxisAbs[7], m_crossAxisAbs[8], tmp, m_crossAxisAbs[8], m_crossAxisAbs[8], m_crossAxisAbs[8], m_crossAxisAbs[8]); + + dgVector::Transpose4x4 (m_crossAxisDotAbs[0], m_crossAxisDotAbs[1], m_crossAxisDotAbs[2], m_crossAxisDotAbs[3], m_crossAxisDotAbs[0], m_crossAxisDotAbs[1], m_crossAxisDotAbs[2], m_crossAxisDotAbs[3]); + dgVector::Transpose4x4 (m_crossAxisDotAbs[3], m_crossAxisDotAbs[4], m_crossAxisDotAbs[5], m_crossAxisDotAbs[6], m_crossAxisDotAbs[4], m_crossAxisDotAbs[5], m_crossAxisDotAbs[6], m_crossAxisDotAbs[7]); + dgVector::Transpose4x4 (m_crossAxisDotAbs[6], m_crossAxisDotAbs[7], m_crossAxisDotAbs[8], tmp, m_crossAxisDotAbs[8], m_crossAxisDotAbs[8], m_crossAxisDotAbs[8], m_crossAxisDotAbs[8]); +} + + +dgCollisionCompound::dgOOBBTestData::dgOOBBTestData (const dgMatrix& matrix, const dgVector& localOrigin, const dgVector& localSize) + :m_matrix (matrix) + ,m_origin(localOrigin) + ,m_size(localSize) + ,m_localP0 (localOrigin - localSize) + ,m_localP1 (localOrigin + localSize) + ,m_separatingDistance(dgFloat32 (1.0e10f)) +{ + m_absMatrix[0] = m_matrix[0].Abs(); + m_absMatrix[1] = m_matrix[1].Abs(); + m_absMatrix[2] = m_matrix[2].Abs(); + m_absMatrix[3] = dgVector::m_wOne; + + dgInt32 index = 0; + for (dgInt32 i = 0; i < 3; i ++) { + dgVector dir(dgFloat32 (0.0f)); + dir[i] = dgFloat32 (1.0f); + for (dgInt32 j = 0; j < 3; j ++) { + m_crossAxis[index] = dir.CrossProduct(m_matrix[j]); + index ++; + } + } + + dgVector size (m_absMatrix.RotateVector(m_size)); + dgVector origin (m_matrix.TransformVector (m_origin)); + m_aabbP0 = origin - size; + m_aabbP1 = origin + size; + + index = 0; + dgVector extends[9]; + for (dgInt32 i = 0; i < 3; i ++) { + for (dgInt32 j = 0; j < 3; j ++) { + const dgVector& axis = m_crossAxis[index]; + dgAssert (axis.m_w == dgFloat32 (0.0f)); + dgVector tmp (m_matrix.UnrotateVector(axis)); + dgVector d (m_size.DotProduct(tmp.Abs()) + m_padding); + dgVector c (origin.DotProduct(axis)); + dgVector diff (c - d); + dgVector sum (c + d); + extends[index] = dgVector (diff.m_x, sum.m_x, diff.m_y, sum.m_y); + m_crossAxisAbs[index] = axis.Abs(); + index ++; + } + } + + dgVector tmp; + dgVector::Transpose4x4 (m_crossAxis[0], m_crossAxis[1], m_crossAxis[2], m_crossAxis[3], m_crossAxis[0], m_crossAxis[1], m_crossAxis[2], m_crossAxis[3]); + dgVector::Transpose4x4 (m_crossAxis[3], m_crossAxis[4], m_crossAxis[5], m_crossAxis[6], m_crossAxis[4], m_crossAxis[5], m_crossAxis[6], m_crossAxis[7]); + dgVector::Transpose4x4 (m_crossAxis[6], m_crossAxis[7], m_crossAxis[8], tmp, m_crossAxis[8], m_crossAxis[8], m_crossAxis[8], m_crossAxis[8]); + + dgVector::Transpose4x4 (m_crossAxisAbs[0], m_crossAxisAbs[1], m_crossAxisAbs[2], m_crossAxisAbs[3], m_crossAxisAbs[0], m_crossAxisAbs[1], m_crossAxisAbs[2], m_crossAxisAbs[3]); + dgVector::Transpose4x4 (m_crossAxisAbs[3], m_crossAxisAbs[4], m_crossAxisAbs[5], m_crossAxisAbs[6], m_crossAxisAbs[4], m_crossAxisAbs[5], m_crossAxisAbs[6], m_crossAxisAbs[7]); + dgVector::Transpose4x4 (m_crossAxisAbs[6], m_crossAxisAbs[7], m_crossAxisAbs[8], tmp, m_crossAxisAbs[8], m_crossAxisAbs[8], m_crossAxisAbs[8], m_crossAxisAbs[8]); + + dgVector::Transpose4x4 (m_extendsMinX[0], m_extendsMaxX[0], tmp, tmp, extends[0], extends[1], extends[2], extends[3]); + dgVector::Transpose4x4 (m_extendsMinX[1], m_extendsMaxX[1], tmp, tmp, extends[4], extends[5], extends[6], extends[7]); + dgVector::Transpose4x4 (m_extendsMinX[2], m_extendsMaxX[2], tmp, tmp, extends[8], extends[8], extends[8], extends[8]); +} + + +dgCollisionCompound::dgNodeBase::dgNodeBase () + :m_left(NULL) + ,m_right(NULL) + ,m_parent(NULL) + ,m_shape(NULL) + ,m_myNode(NULL) +{ +} + +dgCollisionCompound::dgNodeBase::dgNodeBase (const dgNodeBase& copyFrom) + :m_p0(copyFrom.m_p0) + ,m_p1(copyFrom.m_p1) + ,m_size(copyFrom.m_size) + ,m_origin(copyFrom.m_origin) + ,m_area(copyFrom.m_area) + ,m_type(copyFrom.m_type) + ,m_left(NULL) + ,m_right(NULL) + ,m_parent(NULL) + ,m_shape(copyFrom.m_shape) + ,m_myNode(NULL) +{ + dgAssert (!copyFrom.m_shape); +} + + +dgCollisionCompound::dgNodeBase::dgNodeBase (dgCollisionInstance* const instance) + :m_type(m_leaf) + ,m_left(NULL) + ,m_right(NULL) + ,m_parent(NULL) + ,m_shape(new (instance->GetAllocator()) dgCollisionInstance (*instance)) + ,m_myNode(NULL) +{ + CalculateAABB(); +} + + +dgCollisionCompound::dgNodeBase::dgNodeBase (dgNodeBase* const left, dgNodeBase* const right) + :m_type(m_node) + ,m_left(left) + ,m_right(right) + ,m_parent(NULL) + ,m_shape(NULL) + ,m_myNode(NULL) +{ + m_left->m_parent = this; + m_right->m_parent = this; + + dgVector p0 (left->m_p0.GetMin(right->m_p0)); + dgVector p1 (left->m_p1.GetMax(right->m_p1)); + SetBox(p0, p1); +} + +dgCollisionCompound::dgNodeBase::~dgNodeBase() +{ + if (m_shape) { + m_shape->Release (); + } + if (m_left) { + delete m_left; + } + if (m_right) { + delete m_right; + } +} + + +void dgCollisionCompound::dgNodeBase::SetBox (const dgVector& p0, const dgVector& p1) +{ + m_p0 = p0; + m_p1 = p1; + dgAssert (m_p0.m_w == dgFloat32 (0.0f)); + dgAssert (m_p1.m_w == dgFloat32 (0.0f)); + m_size = dgVector::m_half * (m_p1 - m_p0); + m_origin = dgVector::m_half * (m_p1 + m_p0); + m_area = m_size.DotProduct(m_size.ShiftTripleRight()).m_x; +} + +void dgCollisionCompound::dgNodeBase::CalculateAABB() +{ + dgVector p0; + dgVector p1; + m_shape->CalcAABB(m_shape->GetLocalMatrix (), p0, p1); + SetBox (p0, p1); +} + +bool dgCollisionCompound::dgNodeBase::BoxTest (const dgOOBBTestData& data) const +{ + dgFloat32 separatingDistance = data.UpdateSeparatingDistance (data.m_aabbP0, data.m_aabbP1, m_p0, m_p1); + if (dgOverlapTest (data.m_aabbP0, data.m_aabbP1, m_p0, m_p1)) { + // this assert is in fact a bug + //dgAssert (separatingDistance > dgFloat32 (1000.0f)); + dgVector origin (data.m_matrix.UntransformVector(m_origin)); + dgVector size (data.m_absMatrix.UnrotateVector(m_size)); + dgVector p0 (origin - size); + dgVector p1 (origin + size); + data.m_separatingDistance = dgMin(data.m_separatingDistance, data.UpdateSeparatingDistance (p0, p1, data.m_localP0, data.m_localP1)); + if (dgOverlapTest (p0, p1, data.m_localP0, data.m_localP1)) { + dgVector size_x (m_size.m_x); + dgVector size_y (m_size.m_y); + dgVector size_z (m_size.m_z); + + dgVector origin_x (m_origin.m_x); + dgVector origin_y (m_origin.m_y); + dgVector origin_z (m_origin.m_z); + + bool ret = true; + for (dgInt32 i = 0; (i < 3) && ret; i ++) { + const dgInt32 j = i * 3; + dgVector c (origin_x * data.m_crossAxis[j + 0] + origin_y * data.m_crossAxis[j + 1] + origin_z * data.m_crossAxis[j + 2]); + dgVector d (size_x * data.m_crossAxisAbs[j + 0] + size_y * data.m_crossAxisAbs[j + 1] + size_z * data.m_crossAxisAbs[j + 2] + m_padding); + dgVector x0 (c - d); + dgVector x1 (c + d); + dgVector box0 (x0 - data.m_extendsMaxX[i]); + dgVector box1 (x1 - data.m_extendsMinX[i]); + dgVector test (box0 * box1); + ret = (test.GetSignMask() & 0x0f) == 0x0f; + } + return ret; + } + } + data.m_separatingDistance = dgMin(data.m_separatingDistance, separatingDistance); + return false; +} + + +bool dgCollisionCompound::dgNodeBase::BoxTest (const dgOOBBTestData& data, const dgNodeBase* const otherNode) const +{ + dgVector otherOrigin (data.m_matrix.TransformVector(otherNode->m_origin)); + dgVector otherSize (data.m_absMatrix.RotateVector(otherNode->m_size)); + dgVector otherP0 ((otherOrigin - otherSize) & dgVector::m_triplexMask); + dgVector otherP1 ((otherOrigin + otherSize) & dgVector::m_triplexMask); + + dgFloat32 separatingDistance = data.UpdateSeparatingDistance (m_p0, m_p1, otherP0, otherP1); + if (dgOverlapTest (m_p0, m_p1, otherP0, otherP1)) { + dgAssert (separatingDistance > dgFloat32 (1000.0f)); + dgVector origin (data.m_matrix.UntransformVector(m_origin)); + dgVector size (data.m_absMatrix.UnrotateVector(m_size)); + dgVector p0 (origin - size); + dgVector p1 (origin + size); + data.m_separatingDistance = dgMin(data.m_separatingDistance, data.UpdateSeparatingDistance (p0, p1, otherNode->m_p0, otherNode->m_p1)); + if (dgOverlapTest (p0, p1, otherNode->m_p0, otherNode->m_p1)) { + dgVector size0_x (m_size.m_x); + dgVector size0_y (m_size.m_y); + dgVector size0_z (m_size.m_z); + + dgVector origin0_x (m_origin.m_x); + dgVector origin0_y (m_origin.m_y); + dgVector origin0_z (m_origin.m_z); + + dgVector size1_x (otherNode->m_size.m_x); + dgVector size1_y (otherNode->m_size.m_y); + dgVector size1_z (otherNode->m_size.m_z); + + dgVector origin1_x (otherOrigin.m_x); + dgVector origin1_y (otherOrigin.m_y); + dgVector origin1_z (otherOrigin.m_z); + + bool ret = true; + for (dgInt32 j = 0; (j < 9) && ret; j += 3) { + dgVector c0 (origin0_x * data.m_crossAxis[j + 0] + origin0_y * data.m_crossAxis[j + 1] + origin0_z * data.m_crossAxis[j + 2]); + dgVector d0 (size0_x * data.m_crossAxisAbs[j + 0] + size0_y * data.m_crossAxisAbs[j + 1] + size0_z * data.m_crossAxisAbs[j + 2] + m_padding); + dgVector x0 (c0 - d0); + dgVector x1 (c0 + d0); + + dgVector c1 (origin1_x * data.m_crossAxis[j + 0] + origin1_y * data.m_crossAxis[j + 1] + origin1_z * data.m_crossAxis[j + 2]); + dgVector d1 (size1_x * data.m_crossAxisDotAbs[j + 0] + size1_y * data.m_crossAxisDotAbs[j + 1] + size1_z * data.m_crossAxisDotAbs[j + 2] + m_padding); + dgVector z0 (c1 - d1); + dgVector z1 (c1 + d1); + + dgVector box0 (x0 - z1); + dgVector box1 (x1 - z0); + dgVector test (box0 * box1); + ret = (test.GetSignMask() & 0x0f) == 0x0f; + } + return ret; + } + } + data.m_separatingDistance = dgMin(data.m_separatingDistance, separatingDistance); + return false; +} + +dgFloat32 dgCollisionCompound::dgNodeBase::RayBoxDistance (const dgOOBBTestData& data, const dgFastRayTest& myRay, const dgFastRayTest& otherRay, const dgNodeBase* const otherNode) const +{ + dgVector otherOrigin (data.m_matrix.TransformVector(otherNode->m_origin)); + dgVector otherSize (data.m_absMatrix.RotateVector(otherNode->m_size)); + dgVector otherP0 ((otherOrigin - otherSize) & dgVector::m_triplexMask); + dgVector otherP1 ((otherOrigin + otherSize) & dgVector::m_triplexMask); + dgVector minBox (m_p0 - otherP1); + dgVector maxBox (m_p1 - otherP0); + dgFloat32 dist = myRay.BoxIntersect (minBox, maxBox); + if (dist <= 1.0f) { + dgVector origin (data.m_matrix.UntransformVector(m_origin)); + dgVector size (data.m_absMatrix.UnrotateVector(m_size)); + dgVector p0 (origin - size); + dgVector p1 (origin + size); + dgVector minBox1 (p0 - otherNode->m_p1); + dgVector maxBox1 (p1 - otherNode->m_p0); + dgFloat32 dist1 = otherRay.BoxIntersect (minBox1, maxBox1); + dist = dgMax(dist, dist1); + } + + return dist; +} + +class dgCollisionCompound::dgSpliteInfo +{ + public: + dgSpliteInfo (dgNodeBase** const boxArray, dgInt32 boxCount) + { + dgVector minP ( dgFloat32 (1.0e15f)); + dgVector maxP (-dgFloat32 (1.0e15f)); + + if (boxCount == 2) { + m_axis = 1; + for (dgInt32 i = 0; i < boxCount; i ++) { + dgNodeBase* const node = boxArray[i]; + dgAssert (node->m_type == m_leaf); + minP = minP.GetMin (node->m_p0); + maxP = maxP.GetMax (node->m_p1); + } + } else { + dgVector median (dgFloat32 (0.0f)); + dgVector varian (dgFloat32 (0.0f)); + + for (dgInt32 i = 0; i < boxCount; i ++) { + dgNodeBase* const node = boxArray[i]; + dgAssert (node->m_type == m_leaf); + minP = minP.GetMin (node->m_p0); + maxP = maxP.GetMax (node->m_p1); + dgVector p (dgVector::m_half * (node->m_p0 + node->m_p1)); + median += p; + varian += p * p; + } + + varian = varian.Scale (dgFloat32 (boxCount)) - median * median; + + dgInt32 index = 0; + dgFloat32 maxVarian = dgFloat32 (-1.0e10f); + for (dgInt32 i = 0; i < 3; i ++) { + if (varian[i] > maxVarian) { + index = i; + maxVarian = varian[i]; + } + } + + dgVector center = median.Scale (dgFloat32 (1.0f) / dgFloat32 (boxCount)); + + dgFloat32 test = center[index]; + + dgInt32 i0 = 0; + dgInt32 i1 = boxCount - 1; + do { + for (; i0 <= i1; i0 ++) { + dgNodeBase* const node = boxArray[i0]; + dgFloat32 val = (node->m_p0[index] + node->m_p1[index]) * dgFloat32 (0.5f); + if (val > test) { + break; + } + } + + for (; i1 >= i0; i1 --) { + dgNodeBase* const node = boxArray[i1]; + dgFloat32 val = (node->m_p0[index] + node->m_p1[index]) * dgFloat32 (0.5f); + if (val < test) { + break; + } + } + + if (i0 < i1) { + dgSwap(boxArray[i0], boxArray[i1]); + i0++; + i1--; + } + + } while (i0 <= i1); + + if (i0 > 0){ + i0 --; + } + if ((i0 + 1) >= boxCount) { + i0 = boxCount - 2; + } + + m_axis = i0 + 1; + } + + dgAssert (maxP.m_x - minP.m_x >= dgFloat32 (0.0f)); + dgAssert (maxP.m_y - minP.m_y >= dgFloat32 (0.0f)); + dgAssert (maxP.m_z - minP.m_z >= dgFloat32 (0.0f)); + m_p0 = minP; + m_p1 = maxP; + } + + dgInt32 m_axis; + dgVector m_p0; + dgVector m_p1; +}; + + +dgCollisionCompound::dgCollisionCompound(dgWorld* const world) + :dgCollision (world->GetAllocator(), 0, m_compoundCollision) + ,m_world(world) + ,m_root(NULL) + ,m_myInstance(NULL) + ,m_array (world->GetAllocator()) + ,m_treeEntropy (dgFloat32 (0.0f)) + ,m_boxMinRadius(dgFloat32(0.0f)) + ,m_boxMaxRadius(dgFloat32(0.0f)) + ,m_idIndex(0) + ,m_criticalSectionLock(0) +{ + m_rtti |= dgCollisionCompound_RTTI; +} + +dgCollisionCompound::dgCollisionCompound (const dgCollisionCompound& source, const dgCollisionInstance* const myInstance) + :dgCollision (source) + ,m_world (source.m_world) + ,m_root(NULL) + ,m_myInstance(myInstance) + ,m_array (source.GetAllocator()) + ,m_treeEntropy(source.m_treeEntropy) + ,m_boxMinRadius(source.m_boxMinRadius) + ,m_boxMaxRadius(source.m_boxMaxRadius) + ,m_idIndex(source.m_idIndex) + ,m_criticalSectionLock(0) +{ + m_rtti |= dgCollisionCompound_RTTI; + + dgTreeArray::Iterator iter (source.m_array); + for (iter.Begin(); iter; iter ++) { + dgNodeBase* const node = iter.GetNode()->GetInfo(); + dgNodeBase* const newNode = new (m_allocator) dgNodeBase (node->GetShape()); + m_array.AddNode(newNode, iter.GetNode()->GetKey(), m_myInstance); + } + + if (source.m_root) { + dgNodeBase* pool[DG_COMPOUND_STACK_DEPTH]; + dgNodeBase* parents[DG_COMPOUND_STACK_DEPTH]; + pool[0] = source.m_root; + parents[0] = NULL; + dgInt32 stack = 1; + while (stack) { + stack --; + dgNodeBase* const sourceNode = pool [stack]; + + dgNodeBase* parent = NULL; + if (sourceNode->m_type == m_node) { + parent = new (m_allocator) dgNodeBase (*sourceNode); + if (!sourceNode->m_parent) { + m_root = parent; + } else { + parent->m_parent = parents[stack]; + if (parent->m_parent) { + if (sourceNode->m_parent->m_left == sourceNode) { + parent->m_parent->m_left = parent; + } else { + dgAssert (sourceNode->m_parent->m_right == sourceNode); + parent->m_parent->m_right = parent; + } + } + } + } else { + //dgNodeBase* const node = m_array.Find (sourceNode->m_shape)->GetInfo(); + dgNodeBase* const node = m_array.Find (sourceNode->m_myNode->GetKey())->GetInfo(); + dgAssert (node); + node->m_parent = parents[stack]; + if (node->m_parent) { + if (sourceNode->m_parent->m_left == sourceNode) { + node->m_parent->m_left = node; + } else { + dgAssert (sourceNode->m_parent->m_right == sourceNode); + node->m_parent->m_right = node; + } + } else { + m_root = node; + } + } + + if (sourceNode->m_left) { + parents[stack] = parent; + pool[stack] = sourceNode->m_left; + stack ++; + dgAssert (stack < DG_COMPOUND_STACK_DEPTH); + } + + if (sourceNode->m_right) { + parents[stack] = parent; + pool[stack] = sourceNode->m_right; + stack ++; + dgAssert (stack < DG_COMPOUND_STACK_DEPTH); + } + } + } +} + +dgCollisionCompound::dgCollisionCompound (dgWorld* const world, dgDeserialize deserialization, void* const userData, const dgCollisionInstance* const myInstance, dgInt32 revisionNumber) + :dgCollision (world, deserialization, userData, revisionNumber) + ,m_world(world) + ,m_root(NULL) + ,m_myInstance(myInstance) + ,m_array (world->GetAllocator()) + ,m_treeEntropy(dgFloat32(0.0f)) + ,m_boxMinRadius(dgFloat32(0.0f)) + ,m_boxMaxRadius(dgFloat32(0.0f)) + ,m_idIndex(0) + ,m_criticalSectionLock(0) +{ + dgAssert (m_rtti | dgCollisionCompound_RTTI); + + dgInt32 count; + deserialization (userData, &count, sizeof (count)); + BeginAddRemove (); + for (dgInt32 i = 0; i < count; i ++) { + dgCollisionInstance* const collision = new (world->GetAllocator()) dgCollisionInstance (world, deserialization, userData, revisionNumber); + AddCollision (collision); + collision->Release(); + } + EndAddRemove(); +} + + +dgCollisionCompound::~dgCollisionCompound() +{ + if (m_root) { + delete m_root; + } +} + +void dgCollisionCompound::SetParent (const dgCollisionInstance* const instance) +{ + m_myInstance = instance; +} + +void dgCollisionCompound::SetCollisionBBox (const dgVector& p0__, const dgVector& p1__) +{ + dgAssert (0); +} + +void dgCollisionCompound::GetAABB (dgVector& boxMin, dgVector& boxMax) const +{ + if (m_root) { + boxMin = m_root->m_p0; + boxMax = m_root->m_p1; + } else { + boxMin = dgVector (dgFloat32 (0.0f)); + boxMax = dgVector (dgFloat32 (0.0f)); + } +} + + +dgInt32 dgCollisionCompound::CalculateSignature () const +{ + dgAssert (0); + return 0; +} + + + +void dgCollisionCompound::CalcAABB (const dgMatrix& matrix, dgVector& p0, dgVector& p1) const +{ + if (m_root) { + dgVector origin (matrix.TransformVector(m_root->m_origin)); + dgVector size (matrix.m_front.Abs().Scale(m_root->m_size.m_x) + matrix.m_up.Abs().Scale(m_root->m_size.m_y) + matrix.m_right.Abs().Scale(m_root->m_size.m_z)); + + size -= dgCollisionInstance::m_padding; + p0 = (origin - size) & dgVector::m_triplexMask; + p1 = (origin + size) & dgVector::m_triplexMask; + } else { + p0 = dgVector (dgFloat32 (0.0f)); + p1 = dgVector (dgFloat32 (0.0f)); + } +} + +dgInt32 dgCollisionCompound::CalculatePlaneIntersection (const dgVector& normal, const dgVector& point, dgVector* const contactsOut) const +{ + return 0; +} + +void dgCollisionCompound::DebugCollision (const dgMatrix& matrix, dgCollision::OnDebugCollisionMeshCallback callback, void* const userData) const +{ + dgTreeArray::Iterator iter (m_array); + for (iter.Begin(); iter; iter ++) { + dgCollisionInstance* const collision = iter.GetNode()->GetInfo()->GetShape(); + collision->DebugCollision (matrix, callback, userData); + } +} + +dgFloat32 dgCollisionCompound::RayCast (const dgVector& localP0, const dgVector& localP1, dgFloat32 maxT, dgContactPoint& contactOut, const dgBody* const body, void* const userData, OnRayPrecastAction preFilter) const +{ + if (!m_root) { + return dgFloat32 (1.2f); + } + + dgFloat32 distance[DG_COMPOUND_STACK_DEPTH]; + const dgNodeBase* stackPool[DG_COMPOUND_STACK_DEPTH]; + +// dgFloat32 maxParam = maxT; + dgFastRayTest ray (localP0, localP1); + + dgInt32 stack = 1; + stackPool[0] = m_root; + distance[0] = ray.BoxIntersect(m_root->m_p0, m_root->m_p1); + while (stack) { + stack --; + dgFloat32 dist = distance[stack]; + + if (dist > maxT) { + break; + } else { + const dgNodeBase* const me = stackPool[stack]; + dgAssert (me); + if (me->m_type == m_leaf) { + dgContactPoint tmpContactOut; + dgCollisionInstance* const shape = me->GetShape(); + dgVector p0 (shape->GetLocalMatrix().UntransformVector (localP0)); + dgVector p1 (shape->GetLocalMatrix().UntransformVector (localP1)); + dgFloat32 param = shape->RayCast (p0, p1, maxT, tmpContactOut, preFilter, body, userData); + if (param < maxT) { + maxT = param; + contactOut.m_normal = shape->GetLocalMatrix().RotateVector (tmpContactOut.m_normal); + contactOut.m_shapeId0 = tmpContactOut.m_shapeId0; + contactOut.m_shapeId1 = tmpContactOut.m_shapeId0; + contactOut.m_collision0 = tmpContactOut.m_collision0; + contactOut.m_collision1 = tmpContactOut.m_collision1; + } + + } else { + dgAssert (me->m_type == m_node); + const dgNodeBase* const left = me->m_left; + dgAssert (left); + dgFloat32 dist1 = ray.BoxIntersect(left->m_p0, left->m_p1); + if (dist1 < maxT) { + dgInt32 j = stack; + for ( ; j && (dist1 > distance[j - 1]); j --) { + stackPool[j] = stackPool[j - 1]; + distance[j] = distance[j - 1]; + } + stackPool[j] = left; + distance[j] = dist1; + stack++; + dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (stackPool[0]))); + } + + const dgNodeBase* const right = me->m_right; + dgAssert (right); + dist1 = ray.BoxIntersect(right->m_p0, right->m_p1); + if (dist1 < maxT) { + dgInt32 j = stack; + for ( ; j && (dist1 > distance[j - 1]); j --) { + stackPool[j] = stackPool[j - 1]; + distance[j] = distance[j - 1]; + } + stackPool[j] = right; + distance[j] = dist1; + stack++; + dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (stackPool[0]))); + } + } + } + } + return maxT; +} + +dgFloat32 dgCollisionCompound::GetVolume () const +{ + dgFloat32 volume = dgFloat32 (0.0f); + dgTreeArray::Iterator iter (m_array); + for (iter.Begin(); iter; iter ++) { + dgCollisionConvex* const collision = (dgCollisionConvex*)iter.GetNode()->GetInfo()->GetShape()->GetChildShape(); + volume += collision->GetVolume(); + } + return volume; +} + +dgFloat32 dgCollisionCompound::GetBoxMinRadius () const +{ + return m_boxMinRadius; +} + +dgFloat32 dgCollisionCompound::GetBoxMaxRadius () const +{ + return m_boxMaxRadius; +} + + + +dgVector dgCollisionCompound::CalculateVolumeIntegral (const dgMatrix& globalMatrix, const dgVector& plane, const dgCollisionInstance& parentScale) const +{ + dgVector totalVolume (dgVector::m_zero); + dgTreeArray::Iterator iter (m_array); + for (iter.Begin(); iter; iter ++) { + const dgCollisionInstance* const childInstance = iter.GetNode()->GetInfo()->GetShape(); + dgCollisionConvex* const collision = (dgCollisionConvex*)childInstance->GetChildShape(); + dgMatrix matrix (childInstance->m_localMatrix * globalMatrix); + dgVector vol (collision->CalculateVolumeIntegral (matrix, plane, *childInstance)); + totalVolume.m_x += vol.m_x * vol.m_w; + totalVolume.m_y += vol.m_y * vol.m_w; + totalVolume.m_z += vol.m_z * vol.m_w; + totalVolume.m_w += vol.m_w; + } + + dgFloat32 scale = dgFloat32 (0.0f); + if (m_root) { + scale = dgFloat32 (1.0f) / (totalVolume.m_w + dgFloat32 (1.0e-6f)); + } + totalVolume.m_x *= scale; + totalVolume.m_y *= scale; + totalVolume.m_z *= scale; + + return totalVolume; +} + +dgFloat32 dgCollisionCompound::CalculateMassProperties (const dgMatrix& offset, dgVector& inertia, dgVector& crossInertia, dgVector& centerOfMass) const +{ + dgPolyhedraMassProperties localData; + DebugCollision (offset, CalculateInertia, &localData); + return localData.MassProperties (centerOfMass, inertia, crossInertia); +} + + +dgMatrix dgCollisionCompound::CalculateInertiaAndCenterOfMass (const dgMatrix& m_alignMatrix, const dgVector& localScale, const dgMatrix& matrix) const +{ + dgVector inertiaII; + dgVector crossInertia; + dgVector centerOfMass; + dgMatrix scaledMatrix(matrix); + scaledMatrix[0] = scaledMatrix[0].Scale(localScale.m_x); + scaledMatrix[1] = scaledMatrix[1].Scale(localScale.m_y); + scaledMatrix[2] = scaledMatrix[2].Scale(localScale.m_z); + scaledMatrix = m_alignMatrix * scaledMatrix; + + dgFloat32 volume = CalculateMassProperties (scaledMatrix, inertiaII, crossInertia, centerOfMass); + if (volume < DG_MAX_MIN_VOLUME) { + volume = DG_MAX_MIN_VOLUME; + } + + dgFloat32 invVolume = dgFloat32 (1.0f) / volume; + centerOfMass = centerOfMass.Scale(invVolume); + inertiaII = inertiaII.Scale (invVolume); + crossInertia = crossInertia.Scale (invVolume); + dgMatrix inertia (dgGetIdentityMatrix()); + inertia[0][0] = inertiaII[0]; + inertia[1][1] = inertiaII[1]; + inertia[2][2] = inertiaII[2]; + inertia[0][1] = crossInertia[2]; + inertia[1][0] = crossInertia[2]; + inertia[0][2] = crossInertia[1]; + inertia[2][0] = crossInertia[1]; + inertia[1][2] = crossInertia[0]; + inertia[2][1] = crossInertia[0]; + inertia[3] = centerOfMass; + return inertia; +} + + +void dgCollisionCompound::CalculateInertia (void* userData, int indexCount, const dgFloat32* const faceVertex, int faceId) +{ + dgPolyhedraMassProperties& localData = *((dgPolyhedraMassProperties*) userData); + localData.AddInertiaAndCrossFace(indexCount, faceVertex); +} + + +void dgCollisionCompound::MassProperties () +{ +#ifdef _DEBUG +// dgVector origin_ (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f)); +// dgVector inertia_ (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f)); +// dgVector crossInertia_ (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f)); +// dgPolyhedraMassProperties localData; +// DebugCollision (dgGetIdentityMatrix(), CalculateInertia, &localData); +// dgFloat32 volume_ = localData.MassProperties (origin_, inertia_, crossInertia_); +// dgAssert (volume_ > dgFloat32 (0.0f)); +// dgFloat32 invVolume_ = dgFloat32 (1.0f)/volume_; +// m_centerOfMass = origin_.Scale (invVolume_); +// m_centerOfMass.m_w = volume_; +// m_inertia = inertia_.Scale (invVolume_); +// m_crossInertia = crossInertia_.Scale(invVolume_); +#endif + + + dgFloat32 volume = dgFloat32 (0.0f); + dgVector origin (dgFloat32 (0.0f)); + dgVector inertiaII (dgFloat32 (0.0f)); + dgVector inertiaIJ (dgFloat32 (0.0f)); + dgTreeArray::Iterator iter (m_array); + for (iter.Begin(); iter; iter ++) { + dgCollisionInstance* const collision = iter.GetNode()->GetInfo()->GetShape(); + dgMatrix shapeInertia (collision->CalculateInertia()); + dgFloat32 shapeVolume = collision->GetVolume(); + + volume += shapeVolume; + origin += shapeInertia.m_posit.Scale(shapeVolume); + inertiaII += dgVector (shapeInertia[0][0], shapeInertia[1][1], shapeInertia[2][2], dgFloat32 (0.0f)).Scale (shapeVolume); + inertiaIJ += dgVector (shapeInertia[1][2], shapeInertia[0][2], shapeInertia[0][1], dgFloat32 (0.0f)).Scale (shapeVolume); + } + if (volume > dgFloat32 (0.0f)) { + dgFloat32 invVolume = dgFloat32 (1.0f)/volume; + m_inertia = inertiaII.Scale (invVolume); + m_crossInertia = inertiaIJ.Scale (invVolume); + m_centerOfMass = origin.Scale (invVolume); + m_centerOfMass.m_w = volume; + } + + dgCollision::MassProperties (); +} + +void dgCollisionCompound::ApplyScale (const dgVector& scale) +{ + dgTreeArray::Iterator iter (m_array); + for (iter.Begin(); iter; iter ++) { + dgNodeBase* const node = iter.GetNode()->GetInfo(); + dgCollisionInstance* const collision = node->GetShape(); + collision->SetGlobalScale (scale); + } + m_treeEntropy = dgFloat32 (0.0f); + EndAddRemove (); +} + + +void dgCollisionCompound::BeginAddRemove () +{ +} + + +dgCollisionCompound::dgNodeBase* dgCollisionCompound::BuildTopDown (dgNodeBase** const leafArray, dgInt32 firstBox, dgInt32 lastBox, dgList::dgListNode** const nextNode) +{ + dgAssert (firstBox >= 0); + dgAssert (lastBox >= 0); + + if (lastBox == firstBox) { + return leafArray[firstBox]; + } else { + dgSpliteInfo info (&leafArray[firstBox], lastBox - firstBox + 1); + + dgNodeBase* const parent = (*nextNode)->GetInfo(); + parent->m_parent = NULL; + *nextNode = (*nextNode)->GetNext(); + + parent->SetBox (info.m_p0, info.m_p1); + parent->m_right = BuildTopDown (leafArray, firstBox + info.m_axis, lastBox, nextNode); + parent->m_right->m_parent = parent; + + parent->m_left = BuildTopDown (leafArray, firstBox, firstBox + info.m_axis - 1, nextNode); + parent->m_left->m_parent = parent; + return parent; + } +} + +dgCollisionCompound::dgNodeBase* dgCollisionCompound::BuildTopDownBig (dgNodeBase** const leafArray, dgInt32 firstBox, dgInt32 lastBox, dgList::dgListNode** const nextNode) +{ + if (lastBox == firstBox) { + return BuildTopDown (leafArray, firstBox, lastBox, nextNode); + } + + dgInt32 midPoint = -1; + const dgFloat32 scale = dgFloat32 (10.0f); + const dgFloat32 scale2 = dgFloat32 (3.0f) * scale * scale; + const dgInt32 count = lastBox - firstBox; + for (dgInt32 i = 0; i < count; i ++) { + const dgNodeBase* const node0 = leafArray[firstBox + i]; + const dgNodeBase* const node1 = leafArray[firstBox + i + 1]; + if (node1->m_area > (scale2 * node0->m_area)) { + midPoint = i; + break; + } + } + + if (midPoint == -1) { + return BuildTopDown (leafArray, firstBox, lastBox, nextNode); + } else { + dgNodeBase* const parent = (*nextNode)->GetInfo(); + + parent->m_parent = NULL; + *nextNode = (*nextNode)->GetNext(); + + dgVector minP ( dgFloat32 (1.0e15f)); + dgVector maxP (-dgFloat32 (1.0e15f)); + for (dgInt32 i = 0; i <= count; i ++) { + const dgNodeBase* const node = leafArray[firstBox + i]; + dgAssert (node->m_shape); + minP = minP.GetMin (node->m_p0); + maxP = maxP.GetMax (node->m_p1); + } + + parent->SetBox (minP, maxP); + parent->m_left = BuildTopDown (leafArray, firstBox, firstBox + midPoint, nextNode); + parent->m_left->m_parent = parent; + + parent->m_right = BuildTopDownBig (leafArray, firstBox + midPoint + 1, lastBox, nextNode); + parent->m_right->m_parent = parent; + return parent; + } +} + +dgInt32 dgCollisionCompound::CompareNodes (const dgNodeBase* const nodeA, const dgNodeBase* const nodeB, void* ) +{ + dgFloat32 areaA = nodeA->m_area; + dgFloat32 areaB = nodeB->m_area; + if (areaA < areaB) { + return -1; + } + if (areaA > areaB) { + return 1; + } + return 0; +} + + +dgFloat64 dgCollisionCompound::CalculateEntropy (dgList& list) +{ + dgFloat64 cost0 = dgFloat32 (1.0e20f); + dgFloat64 cost1 = cost0; + do { + cost1 = cost0; + for (dgList::dgListNode* listNode = list.GetFirst(); listNode; listNode = listNode->GetNext()) { + dgNodeBase* const node = listNode->GetInfo(); + ImproveNodeFitness (node); + } + + cost0 = dgFloat32 (0.0f); + for (dgList::dgListNode* listNode = list.GetFirst(); listNode; listNode = listNode->GetNext()) { + dgNodeBase* const node = listNode->GetInfo(); + cost0 += node->m_area; + } + } while (cost0 < (cost1 * dgFloat32 (0.9999f))); + return cost0; +} + +void dgCollisionCompound::EndAddRemove (bool flushCache) +{ + if (m_root) { + //dgWorld* const world = m_world; + //dgThreadHiveScopeLock lock (world, &m_criticalSectionLock); + dgScopeSpinLock lock(&m_criticalSectionLock); + + dgTreeArray::Iterator iter (m_array); + for (iter.Begin(); iter; iter ++) { + dgNodeBase* const node = iter.GetNode()->GetInfo(); + node->CalculateAABB(); + } + + dgList list (GetAllocator()); + dgList stack (GetAllocator()); + stack.Append(m_root); + while (stack.GetCount()) { + dgList::dgListNode* const stackNode = stack.GetLast(); + dgNodeBase* const node = stackNode->GetInfo(); + stack.Remove(stackNode); + + //if (node->m_type == m_node) { + // list.Append(node); + //} + + if (node->m_type == m_node) { + list.Append(node); + stack.Append(node->m_right); + stack.Append(node->m_left); + } + } + + if (list.GetCount()) { + dgFloat64 cost = CalculateEntropy (list); + if ((cost > m_treeEntropy * dgFloat32 (2.0f)) || (cost < m_treeEntropy * dgFloat32 (0.5f))) { + dgInt32 count = list.GetCount() * 2 + 12; + dgInt32 leafNodesCount = 0; + dgStack leafArray(count); + for (dgList::dgListNode* listNode = list.GetFirst(); listNode; listNode = listNode->GetNext()) { + dgNodeBase* const node = listNode->GetInfo(); + if (node->m_left->m_type == m_leaf) { + leafArray[leafNodesCount] = node->m_left; + leafNodesCount ++; + } + if (node->m_right->m_type == m_leaf) { + leafArray[leafNodesCount] = node->m_right; + leafNodesCount ++; + } + } + + dgList::dgListNode* nodePtr = list.GetFirst(); + + dgSortIndirect (&leafArray[0], leafNodesCount, CompareNodes); + + m_root = BuildTopDownBig (&leafArray[0], 0, leafNodesCount - 1, &nodePtr); + m_treeEntropy = CalculateEntropy (list); + } + while (m_root->m_parent) { + m_root = m_root->m_parent; + } + } else { + m_treeEntropy = dgFloat32 (2.0f); + } + + dgAssert (m_root->m_size.m_w == dgFloat32 (0.0f)); + m_boxMinRadius = dgMin(m_root->m_size.m_x, m_root->m_size.m_y, m_root->m_size.m_z); + m_boxMaxRadius = dgSqrt (m_root->m_size.DotProduct(m_root->m_size).GetScalar()); + + m_boxSize = m_root->m_size; + m_boxOrigin = m_root->m_origin; + MassProperties (); + + if (flushCache) { + m_world->FlushCache (); + } + } +} + +dgTree::dgTreeNode* dgCollisionCompound::AddCollision (dgCollisionInstance* const shape) +{ + dgNodeBase* const newNode = new (m_allocator) dgNodeBase (shape); + m_array.AddNode(newNode, m_idIndex, m_myInstance); + + m_idIndex ++; + + if (!m_root) { + m_root = newNode; + } else { + dgVector p0; + dgVector p1; + dgNodeBase* sibling = m_root; + dgFloat32 surfaceArea = CalculateSurfaceArea (newNode, sibling, p0, p1); + while(sibling->m_left && sibling->m_right) { + + if (surfaceArea > sibling->m_area) { + break; + } + + sibling->SetBox (p0, p1); + + dgVector leftP0; + dgVector leftP1; + dgFloat32 leftSurfaceArea = CalculateSurfaceArea (newNode, sibling->m_left, leftP0, leftP1); + + dgVector rightP0; + dgVector rightP1; + dgFloat32 rightSurfaceArea = CalculateSurfaceArea (newNode, sibling->m_right, rightP0, rightP1); + + if (leftSurfaceArea < rightSurfaceArea) { + sibling = sibling->m_left; + p0 = leftP0; + p1 = leftP1; + surfaceArea = leftSurfaceArea; + } else { + sibling = sibling->m_right; + p0 = rightP0; + p1 = rightP1; + surfaceArea = rightSurfaceArea; + } + } + + if (!sibling->m_parent) { + m_root = new (m_world->GetAllocator()) dgNodeBase (sibling, newNode); + } else { + dgNodeBase* const parent = sibling->m_parent; + if (parent->m_left == sibling) { + dgNodeBase* const node = new (m_world->GetAllocator()) dgNodeBase (sibling, newNode); + parent->m_left = node; + node->m_parent = parent; + } else { + dgAssert (parent->m_right == sibling); + dgNodeBase* const node = new (m_world->GetAllocator()) dgNodeBase (sibling, newNode); + parent->m_right = node; + node->m_parent = parent; + } + } + } + + return newNode->m_myNode; +} + + +void dgCollisionCompound::RemoveCollision (dgTreeArray::dgTreeNode* const node) +{ + if (node) { + dgCollisionInstance* const instance = node->GetInfo()->GetShape(); + instance->AddRef(); + RemoveCollision (node->GetInfo()); + instance->Release(); + m_array.Remove(node); + } +} + +void dgCollisionCompound::SetCollisionMatrix (dgTreeArray::dgTreeNode* const node, const dgMatrix& matrix) +{ + if (node) { + + //dgWorld* const world = m_world; + dgNodeBase* const baseNode = node->GetInfo(); + dgCollisionInstance* const instance = baseNode->GetShape(); + + dgVector scale; + dgMatrix localMatrix; + matrix.PolarDecomposition(localMatrix, scale, instance->m_aligmentMatrix); + +// instance->SetLocalMatrix(matrix); + instance->SetLocalMatrix(localMatrix); + instance->SetScale(scale); + + dgVector p0; + dgVector p1; + instance->CalcAABB(instance->GetLocalMatrix (), p0, p1); + + //dgThreadHiveScopeLock lock (world, &m_criticalSectionLock); + dgScopeSpinLock lock(&m_criticalSectionLock); + + baseNode->SetBox (p0, p1); + for (dgNodeBase* parent = baseNode->m_parent; parent; parent = parent->m_parent) { + dgVector minBox; + dgVector maxBox; + CalculateSurfaceArea (parent->m_left, parent->m_right, minBox, maxBox); + if (dgBoxInclusionTest (minBox, maxBox, parent->m_p0, parent->m_p1)) { + break; + } + parent->SetBox (minBox, maxBox); + } + } +} + + +void dgCollisionCompound::RemoveCollision (dgNodeBase* const treeNode) +{ + if (!treeNode->m_parent) { + delete (m_root); + m_root = NULL; + } else if (!treeNode->m_parent->m_parent) { + dgNodeBase* const root = m_root; + if (treeNode->m_parent->m_left == treeNode) { + m_root = treeNode->m_parent->m_right; + treeNode->m_parent->m_right = NULL; + } else { + dgAssert (treeNode->m_parent->m_right == treeNode); + m_root = treeNode->m_parent->m_left; + treeNode->m_parent->m_left= NULL; + } + m_root->m_parent = NULL; + delete (root); + + } else { + dgNodeBase* const root = treeNode->m_parent->m_parent; + if (treeNode->m_parent == root->m_left) { + if (treeNode->m_parent->m_right == treeNode) { + root->m_left = treeNode->m_parent->m_left; + treeNode->m_parent->m_left = NULL; + } else { + dgAssert (treeNode->m_parent->m_left == treeNode); + root->m_left = treeNode->m_parent->m_right; + treeNode->m_parent->m_right = NULL; + } + root->m_left->m_parent = root; + } else { + if (treeNode->m_parent->m_right == treeNode) { + root->m_right = treeNode->m_parent->m_left; + treeNode->m_parent->m_left = NULL; + } else { + dgAssert (treeNode->m_parent->m_left == treeNode); + root->m_right = treeNode->m_parent->m_right; + treeNode->m_parent->m_right = NULL; + } + root->m_right->m_parent = root; + } + delete (treeNode->m_parent); + } +} + +dgInt32 dgCollisionCompound::GetNodeIndex(dgTreeArray::dgTreeNode* const node) const +{ + return node->GetKey(); +} + +dgTree::dgTreeNode* dgCollisionCompound::FindNodeByIndex (dgInt32 index) const +{ + return m_array.Find (index); +} + +dgTree::dgTreeNode* dgCollisionCompound::GetFirstNode () const +{ + dgTreeArray::Iterator iter (m_array); + iter.Begin(); + + dgTreeArray::dgTreeNode* node = NULL; + if (iter) { + node = iter.GetNode(); + } + return node; +} + +dgTree::dgTreeNode* dgCollisionCompound::GetNextNode (dgTree::dgTreeNode* const node) const +{ + dgTreeArray::Iterator iter (m_array); + iter.Set (node); + iter ++; + dgTreeArray::dgTreeNode* nextNode = NULL; + if (iter) { + nextNode = iter.GetNode(); + } + return nextNode; +} + +dgCollisionInstance* dgCollisionCompound::GetCollisionFromNode (dgTree::dgTreeNode* const node) const +{ + dgAssert (node->GetInfo()); + dgAssert (node->GetInfo()->GetShape()); + return node->GetInfo()->GetShape(); +} + + +dgVector dgCollisionCompound::SupportVertex (const dgVector& dir, dgInt32* const vertexIndex) const +{ + dgFloat32 aabbProjection[DG_COMPOUND_STACK_DEPTH]; + const dgNodeBase* stackPool[DG_COMPOUND_STACK_DEPTH]; + + dgInt32 stack = 1; + stackPool[0] = m_root; + aabbProjection[0] = dgFloat32 (1.0e10f); + + dgFloat32 maxProj = dgFloat32 (-1.0e20f); + //dgVector searchDir (m_offset.UnrotateVector(dir)); + + dgInt32 ix = (dir[0] > dgFloat32 (0.0f)) ? 1 : 0; + dgInt32 iy = (dir[1] > dgFloat32 (0.0f)) ? 1 : 0; + dgInt32 iz = (dir[2] > dgFloat32 (0.0f)) ? 1 : 0; + dgVector supportVertex (dgFloat32 (0.0f)); + + dgAssert (dir.m_w == dgFloat32 (0.0f)); + while (stack) { + + stack--; + dgFloat32 boxSupportValue = aabbProjection[stack]; + if (boxSupportValue > maxProj) { + const dgNodeBase* const me = stackPool[stack]; + + if (me->m_type == m_leaf) { + //dgInt32 index; + dgCollisionInstance* const subShape = me->GetShape(); + const dgMatrix& matrix = subShape->GetLocalMatrix(); + dgVector newDir (matrix.UnrotateVector(dir)); + dgVector vertex (matrix.TransformVector (subShape->SupportVertex(newDir))); + dgFloat32 dist = dir.DotProduct(vertex).GetScalar(); + if (dist > maxProj) { + maxProj = dist; + supportVertex = vertex; + } + + } else { + const dgNodeBase* const left = me->m_left; + const dgNodeBase* const right = me->m_right; + + const dgVector* const box0 = &left->m_p0; + dgVector p0 (box0[ix].m_x, box0[iy].m_y, box0[iz].m_z, dgFloat32 (0.0f)); + + const dgVector* const box1 = &right->m_p0; + dgVector p1 (box1[ix].m_x, box1[iy].m_y, box1[iz].m_z, dgFloat32 (0.0f)); + + dgFloat32 dist0 = p0.DotProduct(dir).GetScalar(); + dgFloat32 dist1 = p1.DotProduct(dir).GetScalar(); + if (dist0 > dist1) { + stackPool[stack] = right; + aabbProjection[stack] = dist1; + stack ++; + + stackPool[stack] = left; + aabbProjection[stack] = dist0; + stack ++; + } else { + stackPool[stack] = left; + aabbProjection[stack] = dist0; + stack ++; + + stackPool[stack] = right; + aabbProjection[stack] = dist1; + stack ++; + } + } + } + } + +// return m_offset.TransformVector (supportVertex); + return supportVertex; +} + +dgVector dgCollisionCompound::SupportVertexSpecial (const dgVector& dir, dgFloat32 skinThickness, dgInt32* const vertexIndex) const +{ + dgAssert (0); + return SupportVertex (dir, vertexIndex); +} + + +void dgCollisionCompound::GetCollisionInfo(dgCollisionInfo* const info) const +{ + dgCollision::GetCollisionInfo(info); + + info->m_compoundCollision.m_chidrenCount = m_array.GetCount(); + info->m_collisionType = m_compoundCollision; +} + +void dgCollisionCompound::Serialize(dgSerialize callback, void* const userData) const +{ + SerializeLow(callback, userData); + + dgInt32 count = m_array.GetCount(); + callback (userData, &count, sizeof (count)); + dgTreeArray::Iterator iter (m_array); + for (iter.Begin(); iter; iter ++) { + dgCollisionInstance* const collision = iter.GetNode()->GetInfo()->GetShape(); + collision->Serialize(callback, userData); + } +} + +DG_INLINE dgFloat32 dgCollisionCompound::CalculateSurfaceArea (dgNodeBase* const node0, dgNodeBase* const node1, dgVector& minBox, dgVector& maxBox) const +{ + minBox = node0->m_p0.GetMin(node1->m_p0); + maxBox = node0->m_p1.GetMax(node1->m_p1); + dgVector side0 (dgVector::m_half * (maxBox - minBox)); + return side0.DotProduct(side0.ShiftTripleRight()).GetScalar(); +} + +void dgCollisionCompound::ImproveNodeFitness (dgNodeBase* const node) const +{ + dgAssert (node->m_left); + dgAssert (node->m_right); + + if (node->m_parent) { + if (node->m_parent->m_left == node) { + dgFloat32 cost0 = node->m_area; + + dgVector cost1P0; + dgVector cost1P1; + dgFloat32 cost1 = CalculateSurfaceArea (node->m_right, node->m_parent->m_right, cost1P0, cost1P1); + + dgVector cost2P0; + dgVector cost2P1; + dgFloat32 cost2 = CalculateSurfaceArea (node->m_left, node->m_parent->m_right, cost2P0, cost2P1); + + dgAssert (node->m_parent->m_p0.m_w == dgFloat32 (0.0f)); + dgAssert (node->m_parent->m_p1.m_w == dgFloat32 (0.0f)); + + if ((cost1 <= cost0) && (cost1 <= cost2)) { + dgNodeBase* const parent = node->m_parent; + node->m_p0 = parent->m_p0; + node->m_p1 = parent->m_p1; + node->m_area = parent->m_area; + node->m_size = parent->m_size; + node->m_origin = parent->m_origin; + + if (parent->m_parent) { + if (parent->m_parent->m_left == parent) { + parent->m_parent->m_left = node; + } else { + dgAssert (parent->m_parent->m_right == parent); + parent->m_parent->m_right = node; + } + } + node->m_parent = parent->m_parent; + parent->m_parent = node; + node->m_right->m_parent = parent; + parent->m_left = node->m_right; + node->m_right = parent; + parent->m_p0 = cost1P0; + parent->m_p1 = cost1P1; + parent->m_area = cost1; + parent->m_size = (parent->m_p1 - parent->m_p0) * dgVector::m_half; + parent->m_origin = (parent->m_p1 + parent->m_p0) * dgVector::m_half; + + } else if ((cost2 <= cost0) && (cost2 <= cost1)) { + dgNodeBase* const parent = node->m_parent; + node->m_p0 = parent->m_p0; + node->m_p1 = parent->m_p1; + node->m_area = parent->m_area; + node->m_size = parent->m_size; + node->m_origin = parent->m_origin; + + if (parent->m_parent) { + if (parent->m_parent->m_left == parent) { + parent->m_parent->m_left = node; + } else { + dgAssert (parent->m_parent->m_right == parent); + parent->m_parent->m_right = node; + } + } + node->m_parent = parent->m_parent; + parent->m_parent = node; + node->m_left->m_parent = parent; + parent->m_left = node->m_left; + node->m_left = parent; + + parent->m_p0 = cost2P0; + parent->m_p1 = cost2P1; + parent->m_area = cost2; + parent->m_size = (parent->m_p1 - parent->m_p0) * dgVector::m_half; + parent->m_origin = (parent->m_p1 + parent->m_p0) * dgVector::m_half; + } + } else { + dgFloat32 cost0 = node->m_area; + + dgVector cost1P0; + dgVector cost1P1; + dgFloat32 cost1 = CalculateSurfaceArea (node->m_left, node->m_parent->m_left, cost1P0, cost1P1); + + dgVector cost2P0; + dgVector cost2P1; + dgFloat32 cost2 = CalculateSurfaceArea (node->m_right, node->m_parent->m_left, cost2P0, cost2P1); + + if ((cost1 <= cost0) && (cost1 <= cost2)) { + + dgNodeBase* const parent = node->m_parent; + node->m_p0 = parent->m_p0; + node->m_p1 = parent->m_p1; + node->m_area = parent->m_area; + node->m_size = parent->m_size; + node->m_origin = parent->m_origin; + + if (parent->m_parent) { + if (parent->m_parent->m_left == parent) { + parent->m_parent->m_left = node; + } else { + dgAssert (parent->m_parent->m_right == parent); + parent->m_parent->m_right = node; + } + } + node->m_parent = parent->m_parent; + parent->m_parent = node; + node->m_left->m_parent = parent; + parent->m_right = node->m_left; + node->m_left = parent; + + parent->m_p0 = cost1P0; + parent->m_p1 = cost1P1; + parent->m_area = cost1; + parent->m_size = (parent->m_p1 - parent->m_p0) * dgVector::m_half; + parent->m_origin = (parent->m_p1 + parent->m_p0) * dgVector::m_half; + + } else if ((cost2 <= cost0) && (cost2 <= cost1)) { + dgNodeBase* const parent = node->m_parent; + node->m_p0 = parent->m_p0; + node->m_p1 = parent->m_p1; + node->m_area = parent->m_area; + node->m_size = parent->m_size; + node->m_origin = parent->m_origin; + + if (parent->m_parent) { + if (parent->m_parent->m_left == parent) { + parent->m_parent->m_left = node; + } else { + dgAssert (parent->m_parent->m_right == parent); + parent->m_parent->m_right = node; + } + } + node->m_parent = parent->m_parent; + parent->m_parent = node; + node->m_right->m_parent = parent; + parent->m_right = node->m_right; + node->m_right = parent; + + parent->m_p0 = cost2P0; + parent->m_p1 = cost2P1; + parent->m_area = cost2; + parent->m_size = (parent->m_p1 - parent->m_p0) * dgVector::m_half; + parent->m_origin = (parent->m_p1 + parent->m_p0) * dgVector::m_half; + } + } + } else { + // in the future I can handle this but it is too much work for little payoff + } +} + + + +dgInt32 dgCollisionCompound::CalculateContacts (dgBroadPhase::dgPair* const pair, dgCollisionParamProxy& proxy) const +{ + dgInt32 contactCount = 0; + if (m_root) { + dgAssert (IsType (dgCollision::dgCollisionCompound_RTTI)); + dgContact* const constraint = pair->m_contact; + dgBody* const body1 = constraint->GetBody1(); + + if (proxy.m_continueCollision) { + if (body1->m_collision->IsType (dgCollision::dgCollisionConvexShape_RTTI)) { + contactCount = CalculateContactsToSingleContinue (pair, proxy); + } else if (body1->m_collision->IsType (dgCollision::dgCollisionCompound_RTTI)) { + contactCount = CalculateContactsToCompoundContinue (pair, proxy); + } else if (body1->m_collision->IsType (dgCollision::dgCollisionBVH_RTTI)) { + contactCount = CalculateContactsToCollisionTreeContinue (pair, proxy); + } else if (body1->m_collision->IsType (dgCollision::dgCollisionHeightField_RTTI)) { + dgAssert (0); +// contactCount = CalculateContactsToHeightField (pair, proxy); + } else { + dgAssert (0); + dgAssert (body1->m_collision->IsType (dgCollision::dgCollisionUserMesh_RTTI)); +// contactCount = CalculateContactsUserDefinedCollision (pair, proxy); + } + + } else { + if (body1->m_collision->IsType (dgCollision::dgCollisionConvexShape_RTTI)) { + contactCount = CalculateContactsToSingle (pair, proxy); + } else if (body1->m_collision->IsType (dgCollision::dgCollisionCompound_RTTI)) { + contactCount = CalculateContactsToCompound (pair, proxy); + } else if (body1->m_collision->IsType (dgCollision::dgCollisionBVH_RTTI)) { + contactCount = CalculateContactsToCollisionTree (pair, proxy); + } else if (body1->m_collision->IsType (dgCollision::dgCollisionHeightField_RTTI)) { + contactCount = CalculateContactsToHeightField (pair, proxy); + } else { + dgAssert (body1->m_collision->IsType (dgCollision::dgCollisionUserMesh_RTTI)); + contactCount = CalculateContactsUserDefinedCollision (pair, proxy); + } + } + } + pair->m_contactCount = contactCount; + return contactCount; +} + + +dgInt32 dgCollisionCompound::ClosestDistance (dgCollisionParamProxy& proxy) const +{ + int count = 0; + if (m_root) { + if (proxy.m_instance1->IsType (dgCollision::dgCollisionConvexShape_RTTI)) { + count = ClosestDistanceToConvex (proxy); + } else if (proxy.m_instance1->IsType (dgCollision::dgCollisionCompound_RTTI)) { + count = ClosestDistanceToCompound (proxy); + } else if (proxy.m_instance1->IsType (dgCollision::dgCollisionBVH_RTTI)) { + dgAssert(0); + } else { + dgAssert(0); + } + } + + return count; +} + + +dgInt32 dgCollisionCompound::ClosestDistanceToConvex (dgCollisionParamProxy& proxy) const +{ + dgInt32 retFlag = 0; + + dgCollisionInstance* const compoundInstance = proxy.m_instance0; + dgCollisionInstance* const otherInstance = proxy.m_instance1; + + const dgMatrix myMatrix = compoundInstance->GetGlobalMatrix(); + dgMatrix matrix (otherInstance->GetGlobalMatrix() * myMatrix.Inverse()); + + dgVector p0; + dgVector p1; + otherInstance->CalcAABB(matrix, p0, p1); + + dgUnsigned8 pool[64 * (sizeof (dgNodeBase*) + sizeof (dgFloat32))]; + dgUpHeap heap (pool, sizeof (pool)); + + dgNodeBase* node = m_root; + dgVector boxP0 (p0 - m_root->m_p1); + dgVector boxP1 (p1 - m_root->m_p0); + heap.Push(node, dgBoxDistanceToOrigin2 (boxP0, boxP1)); + + dgContactPoint contact0; + dgContactPoint contact1; + dgFloat32 minDist2 = dgFloat32 (1.0e10f); + while (heap.GetCount() && (heap.Value() <= minDist2)) { + const dgNodeBase* const node1 = heap[0]; + heap.Pop(); + if (node1->m_type == m_leaf) { + dgCollisionInstance* const subShape = node1->GetShape(); + dgCollisionInstance childInstance (*subShape, subShape->GetChildShape()); + + childInstance.m_globalMatrix = childInstance.GetLocalMatrix() * myMatrix; + proxy.m_instance0 = &childInstance; + dgInt32 flag = m_world->ClosestPoint (proxy); + + childInstance.m_material.m_userData = NULL; + + if (flag) { + retFlag = 1; + dgFloat32 dist2 = proxy.m_contactJoint->m_closestDistance * proxy.m_contactJoint->m_closestDistance; + if (dist2 < minDist2) { + minDist2 = dist2; + contact0 = proxy.m_contacts[0]; + contact1 = proxy.m_contacts[1]; + } + } else { + break; + } + + } else { + dgNodeBase* left = node1->m_left; + dgNodeBase* right = node1->m_right; + + dgVector leftBoxP0 (p0 - left->m_p1); + dgVector leftBoxP1 (p1 - left->m_p0); + heap.Push(left, dgBoxDistanceToOrigin2 (leftBoxP0, leftBoxP1)); + + dgVector rightBoxP0 (p0 - right->m_p1); + dgVector rightBoxP1 (p1 - right->m_p0); + heap.Push(right, dgBoxDistanceToOrigin2 (rightBoxP0, rightBoxP1)); + } + } + + if (retFlag) { + proxy.m_contacts[0] = contact0; + proxy.m_contacts[1] = contact1; + proxy.m_contactJoint->m_closestDistance = dgSqrt (minDist2); + } + return retFlag; +} + +DG_INLINE void dgCollisionCompound::PushNode (const dgMatrix& matrix, dgUpHeap& heap, dgNodeBase* const myNode, dgNodeBase* const otherNode) const +{ + dgVector p0; + dgVector p1; + dgHeapNodePair pair; + pair.m_nodeA = myNode; + pair.m_nodeB = otherNode; + matrix.TransformBBox (otherNode->m_p0, otherNode->m_p1, p0, p1); + dgVector boxP0 (p0 - myNode->m_p1); + dgVector boxP1 (p1 - myNode->m_p0); + heap.Push(pair, dgBoxDistanceToOrigin2(boxP0, boxP1)); +} + +dgInt32 dgCollisionCompound::ClosestDistanceToCompound (dgCollisionParamProxy& proxy) const +{ + dgInt32 retFlag = 0; + + dgUnsigned8 pool[128 * (sizeof (dgHeapNodePair) + sizeof (dgFloat32))]; + dgUpHeap heap (pool, sizeof (pool)); + + dgCollisionInstance* const compoundInstance = proxy.m_instance0; + dgCollisionInstance* const otherCompoundInstance = proxy.m_instance1; + + const dgMatrix& myMatrix = compoundInstance->GetGlobalMatrix(); + const dgMatrix& otherMatrix = otherCompoundInstance->GetGlobalMatrix(); + dgMatrix matrix (otherMatrix * myMatrix.Inverse()); + + const dgCollisionCompound* const otherShape = (dgCollisionCompound*)otherCompoundInstance->GetChildShape(); + PushNode (matrix, heap, m_root, otherShape->m_root); + + dgContactPoint contact0; + dgContactPoint contact1; + dgFloat32 minDist2 = dgFloat32 (1.0e10f); + while (heap.GetCount() && (heap.Value() <= minDist2)) { + + dgHeapNodePair pair = heap[0]; + heap.Pop(); + + if ((pair.m_nodeA->m_type == m_leaf) && (pair.m_nodeB->m_type == m_leaf)) { + dgCollisionInstance* const mySubShape = pair.m_nodeA->GetShape(); + dgCollisionInstance myChildInstance (*mySubShape, mySubShape->GetChildShape()); + myChildInstance.m_globalMatrix = myChildInstance.GetLocalMatrix() * myMatrix; + proxy.m_instance0 = &myChildInstance; + + dgCollisionInstance* const otherSubShape = pair.m_nodeB->GetShape(); + dgCollisionInstance otherChildInstance (*otherSubShape, otherSubShape->GetChildShape()); + otherChildInstance.m_globalMatrix = otherChildInstance.GetLocalMatrix() * otherMatrix; + proxy.m_instance1 = &otherChildInstance; + + dgInt32 flag = m_world->ClosestPoint (proxy); + + myChildInstance.m_material.m_userData = NULL; + otherChildInstance.m_material.m_userData = NULL; + if (flag) { + retFlag = 1; + dgFloat32 dist2 = proxy.m_contactJoint->m_closestDistance * proxy.m_contactJoint->m_closestDistance; + if (dist2 < minDist2) { + minDist2 = dist2; + contact0 = proxy.m_contacts[0]; + contact1 = proxy.m_contacts[1]; + } + } else { + break; + } + + + } else if (pair.m_nodeA->m_type == m_leaf) { + PushNode (matrix, heap, pair.m_nodeA, pair.m_nodeB->m_left); + PushNode (matrix, heap, pair.m_nodeA, pair.m_nodeB->m_right); + + } else if (pair.m_nodeB->m_type == m_leaf) { + PushNode (matrix, heap, pair.m_nodeA->m_left, pair.m_nodeB); + PushNode (matrix, heap, pair.m_nodeA->m_right, pair.m_nodeB); + + } else { + PushNode (matrix, heap, pair.m_nodeA->m_left, pair.m_nodeB->m_left); + PushNode (matrix, heap, pair.m_nodeA->m_left, pair.m_nodeB->m_right); + PushNode (matrix, heap, pair.m_nodeA->m_right, pair.m_nodeB->m_left); + PushNode (matrix, heap, pair.m_nodeA->m_right, pair.m_nodeB->m_right); + } + } + + if (retFlag) { + proxy.m_contacts[0] = contact0; + proxy.m_contacts[1] = contact1; + proxy.m_contactJoint->m_closestDistance = dgSqrt (minDist2); + } + return retFlag; +} + + + +dgInt32 dgCollisionCompound::CalculateContactsToCompound (dgBroadPhase::dgPair* const pair, dgCollisionParamProxy& proxy) const +{ + dgContactPoint* const contacts = proxy.m_contacts; + const dgNodeBase* stackPool[4 * DG_COMPOUND_STACK_DEPTH][2]; + + dgInt32 contactCount = 0; + dgContact* const contactJoint = pair->m_contact; + dgBody* const myBody = contactJoint->GetBody0(); + dgBody* const otherBody = contactJoint->GetBody1(); + + dgCollisionInstance* const myCompoundInstance = myBody->m_collision; + dgCollisionInstance* const otherCompoundInstance = otherBody->m_collision; + + dgAssert (myCompoundInstance->GetChildShape() == this); + dgAssert (otherCompoundInstance->IsType (dgCollision::dgCollisionCompound_RTTI)); + dgCollisionCompound* const otherCompound = (dgCollisionCompound*)otherCompoundInstance->GetChildShape(); + + proxy.m_body0 = myBody; + proxy.m_body1 = otherBody; + + const dgMatrix& myMatrix = myCompoundInstance->GetGlobalMatrix(); + const dgMatrix& otherMatrix = otherCompoundInstance->GetGlobalMatrix(); + dgOOBBTestData data (otherMatrix * myMatrix.Inverse()); + + dgInt32 stack = 1; + stackPool[0][0] = m_root; + stackPool[0][1] = otherCompound->m_root; + const dgContactMaterial* const material = contactJoint->GetMaterial(); + + dgAssert ((contacts != NULL) ^ proxy.m_intersectionTestOnly); + + dgFloat32 timestep = pair->m_timestep; + dgFloat32 closestDist = dgFloat32 (1.0e10f); + while (stack) { + stack --; + const dgNodeBase* const me = stackPool[stack][0]; + const dgNodeBase* const other = stackPool[stack][1]; + + dgAssert (me && other); + + if (me->BoxTest (data, other)) { + + if ((me->m_type == m_leaf) && (other->m_type == m_leaf)) { + bool processContacts = true; + if (material->m_compoundAABBOverlap) { + processContacts = material->m_compoundAABBOverlap (*contactJoint, timestep, myBody, me->m_myNode, otherBody, other->m_myNode, proxy.m_threadIndex); + } + if (processContacts) { + if (me->GetShape()->GetCollisionMode() & other->GetShape()->GetCollisionMode()) { + const dgCollisionInstance* const subShape = me->GetShape(); + const dgCollisionInstance* const otherSubShape = other->GetShape(); + + dgCollisionInstance childInstance (*subShape, subShape->GetChildShape()); + childInstance.m_globalMatrix = childInstance.GetLocalMatrix() * myMatrix; + proxy.m_instance0 = &childInstance; + + dgCollisionInstance otherChildInstance (*otherSubShape, otherSubShape->GetChildShape()); + otherChildInstance.m_globalMatrix = otherChildInstance.GetLocalMatrix() * otherMatrix; + proxy.m_instance1 = &otherChildInstance; + + proxy.m_maxContacts = DG_MAX_CONTATCS - contactCount; + proxy.m_contacts = contacts ? &contacts[contactCount] : contacts; + + dgInt32 count = m_world->CalculateConvexToConvexContacts (proxy); + closestDist = dgMin(closestDist, contactJoint->m_closestDistance); + + if (!proxy.m_intersectionTestOnly) { + for (dgInt32 i = 0; i < count; i ++) { + dgAssert (contacts[contactCount + i].m_collision0 == &childInstance); + dgAssert (contacts[contactCount + i].m_collision1 == &otherChildInstance); + contacts[contactCount + i].m_collision0 = subShape; + contacts[contactCount + i].m_collision1 = otherSubShape; + } + contactCount += count; + if (contactCount > (DG_MAX_CONTATCS - 2 * (DG_CONSTRAINT_MAX_ROWS / 3))) { + contactCount = m_world->PruneContacts(contactCount, contacts, proxy.m_contactJoint->GetPruningTolerance(), 16); + } + } else if (count == -1) { + contactCount = -1; + break; + } + childInstance.m_material.m_userData = NULL; + otherChildInstance.m_material.m_userData = NULL; + + proxy.m_instance0 = NULL; + proxy.m_instance1 = NULL; + } + } + + } else if (me->m_type == m_leaf) { + dgAssert (other->m_type == m_node); + + stackPool[stack][0] = me; + stackPool[stack][1] = other->m_left; + stack++; + dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgNodeBase*))); + + stackPool[stack][0] = me; + stackPool[stack][1] = other->m_right; + stack++; + dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgNodeBase*))); + + + } else if (other->m_type == m_leaf) { + dgAssert (me->m_type == m_node); + + stackPool[stack][0] = me->m_left; + stackPool[stack][1] = other; + stack++; + dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgNodeBase*))); + + stackPool[stack][0] = me->m_right; + stackPool[stack][1] = other; + stack++; + dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgNodeBase*))); + } else { + dgAssert (me->m_type == m_node); + dgAssert (other->m_type == m_node); + + stackPool[stack][0] = me->m_left; + stackPool[stack][1] = other->m_left; + stack++; + dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgNodeBase*))); + + stackPool[stack][0] = me->m_left; + stackPool[stack][1] = other->m_right; + stack++; + dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgNodeBase*))); + + stackPool[stack][0] = me->m_right; + stackPool[stack][1] = other->m_left; + stack++; + dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgNodeBase*))); + + stackPool[stack][0] = me->m_right; + stackPool[stack][1] = other->m_right; + stack++; + dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgNodeBase*))); + + } + } + } + + contactJoint->m_closestDistance = closestDist; + proxy.m_contacts = contacts; + return contactCount; +} + +dgInt32 dgCollisionCompound::CalculateContactsToHeightField (dgBroadPhase::dgPair* const pair, dgCollisionParamProxy& proxy) const +{ + dgContactPoint* const contacts = proxy.m_contacts; + + const dgNodeBase* stackPool[DG_COMPOUND_STACK_DEPTH]; + + dgInt32 contactCount = 0; + dgContact* const contactJoint = pair->m_contact; + dgBody* const myBody = contactJoint->GetBody0(); + dgBody* const terrainBody = contactJoint->GetBody1(); + + dgCollisionInstance* const compoundInstance = myBody->m_collision; + dgCollisionInstance* const terrainInstance = terrainBody->m_collision; + + dgAssert (compoundInstance->GetChildShape() == this); + dgAssert (terrainInstance->IsType (dgCollision::dgCollisionHeightField_RTTI)); + dgCollisionHeightField* const terrainCollision = (dgCollisionHeightField*)terrainInstance->GetChildShape(); + + proxy.m_body0 = myBody; + proxy.m_body1 = terrainBody; + + proxy.m_instance1 = terrainInstance; + const dgMatrix& compoundMatrix = compoundInstance->GetGlobalMatrix(); + const dgMatrix& hieghFieldMatrix = terrainInstance->GetGlobalMatrix(); + dgOOBBTestData data (hieghFieldMatrix * compoundMatrix.Inverse()); + + dgInt32 stack = 1; + stackPool[0] = m_root; + + dgNodeBase nodeProxi; + nodeProxi.m_left = NULL; + nodeProxi.m_right = NULL; + const dgContactMaterial* const material = contactJoint->GetMaterial(); + + dgAssert ((contacts != NULL) ^ proxy.m_intersectionTestOnly); + + dgFloat32 timestep = pair->m_timestep; + dgFloat32 closestDist = dgFloat32 (1.0e10f); + + const dgVector heighFieldScale(terrainInstance->GetScale()); + const dgVector heighFieldInvScale(terrainInstance->GetInvScale()); + while (stack) { + stack --; + const dgNodeBase* const me = stackPool[stack]; + + dgVector origin (heighFieldInvScale * data.m_matrix.UntransformVector(me->m_origin)); + dgVector size (heighFieldInvScale * data.m_absMatrix.UnrotateVector(me->m_size)); + dgVector p0 (origin - size); + dgVector p1 (origin + size); + terrainCollision->GetLocalAABB (p0, p1, nodeProxi.m_p0, nodeProxi.m_p1); + nodeProxi.m_p0 *= heighFieldScale; + nodeProxi.m_p1 *= heighFieldScale; + nodeProxi.m_size = dgVector::m_half * (nodeProxi.m_p1 - nodeProxi.m_p0); + nodeProxi.m_origin = dgVector::m_half * (nodeProxi.m_p1 + nodeProxi.m_p0); + if (me->BoxTest (data, &nodeProxi)) { + if (me->m_type == m_leaf) { + dgCollisionInstance* const subShape = me->GetShape(); + if (subShape->GetCollisionMode()) { + bool processContacts = true; + if (material->m_compoundAABBOverlap) { + processContacts = material->m_compoundAABBOverlap (*contactJoint, timestep, myBody, me->m_myNode, terrainBody, NULL, proxy.m_threadIndex); + } + if (processContacts) { + dgCollisionInstance childInstance (*subShape, subShape->GetChildShape()); + childInstance.m_globalMatrix = childInstance.GetLocalMatrix() * compoundMatrix; + proxy.m_instance0 = &childInstance; + + proxy.m_maxContacts = DG_MAX_CONTATCS - contactCount; + proxy.m_contacts = contacts ? &contacts[contactCount] : contacts; + + dgInt32 count = 0; + count += m_world->CalculateConvexToNonConvexContacts (proxy); + closestDist = dgMin(closestDist, contactJoint->m_closestDistance); + + if (!proxy.m_intersectionTestOnly) { + for (dgInt32 i = 0; i < count; i ++) { + dgAssert (contacts[contactCount + i].m_collision0 == &childInstance); + contacts[contactCount + i].m_collision0 = subShape; + } + contactCount += count; + + if (contactCount > (DG_MAX_CONTATCS - 2 * (DG_CONSTRAINT_MAX_ROWS / 3))) { + contactCount = m_world->PruneContacts(contactCount, contacts, proxy.m_contactJoint->GetPruningTolerance(), 16); + } + } else if (count == -1) { + contactCount = -1; + break; + } + childInstance.m_material.m_userData = NULL; + proxy.m_instance0 = NULL; + } + } + + } else { + dgAssert (me->m_type == m_node); + stackPool[stack] = me->m_left; + stack++; + + stackPool[stack] = me->m_right; + stack++; + } + } + } + + contactJoint->m_closestDistance = closestDist; + proxy.m_contacts = contacts; + return contactCount; +} + + +dgInt32 dgCollisionCompound::CalculateContactsUserDefinedCollision (dgBroadPhase::dgPair* const pair, dgCollisionParamProxy& proxy) const +{ + dgContactPoint* const contacts = proxy.m_contacts; + + const dgNodeBase* stackPool[DG_COMPOUND_STACK_DEPTH]; + + dgInt32 contactCount = 0; + dgContact* const contactJoint = pair->m_contact; + dgBody* const myBody = contactJoint->GetBody0(); + dgBody* const userBody = contactJoint->GetBody1(); + + dgCollisionInstance* const compoundInstance = myBody->m_collision; + dgCollisionInstance* const userMeshInstance = userBody->m_collision; + + dgAssert (compoundInstance->GetChildShape() == this); + dgAssert (userMeshInstance->IsType (dgCollision::dgCollisionUserMesh_RTTI)); + dgCollisionUserMesh* const userMeshCollision = (dgCollisionUserMesh*)userMeshInstance->GetChildShape(); + + proxy.m_body0 = myBody; + proxy.m_body1 = userBody; + + proxy.m_instance1 = userMeshInstance; + const dgMatrix& myMatrix = compoundInstance->GetGlobalMatrix(); + dgOOBBTestData data (userMeshInstance->GetGlobalMatrix() * myMatrix.Inverse()); + + dgInt32 stack = 1; + stackPool[0] = m_root; + + dgNodeBase nodeProxi; + nodeProxi.m_left = NULL; + nodeProxi.m_right = NULL; + const dgContactMaterial* const material = contactJoint->GetMaterial(); + + dgAssert ((contacts != NULL) ^ proxy.m_intersectionTestOnly); + + dgFloat32 timestep = pair->m_timestep; + dgFloat32 closestDist = dgFloat32 (1.0e10f); + while (stack) { + stack --; + const dgNodeBase* const me = stackPool[stack]; + + dgVector origin (data.m_matrix.UntransformVector(me->m_origin)); + dgVector size (data.m_absMatrix.UnrotateVector(me->m_size)); + dgVector p0 (origin - size); + dgVector p1 (origin + size); + + if (userMeshCollision->AABBOvelapTest (p0, p1)) { + if (me->m_type == m_leaf) { + dgCollisionInstance* const subShape = me->GetShape(); + if (subShape->GetCollisionMode()) { + bool processContacts = true; + if (material->m_compoundAABBOverlap) { + processContacts = material->m_compoundAABBOverlap (*contactJoint, timestep, myBody, me->m_myNode, userBody, NULL, proxy.m_threadIndex); + } + if (processContacts) { + dgCollisionInstance childInstance (*subShape, subShape->GetChildShape()); + childInstance.m_globalMatrix = childInstance.GetLocalMatrix() * myMatrix; + proxy.m_instance0 = &childInstance; + + proxy.m_maxContacts = DG_MAX_CONTATCS - contactCount; + proxy.m_contacts = contacts ? &contacts[contactCount] : contacts; + + dgInt32 count = 0; + count += m_world->CalculateConvexToNonConvexContacts (proxy); + closestDist = dgMin(closestDist, contactJoint->m_closestDistance); + + if (!proxy.m_intersectionTestOnly) { + for (dgInt32 i = 0; i < count; i ++) { + dgAssert (contacts[contactCount + i].m_collision0 == &childInstance); + contacts[contactCount + i].m_collision0 = subShape; + } + contactCount += count; + + if (contactCount > (DG_MAX_CONTATCS - 2 * (DG_CONSTRAINT_MAX_ROWS / 3))) { + contactCount = m_world->PruneContacts(contactCount, contacts, proxy.m_contactJoint->GetPruningTolerance(), 16); + } + } else if (count == -1) { + contactCount = -1; + break; + } + childInstance.m_material.m_userData = NULL; + proxy.m_instance0 = NULL; + } + } + + } else { + dgAssert (me->m_type == m_node); + stackPool[stack] = me->m_left; + stack++; + + stackPool[stack] = me->m_right; + stack++; + } + } + } + + contactJoint->m_closestDistance = closestDist; + proxy.m_contacts = contacts; + return contactCount; +} + + +dgInt32 dgCollisionCompound::CalculateContactsToSingle (dgBroadPhase::dgPair* const pair, dgCollisionParamProxy& proxy) const +{ + dgContactPoint* const contacts = proxy.m_contacts; + const dgNodeBase* stackPool[DG_COMPOUND_STACK_DEPTH]; + + dgContact* const contactJoint = pair->m_contact; + + dgBody* const compoundBody = contactJoint->GetBody0(); + dgBody* const otherBody = contactJoint->GetBody1(); + + dgCollisionInstance* const compoundInstance = compoundBody->m_collision; + dgCollisionInstance* const otherInstance = otherBody->m_collision; + + dgAssert (compoundInstance->GetChildShape() == this); + dgAssert (otherInstance->IsType (dgCollision::dgCollisionConvexShape_RTTI)); + + proxy.m_body0 = compoundBody; + + proxy.m_body1 = otherBody; + proxy.m_instance1 = otherBody->m_collision; + + dgInt32 contactCount = 0; + const dgMatrix& myMatrix = compoundInstance->GetGlobalMatrix(); + dgMatrix matrix (otherBody->m_collision->GetGlobalMatrix() * myMatrix.Inverse()); + + dgVector size; + dgVector origin; + otherInstance->CalcObb (origin, size); + dgOOBBTestData data (matrix, origin, size); + + dgInt32 stack = 1; + stackPool[0] = m_root; + const dgContactMaterial* const material = contactJoint->GetMaterial(); + + dgAssert ((contacts != NULL) ^ proxy.m_intersectionTestOnly); + + dgFloat32 timestep = pair->m_timestep; + dgFloat32 closestDist = dgFloat32 (1.0e10f); + while (stack) { + stack --; + const dgNodeBase* const me = stackPool[stack]; + dgAssert (me); + + if (me->BoxTest (data)) { + if (me->m_type == m_leaf) { + dgCollisionInstance* const subShape = me->GetShape(); + if (subShape->GetCollisionMode()) { + bool processContacts = true; + if (material->m_compoundAABBOverlap) { + processContacts = material->m_compoundAABBOverlap (*contactJoint, timestep, compoundBody, me->m_myNode, otherBody, NULL, proxy.m_threadIndex); + } + if (processContacts) { + dgCollisionInstance childInstance (*subShape, subShape->GetChildShape()); + childInstance.m_globalMatrix = childInstance.GetLocalMatrix() * myMatrix; + proxy.m_instance0 = &childInstance; + + proxy.m_maxContacts = DG_MAX_CONTATCS - contactCount; + proxy.m_contacts = contacts ? &contacts[contactCount] : contacts; + + dgInt32 count = m_world->CalculateConvexToConvexContacts (proxy); + closestDist = dgMin(closestDist, contactJoint->m_closestDistance); + if (!proxy.m_intersectionTestOnly) { + for (dgInt32 i = 0; i < count; i ++) { + dgAssert (contacts[contactCount + i].m_collision0 == &childInstance); + contacts[contactCount + i].m_collision0 = subShape; + } + contactCount += count; + if (contactCount > (DG_MAX_CONTATCS - 2 * (DG_CONSTRAINT_MAX_ROWS / 3))) { + contactCount = m_world->PruneContacts(contactCount, contacts, proxy.m_contactJoint->GetPruningTolerance(), 16); + } + } else if (count == -1) { + contactCount = -1; + break; + } + childInstance.m_material.m_userData = NULL; + proxy.m_instance0 = NULL; + } + } + + } else { + dgAssert (me->m_type == m_node); + stackPool[stack] = me->m_left; + stack++; + dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgNodeBase*))); + + stackPool[stack] = me->m_right; + stack++; + dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgNodeBase*))); + } + } + } + + contactJoint->m_closestDistance = closestDist; + proxy.m_contacts = contacts; + return contactCount; +} + + +dgInt32 dgCollisionCompound::CalculateContactsToCollisionTree (dgBroadPhase::dgPair* const pair, dgCollisionParamProxy& proxy) const +{ + dgContactPoint* const contacts = proxy.m_contacts; + + dgNodePairs stackPool[4 * DG_COMPOUND_STACK_DEPTH]; + + dgInt32 contactCount = 0; + + dgContact* const contactJoint = pair->m_contact; + dgBody* const myBody = contactJoint->GetBody0(); + dgBody* const treeBody = contactJoint->GetBody1(); + + dgCollisionInstance* const compoundInstance = myBody->m_collision; + dgCollisionInstance* const treeCollisionInstance = treeBody->m_collision; + + dgAssert (compoundInstance->GetChildShape() == this); + dgAssert (treeCollisionInstance->IsType (dgCollision::dgCollisionBVH_RTTI)); + dgCollisionBVH* const treeCollision = (dgCollisionBVH*)treeCollisionInstance->GetChildShape(); + + proxy.m_body0 = myBody; + proxy.m_body1 = treeBody; + proxy.m_instance1 = treeCollisionInstance; + + const dgMatrix& myMatrix (compoundInstance->GetGlobalMatrix()); + dgOOBBTestData data (treeCollisionInstance->GetGlobalMatrix() * myMatrix.Inverse()); + + dgInt32 stack = 1; + stackPool[0].m_myNode = m_root; + stackPool[0].m_treeNode = treeCollision->GetRootNode(); + stackPool[0].m_treeNodeIsLeaf = 0; + + dgNodeBase nodeProxi; + nodeProxi.m_left = NULL; + nodeProxi.m_right = NULL; + + const dgContactMaterial* const material = contactJoint->GetMaterial(); + const dgVector& treeScale = treeCollisionInstance->GetScale(); + + dgAssert ((contacts != NULL) ^ proxy.m_intersectionTestOnly); + + dgFloat32 timestep = pair->m_timestep; + dgFloat32 closestDist = dgFloat32 (1.0e10f); + while (stack) { + + stack --; + const dgNodePairs* const stackEntry = &stackPool[stack]; + + dgNodeBase* const me = stackEntry->m_myNode; + const void* const other = stackEntry->m_treeNode; + dgInt32 treeNodeIsLeaf = stackEntry->m_treeNodeIsLeaf; + + dgAssert (me && other); + dgVector p0; + dgVector p1; + + treeCollision->GetNodeAABB(other, p0, p1); + nodeProxi.m_p0 = p0 * treeScale; + nodeProxi.m_p1 = p1 * treeScale; + + p0 = nodeProxi.m_p0 * dgVector::m_half; + p1 = nodeProxi.m_p1 * dgVector::m_half; + nodeProxi.m_size = p1 - p0; + nodeProxi.m_origin = p1 + p0; + nodeProxi.m_area = nodeProxi.m_size.ShiftTripleRight().DotProduct(nodeProxi.m_size).GetScalar(); + + if (me->BoxTest (data, &nodeProxi)) { + if ((me->m_type == m_leaf) && treeNodeIsLeaf) { + dgCollisionInstance* const subShape = me->GetShape(); + if (subShape->GetCollisionMode()) { + bool processContacts = true; + if (material->m_compoundAABBOverlap) { + processContacts = material->m_compoundAABBOverlap (*contactJoint, timestep, myBody, me->m_myNode, treeBody, NULL, proxy.m_threadIndex); + } + if (processContacts) { + dgCollisionInstance childInstance (*subShape, subShape->GetChildShape()); + childInstance.m_globalMatrix = childInstance.GetLocalMatrix() * myMatrix; + proxy.m_instance0 = &childInstance; + + proxy.m_maxContacts = DG_MAX_CONTATCS - contactCount; + proxy.m_contacts = contacts ? &contacts[contactCount] : contacts; + + dgInt32 count = m_world->CalculateConvexToNonConvexContacts (proxy); + closestDist = dgMin(closestDist, contactJoint->m_closestDistance); + + if (!proxy.m_intersectionTestOnly) { + for (dgInt32 i = 0; i < count; i ++) { + dgAssert (contacts[contactCount + i].m_collision0 == &childInstance); + contacts[contactCount + i].m_collision0 = subShape; + } + contactCount += count; + if (contactCount > (DG_MAX_CONTATCS - 2 * (DG_CONSTRAINT_MAX_ROWS / 3))) { + contactCount = m_world->PruneContacts(contactCount, contacts, proxy.m_contactJoint->GetPruningTolerance(), 16); + } + } else if (count == -1) { + contactCount = -1; + break; + } + //childInstance.SetUserData(NULL); + + childInstance.m_material.m_userData = NULL; + proxy.m_instance0 = NULL; + } + } + + } else if (me->m_type == m_leaf) { + void* const frontNode = treeCollision->GetFrontNode(other); + void* const backNode = treeCollision->GetBackNode(other); + if (backNode && frontNode) { + stackPool[stack].m_myNode = me; + stackPool[stack].m_treeNode = backNode; + stackPool[stack].m_treeNodeIsLeaf = 0; + stack++; + + stackPool[stack].m_myNode = me; + stackPool[stack].m_treeNode = frontNode; + stackPool[stack].m_treeNodeIsLeaf = 0; + stack++; + + } else if (backNode && !frontNode) { + stackPool[stack].m_myNode = me; + stackPool[stack].m_treeNode = backNode; + stackPool[stack].m_treeNodeIsLeaf = 0; + + stack++; + + stackPool[stack].m_myNode = me; + stackPool[stack].m_treeNode = other; + stackPool[stack].m_treeNodeIsLeaf = 1; + stack++; + + } else if (!backNode && frontNode) { + stackPool[stack].m_myNode = me; + stackPool[stack].m_treeNode = frontNode; + stackPool[stack].m_treeNodeIsLeaf = 0; + stack++; + + stackPool[stack].m_myNode = me; + stackPool[stack].m_treeNode = other; + stackPool[stack].m_treeNodeIsLeaf = 1; + stack++; + + } else { + stackPool[stack].m_myNode = me; + stackPool[stack].m_treeNode = other; + stackPool[stack].m_treeNodeIsLeaf = 1; + stack++; + } + + } else if (treeNodeIsLeaf) { + stackPool[stack].m_myNode = me->m_left; + stackPool[stack].m_treeNode = other; + stackPool[stack].m_treeNodeIsLeaf = 1; + stack++; + dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgNodeBase*))); + + stackPool[stack].m_myNode = me->m_right; + stackPool[stack].m_treeNode = other; + stackPool[stack].m_treeNodeIsLeaf = 1; + stack++; + dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgNodeBase*))); + + } else if (nodeProxi.m_area > me->m_area) { + dgAssert (me->m_type == m_node); + void* const frontNode = treeCollision->GetFrontNode(other); + void* const backNode = treeCollision->GetBackNode(other); + if (backNode && frontNode) { + stackPool[stack].m_myNode = (dgNodeBase*) me; + stackPool[stack].m_treeNode = backNode; + stackPool[stack].m_treeNodeIsLeaf = 0; + stack++; + + stackPool[stack].m_myNode = (dgNodeBase*) me; + stackPool[stack].m_treeNode = frontNode; + stackPool[stack].m_treeNodeIsLeaf = 0; + stack++; + } else if (backNode && !frontNode) { + stackPool[stack].m_myNode = (dgNodeBase*) me; + stackPool[stack].m_treeNode = backNode; + stackPool[stack].m_treeNodeIsLeaf = 0; + stack++; + + stackPool[stack].m_myNode = me->m_left; + stackPool[stack].m_treeNode = other; + stackPool[stack].m_treeNodeIsLeaf = 1; + stack++; + + stackPool[stack].m_myNode = me->m_right; + stackPool[stack].m_treeNode = other; + stackPool[stack].m_treeNodeIsLeaf = 1; + stack++; + + } else if (!backNode && frontNode) { + stackPool[stack].m_myNode = me; + stackPool[stack].m_treeNode = frontNode; + stackPool[stack].m_treeNodeIsLeaf = 0; + stack++; + + stackPool[stack].m_myNode = me->m_left; + stackPool[stack].m_treeNode = other; + stackPool[stack].m_treeNodeIsLeaf = 1; + stack++; + + stackPool[stack].m_myNode = me->m_right; + stackPool[stack].m_treeNode = other; + stackPool[stack].m_treeNodeIsLeaf = 1; + stack++; + + } else { + stackPool[stack].m_myNode = me; + stackPool[stack].m_treeNode = other; + stackPool[stack].m_treeNodeIsLeaf = 1; + stack++; + } + + } else { + dgAssert (me->m_type == m_node); + stackPool[stack].m_myNode = me->m_left; + stackPool[stack].m_treeNode = other; + stackPool[stack].m_treeNodeIsLeaf = treeNodeIsLeaf; + stack++; + + stackPool[stack].m_myNode = me->m_right; + stackPool[stack].m_treeNode = other; + stackPool[stack].m_treeNodeIsLeaf = treeNodeIsLeaf; + stack++; + } + } + } + + contactJoint->m_closestDistance = closestDist; + proxy.m_contacts = contacts; + return contactCount; +} + + + +dgInt32 dgCollisionCompound::CalculateContactsToSingleContinue(dgBroadPhase::dgPair* const pair, dgCollisionParamProxy& proxy) const +{ + if (proxy.m_timestep < dgFloat32 (1.0e-4f)) { + return 0; + } + dgContactPoint* const contacts = proxy.m_contacts; + const dgNodeBase* stackPool[DG_COMPOUND_STACK_DEPTH]; + dgContact* const contactJoint = pair->m_contact; + + dgBody* const compoundBody = contactJoint->GetBody0(); + dgBody* const otherBody = contactJoint->GetBody1(); + + dgCollisionInstance* const compoundInstance = compoundBody->m_collision; + dgCollisionInstance* const otherInstance = otherBody->m_collision; + + dgAssert (compoundInstance->GetChildShape() == this); + dgAssert (otherInstance->IsType (dgCollision::dgCollisionConvexShape_RTTI)); + + proxy.m_body0 = compoundBody; + + proxy.m_body1 = otherBody; + proxy.m_instance1 = otherBody->m_collision; + + dgInt32 contactCount = 0; + + const dgMatrix myMatrix = compoundInstance->GetGlobalMatrix(); + dgMatrix matrix (otherBody->m_collision->GetGlobalMatrix() * myMatrix.Inverse()); + + dgVector boxP0; + dgVector boxP1; + otherInstance->CalcAABB(matrix, boxP0, boxP1); + dgVector relVeloc (myMatrix.UnrotateVector (otherBody->GetVelocity() - compoundBody->GetVelocity())); + dgFastRayTest ray (dgVector (dgFloat32 (0.0f)), relVeloc); + + dgInt32 stack = 1; + stackPool[0] = m_root; + const dgContactMaterial* const material = contactJoint->GetMaterial(); + + dgFloat32 maxParam = proxy.m_timestep; + dgFloat32 invMaxParam = dgFloat32 (1.0f) / maxParam; + + dgVector n(dgFloat32(0.0f), dgFloat32(1.0f), dgFloat32(0.0f), dgFloat32(0.0f)); + dgVector p(dgFloat32(0.0f)); + dgVector q(dgFloat32(0.0f)); + + dgFloat32 timestep = pair->m_timestep; + dgFloat32 closestDist = dgFloat32 (1.0e10f); + while (stack) { + stack --; + const dgNodeBase* const me = stackPool[stack]; + dgAssert (me); + + dgVector minBox (me->m_p0 - boxP1); + dgVector maxBox (me->m_p1 - boxP0); + if (ray.BoxTest (minBox, maxBox)) { + if (me->m_type == m_leaf) { + dgCollisionInstance* const subShape = me->GetShape(); + + if (subShape->GetCollisionMode()) { + bool processContacts = true; + if (material->m_compoundAABBOverlap) { + processContacts = material->m_compoundAABBOverlap (*contactJoint, timestep, compoundBody, me->m_myNode, otherBody, NULL, proxy.m_threadIndex); + } + if (processContacts) { + dgCollisionInstance childInstance (*subShape, subShape->GetChildShape()); + childInstance.m_globalMatrix = childInstance.GetLocalMatrix() * myMatrix; + proxy.m_instance0 = &childInstance; + + proxy.m_maxContacts = DG_MAX_CONTATCS - contactCount; + proxy.m_contacts = contacts ? &contacts[contactCount] : contacts; + + dgInt32 count = m_world->CalculateConvexToConvexContacts (proxy); + + closestDist = dgMin(closestDist, contactJoint->m_closestDistance); + dgFloat32 param = proxy.m_timestep; + dgAssert (param >= dgFloat32 (0.0f)); + if (param < maxParam) { + n = proxy.m_normal; + p = proxy.m_closestPointBody0; + q = proxy.m_closestPointBody1; + + if (proxy.m_intersectionTestOnly) { + maxParam = param; + if (count == -1) { + contactCount = -1; + break; + } + + } else { + if (contactCount && ((param - maxParam) * invMaxParam) < dgFloat32(-1.0e-3f)) { + for (dgInt32 i = 0; i < count; i ++) { + contacts[i] = contacts[contactCount + i]; + } + contactCount = 0; + } + maxParam = param; + + for (dgInt32 i = 0; i < count; i ++) { + dgAssert (contacts[contactCount + i].m_collision0 == &childInstance); + contacts[contactCount + i].m_collision0 = subShape; + } + contactCount += count; + + if (contactCount > (DG_MAX_CONTATCS - 2 * (DG_CONSTRAINT_MAX_ROWS / 3))) { + contactCount = m_world->PruneContacts(contactCount, contacts, proxy.m_contactJoint->GetPruningTolerance(), 16); + } + + if (maxParam == dgFloat32 (0.0f)) { + break; + } + } + } + + childInstance.m_material.m_userData = NULL; + proxy.m_instance0 = NULL; + } + } + } else { + dgAssert (me->m_type == m_node); + stackPool[stack] = me->m_left; + stack++; + dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgNodeBase*))); + + stackPool[stack] = me->m_right; + stack++; + dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgNodeBase*))); + } + } + } + + proxy.m_normal = n; + proxy.m_closestPointBody0 = p; + proxy.m_closestPointBody1 = q; + proxy.m_timestep = maxParam; + contactJoint->m_closestDistance = closestDist; + proxy.m_contacts = contacts; + return contactCount; +} + + +dgInt32 dgCollisionCompound::CalculateContactsToCompoundContinue(dgBroadPhase::dgPair* const pair, dgCollisionParamProxy& proxy) const +{ + if (proxy.m_timestep < dgFloat32 (1.0e-4f)) { + return 0; + } + + dgInt32 contactCount = 0; + dgContactPoint* const contacts = proxy.m_contacts; + const dgNodeBase* stackPool[4 * DG_COMPOUND_STACK_DEPTH][2]; + dgContact* const contactJoint = pair->m_contact; + + dgBody* const compoundBody = contactJoint->GetBody0(); + dgBody* const otherCompoundBody = contactJoint->GetBody1(); + + dgCollisionInstance* const compoundInstance = compoundBody->m_collision; + dgCollisionInstance* const otherCompoundInstance = otherCompoundBody->m_collision; + + dgCollisionCompound* const otherCompound = (dgCollisionCompound*)otherCompoundInstance->GetChildShape(); + + dgAssert (compoundInstance->GetChildShape() == this); + dgAssert (otherCompoundInstance->IsType (dgCollision::dgCollisionCompound_RTTI)); + + proxy.m_body0 = compoundBody; + proxy.m_body1 = otherCompoundBody; + + const dgMatrix& myMatrix = compoundInstance->GetGlobalMatrix(); + const dgMatrix& otherMatrix = otherCompoundInstance->GetGlobalMatrix(); + dgOOBBTestData data (otherMatrix * myMatrix.Inverse()); + + dgVector relVeloc (myMatrix.UnrotateVector (otherCompoundBody->GetVelocity() - compoundBody->GetVelocity())); + dgFastRayTest myCompoundRay (dgVector (dgFloat32 (0.0f)), relVeloc); + dgFastRayTest otherCompoundRay (dgVector (dgFloat32 (0.0f)), data.m_matrix.UnrotateVector(relVeloc)); + + dgInt32 stack = 1; + stackPool[0][0] = m_root; + stackPool[0][1] = otherCompound->m_root; + const dgContactMaterial* const material = contactJoint->GetMaterial(); + + dgVector n(dgFloat32(0.0f), dgFloat32(1.0f), dgFloat32(0.0f), dgFloat32(0.0f)); + dgVector p(dgFloat32(0.0f)); + dgVector q(dgFloat32(0.0f)); + + dgFloat32 maxParam = proxy.m_timestep; + dgFloat32 invMaxParam = dgFloat32 (1.0f) / maxParam; + + dgFloat32 upperBound = dgFloat32 (1.0f); + + dgFloat32 timestep = pair->m_timestep; + dgFloat32 closestDist = dgFloat32 (1.0e10f); + + while (stack) { + stack --; + const dgNodeBase* const me = stackPool[stack][0]; + const dgNodeBase* const other = stackPool[stack][1]; + dgAssert (me && other); + + dgFloat32 dist = me->RayBoxDistance (data, myCompoundRay, otherCompoundRay, other); + if (dist <= upperBound) { + if ((me->m_type == m_leaf) && (other->m_type == m_leaf)) { + dgCollisionInstance* const subShape = me->GetShape(); + dgCollisionInstance* const otherSubShape = other->GetShape(); + + if (subShape->GetCollisionMode() & otherSubShape->GetCollisionMode()) { + + bool processContacts = true; + if (material->m_compoundAABBOverlap) { + processContacts = material->m_compoundAABBOverlap (*contactJoint, timestep, compoundBody, me->m_myNode, otherCompoundBody, other->m_myNode, proxy.m_threadIndex); + } + + if (processContacts) { + + dgCollisionInstance childInstance (*subShape, subShape->GetChildShape()); + childInstance.m_globalMatrix = childInstance.GetLocalMatrix() * myMatrix; + proxy.m_instance0 = &childInstance; + + dgCollisionInstance otherChildInstance (*otherSubShape, otherSubShape->GetChildShape()); + otherChildInstance.m_globalMatrix = otherChildInstance.GetLocalMatrix() * otherMatrix; + proxy.m_instance1 = &otherChildInstance; + + proxy.m_maxContacts = DG_MAX_CONTATCS - contactCount; + proxy.m_contacts = contacts ? &contacts[contactCount] : contacts; + + dgInt32 count = m_world->CalculateConvexToConvexContacts (proxy); + + closestDist = dgMin(closestDist, contactJoint->m_closestDistance); + + dgFloat32 param = proxy.m_timestep; + dgAssert (param >= dgFloat32 (0.0f)); + if (param < maxParam) { + n = proxy.m_normal; + p = proxy.m_closestPointBody0; + q = proxy.m_closestPointBody1; + + upperBound = param * invMaxParam; + if (proxy.m_intersectionTestOnly) { + maxParam = param; + if (count == -1) { + contactCount = -1; + break; + } + + } else { + if (contactCount && ((param - maxParam) * invMaxParam) < dgFloat32(-1.0e-3f)) { + for (dgInt32 i = 0; i < count; i ++) { + contacts[i] = contacts[contactCount + i]; + } + contactCount = 0; + } + maxParam = param; + + for (dgInt32 i = 0; i < count; i ++) { + dgAssert (contacts[contactCount + i].m_collision0 == &childInstance); + contacts[contactCount + i].m_collision0 = subShape; + } + contactCount += count; + + if (contactCount > (DG_MAX_CONTATCS - 2 * (DG_CONSTRAINT_MAX_ROWS / 3))) { + contactCount = m_world->PruneContacts(contactCount, contacts, proxy.m_contactJoint->GetPruningTolerance(), 16); + } + + if (maxParam == dgFloat32 (0.0f)) { + break; + } + } + } + + childInstance.m_material.m_userData = NULL; + otherChildInstance.m_material.m_userData = NULL; + + proxy.m_instance0 = NULL; + proxy.m_instance1 = NULL; + } + } + } else if (me->m_type == m_leaf) { + dgAssert (other->m_type == m_node); + stackPool[stack][0] = me; + stackPool[stack][1] = other->m_left; + stack++; + dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgNodeBase*))); + + stackPool[stack][0] = me; + stackPool[stack][1] = other->m_right; + stack++; + dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgNodeBase*))); + + } else if (other->m_type == m_leaf) { + dgAssert (me->m_type == m_node); + stackPool[stack][0] = me->m_left; + stackPool[stack][1] = other; + stack++; + dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgNodeBase*))); + + stackPool[stack][0] = me->m_right; + stackPool[stack][1] = other; + stack++; + dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgNodeBase*))); + + } else { + dgAssert (me->m_type == m_node); + dgAssert (other->m_type == m_node); + + stackPool[stack][0] = me->m_left; + stackPool[stack][1] = other->m_left; + stack++; + dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgNodeBase*))); + + stackPool[stack][0] = me->m_left; + stackPool[stack][1] = other->m_right; + stack++; + dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgNodeBase*))); + + stackPool[stack][0] = me->m_right; + stackPool[stack][1] = other->m_left; + stack++; + dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgNodeBase*))); + + stackPool[stack][0] = me->m_right; + stackPool[stack][1] = other->m_right; + stack++; + dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgNodeBase*))); + } + } + } + + proxy.m_normal = n; + proxy.m_closestPointBody0 = p; + proxy.m_closestPointBody1 = q; + proxy.m_timestep = maxParam; + + contactJoint->m_closestDistance = closestDist; + proxy.m_contacts = contacts; + return contactCount; +} + +dgInt32 dgCollisionCompound::CalculateContactsToCollisionTreeContinue (dgBroadPhase::dgPair* const pair, dgCollisionParamProxy& proxy) const +{ + if (proxy.m_timestep < dgFloat32 (1.0e-4f)) { + return 0; + } + + dgContactPoint* const contacts = proxy.m_contacts; + + dgNodePairs stackPool[4 * DG_COMPOUND_STACK_DEPTH]; + + dgInt32 contactCount = 0; + + dgContact* const contactJoint = pair->m_contact; + dgBody* const myBody = contactJoint->GetBody0(); + dgBody* const treeBody = contactJoint->GetBody1(); + + dgCollisionInstance* const compoundInstance = myBody->m_collision; + dgCollisionInstance* const treeCollisionInstance = treeBody->m_collision; + + dgAssert (compoundInstance->GetChildShape() == this); + dgAssert (treeCollisionInstance->IsType (dgCollision::dgCollisionBVH_RTTI)); + dgCollisionBVH* const treeCollision = (dgCollisionBVH*)treeCollisionInstance->GetChildShape(); + + proxy.m_body0 = myBody; + proxy.m_body1 = treeBody; + proxy.m_instance1 = treeCollisionInstance; + + const dgMatrix& myMatrix = compoundInstance->GetGlobalMatrix(); + const dgMatrix& otherMatrix = treeCollisionInstance->GetGlobalMatrix(); + dgOOBBTestData data (otherMatrix * myMatrix.Inverse()); + + dgVector relVeloc (myMatrix.UnrotateVector (treeBody->GetVelocity() - myBody->GetVelocity())); + dgFastRayTest myCompoundRay (dgVector (dgFloat32 (0.0f)), relVeloc); + dgFastRayTest otherTreedRay (dgVector (dgFloat32 (0.0f)), data.m_matrix.UnrotateVector(relVeloc)); + + dgInt32 stack = 1; + stackPool[0].m_myNode = m_root; + stackPool[0].m_treeNode = treeCollision->GetRootNode(); + stackPool[0].m_treeNodeIsLeaf = 0; + + dgNodeBase nodeProxi; + nodeProxi.m_left = NULL; + nodeProxi.m_right = NULL; + + const dgVector& treeScale = treeCollisionInstance->GetScale(); + const dgContactMaterial* const material = contactJoint->GetMaterial(); + + dgVector n(dgFloat32(0.0f), dgFloat32(1.0f), dgFloat32(0.0f), dgFloat32(0.0f)); + dgVector p(dgFloat32(0.0f)); + dgVector q(dgFloat32(0.0f)); + + dgFloat32 maxParam = proxy.m_timestep; + dgFloat32 invMaxParam = dgFloat32 (1.0f) / maxParam; + + dgFloat32 upperBound = dgFloat32 (1.0f); + + dgFloat32 timestep = pair->m_timestep; + dgFloat32 closestDist = dgFloat32 (1.0e10f); + while (stack) { + + stack --; + const dgNodePairs* const stackEntry = &stackPool[stack]; + + dgNodeBase* const me = stackEntry->m_myNode; + const void* const other = stackEntry->m_treeNode; + dgInt32 treeNodeIsLeaf = stackEntry->m_treeNodeIsLeaf; + + dgAssert (me && other); + + dgVector p0; + dgVector p1; + + treeCollision->GetNodeAABB(other, p0, p1); + nodeProxi.m_p0 = p0 * treeScale; + nodeProxi.m_p1 = p1 * treeScale; + + p0 = nodeProxi.m_p0 * dgVector::m_half; + p1 = nodeProxi.m_p1 * dgVector::m_half; + nodeProxi.m_size = p1 - p0; + nodeProxi.m_origin = p1 + p0; + nodeProxi.m_area = nodeProxi.m_size.ShiftTripleRight().DotProduct(nodeProxi.m_size).GetScalar(); + + dgFloat32 dist = me->RayBoxDistance (data, myCompoundRay, otherTreedRay, &nodeProxi); + if (dist <= upperBound) { + if ((me->m_type == m_leaf) && treeNodeIsLeaf) { + dgCollisionInstance* const subShape = me->GetShape(); + if (subShape->GetCollisionMode()) { + bool processContacts = true; + if (material->m_compoundAABBOverlap) { + processContacts = material->m_compoundAABBOverlap (*contactJoint, timestep, myBody, me->m_myNode, treeBody, NULL, proxy.m_threadIndex); + } + if (processContacts) { + + dgCollisionInstance childInstance (*subShape, subShape->GetChildShape()); + childInstance.m_globalMatrix = childInstance.GetLocalMatrix() * myMatrix; + proxy.m_instance0 = &childInstance; + + proxy.m_maxContacts = DG_MAX_CONTATCS - contactCount; + proxy.m_contacts = contacts ? &contacts[contactCount] : contacts; + + dgInt32 count = m_world->CalculateConvexToNonConvexContacts (proxy); + closestDist = dgMin(closestDist, contactJoint->m_closestDistance); + + dgFloat32 param = proxy.m_timestep; + dgAssert (param >= dgFloat32 (0.0f)); + if (param < maxParam) { + n = proxy.m_normal; + p = proxy.m_closestPointBody0; + q = proxy.m_closestPointBody1; + + upperBound = param * invMaxParam; + if (proxy.m_intersectionTestOnly) { + maxParam = param; + if (count == -1) { + contactCount = -1; + break; + } + + } else { + if (contactCount && ((param - maxParam) * invMaxParam) < dgFloat32(-1.0e-3f)) { + for (dgInt32 i = 0; i < count; i ++) { + contacts[i] = contacts[contactCount + i]; + } + contactCount = 0; + } + maxParam = param; + + for (dgInt32 i = 0; i < count; i ++) { + dgAssert (contacts[contactCount + i].m_collision0 == &childInstance); + contacts[contactCount + i].m_collision0 = subShape; + } + contactCount += count; + + if (contactCount > (DG_MAX_CONTATCS - 2 * (DG_CONSTRAINT_MAX_ROWS / 3))) { + contactCount = m_world->PruneContacts(contactCount, contacts, proxy.m_contactJoint->GetPruningTolerance(), 16); + } + + if (maxParam == dgFloat32 (0.0f)) { + break; + } + } + } + childInstance.m_material.m_userData = NULL; + proxy.m_instance0 = NULL; + } + } + + } else if (me->m_type == m_leaf) { + void* const frontNode = treeCollision->GetFrontNode(other); + void* const backNode = treeCollision->GetBackNode(other); + + if (backNode && frontNode) { + stackPool[stack].m_myNode = me; + stackPool[stack].m_treeNode = backNode; + stackPool[stack].m_treeNodeIsLeaf = 0; + stack++; + + stackPool[stack].m_myNode = me; + stackPool[stack].m_treeNode = frontNode; + stackPool[stack].m_treeNodeIsLeaf = 0; + stack++; + + } else if (backNode && !frontNode) { + stackPool[stack].m_myNode = me; + stackPool[stack].m_treeNode = backNode; + stackPool[stack].m_treeNodeIsLeaf = 0; + stack++; + + stackPool[stack].m_myNode = me; + stackPool[stack].m_treeNode = other; + stackPool[stack].m_treeNodeIsLeaf = 1; + stack++; + } else if (!backNode && frontNode) { + stackPool[stack].m_myNode = me; + stackPool[stack].m_treeNode = frontNode; + stackPool[stack].m_treeNodeIsLeaf = 0; + stack++; + + stackPool[stack].m_myNode = me; + stackPool[stack].m_treeNode = other; + stackPool[stack].m_treeNodeIsLeaf = 1; + stack++; + } else { + stackPool[stack].m_myNode = me; + stackPool[stack].m_treeNode = other; + stackPool[stack].m_treeNodeIsLeaf = 1; + stack++; + } + + } else if (treeNodeIsLeaf) { + + stackPool[stack].m_myNode = me->m_left; + stackPool[stack].m_treeNode = other; + stackPool[stack].m_treeNodeIsLeaf = 1; + stack++; + dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgNodeBase*))); + + stackPool[stack].m_myNode = me->m_right; + stackPool[stack].m_treeNode = other; + stackPool[stack].m_treeNodeIsLeaf = 1; + stack++; + dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgNodeBase*))); + + } else if (nodeProxi.m_area > me->m_area) { + + dgAssert (me->m_type == m_node); + void* const frontNode = treeCollision->GetFrontNode(other); + void* const backNode = treeCollision->GetBackNode(other); + if (backNode && frontNode) { + stackPool[stack].m_myNode = (dgNodeBase*) me; + stackPool[stack].m_treeNode = backNode; + stackPool[stack].m_treeNodeIsLeaf = 0; + stack++; + + stackPool[stack].m_myNode = (dgNodeBase*) me; + stackPool[stack].m_treeNode = frontNode; + stackPool[stack].m_treeNodeIsLeaf = 0; + stack++; + } else if (backNode && !frontNode) { + stackPool[stack].m_myNode = (dgNodeBase*) me; + stackPool[stack].m_treeNode = backNode; + stackPool[stack].m_treeNodeIsLeaf = 0; + stack++; + + stackPool[stack].m_myNode = me->m_left; + stackPool[stack].m_treeNode = other; + stackPool[stack].m_treeNodeIsLeaf = 1; + stack++; + + stackPool[stack].m_myNode = me->m_right; + stackPool[stack].m_treeNode = other; + stackPool[stack].m_treeNodeIsLeaf = 1; + stack++; + + } else if (!backNode && frontNode) { + + stackPool[stack].m_myNode = me; + stackPool[stack].m_treeNode = frontNode; + stackPool[stack].m_treeNodeIsLeaf = 0; + stack++; + + stackPool[stack].m_myNode = me->m_left; + stackPool[stack].m_treeNode = other; + stackPool[stack].m_treeNodeIsLeaf = 1; + stack++; + + stackPool[stack].m_myNode = me->m_right; + stackPool[stack].m_treeNode = other; + stackPool[stack].m_treeNodeIsLeaf = 1; + stack++; + + } else { + stackPool[stack].m_myNode = me; + stackPool[stack].m_treeNode = other; + stackPool[stack].m_treeNodeIsLeaf = 1; + stack++; + } + + } else { + dgAssert (me->m_type == m_node); + stackPool[stack].m_myNode = me->m_left; + stackPool[stack].m_treeNode = other; + stackPool[stack].m_treeNodeIsLeaf = treeNodeIsLeaf; + stack++; + + stackPool[stack].m_myNode = me->m_right; + stackPool[stack].m_treeNode = other; + stackPool[stack].m_treeNodeIsLeaf = treeNodeIsLeaf; + stack++; + } + } + } + + proxy.m_normal = n; + proxy.m_closestPointBody0 = p; + proxy.m_closestPointBody1 = q; + proxy.m_timestep = maxParam; + + contactJoint->m_closestDistance = closestDist; + proxy.m_contacts = contacts; + return contactCount; +} diff --git a/thirdparty/src/newton/dgPhysics/dgCollisionCompound.h b/thirdparty/src/newton/dgPhysics/dgCollisionCompound.h new file mode 100644 index 000000000..c32568d13 --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgCollisionCompound.h @@ -0,0 +1,252 @@ +/* Copyright (c) <2003-2019> +* +* 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 __DGCOLLISIONCOMPOUND_H__ +#define __DGCOLLISIONCOMPOUND_H__ + + +#include "dgCollision.h" + +class dgFastRayTest; +class dgCollisionBVH; +class dgCollisionInstance; + + +#define DG_COMPOUND_STACK_DEPTH 256 + +class dgCollisionCompound: public dgCollision +{ + protected: + enum dgNodeType + { + m_leaf, + m_node, + }; + + public: + + class dgNodeBase; + class dgTreeArray: public dgTree + { + public: + dgTreeArray (dgMemoryAllocator* const allocator); + void AddNode (dgNodeBase* const node, dgInt32 index, const dgCollisionInstance* const parent); + }; + + DG_MSC_VECTOR_ALIGNMENT + class dgOOBBTestData + { + public: + dgOOBBTestData (const dgMatrix& matrix); + dgOOBBTestData (const dgMatrix& matrix, const dgVector& origin, const dgVector& size); + + DG_INLINE dgFloat32 UpdateSeparatingDistance(const dgVector& box0Min, const dgVector& box0Max, const dgVector& box1Min, const dgVector& box1Max) const; + + dgMatrix m_matrix; + dgMatrix m_absMatrix; + dgVector m_origin; + dgVector m_size; + dgVector m_localP0; + dgVector m_localP1; + dgVector m_aabbP0; + dgVector m_aabbP1; + + dgVector m_crossAxis[9]; + dgVector m_crossAxisAbs[9]; + dgVector m_crossAxisDotAbs[9]; + dgVector m_extendsMinX[3]; + dgVector m_extendsMaxX[3]; + mutable dgFloat32 m_separatingDistance; + static dgVector m_maxDist; + } DG_GCC_VECTOR_ALIGNMENT; + + + DG_MSC_VECTOR_ALIGNMENT + class dgNodeBase + { + public: + DG_CLASS_ALLOCATOR(allocator) + dgNodeBase (); + dgNodeBase (const dgNodeBase& copyFrom); + dgNodeBase (dgCollisionInstance* const instance); + dgNodeBase (dgNodeBase* const left, dgNodeBase* const right); + ~dgNodeBase(); + + void CalculateAABB(); + void SetBox (const dgVector& p0, const dgVector& p1); + bool BoxTest (const dgOOBBTestData& data) const; + bool BoxTest (const dgOOBBTestData& data, const dgNodeBase* const otherNode) const; + dgFloat32 RayBoxDistance (const dgOOBBTestData& data, const dgFastRayTest& myRay, const dgFastRayTest& otherRay, const dgNodeBase* const otherNode) const; + + DG_INLINE dgCollisionInstance* GetShape() const + { + return m_shape; + } + + DG_INLINE dgInt32 BoxIntersect (const dgFastRayTest& ray, const dgVector& boxP0, const dgVector& boxP1) const + { + dgVector minBox (m_p0 - boxP1); + dgVector maxBox (m_p1 - boxP0); + return ray.BoxTest(minBox, maxBox); + } + + dgVector m_p0; + dgVector m_p1; + dgVector m_size; + dgVector m_origin; + dgFloat32 m_area; + dgInt32 m_type; + dgNodeBase* m_left; + dgNodeBase* m_right; + dgNodeBase* m_parent; + dgCollisionInstance* m_shape; + dgTreeArray::dgTreeNode* m_myNode; + } DG_GCC_VECTOR_ALIGNMENT; + + protected: + class dgNodePairs + { + public: + const void* m_treeNode; + dgNodeBase* m_myNode; + dgInt32 m_treeNodeIsLeaf; + }; + + class dgSpliteInfo; + class dgHeapNodePair; + + public: + dgCollisionCompound (dgWorld* const world); + dgCollisionCompound (const dgCollisionCompound& source, const dgCollisionInstance* const myInstance); + dgCollisionCompound (dgWorld* const world, dgDeserialize deserialization, void* const userData, const dgCollisionInstance* const myInstance, dgInt32 revisionNumber); + + void SetParent (const dgCollisionInstance* const myInstance); + virtual ~dgCollisionCompound(); + + virtual void BeginAddRemove (); + virtual dgTreeArray::dgTreeNode* AddCollision (dgCollisionInstance* const part); + virtual void RemoveCollision (dgTreeArray::dgTreeNode* const node); + virtual void SetCollisionMatrix (dgTreeArray::dgTreeNode* const node, const dgMatrix& matrix); + virtual void EndAddRemove (bool flushCache = true); + + void ApplyScale (const dgVector& scale); + void GetAABB (dgVector& p0, dgVector& p1) const; + + dgInt32 GetNodeIndex(dgTreeArray::dgTreeNode* const node) const; + dgTreeArray::dgTreeNode* FindNodeByIndex (dgInt32 index) const; + + dgTreeArray::dgTreeNode* GetFirstNode () const; + dgTreeArray::dgTreeNode* GetNextNode (dgTreeArray::dgTreeNode* const node) const; + dgCollisionInstance* GetCollisionFromNode (dgTreeArray::dgTreeNode* const node) const; + + protected: + void RemoveCollision (dgNodeBase* const node); + virtual dgFloat32 GetVolume () const; + virtual dgFloat32 GetBoxMinRadius () const; + virtual dgFloat32 GetBoxMaxRadius () const; + + virtual dgVector SupportVertex (const dgVector& dir, dgInt32* const vertexIndex) const; + virtual dgVector SupportVertexSpecial (const dgVector& dir, dgFloat32 skinThickness, dgInt32* const vertexIndex) const; + virtual dgVector SupportVertexSpecialProjectPoint (const dgVector& point, const dgVector& dir) const {return point;} + + virtual void CalcAABB (const dgMatrix& matrix, dgVector& p0, dgVector& p1) const; + + static void CalculateInertia (void* userData, int vertexCount, const dgFloat32* const FaceArray, int faceId); + + virtual void MassProperties (); + dgMatrix CalculateInertiaAndCenterOfMass (const dgMatrix& m_alignMatrix, const dgVector& localScale, const dgMatrix& matrix) const; + dgFloat32 CalculateMassProperties (const dgMatrix& offset, dgVector& inertia, dgVector& crossInertia, dgVector& centerOfMass) const; + virtual dgVector CalculateVolumeIntegral (const dgMatrix& globalMatrix, const dgVector& plane, const dgCollisionInstance& parentScale) const; + + virtual void DebugCollision (const dgMatrix& matrix, dgCollision::OnDebugCollisionMeshCallback callback, void* const userData) const; + virtual dgFloat32 RayCast (const dgVector& localP0, const dgVector& localP1, dgFloat32 maxT, dgContactPoint& contactOut, const dgBody* const body, void* const userData, OnRayPrecastAction preFilter) const; + + virtual dgInt32 CalculateSignature () const; + virtual void SetCollisionBBox (const dgVector& p0, const dgVector& p1); + virtual void GetCollisionInfo(dgCollisionInfo* const info) const; + virtual void Serialize(dgSerialize callback, void* const userData) const; + virtual dgInt32 CalculateContacts (dgBroadPhase::dgPair* const pair, dgCollisionParamProxy& proxy) const; + + dgInt32 CalculateContactsToSingle (dgBroadPhase::dgPair* const pair, dgCollisionParamProxy& proxy) const; + dgInt32 CalculateContactsToSingleContinue (dgBroadPhase::dgPair* const pair, dgCollisionParamProxy& proxy) const; + dgInt32 CalculateContactsToCompound (dgBroadPhase::dgPair* const pair, dgCollisionParamProxy& proxy) const; + dgInt32 CalculateContactsToCompoundContinue (dgBroadPhase::dgPair* const pair, dgCollisionParamProxy& proxy) const; + dgInt32 CalculateContactsToCollisionTree (dgBroadPhase::dgPair* const pair, dgCollisionParamProxy& proxy) const; + dgInt32 CalculateContactsToCollisionTreeContinue (dgBroadPhase::dgPair* const pair, dgCollisionParamProxy& proxy) const; + dgInt32 CalculateContactsToHeightField (dgBroadPhase::dgPair* const pair, dgCollisionParamProxy& proxy) const; + dgInt32 CalculateContactsUserDefinedCollision (dgBroadPhase::dgPair* const pair, dgCollisionParamProxy& proxy) const; + dgInt32 ClosestDistance (dgCollisionParamProxy& proxy) const; + dgInt32 ClosestDistanceToConvex (dgCollisionParamProxy& proxy) const; + dgInt32 ClosestDistanceToCompound (dgCollisionParamProxy& proxy) const; + +#ifdef _DEBUG + dgVector InternalSupportVertex (const dgVector& dir) const; +#endif + + dgNodeBase* BuildTopDown (dgNodeBase** const leafArray, dgInt32 firstBox, dgInt32 lastBox, dgList::dgListNode** const nextNode); + dgNodeBase* BuildTopDownBig (dgNodeBase** const leafArray, dgInt32 firstBox, dgInt32 lastBox, dgList::dgListNode** const nextNode); + + dgFloat64 CalculateEntropy (dgList& list); + + void ImproveNodeFitness (dgNodeBase* const node) const; + DG_INLINE dgFloat32 CalculateSurfaceArea (dgNodeBase* const node0, dgNodeBase* const node1, dgVector& minBox, dgVector& maxBox) const; + + dgInt32 CalculatePlaneIntersection (const dgVector& normal, const dgVector& point, dgVector* const contactsOut) const; + + void PushNode (const dgMatrix& matrix, dgUpHeap& heap, dgNodeBase* const myNode, dgNodeBase* const otehrNode) const; + + static dgInt32 CompareNodes (const dgNodeBase* const nodeA, const dgNodeBase* const nodeB, void* notUsed); + + + dgWorld* m_world; + dgNodeBase* m_root; + const dgCollisionInstance* m_myInstance; + dgTreeArray m_array; + dgFloat64 m_treeEntropy; + dgFloat32 m_boxMinRadius; + dgFloat32 m_boxMaxRadius; + dgInt32 m_idIndex; + dgInt32 m_criticalSectionLock; + + static dgVector m_padding; + friend class dgBody; + friend class dgWorld; + friend class dgCollisionScene; +}; + +DG_INLINE dgFloat32 dgCollisionCompound::dgOOBBTestData::UpdateSeparatingDistance(const dgVector& box0Min, const dgVector& box0Max, const dgVector& box1Min, const dgVector& box1Max) const +{ + const dgVector minBox(box0Min - box1Max); + const dgVector maxBox(box0Max - box1Min); + const dgVector mask((minBox * maxBox) < dgVector::m_zero); + const dgVector boxDist ((maxBox.Abs()).GetMin(minBox.Abs())); + + //dgVector dist((mask & m_maxDist) | boxDist.AndNot(mask)); + dgVector dist(boxDist.Select(m_maxDist, mask)); + + dist = dist.GetMin(dist.ShiftTripleRight()); + dist = dist.GetMin(dist.ShiftTripleRight()); + return dist.GetScalar(); +} + + +#endif + diff --git a/thirdparty/src/newton/dgPhysics/dgCollisionCompoundFractured.cpp b/thirdparty/src/newton/dgPhysics/dgCollisionCompoundFractured.cpp new file mode 100644 index 000000000..2cd3e03bf --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgCollisionCompoundFractured.cpp @@ -0,0 +1,1758 @@ +/* Copyright (c) <2003-2019> +* +* 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 "dgPhysicsStdafx.h" +#include "dgWorld.h" +#include "dgMeshEffect.h" +#include "dgDynamicBody.h" +#include "dgCollisionInstance.h" +#include "dgCollisionConvexHull.h" +#include "dgCollisionCompoundFractured.h" + + +#define DG_FRACTURE_AABB_GUARD_DISTANCE 1.0f +#define DG_FRACTURE_MAX_METERIAL_COUNT 256 + +dgCollisionCompoundFractured::dgConectivityGraph::dgConectivityGraph (dgMemoryAllocator* const allocator) + :dgGraph(allocator) +{ +} + +dgCollisionCompoundFractured::dgConectivityGraph::dgConectivityGraph (const dgConectivityGraph& source) + :dgGraph(source.GetAllocator()) +{ + dgTree filter(GetAllocator()); + + for (dgConectivityGraph::dgListNode* node = source.GetFirst(); node != source.GetLast(); node = node->GetNext() ) { + dgListNode* const newNode = dgGraph::AddNode(); + + dgDebriNodeInfo& srcData = node->GetInfo().m_nodeData; + dgDebriNodeInfo& data = newNode->GetInfo().m_nodeData; + data.m_mesh = srcData.m_mesh; + data.m_mesh->AddRef(); + + filter.Insert(newNode, node); + } + + for (dgListNode* node = source.GetFirst(); node != source.GetLast(); node = node->GetNext()) { + dgListNode* const myNode = filter.Find(node)->GetInfo(); + for (dgGraphNode::dgListNode* edgeNode = node->GetInfo().GetFirst(); edgeNode; edgeNode = edgeNode->GetNext()) { + dgListNode* const otherNode = filter.Find(edgeNode->GetInfo().m_node)->GetInfo(); + dgGraphNode::dgListNode* const myEdgeNode = myNode->GetInfo().AddEdge (otherNode); + myEdgeNode->GetInfo().m_edgeData.m_normal = edgeNode->GetInfo().m_edgeData.m_normal; + } + } +} + + +dgCollisionCompoundFractured::dgConectivityGraph::~dgConectivityGraph () +{ +} + +void dgCollisionCompoundFractured::dgConectivityGraph::Serialize(dgSerialize callback, void* const userData) const +{ + dgInt32 count = GetCount(); + + dgInt32 index = 0; + dgTree enumMap (GetAllocator()); + callback (userData, &count, dgInt32 (sizeof (dgInt32))); + for (dgConectivityGraph::dgListNode* node = GetFirst(); node != GetLast(); node = node->GetNext() ) { + enumMap.Insert(index, node); + index ++; + + dgDebriNodeInfo& data = node->GetInfo().m_nodeData; + dgInt32 nodeIndex = data.m_shapeNode->GetKey(); + callback (userData, &nodeIndex, dgInt32 (sizeof (dgInt32))); + data.m_mesh->Serialize(callback, userData); + } + + dgListNode* const lastNode = GetLast(); + dgDebriNodeInfo& data = lastNode->GetInfo().m_nodeData; + data.m_mesh->Serialize(callback, userData); + + for (dgListNode* node = GetFirst(); node != GetLast(); node = node->GetNext()) { + dgInt32 count1 = node->GetInfo().GetCount(); + callback (userData, &count1, dgInt32 (sizeof (dgInt32))); + for (dgGraphNode::dgListNode* edgeNode = node->GetInfo().GetFirst(); edgeNode; edgeNode = edgeNode->GetNext()) { + dgListNode* const otherNode = edgeNode->GetInfo().m_node; + dgInt32 index1 = enumMap.Find(otherNode)->GetInfo(); + dgVector normal (edgeNode->GetInfo().m_edgeData.m_normal); + callback (userData, &index1, dgInt32 (sizeof (dgInt32))); + callback (userData, &normal, dgInt32 (sizeof (dgVector))); + } + } +} + +void dgCollisionCompoundFractured::dgConectivityGraph::Deserialize (dgCollisionCompoundFractured* const source, dgDeserialize callback, void* const userData) +{ + dgInt32 count = 0; + callback (userData, &count, dgInt32 (sizeof (dgInt32))); + + dgTree enumMap (GetAllocator()); + for (dgInt32 i = 0; i < count - 1; i ++) { + + dgListNode* const node = dgGraph::AddNode(); + enumMap.Insert(node, i); + + dgDebriNodeInfo& data = node->GetInfo().m_nodeData; + dgInt32 nodeIndex; + callback (userData, &nodeIndex, dgInt32 (sizeof (dgInt32))); + data.m_shapeNode = source->FindNodeByIndex (nodeIndex); + dgAssert (data.m_shapeNode); + data.m_mesh = new (GetAllocator()) dgMesh (GetAllocator(), callback, userData); + } + + dgListNode* const nodePtr = dgGraph::AddNode(); + dgDebriNodeInfo& data = nodePtr->GetInfo().m_nodeData; + data.m_mesh = new (GetAllocator()) dgMesh (GetAllocator(), callback, userData); + + for (dgListNode* node = GetFirst(); node != GetLast(); node = node->GetNext()) { + dgInt32 count1; + callback (userData, &count1, dgInt32 (sizeof (dgInt32))); + + for (dgInt32 i = 0; i < count1; i ++) { + dgInt32 index; + dgVector normal; + callback (userData, &index, dgInt32 (sizeof (dgInt32))); + callback (userData, &normal, dgInt32 (sizeof (dgVector))); + + dgListNode* const otherNode = enumMap.Find(index)->GetInfo(); + dgGraphNode::dgListNode* const edgeNode = node->GetInfo().AddEdge (otherNode); + edgeNode->GetInfo().m_edgeData.m_normal = normal; + } + } +} + + + +class dgCollisionCompoundFractured::dgFractureBuilder: public dgTree +{ + public: + class dgPerimenterEdge + { + public: + dgPerimenterEdge* m_next; + dgPerimenterEdge* m_prev; + const dgBigVector* m_vertex; + }; + + class dgFractureConectivity: public dgGraph + { + public: + dgFractureConectivity(dgMemoryAllocator* const allocator) + :dgGraph (allocator) + { + } + }; + + class dgConvexSolidVertices: public dgList + { + public: + dgConvexSolidVertices(dgMemoryAllocator* const allocator) + :dgList (allocator) + { + } + }; + + class dgConvexSolidArray: public dgTree + { + public: + dgConvexSolidArray(dgMemoryAllocator* const allocator) + :dgTree (allocator) + { + } + }; + + + dgFractureBuilder (dgMemoryAllocator* const allocator, dgMeshEffect* const solidMesh, dgInt32 pointcloudCount, const dgFloat32* const vertexCloud, dgInt32 strideInBytes, int materialId, const dgMatrix& textureProjectionMatrix) + :dgTree(allocator) + ,m_conectivity(allocator) + { + dgAssert (0); +/* + dgStack buffer(pointcloudCount + 16); + dgBigVector* const pool = &buffer[0]; + dgFloat64 quantizeFactor = dgFloat64 (16.0f); + dgFloat64 invQuantizeFactor = dgFloat64 (1.0f) / quantizeFactor; + dgInt32 stride = strideInBytes / sizeof (dgFloat32); + + dgBigVector minAABB; + dgBigVector maxAABB; + solidMesh->CalculateAABB (minAABB, maxAABB); + for (dgInt32 i = 0; i < pointcloudCount; i ++) { + dgFloat64 x = vertexCloud[i * stride + 0]; + dgFloat64 y = vertexCloud[i * stride + 1]; + dgFloat64 z = vertexCloud[i * stride + 2]; + x = floor (x * quantizeFactor) * invQuantizeFactor; + y = floor (y * quantizeFactor) * invQuantizeFactor; + z = floor (z * quantizeFactor) * invQuantizeFactor; + pool[i] = dgBigVector (x, y, z, dgFloat64 (0.0f)); + } + + // add the bbox as a barrier + int count = pointcloudCount; + pool[count + 0] = dgBigVector ( minAABB.m_x, minAABB.m_y, minAABB.m_z, dgFloat64 (0.0f)); + pool[count + 1] = dgBigVector ( maxAABB.m_x, minAABB.m_y, minAABB.m_z, dgFloat64 (0.0f)); + pool[count + 2] = dgBigVector ( minAABB.m_x, maxAABB.m_y, minAABB.m_z, dgFloat64 (0.0f)); + pool[count + 3] = dgBigVector ( maxAABB.m_x, maxAABB.m_y, minAABB.m_z, dgFloat64 (0.0f)); + pool[count + 4] = dgBigVector ( minAABB.m_x, minAABB.m_y, maxAABB.m_z, dgFloat64 (0.0f)); + pool[count + 5] = dgBigVector ( maxAABB.m_x, minAABB.m_y, maxAABB.m_z, dgFloat64 (0.0f)); + pool[count + 6] = dgBigVector ( minAABB.m_x, maxAABB.m_y, maxAABB.m_z, dgFloat64 (0.0f)); + pool[count + 7] = dgBigVector ( maxAABB.m_x, maxAABB.m_y, maxAABB.m_z, dgFloat64 (0.0f)); + count += 8; + + dgStack indexList(count); + count = dgVertexListToIndexList(&pool[0].m_x, sizeof (dgBigVector), 3, count, &indexList[0], dgFloat64 (5.0e-2f)); + dgAssert (count >= 8); + + dgFloat64 maxSize = dgMax(maxAABB.m_x - minAABB.m_x, maxAABB.m_y - minAABB.m_y, maxAABB.m_z - minAABB.m_z); + minAABB -= dgBigVector (maxSize, maxSize, maxSize, dgFloat64 (0.0f)); + maxAABB += dgBigVector (maxSize, maxSize, maxSize, dgFloat64 (0.0f)); + + // add the a guard zone, so that we do not have to clip + dgInt32 guadVertexKey = count; + pool[count + 0] = dgBigVector ( minAABB.m_x, minAABB.m_y, minAABB.m_z, dgFloat64 (0.0f)); + pool[count + 1] = dgBigVector ( maxAABB.m_x, minAABB.m_y, minAABB.m_z, dgFloat64 (0.0f)); + pool[count + 2] = dgBigVector ( minAABB.m_x, maxAABB.m_y, minAABB.m_z, dgFloat64 (0.0f)); + pool[count + 3] = dgBigVector ( maxAABB.m_x, maxAABB.m_y, minAABB.m_z, dgFloat64 (0.0f)); + pool[count + 4] = dgBigVector ( minAABB.m_x, minAABB.m_y, maxAABB.m_z, dgFloat64 (0.0f)); + pool[count + 5] = dgBigVector ( maxAABB.m_x, minAABB.m_y, maxAABB.m_z, dgFloat64 (0.0f)); + pool[count + 6] = dgBigVector ( minAABB.m_x, maxAABB.m_y, maxAABB.m_z, dgFloat64 (0.0f)); + pool[count + 7] = dgBigVector ( maxAABB.m_x, maxAABB.m_y, maxAABB.m_z, dgFloat64 (0.0f)); + count += 8; + + dgDelaunayTetrahedralization delaunayTetrahedras (allocator, &pool[0].m_x, count, sizeof (dgBigVector), dgFloat32 (0.0f)); + delaunayTetrahedras.RemoveUpperHull (); + + dgInt32 tetraCount = delaunayTetrahedras.GetCount(); + dgStack voronoiPoints(tetraCount + 32); + dgStack tetradrumNode(tetraCount); + + dgConvexSolidArray delanayNodes (allocator); + dgTree, dgConvexSolidArray::dgTreeNode*> adjacentCell(allocator); + + dgInt32 index = 0; + const dgHullVector* const delanayPoints = delaunayTetrahedras.GetHullVertexArray(); + for (dgDelaunayTetrahedralization::dgListNode* node = delaunayTetrahedras.GetFirst(); node; node = node->GetNext()) { + dgConvexHull4dTetraherum& tetra = node->GetInfo(); + voronoiPoints[index] = tetra.CircumSphereCenter (delanayPoints); + tetradrumNode[index] = node; + + dgConvexSolidArray::dgTreeNode* array[4]; + for (dgInt32 i = 0; i < 4; i ++) { + dgConvexSolidArray::dgTreeNode* header = delanayNodes.Find(tetra.m_faces[0].m_index[i]); + if (!header) { + dgConvexSolidVertices list (allocator); + header = delanayNodes.Insert(list, tetra.m_faces[0].m_index[i]); + } + header->GetInfo().Append (index); + array[i] = header; + } + + for (dgInt32 i = 0; i < 4; i ++) { + dgTree, dgConvexSolidArray::dgTreeNode*>::dgTreeNode* header = adjacentCell.Find (array[i]); + if (!header) { + dgTree list (allocator); + header = adjacentCell.Insert(list, array[i]); + } + + for (dgInt32 j = 0; j < 4; j ++) { + if (i != j) { + header->GetInfo().Insert(array[j], array[j]); + } + } + } + + index ++; + } + + dgConvexSolidArray::Iterator iter (delanayNodes); + dgFloat32 normalAngleInRadians = 30.0f * dgDEG2RAD; + + dgTree graphMap(allocator); + for (iter.Begin(); iter; iter ++) { + + dgConvexSolidArray::dgTreeNode* const nodeNode = iter.GetNode(); + const dgList& list = nodeNode->GetInfo(); + dgInt32 key = nodeNode->GetKey(); + if (key < guadVertexKey) { + dgBigVector pointArray[512]; + dgInt32 indexArray[512]; + + dgInt32 count = 0; + for (dgList::dgListNode* ptr = list.GetFirst(); ptr; ptr = ptr->GetNext()) { + dgInt32 i = ptr->GetInfo(); + pointArray[count] = voronoiPoints[i]; + count ++; + dgAssert (count < dgInt32 (sizeof (pointArray) / sizeof (pointArray[0]))); + } + + count = dgVertexListToIndexList(&pointArray[0].m_x, sizeof (dgBigVector), 3, count, &indexArray[0], dgFloat64 (1.0e-3f)); + if (count >= 4) { + dgMeshEffect* const convexMesh = new (allocator) dgMeshEffect (allocator, &pointArray[0].m_x, count, sizeof (dgBigVector), dgFloat64 (0.0f)); + if (convexMesh->GetCount()) { + convexMesh->AddRef(); + Insert (convexMesh, key); + + convexMesh->CalculateNormals(normalAngleInRadians); + convexMesh->UniformBoxMapping (materialId, textureProjectionMatrix); + + dgInt32 vCount = convexMesh->GetVertexCount(); + for (dgInt32 i = 0; i < vCount; i ++) { + dgBigVector& point = convexMesh->GetVertex (i); + point.m_w = key; + } + dgInt32 aCount = convexMesh->GetPropertiesCount(); + for (dgInt32 i = 0; i < aCount; i ++) { + dgMeshEffect::dgVertexAtribute& attib = convexMesh->GetAttribute (i); + attib.m_vertex.m_w = key; + } + + dgFractureConectivity::dgListNode* const node = m_conectivity.AddNode (); + node->GetInfo().m_nodeData = key; + graphMap.Insert(node, nodeNode); + } + convexMesh->Release(); + } + } + } + + + dgTree, dgConvexSolidArray::dgTreeNode*>::Iterator adjacentIter (adjacentCell); + for (adjacentIter.Begin(); adjacentIter; adjacentIter++) { + dgConvexSolidArray::dgTreeNode* const solidTree0 = adjacentIter.GetKey(); + + dgTree::dgTreeNode* const mapNode0 = graphMap.Find(solidTree0); + if (mapNode0) { + dgFractureConectivity::dgListNode* const node0 = mapNode0->GetInfo(); + + const dgTree& cell = adjacentIter.GetNode()->GetInfo(); + dgTree::Iterator iter (cell); + for (iter.Begin(); iter; iter ++) { + dgConvexSolidArray::dgTreeNode* const solidTree1 = iter.GetNode()->GetInfo(); + dgTree::dgTreeNode* const mapNode1 = graphMap.Find(solidTree1); + if (mapNode1) { + dgFractureConectivity::dgListNode* const node1 = mapNode1->GetInfo(); + if (!IsPairConnected (node0, node1)) { + node0->GetInfo().AddEdge(node1); + node1->GetInfo().AddEdge(node0); + } + } + } + } + } + + dgAssert (SanityCheck()); + ClipFractureParts (solidMesh); + + for (dgFractureConectivity::dgListNode* node = m_conectivity.GetFirst(); node; node = node->GetNext()) { + dgInt32 index0 = node->GetInfo().m_nodeData; + + dgGraphNode::dgListNode* nextEdge; + for (dgGraphNode::dgListNode* edgeNode = node->GetInfo().GetFirst(); edgeNode; edgeNode = nextEdge) { + nextEdge = edgeNode->GetNext(); + dgFractureConectivity::dgListNode* const otherNode = edgeNode->GetInfo().m_node; + dgInt32 index1 = otherNode->GetInfo().m_nodeData; + if (!AreSolidNeigborg (index0, index1)) { + node->GetInfo().DeleteEdge(edgeNode); + } + } + } + +#if 0 +for (dgFractureConectivity::dgListNode* node = m_conectivity.GetFirst(); node; node = node->GetNext()) { + dgInt32 index = node->GetInfo().m_nodeData; + dgTrace (("node %d: ", index)); + for (dgGraphNode::dgListNode* edge = node->GetInfo().GetFirst(); edge; edge = edge->GetNext()) { + dgFractureConectivity::dgListNode* const otherNode = edge->GetInfo().m_node; + dgInt32 index1 = otherNode->GetInfo().m_nodeData; + dgTrace (("%d ", index1)); + } + dgTrace (("\n")); +} +#endif +*/ + dgAssert (SanityCheck()); + } + + ~dgFractureBuilder() + { + Iterator iter (*this); + for (iter.Begin(); iter; iter ++) { + dgMeshEffect* const mesh = iter.GetNode()->GetInfo(); + mesh->Release(); + } + } + + + bool IsPairConnected (dgFractureConectivity::dgListNode* const nodeA, dgFractureConectivity::dgListNode* const nodeB) const + { + for (dgGraphNode::dgListNode* edgeNodeAB = nodeA->GetInfo().GetFirst(); edgeNodeAB; edgeNodeAB = edgeNodeAB->GetNext()) { + dgFractureConectivity::dgListNode* const otherNode = edgeNodeAB->GetInfo().m_node; + if (otherNode == nodeB) + return true; + } + return false; + } + + bool ArePlaneCoplanar (dgMeshEffect* const meshA, void* faceA, const dgBigVector& planeA, dgMeshEffect* const meshB, void* faceB, const dgBigVector& planeB) const + { + if ((planeA.DotProduct(planeB & dgBigVector::m_triplexMask).GetScalar() < dgFloat64(-1.0 + 1.0e-6f)) && ((fabs(planeA.m_w + planeB.m_w) < dgFloat64(1.0e-6f)))) { + const dgBigVector* const pointsA = (dgBigVector*)meshA->GetVertexPool(); + const dgBigVector* const pointsB = (dgBigVector*)meshB->GetVertexPool(); + + dgInt32 indexA[128]; + dgInt32 indexB[128]; + + dgInt32 indexCountA = meshA->GetFaceIndexCount(faceA); + dgInt32 indexCountB = meshB->GetFaceIndexCount(faceB); + dgAssert(indexCountA < sizeof (indexA) / sizeof(indexA[0])); + dgAssert(indexCountB < sizeof (indexB) / sizeof(indexB[0])); + + meshA->GetFaceIndex(faceA, indexA); + meshA->GetFaceIndex(faceB, indexB); + + //dgTrace (("faceA:\n")); + dgPerimenterEdge subdivision[256]; + dgAssert((2 * (indexCountA + indexCountB)) < dgInt32(sizeof (subdivision) / sizeof (subdivision[0]))); + for (dgInt32 i = 1; i < indexCountB; i++) { + subdivision[i].m_vertex = &pointsB[indexB[i]]; + subdivision[i].m_prev = &subdivision[i - 1]; + subdivision[i].m_next = &subdivision[i + 1]; + + //dgTrace (("%f %f %f\n", pointsB[indexB[i]].m_x, pointsB[indexB[i]].m_y, pointsB[indexB[i]].m_z)); + } + subdivision[0].m_vertex = &pointsB[indexB[0]]; + subdivision[0].m_next = &subdivision[1]; + subdivision[0].m_prev = &subdivision[indexCountB - 1]; + + subdivision[indexCountB - 1].m_next = &subdivision[0]; + + dgInt32 edgeIndex = indexCountB; + + dgBigVector outputPool[128]; + dgPerimenterEdge* edgeClipped[2]; + dgBigVector* output = &outputPool[0]; + edgeClipped[0] = NULL; + edgeClipped[1] = NULL; + + //dgTrace (("faceB:\n")); + dgPerimenterEdge* poly = &subdivision[0]; + dgInt32 i0 = indexCountA - 1; + for (dgInt32 i1 = 0; i1 < indexCountA; i1++) { + const dgBigVector& q0 = pointsA[indexA[i0]]; + const dgBigVector& q1 = pointsA[indexA[i1]]; + dgBigVector n(planeA.CrossProduct(q1 - q0)); + dgAssert(n.m_w == dgFloat32(0.0f)); + dgBigPlane plane(n, -n.DotProduct(q0).GetScalar()); + i0 = i1; + //dgTrace (("%f %f %f\n", q0.m_x, q0.m_y, q0.m_z)); + + dgInt32 count = 0; + dgPerimenterEdge* tmp = poly; + dgInt32 isInside = 0; + dgFloat64 test0 = plane.Evalue(*tmp->m_vertex); + do { + dgFloat64 test1 = plane.Evalue(*tmp->m_next->m_vertex); + if (test0 >= dgFloat32(0.0f)) { + isInside |= 1; + if (test1 < dgFloat32(0.0f)) { + const dgBigVector& p0 = *tmp->m_vertex; + const dgBigVector& p1 = *tmp->m_next->m_vertex; + + dgBigVector dp(p1 - p0); + dgAssert(dp.m_w == dgFloat32(0.0f)); + dgFloat64 den = plane.DotProduct(dp).GetScalar(); + if (fabs(den) < dgFloat32(1.0e-24f)) { + den = (den >= dgFloat32(0.0f)) ? dgFloat32(1.0e-24f) : dgFloat32(-1.0e-24f); + } + + den = test0 / den; + if (den >= dgFloat32(0.0f)) { + den = dgFloat32(0.0f); + } else if (den <= -1.0f) { + den = dgFloat32(-1.0f); + } + output[0] = p0 - dp.Scale(den); + edgeClipped[0] = tmp; + count++; + } + } else if (test1 >= dgFloat32(0.0f)) { + const dgBigVector& p0 = *tmp->m_vertex; + const dgBigVector& p1 = *tmp->m_next->m_vertex; + isInside |= 1; + dgBigVector dp(p1 - p0); + dgAssert(dp.m_w == dgFloat32(0.0f)); + dgFloat64 den = plane.DotProduct(dp).GetScalar(); + if (fabs(den) < dgFloat32(1.0e-24f)) { + den = (den >= dgFloat32(0.0f)) ? dgFloat32(1.0e-24f) : dgFloat32(-1.0e-24f); + } + den = test0 / den; + if (den >= dgFloat32(0.0f)) { + den = dgFloat32(0.0f); + } else if (den <= -1.0f) { + den = dgFloat32(-1.0f); + } + output[1] = p0 - dp.Scale(den); + edgeClipped[1] = tmp; + count++; + } + test0 = test1; + tmp = tmp->m_next; + } while ((tmp != poly) && count < 2); + + if (!isInside) { + return false; + } + + if (count == 2) { + dgPerimenterEdge* const newEdge = &subdivision[edgeIndex]; + newEdge->m_next = edgeClipped[1]; + newEdge->m_prev = edgeClipped[0]; + edgeClipped[0]->m_next = newEdge; + edgeClipped[1]->m_prev = newEdge; + + newEdge->m_vertex = &output[0]; + edgeClipped[1]->m_vertex = &output[1]; + poly = newEdge; + + output += 2; + edgeIndex++; + dgAssert(edgeIndex < dgInt32(sizeof (subdivision) / sizeof (subdivision[0]))); + } + } + //dgTrace (("\n")); + dgAssert(poly); + dgBigVector area(dgFloat32(0.0f)); + dgBigVector r0(*poly->m_vertex); + dgBigVector r1(*poly->m_next->m_vertex); + dgBigVector r1r0(r1 - r0); + dgPerimenterEdge* polyPtr = poly->m_next->m_next; + do { + dgBigVector r2(*polyPtr->m_vertex); + dgBigVector r2r0(r2 - r0); + area += r2r0.CrossProduct(r1r0); + r1r0 = r2r0; + polyPtr = polyPtr->m_next; + } while (polyPtr != poly); + dgAssert(area.m_w == dgFloat32(0.0f)); + return fabs(area.DotProduct(planeA).GetScalar()) > dgFloat32(1.0e-5f); + } + + return false; + } + + bool AreSolidNeigborg (int indexA, int indexB) const + { + dgMeshEffect* const meshA = Find(indexA)->GetInfo(); + dgMeshEffect* const meshB = Find(indexB)->GetInfo(); + + const dgBigVector* const pointsA = (dgBigVector*)meshA->GetVertexPool(); + const dgBigVector* const pointsB = (dgBigVector*)meshB->GetVertexPool(); + + dgBigVector planeB_array[512]; + + dgInt32 planeB_Count = 0; + for (void* faceB = meshB->GetFirstFace(); faceB; faceB = meshB->GetNextFace(faceB)) { + if (!meshB->IsFaceOpen(faceB)) { + dgInt32 vertexIndexB = meshB->GetVertexIndex(faceB); + dgBigVector planeB(meshB->CalculateFaceNormal(faceB)); + planeB.m_w = -planeB.DotProduct(pointsB[vertexIndexB]).GetScalar(); + planeB_array[planeB_Count] = planeB; + planeB_Count++; + dgAssert(planeB_Count < sizeof (planeB_array) / sizeof (planeB_array[0])); + } + } + + for (void* faceA = meshA->GetFirstFace(); faceA; faceA = meshA->GetNextFace(faceA)) { + if (!meshA->IsFaceOpen(faceA)) { + dgInt32 vertexIndexA = meshA->GetVertexIndex(faceA); + dgBigVector planeA(meshA->CalculateFaceNormal(faceA)); + planeA.m_w = -planeA.DotProduct(pointsA[vertexIndexA]).GetScalar(); + + dgInt32 index = 0; + for (void* faceB = meshB->GetFirstFace(); faceB; faceB = meshB->GetNextFace(faceB)) { + if (!meshB->IsFaceOpen(faceB)) { + if (ArePlaneCoplanar(meshA, faceA, planeA, meshB, faceB, planeB_array[index])) { + return true; + } + index++; + } + } + } + } + return false; + } + + void ClipFractureParts (dgMeshEffect* const solidMesh) + { + dgFractureBuilder::dgFractureConectivity::dgListNode* nextNode; + for (dgFractureConectivity::dgListNode* node = m_conectivity.GetFirst(); node; node = nextNode) { + nextNode = node->GetNext(); + dgInt32 index = node->GetInfo().m_nodeData; + dgTreeNode* const fractureNode = Find(index); + dgAssert (fractureNode); + dgMeshEffect* const voronoiConvex = fractureNode->GetInfo(); + dgMeshEffect* const fracturePiece = solidMesh->ConvexMeshIntersection (voronoiConvex); + if (fracturePiece) { + voronoiConvex->Release(); + fractureNode->GetInfo() = fracturePiece; + } else { + m_conectivity.DeleteNode(node); + } + } + } + + bool SanityCheck() const + { + for (dgFractureConectivity::dgListNode* rootNode = m_conectivity.GetFirst(); rootNode; rootNode = rootNode->GetNext() ) { + dgTree filter(GetAllocator()); + for (dgGraphNode::dgListNode* edgeNode = rootNode->GetInfo().GetFirst(); edgeNode; edgeNode = edgeNode->GetNext()) { + dgFractureConectivity::dgListNode* node = edgeNode->GetInfo().m_node; + dgAssert (!filter.Find(node)); + filter.Insert(node, node); + } + } + return true; + } + + dgFractureConectivity m_conectivity; +}; + + + +dgCollisionCompoundFractured::dgDebriNodeInfo::dgDebriNodeInfo () + :m_mesh(NULL) + ,m_shapeNode(NULL) + ,m_lru(0) +{ +} + +dgCollisionCompoundFractured::dgDebriNodeInfo::~dgDebriNodeInfo () +{ + if (m_mesh) { + m_mesh->Release(); + } +} + + +dgCollisionCompoundFractured::dgVertexBuffer::dgVertexBuffer(dgInt32 vertsCount, dgMemoryAllocator* allocator) + :m_allocator(allocator) + ,m_vertexCount(vertsCount) +{ + m_uv = (dgFloat32 *) m_allocator->Malloc (2 * vertsCount * dgInt32 (sizeof (dgFloat32))); + m_vertex = (dgFloat32 *) m_allocator->Malloc (3 * vertsCount * dgInt32 (sizeof (dgFloat32))); + m_normal = (dgFloat32 *) m_allocator->Malloc (3 * vertsCount * dgInt32 (sizeof (dgFloat32))); +} + +dgCollisionCompoundFractured::dgVertexBuffer::~dgVertexBuffer () +{ + m_allocator->Free (m_normal); + m_allocator->Free (m_vertex); + m_allocator->Free (m_uv); +} + +dgCollisionCompoundFractured::dgVertexBuffer::dgVertexBuffer (dgMemoryAllocator* const allocator, dgDeserialize callback, void* const userData) +{ + m_allocator = allocator; + callback (userData, &m_vertexCount, dgInt32 (sizeof (dgInt32))); + + m_uv = (dgFloat32 *) m_allocator->Malloc (2 * m_vertexCount * dgInt32 (sizeof (dgFloat32))); + m_vertex = (dgFloat32 *) m_allocator->Malloc (3 * m_vertexCount * dgInt32 (sizeof (dgFloat32))); + m_normal = (dgFloat32 *) m_allocator->Malloc (3 * m_vertexCount * dgInt32 (sizeof (dgFloat32))); + + callback (userData, m_vertex, size_t (3 * m_vertexCount * dgInt32 (sizeof (dgFloat32)))); + callback (userData, m_normal, size_t (3 * m_vertexCount * dgInt32 (sizeof (dgFloat32)))); + callback (userData, m_uv, size_t (2 * m_vertexCount * dgInt32 (sizeof (dgFloat32)))); +} + +void dgCollisionCompoundFractured::dgVertexBuffer::Serialize(dgSerialize callback, void* const userData) const +{ + callback (userData, &m_vertexCount, dgInt32 (sizeof (dgInt32))); + callback (userData, m_vertex, size_t (3 * m_vertexCount * dgInt32 (sizeof (dgFloat32)))); + callback (userData, m_normal, size_t (3 * m_vertexCount * dgInt32 (sizeof (dgFloat32)))); + callback (userData, m_uv, size_t (2 * m_vertexCount * dgInt32 (sizeof (dgFloat32)))); +} + + +dgCollisionCompoundFractured::dgSubMesh::dgSubMesh (dgMemoryAllocator* const allocator) + :m_indexes(NULL) + ,m_allocator(allocator) + ,m_material(0) + ,m_faceCount(0) + ,m_materialOrdinal(0) + ,m_visibleFaces(true) +{ +} + +dgCollisionCompoundFractured::dgSubMesh::~dgSubMesh () +{ + if (m_indexes) { + m_allocator->Free (m_indexes); + } +} + +void dgCollisionCompoundFractured::dgSubMesh::Serialize(dgSerialize callback, void* const userData) const +{ + dgInt32 visible = m_visibleFaces ? 1 : 0; + callback (userData, &m_material, dgInt32 (sizeof (dgInt32))); + callback (userData, &m_materialOrdinal, dgInt32 (sizeof (dgInt32))); + callback (userData, &m_faceCount, dgInt32 (sizeof (dgInt32))); + callback (userData, &visible, dgInt32 (sizeof (dgInt32))); + callback (userData, m_indexes, size_t (3 * m_faceCount * dgInt32 (sizeof (dgInt32)))); +} + +dgCollisionCompoundFractured::dgSharedNodeMesh::dgSharedNodeMesh () +{ +} + +dgCollisionCompoundFractured::dgSharedNodeMesh::~dgSharedNodeMesh () +{ +} + + +dgCollisionCompoundFractured::dgMesh::dgMesh(dgMemoryAllocator* const allocator) + :dgList(allocator) + ,m_vertexOffsetStart(0) + ,m_vertexCount(0) + ,m_isVisible(false) +{ +} + +dgCollisionCompoundFractured::dgMesh::~dgMesh() +{ +} + +dgCollisionCompoundFractured::dgMesh::dgMesh (dgMemoryAllocator* const allocator, dgDeserialize callback, void* const userData) + :dgList(allocator), dgRefCounter () +{ + dgInt32 count; + dgInt32 isVisible; + callback (userData, &m_vertexOffsetStart, dgInt32 (sizeof (m_vertexOffsetStart))); + callback (userData, &m_vertexCount, dgInt32 (sizeof (m_vertexCount))); + callback (userData, &isVisible, dgInt32 (sizeof (m_vertexCount))); + callback (userData, &count, dgInt32 (sizeof (dgInt32))); + m_isVisible = isVisible ? true : false; + + for (dgInt32 i = 0; i < count; i ++) { + dgInt32 visible; + dgInt32 material; + dgInt32 faceCount; + dgInt32 materialOrdinal; + + callback (userData, &material, dgInt32 (sizeof (dgInt32))); + callback (userData, &materialOrdinal, dgInt32 (sizeof (dgInt32))); + callback (userData, &faceCount, dgInt32 (sizeof (dgInt32))); + callback (userData, &visible, dgInt32 (sizeof (dgInt32))); + + dgSubMesh* const subMesh = AddgSubMesh (faceCount * 3, material); + subMesh->m_faceCount = faceCount; + subMesh->m_material = material; + subMesh->m_materialOrdinal = materialOrdinal; + subMesh->m_visibleFaces = visible ? true : false; + callback (userData, subMesh->m_indexes, size_t (3 * faceCount * dgInt32 (sizeof (dgInt32)))); + } +} + +void dgCollisionCompoundFractured::dgMesh::Serialize(dgSerialize callback, void* const userData) const +{ + dgInt32 count = GetCount(); + dgInt32 visible = m_isVisible ? 1 : 0; + + callback (userData, &m_vertexOffsetStart, dgInt32 (sizeof (m_vertexOffsetStart))); + callback (userData, &m_vertexCount, dgInt32 (sizeof (m_vertexCount))); + callback (userData, &visible, dgInt32 (sizeof (m_vertexCount))); + callback (userData, &count, dgInt32 (sizeof (dgInt32))); + for (dgListNode* node = GetFirst(); node; node = node->GetNext()) { + dgSubMesh& subMesh = node->GetInfo(); + subMesh.Serialize(callback, userData); + } +} + +dgCollisionCompoundFractured::dgSubMesh* dgCollisionCompoundFractured::dgMesh::AddgSubMesh(dgInt32 indexCount, dgInt32 material) +{ + dgSubMesh tmp (GetAllocator()); + dgSubMesh& subMesh = Append(tmp)->GetInfo(); + + subMesh.m_visibleFaces = true; + + subMesh.m_material = material; + subMesh.m_faceCount = indexCount / 3; + + subMesh.m_indexes = (dgInt32 *) subMesh.m_allocator->Malloc (indexCount * dgInt32 (sizeof (dgInt32))); + return &subMesh; +} + + + dgCollisionCompoundFractured::dgConectivityGraph::dgListNode* dgCollisionCompoundFractured::dgConectivityGraph::AddNode (dgFlatVertexArray& vertexArray, dgMeshEffect* const factureVisualMesh, dgTreeArray::dgTreeNode* const collisionNode, dgInt32 interiorMaterialBase) +{ + dgListNode* const node = dgGraph::AddNode (); + dgDebriNodeInfo& data = node->GetInfo().m_nodeData; + + data.m_mesh = new (GetAllocator()) dgMesh(GetAllocator()); + data.m_shapeNode = collisionNode; + + dgInt32 vertexCount = factureVisualMesh->GetPropertiesCount(); + + dgStackvertex (vertexCount); + dgStacknormal (vertexCount); + dgStackuv0 (vertexCount); + dgStackuv1 (vertexCount); + dgAssert (0); +// factureVisualMesh->GetVertexStreams (sizeof (dgVector), &vertex[0].m_x, sizeof (dgVector), &normal[0].m_x, sizeof (dgVector), &uv0[0].m_x, sizeof (dgVector), &uv1[0].m_x); + + // extract the materials index array for mesh + dgInt32 baseVertexCount = vertexArray.m_count; + data.m_mesh->m_vertexOffsetStart = baseVertexCount; + + vertexArray[baseVertexCount + vertexCount].m_point[0] = 0.0f; + for (dgInt32 i = 0; i < vertexCount; i ++) { + dgInt32 j = baseVertexCount + i; + vertexArray[j].m_point[0] = vertex[i].m_x; + vertexArray[j].m_point[1] = vertex[i].m_y; + vertexArray[j].m_point[2] = vertex[i].m_z; + vertexArray[j].m_point[3] = normal[i].m_x; + vertexArray[j].m_point[4] = normal[i].m_y; + vertexArray[j].m_point[5] = normal[i].m_z; + vertexArray[j].m_point[6] = uv0[i].m_x; + vertexArray[j].m_point[7] = uv0[i].m_y; + vertexArray[j].m_point[8] = uv1[i].m_x; + vertexArray[j].m_point[9] = uv1[i].m_y; + } + + + dgStack indexBuffer (vertexCount); + data.m_mesh->m_vertexCount = dgVertexListToIndexList (&vertexArray[baseVertexCount].m_point[0], sizeof (dgFlatVertex), sizeof (dgFlatVertex), 0, vertexCount, &indexBuffer[0], dgFloat32 (1.0e-6f)); + vertexArray.m_count += data.m_mesh->m_vertexCount; + + dgMeshEffect::dgIndexArray* const geometryHandle = factureVisualMesh->MaterialGeometryBegin(); + for (dgInt32 handle = factureVisualMesh->GetFirstMaterial (geometryHandle); handle != -1; handle = factureVisualMesh->GetNextMaterial (geometryHandle, handle)) { + dgInt32 material = factureVisualMesh->GetMaterialID (geometryHandle, handle); + + bool isVisible = (material < interiorMaterialBase) ? true : false; + data.m_mesh->m_isVisible |= isVisible; + + dgInt32 indexCount = factureVisualMesh->GetMaterialIndexCount (geometryHandle, handle); + dgSubMesh* const segment = data.m_mesh->AddgSubMesh(indexCount, isVisible ? material : material - interiorMaterialBase); + + segment->m_visibleFaces = isVisible; + + factureVisualMesh->GetMaterialGetIndexStream (geometryHandle, handle, segment->m_indexes); + + for (dgInt32 i = 0; i < indexCount; i++) { + dgInt32 j = segment->m_indexes[i]; + segment->m_indexes[i] = indexBuffer[j]; + } + } + factureVisualMesh->MaterialGeomteryEnd(geometryHandle); + return node; +} + + +dgCollisionCompoundFractured::dgCollisionCompoundFractured (const dgCollisionCompoundFractured& source, const dgCollisionInstance* const myInstance) + :dgCollisionCompound(source, myInstance) + ,m_conectivity(source.m_conectivity) + ,m_conectivityMap (source.m_conectivityMap) + ,m_vertexBuffer(source.m_vertexBuffer) + ,m_impulseStrengthPerUnitMass(source.m_impulseStrengthPerUnitMass) + ,m_impulseAbsortionFactor(source.m_impulseAbsortionFactor) + ,m_density(dgFloat32 (-1.0f)) + ,m_lru(0) + ,m_materialCount(source.m_materialCount) + ,m_emitFracturedChunk(source.m_emitFracturedChunk) + ,m_emitFracturedCompound(source.m_emitFracturedCompound) + ,m_reconstructMainMesh(source.m_reconstructMainMesh) +{ + m_rtti |= dgCollisionCompoundBreakable_RTTI; + + m_vertexBuffer->AddRef(); + + dgTree nodeMap(m_allocator); + dgTreeArray::Iterator iter (source.m_array); + for (iter.Begin(); iter; iter ++) { + nodeMap.Insert(iter.GetKey(), iter.GetNode()); + } + + dgConectivityGraph::dgListNode* myNode = m_conectivity.GetFirst(); + for (dgConectivityGraph::dgListNode* node = source.m_conectivity.GetFirst(); node != source.m_conectivity.GetLast(); myNode = myNode->GetNext(), node = node->GetNext() ) { + dgDebriNodeInfo& nodeInfo = node->GetInfo().m_nodeData; + dgTreeArray::dgTreeNode* const nodeBase = nodeInfo.m_shapeNode; + + dgAssert (nodeMap.Find(nodeBase)); + dgInt32 index = nodeMap.Find(nodeBase)->GetInfo(); + + dgAssert (m_array.Find(index)); + dgDebriNodeInfo& myNodeInfo = myNode->GetInfo().m_nodeData; + myNodeInfo.m_shapeNode = m_array.Find(index); + } + + dgMesh* const mainMesh = new (m_world->GetAllocator()) dgMesh(m_world->GetAllocator()); + dgConectivityGraph::dgListNode* const mainNode = m_conectivity.dgGraph::AddNode (); + dgDebriNodeInfo& mainNodeData = mainNode->GetInfo().m_nodeData; + mainNodeData.m_mesh = mainMesh; + + BuildMainMeshSubMehes(); + m_conectivityMap.Pupolate(m_conectivity); + + m_density = -dgFloat32 (1.0f) / GetVolume(); + dgAssert (SanityCheck()); +} + +dgCollisionCompoundFractured::dgCollisionCompoundFractured (dgCollisionCompoundFractured& source, const dgList& island) + :dgCollisionCompound(source.m_world, NULL) + ,m_conectivity(source.GetAllocator()) + ,m_conectivityMap (source.GetAllocator()) + ,m_vertexBuffer(source.m_vertexBuffer) + ,m_impulseStrengthPerUnitMass(source.m_impulseStrengthPerUnitMass) + ,m_impulseAbsortionFactor(source.m_impulseAbsortionFactor) + ,m_density(source.m_density) + ,m_lru(0) + ,m_materialCount(source.m_materialCount) + ,m_emitFracturedChunk(source.m_emitFracturedChunk) + ,m_emitFracturedCompound(source.m_emitFracturedCompound) + ,m_reconstructMainMesh(source.m_reconstructMainMesh) +{ + m_collisionId = m_compoundFracturedCollision; + m_rtti |= dgCollisionCompoundBreakable_RTTI; + + m_vertexBuffer->AddRef(); + + dgCollisionCompound::BeginAddRemove (); + for (dgList::dgListNode* node = island.GetFirst(); node; node = node->GetNext()) { + dgConectivityGraph::dgListNode* const chunkNode = node->GetInfo(); + dgDebriNodeInfo& nodeInfo = chunkNode->GetInfo().m_nodeData; + + dgCollisionInstance* const chunkCollision = nodeInfo.m_shapeNode->GetInfo()->GetShape(); + dgTreeArray::dgTreeNode* const treeNode = dgCollisionCompound::AddCollision (chunkCollision); + + source.m_conectivityMap.Remove (chunkCollision); + source.dgCollisionCompound::RemoveCollision (nodeInfo.m_shapeNode); + source.m_conectivity.Unlink(chunkNode); + + m_conectivity.Append(chunkNode); + nodeInfo.m_shapeNode = treeNode; + } + dgCollisionCompound::EndAddRemove(false); + + dgMesh* const mainMesh = new (m_world->GetAllocator()) dgMesh(m_world->GetAllocator()); + dgConectivityGraph::dgListNode* const mainNode = m_conectivity.dgGraph::AddNode (); + dgDebriNodeInfo& mainNodeData = mainNode->GetInfo().m_nodeData; + mainNodeData.m_mesh = mainMesh; + + BuildMainMeshSubMehes(); + m_conectivityMap.Pupolate(m_conectivity); + + m_density = -dgFloat32 (1.0f) / GetVolume(); + dgAssert (SanityCheck()); +} + +dgCollisionCompoundFractured::dgCollisionCompoundFractured (dgWorld* const world, dgDeserialize deserialization, void* const userData, const dgCollisionInstance* const myInstance, dgInt32 revisionNumber) + :dgCollisionCompound (world, deserialization, userData, myInstance, revisionNumber) + ,m_conectivity (world->GetAllocator()) + ,m_conectivityMap (world->GetAllocator()) +{ + m_conectivity.Deserialize(this, deserialization, userData); + m_vertexBuffer = new (m_world->GetAllocator()) dgVertexBuffer(m_world->GetAllocator(), deserialization, userData); + + deserialization (userData, &m_impulseStrengthPerUnitMass, sizeof (m_impulseStrengthPerUnitMass)); + deserialization (userData, &m_impulseAbsortionFactor, sizeof (m_impulseAbsortionFactor)); + deserialization (userData, &m_density, sizeof (m_density)); + deserialization (userData, &m_materialCount, sizeof (m_materialCount)); +} + + +dgCollisionCompoundFractured::dgCollisionCompoundFractured ( + dgWorld* const world, dgMeshEffect* const solidMesh, int fracturePhysicsMaterialID, int pointcloudCount, const dgFloat32* const vertexCloud, int strideInBytes, int materialID, const dgMatrix& offsetMatrix, + OnEmitFractureChunkCallBack emitFracturedChunk, OnEmitNewCompundFractureCallBack emitNewCompoundFactured, OnReconstructFractureMainMeshCallBack reconstructMainMesh) + :dgCollisionCompound (world) + ,m_conectivity(world->GetAllocator()) + ,m_conectivityMap (world->GetAllocator()) + ,m_vertexBuffer(NULL) + ,m_impulseStrengthPerUnitMass(10.0f) + ,m_impulseAbsortionFactor(0.5f) + ,m_density(dgFloat32 (-1.0f)) + ,m_lru(0) + ,m_materialCount(0) + ,m_emitFracturedChunk(emitFracturedChunk) + ,m_emitFracturedCompound(emitNewCompoundFactured) + ,m_reconstructMainMesh(reconstructMainMesh) +{ + m_collisionId = m_compoundFracturedCollision; + m_rtti |= dgCollisionCompoundBreakable_RTTI; + + dgInt32 interiorMaterialBase = 1000; + dgFractureBuilder fractureBuilder (GetAllocator(), solidMesh, pointcloudCount, vertexCloud, strideInBytes, materialID + interiorMaterialBase, offsetMatrix); + + dgFlatVertexArray vertexArray(m_world->GetAllocator()); + dgTree conectinyMap(GetAllocator()); + + dgCollisionCompound::BeginAddRemove (); + for (dgFractureBuilder::dgFractureConectivity::dgListNode* node = fractureBuilder.m_conectivity.GetFirst(); node; node = node->GetNext()) { + dgInt32 index = node->GetInfo().m_nodeData; + dgFractureBuilder::dgTreeNode* const fractureNode = fractureBuilder.Find(index); + dgAssert (fractureNode); + dgMeshEffect* const fractureSubMesh = fractureNode->GetInfo(); + dgCollisionInstance* const collisionInstance = fractureSubMesh->CreateConvexCollision(world, dgFloat32 (1.0e-5f), fracturePhysicsMaterialID); + dgTreeArray::dgTreeNode* const shapeNode = AddCollision (collisionInstance); + dgConectivityGraph::dgListNode* const conectivityNode = m_conectivity.AddNode(vertexArray, fractureSubMesh, shapeNode, interiorMaterialBase); + conectinyMap.Insert(conectivityNode, index); + collisionInstance->Release(); + } + dgCollisionCompound::EndAddRemove (); + + for (dgFractureBuilder::dgFractureConectivity::dgListNode* node = fractureBuilder.m_conectivity.GetFirst(); node; node = node->GetNext()) { + dgInt32 index0 = node->GetInfo().m_nodeData; + dgConectivityGraph::dgListNode* const conectivityNode0 = conectinyMap.Find(index0)->GetInfo(); + for (dgGraphNode::dgListNode* edge = node->GetInfo().GetFirst(); edge; edge = edge->GetNext()) { + dgFractureBuilder::dgFractureConectivity::dgListNode* const otherNode = edge->GetInfo().m_node; + dgInt32 index1 = otherNode->GetInfo().m_nodeData; + dgConectivityGraph::dgListNode* const conectivityNode1 = conectinyMap.Find(index1)->GetInfo(); + dgGraphNode::dgListNode* const edgeNode = conectivityNode0->GetInfo().AddEdge(conectivityNode1); + + dgDebriNodeInfo& nodeInfo0 = conectivityNode0->GetInfo().m_nodeData; + dgDebriNodeInfo& nodeInfo1 = conectivityNode1->GetInfo().m_nodeData; + dgCollisionInstance* const chunkCollision0 = nodeInfo0.m_shapeNode->GetInfo()->GetShape(); + dgCollisionInstance* const chunkCollision1 = nodeInfo1.m_shapeNode->GetInfo()->GetShape(); + + dgMatrix inertia0 (chunkCollision0->CalculateInertia()); + dgMatrix inertia1 (chunkCollision1->CalculateInertia()); + dgVector normal (inertia1.m_posit - inertia0.m_posit); + normal = normal.Normalize(); + edgeNode->GetInfo().m_edgeData.m_normal = normal; + } + } + + m_vertexBuffer = new (m_world->GetAllocator()) dgVertexBuffer(vertexArray.m_count, m_world->GetAllocator()); + for (dgInt32 i = 0; i < vertexArray.m_count; i ++) { + m_vertexBuffer->m_vertex[i * 3 + 0] = vertexArray[i].m_point[0]; + m_vertexBuffer->m_vertex[i * 3 + 1] = vertexArray[i].m_point[1]; + m_vertexBuffer->m_vertex[i * 3 + 2] = vertexArray[i].m_point[2]; + m_vertexBuffer->m_normal[i * 3 + 0] = vertexArray[i].m_point[3]; + m_vertexBuffer->m_normal[i * 3 + 1] = vertexArray[i].m_point[4]; + m_vertexBuffer->m_normal[i * 3 + 2] = vertexArray[i].m_point[5]; + m_vertexBuffer->m_uv[i * 2 + 0] = vertexArray[i].m_point[6]; + m_vertexBuffer->m_uv[i * 2 + 1] = vertexArray[i].m_point[7]; + } + + dgTree materailOrdinalMap(GetAllocator()); + for (dgConectivityGraph::dgListNode* node = m_conectivity.GetFirst(); node; node = node->GetNext()) { + dgDebriNodeInfo& data = node->GetInfo().m_nodeData; + for (dgMesh::dgListNode* meshSegment = data.m_mesh->GetFirst(); meshSegment; meshSegment = meshSegment->GetNext()) { + dgSubMesh* const subMesh = &meshSegment->GetInfo(); + dgTree::dgTreeNode* node1 = materailOrdinalMap.Find(subMesh->m_material); + if (!node1) { + node1 = materailOrdinalMap.Insert(m_materialCount, subMesh->m_material); + m_materialCount ++; + dgAssert (m_materialCount < DG_FRACTURE_MAX_METERIAL_COUNT); + } + + dgInt32 materialIndex = node1->GetInfo(); + subMesh->m_materialOrdinal = materialIndex; + } + } + + dgMesh* const mainMesh = new (m_world->GetAllocator()) dgMesh(m_world->GetAllocator()); + dgConectivityGraph::dgListNode* const mainNode = m_conectivity.dgGraph::AddNode (); + dgDebriNodeInfo& mainNodeData = mainNode->GetInfo().m_nodeData; + mainNodeData.m_mesh = mainMesh; + + BuildMainMeshSubMehes(); + m_conectivityMap.Pupolate(m_conectivity); + m_density = -dgFloat32 (1.0f) / GetVolume(); + + dgAssert (SanityCheck()); +} + +dgCollisionCompoundFractured::~dgCollisionCompoundFractured(void) +{ + if (m_vertexBuffer) { + m_vertexBuffer->Release(); + } +} + +void dgCollisionCompoundFractured::Serialize(dgSerialize callback, void* const userData) const +{ + dgCollisionCompound::Serialize(callback, userData); + m_conectivity.Serialize(callback, userData); + m_vertexBuffer->Serialize(callback, userData); + + callback (userData, &m_impulseStrengthPerUnitMass, sizeof (m_impulseStrengthPerUnitMass)); + callback (userData, &m_impulseAbsortionFactor, sizeof (m_impulseAbsortionFactor)); + callback (userData, &m_density, sizeof (m_density)); + callback (userData, &m_materialCount, sizeof (m_materialCount)); +} + +void dgCollisionCompoundFractured::SetCallbacks(OnEmitFractureChunkCallBack emitFracturedChunk, OnEmitNewCompundFractureCallBack emitNewCompoundFactured, OnReconstructFractureMainMeshCallBack reconstructMainMesh) +{ + m_emitFracturedChunk = emitFracturedChunk; + m_reconstructMainMesh = reconstructMainMesh; + m_emitFracturedCompound = emitNewCompoundFactured; +} + +void dgCollisionCompoundFractured::BuildMainMeshSubMehes() const +{ + dgConectivityGraph::dgListNode* const mainNode = m_conectivity.GetLast(); + dgMesh* const mainMesh = mainNode->GetInfo().m_nodeData.m_mesh; + + mainMesh->RemoveAll(); + + dgAssert (mainMesh->m_vertexOffsetStart == 0); + mainMesh->m_vertexCount = m_vertexBuffer->m_vertexCount; + + dgInt32 histogram[DG_FRACTURE_MAX_METERIAL_COUNT]; + dgInt32 materials[DG_FRACTURE_MAX_METERIAL_COUNT]; + memset (histogram, 0, m_materialCount * sizeof (dgInt32)); + + for (dgConectivityGraph::dgListNode* node = m_conectivity.GetFirst(); node != mainNode; node = node->GetNext() ) { + dgDebriNodeInfo& data = node->GetInfo().m_nodeData; + for (dgMesh::dgListNode* meshSegment = data.m_mesh->GetFirst(); meshSegment; meshSegment = meshSegment->GetNext()) { + dgSubMesh* const subMesh = &meshSegment->GetInfo(); + if (subMesh->m_visibleFaces) { + dgInt32 index = subMesh->m_materialOrdinal; + histogram[index] += subMesh->m_faceCount; + materials[index] = subMesh->m_material; + } + } + } + + dgSubMesh* mainSubMeshes[DG_FRACTURE_MAX_METERIAL_COUNT]; + for (dgInt32 i = 0; i < m_materialCount; i ++) { + if (histogram[i]) { + mainSubMeshes[i] = mainMesh->AddgSubMesh (histogram[i] * 3, materials[i]); + } + } + + dgInt32 faceIndexIndexOffset[DG_FRACTURE_MAX_METERIAL_COUNT]; + memset (faceIndexIndexOffset, 0, m_materialCount * sizeof (dgInt32)); + + for (dgConectivityGraph::dgListNode* node = m_conectivity.GetFirst(); node != mainNode; node = node->GetNext() ) { + dgDebriNodeInfo& data = node->GetInfo().m_nodeData; + const dgInt32 vertexOffsetStart = data.m_mesh->m_vertexOffsetStart; + for (dgMesh::dgListNode* meshSegment = data.m_mesh->GetFirst(); meshSegment; meshSegment = meshSegment->GetNext()) { + dgSubMesh* const subMesh = &meshSegment->GetInfo(); + if (subMesh->m_visibleFaces) { + dgInt32 index = subMesh->m_materialOrdinal; + dgInt32 base = faceIndexIndexOffset[index]; + dgSubMesh* const mainSubMesh = mainSubMeshes[index]; + const dgInt32 count = 3 * subMesh->m_faceCount; + for (dgInt32 i = 0; i < count; i ++) { + mainSubMesh->m_indexes[base + i] = subMesh->m_indexes[i] + vertexOffsetStart; + } + faceIndexIndexOffset[index] += count; + dgAssert (faceIndexIndexOffset[index] <= histogram[index] * 3); + } + } + } +} + +bool dgCollisionCompoundFractured::SanityCheck() const +{ + for (dgConectivityGraph::dgListNode* rootNode = m_conectivity.GetFirst(); rootNode; rootNode = rootNode->GetNext() ) { + dgTree filter(GetAllocator()); + for (dgGraphNode::dgListNode* edgeNode = rootNode->GetInfo().GetFirst(); edgeNode; edgeNode = edgeNode->GetNext()) { + dgConectivityGraph::dgListNode* const node = edgeNode->GetInfo().m_node; + dgAssert (!filter.Find(node)); + filter.Insert(node, node); + } + } + + return true; +} + + +dgCollisionCompoundFractured::dgConectivityGraph::dgListNode* dgCollisionCompoundFractured::GetMainMesh() const +{ + return m_conectivity.GetLast(); +} + +dgCollisionCompoundFractured::dgConectivityGraph::dgListNode* dgCollisionCompoundFractured::GetFirstMesh() const +{ + return m_conectivity.GetFirst(); +} + +dgCollisionCompoundFractured::dgConectivityGraph::dgListNode* dgCollisionCompoundFractured::GetNextMesh (dgConectivityGraph::dgListNode* const mesNodeh) const +{ + return (mesNodeh->GetNext() != m_conectivity.GetLast()) ? mesNodeh->GetNext() : NULL; +} + +dgInt32 dgCollisionCompoundFractured::GetVertecCount(dgConectivityGraph::dgListNode* const node) const +{ + dgMesh* const mesh = node->GetInfo().m_nodeData.m_mesh; + return mesh->m_vertexCount; +} + +const dgFloat32* dgCollisionCompoundFractured::GetVertexPositions (dgConectivityGraph::dgListNode* const node) const +{ + dgMesh* const mesh = node->GetInfo().m_nodeData.m_mesh; + return m_vertexBuffer->GetVertexPositions() + mesh->m_vertexOffsetStart * 3; +} + + +const dgFloat32* dgCollisionCompoundFractured::GetVertexNormal (dgConectivityGraph::dgListNode* const node) const +{ + dgMesh* const mesh = node->GetInfo().m_nodeData.m_mesh; + return m_vertexBuffer->GetVertexNormals() + mesh->m_vertexOffsetStart * 3; +} + + +const dgFloat32* dgCollisionCompoundFractured::GetVertexUVs (dgConectivityGraph::dgListNode* const node) const +{ + dgMesh* const mesh = node->GetInfo().m_nodeData.m_mesh; + return m_vertexBuffer->GetVertexUVs() + mesh->m_vertexOffsetStart * 2; +} + +dgInt32 dgCollisionCompoundFractured::GetSegmentIndexStream (dgConectivityGraph::dgListNode* const node___, dgMesh::dgListNode* const subMeshNode, dgInt32* const index) const +{ + dgSubMesh* const subMesh = &subMeshNode->GetInfo(); + const dgInt32* const indexes = subMesh->m_indexes; + memcpy (index, indexes, 3 * subMesh->m_faceCount * sizeof(dgInt32)); + return subMesh->m_faceCount * 3; +} + + +void dgCollisionCompoundFractured::SetImpulseStrength(dgFloat32 impulseStrength) +{ + m_impulseStrengthPerUnitMass = dgMax (impulseStrength, dgFloat32 (0.0f), impulseStrength) ; +} + +dgFloat32 dgCollisionCompoundFractured::GetImpulseStrength() const +{ + return m_impulseStrengthPerUnitMass; +} + +void dgCollisionCompoundFractured::SetImpulsePropgationFactor(dgFloat32 factor) +{ + m_impulseAbsortionFactor = dgClamp(factor, dgFloat32 (0.0f), dgFloat32 (1.0f)); +} + +dgFloat32 dgCollisionCompoundFractured::GetSetImpulsePropgationFactor() const +{ + return m_impulseAbsortionFactor; +} + + + +dgVector dgCollisionCompoundFractured::GetObbSize() const +{ + return dgCollisionCompound::GetObbSize() + (dgVector (DG_FRACTURE_AABB_GUARD_DISTANCE) & dgVector::m_triplexMask); +} + + +void dgCollisionCompoundFractured::CalcAABB (const dgMatrix& matrix, dgVector& p0, dgVector& p1) const +{ + dgCollisionCompound::CalcAABB (matrix, p0, p1); + p0 -= (dgVector (DG_FRACTURE_AABB_GUARD_DISTANCE) & dgVector::m_triplexMask); + p1 += (dgVector (DG_FRACTURE_AABB_GUARD_DISTANCE) & dgVector::m_triplexMask); +} + + +dgInt32 dgCollisionCompoundFractured::CalculateContacts (dgBroadPhase::dgPair* const pair, dgCollisionParamProxy& proxy) const +{ + dgAssert (0); + return 0; +/* + dgBroadPhase* const broaphaPhase = m_world->GetBroadPhase(); + dgInt32 count = dgCollisionCompound::CalculateContacts (pair, proxy); + + if (!count && broaphaPhase->m_recursiveChunks && m_emitFracturedChunk && m_root) { + dgContact* const constraint = pair->m_contact; + dgBody* const myBody = constraint->GetBody0(); + dgBody* const otherBody = constraint->GetBody1(); + dgVector relVeloc (otherBody->GetVelocity() - myBody->GetVelocity()); + dgAssert (relVeloc.m_w == dgFloat32 (0.0f)); + dgVector impulseStimate (relVeloc.Scale (dgFloat32 (1.0f) / (myBody->GetInvMass().m_w + otherBody->GetInvMass().m_w))); + dgFloat32 impulseStimate2 = impulseStimate.DotProduct(impulseStimate).m_w; + dgFloat32 impulseStrength = m_impulseStrengthPerUnitMass * myBody->GetMass().m_w; + + if (impulseStimate2 > (impulseStrength * impulseStrength)) { + dgCollisionInstance* const myInstance = myBody->GetCollision(); + dgCollisionInstance* const otherInstance = otherBody->GetCollision(); + dgAssert (myInstance->GetChildShape() == this); + dgContactPoint contactOut; + dgAssert (0); +// dgFloat32 dist = ConvexRayCast (otherInstance, otherInstance->GetGlobalMatrix(), relVeloc, proxy.m_timestep, contactOut, myBody, myInstance, NULL, proxy.m_threadIndex); + dgFloat32 dist = 0; + if (dist < proxy.m_timestep) { + dgAssert (m_conectivityMap.Find(contactOut.m_collision0)); + dgConectivityGraph::dgListNode* const rootNode = m_conectivityMap.Find(contactOut.m_collision0)->GetInfo(); + dgDebriNodeInfo& nodeInfo = rootNode->GetInfo().m_nodeData; + nodeInfo.m_lru = 1; + dgCollisionCompoundFractured* const me = (dgCollisionCompoundFractured*)this; + + m_world->GlobalLock(true); + if (me->SpawnChunks (myBody, myInstance, rootNode, impulseStimate2, impulseStrength * impulseStrength)) { + me->SpawnDisjointChunks (myBody, myInstance, rootNode, impulseStimate2, impulseStrength * impulseStrength); + + BuildMainMeshSubMehes(); + m_reconstructMainMesh (myBody, m_conectivity.GetLast(), myInstance); + if (m_root) { + me->MassProperties (); + dgFloat32 mass = m_centerOfMass.m_w * m_density; + myBody->SetMassProperties(mass, myInstance); + } else { + myBody->SetMassProperties(dgFloat32 (0.0f), myInstance); + } + } + m_world->GlobalUnlock(); + } + } + } + return count; +*/ +} + +void dgCollisionCompoundFractured::BeginAddRemove () +{ + dgCollisionCompound::BeginAddRemove (); +} + +void dgCollisionCompoundFractured::EndAddRemove (bool flushCache) +{ + dgCollisionCompound::EndAddRemove (flushCache); + BuildMainMeshSubMehes(); +} + +void dgCollisionCompoundFractured::RemoveCollision (dgTreeArray::dgTreeNode* const node) +{ + dgConectivityGraphMap::dgTreeNode* const mapNode = m_conectivityMap.Find(node->GetInfo()->GetShape()); + dgAssert (mapNode); + + dgConectivityGraph::dgListNode* const chunkNode = mapNode->GetInfo(); + + for (dgGraphNode::dgListNode* edgeNode = chunkNode->GetInfo().GetFirst(); edgeNode; edgeNode = edgeNode->GetNext()) { + dgConectivityGraph::dgListNode* const node1 = edgeNode->GetInfo().m_node; + dgDebriNodeInfo& childNodeInfo = node1->GetInfo().m_nodeData; + childNodeInfo.m_mesh->m_isVisible = true; + for (dgMesh::dgListNode* meshSegment = childNodeInfo.m_mesh->GetFirst(); meshSegment; meshSegment = meshSegment->GetNext()) { + dgSubMesh* const subMesh = &meshSegment->GetInfo(); + subMesh->m_visibleFaces = true; + } + } + + dgDebriNodeInfo& nodeInfo = chunkNode->GetInfo().m_nodeData; + dgCollisionInstance* const chunkCollision = nodeInfo.m_shapeNode->GetInfo()->GetShape(); + + m_conectivityMap.Remove (chunkCollision); + m_conectivity.DeleteNode(chunkNode); + dgCollisionCompound::RemoveCollision (node); +} + +bool dgCollisionCompoundFractured::IsNodeSaseToDetach(dgTreeArray::dgTreeNode* const node) const +{ + dgConectivityGraphMap::dgTreeNode* const mapNode = m_conectivityMap.Find(node->GetInfo()->GetShape()); + dgAssert (mapNode); + return mapNode ? CanChunk (mapNode->GetInfo()) : false; +} + +int dgCollisionCompoundFractured::GetFirstNiegborghArray (dgTreeArray::dgTreeNode* const node, dgTreeArray::dgTreeNode** const nodesArray, int maxCount) const +{ + int count = 0; + dgConectivityGraphMap::dgTreeNode* const mapNode = m_conectivityMap.Find(node->GetInfo()->GetShape()); + dgAssert (mapNode); + + dgConectivityGraph::dgListNode* const chunkNode = mapNode->GetInfo(); + for (dgGraphNode::dgListNode* edgeNode = chunkNode->GetInfo().GetFirst(); edgeNode && (count < maxCount); edgeNode = edgeNode->GetNext()) { + dgConectivityGraph::dgListNode* const node1 = edgeNode->GetInfo().m_node; + dgDebriNodeInfo& childNodeInfo = node1->GetInfo().m_nodeData; + nodesArray[count] = childNodeInfo.m_shapeNode; + count ++; + } + + return count; +} + +bool dgCollisionCompoundFractured::CanChunk (dgConectivityGraph::dgListNode* const chunkNode) const +{ + dgVector directionsMap[32]; + dgInt32 count = 0; + for (dgGraphNode::dgListNode* edgeNode = chunkNode->GetInfo().GetFirst(); edgeNode && (count < dgInt32 (sizeof (directionsMap)/sizeof (directionsMap[0]))); edgeNode = edgeNode->GetNext()) { + directionsMap[count] = edgeNode->GetInfo().m_edgeData.m_normal; + count ++; + } + + dgVector error (dgFloat32 (1.0e-3f)); + dgVector himespherePlane (directionsMap[0]); + for (dgInt32 i = 1; i < count; i ++) { + dgVector projection (himespherePlane.DotProduct(directionsMap[i])); + dgInt32 sign = (projection < error).GetSignMask(); + if (sign & 0x0f) { +// dgFloat32 val; +// projection.StoreScalar(&val); + dgFloat32 val = projection.GetScalar(); + dgAssert (val > dgFloat32 (-1.0f)); + dgFloat32 angle = dgAcos (val) - dgFloat32 (90.0f * dgDegreeToRad) + dgFloat32 (15.0f * dgDegreeToRad); + dgVector axis (himespherePlane.CrossProduct(directionsMap[i])); + axis = axis.Normalize(); + dgQuaternion rot (axis, angle); + himespherePlane = dgMatrix (rot, dgVector::m_wOne).RotateVector(himespherePlane); + + for (dgInt32 j = 0; j < i; j ++) { + dgVector projection1 (himespherePlane.DotProduct(directionsMap[j])); + dgInt32 sign1 = (projection1 < error).GetSignMask(); + if (sign1 & 0x0f) { + return false; + } + } + } + } + return true; +} + + + + +bool dgCollisionCompoundFractured::IsBelowPlane (dgConectivityGraph::dgListNode* const node, const dgVector& plane) const +{ + dgDebriNodeInfo& nodeInfo = node->GetInfo().m_nodeData; + dgCollisionInstance* const instance = nodeInfo.m_shapeNode->GetInfo()->GetShape(); + + dgVector dir (plane & dgVector::m_triplexMask); + + const dgMatrix& matrix = instance->GetLocalMatrix(); + dgVector support (matrix.TransformVector(instance->SupportVertex(matrix.UnrotateVector(dir)))); + + dgFloat32 dist = (support.DotProduct(plane)).GetScalar(); + return dist < dgFloat32 (0.0f); +} + +bool dgCollisionCompoundFractured::IsAbovePlane (dgConectivityGraph::dgListNode* const node, const dgVector& plane) const +{ + dgDebriNodeInfo& nodeInfo = node->GetInfo().m_nodeData; + dgCollisionInstance* const instance = nodeInfo.m_shapeNode->GetInfo()->GetShape(); + + dgVector dir (dgVector::m_negOne * (plane & dgVector::m_triplexMask)); + + const dgMatrix& matrix = instance->GetLocalMatrix(); + dgVector support (matrix.TransformVector(instance->SupportVertex(matrix.UnrotateVector(dir)))); + + dgFloat32 dist = (support.DotProduct(plane)).GetScalar(); + return dist > dgFloat32 (0.0f); +} + +dgCollisionCompoundFractured::dgConectivityGraph::dgListNode* dgCollisionCompoundFractured::FirstAcrossPlane (dgConectivityGraph::dgListNode* const nodeBelowPlane, const dgVector& plane) const +{ + dgDebriNodeInfo& nodeInfo = nodeBelowPlane->GetInfo().m_nodeData; + dgCollisionInstance* const instance = nodeInfo.m_shapeNode->GetInfo()->GetShape(); + + dgVector dir (plane & dgVector::m_triplexMask); + + const dgMatrix& matrix = instance->GetLocalMatrix(); + dgVector support (matrix.TransformVector(instance->SupportVertex(matrix.UnrotateVector(dir)))); + + dgFloat32 dist = (support.DotProduct(plane)).GetScalar(); + dgAssert (dist < dgFloat32 (0.0f)); + + dgConectivityGraph::dgListNode* startNode = nodeBelowPlane; + for (bool foundBetterNode = true; foundBetterNode; ) { + foundBetterNode = false; + for (dgGraphNode::dgListNode* edgeNode = startNode->GetInfo().GetFirst(); edgeNode; edgeNode = edgeNode->GetNext()) { + dgConectivityGraph::dgListNode* const node = edgeNode->GetInfo().m_node; + dgDebriNodeInfo& neighborgInfo = node->GetInfo().m_nodeData; + dgCollisionInstance* const instance1 = neighborgInfo.m_shapeNode->GetInfo()->GetShape(); + + const dgMatrix& matrix1 = instance1->GetLocalMatrix(); + dgVector support1 (matrix1.TransformVector(instance1->SupportVertex(matrix1.UnrotateVector(dir)))); + dgFloat32 dist1 = (support1.DotProduct(plane)).GetScalar(); + if (dist1 > dist) { + dist = dist1; + foundBetterNode = true; + startNode = node; + if (dist > dgFloat32 (0.0f)) { + foundBetterNode = false; + return startNode; + } + } + } + } + + return NULL; +} + +dgCollisionCompoundFractured* dgCollisionCompoundFractured::PlaneClip (const dgVector& plane) +{ + dgConectivityGraph::dgListNode* startNode = m_conectivity.GetFirst(); + if (IsAbovePlane (startNode, plane)) { + startNode = FirstAcrossPlane (startNode, plane * dgVector::m_negOne); + } else if (IsBelowPlane (startNode, plane)) { + startNode = FirstAcrossPlane (startNode, plane); + } + + if (startNode) { + dgVector posgDir (plane & dgVector::m_triplexMask); + dgVector negDir (posgDir * dgVector::m_negOne); + dgTree upperSide (GetAllocator()); + + dgInt32 stack = 1; + dgConectivityGraph::dgListNode* pool[512]; + pool[0] = startNode; + m_lru += 2; + while (stack) { + stack --; + dgConectivityGraph::dgListNode* const planeNode = pool[stack]; + dgDebriNodeInfo& nodeInfo = planeNode->GetInfo().m_nodeData; + + if (nodeInfo.m_lru != m_lru) { + + nodeInfo.m_lru = m_lru; + + dgGraphNode::dgListNode* nextEdge; + for (dgGraphNode::dgListNode* edgeNode = planeNode->GetInfo().GetFirst(); edgeNode; edgeNode = nextEdge) { + nextEdge = edgeNode->GetNext(); + + dgConectivityGraph::dgListNode* const node = edgeNode->GetInfo().m_node; + dgDebriNodeInfo& neighborgInfo = node->GetInfo().m_nodeData; + if (neighborgInfo.m_lru != m_lru) { + dgCollisionInstance* const instance = neighborgInfo.m_shapeNode->GetInfo()->GetShape(); + const dgMatrix& matrix = instance->GetLocalMatrix(); + + dgVector support (matrix.TransformVector(instance->SupportVertex(matrix.UnrotateVector(negDir)))); + dgFloat32 dist = (support.DotProduct(plane)).GetScalar(); + if (dist > dgFloat32 (0.0f)) { + upperSide.Insert(node, node); + planeNode->GetInfo().DeleteEdge (edgeNode); + node->GetInfo().m_nodeData.m_mesh->m_isVisible = true; + planeNode->GetInfo().m_nodeData.m_mesh->m_isVisible = true; + + for (dgMesh::dgListNode* meshSegment = node->GetInfo().m_nodeData.m_mesh->GetFirst(); meshSegment; meshSegment = meshSegment->GetNext()) { + dgSubMesh* const subMesh = &meshSegment->GetInfo(); + subMesh->m_visibleFaces = true; + } + + for (dgMesh::dgListNode* meshSegment = planeNode->GetInfo().m_nodeData.m_mesh->GetFirst(); meshSegment; meshSegment = meshSegment->GetNext()) { + dgSubMesh* const subMesh = &meshSegment->GetInfo(); + subMesh->m_visibleFaces = true; + } + + + } else { + dgVector support1 (matrix.TransformVector(instance->SupportVertex(matrix.UnrotateVector(posgDir)))); + dgFloat32 dist1 = (support1.DotProduct(plane)).GetScalar(); + if (dist1 > dgFloat32 (0.0f)) { + pool[stack] = node; + stack ++; + dgAssert (stack < sizeof (pool) / sizeof (pool[0])); + } + } + } + } + } + } + } + + return NULL; +} + +bool dgCollisionCompoundFractured::SpawnChunks (dgBody* const myBody, const dgCollisionInstance* const myInstance, dgConectivityGraph::dgListNode* const rootNode, dgFloat32 impulseStimate2, dgFloat32 impulseStimateCut2) +{ + dgFloat32 strengthPool[512]; + dgConectivityGraph::dgListNode* pool[512]; + + dgVector massMatrix (myBody->GetMass()); + if (m_density < dgFloat32 (0.0f)) { + m_density = dgAbs (massMatrix.m_w * m_density); + } + + dgFloat32 attenuation = m_impulseAbsortionFactor; + m_lru ++; + pool[0] = rootNode; + strengthPool[0] = impulseStimate2; + + dgInt32 stack = 1; + bool spawned = false; + while (stack) { + stack --; + dgFloat32 strenght = strengthPool[stack] * attenuation; + dgConectivityGraph::dgListNode* const chunkNode = pool[stack]; + + if ((strenght > impulseStimateCut2) && CanChunk (chunkNode)) { + spawned = true; + dgDebriNodeInfo& nodeInfo = chunkNode->GetInfo().m_nodeData; + + nodeInfo.m_mesh->m_isVisible = true; + for (dgMesh::dgListNode* meshSegment = nodeInfo.m_mesh->GetFirst(); meshSegment; meshSegment = meshSegment->GetNext()) { + dgSubMesh* const subMesh = &meshSegment->GetInfo(); + subMesh->m_visibleFaces = true; + } + + for (dgGraphNode::dgListNode* edgeNode = chunkNode->GetInfo().GetFirst(); edgeNode; edgeNode = edgeNode->GetNext()) { + dgConectivityGraph::dgListNode* const node = edgeNode->GetInfo().m_node; + dgDebriNodeInfo& childNodeInfo = node->GetInfo().m_nodeData; + childNodeInfo.m_mesh->m_isVisible = true; + for (dgMesh::dgListNode* meshSegment = childNodeInfo.m_mesh->GetFirst(); meshSegment; meshSegment = meshSegment->GetNext()) { + dgSubMesh* const subMesh = &meshSegment->GetInfo(); + subMesh->m_visibleFaces = true; + } + } + + for (dgGraphNode::dgListNode* edgeNode = chunkNode->GetInfo().GetFirst(); edgeNode; edgeNode = edgeNode->GetNext()) { + dgConectivityGraph::dgListNode* const node = edgeNode->GetInfo().m_node; + dgDebriNodeInfo& childNodeInfo = node->GetInfo().m_nodeData; + if (childNodeInfo.m_lru != m_lru) { + childNodeInfo.m_lru = m_lru; + strengthPool[stack] = strenght; + pool[stack] = node; + stack ++; + dgAssert (stack < sizeof (pool)/sizeof (pool[0])); + } + } + SpawnSingleChunk (myBody, myInstance, chunkNode); + } + } + return spawned; +} + +void dgCollisionCompoundFractured::ColorDisjoinChunksIsland () +{ + dgInt32 stack = 1; + dgConectivityGraph::dgListNode* pool[512]; + + dgInt32 stackMark = m_lru + 1; + m_lru += 2; + pool[0] = m_conectivity.GetFirst(); + stack = 1; + while (stack) { + stack --; + dgConectivityGraph::dgListNode* const node = pool[stack]; + dgDebriNodeInfo& nodeInfo = node->GetInfo().m_nodeData; + if (nodeInfo.m_lru <= stackMark) { + nodeInfo.m_lru = m_lru; + for (dgGraphNode::dgListNode* edgeNode = node->GetInfo().GetFirst(); edgeNode; edgeNode = edgeNode->GetNext()) { + dgConectivityGraph::dgListNode* const node1 = edgeNode->GetInfo().m_node; + dgDebriNodeInfo& childNodeInfo1 = node1->GetInfo().m_nodeData; + if (childNodeInfo1.m_lru < stackMark) { + childNodeInfo1.m_lru = stackMark; + pool[stack] = node1; + stack ++; + dgAssert (stack < sizeof (pool)/sizeof (pool[0])); + } + } + } + } +} + +void dgCollisionCompoundFractured::SpawnDisjointChunks (dgBody* const myBody, const dgCollisionInstance* const myInstance, dgConectivityGraph::dgListNode* const rootNode, dgFloat32 impulseStimate2, dgFloat32 impulseStimateCut2) +{ + if (m_conectivity.GetFirst() != m_conectivity.GetLast()) { + ColorDisjoinChunksIsland (); + dgConectivityGraph::dgListNode* nextNode; + for (dgConectivityGraph::dgListNode* node = m_conectivity.GetFirst(); node != m_conectivity.GetLast(); node = nextNode) { + nextNode = node->GetNext(); + dgDebriNodeInfo& nodeInfo = node->GetInfo().m_nodeData; + if (nodeInfo.m_lru != m_lru) { + if (node->GetInfo().GetCount() == 0) { + SpawnSingleChunk (myBody, myInstance, node); + } else { + do { + nextNode = nextNode->GetPrev(); + } while (nextNode && (nextNode->GetInfo().m_nodeData.m_lru != m_lru)); + dgAssert (nextNode); + SpawnComplexChunk (myBody, myInstance, node); + } + } + } + } + +} + + +void dgCollisionCompoundFractured::SpawnSingleChunk (dgBody* const myBody, const dgCollisionInstance* const myInstance, dgConectivityGraph::dgListNode* const chunkNode) +{ + const dgMatrix& matrix = myBody->GetMatrix(); + const dgVector& veloc = myBody->GetVelocity(); + const dgVector& omega = myBody->GetOmega(); + dgVector com (matrix.TransformVector(myBody->GetCentreOfMass())); + + dgDebriNodeInfo& nodeInfo = chunkNode->GetInfo().m_nodeData; + dgCollisionInstance* const chunkCollision = nodeInfo.m_shapeNode->GetInfo()->GetShape(); + dgDynamicBody* const chunkBody = m_world->CreateDynamicBody (chunkCollision, matrix); + chunkBody->SetMassProperties(chunkCollision->GetVolume() * m_density, chunkBody->GetCollision()); + m_world->GetBroadPhase()->AddInternallyGeneratedBody(chunkBody); + + // calculate debris initial velocity + dgVector chunkOrigin (matrix.TransformVector(chunkCollision->GetLocalMatrix().m_posit)); + dgVector chunkVeloc (veloc + omega.CrossProduct(chunkOrigin - com)); + + chunkBody->SetOmega(omega); + chunkBody->SetVelocity(chunkVeloc); + chunkBody->SetGroupID(int (chunkCollision->GetUserDataID())); + + m_emitFracturedChunk(chunkBody, chunkNode, myInstance); + + m_conectivityMap.Remove (chunkCollision); + dgCollisionCompound::RemoveCollision (nodeInfo.m_shapeNode); + m_conectivity.DeleteNode(chunkNode); +} + +void dgCollisionCompoundFractured::SpawnComplexChunk (dgBody* const myBody, const dgCollisionInstance* const parentInstance, dgConectivityGraph::dgListNode* const chunkNode) +{ + dgInt32 stack = 1; + dgConectivityGraph::dgListNode* pool[512]; + + dgList islanList (GetAllocator()); + dgInt32 stackMark = m_lru - 1; + pool[0] = chunkNode; + stack = 1; + while (stack) { + stack --; + dgConectivityGraph::dgListNode* const node = pool[stack]; + dgDebriNodeInfo& nodeInfo = node->GetInfo().m_nodeData; + if (nodeInfo.m_lru <= stackMark) { + islanList.Append(node); + + nodeInfo.m_lru = m_lru; + for (dgGraphNode::dgListNode* edgeNode = node->GetInfo().GetFirst(); edgeNode; edgeNode = edgeNode->GetNext()) { + dgConectivityGraph::dgListNode* const node1 = edgeNode->GetInfo().m_node; + dgDebriNodeInfo& childNodeInfo1 = node1->GetInfo().m_nodeData; + if (childNodeInfo1.m_lru < stackMark) { + childNodeInfo1.m_lru = stackMark; + pool[stack] = node1; + stack ++; + dgAssert (stack < sizeof (pool)/sizeof (pool[0])); + } + } + } + } + + const dgMatrix& matrix = myBody->GetMatrix(); + const dgVector& veloc = myBody->GetVelocity(); + const dgVector& omega = myBody->GetOmega(); + dgVector com (matrix.TransformVector(myBody->GetCentreOfMass())); + + dgCollisionCompoundFractured* const childStructureCollision = new (GetAllocator()) dgCollisionCompoundFractured (*this, islanList); + dgCollisionInstance* const childStructureInstance = m_world->CreateInstance (childStructureCollision, int (parentInstance->GetUserDataID()), parentInstance->GetLocalMatrix()); + childStructureCollision->m_myInstance = childStructureInstance; + childStructureCollision->Release(); + + dgDynamicBody* const chunkBody = m_world->CreateDynamicBody (childStructureInstance, matrix); + chunkBody->SetMassProperties(childStructureInstance->GetVolume() * m_density, chunkBody->GetCollision()); + m_world->GetBroadPhase()->AddInternallyGeneratedBody(chunkBody); + + // calculate debris initial velocity + dgVector chunkOrigin (matrix.TransformVector(childStructureInstance->GetLocalMatrix().m_posit)); + dgVector chunkVeloc (veloc + omega.CrossProduct(chunkOrigin - com)); + + chunkBody->SetOmega(omega); + chunkBody->SetVelocity(chunkVeloc); + chunkBody->SetGroupID(int (childStructureInstance->GetUserDataID())); + + m_emitFracturedCompound (chunkBody); + childStructureInstance->Release(); +} diff --git a/thirdparty/src/newton/dgPhysics/dgCollisionCompoundFractured.h b/thirdparty/src/newton/dgPhysics/dgCollisionCompoundFractured.h new file mode 100644 index 000000000..2ec7fada1 --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgCollisionCompoundFractured.h @@ -0,0 +1,233 @@ +/* Copyright (c) <2003-2019> +* +* 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_COLLISION_COMPOUND_FRACTURED_H +#define DG_COLLISION_COMPOUND_FRACTURED_H + +#include "dgCollisionCompound.h" + + +class dgMeshEffect; + + + +class dgCollisionCompoundFractured: public dgCollisionCompound +{ + class dgFractureBuilder; + + public: + class dgFlatVertex + { + public: + dgFloat32 m_point[10]; // 3 point, 3 normal. 2 uv0, 2 uv1 + }; + + class dgFlatVertexArray: public dgArray + { + public: + dgFlatVertexArray(dgMemoryAllocator* const allocator) + :dgArray (allocator) + { + dgAssert(0); + m_count = 0; + } + + dgInt32 m_count; + }; + + class dgVertexBuffer: public dgRefCounter + { + public: + DG_CLASS_ALLOCATOR(allocator) + dgVertexBuffer (dgInt32 count, dgMemoryAllocator* allocator); + dgVertexBuffer (dgMemoryAllocator* const allocator, dgDeserialize deserialization, void* const userData); + ~dgVertexBuffer (); + + void Serialize(dgSerialize callback, void* const userData) const; + const dgFloat32* GetVertexPositions() const {return m_vertex;}; + const dgFloat32* GetVertexNormals() const {return m_normal;}; + const dgFloat32* GetVertexUVs() const {return m_uv;}; + + dgFloat32 *m_uv; + dgFloat32 *m_vertex; + dgFloat32 *m_normal; + dgMemoryAllocator* m_allocator; + dgInt32 m_vertexCount; + }; + + class dgSubMesh + { + public: + dgSubMesh (dgMemoryAllocator* const allocator); + ~dgSubMesh (); + void Serialize(dgSerialize callback, void* const userData) const; + + dgInt32 *m_indexes; + dgMemoryAllocator* m_allocator; + dgInt32 m_material; + dgInt32 m_faceCount; + dgInt32 m_materialOrdinal; + bool m_visibleFaces; + }; + + class dgMesh: public dgList, public dgRefCounter + { + public: + dgMesh(dgMemoryAllocator* const allocator); + dgMesh (dgMemoryAllocator* const allocator, dgDeserialize deserialization, void* const userData); + ~dgMesh(); + void Serialize (dgSerialize callback, void* const userData) const; + dgSubMesh* AddgSubMesh (dgInt32 indexCount, dgInt32 material); + + dgInt32 m_vertexOffsetStart; + dgInt32 m_vertexCount; + bool m_isVisible; + }; + + class dgDebriNodeInfo + { + public: + dgDebriNodeInfo (); + ~dgDebriNodeInfo (); + + dgMesh* m_mesh; + dgTreeArray::dgTreeNode* m_shapeNode; + dgInt32 m_lru; + }; + + + class dgSharedNodeMesh + { + public: + dgSharedNodeMesh (); + ~dgSharedNodeMesh (); + dgVector m_normal; + }; + + class dgConectivityGraph: public dgGraph + { + public: + dgConectivityGraph (dgMemoryAllocator* const allocator); + dgConectivityGraph (const dgConectivityGraph& source); + ~dgConectivityGraph (); + + dgListNode* AddNode (dgFlatVertexArray& vertexArray, dgMeshEffect* const factureVisualMesh, dgTreeArray::dgTreeNode* const collisionNode, dgInt32 interiorMaterialBase); + void Serialize(dgSerialize callback, void* const userData) const; + void Deserialize (dgCollisionCompoundFractured* const source, dgDeserialize callback, void* const userData); + }; + + class dgConectivityGraphMap: public dgTree + { + public: + dgConectivityGraphMap (const dgConectivityGraphMap& source) + :dgTree(source.GetAllocator()) + { + } + + dgConectivityGraphMap (dgMemoryAllocator* const allocator) + :dgTree(allocator) + { + } + + void Pupolate(const dgConectivityGraph& graph) + { + for (dgConectivityGraph::dgListNode* node = graph.GetFirst(); node != graph.GetLast(); node = node->GetNext() ) { + dgDebriNodeInfo& nodeInfo = node->GetInfo().m_nodeData; + Insert(node, nodeInfo.m_shapeNode->GetInfo()->GetShape()); + } + } + }; + + public: + typedef void (*OnEmitNewCompundFractureCallBack) (dgBody* const body); + typedef void (*OnEmitFractureChunkCallBack) (dgBody* const body, dgConectivityGraph::dgListNode* const chunkMeshNode, const dgCollisionInstance* const myInstance); + typedef void (*OnReconstructFractureMainMeshCallBack) (dgBody* const body, dgConectivityGraph::dgListNode* const mainMeshNode, const dgCollisionInstance* const myInstance); + + dgCollisionCompoundFractured (const dgCollisionCompoundFractured& source, const dgCollisionInstance* const myInstance); + dgCollisionCompoundFractured (dgCollisionCompoundFractured& source, const dgList& island); + dgCollisionCompoundFractured (dgWorld* const world, dgMeshEffect* const solidMesh, dgInt32 fracturePhysicsMaterialID, int pointcloudCount, const dgFloat32* const vertexCloud, int strideInBytes, int materialID, const dgMatrix& offsetMatrix, + OnEmitFractureChunkCallBack emitFracturedChunk, OnEmitNewCompundFractureCallBack emitNewCompoundFactured, OnReconstructFractureMainMeshCallBack reconstructMainMesh); + + dgCollisionCompoundFractured (dgWorld* const world, dgDeserialize deserialization, void* const userData, const dgCollisionInstance* const myInstance, dgInt32 revisionNumber); + virtual ~dgCollisionCompoundFractured(void); + + void SetCallbacks(OnEmitFractureChunkCallBack emitFracturedChunk, OnEmitNewCompundFractureCallBack emitNewCompoundFactured, OnReconstructFractureMainMeshCallBack reconstructMainMesh); + + dgConectivityGraph::dgListNode* GetMainMesh() const; + dgConectivityGraph::dgListNode* GetFirstMesh() const; + dgConectivityGraph::dgListNode* GetNextMesh(dgConectivityGraph::dgListNode* const mesh) const; + + dgInt32 GetVertecCount(dgConectivityGraph::dgListNode* const node) const; + const dgFloat32* GetVertexPositions (dgConectivityGraph::dgListNode* const node) const; + const dgFloat32* GetVertexNormal (dgConectivityGraph::dgListNode* const node) const; + const dgFloat32* GetVertexUVs (dgConectivityGraph::dgListNode* const node) const; + dgInt32 GetSegmentIndexStream (dgConectivityGraph::dgListNode* const node, dgMesh::dgListNode* const segment, dgInt32* const index) const; + + void SetImpulseStrength(dgFloat32 impulseStrength); + dgFloat32 GetImpulseStrength() const; + + void SetImpulsePropgationFactor(dgFloat32 factor); + dgFloat32 GetSetImpulsePropgationFactor() const; + + virtual void BeginAddRemove (); + virtual void RemoveCollision (dgTreeArray::dgTreeNode* const node); + virtual void EndAddRemove (bool flushCache = true); + bool IsNodeSaseToDetach (dgTreeArray::dgTreeNode* const node) const; + + int GetFirstNiegborghArray (dgTreeArray::dgTreeNode* const node, dgTreeArray::dgTreeNode** const nodesArray, int maxCount) const; + + dgCollisionCompoundFractured* PlaneClip (const dgVector& plane); + + private: + void BuildMainMeshSubMehes() const; + dgVector GetObbSize() const; + + virtual void Serialize(dgSerialize callback, void* const userData) const; + virtual void CalcAABB (const dgMatrix& matrix, dgVector& p0, dgVector& p1) const; + dgInt32 CalculateContacts (dgBroadPhase::dgPair* const pair, dgCollisionParamProxy& proxy) const; + + void ColorDisjoinChunksIsland (); + bool SpawnChunks (dgBody* const myBody, const dgCollisionInstance* const myInstance, dgConectivityGraph::dgListNode* const rootNode, dgFloat32 impulseStimate2, dgFloat32 impulseStimateCut2); + void SpawnDisjointChunks (dgBody* const myBody, const dgCollisionInstance* const myInstance, dgConectivityGraph::dgListNode* const rootNode, dgFloat32 impulseStimate2, dgFloat32 impulseStimateCut2); + + void SpawnSingleChunk (dgBody* const myBody, const dgCollisionInstance* const myInstance, dgConectivityGraph::dgListNode* const chunkNode); + void SpawnComplexChunk (dgBody* const myBody, const dgCollisionInstance* const myInstance, dgConectivityGraph::dgListNode* const chunkNode); + bool CanChunk (dgConectivityGraph::dgListNode* const node) const; + + bool SanityCheck() const; + + inline bool IsAbovePlane (dgConectivityGraph::dgListNode* const node, const dgVector& plane) const; + inline bool IsBelowPlane (dgConectivityGraph::dgListNode* const node, const dgVector& plane) const; + inline dgConectivityGraph::dgListNode* FirstAcrossPlane (dgConectivityGraph::dgListNode* const node, const dgVector& plane) const; + + dgConectivityGraph m_conectivity; + dgConectivityGraphMap m_conectivityMap; + dgVertexBuffer* m_vertexBuffer; + dgFloat32 m_impulseStrengthPerUnitMass; + dgFloat32 m_impulseAbsortionFactor; + dgFloat32 m_density; + dgInt32 m_lru; + dgInt32 m_materialCount; + OnEmitFractureChunkCallBack m_emitFracturedChunk; + OnEmitNewCompundFractureCallBack m_emitFracturedCompound; + OnReconstructFractureMainMeshCallBack m_reconstructMainMesh; +}; +#endif diff --git a/thirdparty/src/newton/dgPhysics/dgCollisionCone.cpp b/thirdparty/src/newton/dgPhysics/dgCollisionCone.cpp new file mode 100644 index 000000000..39c6275e2 --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgCollisionCone.cpp @@ -0,0 +1,371 @@ +/* Copyright (c) <2003-2019> +* +* 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 "dgPhysicsStdafx.h" +#include "dgBody.h" +#include "dgContact.h" +#include "dgCollisionCone.h" + +////////////////////////////////////////////////////////////////////// +// Construction/Destruction +////////////////////////////////////////////////////////////////////// + +dgInt32 dgCollisionCone::m_shapeRefCount = 0; +dgCollisionConvex::dgConvexSimplexEdge dgCollisionCone::m_edgeArray[DG_CONE_SEGMENTS * 4]; + +dgCollisionCone::dgCollisionCone(dgMemoryAllocator* allocator, dgUnsigned32 signature, dgFloat32 radius, dgFloat32 height) + :dgCollisionConvex(allocator, signature, m_coneCollision) +{ + Init (radius, height); +} + +dgCollisionCone::dgCollisionCone(dgWorld* const world, dgDeserialize deserialization, void* const userData, dgInt32 revisionNumber) + :dgCollisionConvex (world, deserialization, userData, revisionNumber) +{ + dgVector size; + deserialization (userData, &size, sizeof (dgVector)); + Init (size.m_x, size.m_y); +} + + +dgCollisionCone::~dgCollisionCone() +{ + m_shapeRefCount --; + dgAssert (m_shapeRefCount >= 0); + + dgCollisionConvex::m_simplex = NULL; + dgCollisionConvex::m_vertex = NULL; +} + +void dgCollisionCone::Init (dgFloat32 radius, dgFloat32 height) +{ + m_rtti |= dgCollisionCone_RTTI; + + m_radius = dgMax(dgAbs(radius), D_MIN_CONVEX_SHAPE_SIZE); + m_height = dgMax (dgAbs (height * dgFloat32 (0.5f)), D_MIN_CONVEX_SHAPE_SIZE); + + dgFloat32 angle = dgFloat32(0.0f); + for (dgInt32 i = 0; i < DG_CONE_SEGMENTS; i++) { + dgFloat32 sinAngle = dgSin(angle); + dgFloat32 cosAngle = dgCos(angle); + m_vertex[i] = dgVector(-m_height, m_radius * cosAngle, m_radius * sinAngle, dgFloat32(0.0f)); + angle += dgPI2 / DG_CONE_SEGMENTS; + } + m_vertex[DG_CONE_SEGMENTS] = dgVector (m_height, dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f)); + + m_edgeCount = DG_CONE_SEGMENTS * 4; + m_vertexCount = DG_CONE_SEGMENTS + 1; + dgCollisionConvex::m_vertex = m_vertex; + + if (!m_shapeRefCount) { + dgPolyhedra polyhedra(m_allocator); + dgInt32 wireframe[DG_CONE_SEGMENTS]; + + dgInt32 j = DG_CONE_SEGMENTS - 1; + polyhedra.BeginFace (); + for (dgInt32 i = 0; i < DG_CONE_SEGMENTS; i ++) { + wireframe[0] = j; + wireframe[1] = i; + wireframe[2] = DG_CONE_SEGMENTS; + j = i; + polyhedra.AddFace (3, wireframe); + } + + for (dgInt32 i = 0; i < DG_CONE_SEGMENTS; i ++) { + wireframe[i] = DG_CONE_SEGMENTS - 1 - i; + } + polyhedra.AddFace (DG_CONE_SEGMENTS, wireframe); + polyhedra.EndFace (); + + dgAssert (SanityCheck (polyhedra)); + + dgUnsigned64 i = 0; + dgPolyhedra::Iterator iter (polyhedra); + for (iter.Begin(); iter; iter ++) { + dgEdge* const edge = &(*iter); + edge->m_userData = i; + i ++; + } + + for (iter.Begin(); iter; iter ++) { + dgEdge* const edge = &(*iter); + + dgConvexSimplexEdge* const ptr = &m_edgeArray[edge->m_userData]; + + ptr->m_vertex = edge->m_incidentVertex; + ptr->m_next = &m_edgeArray[edge->m_next->m_userData]; + ptr->m_prev = &m_edgeArray[edge->m_prev->m_userData]; + ptr->m_twin = &m_edgeArray[edge->m_twin->m_userData]; + } + } + + m_profile[0] = dgVector(m_height, dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f)); + m_profile[1] = dgVector(-m_height, m_radius, dgFloat32(0.0f), dgFloat32(0.0f)); + m_profile[2] = dgVector(-m_height, -m_radius, dgFloat32(0.0f), dgFloat32(0.0f)); + + m_shapeRefCount ++; + dgCollisionConvex::m_simplex = m_edgeArray; + + SetVolumeAndCG (); +} + +dgInt32 dgCollisionCone::CalculateSignature (dgFloat32 radius, dgFloat32 height) +{ + dgUnsigned32 buffer[3]; + + memset (buffer, 0, sizeof (buffer)); + buffer[0] = m_coneCollision; + buffer[1] = dgCollision::Quantize (radius); + buffer[2] = dgCollision::Quantize (height); + return dgInt32 (dgCollision::Quantize(buffer, sizeof (buffer))); +} + +dgInt32 dgCollisionCone::CalculateSignature () const +{ + return CalculateSignature (m_radius, m_height); +} + + +void dgCollisionCone::DebugCollision (const dgMatrix& matrix, dgCollision::OnDebugCollisionMeshCallback callback, void* const userData) const +{ + #define NUMBER_OF_DEBUG_SEGMENTS 40 + dgTriplex pool[NUMBER_OF_DEBUG_SEGMENTS + 1]; + dgTriplex face[NUMBER_OF_DEBUG_SEGMENTS]; + + dgFloat32 angle = dgFloat32 (0.0f); + for (dgInt32 i = 0; i < NUMBER_OF_DEBUG_SEGMENTS; i ++) { + dgFloat32 z = dgSin (angle) * m_radius; + dgFloat32 y = dgCos (angle) * m_radius; + pool[i].m_x = -m_height; + pool[i].m_y = y; + pool[i].m_z = z; + angle += dgPI2 / dgFloat32 (NUMBER_OF_DEBUG_SEGMENTS); + } + + pool[NUMBER_OF_DEBUG_SEGMENTS].m_x = m_height; + pool[NUMBER_OF_DEBUG_SEGMENTS].m_y = dgFloat32 (0.0f); + pool[NUMBER_OF_DEBUG_SEGMENTS].m_z = dgFloat32 (0.0f); + + matrix.TransformTriplex (&pool[0].m_x, sizeof (dgTriplex), &pool[0].m_x, sizeof (dgTriplex), NUMBER_OF_DEBUG_SEGMENTS + 1); + dgInt32 j = NUMBER_OF_DEBUG_SEGMENTS - 1; + for (dgInt32 i = 0; i < NUMBER_OF_DEBUG_SEGMENTS; i ++) { + face[0] = pool[j]; + face[1] = pool[i]; + face[2] = pool[NUMBER_OF_DEBUG_SEGMENTS]; + j = i; + callback (userData, 3, &face[0].m_x, 0); + } + + for (dgInt32 i = 0; i < NUMBER_OF_DEBUG_SEGMENTS; i ++) { + face[i] = pool[NUMBER_OF_DEBUG_SEGMENTS - 1 - i]; + } + callback (userData, NUMBER_OF_DEBUG_SEGMENTS, &face[0].m_x, 0); +} + +void dgCollisionCone::SetCollisionBBox (const dgVector& p0__, const dgVector& p1__) +{ + dgAssert (0); +} + + +dgVector dgCollisionCone::SupportVertex (const dgVector& dir, dgInt32* const vertexIndex) const +{ + dgAssert (dir.m_w == dgFloat32 (0.0f)); + dgAssert(dgAbs(dir.DotProduct(dir).GetScalar() - dgFloat32(1.0f)) < dgFloat32(1.0e-3f)); + + if (dir.m_x < dgFloat32(-0.9999f)) { + return dgVector(-m_height, dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f)); + } else if (dir.m_x > dgFloat32(0.9999f)) { + return dgVector(m_height, dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f)); + } else { + dgVector dir_yz(dir); + dir_yz.m_x = dgFloat32(0.0f); + dgFloat32 mag2 = dir_yz.DotProduct(dir_yz).GetScalar(); + dgAssert(mag2 > dgFloat32(0.0f)); + dir_yz = dir_yz.Scale(dgFloat32(1.0f) / dgSqrt(mag2)); + + dgVector p0(dir_yz.Scale(m_radius)); + dgVector p1(dgVector::m_zero); + + p0.m_x = -m_height; + p1.m_x = m_height; + + dgFloat32 dist0 = dir.DotProduct(p0).GetScalar(); + dgFloat32 dist1 = dir.DotProduct(p1).GetScalar(); + + if (dist1 >= dist0) { + p0 = p1; + } + return p0; + } +} + +dgVector dgCollisionCone::SupportVertexSpecial(const dgVector& dir, dgFloat32 skinThickness, dgInt32* const vertexIndex) const +{ + dgAssert (dir.m_w == dgFloat32 (0.0f)); + dgAssert(dgAbs(dir.DotProduct(dir).GetScalar() - dgFloat32(1.0f)) < dgFloat32(1.0e-3f)); + + if (dir.m_x < dgFloat32(-0.9999f)) { + return dgVector(-(m_height - DG_PENETRATION_TOL), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f)); + } else if (dir.m_x > dgFloat32(0.9999f)) { + return dgVector(m_height - DG_PENETRATION_TOL, dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f)); + } else { + dgVector dir_yz(dir); + dir_yz.m_x = dgFloat32(0.0f); + dgFloat32 mag2 = dir_yz.DotProduct(dir_yz).GetScalar(); + dgAssert(mag2 > dgFloat32(0.0f)); + dir_yz = dir_yz.Scale(dgFloat32(1.0f) / dgSqrt(mag2)); + + dgVector p0(dir_yz.Scale(m_radius - DG_PENETRATION_TOL)); + dgVector p1(dgVector::m_zero); + + p0.m_x = -(m_height - DG_PENETRATION_TOL); + p1.m_x = m_height - DG_PENETRATION_TOL; + + dgFloat32 dist0 = dir.DotProduct(p0).GetScalar(); + dgFloat32 dist1 = dir.DotProduct(p1).GetScalar(); + + if (dist1 >= dist0) { + p0 = p1; + } + return p0; + } +} + +dgVector dgCollisionCone::SupportVertexSpecialProjectPoint(const dgVector& point, const dgVector& dir) const +{ + dgAssert (dir.m_w == dgFloat32 (0.0f)); + dgAssert(dgAbs(dir.DotProduct(dir).GetScalar() - dgFloat32(1.0f)) < dgFloat32(1.0e-3f)); + return point + dir.Scale(DG_PENETRATION_TOL); +} + +void dgCollisionCone::MassProperties () +{ + m_centerOfMass = dgVector (-dgFloat32 (1.0f/2.0f) * m_height, dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f)); + m_crossInertia = dgVector (dgFloat32 (0.0f)); + + dgFloat32 volume = dgFloat32 (dgPi * 2.0f / 3.0f) * m_radius * m_radius * m_height; + dgFloat32 inertiaxx = dgFloat32 (3.0f / 10.0f) * m_radius * m_radius; + dgFloat32 inertiayyzz = (dgFloat32 (3.0f / 20.0f) * m_radius * m_radius + dgFloat32 (3.0f / 20.0f) * m_height * m_height); + + m_inertia[0] = inertiaxx; + m_inertia[1] = inertiayyzz; + m_inertia[2] = inertiayyzz; + m_centerOfMass.m_w = volume; +} + +dgInt32 dgCollisionCone::CalculatePlaneIntersection (const dgVector& normal, const dgVector& origin, dgVector* const contactsOut) const +{ + dgInt32 count = 0; + if (normal.m_x > dgFloat32(0.99f)) { + contactsOut[0] = dgVector(m_height, dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f)); + count = 1; + } else if (normal.m_x < dgFloat32(-0.995f)) { + const dgFloat32 inclination = dgFloat32(0.9998f); + if (normal.m_x < -inclination) { + dgMatrix matrix(normal); + matrix.m_posit.m_x = origin.m_x; + count = BuildCylinderCapPoly (m_radius, matrix, contactsOut); + //count = RectifyConvexSlice(n, normal, contactsOut); + } else { + dgFloat32 magInv = dgRsqrt(normal.m_y * normal.m_y + normal.m_z * normal.m_z); + dgFloat32 cosAng = normal.m_y * magInv; + dgFloat32 sinAng = normal.m_z * magInv; + + dgAssert(dgAbs(normal.m_z * cosAng - normal.m_y * sinAng) < dgFloat32(1.0e-4f)); + dgVector normal1(normal.m_x, normal.m_y * cosAng + normal.m_z * sinAng, dgFloat32(0.0f), dgFloat32(0.0f)); + dgVector origin1(origin.m_x, origin.m_y * cosAng + origin.m_z * sinAng, origin.m_z * cosAng - origin.m_y * sinAng, dgFloat32(0.0f)); + + count = dgCollisionConvex::CalculatePlaneIntersection(normal1, origin1, contactsOut); + if (count > 6) { + dgInt32 dy = 2 * 6; + dgInt32 dx = 2 * count; + dgInt32 acc = dy - count; + dgInt32 index = 0; + for (dgInt32 i = 0; i < count; i++) { + if (acc > 0) { + contactsOut[index] = contactsOut[i]; + index++; + acc -= dx; + } + acc += dy; + } + count = index; + } + + for (dgInt32 i = 0; i < count; i++) { + dgFloat32 y = contactsOut[i].m_y; + dgFloat32 z = contactsOut[i].m_z; + contactsOut[i].m_y = y * cosAng - z * sinAng; + contactsOut[i].m_z = z * cosAng + y * sinAng; + } + } + } else { + dgFloat32 magInv = dgRsqrt(normal.m_y * normal.m_y + normal.m_z * normal.m_z); + dgFloat32 cosAng = normal.m_y * magInv; + dgFloat32 sinAng = normal.m_z * magInv; + + dgAssert(dgAbs(normal.m_z * cosAng - normal.m_y * sinAng) < dgFloat32(1.0e-4f)); + dgVector normal1(normal.m_x, normal.m_y * cosAng + normal.m_z * sinAng, dgFloat32(0.0f), dgFloat32(0.0f)); + dgVector origin1(origin.m_x, origin.m_y * cosAng + origin.m_z * sinAng, origin.m_z * cosAng - origin.m_y * sinAng, dgFloat32(0.0f)); + + count = 0; + int i0 = 2; + dgVector test0((m_profile[i0] - origin1).DotProduct(normal1)); + for (int i = 0; (i < 3) && (count < 2); i++) { + dgVector test1((m_profile[i] - origin1).DotProduct(normal1)); + dgVector acrossPlane(test0 * test1); + if (acrossPlane.m_x < 0.0f) { + dgVector step(m_profile[i] - m_profile[i0]); + contactsOut[count] = m_profile[i0] - step.Scale(test0.m_x / (step.DotProduct(normal1).m_x)); + count++; + } + i0 = i; + test0 = test1; + } + + for (dgInt32 i = 0; i < count; i++) { + dgFloat32 y = contactsOut[i].m_y; + dgFloat32 z = contactsOut[i].m_z; + contactsOut[i].m_y = y * cosAng - z * sinAng; + contactsOut[i].m_z = z * cosAng + y * sinAng; + } + } + + return count; +} + + + +void dgCollisionCone::GetCollisionInfo(dgCollisionInfo* const info) const +{ + dgCollisionConvex::GetCollisionInfo(info); + + info->m_cone.m_r = m_radius; + info->m_cone.m_height = m_height * dgFloat32 (2.0f); +} + +void dgCollisionCone::Serialize(dgSerialize callback, void* const userData) const +{ + dgVector size (m_radius, m_height * dgFloat32 (2.0f), dgFloat32 (0.0f), dgFloat32 (0.0f)); + SerializeLow(callback, userData); + callback (userData, &size, sizeof (dgVector)); +} diff --git a/thirdparty/src/newton/dgPhysics/dgCollisionCone.h b/thirdparty/src/newton/dgPhysics/dgCollisionCone.h new file mode 100644 index 000000000..accc521b2 --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgCollisionCone.h @@ -0,0 +1,66 @@ +/* Copyright (c) <2003-2019> +* +* 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_DGCOLLISIONCONE_H__AS235640FER_H) +#define AFX_DGCOLLISIONCONE_H__AS235640FER_H + +#include "dgCollisionConvex.h" + +#define DG_CONE_SEGMENTS 12 + + +class dgCollisionCone: public dgCollisionConvex +{ + public: + dgCollisionCone (dgMemoryAllocator* const allocator, dgUnsigned32 signature, dgFloat32 radius, dgFloat32 height); + dgCollisionCone(dgWorld* const world, dgDeserialize deserialization, void* const userData, dgInt32 revisionNumber); + virtual ~dgCollisionCone(); + + private: + void Init (dgFloat32 radius, dgFloat32 height); + virtual dgVector SupportVertex (const dgVector& dir, dgInt32* const vertexIndex) const; + virtual dgVector SupportVertexSpecial (const dgVector& dir, dgFloat32 skinThickness, dgInt32* const vertexIndex) const; + virtual dgVector SupportVertexSpecialProjectPoint (const dgVector& point, const dgVector& dir) const; + + virtual dgInt32 CalculatePlaneIntersection (const dgVector& normal, const dgVector& origin, dgVector* const contactsOut) const; + + virtual void MassProperties (); + virtual void DebugCollision (const dgMatrix& matrix, dgCollision::OnDebugCollisionMeshCallback callback, void* const userData) const; + + virtual dgInt32 CalculateSignature () const; + virtual void SetCollisionBBox (const dgVector& p0, const dgVector& p1); + virtual void GetCollisionInfo(dgCollisionInfo* const info) const; + virtual void Serialize(dgSerialize callback, void* const userData) const; + + static dgInt32 CalculateSignature (dgFloat32 radius, dgFloat32 height); + + dgVector m_profile[3]; + dgFloat32 m_height; + dgFloat32 m_radius; + dgVector m_vertex[DG_CONE_SEGMENTS + 1]; + static dgInt32 m_shapeRefCount; + static dgConvexSimplexEdge m_edgeArray[]; + + friend class dgWorld; +}; + +#endif + diff --git a/thirdparty/src/newton/dgPhysics/dgCollisionConvex.cpp b/thirdparty/src/newton/dgPhysics/dgCollisionConvex.cpp new file mode 100644 index 000000000..833036563 --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgCollisionConvex.cpp @@ -0,0 +1,938 @@ +/* Copyright (c) <2003-2019> +* +* 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 "dgPhysicsStdafx.h" +#include "dgBody.h" +#include "dgWorld.h" +#include "dgContact.h" +#include "dgContactSolver.h" +#include "dgCollisionMesh.h" +#include "dgCollisionConvex.h" +#include "dgCollisionInstance.h" +#include "dgCollisionConvexHull.h" +#include "dgCollisionConvexPolygon.h" + +#define DG_MAX_MIN_VOLUME dgFloat32 (1.0e-6f) +#define DG_MAX_VERTEX_CLIP_FACE 16 + + +////////////////////////////////////////////////////////////////////// +// Construction/Destruction +////////////////////////////////////////////////////////////////////// + + +dgCollisionConvex::dgCollisionConvex (dgMemoryAllocator* const allocator, dgUnsigned32 signature, dgCollisionID id) + :dgCollision(allocator, signature, id) + ,m_vertex (NULL) + ,m_simplex (NULL) + ,m_boxMinRadius (dgFloat32 (0.0f)) + ,m_boxMaxRadius (dgFloat32 (0.0f)) + ,m_simplexVolume (dgFloat32 (0.0f)) + ,m_edgeCount (0) + ,m_vertexCount (0) +{ + m_rtti |= dgCollisionConvexShape_RTTI; +} + + +dgCollisionConvex::dgCollisionConvex (dgWorld* const world, dgDeserialize deserialization, void* const userData, dgInt32 revisionNumber) + :dgCollision (world, deserialization, userData, revisionNumber) + ,m_vertex (NULL) + ,m_simplex (NULL) + ,m_boxMinRadius (dgFloat32 (0.0f)) + ,m_boxMaxRadius (dgFloat32 (0.0f)) + ,m_simplexVolume (dgFloat32 (0.0f)) + ,m_edgeCount (0) + ,m_vertexCount (0) +{ + dgAssert (m_rtti | dgCollisionConvexShape_RTTI); +} + + +dgCollisionConvex::~dgCollisionConvex () +{ + if (m_vertex) { + m_allocator->Free (m_vertex); + } + + if (m_simplex) { + m_allocator->Free (m_simplex); + } +} + +void dgCollisionConvex::SerializeLow(dgSerialize callback, void* const userData) const +{ + dgCollision::SerializeLow(callback, userData); +} + + +void dgCollisionConvex::SetVolumeAndCG () +{ + dgVector faceVertex[DG_MAX_EDGE_COUNT]; + dgStack edgeMarks (m_edgeCount); + memset (&edgeMarks[0], 0, sizeof (dgInt8) * m_edgeCount); + + dgPolyhedraMassProperties localData; + for (dgInt32 i = 0; i < m_edgeCount; i ++) { + dgConvexSimplexEdge* const face = &m_simplex[i]; + if (!edgeMarks[i]) { + dgConvexSimplexEdge* edge = face; + dgInt32 count = 0; + do { + dgAssert ((edge - m_simplex) >= 0); + edgeMarks[dgInt32 (edge - m_simplex)] = '1'; + faceVertex[count] = m_vertex[edge->m_vertex]; + count ++; + dgAssert (count < dgInt32 (sizeof (faceVertex) / sizeof (faceVertex[0]))); + edge = edge->m_next; + } while (edge != face); + localData.AddCGFace (count, faceVertex); + } + } + + dgVector origin; + dgVector inertia; + dgVector crossInertia; + dgFloat32 volume = localData.MassProperties (origin, inertia, crossInertia); + m_simplexVolume = volume; + + // calculate the origin of the bound box of this primitive + dgVector p0(dgFloat32 (0.0f)); + dgVector p1(dgFloat32 (0.0f)); + for (dgInt32 i = 0; i < 3; i ++) { + dgVector dir (dgFloat32 (0.0f)); + dir[i] = dgFloat32 (-1.0f); + p0[i] = SupportVertex(dir, NULL)[i]; + + dir[i] = dgFloat32 (1.0f); + p1[i] = SupportVertex(dir, NULL)[i]; + } + + dgAssert (p0.m_w == dgFloat32 (0.0f)); + dgAssert (p1.m_w == dgFloat32 (0.0f)); + m_boxSize = (p1 - p0) * dgVector::m_half; + m_boxOrigin = (p1 + p0) * dgVector::m_half; + m_boxMinRadius = dgMin(m_boxSize.m_x, m_boxSize.m_y, m_boxSize.m_z); + m_boxMaxRadius = dgSqrt ((m_boxSize.DotProduct(m_boxSize)).GetScalar()); + + MassProperties (); +} + +bool dgCollisionConvex::SanityCheck (dgPolyhedra& hull) const +{ + dgPolyhedra::Iterator iter (hull); + for (iter.Begin(); iter; iter ++) { + dgEdge* const edge = &(*iter); + if (edge->m_incidentFace < 0) { + return false; + } + dgEdge* ptr = edge; + dgVector p0 (m_vertex[edge->m_incidentVertex]); + ptr = ptr->m_next; + dgVector p1 (m_vertex[ptr->m_incidentVertex]); + dgVector e1 (p1 - p0); + dgVector n0 (dgFloat32 (0.0f)); + for (ptr = ptr->m_next; ptr != edge; ptr = ptr->m_next) { + dgVector p2 (m_vertex[ptr->m_incidentVertex]); + dgVector e2 (p2 - p0); + n0 += e1.CrossProduct(e2); + e1 = e2; + } + + dgAssert (n0.m_w == dgFloat32 (0.0f)); + ptr = edge; + do { + dgVector q0 (m_vertex[ptr->m_twin->m_incidentVertex]); + for (dgEdge* neiborg = ptr->m_twin->m_next->m_next; neiborg != ptr->m_twin; neiborg = neiborg->m_next) { + dgVector q1 (m_vertex[neiborg->m_incidentVertex]); + dgVector q1q0 (q1 - q0); + dgFloat32 project = q1q0.DotProduct(n0).GetScalar(); + if (project > dgFloat32 (1.0e-5f)) { + return false; + } + } + + ptr = ptr->m_next; + } while (ptr != edge); + } + + return true; +} + +void dgCollisionConvex::DebugCollision (const dgMatrix& matrix, dgCollision::OnDebugCollisionMeshCallback callback, void* const userData) const +{ + dgInt8 mark[DG_MAX_EDGE_COUNT]; + dgVector tmp[DG_MAX_EDGE_COUNT]; + dgTriplex vertex[DG_MAX_EDGE_COUNT]; + + matrix.TransformTriplex (&tmp[0].m_x, sizeof (dgVector), &m_vertex[0].m_x, sizeof (dgVector), m_vertexCount); + + memset (mark, 0, sizeof (mark)); + for (dgInt32 i = 0; i < m_edgeCount; i ++) { + if (!mark[i]) { + dgConvexSimplexEdge* const face = &m_simplex[i]; + dgConvexSimplexEdge* edge = face; + dgInt32 count = 0; + do { + mark[edge - m_simplex] = '1'; + dgInt32 index = edge->m_vertex; + vertex[count].m_x = tmp[index].m_x; + vertex[count].m_y = tmp[index].m_y; + vertex[count].m_z = tmp[index].m_z; + count ++; + edge = edge->m_next; + } while (edge != face); + callback (userData, count, &vertex[0].m_x, 0); + } + } +} + +void dgCollisionConvex::CalcAABB (const dgMatrix& matrix, dgVector& p0, dgVector& p1) const +{ + dgVector origin (matrix.TransformVector(m_boxOrigin)); + dgVector size (matrix.m_front.Abs().Scale(m_boxSize.m_x) + matrix.m_up.Abs().Scale(m_boxSize.m_y) + matrix.m_right.Abs().Scale(m_boxSize.m_z)); + + p0 = (origin - size) & dgVector::m_triplexMask; + p1 = (origin + size) & dgVector::m_triplexMask; +} + +void dgCollisionConvex::CalculateInertia (void* userData, int indexCount, const dgFloat32* const faceVertex, int faceId) +{ + dgPolyhedraMassProperties& localData = *((dgPolyhedraMassProperties*) userData); + localData.AddInertiaAndCrossFace(indexCount, faceVertex); +} + +dgFloat32 dgCollisionConvex::CalculateMassProperties (const dgMatrix& offset, dgVector& inertia, dgVector& crossInertia, dgVector& centerOfMass) const +{ + dgPolyhedraMassProperties localData; + DebugCollision (offset, CalculateInertia, &localData); + return localData.MassProperties (centerOfMass, inertia, crossInertia); +} + +void dgCollisionConvex::MassProperties () +{ + dgFloat32 volume = dgCollisionConvex::CalculateMassProperties (dgGetIdentityMatrix(), m_inertia, m_crossInertia, m_centerOfMass); + if (volume < DG_MAX_MIN_VOLUME) { + volume = DG_MAX_MIN_VOLUME; + } + dgFloat32 invVolume = dgFloat32 (1.0f) / volume; + m_inertia = m_inertia.Scale (invVolume); + m_crossInertia = m_crossInertia.Scale (invVolume); + m_centerOfMass = m_centerOfMass.Scale (invVolume); + m_centerOfMass.m_w = volume; + + // complete the calculation + dgCollision::MassProperties (); +} + +dgMatrix dgCollisionConvex::CalculateInertiaAndCenterOfMass (const dgMatrix& alignMatrix, const dgVector& localScale, const dgMatrix& matrix) const +{ + if ((dgAbs (localScale.m_x - localScale.m_y) < dgFloat32 (1.0e-5f)) && (dgAbs (localScale.m_x - localScale.m_z) < dgFloat32 (1.0e-5f))) { + dgAssert (alignMatrix[0][0] == dgFloat32(1.0f)); + dgAssert (alignMatrix[1][1] == dgFloat32(1.0f)); + dgAssert (alignMatrix[2][2] == dgFloat32(1.0f)); + + // using general central theorem, is much faster and more accurate; + //IImatrix = IIorigin + mass * [(displacemnet % displacemnet) * identityMatrix - transpose(displacement) * displacement)]; + dgFloat32 mag2 = localScale.m_x * localScale.m_x; + dgMatrix inertia (dgGetIdentityMatrix()); + inertia[0][0] = m_inertia[0] * mag2; + inertia[1][1] = m_inertia[1] * mag2; + inertia[2][2] = m_inertia[2] * mag2; + inertia[0][1] = m_crossInertia[2] * mag2; + inertia[1][0] = m_crossInertia[2] * mag2; + inertia[0][2] = m_crossInertia[1] * mag2; + inertia[2][0] = m_crossInertia[1] * mag2; + inertia[1][2] = m_crossInertia[0] * mag2; + inertia[2][1] = m_crossInertia[0] * mag2; + inertia = matrix.Inverse() * inertia * matrix; + + dgAssert (localScale.m_w == dgFloat32 (0.0f)); + dgVector origin (matrix.TransformVector (m_centerOfMass * localScale)); + + origin.m_w = dgFloat32 (0.0f); + dgFloat32 originMag2 = origin.DotProduct(origin).GetScalar(); + dgMatrix Covariance(origin, origin); + dgMatrix parallel(dgGetIdentityMatrix()); + for (dgInt32 i = 0; i < 3; i++) { + parallel[i][i] = originMag2; + inertia[i] += (parallel[i] - Covariance[i]); + dgAssert(inertia[i][i] > dgFloat32(0.0f)); + } + + inertia.m_posit = origin; + inertia.m_posit.m_w = 1.0f; + return inertia; + } else { + // for non uniform scale we need to the general divergence theorem + dgVector inertiaII; + dgVector crossInertia; + dgVector centerOfMass; + dgMatrix scaledMatrix(matrix); + scaledMatrix[0] = scaledMatrix[0].Scale(localScale.m_x); + scaledMatrix[1] = scaledMatrix[1].Scale(localScale.m_y); + scaledMatrix[2] = scaledMatrix[2].Scale(localScale.m_z); + scaledMatrix = alignMatrix * scaledMatrix; + + dgFloat32 volume = CalculateMassProperties (scaledMatrix, inertiaII, crossInertia, centerOfMass); + if (volume < DG_MAX_MIN_VOLUME) { + volume = DG_MAX_MIN_VOLUME; + } + + dgFloat32 invVolume = dgFloat32 (1.0f) / volume; + centerOfMass = centerOfMass.Scale(invVolume); + inertiaII = inertiaII.Scale (invVolume); + crossInertia = crossInertia.Scale (invVolume); + dgMatrix inertia (dgGetIdentityMatrix()); + inertia[0][0] = inertiaII[0]; + inertia[1][1] = inertiaII[1]; + inertia[2][2] = inertiaII[2]; + inertia[0][1] = crossInertia[2]; + inertia[1][0] = crossInertia[2]; + inertia[0][2] = crossInertia[1]; + inertia[2][0] = crossInertia[1]; + inertia[1][2] = crossInertia[0]; + inertia[2][1] = crossInertia[0]; + inertia[3] = centerOfMass; + return inertia; + } +} + + +dgFloat32 dgCollisionConvex::GetVolume () const +{ + return m_centerOfMass.m_w; +} + +dgFloat32 dgCollisionConvex::GetBoxMinRadius () const +{ + return m_boxMinRadius; +} + +dgFloat32 dgCollisionConvex::GetBoxMaxRadius () const +{ + return m_boxMaxRadius; +} + +dgVector dgCollisionConvex::CalculateVolumeIntegral (const dgMatrix& globalMatrix, const dgVector& globalPlane, const dgCollisionInstance& parentScale) const +{ + dgPlane localPlane (globalMatrix.UntransformPlane (globalPlane)); + + const dgVector& scale = parentScale.m_scale; + switch (parentScale.m_scaleType) + { + case dgCollisionInstance::m_unit: + break; + + case dgCollisionInstance::m_uniform: + { + localPlane.m_w *= parentScale.m_invScale.m_x; + break; + } + + case dgCollisionInstance::m_nonUniform: + { + localPlane = localPlane * (scale | dgVector::m_wOne); + dgFloat32 mag2 = localPlane.DotProduct(localPlane & dgVector::m_triplexMask).GetScalar(); + localPlane = localPlane.Scale (dgRsqrt(mag2)); + break; + } + + default: + { + localPlane = localPlane * (scale | dgVector::m_wOne); + dgFloat32 mag2 = localPlane.DotProduct(localPlane & dgVector::m_triplexMask).GetScalar(); + localPlane = localPlane.Scale (dgRsqrt(mag2)); + localPlane = parentScale.m_aligmentMatrix.UntransformPlane (localPlane); + } + } + + dgVector cg (CalculateVolumeIntegral (localPlane)); + + dgFloat32 volume = cg.m_w * scale.m_x * scale.m_y * scale.m_z; + cg = parentScale.m_aligmentMatrix.RotateVector (cg); + cg = cg * scale; + cg = globalMatrix.TransformVector (cg); + cg.m_w = volume; + return cg; +} + +dgVector dgCollisionConvex::CalculateVolumeIntegral (const dgPlane& plane) const +{ + dgInt8 mark[DG_MAX_EDGE_COUNT]; + dgFloat32 test[DG_MAX_EDGE_COUNT]; + dgVector faceVertex[DG_MAX_EDGE_COUNT]; + + dgInt32 positive = 0; + dgInt32 negative = 0; + for (dgInt32 i = 0; i < m_vertexCount; i ++) { + test[i] = plane.Evalue (m_vertex[i]); + if (test[i] > dgFloat32 (1.0e-5f)) { + positive ++; + } else if (test[i] < -dgFloat32 (1.0e-5f)) { + negative ++; + } else { + test[i] = dgFloat32 (0.0f); + } + } + + if (positive == m_vertexCount) { + return dgVector::m_zero; + } + + if (negative == m_vertexCount) { + return m_centerOfMass; + } + + dgPolyhedraMassProperties localData; + dgConvexSimplexEdge* capEdge = NULL; + + dgVector cg (dgVector::m_zero); + memset (mark, 0, sizeof (mark)); + for (dgInt32 i = 0; i < m_edgeCount; i ++) { + if (!mark[i]) { + dgConvexSimplexEdge* const face = &m_simplex[i]; + dgConvexSimplexEdge* edge = face; + dgInt32 count = 0; + dgFloat32 size0 = test[edge->m_prev->m_vertex]; + do { + //edge->m_mark = m_mark; + mark[edge - m_simplex] = '1'; + dgFloat32 size1 = test[edge->m_vertex]; + if (size0 <= dgFloat32 (0.0f)) { + faceVertex[count] = m_vertex[edge->m_prev->m_vertex]; + count ++; + if (size1 > dgFloat32 (0.0f)) { + dgVector dp (m_vertex[edge->m_vertex] - m_vertex[edge->m_prev->m_vertex]); + dgAssert (dp.m_w == dgFloat32 (0.0f)); + faceVertex[count] = m_vertex[edge->m_prev->m_vertex] - dp.Scale (size0 / dp.DotProduct(plane).GetScalar()); + count ++; + } + } else if (size1 < dgFloat32 (0.0f)) { + dgVector dp (m_vertex[edge->m_vertex] - m_vertex[edge->m_prev->m_vertex]); + dgAssert (dp.m_w == dgFloat32 (0.0f)); + faceVertex[count] = m_vertex[edge->m_prev->m_vertex] - dp.Scale (size0 / dp.DotProduct(plane).GetScalar()); + count ++; + dgAssert (count < dgInt32 (sizeof (faceVertex) / sizeof (faceVertex[0]))); + } + + if (!capEdge) { + if ((size1 > dgFloat32 (0.0f)) && (size0 < dgFloat32 (0.0f))) { + capEdge = edge->m_prev->m_twin; + } + } + + size0 = size1; + edge = edge->m_next; + } while (edge != face); + + if (count) { + localData.AddCGFace(count, faceVertex); + } + } + } + + if (capEdge) { + dgInt32 count = 0; + dgConvexSimplexEdge* edge = capEdge; + dgConvexSimplexEdge* ptr = NULL; + do { + dgVector dp (m_vertex[edge->m_twin->m_vertex] - m_vertex[edge->m_vertex]); + dgAssert (dp.m_w == dgFloat32 (0.0f)); + faceVertex[count] = m_vertex[edge->m_vertex] - dp.Scale (test[edge->m_vertex] / dp.DotProduct(plane).GetScalar()); + count ++; + if (count == 127) { + // something is wrong return zero + return dgVector (dgFloat32 (0.0f)); + } + + for (ptr = edge->m_next; ptr != edge; ptr = ptr->m_next) { + dgInt32 index0 = ptr->m_twin->m_vertex; + if (test[index0] > dgFloat32 (0.0f)) { + index0 = ptr->m_vertex; + if (test[index0] < dgFloat32 (0.0f)) { + break; + } + } + } + edge = ptr->m_twin; + } while (edge != capEdge); + localData.AddCGFace(count, faceVertex); + } + + dgVector inertia; + dgVector crossInertia; + dgFloat32 volume = localData.MassProperties (cg, inertia, crossInertia); + cg = cg.Scale (dgFloat32 (1.0f) / dgMax (volume, dgFloat32 (1.0e-6f))); + cg.m_w = volume; + return cg; +} + +dgVector dgCollisionConvex::SupportVertex (const dgVector& dir, dgInt32* const vertexIndex) const +{ + dgAssert (dir.m_w == dgFloat32 (0.0f)); + dgAssert (dgAbs(dir.DotProduct(dir).GetScalar() - dgFloat32 (1.0f)) < dgFloat32 (1.0e-3f)); + + dgInt16 cache[16]; + memset (cache, -1, sizeof (cache)); + dgConvexSimplexEdge* edge = &m_simplex[0]; + + dgInt32 index = edge->m_vertex; + dgFloat32 side0 = m_vertex[index].DotProduct(dir).GetScalar(); + + cache [index & (sizeof (cache) / sizeof (cache[0]) - 1)] = dgInt16 (index); + dgConvexSimplexEdge* ptr = edge; + dgInt32 maxCount = 128; + do { + dgInt32 index1 = ptr->m_twin->m_vertex; + if (cache [index1 & (sizeof (cache) / sizeof (cache[0]) - 1)] != index1) { + cache [index1 & (sizeof (cache) / sizeof (cache[0]) - 1)] = dgInt16 (index1); + dgFloat32 side1 = m_vertex[index1].DotProduct(dir).GetScalar(); + if (side1 > side0) { + index = index1; + side0 = side1; + edge = ptr->m_twin; + ptr = edge; + } + } + ptr = ptr->m_twin->m_next; + maxCount --; + } while ((ptr != edge) && maxCount); + dgAssert (maxCount); + + dgAssert (index != -1); + return m_vertex[index]; +} + + +bool dgCollisionConvex::SanityCheck(dgInt32 count, const dgVector& normal, dgVector* const contactsOut) const +{ + if (count > 1) { + dgInt32 j = count - 1; + for (dgInt32 i = 0; i < count; i ++) { + dgVector error (contactsOut[i] - contactsOut[j]); + dgAssert (error.m_w == dgFloat32 (0.0f)); + if (error.DotProduct(error).GetScalar() <= dgFloat32 (1.0e-20f)) { + return false; + } + j = i; + } + + if (count >= 3) { + dgVector n (dgFloat32 (0.0f)); + dgVector e0 (contactsOut[1] - contactsOut[0]); + for (dgInt32 i = 2; i < count; i ++) { + dgVector e1 (contactsOut[i] - contactsOut[0]); + n += e0.CrossProduct(e1); + e0 = e1; + } + dgAssert (n.m_w == dgFloat32 (0.0f)); + dgAssert (n.DotProduct(n).GetScalar() > dgFloat32 (0.0f)); + n = n.Scale (dgRsqrt(n.DotProduct(n).GetScalar())); + dgFloat32 projection; + projection = n.DotProduct(normal).GetScalar(); + dgAssert (projection > dgFloat32 (0.9f)); + if (projection < dgFloat32 (0.9f)) { + return false; + } + + e0 = contactsOut[count-1] - contactsOut[count-2]; + j = count - 1; + for (dgInt32 i = 0; i < count; i ++) { + dgVector e1 (contactsOut[i] - contactsOut[j]); + dgVector n1 (e0.CrossProduct(e1)); + dgAssert (n1.m_w == dgFloat32 (0.0f)); + dgFloat32 error = n1.DotProduct(normal).GetScalar(); + dgAssert (error >= dgFloat32 (-1.0e-4f)); + if (error < dgFloat32 (-1.0e-4f)) { + return false; + } + j = i; + e0 = e1; + } + } + } + return true; +} + +dgInt32 dgCollisionConvex::RectifyConvexSlice (dgInt32 count, const dgVector& normal, dgVector* const contactsOut) const +{ + class dgConveFaceNode + { + public: + dgInt32 m_vertexIndex; + dgConveFaceNode* m_next; + dgConveFaceNode* m_prev; + dgInt32 m_mask; + }; + + dgConveFaceNode convexHull[DG_MAX_CONTATCS + 1]; + char buffer[DG_MAX_CONTATCS * (sizeof (void*) + sizeof (dgFloat32))]; + + dgInt32 start = count; + dgInt32 i0 = count - 1; + for (dgInt32 i = 0; i < count; i ++) { + convexHull[i].m_vertexIndex = i; + convexHull[i].m_next = &convexHull[i + 1]; + convexHull[i].m_prev = &convexHull[i0]; + i0 = i; + } + convexHull[count - 1].m_next = &convexHull[0]; + + dgVector hullArea (dgVector::m_zero); + dgVector edge0 (contactsOut[1] - contactsOut[0]); + for (dgInt32 i = 2; i < count; i ++) { + dgVector edge1 (contactsOut[i] - contactsOut[0]); + hullArea += edge1.CrossProduct(edge0); + edge0 = edge1; + } + + dgFloat32 totalArea = dgAbs (hullArea.DotProduct(normal).GetScalar()); + if (totalArea < dgFloat32 (1.0e-5f)) { + return 1; + } + dgConveFaceNode* hullPoint = &convexHull[0]; + + bool hasLinearCombination = true; + dgUpHeap sortHeap(buffer, sizeof (buffer)); + while (hasLinearCombination) { + hasLinearCombination = false; + sortHeap.Flush(); + dgConveFaceNode* ptr = hullPoint; + dgVector e0(contactsOut[ptr->m_next->m_vertexIndex] - contactsOut[ptr->m_vertexIndex]); + do { + dgVector e1(contactsOut[ptr->m_next->m_next->m_vertexIndex] - contactsOut[ptr->m_next->m_vertexIndex]); + dgFloat32 area = dgAbs (e0.CrossProduct(e1).DotProduct(normal).GetScalar()); + sortHeap.Push(ptr->m_next, area); + e0 = e1; + ptr->m_mask = 1; + ptr = ptr->m_next; + } while (ptr != hullPoint); + + while (sortHeap.GetCount() && (sortHeap.Value() * dgFloat32(32.0f) < totalArea)) { + dgConveFaceNode* const corner = sortHeap[0]; + if (corner->m_mask && corner->m_prev->m_mask) { + if (hullPoint == corner) { + hullPoint = corner->m_prev; + } + count --; + hasLinearCombination = true; + corner->m_prev->m_mask = 0; + corner->m_next->m_prev = corner->m_prev; + corner->m_prev->m_next = corner->m_next; + } + sortHeap.Pop(); + } + } + + const dgInt32 maxVertexCount = DG_MAX_VERTEX_CLIP_FACE; + while (count > maxVertexCount) { + sortHeap.Flush(); + dgConveFaceNode* ptr = hullPoint; + dgVector e0(contactsOut[ptr->m_next->m_vertexIndex] - contactsOut[ptr->m_vertexIndex]); + do { + dgVector e1(contactsOut[ptr->m_next->m_next->m_vertexIndex] - contactsOut[ptr->m_next->m_vertexIndex]); + dgFloat32 area = dgAbs(e0.CrossProduct(e1).DotProduct(normal).GetScalar()); + sortHeap.Push(ptr->m_next, area); + e0 = e1; + ptr->m_mask = 1; + ptr = ptr->m_next; + } while (ptr != hullPoint); + + while (sortHeap.GetCount() && (count > maxVertexCount)) { + dgConveFaceNode* const corner = sortHeap[0]; + if (corner->m_mask && corner->m_prev->m_mask) { + if (hullPoint == corner) { + hullPoint = corner->m_prev; + } + count--; + corner->m_prev->m_mask = 0; + corner->m_next->m_prev = corner->m_prev; + corner->m_prev->m_next = corner->m_next; + } + sortHeap.Pop(); + } + } + + dgInt32 index = start; + dgConveFaceNode* ptr = hullPoint; + do { + contactsOut[index] = contactsOut[ptr->m_vertexIndex]; + index ++; + ptr = ptr->m_next; + } while (ptr != hullPoint); + memcpy (contactsOut, &contactsOut[start], count * sizeof (dgVector)); + + dgAssert (SanityCheck(count, normal, contactsOut)); + return count; +} + +dgVector dgCollisionConvex::SupportVertexSpecial (const dgVector& dir, dgFloat32 skinThickness, dgInt32* const vertexIndex) const +{ + return SupportVertex(dir, vertexIndex); +} + +dgVector dgCollisionConvex::SupportVertexSpecialProjectPoint (const dgVector& point, const dgVector& dir) const + { + return point; + } + + +dgFloat32 dgCollisionConvex::RayCast(const dgVector& localP0, const dgVector& localP1, dgFloat32 maxT, dgContactPoint& contactOut, const dgBody* const body, void* const userData, OnRayPrecastAction preFilter) const +{ + dgCollisionInstance instance(*body->GetCollision(), this); + dgContactSolver rayCaster(&instance); + instance.m_material.m_userData = NULL; + return rayCaster.RayCast(localP0, localP1, maxT, contactOut); +} + +dgInt32 dgCollisionConvex::CalculatePlaneIntersection (const dgVector& normal, const dgVector& origin, dgVector* const contactsOut) const +{ + dgVector support[4]; + dgInt32 featureCount = 3; + const dgConvexSimplexEdge* edge = &m_simplex[0]; + const dgConvexSimplexEdge** const vertToEdgeMapping = GetVertexToEdgeMapping(); + dgAssert (normal.m_w == dgFloat32 (0.0f)); + if (vertToEdgeMapping) { + dgInt32 edgeIndex; + featureCount = 1; + support[0] = SupportVertex (normal, &edgeIndex); + edge = vertToEdgeMapping[edgeIndex]; + + // 5 degrees + const dgFloat32 tiltAngle = dgFloat32 (0.087f); + const dgFloat32 tiltAngle2 = tiltAngle * tiltAngle ; + dgPlane testPlane (normal, - (normal.DotProduct(support[0]).GetScalar())); + const dgConvexSimplexEdge* ptr = edge; + do { + const dgVector& p = m_vertex[ptr->m_twin->m_vertex]; + dgFloat32 test = testPlane.Evalue(p); + dgVector dist (p - support[0]); + dgAssert (dist.m_w == dgFloat32 (0.0f)); + dgFloat32 angle2 = test * test / (dist.DotProduct(dist).GetScalar()); + + if (angle2 < tiltAngle2) { + support[featureCount] = p; + featureCount ++; + } + ptr = ptr->m_twin->m_next; + } while ((ptr != edge) && (featureCount < 3)); + } + + dgInt32 count = 0; + dgPlane plane (normal, - normal.DotProduct(origin).GetScalar()); + switch (featureCount) + { + case 1: + contactsOut[0] = support[0] - normal * normal.DotProduct(support[0] - origin); + count = 1; + break; + + case 2: + contactsOut[0] = support[0] - normal * normal.DotProduct(support[0] - origin); + contactsOut[1] = support[1] - normal * normal.DotProduct(support[1] - origin); + count = 2; + break; + + default: + { + dgFloat32 side0 = plane.Evalue(m_vertex[edge->m_vertex]); + dgFloat32 side1 = side0; + const dgConvexSimplexEdge* firstEdge = NULL; + if (side0 > dgFloat32 (0.0f)) { + const dgConvexSimplexEdge* ptr = edge; + do { + dgAssert (m_vertex[ptr->m_twin->m_vertex].m_w == dgFloat32 (0.0f)); + side1 = plane.Evalue (m_vertex[ptr->m_twin->m_vertex]); + if (side1 < side0) { + if (side1 < dgFloat32 (0.0f)) { + firstEdge = ptr; + break; + } + + side0 = side1; + edge = ptr->m_twin; + ptr = edge; + } + ptr = ptr->m_twin->m_next; + } while (ptr != edge); + + + if (!firstEdge) { + // we may have a local minimal in the convex hull do to a big flat face + for (dgInt32 i = 0; i < m_edgeCount; i ++) { + ptr = &m_simplex[i]; + side0 = plane.Evalue (m_vertex[ptr->m_vertex]); + side1 = plane.Evalue (m_vertex[ptr->m_twin->m_vertex]); + if ((side1 < dgFloat32 (0.0f)) && (side0 > dgFloat32 (0.0f))){ + firstEdge = ptr; + break; + } + } + } + + } else if (side0 < dgFloat32 (0.0f)) { + const dgConvexSimplexEdge* ptr = edge; + do { + dgAssert (m_vertex[ptr->m_twin->m_vertex].m_w == dgFloat32 (0.0f)); + side1 = plane.Evalue (m_vertex[ptr->m_twin->m_vertex]); + if (side1 > side0) { + if (side1 >= dgFloat32 (0.0f)) { + side0 = side1; + firstEdge = ptr->m_twin; + break; + } + + side0 = side1; + edge = ptr->m_twin; + ptr = edge; + } + ptr = ptr->m_twin->m_next; + } while (ptr != edge); + + if (!firstEdge) { + // we may have a local minimal in the convex hull due to a big flat face + for (dgInt32 i = 0; i < m_edgeCount; i ++) { + ptr = &m_simplex[i]; + side0 = plane.Evalue (m_vertex[ptr->m_vertex]); + //dgFloat32 side1 = plane.Evalue (m_vertex[ptr->m_twin->m_vertex]); + side1 = plane.Evalue(m_vertex[ptr->m_twin->m_vertex]); + if ((side1 < dgFloat32 (0.0f)) && (side0 > dgFloat32 (0.0f))){ + firstEdge = ptr; + break; + } + } + } + } + + if (firstEdge) { + dgAssert (side0 >= dgFloat32 (0.0f)); + dgAssert ((side1 = plane.Evalue (m_vertex[firstEdge->m_vertex])) >= dgFloat32 (0.0f)); + dgAssert ((side1 = plane.Evalue (m_vertex[firstEdge->m_twin->m_vertex])) < dgFloat32 (0.0f)); + dgAssert (dgAbs (side0 - plane.Evalue (m_vertex[firstEdge->m_vertex])) < dgFloat32 (1.0e-5f)); + + dgInt32 maxCount = 0; + const dgConvexSimplexEdge* ptr = firstEdge; + do { + if (side0 > dgFloat32 (0.0f)) { + dgAssert (plane.Evalue (m_vertex[ptr->m_vertex]) > dgFloat32 (0.0f)); + dgAssert (plane.Evalue (m_vertex[ptr->m_twin->m_vertex]) < dgFloat32 (0.0f)); + + dgVector dp (m_vertex[ptr->m_twin->m_vertex] - m_vertex[ptr->m_vertex]); + dgAssert (dp.m_w == dgFloat32 (0.0f)); + dgFloat32 t = plane.DotProduct(dp).GetScalar(); + if (t >= dgFloat32 (-1.e-24f)) { + t = dgFloat32 (0.0f); + } else { + t = side0 / t; + if (t > dgFloat32 (0.0f)) { + t = dgFloat32 (0.0f); + } + if (t < dgFloat32 (-1.0f)) { + t = dgFloat32 (-1.0f); + } + } + + dgAssert (t <= dgFloat32 (0.01f)); + dgAssert (t >= dgFloat32 (-1.05f)); + contactsOut[count] = m_vertex[ptr->m_vertex] - dp.Scale (t); + + dgConvexSimplexEdge* ptr1 = ptr->m_next; + for (; ptr1 != ptr; ptr1 = ptr1->m_next) { + dgAssert (m_vertex[ptr->m_twin->m_vertex].m_w == dgFloat32 (0.0f)); + side0 = plane.Evalue (m_vertex[ptr1->m_twin->m_vertex]); + if (side0 >= dgFloat32 (0.0f)) { + break; + } + } + dgAssert (ptr1 != ptr); + ptr = ptr1->m_twin; + } else { + contactsOut[count] = m_vertex[ptr->m_vertex]; + dgConvexSimplexEdge* ptr1 = ptr->m_next; + for (; ptr1 != ptr; ptr1 = ptr1->m_next) { + dgAssert (m_vertex[ptr1->m_twin->m_vertex].m_w == dgFloat32 (0.0f)); + side0 = plane.Evalue (m_vertex[ptr1->m_twin->m_vertex]); + if (side0 >= dgFloat32 (0.0f)) { + break; + } + } + + if (ptr1 == ptr) { + ptr = ptr1->m_prev->m_twin; + } else { + ptr = ptr1->m_twin; + } + } + + count ++; + maxCount ++; + if (count >= DG_CLIP_MAX_POINT_COUNT) { + for (count = 0; count < (DG_CLIP_MAX_POINT_COUNT >> 1); count ++) { + contactsOut[count] = contactsOut[count * 2]; + } + } + + } while ((ptr != firstEdge) && (maxCount < DG_CLIP_MAX_COUNT)); + dgAssert (maxCount < DG_CLIP_MAX_COUNT); + + if (count > 2) { + count = RectifyConvexSlice (count, normal, contactsOut); + } + } + } + } + return count; +} + +dgInt32 dgCollisionConvex::BuildCylinderCapPoly (dgFloat32 radius, const dgMatrix& transform, dgVector* const vertexOut) const +{ +/* + dgFloat32 h = 2.0; + dgInt32 n = 8; + dgFloat32 a0 = h * h * (dgPi / n); + + dgFloat32 h0 = h * dgSin (0.5 * dgPI2 / n); + dgFloat32 h1 = h * dgCos (0.5 * dgPI2 / n); + dgFloat32 a1 = h * h * (dgSin (0.5 * dgPI2 / n) * dgCos (0.5 * dgPI2 / n)); + + dgFloat32 a = h * h * (dgPi / n - 0.5f * dgSin (dgPI2 / n)); + + for (int i = 8; i < 16; i ++) { + dgFloat32 den = dgPi / i - 0.5f * dgSin (dgPI2 / i); + dgFloat32 h1 = dgSqrt (a / den); + dgFloat32 h2 = dgSqrt (a / den); + } +*/ + + dgInt32 count = (radius < dgFloat32 (1.0f)) ? 8 : ((radius < dgFloat32 (2.0f)) ? 12 : 16); + + dgFloat32 angle = dgPI2 / count; + dgVector r (dgFloat32 (0.0f), dgFloat32 (0.0f), radius, dgFloat32 (0.0f)); + dgMatrix rotation (dgPitchMatrix(angle)); + + for (dgInt32 i = 0; i < count; i++) { + vertexOut[i] = transform.TransformVector(r); + r = rotation.RotateVector(r); + } + + return count; +} \ No newline at end of file diff --git a/thirdparty/src/newton/dgPhysics/dgCollisionConvex.h b/thirdparty/src/newton/dgPhysics/dgCollisionConvex.h new file mode 100644 index 000000000..c3914356f --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgCollisionConvex.h @@ -0,0 +1,110 @@ +/* Copyright (c) <2003-2019> +* +* 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_CONVEX_COLLISION_H__ +#define _DG_CONVEX_COLLISION_H__ + +#include "dgCollision.h" + +#define DG_CLIP_MAX_COUNT 512 +#define DG_CLIP_MAX_POINT_COUNT 64 +#define D_MIN_CONVEX_SHAPE_SIZE dgFloat32 (1.0f/128.0f) + + +DG_MSC_VECTOR_ALIGNMENT +class dgCollisionConvex: public dgCollision +{ + public: + class dgConvexSimplexEdge + { + public: + dgConvexSimplexEdge* m_twin; + dgConvexSimplexEdge* m_next; + dgConvexSimplexEdge* m_prev; + dgInt32 m_vertex; + }; + + virtual dgInt32 GetConvexVertexCount() const { return m_vertexCount;} + virtual void CalcAABB (const dgMatrix& matrix, dgVector& p0, dgVector& p1) const; + virtual dgVector SupportVertex (const dgVector& dir, dgInt32* const vertexIndex) const; + virtual dgInt32 CalculatePlaneIntersection (const dgVector& normal, const dgVector& point, dgVector* const contactsOut) const; + virtual dgFloat32 RayCast (const dgVector& localP0, const dgVector& localP1, dgFloat32 maxT, dgContactPoint& contactOut, const dgBody* const body, void* const userData, OnRayPrecastAction preFilter) const; + + bool IntesectionTest (dgCollisionParamProxy& proxy) const; + + protected: + dgCollisionConvex (dgMemoryAllocator* const allocator, dgUnsigned32 signature, dgCollisionID id); + dgCollisionConvex (dgWorld* const world, dgDeserialize deserialization, void* const userData, dgInt32 revisionNumber); + ~dgCollisionConvex (); + + virtual void SerializeLow(dgSerialize callback, void* const userData) const; + + virtual dgVector CalculateVolumeIntegral (const dgMatrix& globalMatrix, const dgVector& plane, const dgCollisionInstance& parentScale) const; + static void CalculateInertia (void *userData, int vertexCount, const dgFloat32* const FaceArray, int faceId); + + virtual dgFloat32 GetVolume () const; + + virtual dgFloat32 GetBoxMinRadius () const; + virtual dgFloat32 GetBoxMaxRadius () const; + + dgInt32 RayCastClosestFace (dgVector* tetrahedrum, const dgVector& origin, dgFloat32& pointDist) const; + dgVector CalculateVolumeIntegral (const dgPlane& plane) const; + + void SetVolumeAndCG (); + bool SanityCheck (dgPolyhedra& hull) const; + virtual void DebugCollision (const dgMatrix& matrix, dgCollision::OnDebugCollisionMeshCallback callback, void* const userData) const; + + virtual void MassProperties (); + virtual dgMatrix CalculateInertiaAndCenterOfMass (const dgMatrix& m_alignMatrix, const dgVector& localScale, const dgMatrix& matrix) const; + virtual dgFloat32 CalculateMassProperties (const dgMatrix& offset, dgVector& inertia, dgVector& crossInertia, dgVector& centerOfMass) const; + + bool SanityCheck(dgInt32 count, const dgVector& normal, dgVector* const contactsOut) const; + + dgInt32 RectifyConvexSlice (dgInt32 count, const dgVector& normal, dgVector* const contactsOut) const; + + virtual dgVector SupportVertexSpecial (const dgVector& dir, dgFloat32 skinThickness, dgInt32* const vertexIndex) const; + virtual dgVector SupportVertexSpecialProjectPoint (const dgVector& point, const dgVector& dir) const; + virtual const dgConvexSimplexEdge** GetVertexToEdgeMapping() const {return NULL;} + + dgInt32 BuildCylinderCapPoly (dgFloat32 radius, const dgMatrix& transform, dgVector* const vertexOut) const; + + dgVector* m_vertex; + dgConvexSimplexEdge* m_simplex; + + dgFloat32 m_boxMinRadius; + dgFloat32 m_boxMaxRadius; + dgFloat32 m_simplexVolume; + + dgUnsigned16 m_edgeCount; + dgUnsigned16 m_vertexCount; + + friend class dgWorld; + friend class dgBroadPhase; + friend class dgMinkowskiConv; + friend class dgCollisionCompound; + friend class dgCollisionConvexModifier; +} DG_GCC_VECTOR_ALIGNMENT; + + + +#endif //AFX_DGCONVEXCOLLISION_H__57E159CE_6B6F_42DE_891C_1F6C38EB9D29_H + + diff --git a/thirdparty/src/newton/dgPhysics/dgCollisionConvexHull.cpp b/thirdparty/src/newton/dgPhysics/dgCollisionConvexHull.cpp new file mode 100644 index 000000000..45edb170b --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgCollisionConvexHull.cpp @@ -0,0 +1,1046 @@ +/* Copyright (c) <2003-2019> +* +* 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 "dgPhysicsStdafx.h" +#include "dgBody.h" +#include "dgContact.h" +#include "dgMeshEffect.h" +#include "dgCollisionConvexHull.h" + + + +////////////////////////////////////////////////////////////////////// +// Construction/Destruction +////////////////////////////////////////////////////////////////////// + +#define DG_CONVEX_VERTEX_BOX_CELL_SIZE (1<<3) +#define DG_CONVEX_VERTEX_SPLITE_SIZE (DG_CONVEX_VERTEX_BOX_CELL_SIZE * 3) + +DG_MSC_VECTOR_ALIGNMENT +class dgCollisionConvexHull::dgSOAVectorArray +{ + public: + dgVector m_x[DG_CONVEX_VERTEX_SPLITE_SIZE/4]; + dgVector m_y[DG_CONVEX_VERTEX_SPLITE_SIZE/4]; + dgVector m_z[DG_CONVEX_VERTEX_SPLITE_SIZE/4]; + dgVector m_index[DG_CONVEX_VERTEX_SPLITE_SIZE/4]; +} DG_GCC_VECTOR_ALIGNMENT; + +DG_MSC_VECTOR_ALIGNMENT +class dgCollisionConvexHull::dgConvexBox +{ + public: + dgVector m_box[2]; + dgInt32 m_vertexStart; + dgInt32 m_vertexCount; + dgInt32 m_leftBox; + dgInt32 m_rightBox; +} DG_GCC_VECTOR_ALIGNMENT; + +dgCollisionConvexHull::dgCollisionConvexHull(dgMemoryAllocator* const allocator, dgUnsigned32 signature) + :dgCollisionConvex(allocator, signature, m_convexHullCollision) + ,m_supportTree(NULL) + ,m_faceArray(NULL) + ,m_soaVertexArray(NULL) + ,m_vertexToEdgeMapping(NULL) + ,m_faceCount(0) + ,m_soaVertexCount(0) + ,m_supportTreeCount(0) +{ + m_edgeCount = 0; + m_vertexCount = 0; + m_vertex = NULL; + m_simplex = NULL; + m_rtti |= dgCollisionConvexHull_RTTI; +} + +dgCollisionConvexHull::dgCollisionConvexHull(dgMemoryAllocator* const allocator, dgUnsigned32 signature, dgInt32 count, dgInt32 strideInBytes, dgFloat32 tolerance, const dgFloat32* const vertexArray) + :dgCollisionConvex(allocator, signature, m_convexHullCollision) + ,m_supportTree(NULL) + ,m_faceArray(NULL) + ,m_soaVertexArray(NULL) + ,m_vertexToEdgeMapping(NULL) + ,m_faceCount(0) + ,m_soaVertexCount(0) + ,m_supportTreeCount(0) +{ + m_edgeCount = 0; + m_vertexCount = 0; + m_vertex = NULL; + m_simplex = NULL; + m_rtti |= dgCollisionConvexHull_RTTI; + + BuildHull (count, strideInBytes, tolerance, vertexArray); +} + +dgCollisionConvexHull::dgCollisionConvexHull(dgWorld* const world, dgDeserialize callback, void* const userData, dgInt32 revisionNumber) + :dgCollisionConvex (world, callback, userData, revisionNumber) + ,m_supportTree(NULL) + ,m_faceArray(NULL) + ,m_soaVertexArray(NULL) + ,m_vertexToEdgeMapping(NULL) + ,m_faceCount(0) + ,m_soaVertexCount(0) + ,m_supportTreeCount(0) +{ + m_rtti |= dgCollisionConvexHull_RTTI; + + dgInt32 edgeCount; + dgInt32 vertexCount; + callback(userData, &vertexCount, sizeof(dgInt32)); + callback(userData, &edgeCount, sizeof(dgInt32)); + callback(userData, &m_faceCount, sizeof(dgInt32)); + callback(userData, &m_supportTreeCount, sizeof(dgInt32)); + + m_edgeCount = dgUnsigned16(edgeCount); + m_vertexCount = dgUnsigned16 (vertexCount); + + m_vertex = (dgVector*) m_allocator->Malloc (dgInt32 (m_vertexCount * sizeof (dgVector))); + m_simplex = (dgConvexSimplexEdge*) m_allocator->Malloc (dgInt32 (m_edgeCount * sizeof (dgConvexSimplexEdge))); + m_faceArray = (dgConvexSimplexEdge **) m_allocator->Malloc(dgInt32 (m_faceCount * sizeof(dgConvexSimplexEdge *))); + m_vertexToEdgeMapping = (const dgConvexSimplexEdge **) m_allocator->Malloc(dgInt32 (m_vertexCount * sizeof(dgConvexSimplexEdge *))); + + callback(userData, m_vertex, m_vertexCount * sizeof(dgVector)); + + if (m_supportTreeCount) { + m_supportTree = (dgConvexBox *) m_allocator->Malloc(dgInt32 (m_supportTreeCount * sizeof(dgConvexBox))); + callback (userData, m_supportTree, m_supportTreeCount * sizeof(dgConvexBox)); + } + + for (dgInt32 i = 0; i < m_edgeCount; i ++) { + dgInt32 serialization[4]; + callback (userData, serialization, sizeof (serialization)); + + m_simplex[i].m_vertex = serialization[0]; + m_simplex[i].m_twin = m_simplex + serialization[1]; + m_simplex[i].m_next = m_simplex + serialization[2]; + m_simplex[i].m_prev = m_simplex + serialization[3]; + } + + for (dgInt32 i = 0; i < m_faceCount; i ++) { + dgInt32 faceOffset; + callback (userData, &faceOffset, sizeof (dgInt32)); + m_faceArray[i] = m_simplex + faceOffset; + } + + for (dgInt32 i = 0; i < m_vertexCount; i ++) { + dgInt32 faceOffset; + callback (userData, &faceOffset, sizeof (dgInt32)); + m_vertexToEdgeMapping[i] = m_simplex + faceOffset; + } + + if (vertexCount <= DG_CONVEX_VERTEX_SPLITE_SIZE) { + CreateSOAdata(); + } + + SetVolumeAndCG (); +} + +dgCollisionConvexHull::~dgCollisionConvexHull() +{ + if (m_vertexToEdgeMapping) { + m_allocator->Free(m_vertexToEdgeMapping); + } + + if (m_faceArray) { + m_allocator->Free(m_faceArray); + } + + if (m_supportTree) { + m_allocator->Free(m_supportTree); + } + + if (m_soaVertexArray) { + m_allocator->Free(m_soaVertexArray); + } +} + +void dgCollisionConvexHull::Serialize(dgSerialize callback, void* const userData) const +{ + SerializeLow(callback, userData); + + dgInt32 edgeCount = m_edgeCount; + dgInt32 vertexCount = m_vertexCount; + callback(userData, &vertexCount, sizeof(dgInt32)); + callback(userData, &edgeCount, sizeof(dgInt32)); + callback(userData, &m_faceCount, sizeof(dgInt32)); + callback(userData, &m_supportTreeCount, sizeof(dgInt32)); + + callback(userData, m_vertex, m_vertexCount * sizeof(dgVector)); + + if (m_supportTreeCount) { + callback(userData, m_supportTree, m_supportTreeCount * sizeof(dgConvexBox)); + } + + for (dgInt32 i = 0; i < m_edgeCount; i++) { + dgInt32 serialization[4]; + serialization[0] = m_simplex[i].m_vertex; + serialization[1] = dgInt32(m_simplex[i].m_twin - m_simplex); + serialization[2] = dgInt32(m_simplex[i].m_next - m_simplex); + serialization[3] = dgInt32(m_simplex[i].m_prev - m_simplex); + callback(userData, serialization, sizeof(serialization)); + } + + for (dgInt32 i = 0; i < m_faceCount; i++) { + dgInt32 faceOffset; + faceOffset = dgInt32(m_faceArray[i] - m_simplex); + callback(userData, &faceOffset, sizeof(dgInt32)); + } + + for (dgInt32 i = 0; i < m_vertexCount; i++) { + dgInt32 faceOffset; + faceOffset = dgInt32(m_vertexToEdgeMapping[i] - m_simplex); + callback(userData, &faceOffset, sizeof(dgInt32)); + } +} + +void dgCollisionConvexHull::BuildHull (dgInt32 count, dgInt32 strideInBytes, dgFloat32 tolerance, const dgFloat32* const vertexArray) +{ + Create (count, strideInBytes, vertexArray, tolerance); +} + +void dgCollisionConvexHull::MassProperties () +{ + dgFloat32 volume = dgCollisionConvex::CalculateMassProperties(dgGetIdentityMatrix(), m_inertia, m_crossInertia, m_centerOfMass); + if (volume < dgFloat32 (1.0e-6f)) { + volume = dgFloat32 (1.0e-6f); + } + dgFloat32 invVolume = dgFloat32(1.0f) / volume; + m_inertia = m_inertia.Scale(invVolume); + m_crossInertia = m_crossInertia.Scale(invVolume); + m_centerOfMass = m_centerOfMass.Scale(invVolume); + m_centerOfMass.m_w = volume; + + + dgMatrix inertia(dgGetIdentityMatrix()); + inertia[0][0] = m_inertia[0]; + inertia[1][1] = m_inertia[1]; + inertia[2][2] = m_inertia[2]; + inertia[0][1] = m_crossInertia[2]; + inertia[1][0] = m_crossInertia[2]; + inertia[0][2] = m_crossInertia[1]; + inertia[2][0] = m_crossInertia[1]; + inertia[1][2] = m_crossInertia[0]; + inertia[2][1] = m_crossInertia[0]; + + dgVector origin(m_centerOfMass); + dgFloat32 originMag2 = origin.DotProduct(origin & dgVector::m_triplexMask).GetScalar(); + + dgMatrix Covariance(origin, origin); + dgMatrix parallel(dgGetIdentityMatrix()); + for (dgInt32 i = 0; i < 3; i++) { + parallel[i][i] = originMag2; + inertia[i] -= (parallel[i] - Covariance[i]); + dgAssert(inertia[i][i] > dgFloat32(0.0f)); + } + + m_inertia[0] = inertia[0][0]; + m_inertia[1] = inertia[1][1]; + m_inertia[2] = inertia[2][2]; + m_crossInertia[0] = inertia[2][1]; + m_crossInertia[1] = inertia[2][0]; + m_crossInertia[2] = inertia[1][0]; +} + +dgInt32 dgCollisionConvexHull::GetFaceIndices (dgInt32 index, dgInt32* const indices) const +{ + dgInt32 count = 0; + const dgConvexSimplexEdge* face = m_faceArray[index]; + do { + indices [count] = face->m_vertex; + count ++; + face = face->m_next; + } while (face != m_faceArray[index]); + + return count; +} + +dgBigVector dgCollisionConvexHull::FaceNormal (const dgEdge *face, const dgBigVector* const pool) const +{ + const dgEdge* edge = face; + dgBigVector p0 (pool[edge->m_incidentVertex]); + edge = edge->m_next; + + dgBigVector p1 (pool[edge->m_incidentVertex]); + dgBigVector e1 (p1 - p0); + + dgBigVector normal (dgFloat32 (0.0f)); + for (edge = edge->m_next; edge != face; edge = edge->m_next) { + dgBigVector p2 (pool[edge->m_incidentVertex]); + dgBigVector e2 (p2 - p0); + dgBigVector n1 (e1.CrossProduct(e2)); +#ifdef _DEBUG + dgAssert(n1.m_w == dgFloat32(0.0f)); + dgFloat64 mag = normal.DotProduct(n1).GetScalar(); + dgAssert ( mag >= -dgFloat32 (0.1f)); +#endif + normal += n1; + e1 = e2; + } + + dgFloat64 den = sqrt (normal.DotProduct(normal).GetScalar()) + dgFloat64 (1.0e-24f); + normal = normal.Scale (dgFloat64 (1.0f)/ den); + +#ifdef _DEBUG + edge = face; + dgBigVector e0 (pool[edge->m_incidentVertex] - pool[edge->m_prev->m_incidentVertex]); + do { + dgBigVector de1 (pool[edge->m_next->m_incidentVertex] - pool[edge->m_incidentVertex]); + dgBigVector dn1 (e0.CrossProduct(de1)); + dgFloat64 x = normal.DotProduct(dn1).GetScalar(); + dgAssert (x > -dgFloat64 (0.01f)); + e0 = de1; + edge = edge->m_next; + } while (edge != face); +#endif + return normal; +} + +bool dgCollisionConvexHull::RemoveCoplanarEdge (dgPolyhedra& polyhedra, const dgBigVector* const hullVertexArray) const +{ + bool removeEdge = false; + // remove coplanar edges + dgInt32 mark = polyhedra.IncLRU(); + dgPolyhedra::Iterator iter (polyhedra); + for (iter.Begin(); iter; ) { + dgEdge* edge0 = &(*iter); + iter ++; + + if (edge0->m_incidentFace != -1) { + + if (edge0->m_mark < mark) { + edge0->m_mark = mark; + edge0->m_twin->m_mark = mark; + dgBigVector normal0 (FaceNormal (edge0, &hullVertexArray[0])); + dgBigVector normal1 (FaceNormal (edge0->m_twin, &hullVertexArray[0])); + + dgFloat64 test = normal0.DotProduct(normal1).GetScalar(); + if (test > dgFloat64 (0.99995f)) { + + if ((edge0->m_twin->m_next->m_twin->m_next != edge0) && (edge0->m_next->m_twin->m_next != edge0->m_twin)) { + #define DG_MAX_EDGE_ANGLE dgFloat32 (1.0e-3f) + + if (edge0->m_twin == &(*iter)) { + if (iter) { + iter ++; + } + } + + dgBigVector e1 (hullVertexArray[edge0->m_twin->m_next->m_next->m_incidentVertex] - hullVertexArray[edge0->m_incidentVertex]); + dgBigVector e0 (hullVertexArray[edge0->m_incidentVertex] - hullVertexArray[edge0->m_prev->m_incidentVertex]); + + dgAssert(e0.m_w == dgFloat64(0.0f)); + dgAssert(e1.m_w == dgFloat64(0.0f)); + dgAssert (e0.DotProduct(e0).GetScalar() >= dgFloat64 (0.0f)); + dgAssert (e1.DotProduct(e1).GetScalar() >= dgFloat64 (0.0f)); + + e0 = e0.Scale (dgFloat64 (1.0f) / sqrt (e0.DotProduct(e0).GetScalar())); + e1 = e1.Scale (dgFloat64 (1.0f) / sqrt (e1.DotProduct(e1).GetScalar())); + dgBigVector n1 (e0.CrossProduct(e1)); + + dgFloat64 projection = n1.DotProduct(normal0).GetScalar(); + if (projection >= DG_MAX_EDGE_ANGLE) { + + dgBigVector e11 (hullVertexArray[edge0->m_next->m_next->m_incidentVertex] - hullVertexArray[edge0->m_twin->m_incidentVertex]); + dgBigVector e00 (hullVertexArray[edge0->m_twin->m_incidentVertex] - hullVertexArray[edge0->m_twin->m_prev->m_incidentVertex]); + dgAssert (e00.m_w == dgFloat64 (0.0f)); + dgAssert (e11.m_w == dgFloat64 (0.0f)); + dgAssert (e00.DotProduct(e00).GetScalar() >= dgFloat64 (0.0f)); + dgAssert (e11.DotProduct(e11).GetScalar() >= dgFloat64 (0.0f)); + e00 = e00.Scale(dgFloat64(1.0f) / sqrt(e00.DotProduct(e00).GetScalar())); + e11 = e11.Scale(dgFloat64(1.0f) / sqrt(e11.DotProduct(e11).GetScalar())); + + dgBigVector n11 (e00.CrossProduct(e11)); + projection = n11.DotProduct(normal0).GetScalar(); + if (projection >= DG_MAX_EDGE_ANGLE) { + dgAssert (&(*iter) != edge0); + dgAssert (&(*iter) != edge0->m_twin); + polyhedra.DeleteEdge(edge0); + removeEdge = true; + } + } + + } else { + dgEdge* next = edge0->m_next; + dgEdge* prev = edge0->m_prev; + polyhedra.DeleteEdge(edge0); + for (edge0 = next; edge0->m_prev->m_twin == edge0; edge0 = next) { + next = edge0->m_next; + polyhedra.DeleteEdge(edge0); + } + + for (edge0 = prev; edge0->m_next->m_twin == edge0; edge0 = prev) { + prev = edge0->m_prev; + polyhedra.DeleteEdge(edge0); + } + iter.Begin(); + removeEdge = true; + } + } + } + } + } + + return removeEdge; +} + + +bool dgCollisionConvexHull::CheckConvex (dgPolyhedra& polyhedra1, const dgBigVector* hullVertexArray) const +{ + dgPolyhedra polyhedra(polyhedra1); + + dgPolyhedra::Iterator iter (polyhedra); + dgBigVector center (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f)); + + dgInt32 count = 0; + dgInt32 mark = polyhedra.IncLRU(); + for (iter.Begin(); iter; iter ++) { + dgEdge* const edge = &(*iter); + if (edge->m_mark < mark) { + count ++; + center += hullVertexArray[edge->m_incidentVertex]; + dgEdge* ptr = edge; + do { + ptr->m_mark = mark; + ptr = ptr->m_twin->m_next; + } while (ptr != edge); + } + } + center = center.Scale (dgFloat64 (1.0f) / dgFloat64 (count)); + + for (iter.Begin(); iter; iter ++) { + dgEdge* const edge = &(*iter); + dgBigVector normal0 (FaceNormal (edge, hullVertexArray)); + dgBigVector normal1 (FaceNormal (edge->m_twin, hullVertexArray)); + + dgBigPlane plane0 (normal0, - normal0.DotProduct(hullVertexArray[edge->m_incidentVertex]).GetScalar()); + dgBigPlane plane1 (normal1, - normal1.DotProduct(hullVertexArray[edge->m_twin->m_incidentVertex]).GetScalar()); + dgFloat64 test0 = plane0.Evalue(center); + if (test0 > dgFloat64 (1.0e-3f)) { + return false; + } + dgFloat64 test1 = plane1.Evalue(center); +// if (test1 > dgFloat64 (0.0f)) { + if (test1 > dgFloat64 (1.0e-3f)) { + return false; + } + } + return true; +} + +bool dgCollisionConvexHull::Create (dgInt32 count, dgInt32 strideInBytes, const dgFloat32* const vertexArray, dgFloat32 tolerance) +{ + dgInt32 stride = strideInBytes / sizeof (dgFloat32); + dgStack buffer(3 * 2 * count); + for (dgInt32 i = 0; i < count; i ++) { + buffer[i * 3 + 0] = vertexArray[i * stride + 0]; + buffer[i * 3 + 1] = vertexArray[i * stride + 1]; + buffer[i * 3 + 2] = vertexArray[i * stride + 2]; + } + + dgConvexHull3d* convexHull = new (GetAllocator()) dgConvexHull3d (GetAllocator(), &buffer[0], 3 * sizeof (dgFloat64), count, tolerance); + if (!convexHull->GetCount()) { + // this is a degenerated hull hull to add some thickness and for a thick plane + delete convexHull; + + dgStack tmp(3 * count); + for (dgInt32 i = 0; i < count; i ++) { + tmp[i][0] = dgFloat32 (buffer[i*3 + 0]); + tmp[i][1] = dgFloat32 (buffer[i*3 + 1]); + tmp[i][2] = dgFloat32 (buffer[i*3 + 2]); + tmp[i][2] = dgFloat32 (0.0f); + } + + dgObb sphere; + sphere.SetDimensions (&tmp[0][0], sizeof (dgVector), count); + + dgInt32 index = 0; + dgFloat32 size = dgFloat32 (1.0e10f); + for (dgInt32 i = 0; i < 3; i ++) { + if (sphere.m_size[i] < size) { + index = i; + size = sphere.m_size[i]; + } + } + dgVector normal (dgFloat32 (0.0f)); + normal[index] = dgFloat32 (1.0f); + dgVector step = sphere.RotateVector (normal.Scale (dgFloat32 (0.05f))); + for (dgInt32 i = 0; i < count; i ++) { + dgVector p1 (tmp[i] + step); + dgVector p2 (tmp[i] - step); + + buffer[i * 3 + 0] = p1.m_x; + buffer[i * 3 + 1] = p1.m_y; + buffer[i * 3 + 2] = p1.m_z; + buffer[(i + count) * 3 + 0] = p2.m_x; + buffer[(i + count) * 3 + 1] = p2.m_y; + buffer[(i + count) * 3 + 2] = p2.m_z; + } + count *= 2; + convexHull = new (GetAllocator()) dgConvexHull3d (GetAllocator(), &buffer[0], 3 * sizeof (dgFloat64), count, tolerance); + if (!convexHull->GetCount()) { + delete convexHull; + return false; + } + } + + // check for degenerated faces + for (bool success = false; !success; ) { + success = true; + const dgBigVector* const hullVertexArray = convexHull->GetVertexPool(); + + dgStack mask(convexHull->GetVertexCount()); + memset (&mask[0], 1, mask.GetSizeInBytes()); + for (dgConvexHull3d::dgListNode* node = convexHull->GetFirst(); node; node = node->GetNext()) { + dgConvexHull3DFace& face = node->GetInfo(); + const dgBigVector& p0 = hullVertexArray[face.m_index[0]]; + const dgBigVector& p1 = hullVertexArray[face.m_index[1]]; + const dgBigVector& p2 = hullVertexArray[face.m_index[2]]; + dgAssert(p0.m_w == p1.m_w); + dgAssert(p0.m_w == p2.m_w); + dgBigVector p1p0 (p1 - p0); + dgBigVector p2p0 (p2 - p0); + dgBigVector normal (p2p0.CrossProduct(p1p0)); + dgFloat64 mag2 = normal.DotProduct(normal).GetScalar(); + if (mag2 < dgFloat64 (1.0e-6f * 1.0e-6f)) { + success = false; + dgInt32 index = -1; + dgBigVector p2p1 (p2 - p1); + dgFloat64 dist10 = p1p0.DotProduct(p1p0).GetScalar(); + dgFloat64 dist20 = p2p0.DotProduct(p2p0).GetScalar(); + dgFloat64 dist21 = p2p1.DotProduct(p2p1).GetScalar(); + if ((dist10 >= dist20) && (dist10 >= dist21)) { + index = 2; + } else if ((dist20 >= dist10) && (dist20 >= dist21)) { + index = 1; + } else if ((dist21 >= dist10) && (dist21 >= dist20)) { + index = 0; + } + dgAssert (index != -1); + mask[face.m_index[index]] = 0; + } + } + if (!success) { + dgInt32 count1 = 0; + dgInt32 vertexCount = convexHull->GetVertexCount(); + for (dgInt32 i = 0; i < vertexCount; i ++) { + if (mask[i]) { + buffer[count1 * 3 + 0] = hullVertexArray[i].m_x; + buffer[count1 * 3 + 1] = hullVertexArray[i].m_y; + buffer[count1 * 3 + 2] = hullVertexArray[i].m_z; + count1 ++; + } + } + delete convexHull; + convexHull = new (GetAllocator()) dgConvexHull3d (GetAllocator(), &buffer[0], 3 * sizeof (dgFloat64), count1, tolerance); + } + } + + dgAssert (convexHull); + dgInt32 vertexCount = convexHull->GetVertexCount(); + if (vertexCount < 4) { + delete convexHull; + return false; + } + + const dgBigVector* const hullVertexArray = convexHull->GetVertexPool(); + dgPolyhedra polyhedra (GetAllocator()); + polyhedra.BeginFace(); + for (dgConvexHull3d::dgListNode* node = convexHull->GetFirst(); node; node = node->GetNext()) { + dgConvexHull3DFace& face = node->GetInfo(); + polyhedra.AddFace (face.m_index[0], face.m_index[1], face.m_index[2]); + } + polyhedra.EndFace(); + + if (vertexCount > 4) { + while (RemoveCoplanarEdge (polyhedra, hullVertexArray)); + } + + dgStack vertexMap(vertexCount); + memset (&vertexMap[0], -1, vertexCount * sizeof (dgInt32)); + + dgInt32 mark = polyhedra.IncLRU(); + dgPolyhedra::Iterator iter (polyhedra); + for (iter.Begin(); iter; iter ++) { + dgEdge* const edge = &iter.GetNode()->GetInfo(); + if (edge->m_mark != mark) { + if (vertexMap[edge->m_incidentVertex] == -1) { + vertexMap[edge->m_incidentVertex] = m_vertexCount; + m_vertexCount ++; + } + dgEdge* ptr = edge; + do { + ptr->m_mark = mark; + ptr->m_userData = m_edgeCount; + m_edgeCount ++; + ptr = ptr->m_twin->m_next; + } while (ptr != edge) ; + } + } + + m_vertex = (dgVector*) m_allocator->Malloc (dgInt32 (m_vertexCount * sizeof (dgVector))); + m_simplex = (dgConvexSimplexEdge*) m_allocator->Malloc (dgInt32 (m_edgeCount * sizeof (dgConvexSimplexEdge))); + m_vertexToEdgeMapping = (const dgConvexSimplexEdge**) m_allocator->Malloc (dgInt32 (m_vertexCount * sizeof (dgConvexSimplexEdge*))); + + for (dgInt32 i = 0; i < vertexCount; i ++) { + if (vertexMap[i] != -1) { + m_vertex[vertexMap[i]] = hullVertexArray[i]; + m_vertex[vertexMap[i]].m_w = dgFloat32 (0.0f); + } + } + delete convexHull; + + vertexCount = m_vertexCount; + mark = polyhedra.IncLRU();; + for (iter.Begin(); iter; iter ++) { + dgEdge* const edge = &iter.GetNode()->GetInfo(); + if (edge->m_mark != mark) { + dgEdge *ptr = edge; + do { + ptr->m_mark = mark; + dgConvexSimplexEdge* const simplexPtr = &m_simplex[ptr->m_userData]; + simplexPtr->m_vertex = vertexMap[ptr->m_incidentVertex]; + simplexPtr->m_next = &m_simplex[ptr->m_next->m_userData]; + simplexPtr->m_prev = &m_simplex[ptr->m_prev->m_userData]; + simplexPtr->m_twin = &m_simplex[ptr->m_twin->m_userData]; + + ptr = ptr->m_twin->m_next; + } while (ptr != edge) ; + } + } + + + m_faceCount = 0; + dgStack faceMarks (m_edgeCount); + memset (&faceMarks[0], 0, m_edgeCount * sizeof (dgInt8)); + + dgStack faceArray (m_edgeCount); + for (dgInt32 i = 0; i < m_edgeCount; i ++) { + dgConvexSimplexEdge* const face = &m_simplex[i]; + if (!faceMarks[i]) { + dgConvexSimplexEdge* ptr = face; + do { + dgAssert ((ptr - m_simplex) >= 0); + faceMarks[dgInt32 (ptr - m_simplex)] = '1'; + ptr = ptr->m_next; + } while (ptr != face); + + faceArray[m_faceCount] = face; + m_faceCount ++; + } + } + m_faceArray = (dgConvexSimplexEdge **) m_allocator->Malloc(dgInt32 (m_faceCount * sizeof(dgConvexSimplexEdge *))); + memcpy (m_faceArray, &faceArray[0], m_faceCount * sizeof(dgConvexSimplexEdge *)); + + if (vertexCount > DG_CONVEX_VERTEX_SPLITE_SIZE) { + // create a face structure for support vertex + dgStack boxTree (vertexCount); + dgTree sortTree(GetAllocator()); + dgStack::dgTreeNode*> vertexNodeList(vertexCount); + + dgVector boxP0 ( dgFloat32 (1.0e15f)); + dgVector boxP1 (-dgFloat32 (1.0e15f)); + for (dgInt32 i = 0; i < vertexCount; i ++) { + const dgVector& p = m_vertex[i]; + vertexNodeList[i] = sortTree.Insert (p, i); + boxP0 = boxP0.GetMin(p); + boxP1 = boxP1.GetMax(p); + } + + boxTree[0].m_box[0] = boxP0 & dgVector::m_triplexMask; + boxTree[0].m_box[1] = boxP1 & dgVector::m_triplexMask; + boxTree[0].m_leftBox = -1; + boxTree[0].m_rightBox = -1; + boxTree[0].m_vertexStart = 0; + boxTree[0].m_vertexCount = vertexCount; + dgInt32 boxCount = 1; + + dgInt32 stack = 1; + dgInt32 stackBoxPool[64]; + stackBoxPool[0] = 0; + + while (stack) { + stack --; + dgInt32 boxIndex = stackBoxPool[stack]; + dgConvexBox& box = boxTree[boxIndex]; + if (box.m_vertexCount > DG_CONVEX_VERTEX_BOX_CELL_SIZE) { + dgVector median (dgFloat32 (0.0f)); + dgVector varian (dgFloat32 (0.0f)); + for (dgInt32 i = 0; i < box.m_vertexCount; i ++) { + dgVector& p = vertexNodeList[box.m_vertexStart + i]->GetInfo(); + boxP0 = boxP0.GetMin(p); + boxP1 = boxP1.GetMax(p); + median += p; + varian += p * p; + } + + varian = varian.Scale (dgFloat32 (box.m_vertexCount)) - median * median; + dgInt32 index = 0; + dgFloat64 maxVarian = dgFloat64 (-1.0e10f); + for (dgInt32 i = 0; i < 3; i ++) { + if (varian[i] > maxVarian) { + index = i; + maxVarian = varian[i]; + } + } + dgVector center = median.Scale (dgFloat32 (1.0f) / dgFloat32 (box.m_vertexCount)); + dgFloat32 test = center[index]; + + dgInt32 i0 = 0; + dgInt32 i1 = box.m_vertexCount - 1; + do { + for (; i0 <= i1; i0 ++) { + dgFloat32 val = vertexNodeList[box.m_vertexStart + i0]->GetInfo()[index]; + if (val > test) { + break; + } + } + + for (; i1 >= i0; i1 --) { + dgFloat32 val = vertexNodeList[box.m_vertexStart + i1]->GetInfo()[index]; + if (val < test) { + break; + } + } + + if (i0 < i1) { + dgSwap(vertexNodeList[box.m_vertexStart + i0], vertexNodeList[box.m_vertexStart + i1]); + i0++; + i1--; + } + } while (i0 <= i1); + + if (i0 == 0){ + i0 = box.m_vertexCount / 2; + } + if (i0 >= (box.m_vertexCount - 1)){ + i0 = box.m_vertexCount / 2; + } + + { + // insert right branch AABB + dgVector rightBoxP0 ( dgFloat32 (1.0e15f)); + dgVector rightBoxP1 (-dgFloat32 (1.0e15f)); + for (dgInt32 i = i0; i < box.m_vertexCount; i ++) { + const dgVector& p = vertexNodeList[box.m_vertexStart + i]->GetInfo(); + rightBoxP0 = rightBoxP0.GetMin(p); + rightBoxP1 = rightBoxP1.GetMax(p); + } + + box.m_rightBox = boxCount; + boxTree[boxCount].m_box[0] = rightBoxP0 & dgVector::m_triplexMask; + boxTree[boxCount].m_box[1] = rightBoxP1 & dgVector::m_triplexMask; + boxTree[boxCount].m_leftBox = -1; + boxTree[boxCount].m_rightBox = -1; + boxTree[boxCount].m_vertexStart = box.m_vertexStart + i0; + boxTree[boxCount].m_vertexCount = box.m_vertexCount - i0; + stackBoxPool[stack] = boxCount; + stack ++; + boxCount ++; + } + + { + // insert left branch AABB + dgVector leftBoxP0 ( dgFloat32 (1.0e15f)); + dgVector leftBoxP1 (-dgFloat32 (1.0e15f)); + for (dgInt32 i = 0; i < i0; i ++) { + const dgVector& p = vertexNodeList[box.m_vertexStart + i]->GetInfo(); + leftBoxP0 = leftBoxP0.GetMin(p); + leftBoxP1 = leftBoxP1.GetMax(p); + } + + box.m_leftBox = boxCount; + boxTree[boxCount].m_box[0] = leftBoxP0 & dgVector::m_triplexMask;; + boxTree[boxCount].m_box[1] = leftBoxP1 & dgVector::m_triplexMask;; + boxTree[boxCount].m_leftBox = -1; + boxTree[boxCount].m_rightBox = -1; + boxTree[boxCount].m_vertexStart = box.m_vertexStart; + boxTree[boxCount].m_vertexCount = i0; + stackBoxPool[stack] = boxCount; + stack ++; + boxCount ++; + } + } + } + + for (dgInt32 i = 0; i < m_vertexCount; i ++) { + m_vertex[i] = vertexNodeList[i]->GetInfo(); + vertexNodeList[i]->GetInfo().m_w = dgFloat32 (i); + } + + m_supportTreeCount = boxCount; + m_supportTree = (dgConvexBox*) m_allocator->Malloc(dgInt32 (boxCount * sizeof(dgConvexBox))); + memcpy (m_supportTree, &boxTree[0], boxCount * sizeof(dgConvexBox)); + + for (dgInt32 i = 0; i < m_edgeCount; i ++) { + dgConvexSimplexEdge* const ptr = &m_simplex[i]; + dgTree::dgTreeNode* const node = sortTree.Find(ptr->m_vertex); + dgInt32 index = dgInt32 (node->GetInfo().m_w); + ptr->m_vertex = dgInt16 (index); + } + } else { + CreateSOAdata(); + } + + for (dgInt32 i = 0; i < m_edgeCount; i ++) { + dgConvexSimplexEdge* const edge = &m_simplex[i]; + m_vertexToEdgeMapping[edge->m_vertex] = edge; + } + + SetVolumeAndCG (); + return true; +} + +dgInt32 dgCollisionConvexHull::CalculateSignature (dgInt32 vertexCount, const dgFloat32* const vertexArray, dgInt32 strideInBytes) +{ + dgStack buffer(1 + 3 * vertexCount); + dgInt32 stride = dgInt32 (strideInBytes / sizeof (dgFloat32)); + + memset (&buffer[0], 0, size_t (buffer.GetSizeInBytes())); + buffer[0] = m_convexHullCollision; + + for (dgInt32 i = 0; i < vertexCount; i ++) { + buffer[1 + i * 3 + 0] = dgCollision::Quantize (vertexArray[i * stride + 0]); + buffer[1 + i * 3 + 1] = dgCollision::Quantize (vertexArray[i * stride + 1]); + buffer[1 + i * 3 + 2] = dgCollision::Quantize (vertexArray[i * stride + 2]); + } + return Quantize(&buffer[0], buffer.GetSizeInBytes()); +} + +dgInt32 dgCollisionConvexHull::CalculateSignature () const +{ + return dgInt32 (GetSignature()); +// return CalculateSignature (m_vertexCount, &m_vertex[0].m_x, sizeof (dgVector)); +} + + + +void dgCollisionConvexHull::SetCollisionBBox (const dgVector& p0__, const dgVector& p1__) +{ + dgAssert (0); +} + + + +void dgCollisionConvexHull::DebugCollision (const dgMatrix& matrix, dgCollision::OnDebugCollisionMeshCallback callback, void* const userData) const +{ + dgTriplex vertex[256]; + for (dgInt32 i = 0; i < m_faceCount; i ++) { + dgConvexSimplexEdge* const face = m_faceArray[i]; + dgConvexSimplexEdge* ptr = face; + dgInt32 count = 0; + do { + vertex[count].m_x = m_vertex[ptr->m_vertex].m_x; + vertex[count].m_y = m_vertex[ptr->m_vertex].m_y; + vertex[count].m_z = m_vertex[ptr->m_vertex].m_z; + count ++; + dgAssert (count < sizeof (vertex)/ sizeof (vertex[0])); + ptr = ptr->m_next; + } while (ptr != face); + matrix.TransformTriplex (&vertex[0].m_x, sizeof (dgTriplex), &vertex[0].m_x, sizeof (dgTriplex), count); + callback (userData, count, &vertex[0].m_x, 0); + } +} + +void dgCollisionConvexHull::GetCollisionInfo(dgCollisionInfo* const info) const +{ + dgCollisionConvex::GetCollisionInfo(info); + + info->m_convexHull.m_vertexCount = m_vertexCount; + info->m_convexHull.m_strideInBytes = sizeof (dgVector); + info->m_convexHull.m_faceCount = m_faceCount; + info->m_convexHull.m_vertex = &m_vertex[0]; +} + +void dgCollisionConvexHull::CreateSOAdata() +{ + m_soaVertexArray = (dgSOAVectorArray*)m_allocator->Malloc(sizeof(dgSOAVectorArray)); + + m_soaVertexCount = ((m_vertexCount + 7) & -8) / 8; + dgVector array[DG_CONVEX_VERTEX_SPLITE_SIZE]; + for (dgInt32 i = 0; i < m_vertexCount; i++) { + array[i] = m_vertex[i]; + } + + for (dgInt32 i = m_vertexCount; i < DG_CONVEX_VERTEX_SPLITE_SIZE; i++) { + array[i] = array[0]; + } + + dgVector step(dgFloat32(4.0f)); + dgVector index(dgFloat32(0.0f), dgFloat32(1.0f), dgFloat32(2.0f), dgFloat32(3.0f)); + for (dgInt32 i = 0; i < DG_CONVEX_VERTEX_SPLITE_SIZE; i += 4) { + dgVector temp; + dgInt32 j = i / 4; + dgVector::Transpose4x4(m_soaVertexArray->m_x[j], m_soaVertexArray->m_y[j], m_soaVertexArray->m_z[j], temp, + m_vertex[i + 0], m_vertex[i + 1], m_vertex[i + 2], m_vertex[i + 3]); + m_soaVertexArray->m_index[j] = index; + index += step; + } + + dgFloat32* const indexPtr = &m_soaVertexArray->m_index[0][0]; + for (dgInt32 i = m_vertexCount; i < DG_CONVEX_VERTEX_SPLITE_SIZE; i++) { + indexPtr[i] = dgFloat32(0.0f); + } +} + +dgVector dgCollisionConvexHull::SupportVertex(const dgVector& dir, dgInt32* const vertexIndex) const +{ + dgAssert(dir.m_w == dgFloat32(0.0f)); + dgInt32 index = -1; + dgVector maxProj(dgFloat32(-1.0e20f)); + if (m_vertexCount > DG_CONVEX_VERTEX_SPLITE_SIZE) { + dgFloat32 distPool[32]; + const dgConvexBox* stackPool[32]; + + dgInt32 ix = (dir[0] > dgFloat64(0.0f)) ? 1 : 0; + dgInt32 iy = (dir[1] > dgFloat64(0.0f)) ? 1 : 0; + dgInt32 iz = (dir[2] > dgFloat64(0.0f)) ? 1 : 0; + + const dgConvexBox& leftBox = m_supportTree[m_supportTree[0].m_leftBox]; + const dgConvexBox& rightBox = m_supportTree[m_supportTree[0].m_rightBox]; + + dgVector leftP(leftBox.m_box[ix][0], leftBox.m_box[iy][1], leftBox.m_box[iz][2], dgFloat32(0.0f)); + dgVector rightP(rightBox.m_box[ix][0], rightBox.m_box[iy][1], rightBox.m_box[iz][2], dgFloat32(0.0f)); + + dgFloat32 leftDist = leftP.DotProduct(dir).GetScalar(); + dgFloat32 rightDist = rightP.DotProduct(dir).GetScalar(); + if (rightDist >= leftDist) { + distPool[0] = leftDist; + stackPool[0] = &leftBox; + + distPool[1] = rightDist; + stackPool[1] = &rightBox; + } else { + distPool[0] = rightDist; + stackPool[0] = &rightBox; + + distPool[1] = leftDist; + stackPool[1] = &leftBox; + } + + dgInt32 stack = 2; + + while (stack) { + stack--; + dgFloat32 dist = distPool[stack]; + if (dist > maxProj.m_x) { + const dgConvexBox& box = *stackPool[stack]; + + if (box.m_leftBox > 0) { + dgAssert(box.m_rightBox > 0); + const dgConvexBox& leftBox1 = m_supportTree[box.m_leftBox]; + const dgConvexBox& rightBox1 = m_supportTree[box.m_rightBox]; + + dgVector leftBoxP(leftBox1.m_box[ix][0], leftBox1.m_box[iy][1], leftBox1.m_box[iz][2], dgFloat32(0.0f)); + dgVector rightBoxP(rightBox1.m_box[ix][0], rightBox1.m_box[iy][1], rightBox1.m_box[iz][2], dgFloat32(0.0f)); + + dgFloat32 leftBoxDist = leftBoxP.DotProduct(dir).GetScalar(); + dgFloat32 rightBoxDist = rightBoxP.DotProduct(dir).GetScalar(); + if (rightBoxDist >= leftBoxDist) { + distPool[stack] = leftBoxDist; + stackPool[stack] = &leftBox1; + stack++; + dgAssert(stack < sizeof (distPool) / sizeof (distPool[0])); + + distPool[stack] = rightBoxDist; + stackPool[stack] = &rightBox1; + stack++; + dgAssert(stack < sizeof (distPool) / sizeof (distPool[0])); + + } else { + distPool[stack] = rightBoxDist; + stackPool[stack] = &rightBox1; + stack++; + dgAssert(stack < sizeof (distPool) / sizeof (distPool[0])); + + distPool[stack] = leftBoxDist; + stackPool[stack] = &leftBox1; + stack++; + dgAssert(stack < sizeof (distPool) / sizeof (distPool[0])); + } + } else { + for (dgInt32 i = 0; i < box.m_vertexCount; i++) { + const dgVector& p = m_vertex[box.m_vertexStart + i]; + dgAssert(p.m_x >= box.m_box[0].m_x); + dgAssert(p.m_x <= box.m_box[1].m_x); + dgAssert(p.m_y >= box.m_box[0].m_y); + dgAssert(p.m_y <= box.m_box[1].m_y); + dgAssert(p.m_z >= box.m_box[0].m_z); + dgAssert(p.m_z <= box.m_box[1].m_z); + dgVector projectionDist(p.DotProduct(dir)); + dgVector mask(projectionDist > maxProj); + dgInt32 intMask = *((dgInt32*)&mask.m_x); + index = ((box.m_vertexStart + i) & intMask) | (index & ~intMask); + maxProj = maxProj.GetMax(projectionDist); + } + } + } + } + } else { +#if 0 + for (dgInt32 i = 0; i < m_vertexCount; i++) { + const dgVector& p = m_vertex[i]; + dgVector dist(p.DotProduct(dir)); + dgVector mask(dist > maxProj); + dgInt32 intMask = *((dgInt32*)&mask.m_x); + index = (i & intMask) | (index & ~intMask); + maxProj = maxProj.GetMax(dist); + } +#else + const dgVector x(dir.m_x); + const dgVector y(dir.m_y); + const dgVector z(dir.m_z); + dgVector support (dgVector::m_negOne); + for (dgInt32 i = 0; i < m_soaVertexCount; i+=2) { + dgVector dot (m_soaVertexArray->m_x[i] * x + + m_soaVertexArray->m_y[i] * y + + m_soaVertexArray->m_z[i] * z); + support = support.Select (m_soaVertexArray->m_index[i], dot > maxProj); + maxProj = maxProj.GetMax(dot); + + dot = m_soaVertexArray->m_x[i + 1] * x + + m_soaVertexArray->m_y[i + 1] * y + + m_soaVertexArray->m_z[i + 1] * z; + support = support.Select(m_soaVertexArray->m_index[i + 1], dot > maxProj); + maxProj = maxProj.GetMax(dot); + } + + dgVector dot (maxProj.ShiftRight().ShiftRight()); + dgVector support1 (support.ShiftRight().ShiftRight()); + support = support.Select(support1, dot > maxProj); + maxProj = maxProj.GetMax(dot); + + dot = dgVector (maxProj.ShiftRight()); + support1 = dgVector (support.ShiftRight()); + support = support.Select(support1, dot > maxProj); + + index = dgInt32 (support.GetScalar()); +#endif + } + + if (vertexIndex) { + *vertexIndex = index; + } + dgAssert(index != -1); + return m_vertex[index]; +} + + diff --git a/thirdparty/src/newton/dgPhysics/dgCollisionConvexHull.h b/thirdparty/src/newton/dgPhysics/dgCollisionConvexHull.h new file mode 100644 index 000000000..837d33d72 --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgCollisionConvexHull.h @@ -0,0 +1,76 @@ +/* Copyright (c) <2003-2019> +* +* 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_COLLISIONCONVEXHULL_H__ +#define __DG_COLLISIONCONVEXHULL_H__ + +#include "dgCollisionConvex.h" + +class dgCollisionConvexHull: public dgCollisionConvex +{ + public: + class dgConvexBox; + class dgSOAVectorArray; + + dgCollisionConvexHull(dgMemoryAllocator* const allocator, dgUnsigned32 signature); + dgCollisionConvexHull(dgMemoryAllocator* const allocator, dgUnsigned32 signature, dgInt32 count, dgInt32 strideInBytes, dgFloat32 tolerance, const dgFloat32* const vertexArray); + dgCollisionConvexHull(dgWorld* const world, dgDeserialize deserialization, void* const userData, dgInt32 revisionNumber); + virtual ~dgCollisionConvexHull(); + + dgInt32 GetFaceIndices (dgInt32 index, dgInt32* const indices) const; + + static dgInt32 CalculateSignature (dgInt32 vertexCount, const dgFloat32* const vertexArray, dgInt32 strideInBytes); + + protected: + void BuildHull (dgInt32 count, dgInt32 strideInBytes, dgFloat32 tolerance, const dgFloat32* const vertexArray); + bool Create (dgInt32 count, dgInt32 strideInBytes, const dgFloat32* const vertexArray, dgFloat32 tolerance); + + bool RemoveCoplanarEdge (dgPolyhedra& convex, const dgBigVector* const hullVertexArray) const; + dgBigVector FaceNormal (const dgEdge *face, const dgBigVector* const pool) const; + bool CheckConvex (dgPolyhedra& polyhedra, const dgBigVector* hullVertexArray) const; + + virtual dgVector SupportVertex (const dgVector& dir, dgInt32* const vertexIndex) const; + + virtual dgInt32 CalculateSignature () const; + virtual void SetCollisionBBox (const dgVector& p0, const dgVector& p1); + virtual void DebugCollision (const dgMatrix& matrix, dgCollision::OnDebugCollisionMeshCallback callback, void* const userData) const; + virtual void GetCollisionInfo(dgCollisionInfo* const info) const; + virtual void Serialize(dgSerialize callback, void* const userData) const; + + void MassProperties (); + virtual const dgConvexSimplexEdge** GetVertexToEdgeMapping() const {return m_vertexToEdgeMapping;} + + void CreateSOAdata(); + + dgConvexBox* m_supportTree; + dgConvexSimplexEdge** m_faceArray; + dgSOAVectorArray* m_soaVertexArray; + const dgConvexSimplexEdge** m_vertexToEdgeMapping; + dgInt32 m_faceCount; + dgInt32 m_soaVertexCount; + dgInt32 m_supportTreeCount; + + friend class dgWorld; + friend class dgCollisionConvex; +}; + +#endif + diff --git a/thirdparty/src/newton/dgPhysics/dgCollisionConvexPolygon.cpp b/thirdparty/src/newton/dgPhysics/dgCollisionConvexPolygon.cpp new file mode 100644 index 000000000..7e370831b --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgCollisionConvexPolygon.cpp @@ -0,0 +1,776 @@ +/* Copyright (c) <2003-2019> +* +* 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 "dgPhysicsStdafx.h" +#include "dgBody.h" +#include "dgWorld.h" +#include "dgContact.h" +#include "dgContactSolver.h" +#include "dgCollisionInstance.h" +#include "dgCollisionConvexPolygon.h" + +#define DG_CONVEX_POLYGON_CRC 0x12341234 +#define DG_CONVEX_POLYGON_SKIRT_LENGTH dgFloat32 (0.025f) + +dgCollisionConvexPolygon::dgCollisionConvexPolygon (dgMemoryAllocator* const allocator) + :dgCollisionConvex (allocator, DG_CONVEX_POLYGON_CRC, m_polygonCollision) + ,m_count(0) + ,m_paddedCount(0) + ,m_stride(0) + ,m_faceNormalIndex(0) + ,m_faceClipSize(0) + ,m_vertex(NULL) + ,m_vertexIndex(NULL) + ,m_adjacentFaceEdgeNormalIndex(NULL) +{ + m_rtti |= dgCollisionConvexPolygon_RTTI; +} + +dgCollisionConvexPolygon::~dgCollisionConvexPolygon () +{ +} + +dgInt32 dgCollisionConvexPolygon::CalculateSignature () const +{ + return DG_CONVEX_POLYGON_CRC; +} + +void dgCollisionConvexPolygon::SetCollisionBBox (const dgVector& p0__, const dgVector& p1__) +{ + dgAssert (0); +} + +void dgCollisionConvexPolygon::Serialize(dgSerialize callback, void* const userData) const +{ + dgAssert (0); +} + + +dgFloat32 dgCollisionConvexPolygon::RayCast (const dgVector& localP0, const dgVector& localP1, dgFloat32 maxT, dgContactPoint& contactOut, const dgBody* const body, void* userData, OnRayPrecastAction preFilter) const +{ + dgAssert (0); + return dgFloat32 (1.2f); +} + + +dgFloat32 dgCollisionConvexPolygon::GetVolume () const +{ + dgAssert (0); + return dgFloat32 (0.0f); +} + +dgFloat32 dgCollisionConvexPolygon::GetBoxMinRadius () const +{ + return m_faceClipSize; +} + +dgFloat32 dgCollisionConvexPolygon::GetBoxMaxRadius () const +{ + return GetBoxMinRadius (); +} + +dgVector dgCollisionConvexPolygon::SupportVertex (const dgVector& dir, dgInt32* const vertexIndex) const +{ + dgAssert (dgAbs(dir.m_w) == dgFloat32(0.0f)); + dgAssert (dgAbs (dir.DotProduct(dir).GetScalar() - 1.0f) < dgFloat32 (1.0e-2f)); + + dgInt32 index = 0; + dgFloat32 val = m_localPoly[0].DotProduct(dir).GetScalar(); +// for (dgInt32 i = 1; i < m_count; i ++) { + for (dgInt32 i = 1; i < m_paddedCount; i++) { + dgFloat32 val1 = m_localPoly[i].DotProduct(dir).GetScalar(); + if (val1 > val) { + val = val1; + index = i; + } + } + + dgAssert (vertexIndex == NULL); + return m_localPoly[index]; +} + +bool dgCollisionConvexPolygon::BeamClipping (const dgVector& origin, dgFloat32 dist, const dgCollisionInstance* const parentMesh) +{ + dgPlane planes[4]; + dgVector points[128]; + + dgClippedFaceEdge clippedFace [2 * sizeof (m_localPoly) / sizeof (m_localPoly[0]) + 8]; + + dgVector dir (m_localPoly[1] - m_localPoly[0]); + dgAssert (dir.m_w == dgFloat32 (0.0f)); + dgAssert (dir.DotProduct(dir).GetScalar() > dgFloat32 (1.0e-8f)); + dir = dir.Normalize(); + + dgFloat32 distH = origin.DotProduct(dir).GetScalar(); + planes[0] = dgPlane (dir, dist - distH); + planes[2] = dgPlane (dir * dgVector::m_negOne, dist + distH); + + dir = m_normal.CrossProduct(dir); + dgFloat32 distV = origin.DotProduct(dir).GetScalar(); + planes[1] = dgPlane (dir, dist - distV); + planes[3] = dgPlane (dir * dgVector::m_negOne, dist + distV); + + for (dgInt32 i = 0; i < m_count; i ++) { + dgInt32 j = i << 1; + dgAssert (j < sizeof (clippedFace) / sizeof (clippedFace[0])); + + points[i] = m_localPoly[i]; + + clippedFace[j + 0].m_twin = &clippedFace[j + 1]; + clippedFace[j + 0].m_next = &clippedFace[j + 2]; + clippedFace[j + 0].m_incidentVertex = i; + clippedFace[j + 0].m_incidentNormal = m_adjacentFaceEdgeNormalIndex[i]; + + clippedFace[j + 1].m_twin = &clippedFace[j + 0]; + clippedFace[j + 1].m_next = &clippedFace[j - 2]; + clippedFace[j + 1].m_incidentVertex = i + 1; + clippedFace[j + 1].m_incidentNormal = -1; + } + + clippedFace[1].m_next = &clippedFace[m_count * 2 - 2 + 1]; + dgAssert ((m_count * 2 - 2) >= 0); + clippedFace[m_count * 2 - 2].m_next = &clippedFace[0]; + clippedFace[m_count * 2 - 2 + 1].m_incidentVertex = 0; + + const dgFloat32 tol = dgFloat32 (1.0e-5f); + dgInt32 edgeCount = m_count * 2; + dgInt32 indexCount = m_count; + dgClippedFaceEdge* first = &clippedFace[0]; + for (dgInt32 i = 0; i < 4; i ++) { + const dgPlane& plane = planes[i]; + + dgInt32 conectCount = 0; + dgClippedFaceEdge* connect[2]; + dgClippedFaceEdge* ptr = first; + dgClippedFaceEdge* newFirst = first; + dgFloat32 test0 = plane.Evalue(points[ptr->m_incidentVertex]); + do { + dgFloat32 test1 = plane.Evalue(points[ptr->m_next->m_incidentVertex]); + + if (test0 > tol) { + if (test1 <= - tol) { + const dgVector& p0 = points[ptr->m_incidentVertex]; + const dgVector& p1 = points[ptr->m_next->m_incidentVertex]; + dgVector dp (p1 - p0); + points[indexCount] = p0 - dp.Scale (test0 / dp.DotProduct(plane).GetScalar()); + + dgClippedFaceEdge* const newEdge = &clippedFace[edgeCount]; + newEdge->m_twin = newEdge + 1; + newEdge->m_twin->m_twin = newEdge; + + newEdge->m_twin->m_incidentNormal = ptr->m_incidentNormal; + newEdge->m_incidentNormal = ptr->m_incidentNormal; + + newEdge->m_incidentVertex = indexCount; + newEdge->m_twin->m_incidentVertex = ptr->m_next->m_incidentVertex; + ptr->m_twin->m_incidentVertex = indexCount; + + newEdge->m_next = ptr->m_next; + ptr->m_next->m_twin->m_next = newEdge->m_twin; + newEdge->m_twin->m_next = ptr->m_twin; + ptr->m_next = newEdge; + + connect[conectCount] = ptr; + conectCount ++; + indexCount ++; + edgeCount += 2; + ptr = newEdge; + dgAssert (indexCount < sizeof (points)/sizeof (points[0])); + } + } else { + if ((test1 > tol) && (test0 * test1) < dgFloat32 (0.0f)) { + newFirst = ptr->m_next; + + const dgVector& p0 = points[ptr->m_incidentVertex]; + const dgVector& p1 = points[ptr->m_next->m_incidentVertex]; + dgVector dp (p1 - p0); + points[indexCount] = p0 - dp.Scale (test0 / dp.DotProduct(plane).GetScalar()); + + dgClippedFaceEdge* const newEdge = &clippedFace[edgeCount]; + newEdge->m_twin = newEdge + 1; + newEdge->m_twin->m_twin = newEdge; + + newEdge->m_twin->m_incidentNormal = ptr->m_incidentNormal;; + newEdge->m_incidentNormal = ptr->m_incidentNormal; + + newEdge->m_incidentVertex = indexCount; + newEdge->m_twin->m_incidentVertex = ptr->m_next->m_incidentVertex; + ptr->m_twin->m_incidentVertex = indexCount; + + newEdge->m_next = ptr->m_next; + ptr->m_next->m_twin->m_next = newEdge->m_twin; + newEdge->m_twin->m_next = ptr->m_twin; + ptr->m_next = newEdge; + + connect[conectCount] = ptr; + conectCount ++; + indexCount ++; + edgeCount += 2; + + ptr = newEdge; + dgAssert (indexCount < sizeof (points)/sizeof (points[0])); + } + } + + test0 = test1; + ptr = ptr->m_next; + } while (ptr != first); + + if(conectCount > 1) { + first = newFirst; + dgAssert (conectCount == 2); + + dgClippedFaceEdge* const newEdge = &clippedFace[edgeCount]; + newEdge->m_twin = newEdge + 1; + newEdge->m_twin->m_twin = newEdge; + + newEdge->m_incidentNormal = m_faceNormalIndex;; + newEdge->m_incidentVertex = connect[0]->m_next->m_incidentVertex; + newEdge->m_twin->m_next = connect[0]->m_next; + connect[0]->m_next = newEdge; + + newEdge->m_twin->m_incidentNormal = m_faceNormalIndex;; + newEdge->m_twin->m_incidentVertex = connect[1]->m_next->m_incidentVertex; + newEdge->m_next = connect[1]->m_next; + connect[1]->m_next = newEdge->m_twin; + + edgeCount += 2; + } + } + + dgClippedFaceEdge* ptr = first; + do { + dgVector dist1 (points[ptr->m_next->m_incidentVertex] - points[ptr->m_incidentVertex]); + dgAssert (dist1.m_w == dgFloat32 (0.0f)); + dgFloat32 error = dist1.DotProduct(dist1).GetScalar(); + if (error < dgFloat32 (1.0e-6f)) { + ptr->m_next = ptr->m_next->m_next; + first = ptr; + } + ptr = ptr->m_next; + } while (ptr != first); + + dgInt32 count = 0; + m_adjacentFaceEdgeNormalIndex = &m_clippEdgeNormal[0]; + do { + m_clippEdgeNormal[count] = ptr->m_incidentNormal; + m_localPoly[count] = points[ptr->m_incidentVertex]; + count ++; + ptr = ptr->m_next; + dgAssert (m_count < DG_CONVEX_POLYGON_MAX_VERTEX_COUNT); + } while (ptr != first); + + m_count = count; + + if (m_count >= 3) { + dgInt32 i0 = m_count - 1; + for (dgInt32 i = 0; i < m_count; i++) { + dgVector edge(m_localPoly[i] - m_localPoly[i0]); + dgAssert(edge.DotProduct(edge).GetScalar() > dgFloat32(0.0f)); + edge = edge.Normalize(); + const dgInt32 adjacentNormalIndex = m_adjacentFaceEdgeNormalIndex[i0]; + dgVector localAdjacentNormal(&m_vertex[adjacentNormalIndex * m_stride]); + dgVector adjacentNormal(CalculateGlobalNormal(parentMesh, localAdjacentNormal & dgVector::m_triplexMask)); + dgVector edgeSkirt(edge.CrossProduct(adjacentNormal).Scale(DG_CONVEX_POLYGON_SKIRT_LENGTH)); + + m_localPoly[count + 0] = m_localPoly[i] + edgeSkirt; + m_localPoly[count + 1] = m_localPoly[i0] + edgeSkirt; + count += 2; + i0 = i; + } + + m_paddedCount = count; + } + + return (m_count >= 3); +} + +dgInt32 dgCollisionConvexPolygon::CalculatePlaneIntersection (const dgVector& normalIn, const dgVector& origin, dgVector* const contactsOut) const +{ + dgAssert (normalIn.m_w == dgFloat32 (0.0f)); + dgVector normal(normalIn); + dgInt32 count = 0; + dgFloat32 maxDist = dgFloat32 (1.0f); + dgFloat32 projectFactor = m_normal.DotProduct(normal).GetScalar(); + if (projectFactor < dgFloat32 (0.0f)) { + projectFactor *= dgFloat32 (-1.0f); + normal = normal * dgVector::m_negOne; + } + + if (projectFactor > dgFloat32 (0.9999f)) { + for (dgInt32 i = 0; i < m_count; i ++) { + contactsOut[count] = m_localPoly[i]; + count ++; + } + + #ifdef _DEBUG + dgInt32 j = count - 1; + for (dgInt32 i = 0; i < count; i ++) { + dgVector error (contactsOut[i] - contactsOut[j]); + dgAssert (error.m_w == dgFloat32 (0.0f)); + dgAssert (error.DotProduct(error).GetScalar() > dgFloat32 (1.0e-20f)); + j = i; + } + #endif + + } else if (projectFactor > dgFloat32 (0.1736f)) { + maxDist = dgFloat32 (0.0f); + dgPlane plane (normal, - normal.DotProduct(origin).GetScalar()); + + dgVector p0 (m_localPoly[m_count - 1]); + dgFloat32 side0 = plane.Evalue (p0); + for (dgInt32 i = 0; i < m_count; i ++) { + dgVector p1 (m_localPoly[i]); + dgFloat32 side1 = plane.Evalue (p1); + + if (side0 > dgFloat32 (0.0f)) { + maxDist = dgMax (maxDist, side0); + contactsOut[count] = p0 - normal.Scale (side0); + count ++; + if (count > 1) { + dgVector edgeSegment (contactsOut[count - 1] - contactsOut[count - 2]); + dgAssert (edgeSegment.m_w == dgFloat32 (0.0f)); + dgFloat32 error = edgeSegment.DotProduct(edgeSegment).GetScalar(); + if (error < dgFloat32 (1.0e-8f)) { + count --; + } + } + + if (side1 <= dgFloat32 (0.0f)) { + dgVector dp (p1 - p0); + dgFloat32 t = normal.DotProduct(dp).GetScalar(); + dgAssert (dgAbs (t) >= dgFloat32 (0.0f)); + if (dgAbs (t) < dgFloat32 (1.0e-8f)) { + t = dgSign(t) * dgFloat32 (1.0e-8f); + } + contactsOut[count] = p0 - dp.Scale (side0 / t); + count ++; + if (count > 1) { + dgVector edgeSegment (contactsOut[count - 1] - contactsOut[count - 2]); + dgAssert (edgeSegment.m_w == dgFloat32 (0.0f)); + dgFloat32 error = edgeSegment.DotProduct(edgeSegment).GetScalar(); + if (error < dgFloat32 (1.0e-8f)) { + count --; + } + } + } + } else if (side1 > dgFloat32 (0.0f)) { + dgVector dp (p1 - p0); + dgFloat32 t = normal.DotProduct(dp).GetScalar(); + dgAssert (dgAbs (t) >= dgFloat32 (0.0f)); + if (dgAbs (t) < dgFloat32 (1.0e-8f)) { + t = dgSign(t) * dgFloat32 (1.0e-8f); + } + contactsOut[count] = p0 - dp.Scale (side0 / t); + count ++; + if (count > 1) { + dgVector edgeSegment (contactsOut[count - 1] - contactsOut[count - 2]); + dgAssert (edgeSegment.m_w == dgFloat32 (0.0f)); + dgFloat32 error = edgeSegment.DotProduct(edgeSegment).GetScalar(); + if (error < dgFloat32 (1.0e-8f)) { + count --; + } + } + } + + side0 = side1; + p0 = p1; + } + } else { + maxDist = dgFloat32 (1.0e10f); + dgPlane plane (normal, - normal.DotProduct(origin).GetScalar()); + + dgVector p0 (m_localPoly[m_count - 1]); + dgFloat32 side0 = plane.Evalue (p0); + for (dgInt32 i = 0; i < m_count; i ++) { + dgVector p1 (m_localPoly[i]); + dgFloat32 side1 = plane.Evalue (p1); + + if ((side0 * side1) < dgFloat32 (0.0f)) { + dgVector dp (p1 - p0); + dgFloat32 t = normal.DotProduct(dp).GetScalar(); + dgAssert (dgAbs (t) >= dgFloat32 (0.0f)); + if (dgAbs (t) < dgFloat32 (1.0e-8f)) { + t = dgSign(t) * dgFloat32 (1.0e-8f); + } + contactsOut[count] = p0 - dp.Scale (side0 / t); + count ++; + if (count > 1) { + dgVector edgeSegment (contactsOut[count - 1] - contactsOut[count - 2]); + dgAssert (edgeSegment.m_w == dgFloat32 (0.0f)); + dgFloat32 error = edgeSegment.DotProduct(edgeSegment).GetScalar(); + if (error < dgFloat32 (1.0e-8f)) { + count --; + } + } + } + side0 = side1; + p0 = p1; + } + } + + + if (count > 1) { + if (maxDist < dgFloat32 (1.0e-3f)) { + dgVector maxPoint (contactsOut[0]); + dgVector minPoint (contactsOut[0]); + dgVector lineDir (m_normal.CrossProduct(normal)); + + dgAssert (lineDir.m_w == dgFloat32 (0.0f)); + dgFloat32 proj = contactsOut[0].DotProduct(lineDir).GetScalar(); + dgFloat32 maxProjection = proj; + dgFloat32 minProjection = proj; + for (dgInt32 i = 1; i < count; i ++) { + proj = contactsOut[i].DotProduct(lineDir).GetScalar(); + if (proj > maxProjection) { + maxProjection = proj; + maxPoint = contactsOut[i]; + } + if (proj < minProjection) { + minProjection = proj; + minPoint = contactsOut[i]; + } + } + + contactsOut[0] = maxPoint; + contactsOut[1] = minPoint; + count = 2; + } + + + dgVector error (contactsOut[count - 1] - contactsOut[0]); + dgAssert (error.m_w == dgFloat32 (0.0f)); + if (error.DotProduct(error).GetScalar() < dgFloat32 (1.0e-8f)) { + count --; + } + } + + #ifdef _DEBUG + if (count > 1) { + dgInt32 j = count - 1; + for (dgInt32 i = 0; i < count; i ++) { + dgVector error (contactsOut[i] - contactsOut[j]); + dgAssert (error.m_w == dgFloat32 (0.0f)); + dgAssert (error.DotProduct(error).GetScalar() > dgFloat32 (1.0e-20f)); + j = i; + } + + if (count >= 3) { + dgVector n (dgFloat32 (0.0f)); + dgVector e0 (contactsOut[1] - contactsOut[0]); + for (dgInt32 i = 2; i < count; i ++) { + dgVector e1 (contactsOut[i] - contactsOut[0]); + n += e0.CrossProduct(e1); + e0 = e1; + } + dgAssert (n.m_w == dgFloat32 (0.0f)); + n = n.Normalize(); + dgAssert (n.DotProduct(normal).GetScalar() > dgFloat32 (0.9f)); + } + } + #endif + return count; +} + +dgVector dgCollisionConvexPolygon::CalculateGlobalNormal (const dgCollisionInstance* const parentMesh, const dgVector& localNormal) const +{ + const dgVector& invScale = parentMesh->GetInvScale(); + const dgMatrix& globalMatrix = parentMesh->m_globalMatrix; + const dgMatrix& aligmentMatrix = parentMesh->m_aligmentMatrix; + + dgVector normal (aligmentMatrix.RotateVector(localNormal)); + normal = normal * invScale; + dgAssert(normal.m_w == dgFloat32(0.0f)); + normal = normal.Normalize(); + return globalMatrix.RotateVector(normal); +} + +dgInt32 dgCollisionConvexPolygon::CalculateContactToConvexHullContinue(const dgWorld* const world, const dgCollisionInstance* const parentMesh, dgCollisionParamProxy& proxy) +{ + dgAssert(proxy.m_instance0->IsType(dgCollision::dgCollisionConvexShape_RTTI)); + dgAssert(proxy.m_instance1->IsType(dgCollision::dgCollisionConvexPolygon_RTTI)); + + dgAssert(this == proxy.m_instance1->GetChildShape()); + dgAssert(m_count); + dgAssert(m_count < dgInt32(sizeof (m_localPoly) / sizeof (m_localPoly[0]))); + + const dgBody* const body0 = proxy.m_body0; + const dgBody* const body1 = proxy.m_body1; + + dgAssert (proxy.m_instance1->GetGlobalMatrix().TestIdentity()); + + dgVector relativeVelocity (body0->m_veloc - body1->m_veloc); + dgAssert (relativeVelocity.m_w == dgFloat32 (0.0f)); + if (relativeVelocity.DotProduct(relativeVelocity).GetScalar() < dgFloat32 (1.0e-4f)) { + return 0; + } + + dgFloat32 den = m_normal.DotProduct(relativeVelocity).GetScalar(); + if (den > dgFloat32 (-1.0e-10f)) { + return 0; + } + + dgContact* const contactJoint = proxy.m_contactJoint; + contactJoint->m_closestDistance = dgFloat32(1.0e10f); + + dgMatrix polygonMatrix; + dgVector right (m_localPoly[1] - m_localPoly[0]); + polygonMatrix[0] = right.Normalize(); + polygonMatrix[1] = m_normal; + polygonMatrix[2] = polygonMatrix[0].CrossProduct(m_normal); + polygonMatrix[3] = m_localPoly[0]; + polygonMatrix[3].m_w = dgFloat32 (1.0f); + dgAssert (polygonMatrix.TestOrthogonal()); + + dgVector polyBoxP0(dgFloat32(1.0e15f)); + dgVector polyBoxP1(dgFloat32(-1.0e15f)); + for (dgInt32 i = 0; i < m_count; i++) { + dgVector point (polygonMatrix.UntransformVector(m_localPoly[i])); + polyBoxP0 = polyBoxP0.GetMin(point); + polyBoxP1 = polyBoxP1.GetMax(point); + } + + dgVector hullBoxP0; + dgVector hullBoxP1; + dgMatrix hullMatrix (proxy.m_instance0->m_globalMatrix * polygonMatrix.Inverse()); + proxy.m_instance0->CalcAABB(hullMatrix, hullBoxP0, hullBoxP1); + dgVector minBox(polyBoxP0 - hullBoxP1); + dgVector maxBox(polyBoxP1 - hullBoxP0); + + dgVector relStep (relativeVelocity.Scale(dgMax (proxy.m_timestep, dgFloat32 (1.0e-12f)))); + dgFastRayTest ray(dgVector(dgFloat32(0.0f)), polygonMatrix.UnrotateVector(relStep)); + dgFloat32 distance = ray.BoxIntersect(minBox, maxBox); + + dgInt32 count = 0; + if (distance < dgFloat32(1.0f)) { + bool inside = false; + + dgVector sphOrigin(polygonMatrix.TransformVector((hullBoxP1 + hullBoxP0) * dgVector::m_half)); + dgVector pointInPlane (sphOrigin - relStep.Scale (m_normal.DotProduct(sphOrigin - m_localPoly[0]).GetScalar() / m_normal.DotProduct(relStep).GetScalar())); + + dgVector sphRadius(dgVector::m_half * (hullBoxP1 - hullBoxP0)); + dgFloat32 radius = dgSqrt(sphRadius.DotProduct(sphRadius).GetScalar()); + dgVector planeMinkStep (m_normal.Scale (radius)); + sphOrigin -= planeMinkStep; + dgVector supportPoint (sphOrigin - relStep.Scale (m_normal.DotProduct(sphOrigin - m_localPoly[0]).GetScalar() / m_normal.DotProduct(relStep).GetScalar())); + + supportPoint -= pointInPlane; + dgAssert (supportPoint.m_w == dgFloat32 (0.0f)); + radius = dgMax (dgSqrt (supportPoint.DotProduct(supportPoint).GetScalar()), radius); + +// if (!proxy.m_intersectionTestOnly) { + inside = true; + dgInt32 i0 = m_count - 1; + + for (dgInt32 i = 0; i < m_count; i++) { + dgVector e(m_localPoly[i] - m_localPoly[i0]); + dgVector n((e.CrossProduct(m_normal) & dgVector::m_triplexMask).Normalize()); + dgFloat32 dist1 = n.DotProduct(pointInPlane - m_localPoly[i0]).GetScalar(); + + if (dist1 > radius) { + return 0; + } + inside &= (dist1 <= dgFloat32 (0.0f)); + i0 = i; + } +// } + + dgFloat32 convexSphapeUmbra = dgMax (proxy.m_instance0->GetUmbraClipSize(), radius); + if (m_faceClipSize > convexSphapeUmbra) { + BeamClipping(pointInPlane, convexSphapeUmbra, parentMesh); + m_faceClipSize = proxy.m_instance0->m_childShape->GetBoxMaxRadius(); + } + + const dgInt64 hullId = proxy.m_instance0->GetUserDataID(); + if (inside & !proxy.m_intersectionTestOnly) { + const dgMatrix& matrixInstance0 = proxy.m_instance0->m_globalMatrix; + dgVector normalInHull(matrixInstance0.UnrotateVector(m_normal.Scale(dgFloat32(-1.0f)))); + dgVector pointInHull(proxy.m_instance0->SupportVertex(normalInHull)); + dgVector p0 (matrixInstance0.TransformVector(pointInHull)); + + dgFloat32 timetoImpact = dgFloat32(0.0f); + dgAssert (m_normal.m_w == dgFloat32 (0.0f)); + dgFloat32 penetration = m_normal.DotProduct(m_localPoly[0] - p0).GetScalar() + proxy.m_skinThickness; + if (penetration < dgFloat32(0.0f)) { + timetoImpact = penetration / relativeVelocity.DotProduct(m_normal).GetScalar(); + dgAssert(timetoImpact >= dgFloat32(0.0f)); + } + + if (timetoImpact <= proxy.m_timestep) { + dgVector contactPoints[64]; + contactJoint->m_closestDistance = penetration; + proxy.m_timestep = timetoImpact; + proxy.m_normal = m_normal; + proxy.m_closestPointBody0 = p0; + proxy.m_closestPointBody1 = p0 + m_normal.Scale(penetration); + + if (!proxy.m_intersectionTestOnly) { + //dgAssert (0); + //pointInHull -= normalInHull.Scale (DG_ROBUST_PLANE_CLIP); + pointInHull -= normalInHull.Scale (DG_PENETRATION_TOL); + count = proxy.m_instance0->CalculatePlaneIntersection(normalInHull, pointInHull, contactPoints); + + dgVector step(relativeVelocity.Scale(timetoImpact)); + penetration = dgMax(penetration, dgFloat32(0.0f)); + dgContactPoint* const contactsOut = proxy.m_contacts; + for (dgInt32 i = 0; i < count; i++) { + contactsOut[i].m_point = matrixInstance0.TransformVector(contactPoints[i]) + step; + contactsOut[i].m_normal = m_normal; + contactsOut[i].m_shapeId0 = hullId; + contactsOut[i].m_shapeId1 = m_faceId; + contactsOut[i].m_penetration = penetration; + } + } + } + } else { + m_vertexCount = dgUnsigned16 (m_count); + count = world->CalculateConvexToConvexContacts(proxy); + //dgTrace (("dt %f\n", proxy.m_timestep)); + //proxy.m_closestPointBody0.Trace("p0"); + //proxy.m_closestPointBody1.Trace("p1"); + if (count >= 1) { + dgContactPoint* const contactsOut = proxy.m_contacts; + for (dgInt32 i = 0; i < count; i++) { + contactsOut[i].m_shapeId0 = hullId; + contactsOut[i].m_shapeId1 = m_faceId; + } + } + } + } + return count; +} + +dgInt32 dgCollisionConvexPolygon::CalculateContactToConvexHullDescrete(const dgWorld* const world, const dgCollisionInstance* const parentMesh, dgCollisionParamProxy& proxy) +{ + dgInt32 count = 0; + + dgAssert(proxy.m_instance0->IsType(dgCollision::dgCollisionConvexShape_RTTI)); + dgAssert(proxy.m_instance1->IsType(dgCollision::dgCollisionConvexPolygon_RTTI)); + dgAssert (proxy.m_instance1->GetGlobalMatrix().TestIdentity()); + + dgAssert(this == proxy.m_instance1->GetChildShape()); + dgAssert(m_count); + dgAssert(m_count < dgInt32(sizeof (m_localPoly) / sizeof (m_localPoly[0]))); + + const dgMatrix& hullMatrix = proxy.m_instance0->m_globalMatrix; + dgContact* const contactJoint = proxy.m_contactJoint; + const dgCollisionInstance* const hull = proxy.m_instance0; + + dgAssert(m_normal.m_w == dgFloat32(0.0f)); + //const dgFloat32 shapeSide = m_normal.DotProduct(hullMatrix.m_posit - m_localPoly[0]).GetScalar(); + const dgVector obbOrigin(hullMatrix.TransformVector(proxy.m_instance0->GetChildShape()->GetObbOrigin())); + const dgFloat32 shapeSide = m_normal.DotProduct(obbOrigin - m_localPoly[0]).GetScalar(); + if (shapeSide < dgFloat32(0.0f)) { + //dgTrace(("normal face away (%f %f %f)\n", m_normal[0], m_normal[1], m_normal[2])); + return 0; + } + + dgVector normalInHull(hullMatrix.UnrotateVector(m_normal)); + dgVector pointInHull(hull->SupportVertex(normalInHull.Scale(dgFloat32(-1.0f)))); + dgVector p0(hullMatrix.TransformVector(pointInHull)); + + dgFloat32 penetration = m_normal.DotProduct(m_localPoly[0] - p0).GetScalar() + proxy.m_skinThickness; + + //if (penetration < dgFloat32(-1.0e-5f)) { + if (penetration < -(DG_PENETRATION_TOL * dgFloat32 (5.0f))) { + return 0; + } + + dgVector p1(hullMatrix.TransformVector(hull->SupportVertex(normalInHull))); + contactJoint->m_closestDistance = dgFloat32(0.0f); + dgFloat32 distance = m_normal.DotProduct(m_localPoly[0] - p1).GetScalar(); + if (distance >= dgFloat32(0.0f)) { + return 0; + } + + dgVector boxSize; + dgVector boxOrigin; + hull->CalcObb(boxOrigin, boxSize); + boxOrigin += dgVector::m_wOne; + + bool inside = true; + dgInt32 i0 = m_count - 1; + for (dgInt32 i = 0; i < m_count; i++) { + dgVector e(m_localPoly[i] - m_localPoly[i0]); + dgVector edgeBoundaryNormal(m_normal.CrossProduct(e)); + dgAssert (edgeBoundaryNormal.m_w == dgFloat32 (0.0f)); + dgPlane plane(edgeBoundaryNormal, - m_localPoly[i0].DotProduct (edgeBoundaryNormal).GetScalar()); + plane = hullMatrix.UntransformPlane(plane); + + dgFloat32 supportDist = boxSize.DotProduct (plane.Abs()).GetScalar(); + dgFloat32 centerDist = plane.DotProduct (boxOrigin).GetScalar(); + + if ((centerDist + supportDist) < dgFloat32(0.0f)) { + return 0; + } + + if ((centerDist - supportDist) < dgFloat32(0.0f)) { + inside = false; + break; + } + i0 = i; + } + + dgFloat32 convexSphapeUmbra = hull->GetUmbraClipSize(); + if (m_faceClipSize > convexSphapeUmbra) { + dgVector boxP0; + dgVector boxP1; + hull->CalcAABB (hullMatrix, boxP0, boxP1); + dgVector origin (dgVector::m_half * (boxP1 + boxP1)); + + if (!BeamClipping(origin, convexSphapeUmbra, parentMesh)) { + return 0; + } + m_faceClipSize = hull->m_childShape->GetBoxMaxRadius(); + } + + const dgInt64 hullId = hull->GetUserDataID(); + if (inside & !proxy.m_intersectionTestOnly) { + penetration = dgMax(dgFloat32(0.0f), penetration); + dgAssert(penetration >= dgFloat32(0.0f)); + dgVector contactPoints[128]; + dgVector point(pointInHull + normalInHull.Scale(penetration - DG_PENETRATION_TOL)); + + count = hull->CalculatePlaneIntersection(normalInHull.Scale(dgFloat32(-1.0f)), point, contactPoints); + dgVector step(normalInHull.Scale((proxy.m_skinThickness - penetration) * dgFloat32(0.5f))); + + dgContactPoint* const contactsOut = proxy.m_contacts; + for (dgInt32 i = 0; i < count; i++) { + contactsOut[i].m_point = hullMatrix.TransformVector(contactPoints[i] + step); + contactsOut[i].m_normal = m_normal; + contactsOut[i].m_shapeId0 = hullId; + contactsOut[i].m_shapeId1 = m_faceId; + contactsOut[i].m_penetration = penetration; + } + } else { + m_vertexCount = dgUnsigned16(m_count); + count = world->CalculateConvexToConvexContacts(proxy); + dgAssert(proxy.m_intersectionTestOnly || (count >= 0)); + if (count >= 1) { + dgContactPoint* const contactsOut = proxy.m_contacts; + for (dgInt32 i = 0; i < count; i++) { + contactsOut[i].m_shapeId0 = hullId; + contactsOut[i].m_shapeId1 = m_faceId; + } + } + } + return count; +} diff --git a/thirdparty/src/newton/dgPhysics/dgCollisionConvexPolygon.h b/thirdparty/src/newton/dgPhysics/dgCollisionConvexPolygon.h new file mode 100644 index 000000000..01318b4aa --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgCollisionConvexPolygon.h @@ -0,0 +1,79 @@ +/* Copyright (c) <2003-2019> +* +* 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_COLLISION_CONVEX_POLYGON_H_ +#define _DG_COLLISION_CONVEX_POLYGON_H_ + +#include "dgCollisionConvexHull.h" + + +#define DG_CONVEX_POLYGON_MAX_VERTEX_COUNT 64 + +DG_MSC_VECTOR_ALIGNMENT +class dgCollisionConvexPolygon: public dgCollisionConvex +{ + public: + class dgClippedFaceEdge + { + public: + dgClippedFaceEdge* m_next; + dgClippedFaceEdge* m_twin; + dgInt32 m_incidentNormal; + dgInt32 m_incidentVertex; + }; + + public: + dgCollisionConvexPolygon (dgMemoryAllocator* const allocator); + ~dgCollisionConvexPolygon (); + + virtual dgInt32 CalculateSignature () const; + virtual void Serialize(dgSerialize callback, void* const userData) const; + virtual void SetCollisionBBox (const dgVector& p0, const dgVector& p1); + virtual dgFloat32 RayCast (const dgVector& localP0, const dgVector& localP1, dgFloat32 maxT, dgContactPoint& contactOut, const dgBody* const body, void* const userData, OnRayPrecastAction preFilter) const; + + virtual dgVector SupportVertex (const dgVector& dir, dgInt32* const vertexIndex) const; + + virtual dgInt32 CalculatePlaneIntersection (const dgVector& normal, const dgVector& point, dgVector* const contactsOut) const; + + virtual dgFloat32 GetVolume () const; + virtual dgFloat32 GetBoxMinRadius () const; + virtual dgFloat32 GetBoxMaxRadius () const; + + bool BeamClipping (const dgVector& origin, dgFloat32 size, const dgCollisionInstance* const parentMesh); + dgVector CalculateGlobalNormal (const dgCollisionInstance* const parentMesh, const dgVector& localNormal) const; + dgInt32 CalculateContactToConvexHullDescrete(const dgWorld* const world, const dgCollisionInstance* const parentMesh, dgCollisionParamProxy& proxy); + dgInt32 CalculateContactToConvexHullContinue (const dgWorld* const world, const dgCollisionInstance* const parentMesh, dgCollisionParamProxy& proxy); + + dgVector m_normal; + dgVector m_localPoly[DG_CONVEX_POLYGON_MAX_VERTEX_COUNT]; + dgInt32 m_clippEdgeNormal[DG_CONVEX_POLYGON_MAX_VERTEX_COUNT]; + dgInt32 m_count; + dgInt32 m_paddedCount; + dgInt32 m_faceId; + dgInt32 m_stride; + dgInt32 m_faceNormalIndex; + dgFloat32 m_faceClipSize; + const dgFloat32* m_vertex; + const dgInt32* m_vertexIndex; + const dgInt32* m_adjacentFaceEdgeNormalIndex; +} DG_GCC_VECTOR_ALIGNMENT; + +#endif \ No newline at end of file diff --git a/thirdparty/src/newton/dgPhysics/dgCollisionCylinder.cpp b/thirdparty/src/newton/dgPhysics/dgCollisionCylinder.cpp new file mode 100644 index 000000000..28eb3a1b3 --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgCollisionCylinder.cpp @@ -0,0 +1,428 @@ +/* Copyright (c) <2003-2019> +* +* 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 "dgPhysicsStdafx.h" +#include "dgBody.h" +#include "dgWorld.h" +#include "dgContact.h" +#include "dgCollisionConvexPolygon.h" +#include "dgCollisionCylinder.h" + + +////////////////////////////////////////////////////////////////////// +// Construction/Destruction +////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////// + +dgInt32 dgCollisionCylinder::m_shapeRefCount = 0; +dgCollisionConvex::dgConvexSimplexEdge dgCollisionCylinder::m_edgeArray[DG_TAPED_CYLINDER_SEGMENTS * 2 * 3]; + +dgCollisionCylinder::dgCollisionCylinder(dgMemoryAllocator* allocator, dgUnsigned32 signature, dgFloat32 radio0, dgFloat32 radio1, dgFloat32 height) + :dgCollisionConvex(allocator, signature, m_cylinderCollision) +{ + Init (radio0, radio1, height); +} + + +dgCollisionCylinder::dgCollisionCylinder(dgWorld* const world, dgDeserialize deserialization, void* const userData, dgInt32 revisionNumber) + :dgCollisionConvex (world, deserialization, userData, revisionNumber) +{ + dgVector size; + deserialization (userData, &size, sizeof (dgVector)); + Init (size.m_x, size.m_y, size.m_z); +} + +void dgCollisionCylinder::Init (dgFloat32 radio0, dgFloat32 radio1, dgFloat32 height) +{ + m_rtti |= dgCollisionCylinder_RTTI; + m_radio0 = dgMax (dgAbs (radio0), D_MIN_CONVEX_SHAPE_SIZE); + m_radio1 = dgMax (dgAbs (radio1), D_MIN_CONVEX_SHAPE_SIZE); + m_height = dgMax (dgAbs (height * dgFloat32 (0.5f)), D_MIN_CONVEX_SHAPE_SIZE); + + dgFloat32 angle = dgFloat32 (0.0f); + for (dgInt32 i = 0; i < DG_TAPED_CYLINDER_SEGMENTS; i ++) { + dgFloat32 sinAngle = dgSin (angle); + dgFloat32 cosAngle = dgCos (angle); + m_vertex[i ] = dgVector (- m_height, m_radio0 * cosAngle, m_radio0 * sinAngle, dgFloat32 (0.0f)); + m_vertex[i + DG_TAPED_CYLINDER_SEGMENTS] = dgVector ( m_height, m_radio1 * cosAngle, m_radio1 * sinAngle, dgFloat32 (0.0f)); + angle += dgPI2 / DG_TAPED_CYLINDER_SEGMENTS; + } + + m_edgeCount = DG_TAPED_CYLINDER_SEGMENTS * 6; + m_vertexCount = DG_TAPED_CYLINDER_SEGMENTS * 2; + dgCollisionConvex::m_vertex = m_vertex; + + if (!m_shapeRefCount) { + dgPolyhedra polyhedra(m_allocator); + dgInt32 wireframe[DG_TAPED_CYLINDER_SEGMENTS]; + + dgInt32 j = DG_TAPED_CYLINDER_SEGMENTS - 1; + polyhedra.BeginFace (); + for (dgInt32 i = 0; i < DG_TAPED_CYLINDER_SEGMENTS; i ++) { + wireframe[0] = j; + wireframe[1] = i; + wireframe[2] = i + DG_TAPED_CYLINDER_SEGMENTS; + wireframe[3] = j + DG_TAPED_CYLINDER_SEGMENTS; + j = i; + polyhedra.AddFace (4, wireframe); + } + + for (dgInt32 i = 0; i < DG_TAPED_CYLINDER_SEGMENTS; i ++) { + wireframe[i] = DG_TAPED_CYLINDER_SEGMENTS - 1 - i; + } + polyhedra.AddFace (DG_TAPED_CYLINDER_SEGMENTS, wireframe); + + for (dgInt32 i = 0; i < DG_TAPED_CYLINDER_SEGMENTS; i ++) { + wireframe[i] = i + DG_TAPED_CYLINDER_SEGMENTS; + } + polyhedra.AddFace (DG_TAPED_CYLINDER_SEGMENTS, wireframe); + polyhedra.EndFace (); + + dgAssert (SanityCheck (polyhedra)); + + dgUnsigned64 i = 0; + dgPolyhedra::Iterator iter (polyhedra); + for (iter.Begin(); iter; iter ++) { + dgEdge* const edge = &(*iter); + edge->m_userData = i; + i ++; + } + + for (iter.Begin(); iter; iter ++) { + dgEdge* const edge = &(*iter); + + dgConvexSimplexEdge* const ptr = &m_edgeArray[edge->m_userData]; + ptr->m_vertex = edge->m_incidentVertex; + ptr->m_next = &m_edgeArray[edge->m_next->m_userData]; + ptr->m_prev = &m_edgeArray[edge->m_prev->m_userData]; + ptr->m_twin = &m_edgeArray[edge->m_twin->m_userData]; + } + } + + m_profile[0] = dgVector( m_height, m_radio1, dgFloat32 (0.0f), dgFloat32 (0.0f)); + m_profile[1] = dgVector(-m_height, m_radio0, dgFloat32 (0.0f), dgFloat32 (0.0f)); + m_profile[2] = dgVector(-m_height, -m_radio0, dgFloat32 (0.0f), dgFloat32 (0.0f)); + m_profile[3] = dgVector(m_height, -m_radio1, dgFloat32 (0.0f), dgFloat32 (0.0f)); + + m_shapeRefCount ++; + dgCollisionConvex::m_simplex = m_edgeArray; + + SetVolumeAndCG (); +} + +dgCollisionCylinder::~dgCollisionCylinder() +{ + m_shapeRefCount --; + dgAssert (m_shapeRefCount >= 0); + + dgCollisionConvex::m_simplex = NULL; + dgCollisionConvex::m_vertex = NULL; +} + +dgInt32 dgCollisionCylinder::CalculateSignature (dgFloat32 radio0, dgFloat32 radio1, dgFloat32 height) +{ + dgUnsigned32 buffer[4]; + + buffer[0] = m_cylinderCollision; + buffer[1] = Quantize (radio0); + buffer[2] = Quantize (radio1); + buffer[3] = Quantize (height); + return Quantize(buffer, sizeof (buffer)); +} + +dgInt32 dgCollisionCylinder::CalculateSignature () const +{ + return CalculateSignature (m_radio0, m_radio1, m_height); +} + + +void dgCollisionCylinder::SetCollisionBBox (const dgVector& p0, const dgVector& p1) +{ + dgAssert (0); +} + +void dgCollisionCylinder::DebugCollision (const dgMatrix& matrix, dgCollision::OnDebugCollisionMeshCallback callback, void* const userData) const +{ + dgTriplex pool[24 * 2]; + + dgFloat32 angle = dgFloat32 (0.0f); + for (dgInt32 i = 0; i < 24; i ++) { + dgFloat32 z = dgSin (angle); + dgFloat32 y = dgCos (angle); + pool[i].m_x = - m_height; + pool[i].m_y = y * m_radio0; + pool[i].m_z = z * m_radio0; + pool[i + 24].m_x = m_height; + pool[i + 24].m_y = y * m_radio1; + pool[i + 24].m_z = z * m_radio1; + angle += dgPI2 / dgFloat32 (24.0f); + } + + matrix.TransformTriplex (&pool[0].m_x, sizeof (dgTriplex), &pool[0].m_x, sizeof (dgTriplex), 24 * 2); + + dgTriplex face[24]; + + dgInt32 j = 24 - 1; + for (dgInt32 i = 0; i < 24; i ++) { + face[0] = pool[j]; + face[1] = pool[i]; + face[2] = pool[i + 24]; + face[3] = pool[j + 24]; + j = i; + callback (userData, 4, &face[0].m_x, 0); + } + + for (dgInt32 i = 0; i < 24; i ++) { + face[i] = pool[24 - 1 - i]; + } + callback (userData, 24, &face[0].m_x, 0); + + for (dgInt32 i = 0; i < 24; i ++) { + face[i] = pool[i + 24]; + } + callback (userData, 24, &face[0].m_x, 0); +} + +void dgCollisionCylinder::MassProperties() +{ +// dgFloat32 Ixx = (1.0f / 2.0f) * m_radio0 * m_radio0; +// dgFloat32 Iyy= (1.0f / 12.0f) * (3.0f * m_radio0 * m_radio0 + 4.0f * m_height * m_height); + dgCollisionConvex::MassProperties(); + +// m_centerOfMass = dgVector::m_zero; +// m_crossInertia = dgVector::m_zero; +// dgFloat32 volume = dgFloat32(2.0f * dgPI) * m_radius * m_radius * m_height; +// dgFloat32 II = (1.0f / 2.0f) * m_radio0 * m_radio0; +// dgFloat32 JJ = (1.0f / 12.0f) * (3.0f * m_radio0 * m_radio0 + 4.0f * m_height * m_height); +// m_inertia = dgVector(II, JJ, JJ, dgFloat32(0.0f)); +// m_centerOfMass.m_w = volume; +} + +dgVector dgCollisionCylinder::SupportVertex (const dgVector& dir, dgInt32* const vertexIndex) const +{ + dgAssert (dir.m_w == dgFloat32 (0.0f)); + dgAssert (dgAbs (dir.DotProduct(dir).GetScalar() - dgFloat32 (1.0f)) < dgFloat32 (1.0e-3f)); + + if (dir.m_x < dgFloat32(-0.9999f)) { + return dgVector(-m_height, dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f)); + } else if (dir.m_x > dgFloat32(0.9999f)) { + return dgVector(m_height, dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f)); + } else { + dgVector dir_yz (dir); + dir_yz.m_x = dgFloat32 (0.0f); + dgFloat32 mag2 = dir_yz.DotProduct(dir_yz).GetScalar(); + dgAssert (mag2 > dgFloat32 (0.0f)); + dir_yz = dir_yz.Scale (dgFloat32 (1.0f) / dgSqrt (mag2)); + dgVector p0 (dir_yz.Scale (m_radio0)); + dgVector p1 (dir_yz.Scale (m_radio1)); + + p0.m_x = -m_height; + p1.m_x = m_height; + + dgFloat32 dist0 = dir.DotProduct(p0).GetScalar(); + dgFloat32 dist1 = dir.DotProduct(p1).GetScalar(); + + if (dist1 >= dist0) { + p0 = p1; + } + return p0; + } +} + + +dgVector dgCollisionCylinder::SupportVertexSpecial (const dgVector& dir, dgFloat32 skinThickness, dgInt32* const vertexIndex) const +{ + dgAssert (dir.m_w == dgFloat32 (0.0f)); + dgAssert(dgAbs(dir.DotProduct(dir).GetScalar() - dgFloat32(1.0f)) < dgFloat32(1.0e-3f)); + + const dgFloat32 thickness = DG_PENETRATION_TOL + skinThickness; + if (dir.m_x < dgFloat32 (-0.9999f)) { + return dgVector (-(m_height - thickness), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f)); + } else if (dir.m_x > dgFloat32 (0.9999f)) { + return dgVector ( m_height - thickness, dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f)); + } else { + dgVector dir_yz(dir); + dir_yz.m_x = dgFloat32(0.0f); + dgFloat32 mag2 = dir_yz.DotProduct(dir_yz).GetScalar(); + dgAssert (mag2 > dgFloat32 (0.0f)); + dir_yz = dir_yz.Scale(dgFloat32(1.0f) / dgSqrt(mag2)); + dgVector p0(dir_yz.Scale(m_radio0 - thickness)); + dgVector p1(dir_yz.Scale(m_radio1 - thickness)); + + p0.m_x = - (m_height - thickness); + p1.m_x = m_height - thickness; + + dgFloat32 dist0 = dir.DotProduct(p0).GetScalar(); + dgFloat32 dist1 = dir.DotProduct(p1).GetScalar(); + + if (dist1 >= dist0) { + p0 = p1; + } + return p0; + } +} + +dgVector dgCollisionCylinder::SupportVertexSpecialProjectPoint (const dgVector& point, const dgVector& dir) const +{ + dgAssert (dir.m_w == dgFloat32 (0.0f)); + dgAssert(dgAbs(dir.DotProduct(dir).GetScalar() - dgFloat32(1.0f)) < dgFloat32(1.0e-3f)); + return point + dir.Scale (DG_PENETRATION_TOL); +} + +dgFloat32 dgCollisionCylinder::CalculateMassProperties (const dgMatrix& offset, dgVector& inertia, dgVector& crossInertia, dgVector& centerOfMass) const +{ + return dgCollisionConvex::CalculateMassProperties (offset, inertia, crossInertia, centerOfMass); +} + +dgInt32 dgCollisionCylinder::CalculatePlaneIntersection (const dgVector& normal, const dgVector& origin, dgVector* const contactsOut) const +{ + dgInt32 count = 0; + const dgFloat32 inclination = dgFloat32(0.9998f); + if (normal.m_x < dgFloat32 (-0.995f)) { + if (normal.m_x < -inclination) { + dgMatrix matrix(normal); + matrix.m_posit.m_x = origin.m_x; + count = BuildCylinderCapPoly (m_radio0, matrix, contactsOut); + //count = RectifyConvexSlice(n, normal, contactsOut); + } else { + dgFloat32 magInv = dgRsqrt(normal.m_y * normal.m_y + normal.m_z * normal.m_z); + dgFloat32 cosAng = normal.m_y * magInv; + dgFloat32 sinAng = normal.m_z * magInv; + + dgAssert(dgAbs(normal.m_z * cosAng - normal.m_y * sinAng) < dgFloat32(1.0e-4f)); + dgVector normal1(normal.m_x, normal.m_y * cosAng + normal.m_z * sinAng, dgFloat32(0.0f), dgFloat32(0.0f)); + dgVector origin1(origin.m_x, origin.m_y * cosAng + origin.m_z * sinAng, origin.m_z * cosAng - origin.m_y * sinAng, dgFloat32(0.0f)); + + count = dgCollisionConvex::CalculatePlaneIntersection(normal1, origin1, contactsOut); + if (count > 6) { + dgInt32 dy = 2 * 6; + dgInt32 dx = 2 * count; + dgInt32 acc = dy - count; + dgInt32 index = 0; + for (dgInt32 i = 0; i < count; i++) { + if (acc > 0) { + contactsOut[index] = contactsOut[i]; + index++; + acc -= dx; + } + acc += dy; + } + count = index; + } + + for (dgInt32 i = 0; i < count; i++) { + dgFloat32 y = contactsOut[i].m_y; + dgFloat32 z = contactsOut[i].m_z; + contactsOut[i].m_y = y * cosAng - z * sinAng; + contactsOut[i].m_z = z * cosAng + y * sinAng; + } + } + } else if (normal.m_x > dgFloat32 (0.995f)) { + if (normal.m_x > inclination) { + dgMatrix matrix(normal); + matrix.m_posit.m_x = origin.m_x; + count = BuildCylinderCapPoly (m_radio1, matrix, contactsOut); + //count = RectifyConvexSlice(n, normal, contactsOut); + } else { + dgFloat32 magInv = dgRsqrt(normal.m_y * normal.m_y + normal.m_z * normal.m_z); + dgFloat32 cosAng = normal.m_y * magInv; + dgFloat32 sinAng = normal.m_z * magInv; + + dgAssert(dgAbs(normal.m_z * cosAng - normal.m_y * sinAng) < dgFloat32(1.0e-4f)); + dgVector normal1(normal.m_x, normal.m_y * cosAng + normal.m_z * sinAng, dgFloat32(0.0f), dgFloat32(0.0f)); + dgVector origin1(origin.m_x, origin.m_y * cosAng + origin.m_z * sinAng, origin.m_z * cosAng - origin.m_y * sinAng, dgFloat32(0.0f)); + + count = dgCollisionConvex::CalculatePlaneIntersection(normal1, origin1, contactsOut); + if (count > 6) { + dgInt32 dy = 2 * 6; + dgInt32 dx = 2 * count; + dgInt32 acc = dy - count; + dgInt32 index = 0; + for (dgInt32 i = 0; i < count; i++) { + if (acc > 0) { + contactsOut[index] = contactsOut[i]; + index++; + acc -= dx; + } + acc += dy; + } + count = index; + } + + for (dgInt32 i = 0; i < count; i++) { + dgFloat32 y = contactsOut[i].m_y; + dgFloat32 z = contactsOut[i].m_z; + contactsOut[i].m_y = y * cosAng - z * sinAng; + contactsOut[i].m_z = z * cosAng + y * sinAng; + } + } + } else { + dgFloat32 magInv = dgRsqrt(normal.m_y * normal.m_y + normal.m_z * normal.m_z); + dgFloat32 cosAng = normal.m_y * magInv; + dgFloat32 sinAng = normal.m_z * magInv; + + dgAssert(dgAbs(normal.m_z * cosAng - normal.m_y * sinAng) < dgFloat32(1.0e-4f)); + dgVector normal1(normal.m_x, normal.m_y * cosAng + normal.m_z * sinAng, dgFloat32(0.0f), dgFloat32(0.0f)); + dgVector origin1(origin.m_x, origin.m_y * cosAng + origin.m_z * sinAng, origin.m_z * cosAng - origin.m_y * sinAng, dgFloat32(0.0f)); + + count = 0; + int i0 = 3; + dgVector test0((m_profile[i0] - origin1).DotProduct(normal1)); + for (int i = 0; (i < 4) && (count < 2); i++) { + dgVector test1((m_profile[i] - origin1).DotProduct(normal1)); + dgVector acrossPlane(test0 * test1); + if (acrossPlane.m_x < 0.0f) { + dgVector step(m_profile[i] - m_profile[i0]); + contactsOut[count] = m_profile[i0] - step.Scale(test0.m_x / (step.DotProduct(normal1).m_x)); + count++; + } + i0 = i; + test0 = test1; + } + + for (dgInt32 i = 0; i < count; i++) { + dgFloat32 y = contactsOut[i].m_y; + dgFloat32 z = contactsOut[i].m_z; + contactsOut[i].m_y = y * cosAng - z * sinAng; + contactsOut[i].m_z = z * cosAng + y * sinAng; + } + } + return count; +} + + +void dgCollisionCylinder::GetCollisionInfo(dgCollisionInfo* const info) const +{ + dgCollisionConvex::GetCollisionInfo(info); + + info->m_cylinder.m_radio0 = m_radio0; + info->m_cylinder.m_radio1 = m_radio1; + info->m_cylinder.m_height = m_height * dgFloat32 (2.0f); +} + + +void dgCollisionCylinder::Serialize(dgSerialize callback, void* const userData) const +{ + dgVector size (m_radio0, m_radio1, m_height * dgFloat32 (2.0f), dgFloat32 (0.0f)); + SerializeLow(callback, userData); + callback (userData, &size, sizeof (dgVector)); +} + diff --git a/thirdparty/src/newton/dgPhysics/dgCollisionCylinder.h b/thirdparty/src/newton/dgPhysics/dgCollisionCylinder.h new file mode 100644 index 000000000..8d7ae92d5 --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgCollisionCylinder.h @@ -0,0 +1,74 @@ +/* Copyright (c) <2003-2019> +* +* 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_COLLISION_TAPERED_CYLINDER_H__ +#define _DG_COLLISION_TAPERED_CYLINDER_H__ + +#include "dgCollisionConvex.h" + +#define DG_TAPED_CYLINDER_SEGMENTS 12 + + +class dgCollisionCylinder: public dgCollisionConvex +{ + public: + dgCollisionCylinder(dgMemoryAllocator* const allocator, dgUnsigned32 signature, dgFloat32 radio0, dgFloat32 radio1, dgFloat32 height); + dgCollisionCylinder(dgWorld* const world, dgDeserialize deserialization, void* const userData, dgInt32 revisionNumber); + virtual ~dgCollisionCylinder(); + + + private: + void Init (dgFloat32 radio0, dgFloat32 radio1, dgFloat32 height); + + virtual dgVector SupportVertex (const dgVector& dir, dgInt32* const vertexIndex) const; + + virtual dgFloat32 CalculateMassProperties (const dgMatrix& offset, dgVector& inertia, dgVector& crossInertia, dgVector& centerOfMass) const; + virtual void DebugCollision (const dgMatrix& matrix, dgCollision::OnDebugCollisionMeshCallback callback, void* const userData) const; + + virtual dgInt32 CalculatePlaneIntersection (const dgVector& normal, const dgVector& point, dgVector* const contactsOut) const; + + virtual dgInt32 CalculateSignature () const; + virtual void SetCollisionBBox (const dgVector& p0, const dgVector& p1); + + virtual void GetCollisionInfo(dgCollisionInfo* const info) const; + virtual void Serialize(dgSerialize callback, void* const userData) const; + + static dgInt32 CalculateSignature (dgFloat32 radio0, dgFloat32 radio1, dgFloat32 height); + + virtual dgVector SupportVertexSpecial (const dgVector& dir, dgFloat32 skinThickness, dgInt32* const vertexIndex) const; + virtual dgVector SupportVertexSpecialProjectPoint (const dgVector& point, const dgVector& dir) const; + + virtual void MassProperties(); + + dgVector m_profile[4]; + dgVector m_vertex[DG_TAPED_CYLINDER_SEGMENTS * 2]; + dgFloat32 m_radio0; + dgFloat32 m_radio1; + dgFloat32 m_height; + + static dgInt32 m_shapeRefCount; + static dgConvexSimplexEdge m_edgeArray[]; + + friend class dgWorld; +}; + +#endif + diff --git a/thirdparty/src/newton/dgPhysics/dgCollisionDeformableMesh.cpp b/thirdparty/src/newton/dgPhysics/dgCollisionDeformableMesh.cpp new file mode 100644 index 000000000..559f9fa40 --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgCollisionDeformableMesh.cpp @@ -0,0 +1,182 @@ +/* Copyright (c) <2003-2019> +* +* 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 "dgPhysicsStdafx.h" + +#include "dgWorld.h" +#include "dgContact.h" +#include "dgMeshEffect.h" +#include "dgDynamicBody.h" +#include "dgCollisionDeformableMesh.h" + +////////////////////////////////////////////////////////////////////// +// Construction/Destruction +////////////////////////////////////////////////////////////////////// + +#define DG_SMALLEST_SPRING_LENGTH dgFloat32 (1.0e-3f) + +dgVector dgCollisionDeformableMesh::m_smallestLenght2 (DG_SMALLEST_SPRING_LENGTH * DG_SMALLEST_SPRING_LENGTH); + +dgCollisionDeformableMesh::dgCollisionDeformableMesh(dgWorld* const world, dgCollisionID collisionID) + :dgCollisionLumpedMassParticles(world, collisionID) + ,m_linkList(world->GetAllocator()) + ,m_linksCount(0) +{ + m_rtti |= dgCollisionDeformableMesh_RTTI; +} + +dgCollisionDeformableMesh::dgCollisionDeformableMesh(const dgCollisionDeformableMesh& source) + :dgCollisionLumpedMassParticles(source) + ,m_linkList(source.m_linkList, source.m_linksCount) + ,m_linksCount(source.m_linksCount) +{ + m_rtti = source.m_rtti; +} + +dgCollisionDeformableMesh::dgCollisionDeformableMesh(dgWorld* const world, dgDeserialize deserialization, void* const userData, dgInt32 revisionNumber) + :dgCollisionLumpedMassParticles(world, deserialization, userData, revisionNumber) + ,m_linkList(world->GetAllocator()) + ,m_linksCount(0) +{ + dgAssert(0); +} + +dgCollisionDeformableMesh::~dgCollisionDeformableMesh(void) +{ +} + +void dgCollisionDeformableMesh::FinalizeBuild() +{ + dgCollisionLumpedMassParticles::FinalizeBuild(); +} + +void dgCollisionDeformableMesh::Serialize(dgSerialize callback, void* const userData) const +{ + dgAssert(0); +} + + +dgInt32 dgCollisionDeformableMesh::GetLinksCount() const +{ + return m_linksCount; +} + +void dgCollisionDeformableMesh::DisableInactiveLinks () +{ + dgAssert (0); +/* + for (dgInt32 i = 0; i < m_linksCount; i ++) { + dgInt32 v0 = m_linkList[i].m_v0; + dgInt32 v1 = m_linkList[i].m_v1; + if ((m_unitMassScaler[v0] == dgFloat32 (0.0f)) && (m_unitMassScaler[v1] == dgFloat32 (0.0f))) { + m_linksCount --; + dgSwap(m_linkList[m_linksCount], m_linkList[i]); + i --; + } + } +*/ +} + +const dgInt16* dgCollisionDeformableMesh::GetLinks() const +{ + return &m_linkList[0].m_m0; +} + + + +void dgCollisionDeformableMesh::ConstraintParticle(dgInt32 particleIndex, const dgVector& posit, const dgBody* const body) +{ + dgAssert(0); +} + + +void dgCollisionDeformableMesh::DebugCollision(const dgMatrix& matrix, dgCollision::OnDebugCollisionMeshCallback callback, void* const userData) const +{ + const dgVector* const posit = &m_posit[0]; + const dgSpringDamperLink* const links = &m_linkList[0]; + for (dgInt32 i = 0; i < m_linksCount; i++) { + const dgInt32 j0 = links[i].m_m0; + const dgInt32 j1 = links[i].m_m1; + dgVector p0(matrix.TransformVector(posit[j0])); + dgVector p1(matrix.TransformVector(posit[j1])); + dgTriplex points[2]; + points[0].m_x = p0.m_x; + points[0].m_y = p0.m_y; + points[0].m_z = p0.m_z; + points[1].m_x = p1.m_x; + points[1].m_y = p1.m_y; + points[1].m_z = p1.m_z; + callback(userData, 2, &points[0].m_x, 0); + } +} + + +void dgCollisionDeformableMesh::IntegrateForces(dgFloat32 timestep) +{ + dgAssert(m_body->m_invMass.m_w > dgFloat32(0.0f)); + + // calculate particles accelerations + CalculateAcceleration (timestep); +#ifdef _DEBUG + const dgMatrix& matrix = m_body->GetCollision()->GetGlobalMatrix(); + dgAssert (matrix[0][0] == dgFloat32 (1.0f)); + dgAssert (matrix[1][1] == dgFloat32 (1.0f)); + dgAssert (matrix[2][2] == dgFloat32 (1.0f)); +#endif + +dgAssert (0); +// dgVector damp(dgFloat32(1.0f)); +// if (m_body->m_linearDampOn) { +// const dgFloat32 tau = dgFloat32(1.0f) / (dgFloat32(60.0f) * timestep); +// damp = dgVector(dgPow(dgFloat32(1.0f) - m_body->m_dampCoef.m_w, tau)); +// } + + // rigid body dynamic state + dgVector timeV (timestep); + dgVector invTimestep (dgFloat32 (1.0f) / timestep); + + dgVector comVeloc(dgFloat32(0.0f)); + dgVector xMassSum(dgFloat32(0.0f)); + //dgVector origin(m_body->m_localCentreOfMass + matrix.m_posit); + + dgVector* const posit = &m_posit[0]; + dgVector* const veloc = &m_veloc[0]; + const dgFloat32* const mass = &m_mass[0]; + for (dgInt32 i = 0; i < m_particlesCount; i++) { + comVeloc += veloc[i].Scale(mass[i]); + xMassSum += posit[i].Scale(mass[i]); + } + + dgVector invMass(dgFloat32(1.0f / m_totalMass)); + comVeloc = comVeloc * invMass; + m_body->m_accel = invTimestep * (comVeloc - m_body->m_veloc); + m_body->m_veloc = comVeloc; + m_body->m_alpha = dgVector::m_zero; + m_body->m_omega = dgVector::m_zero; + m_body->m_invWorldInertiaMatrix = dgGetIdentityMatrix(); + + dgVector localCom(xMassSum * invMass); + for (dgInt32 i = 0; i < m_particlesCount; i++) { + posit[i] -= localCom; + } +} + + diff --git a/thirdparty/src/newton/dgPhysics/dgCollisionDeformableMesh.h b/thirdparty/src/newton/dgPhysics/dgCollisionDeformableMesh.h new file mode 100644 index 000000000..1ef260e89 --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgCollisionDeformableMesh.h @@ -0,0 +1,72 @@ +/* Copyright (c) <2003-2019> +* +* 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 __DGCOLLISION_DEFORMABLE_MESH_H__ +#define __DGCOLLISION_DEFORMABLE_MESH_H__ + +#include "dgCollision.h" +#include "dgCollisionLumpedMassParticles.h" + + +class dgCollisionDeformableMesh: public dgCollisionLumpedMassParticles +{ + public: + dgCollisionDeformableMesh (const dgCollisionDeformableMesh& source); + dgCollisionDeformableMesh (dgWorld* const world, dgCollisionID collisionID); + dgCollisionDeformableMesh (dgWorld* const world, dgDeserialize deserialization, void* const userData, dgInt32 revisionNumber); + virtual ~dgCollisionDeformableMesh(void); + + dgInt32 GetLinksCount() const; + const dgInt16* GetLinks() const; + + virtual void ConstraintParticle(dgInt32 particleIndex, const dgVector& posit, const dgBody* const body); + + void DisableInactiveLinks (); + + protected: + class dgSpringDamperLink + { + public: + dgFloat32 m_spring; + dgFloat32 m_damper; + dgFloat32 m_restlength; + dgInt16 m_m0; + dgInt16 m_m1; + }; + + + virtual void CalculateAcceleration(dgFloat32 timestep) = 0; + + virtual void FinalizeBuild(); + virtual void Serialize(dgSerialize callback, void* const userData) const; + virtual void IntegrateForces(dgFloat32 timestep); + virtual void DebugCollision (const dgMatrix& matrix, dgCollision::OnDebugCollisionMeshCallback callback, void* const userData) const; + + dgArray m_linkList; + dgInt32 m_linksCount; + + static dgVector m_smallestLenght2; +}; + + +#endif + diff --git a/thirdparty/src/newton/dgPhysics/dgCollisionDeformableSolidMesh.cpp b/thirdparty/src/newton/dgPhysics/dgCollisionDeformableSolidMesh.cpp new file mode 100644 index 000000000..49c9a70de --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgCollisionDeformableSolidMesh.cpp @@ -0,0 +1,372 @@ +/* Copyright (c) <2003-2019> +* +* 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 "dgPhysicsStdafx.h" +#include "dgBody.h" +#include "dgWorld.h" +#include "dgContact.h" +#include "dgMeshEffect.h" +#include "dgDynamicBody.h" +#include "dgCollisionDeformableSolidMesh.h" + + +dgCollisionDeformableSolidMesh::dgCollisionDeformableSolidMesh(dgWorld* const world, dgMeshEffect* const mesh) + :dgCollisionDeformableMesh(world, m_deformableSolidMesh) + ,m_finiteElements(world->GetAllocator()) + ,m_finiteElementsCount(0) +{ + m_rtti |= dgCollisionDeformableSolidMesh_RTTI; + + dgAssert (0); +/* + dgInt32 count = mesh->GetVertexCount(); + dgVector* const points = dgAlloca (dgVector, count); + + dgAssert (mesh->HasLayers()); + for (dgInt32 i = 0; i < count; i++) { + dgBigVector p(mesh->GetVertex(i)); + points[i] = p; + m_finiteElementsCount = dgMax (mesh->GetVertexLayer(i), m_finiteElementsCount); + } + m_finiteElementsCount ++; + + m_indexToVertexCount = count; + m_indexToVertexMap.Resize(count); + dgInt32* const indexToVertexMap = &m_indexToVertexMap[0]; + m_particlesCount = dgVertexListToIndexList (&points[0].m_x, sizeof (dgVector), 3 * sizeof (dgFloat32), 0, count, indexToVertexMap, dgFloat32 (1.0e-5f)); + for (dgInt32 i = 0; i < m_particlesCount; i++) { + m_posit[i] = points[i]; + } + + dgInt32 edgeCount = 0; + dgSoftLink* const links = dgAlloca (dgSoftLink, mesh->GetCount() / 2); + for (void* edgePtr = mesh->GetFirstEdge(); edgePtr; edgePtr = mesh->GetNextEdge(edgePtr)) { + dgInt32 v0; + dgInt32 v1; + mesh->GetEdgeIndex(edgePtr, v0, v1); + v0 = indexToVertexMap[v0]; + v1 = indexToVertexMap[v1]; + links[edgeCount].m_m0 = dgInt16(dgMin (v0, v1)); + links[edgeCount].m_m1 = dgInt16(dgMax (v0, v1)); + edgeCount ++; + } + dgAssert(edgeCount == mesh->GetCount() / 2); + dgSort(links, edgeCount, CompareEdges); + + dgInt32 uniqueEdgeCount = 0; + for (dgInt32 i = 1; i < edgeCount; i ++) { + if (CompareEdges (&links[i], &links[uniqueEdgeCount], NULL) > 0) { + uniqueEdgeCount++; + links[uniqueEdgeCount] = links[i]; + } + } + uniqueEdgeCount++; + m_linksCount = uniqueEdgeCount; + m_linkList.Resize(m_linksCount); + for (dgInt32 i = 0; i < m_linksCount; i ++) { + m_linkList[i] = links[i]; + } + FinalizeBuild(); + + m_finiteElements.Resize(m_finiteElementsCount); + + dgInt32 elementIndex = 0; + dgInt32 mark = mesh->IncLRU(); + dgMeshEffect::Iterator iter(*mesh); + for (iter.Begin (); iter; iter ++) { + dgEdge* const edge = &(*iter); + dgAssert (edge->m_incidentFace > 0); + if ((edge->m_mark < mark) && (edge->m_incidentFace > 0)) { + dgEdge* edgeStack[32]; + dgFiniteElementCell& fem = m_finiteElements[elementIndex]; + dgAssert (elementIndex < m_finiteElementsCount); + elementIndex ++; + + dgInt32 tetraVertex = 0; + dgInt32 stackIndex = 1; + edgeStack[0] = edge; + while (stackIndex){ + stackIndex --; + dgEdge* const pointEdge = edgeStack[stackIndex]; + dgAssert (pointEdge->m_incidentFace > 0); + if ((pointEdge->m_mark < mark) && (pointEdge->m_incidentFace > 0)) { + fem.m_index[tetraVertex] = dgInt16 (m_indexToVertexMap[pointEdge->m_incidentVertex]); + dgAssert (tetraVertex < 4); + tetraVertex ++; + dgEdge* edgePtr = pointEdge; + do { + dgAssert (mesh->GetVertexLayer(edge->m_incidentVertex) == mesh->GetVertexLayer(edgePtr->m_incidentVertex)); + if (edgePtr->m_mark != mark) { + edgePtr->m_mark = mark; + edgeStack[stackIndex] = edgePtr->m_twin; + stackIndex ++; + } + edgePtr = edgePtr->m_twin->m_next; + } while (edgePtr != pointEdge); + } + } + } + } + dgAssert (elementIndex == m_finiteElementsCount); + + for (dgInt32 i = 0; i < m_finiteElementsCount; i ++) { + dgFiniteElementCell& fem = m_finiteElements[i]; + const dgVector& p0 = points[fem.m_index[0]]; + const dgVector& p1 = points[fem.m_index[1]]; + const dgVector& p2 = points[fem.m_index[2]]; + const dgVector& p3 = points[fem.m_index[3]]; + + dgVector p01 (p0 - p1); + dgVector p12 (p1 - p2); + dgVector p23 (p2 - p3); + dgFloat32 volume = (p01.DotProduct(p12.CrossProduct(p23))).GetScalar(); + if (volume < dgFloat32 (0.0f)) { + volume = -volume; + dgSwap(fem.m_index[0], fem.m_index[1]); + } + fem.m_restVolume = volume; + } +*/ +} + +dgCollisionDeformableSolidMesh::dgCollisionDeformableSolidMesh(const dgCollisionDeformableSolidMesh& source) + :dgCollisionDeformableMesh(source) + ,m_finiteElements(source.m_finiteElements, source.m_finiteElementsCount) + ,m_finiteElementsCount(source.m_finiteElementsCount) +{ + m_rtti = source.m_rtti; +} + +dgCollisionDeformableSolidMesh::dgCollisionDeformableSolidMesh(dgWorld* const world, dgDeserialize deserialization, void* const userData, dgInt32 revisionNumber) + :dgCollisionDeformableMesh(world, deserialization, userData, revisionNumber) + ,m_finiteElements(world->GetAllocator()) + ,m_finiteElementsCount(0) +{ + dgAssert (0); +} + + +dgCollisionDeformableSolidMesh::~dgCollisionDeformableSolidMesh(void) +{ + +} + +dgInt32 dgCollisionDeformableSolidMesh::GetMemoryBufferSizeInBytes() const +{ + dgInt32 sizeInByte = 0; + sizeInByte += 3 * m_linksCount * sizeof (dgVector); + sizeInByte += 2 * m_linksCount * sizeof (dgFloat32); + sizeInByte += 2 * m_particlesCount * sizeof (dgVector); + sizeInByte += 1 * m_particlesCount * sizeof (dgFloat32); + return sizeInByte; +} + + +void dgCollisionDeformableSolidMesh::CalculateAcceleration(dgFloat32 timestep) +{ + dgAssert (0); +/* +// Ks is in [sec^-2] a spring constant unit acceleration, not a spring force acceleration. +// Kc is in [sec^-1] a damper constant unit velocity, not a damper force acceleration. + +// for now make a share value for all springs. later this is a per material feature. +dgFloat32 kSpring = dgFloat32(1000.0f); +dgFloat32 kDamper = dgFloat32(30.0f); +//dgFloat32 kVolumetricStiffness = dgFloat32(200000.0f); +dgFloat32 kVolumetricStiffness = dgFloat32(20000000.0f); +//dgFloat32 kVolumetricStiffness = dgFloat32(1000.0f); +//kVolumetricStiffness = 0.0f; + + dgInt32 iter = 4; + dgVector* const accel = &m_accel[0]; + dgVector* const veloc = &m_veloc[0]; + dgVector* const posit = &m_posit[0]; + const dgFloat32* const restLenght = &m_restlength[0]; + const dgFiniteElementCell* const finiteElements = &m_finiteElements[0]; + +// dgVector* const dx = dgAlloca(dgVector, m_linksCount); +// dgVector* const dv = dgAlloca(dgVector, m_linksCount); +// dgVector* const dpdv = dgAlloca(dgVector, m_linksCount); +// dgVector* const normalAccel = dgAlloca(dgVector, m_particlesCount); +// dgVector* const normalDir = dgAlloca(dgVector, m_particlesCount); +// dgVector* const volumeAccel = dgAlloca(dgVector, m_particlesCount); +// dgFloat32* const spring_A01 = dgAlloca(dgFloat32, m_linksCount); +// dgFloat32* const spring_B01 = dgAlloca(dgFloat32, m_linksCount); +// dgFloat32* const frictionCoeffecient = dgAlloca(dgFloat32, m_particlesCount); + + dgWorld* const world = m_body->GetWorld(); + world->m_solverJacobiansMemory.ResizeIfNecessary (GetMemoryBufferSizeInBytes() + 1024); + dgVector* const dx = (dgVector*)&world->m_solverJacobiansMemory[0]; + dgVector* const dv = &dx[m_linksCount]; + dgVector* const dpdv = &dv[m_linksCount]; + dgVector* const normalAccel = &dpdv[m_linksCount]; + dgVector* const normalDir = &normalAccel[m_particlesCount]; + dgVector* const volumeAccel = &normalDir[m_particlesCount]; + dgFloat32* const spring_A01 = (dgFloat32*) &volumeAccel[m_particlesCount]; + dgFloat32* const spring_B01 = &spring_A01[m_linksCount]; + dgFloat32* const frictionCoeffecient = &spring_B01[m_linksCount]; + + +// dgVector* const collisionDir1 = dgAlloca(dgVector, m_particlesCount); +// dgVector* const collisionDir2 = dgAlloca(dgVector, m_particlesCount); +// dgVector* const tmp1 = dgAlloca(dgVector, m_particlesCount); +// dgVector* const tmp2 = dgAlloca(dgVector, m_particlesCount); +// dgVector* const diag = dgAlloca(dgVector, m_particlesCount); +// dgVector* const offDiag = dgAlloca(dgVector, m_particlesCount); +// dgFloat32* const damper_A01 = dgAlloca(dgFloat32, m_linksCount); +// dgFloat32* const damper_B01 = dgAlloca(dgFloat32, m_linksCount); +// dgFloat32* const damper_C01 = dgAlloca(dgFloat32, m_linksCount); + + + + + dgVector unitAccel(m_body->m_externalForce * m_body->m_invMass.m_w); + dgVector deltaOmega(m_body->m_invWorldInertiaMatrix.RotateVector (m_body->m_externalTorque.Scale (timestep))); + + m_body->m_alpha = dgVector::m_zero; + m_body->m_omega = dgVector::m_zero; + m_body->m_externalForce = dgVector::m_zero; + m_body->m_externalTorque = dgVector::m_zero; + + // here I need to add all other external acceleration like wind and pressure, friction and collision. + for (dgInt32 i = 0; i < m_particlesCount; i++) { + m_externalAccel[i] = unitAccel; + veloc[i] += deltaOmega.CrossProduct(m_posit[i]); + } + + const dgSpringDamperLink* const links = &m_linkList[0]; + dgFloat32 ks_dt = - timestep * kSpring; +// dgFloat32 kd_dt0 = -timestep * kDamper; +// dgFloat32 kd_dt1 = -timestep * kDamper * dgFloat32(2.0f); + + dgVector dtRK4 (timestep / iter); + dgVector volumetricStiffness (-kVolumetricStiffness); + dgVector epsilon (dgFloat32 (1.0e-14f)); + + dtRK4 = dtRK4 & dgVector::m_triplexMask; + HandleCollision (timestep, normalDir, normalAccel, frictionCoeffecient); + for (dgInt32 k = 0; k < iter; k ++) { + + for (dgInt32 i = 0; i < m_particlesCount; i++) { + accel[i] = m_externalAccel[i]; + volumeAccel[i] = dgVector::m_zero; + } + + for (dgInt32 i = 0; i < m_finiteElementsCount; i++) { + const dgFiniteElementCell& fem = finiteElements[i]; + const dgInt32 i0 = fem.m_index[0]; + const dgInt32 i1 = fem.m_index[1]; + const dgInt32 i2 = fem.m_index[2]; + const dgInt32 i3 = fem.m_index[3]; + const dgVector& p0 = posit[i0]; + const dgVector& p1 = posit[i1]; + const dgVector& p2 = posit[i2]; + const dgVector& p3 = posit[i3]; + + const dgVector p01(p0 - p1); + const dgVector p12(p1 - p2); + const dgVector p23(p2 - p3); + + dgVector area123(p12.CrossProduct(p23)); + dgVector area0123(p23.CrossProduct(p01)); + dgVector area012(p12.CrossProduct(p01)); + + dgFloat32 volume = (p01.DotProduct(area123)).GetScalar(); + dgVector deltaVolume(volume - fem.m_restVolume); + dgVector a0(deltaVolume * area123); + dgVector a1(deltaVolume * area0123); + dgVector a3(deltaVolume * area012); + dgAssert (a0.m_w == dgFloat32 (0.0f)); + dgAssert (a1.m_w == dgFloat32 (0.0f)); + dgAssert (a3.m_w == dgFloat32 (0.0f)); + + volumeAccel[i0] += a0; + volumeAccel[i1] += a1 - a0; + volumeAccel[i2] -= a1 + a3; + volumeAccel[i3] += a3; + } + + for (dgInt32 i = 0; i < m_linksCount; i++) { + const dgInt32 j0 = links[i].m_m0; + const dgInt32 j1 = links[i].m_m1; + dv[i] = veloc[j0] - veloc[j1]; + dx[i] = posit[j0] - posit[j1]; + + const dgVector p0p1 (dx[i]); + const dgVector v0v1 (dv[i]); + const dgVector length2(p0p1.DotProduct(p0p1)); + const dgVector mask(length2 > m_smallestLenght2); + + const dgVector lenght2 ((length2 & mask) | length2.And__Not(mask)); + const dgFloat32 length = (lenght2.Sqrt()).GetScalar(); + const dgFloat32 den = dgFloat32 (1.0f) / length; + const dgFloat32 lenghtRatio = restLenght[i] * den; + const dgFloat32 compression = dgFloat32(1.0f) - lenghtRatio; + const dgVector fs(p0p1.Scale(kSpring * compression)); + const dgVector fd(p0p1.Scale(kDamper * den * den * (v0v1.DotProduct(p0p1)).GetScalar())); + + dgAssert(fs.m_w == dgFloat32(0.0f)); + dgAssert(fs.m_w == dgFloat32(0.0f)); + dgAssert(p0p1.m_w == dgFloat32(0.0f)); + + dpdv[i] = p0p1 * v0v1; + accel[j0] -= (fs + fd); + accel[j1] += (fs + fd); + + spring_A01[i] = ks_dt * compression; + spring_B01[i] = ks_dt * lenghtRatio * den * den; + } + + for (dgInt32 i = 0; i < m_linksCount; i++) { + const dgVector dv0 (dv[i]); + const dgVector A01(spring_A01[i]); + const dgVector B01(spring_B01[i]); + const dgVector dfdx (A01 * dv0 + B01 * dx[i] * dpdv[i]); + + const dgInt32 j0 = links[i].m_m0; + const dgInt32 j1 = links[i].m_m1; + + dgAssert (dfdx.m_w == dgFloat32 (0.0f)); + accel[j0] += dfdx; + accel[j1] -= dfdx; + } + + for (dgInt32 i = 0; i < m_particlesCount; i++) { + accel[i] += volumetricStiffness * volumeAccel[i]; + dgVector normalDirAccel (normalDir[i] * accel[i].DotProduct(normalDir[i])); + + //dgVector dirAccel1 (collisionDir1[i] * accel[i].DotProduct(collisionDir1[i])); + //dgVector dirAccel2 (collisionDir2[i] * accel[i].DotProduct(collisionDir2[i])); + //tmp0[i] = accel[i] + collidingAccel[i] - dirAccel0 - dirAccel1 - dirAccel2; + + dgVector tangentDir (veloc[i] - normalDir[i] * normalDir[i].DotProduct(veloc[i])); + dgVector mag (tangentDir.DotProduct(tangentDir) + epsilon); + + dgFloat32 tangentFrictionAccel = dgAbs (accel[i].DotProduct(normalDir[i]).GetScalar()); + dgVector friction (tangentDir.Scale (frictionCoeffecient[i] * tangentFrictionAccel / dgSqrt (mag.GetScalar()))); + + accel[i] += (normalAccel[i] - normalDirAccel - friction); + veloc[i] += accel[i] * dtRK4; + posit[i] += veloc[i] * dtRK4; + } + } +*/ +} diff --git a/thirdparty/src/newton/dgPhysics/dgCollisionDeformableSolidMesh.h b/thirdparty/src/newton/dgPhysics/dgCollisionDeformableSolidMesh.h new file mode 100644 index 000000000..3d7486e05 --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgCollisionDeformableSolidMesh.h @@ -0,0 +1,59 @@ +/* Copyright (c) <2003-2019> +* +* 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 __DGCOLLISION_DEFORMABLE_SOLID_MESH_H__ +#define __DGCOLLISION_DEFORMABLE_SOLID_MESH_H__ + + +#include "dgCollision.h" +#include "dgCollisionConvex.h" +#include "dgCollisionDeformableMesh.h" + + + +class dgCollisionDeformableSolidMesh: public dgCollisionDeformableMesh +{ + public: + class dgFiniteElementCell + { + public: + dgFloat32 m_restVolume; + dgInt16 m_index[4]; + }; + + dgCollisionDeformableSolidMesh (const dgCollisionDeformableSolidMesh& source); + dgCollisionDeformableSolidMesh (dgWorld* const world, dgMeshEffect* const mesh); + dgCollisionDeformableSolidMesh (dgWorld* const world, dgDeserialize deserialization, void* const userData, dgInt32 revisionNumber); + virtual ~dgCollisionDeformableSolidMesh(void); + + virtual void CalculateAcceleration(dgFloat32 timestep); + + dgInt32 GetMemoryBufferSizeInBytes() const; + + dgArray m_finiteElements; + dgInt32 m_finiteElementsCount; +}; + + + +#endif + diff --git a/thirdparty/src/newton/dgPhysics/dgCollisionHeightField.cpp b/thirdparty/src/newton/dgPhysics/dgCollisionHeightField.cpp new file mode 100644 index 000000000..806bc1972 --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgCollisionHeightField.cpp @@ -0,0 +1,1220 @@ +/* Copyright (c) <2003-2019> +* +* 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 "dgPhysicsStdafx.h" +#include "dgBody.h" +#include "dgWorld.h" +#include "dgCollisionHeightField.h" + + +#define DG_HIGHTFIELD_DATA_ID 0x45AF5E07 + +dgVector dgCollisionHeightField::m_yMask (0xffffffff, 0, 0xffffffff, 0); +dgVector dgCollisionHeightField::m_padding (dgFloat32 (0.25f), dgFloat32 (0.25f), dgFloat32 (0.25f), dgFloat32 (0.0f)); +dgVector dgCollisionHeightField::m_elevationPadding (dgFloat32 (0.0f), dgFloat32 (1.0e10f), dgFloat32 (0.0f), dgFloat32 (0.0f)); + +dgInt32 dgCollisionHeightField::m_cellIndices[][4] = +{ + {0, 1, 2, 3}, + {1, 3, 0, 2} +}; + +dgInt32 dgCollisionHeightField::m_verticalEdgeMap[][7] = +{ + {1 * 9 + 1, 0 * 9 + 0, 1 * 9 + 4, 1 * 9 + 6, 0 * 9 + 6, 1 * 9 + 4, 0 * 9 + 4}, + {1 * 9 + 2, 0 * 9 + 1, 1 * 9 + 4, 1 * 9 + 6, 0 * 9 + 7, 1 * 9 + 4, 0 * 9 + 4}, + {1 * 9 + 2, 0 * 9 + 0, 1 * 9 + 4, 1 * 9 + 7, 0 * 9 + 6, 1 * 9 + 4, 0 * 9 + 4}, + {1 * 9 + 0, 0 * 9 + 1, 1 * 9 + 4, 1 * 9 + 7, 0 * 9 + 7, 1 * 9 + 4, 0 * 9 + 4} +}; + +dgInt32 dgCollisionHeightField::m_horizontalEdgeMap[][7] = +{ + {1 * 9 + 0, 2 * 9 + 1, 1 * 9 + 4, 1 * 9 + 7, 2 * 9 + 7, 1 * 9 + 4, 2 * 9 + 4}, + {1 * 9 + 0, 3 * 9 + 0, 1 * 9 + 4, 1 * 9 + 7, 3 * 9 + 6, 1 * 9 + 4, 3 * 9 + 4}, + {0 * 9 + 1, 2 * 9 + 1, 0 * 9 + 4, 1 * 9 + 7, 2 * 9 + 7, 0 * 9 + 4, 2 * 9 + 4}, + {0 * 9 + 1, 3 * 9 + 0, 0 * 9 + 4, 0 * 9 + 6, 3 * 9 + 6, 0 * 9 + 4, 3 * 9 + 4} +}; + + +dgCollisionHeightField::dgCollisionHeightField( + dgWorld* const world, dgInt32 width, dgInt32 height, dgInt32 contructionMode, + const void* const elevationMap, dgElevationType elevationDataType, dgFloat32 verticalScale, + const dgInt8* const atributeMap, dgFloat32 horizontalScale_x, dgFloat32 horizontalScale_z) + :dgCollisionMesh (world, m_heightField) + ,m_width(width) + ,m_height(height) + ,m_diagonalMode (dgCollisionHeightFieldGridConstruction (dgClamp (contructionMode, dgInt32 (m_normalDiagonals), dgInt32 (m_starInvertexDiagonals)))) + ,m_verticalScale(verticalScale) + ,m_horizontalScale_x(horizontalScale_x) + ,m_horizontalScaleInv_x (dgFloat32 (1.0f) / m_horizontalScale_x) + ,m_horizontalScale_z(horizontalScale_z) + ,m_horizontalScaleInv_z(dgFloat32(1.0f) / m_horizontalScale_z) + ,m_userRayCastCallback(NULL) + ,m_elevationDataType(elevationDataType) +{ + m_rtti |= dgCollisionHeightField_RTTI; + + switch (m_elevationDataType) + { + case m_float32Bit: + { + m_elevationMap = dgMallocStack(m_width * m_height * sizeof (dgFloat32)); + memcpy (m_elevationMap, elevationMap, m_width * m_height * sizeof (dgFloat32)); + break; + } + + case m_unsigned16Bit: + { + m_elevationMap = dgMallocStack(m_width * m_height * sizeof (dgUnsigned16)); + memcpy (m_elevationMap, elevationMap, m_width * m_height * sizeof (dgUnsigned16)); + } + } + + dgInt32 attibutePaddedMapSize = (m_width * m_height + 4) & -4; + m_atributeMap = (dgInt8 *)dgMallocStack(attibutePaddedMapSize * sizeof (dgInt8)); + m_diagonals = (dgInt8 *)dgMallocStack(attibutePaddedMapSize * sizeof (dgInt8)); + + switch (m_diagonalMode) + { + case m_normalDiagonals: + { + memset (m_diagonals, 0, m_width * m_height * sizeof (dgInt8)); + break; + } + case m_invertedDiagonals: + { + memset (m_diagonals, 1, m_width * m_height * sizeof (dgInt8)); + break; + } + + case m_alternateOddRowsDiagonals: + { + for (dgInt32 j = 0; j < m_height; j += 2) { + dgInt32 index = j * m_width; + for (dgInt32 i = 0; i < m_width; i ++) { + m_diagonals[index + i] = 0; + } + } + + for (dgInt32 j = 1; j < m_height; j += 2) { + dgInt32 index = j * m_width; + for (dgInt32 i = 0; i < m_width; i ++) { + m_diagonals[index + i] = 1; + } + } + break; + } + + case m_alternateEvenRowsDiagonals: + { + for (dgInt32 j = 0; j < m_height; j += 2) { + dgInt32 index = j * m_width; + for (dgInt32 i = 0; i < m_width; i ++) { + m_diagonals[index + i] = 1; + } + } + + for (dgInt32 j = 1; j < m_height; j += 2) { + dgInt32 index = j * m_width; + for (dgInt32 i = 0; i < m_width; i ++) { + m_diagonals[index + i] = 0; + } + } + break; + } + + + case m_alternateOddColumsDiagonals: + { + for (dgInt32 j = 0; j < m_height; j ++) { + dgInt32 index = j * m_width; + for (dgInt32 i = 0; i < m_width; i += 2) { + m_diagonals[index + i] = 0; + } + + for (dgInt32 i = 1; i < m_width; i += 2) { + m_diagonals[index + i] = 1; + } + } + break; + } + + case m_alternateEvenColumsDiagonals: + { + for (dgInt32 j = 0; j < m_height; j ++) { + dgInt32 index = j * m_width; + for (dgInt32 i = 0; i < m_width; i += 2) { + m_diagonals[index + i] = 1; + } + + for (dgInt32 i = 1; i < m_width; i += 2) { + m_diagonals[index + i] = 0; + } + } + break; + } + + case m_starDiagonals: + { + for (dgInt32 j = 0; j < m_height; j += 2) { + dgInt32 index = j * m_width; + for (dgInt32 i = 0; i < m_width; i += 2) { + m_diagonals[index + i] = 0; + } + for (dgInt32 i = 1; i < m_width; i += 2) { + m_diagonals[index + i] = 1; + } + } + + for (dgInt32 j = 1; j < m_height; j += 2) { + dgInt32 index = j * m_width; + for (dgInt32 i = 0; i < m_width; i += 2) { + m_diagonals[index + i] = 1; + } + for (dgInt32 i = 1; i < m_width; i += 2) { + m_diagonals[index + i] = 0; + } + } + break; + } + + case m_starInvertexDiagonals: + { + for (dgInt32 j = 0; j < m_height; j += 2) { + dgInt32 index = j * m_width; + for (dgInt32 i = 0; i < m_width; i += 2) { + m_diagonals[index + i] = 1; + } + for (dgInt32 i = 1; i < m_width; i += 2) { + m_diagonals[index + i] = 0; + } + } + + for (dgInt32 j = 1; j < m_height; j += 2) { + dgInt32 index = j * m_width; + for (dgInt32 i = 0; i < m_width; i += 2) { + m_diagonals[index + i] = 0; + } + for (dgInt32 i = 1; i < m_width; i += 2) { + m_diagonals[index + i] = 1; + } + } + + break; + } + + default: + dgAssert (0); + + } + memcpy (m_atributeMap, atributeMap, m_width * m_height * sizeof (dgInt8)); + + dgTree::dgTreeNode* nodeData = world->m_perInstanceData.Find(DG_HIGHTFIELD_DATA_ID); + if (!nodeData) { + m_instanceData = (dgPerIntanceData*) new dgPerIntanceData(); + m_instanceData->m_refCount = 0; + m_instanceData->m_world = world; + for (dgInt32 i = 0 ; i < DG_MAX_THREADS_HIVE_COUNT; i ++) { + m_instanceData->m_vertex[i] = NULL; + m_instanceData->m_vertexCount[i] = 0; + m_instanceData->m_vertex[i].SetAllocator(world->GetAllocator()); + AllocateVertex(world, i); + } + nodeData = world->m_perInstanceData.Insert (m_instanceData, DG_HIGHTFIELD_DATA_ID); + } + m_instanceData = (dgPerIntanceData*) nodeData->GetInfo(); + + m_instanceData->m_refCount ++; + + CalculateAABB(); + SetCollisionBBox(m_minBox, m_maxBox); +} + +dgCollisionHeightField::dgCollisionHeightField (dgWorld* const world, dgDeserialize deserialization, void* const userData, dgInt32 revisionNumber) + :dgCollisionMesh (world, deserialization, userData, revisionNumber) +{ + dgAssert (m_rtti | dgCollisionHeightField_RTTI); + + dgInt32 elevationDataType; + + m_userRayCastCallback = NULL; + deserialization (userData, &m_width, sizeof (dgInt32)); + deserialization (userData, &m_height, sizeof (dgInt32)); + deserialization (userData, &m_diagonalMode, sizeof (dgInt32)); + deserialization (userData, &elevationDataType, sizeof (dgInt32)); + deserialization (userData, &m_verticalScale, sizeof (dgFloat32)); + deserialization (userData, &m_horizontalScale_x, sizeof (dgFloat32)); + deserialization (userData, &m_horizontalScale_z, sizeof (dgFloat32)); + deserialization (userData, &m_minBox.m_x, sizeof (dgVector)); + deserialization (userData, &m_maxBox.m_x, sizeof (dgVector)); + + m_elevationDataType = dgElevationType (elevationDataType); + + dgInt32 attibutePaddedMapSize = (m_width * m_height + 4) & -4; + m_atributeMap = (dgInt8 *)dgMallocStack(attibutePaddedMapSize * sizeof (dgInt8)); + m_diagonals = (dgInt8 *)dgMallocStack(attibutePaddedMapSize * sizeof (dgInt8)); + + switch (m_elevationDataType) + { + case m_float32Bit: + { + m_elevationMap = dgMallocStack(m_width * m_height * sizeof (dgFloat32)); + deserialization (userData, m_elevationMap, m_width * m_height * sizeof (dgFloat32)); + break; + } + + case m_unsigned16Bit: + { + m_elevationMap = dgMallocStack(m_width * m_height * sizeof (dgUnsigned16)); + deserialization (userData, m_elevationMap, m_width * m_height * sizeof (dgUnsigned16)); + break; + } + } + deserialization (userData, m_atributeMap, attibutePaddedMapSize * sizeof (dgInt8)); + deserialization (userData, m_diagonals, attibutePaddedMapSize * sizeof (dgInt8)); + + m_horizontalScaleInv_x = dgFloat32 (1.0f) / m_horizontalScale_x; + m_horizontalScaleInv_z = dgFloat32 (1.0f) / m_horizontalScale_z; + + dgTree::dgTreeNode* nodeData = world->m_perInstanceData.Find(DG_HIGHTFIELD_DATA_ID); + if (!nodeData) { + m_instanceData = (dgPerIntanceData*) new dgPerIntanceData(); + m_instanceData->m_refCount = 0; + m_instanceData->m_world = world; + for (dgInt32 i = 0; i < DG_MAX_THREADS_HIVE_COUNT; i++) { + m_instanceData->m_vertex[i] = NULL; + m_instanceData->m_vertexCount[i] = 0; + m_instanceData->m_vertex[i].SetAllocator(world->GetAllocator()); + AllocateVertex(world, i); + } + nodeData = world->m_perInstanceData.Insert(m_instanceData, DG_HIGHTFIELD_DATA_ID); + } + m_instanceData = (dgPerIntanceData*)nodeData->GetInfo(); + + m_instanceData->m_refCount ++; + SetCollisionBBox(m_minBox, m_maxBox); +} + +dgCollisionHeightField::~dgCollisionHeightField(void) +{ + m_instanceData->m_refCount --; + if (!m_instanceData->m_refCount) { + dgWorld* const world = m_instanceData->m_world; + delete m_instanceData; + world->m_perInstanceData.Remove(DG_HIGHTFIELD_DATA_ID); + } + dgFreeStack(m_elevationMap); + dgFreeStack(m_atributeMap); + dgFreeStack(m_diagonals); +} + +void dgCollisionHeightField::Serialize(dgSerialize callback, void* const userData) const +{ + SerializeLow(callback, userData); + + dgInt32 elevationDataType = m_elevationDataType; + + callback (userData, &m_width, sizeof (dgInt32)); + callback (userData, &m_height, sizeof (dgInt32)); + callback (userData, &m_diagonalMode, sizeof (dgInt32)); + callback (userData, &elevationDataType, sizeof (dgInt32)); + callback (userData, &m_verticalScale, sizeof (dgFloat32)); + callback (userData, &m_horizontalScale_x, sizeof (dgFloat32)); + callback (userData, &m_horizontalScale_z, sizeof (dgFloat32)); + callback (userData, &m_minBox.m_x, sizeof (dgVector)); + callback (userData, &m_maxBox.m_x, sizeof (dgVector)); + + switch (m_elevationDataType) + { + case m_float32Bit: + { + callback (userData, m_elevationMap, m_width * m_height * sizeof (dgFloat32)); + break; + } + case m_unsigned16Bit: + { + callback (userData, m_elevationMap, m_width * m_height * sizeof (dgUnsigned16)); + break; + } + } + + dgInt32 attibutePaddedMapSize = (m_width * m_height + 4) & -4; + callback (userData, m_atributeMap, attibutePaddedMapSize * sizeof (dgInt8)); + callback (userData, m_diagonals, attibutePaddedMapSize * sizeof (dgInt8)); +} + +void dgCollisionHeightField::SetCollisionRayCastCallback (dgCollisionHeightFieldRayCastCallback rayCastCallback) +{ + m_userRayCastCallback = rayCastCallback; +} + +void dgCollisionHeightField::AllocateVertex(dgWorld* const world, dgInt32 threadIndex) const +{ + m_instanceData->m_vertex[threadIndex].Resize (m_instanceData->m_vertex[threadIndex].GetElementsCapacity() * 2); + m_instanceData->m_vertexCount[threadIndex] = m_instanceData->m_vertex[threadIndex].GetElementsCapacity(); +} + +DG_INLINE void dgCollisionHeightField::CalculateMinExtend2d(const dgVector& p0, const dgVector& p1, dgVector& boxP0, dgVector& boxP1) const +{ + dgVector scale (m_horizontalScale_x, dgFloat32 (0.0f), m_horizontalScale_z, dgFloat32 (0.0f)); + dgVector q0(p0.GetMin(p1) - m_padding); + dgVector q1(p0.GetMax(p1) + scale + m_padding); + + dgVector invScale (m_horizontalScaleInv_x, dgFloat32 (0.0f), m_horizontalScaleInv_z, dgFloat32 (0.0f)); + boxP0 = (((q0 * invScale).Floor() * scale) & m_yMask) - m_elevationPadding; + boxP1 = (((q1 * invScale).Floor() * scale + scale) & m_yMask) + m_elevationPadding; + dgAssert (boxP0.m_w == dgFloat32 (0.0f)); + dgAssert (boxP1.m_w == dgFloat32 (0.0f)); + +// dgVector minBox((m_minBox & m_yMask) + boxP0.AndNot(m_yMask)); +// dgVector maxBox((m_maxBox & m_yMask) + boxP1.AndNot(m_yMask)); + dgVector minBox(boxP0.Select(m_minBox, m_yMask)); + dgVector maxBox(boxP1.Select(m_maxBox, m_yMask)); + + boxP0 = boxP0.GetMax(minBox); + boxP1 = boxP1.GetMin(maxBox); +} + +DG_INLINE void dgCollisionHeightField::CalculateMinExtend3d(const dgVector& p0, const dgVector& p1, dgVector& boxP0, dgVector& boxP1) const +{ + dgAssert(p0.m_x <= p1.m_x); + dgAssert(p0.m_y <= p1.m_y); + dgAssert(p0.m_z <= p1.m_z); + dgAssert(p0.m_w == dgFloat32(0.0f)); + dgAssert(p1.m_w == dgFloat32(0.0f)); + + dgVector scale (m_horizontalScale_x, dgFloat32 (0.0f), m_horizontalScale_z, dgFloat32 (0.0f)); + dgVector q0(p0.GetMin(p1) - m_padding); + dgVector q1(p0.GetMax(p1) + scale + m_padding); + + dgVector invScale(m_horizontalScaleInv_x, dgFloat32(0.0f), m_horizontalScaleInv_z, dgFloat32(0.0f)); + + boxP0 = q0.Select((q0 * invScale).Floor() * scale, m_yMask); + boxP1 = q1.Select((q1 * invScale).Floor() * scale + scale, m_yMask); + + dgVector minBox(boxP0.Select(m_minBox, m_yMask)); + dgVector maxBox(boxP1.Select(m_maxBox, m_yMask)); + + boxP0 = boxP0.GetMax(minBox); + boxP1 = boxP1.GetMin(maxBox); +} + +void dgCollisionHeightField::CalculateAABB() +{ + dgFloat32 y0 = dgFloat32 (dgFloat32 (1.0e10f)); + dgFloat32 y1 = dgFloat32 (-dgFloat32 (1.0e10f)); + switch (m_elevationDataType) + { + case m_float32Bit: + { + const dgFloat32* const elevation = (dgFloat32*)m_elevationMap; + for (dgInt32 i = 0; i < m_width * m_height; i ++) { + y0 = dgMin(y0, elevation[i]); + y1 = dgMax(y1, elevation[i]); + } + break; + } + + case m_unsigned16Bit: + { + const dgUnsigned16* const elevation = (dgUnsigned16*)m_elevationMap; + for (dgInt32 i = 0; i < m_width * m_height; i ++) { + y0 = dgMin(y0, dgFloat32 (elevation[i])); + y1 = dgMax(y1, dgFloat32 (elevation[i])); + } + } + } + + m_minBox = dgVector (dgFloat32 (dgFloat32 (0.0f)), y0 * m_verticalScale, dgFloat32 (dgFloat32 (0.0f)), dgFloat32 (0.0f)); + m_maxBox = dgVector (dgFloat32 (m_width - 1) * m_horizontalScale_x, y1 * m_verticalScale, dgFloat32 (m_height-1) * m_horizontalScale_z, dgFloat32 (0.0f)); +} + +void dgCollisionHeightField::GetCollisionInfo(dgCollisionInfo* const info) const +{ + dgCollision::GetCollisionInfo(info); + + dgCollisionInfo::dgHeightMapCollisionData& data = info->m_heightFieldCollision; + data.m_width = m_width; + data.m_height = m_height; + data.m_gridsDiagonals = m_diagonalMode; + data.m_elevationDataType = m_elevationDataType; + data.m_verticalScale = m_verticalScale; + data.m_horizonalScale_x = m_horizontalScale_x; + data.m_horizonalScale_z = m_horizontalScale_z; + data.m_atributes = m_atributeMap; + data.m_elevation = m_elevationMap; +} + +dgFloat32 dgCollisionHeightField::RayCastCell (const dgFastRayTest& ray, dgInt32 xIndex0, dgInt32 zIndex0, dgVector& normalOut, dgFloat32 maxT) const +{ + dgVector points[4]; + dgInt32 triangle[3]; + + // get the 3d point at the corner of the cell + if ((xIndex0 < 0) || (zIndex0 < 0) || (xIndex0 >= (m_width - 1)) || (zIndex0 >= (m_height - 1))) { + return dgFloat32 (1.2f); + } + + dgAssert (maxT <= 1.0); + + dgInt32 base = zIndex0 * m_width + xIndex0; + + switch (m_elevationDataType) + { + case m_float32Bit: + { + const dgFloat32* const elevation = (dgFloat32*)m_elevationMap; + points[0 * 2 + 0] = dgVector ((xIndex0 + 0) * m_horizontalScale_x, m_verticalScale * elevation[base], (zIndex0 + 0) * m_horizontalScale_z, dgFloat32 (0.0f)); + points[0 * 2 + 1] = dgVector ((xIndex0 + 1) * m_horizontalScale_x, m_verticalScale * elevation[base + 1], (zIndex0 + 0) * m_horizontalScale_z, dgFloat32 (0.0f)); + points[1 * 2 + 1] = dgVector ((xIndex0 + 1) * m_horizontalScale_x, m_verticalScale * elevation[base + m_width + 1], (zIndex0 + 1) * m_horizontalScale_z, dgFloat32 (0.0f)); + points[1 * 2 + 0] = dgVector ((xIndex0 + 0) * m_horizontalScale_x, m_verticalScale * elevation[base + m_width + 0], (zIndex0 + 1) * m_horizontalScale_z, dgFloat32 (0.0f)); + break; + } + + case m_unsigned16Bit: + default: + { + const dgUnsigned16* const elevation = (dgUnsigned16*)m_elevationMap; + points[0 * 2 + 0] = dgVector ((xIndex0 + 0) * m_horizontalScale_x, m_verticalScale * dgFloat32 (elevation[base]), (zIndex0 + 0) * m_horizontalScale_z, dgFloat32 (0.0f)); + points[0 * 2 + 1] = dgVector ((xIndex0 + 1) * m_horizontalScale_x, m_verticalScale * dgFloat32 (elevation[base + 1]), (zIndex0 + 0) * m_horizontalScale_z, dgFloat32 (0.0f)); + points[1 * 2 + 1] = dgVector ((xIndex0 + 1) * m_horizontalScale_x, m_verticalScale * dgFloat32 (elevation[base + m_width + 1]), (zIndex0 + 1) * m_horizontalScale_z, dgFloat32 (0.0f)); + points[1 * 2 + 0] = dgVector ((xIndex0 + 0) * m_horizontalScale_x, m_verticalScale * dgFloat32 (elevation[base + m_width + 0]), (zIndex0 + 1) * m_horizontalScale_z, dgFloat32 (0.0f)); + break; + } + } + + dgFloat32 t = dgFloat32 (1.2f); + if (!m_diagonals[base]) { + triangle[0] = 1; + triangle[1] = 2; + triangle[2] = 3; + + dgVector e10 (points[2] - points[1]); + dgVector e20 (points[3] - points[1]); + dgVector normal (e10.CrossProduct(e20)); + normal = normal.Normalize(); + t = ray.PolygonIntersect (normal, maxT, &points[0].m_x, sizeof (dgVector), triangle, 3); + if (t < maxT){ + normalOut = normal; + return t; + } + + triangle[0] = 1; + triangle[1] = 0; + triangle[2] = 2; + + dgVector e30 (points[0] - points[1]); + normal = e30.CrossProduct(e10); + normal = normal.Normalize(); + t = ray.PolygonIntersect (normal, maxT, &points[0].m_x, sizeof (dgVector), triangle, 3); + if (t < maxT){ + normalOut = normal; + return t; + } + + } else { + triangle[0] = 0; + triangle[1] = 2; + triangle[2] = 3; + + dgVector e10 (points[2] - points[0]); + dgVector e20 (points[3] - points[0]); + dgVector normal (e10.CrossProduct(e20)); + normal = normal.Normalize(); + t = ray.PolygonIntersect (normal, maxT, &points[0].m_x, sizeof (dgVector), triangle, 3); + if (t < maxT){ + normalOut = normal; + return t; + } + + triangle[0] = 0; + triangle[1] = 3; + triangle[2] = 1; + + dgVector e30 (points[1] - points[0]); + normal = e20.CrossProduct(e30); + normal = normal.Normalize(); + t = ray.PolygonIntersect (normal, maxT, &points[0].m_x, sizeof (dgVector), triangle, 3); + if (t < maxT){ + normalOut = normal; + return t; + } + } + return t; +} + +dgFloat32 dgCollisionHeightField::RayCast (const dgVector& q0, const dgVector& q1, dgFloat32 maxT, dgContactPoint& contactOut, const dgBody* const body, void* const userData, OnRayPrecastAction preFilter) const +{ + dgVector boxP0; + dgVector boxP1; + + // calculate the ray bounding box + CalculateMinExtend2d (q0, q1, boxP0, boxP1); + + dgVector p0 (q0); + dgVector p1 (q1); + + // clip the line against the bounding box + if (dgRayBoxClip (p0, p1, boxP0, boxP1)) { + dgVector dp (p1 - p0); + dgVector normalOut (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f)); + + dgFloat32 scale_x = m_horizontalScale_x; + dgFloat32 invScale_x = m_horizontalScaleInv_x; + dgFloat32 scale_z = m_horizontalScale_z; + dgFloat32 invScale_z = m_horizontalScaleInv_z; + dgInt32 ix0 = dgFastInt (p0.m_x * invScale_x); + dgInt32 iz0 = dgFastInt (p0.m_z * invScale_z); + + // implement a 3ddda line algorithm + dgInt32 xInc; + dgFloat32 tx; + dgFloat32 stepX; + if (dp.m_x > dgFloat32 (0.0f)) { + xInc = 1; + dgFloat32 val = dgFloat32 (1.0f) / dp.m_x; + stepX = scale_x * val; + tx = (scale_x * (ix0 + dgFloat32 (1.0f)) - p0.m_x) * val; + } else if (dp.m_x < dgFloat32 (0.0f)) { + xInc = -1; + dgFloat32 val = -dgFloat32 (1.0f) / dp.m_x; + stepX = scale_x * val; + tx = -(scale_x * ix0 - p0.m_x) * val; + } else { + xInc = 0; + stepX = dgFloat32 (0.0f); + tx = dgFloat32 (1.0e10f); + } + + dgInt32 zInc; + dgFloat32 tz; + dgFloat32 stepZ; + if (dp.m_z > dgFloat32 (0.0f)) { + zInc = 1; + dgFloat32 val = dgFloat32 (1.0f) / dp.m_z; + stepZ = scale_z * val; + tz = (scale_z * (iz0 + dgFloat32 (1.0f)) - p0.m_z) * val; + } else if (dp.m_z < dgFloat32 (0.0f)) { + zInc = -1; + dgFloat32 val = -dgFloat32 (1.0f) / dp.m_z; + stepZ = scale_z * val; + tz = -(scale_z * iz0 - p0.m_z) * val; + } else { + zInc = 0; + stepZ = dgFloat32 (0.0f); + tz = dgFloat32 (1.0e10f); + } + + dgFloat32 txAcc = tx; + dgFloat32 tzAcc = tz; + dgInt32 xIndex0 = ix0; + dgInt32 zIndex0 = iz0; + dgFastRayTest ray (q0, q1); + + // for each cell touched by the line + do { + dgFloat32 t = RayCastCell (ray, xIndex0, zIndex0, normalOut, maxT); + if (t < maxT) { + // bail out at the first intersection and copy the data into the descriptor + dgAssert (normalOut.m_w == dgFloat32 (0.0f)); + contactOut.m_normal = normalOut.Normalize(); + contactOut.m_shapeId0 = m_atributeMap[zIndex0 * m_width + xIndex0]; + contactOut.m_shapeId1 = m_atributeMap[zIndex0 * m_width + xIndex0]; + + if (m_userRayCastCallback) { + dgVector normal (body->GetCollision()->GetGlobalMatrix().RotateVector (contactOut.m_normal)); + m_userRayCastCallback (body, this, t, xIndex0, zIndex0, &normal, dgInt32 (contactOut.m_shapeId0), userData); + } + + return t; + } + + if (txAcc < tzAcc) { + xIndex0 += xInc; + tx = txAcc; + txAcc += stepX; + } else { + zIndex0 += zInc; + tz = tzAcc; + tzAcc += stepZ; + } + } while ((tx <= dgFloat32 (1.0f)) || (tz <= dgFloat32 (1.0f))); + } + + // if no cell was hit, return a large value + return dgFloat32 (1.2f); +} + + +void dgCollisionHeightField::GetVertexListIndexList (const dgVector& p0, const dgVector& p1, dgMeshVertexListIndexList &data) const +{ + dgAssert (0); + data.m_vertexCount = 0; +} + +struct dgCollisionHeightFieldShowPolyContext +{ + dgMatrix m_matrix; + void* m_userData; + dgCollision::OnDebugCollisionMeshCallback m_callback; +}; + +dgVector dgCollisionHeightField::SupportVertex (const dgVector& dir, dgInt32* const vertexIndex) const +{ + dgFloat32 maxProject (dgFloat32 (-1.e-20f)); + dgVector support (dgFloat32 (0.0f)); + if (m_elevationDataType == m_float32Bit) { + const dgFloat32* const elevation = (dgFloat32*)m_elevationMap; + for (dgInt32 z = 0; z < m_height - 1; z ++) { + dgInt32 base = z * m_width; + dgFloat32 zVal = m_horizontalScale_z * z; + for (dgInt32 x = 0; x < m_width; x ++) { + dgVector p (m_horizontalScale_x * x, m_verticalScale * elevation[base + x], zVal, dgFloat32 (0.0f)); + dgFloat32 project = dir.DotProduct(p).m_x; + if (project > maxProject) { + maxProject = project; + support = p; + } + } + } + + } else { + const dgUnsigned16* const elevation = (dgUnsigned16*)m_elevationMap; + for (dgInt32 z = 0; z < m_height - 1; z ++) { + dgInt32 base = z * m_width; + dgFloat32 zVal = m_horizontalScale_z * z; + for (dgInt32 x = 0; x < m_width; x ++) { + dgVector p (m_horizontalScale_x * x, m_verticalScale * elevation[base + x], zVal, dgFloat32 (0.0f)); + dgFloat32 project = dir.DotProduct(p).m_x; + if (project > maxProject) { + maxProject = project; + support = p; + } + } + } + } + return support; +} + +dgVector dgCollisionHeightField::SupportVertexSpecial (const dgVector& dir, dgFloat32 skinThickness, dgInt32* const vertexIndex) const +{ + dgAssert (0); + return SupportVertex (dir, vertexIndex); +} + +void dgCollisionHeightField::DebugCollision (const dgMatrix& matrix, dgCollision::OnDebugCollisionMeshCallback callback, void* const userData) const +{ + dgVector points[4]; + + dgInt32 base = 0; + for (dgInt32 z = 0; z < m_height - 1; z ++) { + switch (m_elevationDataType) + { + case m_float32Bit: + { + const dgFloat32* const elevation = (dgFloat32*)m_elevationMap; + points[0 * 2 + 0] = dgVector ((0 + 0) * m_horizontalScale_x, m_verticalScale * elevation[base + 0 ], (z + 0) * m_horizontalScale_z, dgFloat32 (0.0f)); + points[1 * 2 + 0] = dgVector ((0 + 0) * m_horizontalScale_x, m_verticalScale * elevation[base + 0 + m_width + 0], (z + 1) * m_horizontalScale_z, dgFloat32 (0.0f)); + break; + } + + case m_unsigned16Bit: + { + const dgUnsigned16* const elevation = (dgUnsigned16*)m_elevationMap; + points[0 * 2 + 0] = dgVector ((0 + 0) * m_horizontalScale_x, m_verticalScale * dgFloat32 (elevation[base + 0 ]), (z + 0) * m_horizontalScale_z, dgFloat32 (0.0f)); + points[1 * 2 + 0] = dgVector ((0 + 0) * m_horizontalScale_x, m_verticalScale * dgFloat32 (elevation[base + 0 + m_width + 0]), (z + 1) * m_horizontalScale_z, dgFloat32 (0.0f)); + break; + } + } + + points[0 * 2 + 0] = matrix.TransformVector(points[0 * 2 + 0]); + points[1 * 2 + 0] = matrix.TransformVector(points[1 * 2 + 0]); + + for (dgInt32 x = 0; x < m_width - 1; x ++) { + dgTriplex triangle[3]; + switch (m_elevationDataType) + { + case m_float32Bit: + { + const dgFloat32* const elevation = (dgFloat32*)m_elevationMap; + points[0 * 2 + 1] = dgVector ((x + 1) * m_horizontalScale_x, m_verticalScale * elevation[base + x + 1], (z + 0) * m_horizontalScale_z, dgFloat32 (0.0f)); + points[1 * 2 + 1] = dgVector ((x + 1) * m_horizontalScale_x, m_verticalScale * elevation[base + x + m_width + 1], (z + 1) * m_horizontalScale_z, dgFloat32 (0.0f)); + break; + } + + case m_unsigned16Bit: + { + const dgUnsigned16* const elevation = (dgUnsigned16*)m_elevationMap; + points[0 * 2 + 1] = dgVector ((x + 1) * m_horizontalScale_x, m_verticalScale * dgFloat32 (elevation[base + x + 1]), (z + 0) * m_horizontalScale_z, dgFloat32 (0.0f)); + points[1 * 2 + 1] = dgVector ((x + 1) * m_horizontalScale_x, m_verticalScale * dgFloat32 (elevation[base + x + m_width + 1]), (z + 1) * m_horizontalScale_z, dgFloat32 (0.0f)); + break; + } + } + + points[0 * 2 + 1] = matrix.TransformVector(points[0 * 2 + 1]); + points[1 * 2 + 1] = matrix.TransformVector(points[1 * 2 + 1]); + + const dgInt32* const indirectIndex = &m_cellIndices[dgInt32 (m_diagonals[z * m_width + x])][0]; + + dgInt32 i0 = indirectIndex[0]; + dgInt32 i1 = indirectIndex[1]; + dgInt32 i2 = indirectIndex[2]; + dgInt32 i3 = indirectIndex[3]; + + triangle[0].m_x = points[i1].m_x; + triangle[0].m_y = points[i1].m_y; + triangle[0].m_z = points[i1].m_z; + + triangle[1].m_x = points[i0].m_x; + triangle[1].m_y = points[i0].m_y; + triangle[1].m_z = points[i0].m_z; + + triangle[2].m_x = points[i2].m_x; + triangle[2].m_y = points[i2].m_y; + triangle[2].m_z = points[i2].m_z; + callback (userData, 3, &triangle[0].m_x, m_atributeMap[base]); + + triangle[0].m_x = points[i1].m_x; + triangle[0].m_y = points[i1].m_y; + triangle[0].m_z = points[i1].m_z; + + triangle[1].m_x = points[i2].m_x; + triangle[1].m_y = points[i2].m_y; + triangle[1].m_z = points[i2].m_z; + + triangle[2].m_x = points[i3].m_x; + triangle[2].m_y = points[i3].m_y; + triangle[2].m_z = points[i3].m_z; + callback (userData, 3, &triangle[0].m_x, m_atributeMap[base]); + + points[0 * 2 + 0] = points[0 * 2 + 1]; + points[1 * 2 + 0] = points[1 * 2 + 1]; + } + base += m_width; + } +} + +void dgCollisionHeightField::CalculateMinAndMaxElevation(dgInt32 x0, dgInt32 x1, dgInt32 z0, dgInt32 z1, const dgFloat32* const elevation, dgFloat32& minHeight, dgFloat32& maxHeight) const +{ + dgInt32 base = z0 * m_width; + for (dgInt32 z = z0; z <= z1; z++) { + for (dgInt32 x = x0; x <= x1; x++) { + dgFloat32 high = elevation[base + x]; + minHeight = dgMin(high, minHeight); + maxHeight = dgMax(high, maxHeight); + } + base += m_width; + } +} + +void dgCollisionHeightField::CalculateMinAndMaxElevation(dgInt32 x0, dgInt32 x1, dgInt32 z0, dgInt32 z1, const dgUnsigned16* const elevation, dgFloat32& minHeight, dgFloat32& maxHeight) const +{ + dgInt32 base = z0 * m_width; + for (dgInt32 z = z0; z <= z1; z++) { + for (dgInt32 x = x0; x <= x1; x++) { + dgFloat32 high = dgFloat32 (elevation[base + x]); + minHeight = dgMin(high, minHeight); + maxHeight = dgMax(high, maxHeight); + } + base += m_width; + } +} + + +void dgCollisionHeightField::GetLocalAABB (const dgVector& q0, const dgVector& q1, dgVector& boxP0, dgVector& boxP1) const +{ + // the user data is the pointer to the collision geometry + CalculateMinExtend3d (q0, q1, boxP0, boxP1); + + dgVector p0 (boxP0.Scale(m_horizontalScaleInv_x).GetInt()); + dgVector p1 (boxP1.Scale(m_horizontalScaleInv_x).GetInt()); + + dgAssert (p0.m_ix == dgFastInt (boxP0.m_x * m_horizontalScaleInv_x)); + dgAssert (p0.m_iz == dgFastInt (boxP0.m_z * m_horizontalScaleInv_x)); + dgAssert (p1.m_ix == dgFastInt (boxP1.m_x * m_horizontalScaleInv_x)); + dgAssert (p1.m_iz == dgFastInt (boxP1.m_z * m_horizontalScaleInv_x)); + + dgInt32 x0 = dgInt32 (p0.m_ix); + dgInt32 x1 = dgInt32 (p1.m_ix); + dgInt32 z0 = dgInt32 (p0.m_iz); + dgInt32 z1 = dgInt32 (p1.m_iz); + + dgFloat32 minHeight = dgFloat32 (1.0e10f); + dgFloat32 maxHeight = dgFloat32 (-1.0e10f); + //dgInt32 base = z0 * m_width; + switch (m_elevationDataType) + { + case m_float32Bit: + { + CalculateMinAndMaxElevation(x0, x1, z0, z1, (dgFloat32*)m_elevationMap, minHeight, maxHeight); + break; + } + + case m_unsigned16Bit: + { + CalculateMinAndMaxElevation(x0, x1, z0, z1, (dgUnsigned16*)m_elevationMap, minHeight, maxHeight); + break; + } + } + + boxP0.m_y = m_verticalScale * minHeight; + boxP1.m_y = m_verticalScale * maxHeight; +} + +void dgCollisionHeightField::GetCollidingFaces (dgPolygonMeshDesc* const data) const +{ + dgVector boxP0; + dgVector boxP1; + + dgWorld* const world = data->m_objBody->GetWorld(); + + // the user data is the pointer to the collision geometry + CalculateMinExtend3d (data->m_p0, data->m_p1, boxP0, boxP1); + boxP0 += data->m_boxDistanceTravelInMeshSpace & (data->m_boxDistanceTravelInMeshSpace < dgVector::m_zero); + boxP1 += data->m_boxDistanceTravelInMeshSpace & (data->m_boxDistanceTravelInMeshSpace > dgVector::m_zero); + + //boxP0 = (boxP0.GetMax(dgVector::m_zero) & m_yMask) + boxP0.AndNot(m_yMask); + //boxP1 = (boxP1.GetMax(dgVector::m_zero) & m_yMask) + boxP1.AndNot(m_yMask); + boxP0 = boxP0.Select(boxP0.GetMax(dgVector::m_zero), m_yMask); + boxP1 = boxP1.Select(boxP1.GetMax(dgVector::m_zero), m_yMask); + + dgVector p0 (boxP0.Scale(m_horizontalScaleInv_x).GetInt()); + dgVector p1 (boxP1.Scale(m_horizontalScaleInv_x).GetInt()); + + dgAssert (p0.m_ix == dgFastInt (boxP0.m_x * m_horizontalScaleInv_x)); + dgAssert (p0.m_iz == dgFastInt (boxP0.m_z * m_horizontalScaleInv_x)); + dgAssert (p1.m_ix == dgFastInt (boxP1.m_x * m_horizontalScaleInv_x)); + dgAssert (p1.m_iz == dgFastInt (boxP1.m_z * m_horizontalScaleInv_x)); + + dgInt32 x0 = dgInt32 (p0.m_ix); + dgInt32 x1 = dgInt32 (p1.m_ix); + dgInt32 z0 = dgInt32 (p0.m_iz); + dgInt32 z1 = dgInt32 (p1.m_iz); + + data->m_separationDistance = dgFloat32 (0.0f); + dgFloat32 minHeight = dgFloat32 (1.0e10f); + dgFloat32 maxHeight = dgFloat32 (-1.0e10f); +// dgInt32 base = z0 * m_width; + switch (m_elevationDataType) + { + case m_float32Bit: + { + CalculateMinAndMaxElevation(x0, x1, z0, z1, (dgFloat32*)m_elevationMap, minHeight, maxHeight); + break; + } + + case m_unsigned16Bit: + { + CalculateMinAndMaxElevation(x0, x1, z0, z1, (dgUnsigned16*)m_elevationMap, minHeight, maxHeight); + break; + } + } + + minHeight *= m_verticalScale; + maxHeight *= m_verticalScale; + + if (!((maxHeight < boxP0.m_y) || (minHeight > boxP1.m_y))) { + // scan the vertices's intersected by the box extend + dgInt32 base = (z1 - z0 + 1) * (x1 - x0 + 1) + 2 * (z1 - z0) * (x1 - x0); + while (base > m_instanceData->m_vertexCount[data->m_threadNumber]) { + AllocateVertex(world, data->m_threadNumber); + } + + dgInt32 vertexIndex = 0; + base = z0 * m_width; + dgVector* const vertex = &m_instanceData->m_vertex[data->m_threadNumber][0]; + + switch (m_elevationDataType) + { + case m_float32Bit: + { + const dgFloat32* const elevation = (dgFloat32*)m_elevationMap; + for (dgInt32 z = z0; z <= z1; z ++) { + dgFloat32 zVal = m_horizontalScale_z * z; + for (dgInt32 x = x0; x <= x1; x ++) { + vertex[vertexIndex] = dgVector(m_horizontalScale_x * x, m_verticalScale * elevation[base + x], zVal, dgFloat32 (0.0f)); + vertexIndex ++; + dgAssert (vertexIndex <= m_instanceData->m_vertexCount[data->m_threadNumber]); + } + base += m_width; + } + break; + } + + case m_unsigned16Bit: + { + const dgUnsigned16* const elevation = (dgUnsigned16*)m_elevationMap; + for (dgInt32 z = z0; z <= z1; z ++) { + dgFloat32 zVal = m_horizontalScale_z * z; + for (dgInt32 x = x0; x <= x1; x ++) { + vertex[vertexIndex] = dgVector(m_horizontalScale_x * x, m_verticalScale * dgFloat32 (elevation[base + x]), zVal, dgFloat32 (0.0f)); + vertexIndex ++; + dgAssert (vertexIndex <= m_instanceData->m_vertexCount[data->m_threadNumber]); + } + base += m_width; + } + break; + } + } + + dgInt32 normalBase = vertexIndex; + vertexIndex = 0; + dgInt32 index = 0; + dgInt32 faceCount = 0; + dgInt32 step = x1 - x0 + 1; + dgInt32* const indices = data->m_globalFaceVertexIndex; + dgInt32* const faceIndexCount = data->m_meshData.m_globalFaceIndexCount; + dgInt32 faceSize = dgInt32 (dgMax (m_horizontalScale_x, m_horizontalScale_z) * dgFloat32 (2.0f)); + + for (dgInt32 z = z0; (z < z1) && (faceCount < DG_MAX_COLLIDING_FACES); z ++) { + dgInt32 zStep = z * m_width; + for (dgInt32 x = x0; (x < x1) && (faceCount < DG_MAX_COLLIDING_FACES); x ++) { + const dgInt32* const indirectIndex = &m_cellIndices[dgInt32 (m_diagonals[zStep + x])][0]; + + dgInt32 vIndex[4]; + vIndex[0] = vertexIndex; + vIndex[1] = vertexIndex + 1; + vIndex[2] = vertexIndex + step; + vIndex[3] = vertexIndex + step + 1; + + const dgInt32 i0 = vIndex[indirectIndex[0]]; + const dgInt32 i1 = vIndex[indirectIndex[1]]; + const dgInt32 i2 = vIndex[indirectIndex[2]]; + const dgInt32 i3 = vIndex[indirectIndex[3]]; + + const dgVector e0 (vertex[i0] - vertex[i1]); + const dgVector e1 (vertex[i2] - vertex[i1]); + const dgVector e2 (vertex[i3] - vertex[i1]); + dgVector n0 (e0.CrossProduct(e1)); + dgVector n1 (e1.CrossProduct(e2)); + dgAssert (n0.m_w == dgFloat32 (0.0f)); + dgAssert (n1.m_w == dgFloat32 (0.0f)); + + dgAssert (n0.DotProduct(n0).GetScalar() > dgFloat32 (0.0f)); + dgAssert (n1.DotProduct(n1).GetScalar() > dgFloat32 (0.0f)); + + //normalBase + const dgInt32 normalIndex0 = normalBase; + const dgInt32 normalIndex1 = normalBase + 1; + vertex[normalIndex0] = n0.Normalize(); + vertex[normalIndex1] = n1.Normalize(); + + faceIndexCount[faceCount] = 3; + indices[index + 0 + 0] = i2; + indices[index + 0 + 1] = i1; + indices[index + 0 + 2] = i0; + indices[index + 0 + 3] = m_atributeMap[zStep + x]; + indices[index + 0 + 4] = normalIndex0; + indices[index + 0 + 5] = normalIndex0; + indices[index + 0 + 6] = normalIndex0; + indices[index + 0 + 7] = normalIndex0; + indices[index + 0 + 8] = faceSize; + + faceIndexCount[faceCount + 1] = 3; + indices[index + 9 + 0] = i1; + indices[index + 9 + 1] = i2; + indices[index + 9 + 2] = i3; + indices[index + 9 + 3] = m_atributeMap[zStep + x]; + indices[index + 9 + 4] = normalIndex1; + indices[index + 9 + 5] = normalIndex1; + indices[index + 9 + 6] = normalIndex1; + indices[index + 9 + 7] = normalIndex1; + indices[index + 9 + 8] = faceSize; + + dgVector dp (vertex[i3] - vertex[i1]); + dgAssert (dp.m_w == dgFloat32 (0.0f)); + dgFloat32 dist (vertex[normalIndex0].DotProduct(dp).GetScalar()); + if (dist < -dgFloat32 (1.0e-3f)) { + indices[index + 0 + 5] = normalIndex1; + indices[index + 9 + 5] = normalIndex0; + } + + index += 9 * 2; + normalBase += 2; + faceCount += 2; + vertexIndex ++; + } + vertexIndex ++; + } + + #ifdef _DEBUG + dgAssert (faceCount < DG_MAX_COLLIDING_FACES); + if (faceCount >= DG_MAX_COLLIDING_FACES) { + dgTrace (("buffer Over float, try using a lower resolution mesh for collision\n")); + } + #endif + + const int maxIndex = index; + dgInt32 stepBase = (x1 - x0) * (2 * 9); + for (dgInt32 z = z0; z < z1; z ++) { + const dgInt32 diagBase = m_width * z; + const dgInt32 triangleIndexBase = (z - z0) * stepBase; + for (dgInt32 x = x0; x < (x1 - 1); x ++) { + dgInt32 index1 = (x - x0) * (2 * 9) + triangleIndexBase; + if (index1 < maxIndex) { + const dgInt32 code = (m_diagonals[diagBase + x] << 1) + m_diagonals[diagBase + x + 1]; + const dgInt32* const edgeMap = &m_horizontalEdgeMap[code][0]; + + dgInt32* const triangles = &indices[index1]; + const dgInt32 i0 = triangles[edgeMap[0]]; + const dgInt32 i1 = triangles[edgeMap[1]]; + const dgInt32 i2 = triangles[edgeMap[2]]; + + const dgVector& origin = vertex[i0]; + const dgVector& testPoint = vertex[i1]; + const dgVector& normal = vertex[i2]; + dgAssert (normal.m_w == dgFloat32 (0.0f)); + dgFloat32 dist (normal.DotProduct(testPoint - origin).GetScalar()); + + if (dist < -dgFloat32 (1.0e-3f)) { + const dgInt32 i3 = edgeMap[3]; + const dgInt32 i4 = edgeMap[4]; + const dgInt32 i5 = edgeMap[5]; + const dgInt32 i6 = edgeMap[6]; + triangles[i3] = triangles[i6]; + triangles[i4] = triangles[i5]; + } + } + } + } + + for (dgInt32 x = x0; x < x1; x ++) { + const dgInt32 triangleIndexBase = (x - x0) * (2 * 9); + for (dgInt32 z = z0; z < (z1 - 1); z ++) { + dgInt32 index1 = (z - z0) * stepBase + triangleIndexBase; + if (index1 < maxIndex) { + const dgInt32 diagBase = m_width * z; + const dgInt32 code = (m_diagonals[diagBase + x] << 1) + m_diagonals[diagBase + m_width + x]; + const dgInt32* const edgeMap = &m_verticalEdgeMap[code][0]; + + dgInt32* const triangles = &indices[index1]; + const dgInt32 i0 = triangles[edgeMap[0]]; + const dgInt32 i1 = triangles[edgeMap[1] + stepBase]; + const dgInt32 i2 = triangles[edgeMap[2]]; + + const dgVector& origin = vertex[i0]; + const dgVector& testPoint = vertex[i1]; + const dgVector& normal = vertex[i2]; + dgAssert (normal.m_w == dgFloat32 (0.0f)); + dgFloat32 dist (normal.DotProduct(testPoint - origin).GetScalar()); + + if (dist < -dgFloat32 (1.0e-3f)) { + const dgInt32 i3 = edgeMap[3]; + const dgInt32 i4 = edgeMap[4] + stepBase; + const dgInt32 i5 = edgeMap[5]; + const dgInt32 i6 = edgeMap[6] + stepBase; + triangles[i3] = triangles[i6]; + triangles[i4] = triangles[i5]; + } + } + } + } + + dgInt32 stride = sizeof (dgVector) / sizeof (dgFloat32); + dgInt32 faceCount0 = 0; + dgInt32 faceIndexCount0 = 0; + dgInt32 faceIndexCount1 = 0; + + dgInt32* const address = data->m_meshData.m_globalFaceIndexStart; + dgFloat32* const hitDistance = data->m_meshData.m_globalHitDistance; + + if (data->m_doContinuesCollisionTest) { + dgFastRayTest ray (dgVector (dgFloat32 (0.0f)), data->m_boxDistanceTravelInMeshSpace); + for (dgInt32 i = 0; i < faceCount; i ++) { + const dgInt32* const indexArray = &indices[faceIndexCount1]; + const dgVector& faceNormal = vertex[indexArray[4]]; + dgFloat32 dist = data->PolygonBoxRayDistance (faceNormal, 3, indexArray, stride, &vertex[0].m_x, ray); + if (dist < dgFloat32 (1.0f)) { + hitDistance[faceCount0] = dist; + address[faceCount0] = faceIndexCount0; + memcpy (&indices[faceIndexCount0], indexArray, 9 * sizeof (dgInt32)); + faceCount0 ++; + faceIndexCount0 += 9; + } + faceIndexCount1 += 9; + } + } else { + for (dgInt32 i = 0; i < faceCount; i ++) { + const dgInt32* const indexArray = &indices[faceIndexCount1]; + const dgVector& faceNormal = vertex[indexArray[4]]; + dgFloat32 dist = data->PolygonBoxDistance (faceNormal, 3, indexArray, stride, &vertex[0].m_x); + if (dist > dgFloat32 (0.0f)) { + hitDistance[faceCount0] = dist; + address[faceCount0] = faceIndexCount0; + memcpy (&indices[faceIndexCount0], indexArray, 9 * sizeof (dgInt32)); + faceCount0 ++; + faceIndexCount0 += 9; + } + faceIndexCount1 += 9; + } + } + + if (faceCount0) { + // initialize the callback data structure + data->m_faceCount = faceCount0; + data->m_vertex = &vertex[0].m_x; + data->m_faceVertexIndex = indices; + data->m_faceIndexStart = address; + data->m_hitDistance = hitDistance; + data->m_faceIndexCount = faceIndexCount; + data->m_vertexStrideInBytes = sizeof (dgVector); + + if (GetDebugCollisionCallback()) { + dgTriplex triplex[3]; + const dgVector scale = data->m_polySoupInstance->GetScale(); + dgMatrix matrix(data->m_polySoupInstance->GetLocalMatrix() * data->m_polySoupBody->GetMatrix()); + + for (dgInt32 i = 0; i < data->m_faceCount; i ++) { + dgInt32 base1 = address[i]; + for (dgInt32 j = 0; j < 3; j ++) { + dgInt32 index1 = data->m_faceVertexIndex[base1 + j]; + dgVector p (matrix.TransformVector(scale * dgVector(vertex[index1]))); + triplex[j].m_x = p.m_x; + triplex[j].m_y = p.m_y; + triplex[j].m_z = p.m_z; + } + GetDebugCollisionCallback() (data->m_polySoupBody, data->m_objBody, data->m_faceVertexIndex[base1 + 4], 3, &triplex[0].m_x, sizeof (dgTriplex)); + } + } + } + + #ifdef _DEBUG + for (dgInt32 i = 0; i < data->m_faceCount; i ++) { + dgInt32 base1 = address[i]; + const dgInt32* const localIndexArray = &data->m_faceVertexIndex[base1]; + + dgInt32 index1 = data->GetNormalIndex (localIndexArray, 3); + dgVector n (vertex[index1]); + dgVector q0 (vertex[data->m_faceVertexIndex[base1 + 0]]); + dgVector q1 (vertex[data->m_faceVertexIndex[base1 + 1]]); + dgVector q2 (vertex[data->m_faceVertexIndex[base1 + 2]]); + + dgMatrix polygonMatrix; + polygonMatrix[0] = q1 - q0; + polygonMatrix[0] = polygonMatrix[0].Normalize(); + polygonMatrix[1] = n.CrossProduct(polygonMatrix[0]); + polygonMatrix[2] = n; + polygonMatrix[3] = dgVector::m_wOne; + dgAssert (polygonMatrix.TestOrthogonal()); + } + #endif + } +} diff --git a/thirdparty/src/newton/dgPhysics/dgCollisionHeightField.h b/thirdparty/src/newton/dgPhysics/dgCollisionHeightField.h new file mode 100644 index 000000000..f9b7e0667 --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgCollisionHeightField.h @@ -0,0 +1,135 @@ +/* Copyright (c) <2003-2019> +* +* 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 __DGCOLLISION_HEIGHT_FIELD__ +#define __DGCOLLISION_HEIGHT_FIELD__ + +#include "dgCollision.h" +#include "dgCollisionMesh.h" + +class dgCollisionHeightField; +typedef dgFloat32 (*dgCollisionHeightFieldRayCastCallback) (const dgBody* const body, const dgCollisionHeightField* const heightFieldCollision, dgFloat32 interception, dgInt32 row, dgInt32 col, dgVector* const normal, int faceId, void* const usedData); + + +class dgCollisionHeightField: public dgCollisionMesh +{ + public: + enum dgElevationType + { + m_float32Bit = 0, + m_unsigned16Bit, + }; + + enum dgCollisionHeightFieldGridConstruction + { + m_normalDiagonals = 0, + m_invertedDiagonals, + m_alternateOddRowsDiagonals, + m_alternateEvenRowsDiagonals, + m_alternateOddColumsDiagonals, + m_alternateEvenColumsDiagonals, + m_starDiagonals, + m_starInvertexDiagonals, + }; + dgCollisionHeightField (dgWorld* const world, dgInt32 width, dgInt32 height, dgInt32 contructionMode, + const void* const elevationMap, dgElevationType elevationDataType, dgFloat32 verticalScale, + const dgInt8* const atributeMap, dgFloat32 horizontalScale_x, dgFloat32 horizontalScale_z); + + dgCollisionHeightField (dgWorld* const world, dgDeserialize deserialization, void* const userData, dgInt32 revisionNumber); + + virtual ~dgCollisionHeightField(void); + + void SetCollisionRayCastCallback (dgCollisionHeightFieldRayCastCallback rayCastCallback); + dgCollisionHeightFieldRayCastCallback GetDebugRayCastCallback() const { return m_userRayCastCallback;} + + private: + class dgPerIntanceData + { + public: + dgWorld* m_world; + dgInt32 m_refCount; + dgInt32 m_vertexCount[DG_MAX_THREADS_HIVE_COUNT]; + dgArray m_vertex[DG_MAX_THREADS_HIVE_COUNT]; + }; + + void CalculateAABB(); + void CalculateMinAndMaxElevation(dgInt32 x0, dgInt32 x1, dgInt32 z0, dgInt32 z1, const dgUnsigned16* const elevation, dgFloat32& minHeight, dgFloat32& maxHeight) const; + void CalculateMinAndMaxElevation(dgInt32 x0, dgInt32 x1, dgInt32 z0, dgInt32 z1, const dgFloat32* const elevation, dgFloat32& minHeight, dgFloat32& maxHeight) const; + + void AllocateVertex(dgWorld* const world, dgInt32 thread) const; + void CalculateMinExtend2d (const dgVector& p0, const dgVector& p1, dgVector& boxP0, dgVector& boxP1) const; + void CalculateMinExtend3d (const dgVector& p0, const dgVector& p1, dgVector& boxP0, dgVector& boxP1) const; + dgFloat32 RayCastCell (const dgFastRayTest& ray, dgInt32 xIndex0, dgInt32 zIndex0, dgVector& normalOut, dgFloat32 maxT) const; + + virtual void Serialize(dgSerialize callback, void* const userData) const; + virtual dgFloat32 RayCast (const dgVector& localP0, const dgVector& localP1, dgFloat32 maxT, dgContactPoint& contactOut, const dgBody* const body, void* const userData, OnRayPrecastAction preFilter) const; + virtual void GetCollidingFaces (dgPolygonMeshDesc* const data) const; + + virtual void GetCollisionInfo(dgCollisionInfo* const info) const; + virtual dgVector SupportVertex (const dgVector& dir, dgInt32* const vertexIndex) const; + virtual dgVector SupportVertexSpecial (const dgVector& dir, dgFloat32 skinThickness, dgInt32* const vertexIndex) const; + virtual dgVector SupportVertexSpecialProjectPoint (const dgVector& point, const dgVector& dir) const {return point;}; + + virtual void DebugCollision (const dgMatrix& matrixPtr, dgCollision::OnDebugCollisionMeshCallback callback, void* const userData) const; + void GetVertexListIndexList (const dgVector& p0, const dgVector& p1, dgMeshVertexListIndexList &data) const; + void GetLocalAABB (const dgVector& p0, const dgVector& p1, dgVector& boxP0, dgVector& boxP1) const; + + DG_INLINE dgInt32 dgFastInt(dgFloat32 x) const + { + dgInt32 i = dgInt32(x); + if (dgFloat32(i) > x) { + i--; + } + return i; + } + + dgVector m_minBox; + dgVector m_maxBox; + + dgInt32 m_width; + dgInt32 m_height; + dgInt32 m_diagonalMode; + dgInt8* m_atributeMap; + dgInt8* m_diagonals; + void* m_elevationMap; + dgFloat32 m_verticalScale; + dgFloat32 m_horizontalScale_x; + dgFloat32 m_horizontalScaleInv_x; + dgFloat32 m_horizontalScale_z; + dgFloat32 m_horizontalScaleInv_z; + dgCollisionHeightFieldRayCastCallback m_userRayCastCallback; + dgElevationType m_elevationDataType; + + + static dgVector m_yMask; + static dgVector m_padding; + static dgVector m_elevationPadding; + static dgInt32 m_cellIndices[][4]; + static dgInt32 m_verticalEdgeMap[][7]; + static dgInt32 m_horizontalEdgeMap[][7]; + + dgPerIntanceData* m_instanceData; + friend class dgCollisionCompound; +}; + + + +#endif diff --git a/thirdparty/src/newton/dgPhysics/dgCollisionIncompressibleParticles.cpp b/thirdparty/src/newton/dgPhysics/dgCollisionIncompressibleParticles.cpp new file mode 100644 index 000000000..e14f2b6d4 --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgCollisionIncompressibleParticles.cpp @@ -0,0 +1,111 @@ +/* Copyright (c) <2003-2019> +* +* 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 "dgPhysicsStdafx.h" +#include "dgBody.h" +#include "dgWorld.h" +#include "dgContact.h" +#include "dgMeshEffect.h" +#include "dgDynamicBody.h" +#include "dgCollisionIncompressibleParticles.h" + + +dgCollisionIncompressibleParticles::dgCollisionIncompressibleParticles(dgWorld* const world, dgMeshEffect* const mesh) + :dgCollisionLumpedMassParticles(world, m_deformableSolidMesh) +{ + m_rtti |= dgCollisionIncompressibleParticles_RTTI; + +/* + dgInt32 count = mesh->GetVertexCount(); + dgVector* const points = dgAlloca(dgVector, count); + + for (dgInt32 i = 0; i < count; i++) { + dgBigVector p(mesh->GetVertex(i)); + points[i] = p; + } + + m_indexToVertexCount = count; + m_indexToVertexMap.Resize(count); + dgInt32* const indexToVertexMap = &m_indexToVertexMap[0]; + m_particlesCount = dgVertexListToIndexList(&points[0].m_x, sizeof(dgVector), 3 * sizeof(dgFloat32), 0, count, indexToVertexMap, dgFloat32(1.0e-5f)); + for (dgInt32 i = 0; i < m_particlesCount; i++) { + m_posit[i] = points[i]; + } + + + dgInt32 edgeCount = 0; + dgSoftLink* const links = dgAlloca(dgSoftLink, mesh->GetCount() / 2); + for (void* edgePtr = mesh->GetFirstEdge(); edgePtr; edgePtr = mesh->GetNextEdge(edgePtr)) { + const dgEdge* const edge = mesh->GetPolyhedraEdgeFromNode(edgePtr); + + dgInt32 v0 = indexToVertexMap[edge->m_incidentVertex]; + dgInt32 v1 = indexToVertexMap[edge->m_twin->m_incidentVertex]; + links[edgeCount].m_m0 = dgInt16(dgMin(v0, v1)); + links[edgeCount].m_m1 = dgInt16(dgMax(v0, v1)); + edgeCount++; + if ((edge->m_incidentFace > 0) && (edge->m_twin->m_incidentFace > 0)) { + v0 = indexToVertexMap[edge->m_prev->m_incidentVertex]; + v1 = indexToVertexMap[edge->m_twin->m_prev->m_incidentVertex]; + links[edgeCount].m_m0 = dgInt16(dgMin(v0, v1)); + links[edgeCount].m_m1 = dgInt16(dgMax(v0, v1)); + edgeCount++; + } + } + dgSort(links, edgeCount, CompareEdges); + + dgInt32 uniqueEdgeCount = 0; + for (dgInt32 i = 1; i < edgeCount; i++) { + if (CompareEdges(&links[i], &links[uniqueEdgeCount], NULL) > 0) { + uniqueEdgeCount++; + links[uniqueEdgeCount] = links[i]; + } + } + uniqueEdgeCount++; + m_linksCount = uniqueEdgeCount; + m_linkList.Resize(m_linksCount); + for (dgInt32 i = 0; i < m_linksCount; i++) { + m_linkList[i] = links[i]; + } + FinalizeBuild(); +*/ +} + + +dgCollisionIncompressibleParticles::dgCollisionIncompressibleParticles(const dgCollisionIncompressibleParticles& source) + :dgCollisionLumpedMassParticles(source) +{ + m_rtti |= source.m_rtti; +} + +dgCollisionIncompressibleParticles::dgCollisionIncompressibleParticles(dgWorld* const world, dgDeserialize deserialization, void* const userData, dgInt32 revisionNumber) + :dgCollisionLumpedMassParticles(world, deserialization, userData, revisionNumber) +{ +} + +dgCollisionIncompressibleParticles::~dgCollisionIncompressibleParticles(void) +{ +} + + +void dgCollisionIncompressibleParticles::CalculateAcceleration(dgFloat32 timestep) +{ + dgAssert(0); +} diff --git a/thirdparty/src/newton/dgPhysics/dgCollisionIncompressibleParticles.h b/thirdparty/src/newton/dgPhysics/dgCollisionIncompressibleParticles.h new file mode 100644 index 000000000..324fd34c7 --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgCollisionIncompressibleParticles.h @@ -0,0 +1,47 @@ +/* Copyright (c) <2003-2019> +* +* 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 __DGCOLLISION_INCOMPRESSIBLE_PARTICLES_H__ +#define __DGCOLLISION_INCOMPRESSIBLE_PARTICLES_H__ + + +#include "dgCollision.h" +#include "dgCollisionLumpedMassParticles.h" + + +class dgCollisionIncompressibleParticles: public dgCollisionLumpedMassParticles +{ + public: + dgCollisionIncompressibleParticles (const dgCollisionIncompressibleParticles& source); + dgCollisionIncompressibleParticles (dgWorld* const world, dgMeshEffect* const mesh); + dgCollisionIncompressibleParticles (dgWorld* const world, dgDeserialize deserialization, void* const userData, dgInt32 revisionNumber); + + virtual ~dgCollisionIncompressibleParticles(void); + virtual void CalculateAcceleration(dgFloat32 timestep); +}; + + + + + +#endif + diff --git a/thirdparty/src/newton/dgPhysics/dgCollisionInstance.cpp b/thirdparty/src/newton/dgPhysics/dgCollisionInstance.cpp new file mode 100644 index 000000000..01fc92f6a --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgCollisionInstance.cpp @@ -0,0 +1,689 @@ +/* Copyright (c) <2003-2019> +* +* 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 "dgPhysicsStdafx.h" +#include "dgBody.h" +#include "dgWorld.h" +#include "dgContact.h" +#include "dgCollisionBox.h" +#include "dgCollisionBVH.h" +#include "dgCollisionMesh.h" +#include "dgCollisionNull.h" +#include "dgCollisionCone.h" +#include "dgCollisionSphere.h" +#include "dgCollisionCapsule.h" +#include "dgCollisionCylinder.h" +#include "dgCollisionInstance.h" +#include "dgCollisionCompound.h" +#include "dgCollisionHeightField.h" +#include "dgCollisionConvexPolygon.h" +#include "dgCollisionChamferCylinder.h" +#include "dgCollisionCompoundFractured.h" +#include "dgCollisionDeformableSolidMesh.h" +#include "dgCollisionMassSpringDamperSystem.h" + +////////////////////////////////////////////////////////////////////// +// Construction/Destruction +////////////////////////////////////////////////////////////////////// + + +dgVector dgCollisionInstance::m_padding (DG_MAX_COLLISION_AABB_PADDING, DG_MAX_COLLISION_AABB_PADDING, DG_MAX_COLLISION_AABB_PADDING, dgFloat32 (0.0f)); + +dgCollisionInstance::dgCollisionInstance() + :m_globalMatrix(dgGetIdentityMatrix()) + ,m_localMatrix (dgGetIdentityMatrix()) + ,m_aligmentMatrix (dgGetIdentityMatrix()) + ,m_scale(dgFloat32 (1.0f), dgFloat32 (1.0f), dgFloat32 (1.0f), dgFloat32 (0.0f)) + ,m_invScale(dgFloat32 (1.0f), dgFloat32 (1.0f), dgFloat32 (1.0f), dgFloat32 (0.0f)) + ,m_maxScale(dgFloat32 (1.0f), dgFloat32 (1.0f), dgFloat32 (1.0f), dgFloat32 (0.0f)) + ,m_material() + ,m_world(NULL) + ,m_childShape (NULL) + ,m_subCollisionHandle(NULL) + ,m_parent(NULL) + ,m_skinThickness(dgFloat32 (0.0f)) + ,m_collisionMode(1) + ,m_refCount(1) + ,m_scaleType(m_unit) + ,m_isExternal(true) +{ +} + +dgCollisionInstance::dgCollisionInstance(const dgWorld* const world, const dgCollision* const childCollision, dgInt32 shapeID, const dgMatrix& matrix) + :m_globalMatrix(matrix) + ,m_localMatrix (matrix) + ,m_aligmentMatrix (dgGetIdentityMatrix()) + ,m_scale(dgFloat32 (1.0f), dgFloat32 (1.0f), dgFloat32 (1.0f), dgFloat32 (0.0f)) + ,m_invScale(dgFloat32 (1.0f), dgFloat32 (1.0f), dgFloat32 (1.0f), dgFloat32 (0.0f)) + ,m_maxScale(dgFloat32 (1.0f), dgFloat32 (1.0f), dgFloat32 (1.0f), dgFloat32 (0.0f)) + ,m_material() + ,m_world(world) + ,m_childShape (childCollision) + ,m_subCollisionHandle(NULL) + ,m_parent(NULL) + ,m_skinThickness(dgFloat32 (0.0f)) + ,m_collisionMode(1) + ,m_refCount(1) + ,m_scaleType(m_unit) + ,m_isExternal(true) +{ + m_material.m_userId = shapeID; + m_childShape->AddRef(); +} + +dgCollisionInstance::dgCollisionInstance(const dgCollisionInstance& instance) + :m_globalMatrix(instance.m_globalMatrix) + ,m_localMatrix (instance.m_localMatrix) + ,m_aligmentMatrix (instance.m_aligmentMatrix) + ,m_scale(instance.m_scale) + ,m_invScale(instance.m_invScale) + ,m_maxScale(instance.m_maxScale) + ,m_material(instance.m_material) + ,m_world(instance.m_world) + ,m_childShape (instance.m_childShape) + ,m_subCollisionHandle(NULL) + ,m_parent(NULL) + ,m_skinThickness(instance.m_skinThickness) + ,m_collisionMode(instance.m_collisionMode) + ,m_refCount(1) + ,m_scaleType(instance.m_scaleType) + ,m_isExternal(true) +{ + if (m_childShape->IsType (dgCollision::dgCollisionCompound_RTTI)) { + if (m_childShape->IsType (dgCollision::dgCollisionCompoundBreakable_RTTI)) { + dgCollisionCompoundFractured* const compound = (dgCollisionCompoundFractured*) m_childShape; + m_childShape = new (m_world->GetAllocator()) dgCollisionCompoundFractured (*compound, this); + } else if (m_childShape->IsType (dgCollision::dgCollisionScene_RTTI)) { + dgCollisionScene* const scene = (dgCollisionScene*) m_childShape; + m_childShape = new (m_world->GetAllocator()) dgCollisionScene (*scene, this); + } else { + dgCollisionCompound *const compound = (dgCollisionCompound*) m_childShape; + m_childShape = new (m_world->GetAllocator()) dgCollisionCompound (*compound, this); + } + } else if (m_childShape->IsType (dgCollision::dgCollisionMassSpringDamperSystem_RTTI)) { + dgCollisionMassSpringDamperSystem* const deformable = (dgCollisionMassSpringDamperSystem*) m_childShape; + m_childShape = new (m_world->GetAllocator()) dgCollisionMassSpringDamperSystem (*deformable); + } else if (m_childShape->IsType (dgCollision::dgCollisionDeformableSolidMesh_RTTI)) { + dgCollisionDeformableSolidMesh* const deformable = (dgCollisionDeformableSolidMesh*) m_childShape; + m_childShape = new (m_world->GetAllocator()) dgCollisionDeformableSolidMesh (*deformable); + } else { + m_childShape->AddRef(); + } + + if (m_world->m_onCollisionInstanceCopyConstrutor) { + m_world->m_onCollisionInstanceCopyConstrutor (m_world, this, &instance); + } +} + +dgCollisionInstance::dgCollisionInstance(const dgWorld* const constWorld, dgDeserialize serialize, void* const userData, dgInt32 revisionNumber) + :m_globalMatrix(dgGetIdentityMatrix()) + ,m_localMatrix (dgGetIdentityMatrix()) + ,m_aligmentMatrix (dgGetIdentityMatrix()) + ,m_scale(dgFloat32 (1.0f), dgFloat32 (1.0f), dgFloat32 (1.0f), dgFloat32 (0.0f)) + ,m_invScale(dgFloat32 (1.0f), dgFloat32 (1.0f), dgFloat32 (1.0f), dgFloat32 (0.0f)) + ,m_maxScale(dgFloat32 (1.0f), dgFloat32 (1.0f), dgFloat32 (1.0f), dgFloat32 (0.0f)) + ,m_material() + ,m_world(constWorld) + ,m_childShape (NULL) + ,m_subCollisionHandle(NULL) + ,m_parent(NULL) + ,m_skinThickness(dgFloat32 (0.0f)) + ,m_collisionMode(1) + ,m_refCount(1) + ,m_scaleType(m_unit) + ,m_isExternal(true) +{ + dgInt32 saved; + dgInt32 signature; + dgInt32 primitive; + dgInt32 scaleType; + + serialize (userData, &m_globalMatrix, sizeof (m_globalMatrix)); + serialize (userData, &m_localMatrix, sizeof (m_localMatrix)); + serialize (userData, &m_aligmentMatrix, sizeof (m_aligmentMatrix)); + serialize (userData, &m_scale, sizeof (m_scale)); + serialize (userData, &m_invScale, sizeof (m_invScale)); + serialize (userData, &m_maxScale, sizeof (m_maxScale)); + serialize (userData, &m_skinThickness, sizeof (m_skinThickness)); + serialize (userData, &m_material, sizeof (m_material)); + serialize (userData, &m_collisionMode, sizeof (m_collisionMode)); + serialize (userData, &scaleType, sizeof (scaleType)); + serialize (userData, &primitive, sizeof (primitive)); + serialize (userData, &signature, sizeof (signature)); + serialize (userData, &saved, sizeof (saved)); + + m_scaleType = dgScaleType(scaleType); + + dgWorld* const world = (dgWorld*) constWorld; + if (saved) { + const dgCollision* collision = NULL; + dgBodyCollisionList::dgTreeNode* node = world->dgBodyCollisionList::Find (dgUnsigned32 (signature)); + + if (node) { + collision = node->GetInfo(); + collision->AddRef(); + + } else { + + dgCollisionID primitiveType = dgCollisionID(primitive); + + dgMemoryAllocator* const allocator = world->GetAllocator(); + switch (primitiveType) + { + case m_heightField: + { + collision = new (allocator) dgCollisionHeightField (world, serialize, userData, revisionNumber); + break; + } + + case m_boundingBoxHierachy: + { + collision = new (allocator) dgCollisionBVH (world, serialize, userData, revisionNumber); + break; + } + + case m_compoundCollision: + { + collision = new (allocator) dgCollisionCompound (world, serialize, userData, this, revisionNumber); + break; + } + + case m_compoundFracturedCollision: + { + collision = new (allocator) dgCollisionCompoundFractured (world, serialize, userData, this, revisionNumber); + break; + } + + case m_sceneCollision: + { + collision = new (allocator) dgCollisionScene (world, serialize, userData, this, revisionNumber); + break; + } + + + case m_sphereCollision: + { + collision = new (allocator) dgCollisionSphere (world, serialize, userData, revisionNumber); + node = world->dgBodyCollisionList::Insert (collision, collision->GetSignature()); + collision->AddRef(); + break; + } + + case m_boxCollision: + { + collision = new (allocator) dgCollisionBox (world, serialize, userData, revisionNumber); + node = world->dgBodyCollisionList::Insert (collision, collision->GetSignature()); + collision->AddRef(); + break; + } + + case m_coneCollision: + { + collision = new (allocator) dgCollisionCone (world, serialize, userData, revisionNumber); + node = world->dgBodyCollisionList::Insert (collision, collision->GetSignature()); + collision->AddRef(); + break; + } + + case m_capsuleCollision: + { + collision = new (allocator) dgCollisionCapsule (world, serialize, userData, revisionNumber); + node = world->dgBodyCollisionList::Insert (collision, collision->GetSignature()); + collision->AddRef(); + break; + } + + case m_cylinderCollision: + { + collision = new (allocator) dgCollisionCylinder (world, serialize, userData, revisionNumber); + node = world->dgBodyCollisionList::Insert (collision, collision->GetSignature()); + collision->AddRef(); + break; + } + + case m_chamferCylinderCollision: + { + collision = new (allocator) dgCollisionChamferCylinder (world, serialize, userData, revisionNumber); + node = world->dgBodyCollisionList::Insert (collision, collision->GetSignature()); + collision->AddRef(); + break; + } + + case m_convexHullCollision: + { + collision = new (allocator) dgCollisionConvexHull (world, serialize, userData, revisionNumber); + node = world->dgBodyCollisionList::Insert (collision, collision->GetSignature()); + collision->AddRef(); + break; + } + + case m_nullCollision: + { + collision = new (allocator) dgCollisionNull (world, serialize, userData, revisionNumber); + node = world->dgBodyCollisionList::Insert (collision, collision->GetSignature()); + collision->AddRef(); + break; + } + +// case m_deformableMesh: +// { +// dgAssert (0); +// return NULL; +// } + + default: + dgAssert (0); + } + } + m_childShape = collision; + } + dgDeserializeMarker (serialize, userData); +} + +dgCollisionInstance::~dgCollisionInstance() +{ + if (m_world->m_onCollisionInstanceDestruction && m_isExternal) { + m_world->m_onCollisionInstanceDestruction (m_world, this); + } + dgWorld* const world = (dgWorld*)m_world; + world->ReleaseCollision(m_childShape); +} + +void dgCollisionInstance::Serialize(dgSerialize serialize, void* const userData, bool saveShape) const +{ + dgInt32 save = saveShape ? 1 : 0; + dgInt32 primitiveType = m_childShape->GetCollisionPrimityType(); + dgInt32 signature = m_childShape->GetSignature(); + dgInt32 scaleType = m_scaleType; + + serialize (userData, &m_globalMatrix, sizeof (m_globalMatrix)); + serialize (userData, &m_localMatrix, sizeof (m_localMatrix)); + serialize (userData, &m_aligmentMatrix, sizeof (m_aligmentMatrix)); + serialize (userData, &m_scale, sizeof (m_scale)); + serialize (userData, &m_invScale, sizeof (m_invScale)); + serialize (userData, &m_maxScale, sizeof (m_maxScale)); + serialize (userData, &m_skinThickness, sizeof (m_skinThickness)); + serialize (userData, &m_material, sizeof (m_material)); + serialize (userData, &m_collisionMode, sizeof (m_collisionMode)); + serialize (userData, &scaleType, sizeof (scaleType)); + serialize (userData, &primitiveType, sizeof (primitiveType)); + serialize (userData, &signature, sizeof (signature)); + serialize (userData, &save, sizeof (save)); + if (saveShape) { + m_childShape->Serialize(serialize, userData); + } + dgSerializeMarker(serialize, userData); +} + + +void dgCollisionInstance::SetScale (const dgVector& scale) +{ + dgFloat32 scaleX = dgAbs (scale.m_x); + dgFloat32 scaleY = dgAbs (scale.m_y); + dgFloat32 scaleZ = dgAbs (scale.m_z); + dgAssert (scaleX > dgFloat32 (0.0f)); + dgAssert (scaleY > dgFloat32 (0.0f)); + dgAssert (scaleZ > dgFloat32 (0.0f)); + + if (IsType(dgCollision::dgCollisionCompound_RTTI)) { + dgAssert (m_scaleType == m_unit); + dgCollisionCompound* const compound = (dgCollisionCompound*) m_childShape; + compound->ApplyScale(scale); + } else if ((dgAbs (scaleX - scaleY) < dgFloat32 (1.0e-4f)) && (dgAbs (scaleX - scaleZ) < dgFloat32 (1.0e-4f))) { + if ((dgAbs (scaleX - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f))) { + m_scaleType = m_unit; + m_scale = dgVector (dgFloat32 (1.0f), dgFloat32 (1.0f), dgFloat32 (1.0f), dgFloat32 (0.0f)); + m_maxScale = m_scale; + m_invScale = m_scale; + } else { + m_scaleType = m_uniform; + m_scale = dgVector (scaleX, scaleX, scaleX, dgFloat32 (0.0f)); + m_maxScale = m_scale; + m_invScale = dgVector (dgFloat32 (1.0f) / scaleX, dgFloat32 (1.0f) / scaleX, dgFloat32 (1.0f) / scaleX, dgFloat32 (0.0f)); + } + } else { + m_scaleType = m_nonUniform; + m_maxScale = dgMax(scaleX, scaleY, scaleZ); + m_scale = dgVector (scaleX, scaleY, scaleZ, dgFloat32 (0.0f)); + m_invScale = dgVector (dgFloat32 (1.0f) / scaleX, dgFloat32 (1.0f) / scaleY, dgFloat32 (1.0f) / scaleZ, dgFloat32 (0.0f)); + } +} + +void dgCollisionInstance::SetGlobalScale (const dgVector& scale) +{ + // calculate current matrix + dgMatrix matrix(dgGetIdentityMatrix()); + matrix[0][0] = m_scale.m_x; + matrix[1][1] = m_scale.m_y; + matrix[2][2] = m_scale.m_z; + matrix = m_aligmentMatrix * matrix * m_localMatrix; + + // extract the original local matrix + dgMatrix transpose (matrix.Transpose()); + dgVector globalScale (dgSqrt (transpose[0].DotProduct(transpose[0]).GetScalar()), dgSqrt (transpose[1].DotProduct(transpose[1]).GetScalar()), dgSqrt (transpose[2].DotProduct(transpose[2]).GetScalar()), dgFloat32 (1.0f)); + dgVector invGlobalScale (dgFloat32 (1.0f) / globalScale.m_x, dgFloat32 (1.0f) / globalScale.m_y, dgFloat32 (1.0f) / globalScale.m_z, dgFloat32 (1.0f)); + dgMatrix localMatrix (m_aligmentMatrix.Transpose() * m_localMatrix); + localMatrix.m_posit = matrix.m_posit * invGlobalScale; + dgAssert (localMatrix.m_posit.m_w == dgFloat32 (1.0f)); + + if ((dgAbs (scale[0] - scale[1]) < dgFloat32 (1.0e-4f)) && (dgAbs (scale[0] - scale[2]) < dgFloat32 (1.0e-4f))) { + m_localMatrix = localMatrix; + m_localMatrix.m_posit = m_localMatrix.m_posit * scale | dgVector::m_wOne; + m_aligmentMatrix = dgGetIdentityMatrix(); + SetScale (scale); + } else { + + // create a new scale matrix + localMatrix[0] = localMatrix[0] * scale; + localMatrix[1] = localMatrix[1] * scale; + localMatrix[2] = localMatrix[2] * scale; + localMatrix[3] = localMatrix[3] * scale; + localMatrix[3][3] = dgFloat32 (1.0f); + + // decompose into to align * scale * local + localMatrix.PolarDecomposition (m_localMatrix, m_scale, m_aligmentMatrix); + + m_localMatrix = m_aligmentMatrix * m_localMatrix; + m_aligmentMatrix = m_aligmentMatrix.Transpose(); + + dgAssert (m_localMatrix.TestOrthogonal()); + dgAssert (m_aligmentMatrix.TestOrthogonal()); + +//dgMatrix xxx1 (dgGetIdentityMatrix()); +//xxx1[0][0] = m_scale.m_x; +//xxx1[1][1] = m_scale.m_y; +//xxx1[2][2] = m_scale.m_z; +//dgMatrix xxx (m_aligmentMatrix * xxx1 * m_localMatrix); + + bool isIdentity = true; + for (dgInt32 i = 0; i < 3; i ++) { + isIdentity &= dgAbs (m_aligmentMatrix[i][i] - dgFloat32 (1.0f)) < dgFloat32 (1.0e-5f); + isIdentity &= dgAbs (m_aligmentMatrix[3][i]) < dgFloat32 (1.0e-5f); + } + m_scaleType = isIdentity ? m_nonUniform : m_global; + + m_maxScale = dgMax(m_scale[0], m_scale[1], m_scale[2]); + m_invScale = dgVector (dgFloat32 (1.0f) / m_scale[0], dgFloat32 (1.0f) / m_scale[1], dgFloat32 (1.0f) / m_scale[2], dgFloat32 (0.0f)); + } +} + + +void dgCollisionInstance::SetLocalMatrix (const dgMatrix& matrix) +{ + m_localMatrix = matrix; + m_localMatrix[0][3] = dgFloat32 (0.0f); + m_localMatrix[1][3] = dgFloat32 (0.0f); + m_localMatrix[2][3] = dgFloat32 (0.0f); + m_localMatrix[3][3] = dgFloat32 (1.0f); + dgAssert(m_localMatrix.TestOrthogonal()); +} + + +void dgCollisionInstance::DebugCollision (const dgMatrix& matrix, dgCollision::OnDebugCollisionMeshCallback callback, void* const userData) const +{ + m_childShape->DebugCollision (GetScaledTransform(matrix), callback, userData); +} + + +dgMatrix dgCollisionInstance::CalculateInertia () const +{ + if (IsType(dgCollision::dgCollisionMesh_RTTI)) { + return dgGetZeroMatrix(); + } else { + return m_childShape->CalculateInertiaAndCenterOfMass (m_aligmentMatrix, m_scale, m_localMatrix); + } +} + + +dgInt32 dgCollisionInstance::CalculatePlaneIntersection (const dgVector& normal, const dgVector& point, dgVector* const contactsOut) const +{ + dgInt32 count = 0; + dgAssert(normal.m_w == dgFloat32 (0.0f)); + switch (m_scaleType) + { + case m_unit: + { + count = m_childShape->CalculatePlaneIntersection (normal, point, contactsOut); + break; + } + case m_uniform: + { + dgVector point1 (m_invScale * point); + count = m_childShape->CalculatePlaneIntersection (normal, point1, contactsOut); + for (dgInt32 i = 0; i < count; i ++) { + contactsOut[i] = m_scale * contactsOut[i]; + } + break; + } + + case m_nonUniform: + { + // support((p * S), n) = S * support (p, n * transp(S)) + dgVector point1 (m_invScale * point); + dgVector normal1 (m_scale * normal); + normal1 = normal1.Normalize(); + count = m_childShape->CalculatePlaneIntersection (normal1, point1, contactsOut); + for (dgInt32 i = 0; i < count; i ++) { + contactsOut[i] = m_scale * contactsOut[i]; + } + break; + } + + case m_global: + default: + { + dgVector point1 (m_aligmentMatrix.UntransformVector (m_invScale * point)); + dgVector normal1 (m_aligmentMatrix.UntransformVector (m_scale * normal)); + normal1 = normal1.Normalize(); + count = m_childShape->CalculatePlaneIntersection (normal1, point1, contactsOut); + for (dgInt32 i = 0; i < count; i ++) { + contactsOut[i] = m_scale * m_aligmentMatrix.TransformVector(contactsOut[i]); + } + } + } + return count; +} + + +void dgCollisionInstance::CalcAABB (const dgMatrix& matrix, dgVector& p0, dgVector& p1) const +{ + switch (m_scaleType) + { + case m_unit: + { + m_childShape->CalcAABB (matrix, p0, p1); + p0 -= m_padding; + p1 += m_padding; + break; + } + + case m_uniform: + case m_nonUniform: + { + dgMatrix matrix1 (matrix); + matrix1[0] = matrix1[0].Scale(m_scale.m_x); + matrix1[1] = matrix1[1].Scale(m_scale.m_y); + matrix1[2] = matrix1[2].Scale(m_scale.m_z); + m_childShape->CalcAABB (matrix1, p0, p1); + p0 -= m_padding; + p1 += m_padding; + break; + } + + case m_global: + default: + { + dgMatrix matrix1 (matrix); + matrix1[0] = matrix1[0].Scale(m_scale.m_x); + matrix1[1] = matrix1[1].Scale(m_scale.m_y); + matrix1[2] = matrix1[2].Scale(m_scale.m_z); + m_childShape->CalcAABB (m_aligmentMatrix * matrix1, p0, p1); + p0 -= m_padding; + p1 += m_padding; + break; + } + } + + dgAssert (p0.m_w == dgFloat32 (0.0f)); + dgAssert (p1.m_w == dgFloat32 (0.0f)); +} + +dgFloat32 dgCollisionInstance::RayCast (const dgVector& localP0, const dgVector& localP1, dgFloat32 maxT, dgContactPoint& contactOut, OnRayPrecastAction preFilter, const dgBody* const body, void* const userData) const +{ + if (!preFilter || preFilter(body, this, userData)) { + switch(m_scaleType) + { + case m_unit: + { + dgFloat32 t = m_childShape->RayCast (localP0, localP1, maxT, contactOut, body, userData, preFilter); + if (t <= maxT) { + if (!(m_childShape->IsType(dgCollision::dgCollisionMesh_RTTI) || m_childShape->IsType(dgCollision::dgCollisionCompound_RTTI))) { + contactOut.m_shapeId0 = GetUserDataID(); + contactOut.m_shapeId1 = GetUserDataID(); + } + if (!m_childShape->IsType(dgCollision::dgCollisionCompound_RTTI)) { + contactOut.m_collision0 = this; + contactOut.m_collision1 = this; + } + } + return t; + } + + case m_uniform: + { + dgVector p0 (localP0 * m_invScale); + dgVector p1 (localP1 * m_invScale); + dgFloat32 t = m_childShape->RayCast (p0, p1, maxT, contactOut, body, userData, preFilter); + if (t <= maxT) { + if (!(m_childShape->IsType(dgCollision::dgCollisionMesh_RTTI) || m_childShape->IsType(dgCollision::dgCollisionCompound_RTTI))) { + contactOut.m_shapeId0 = GetUserDataID(); + contactOut.m_shapeId1 = GetUserDataID(); + } + if (!m_childShape->IsType(dgCollision::dgCollisionCompound_RTTI)) { + contactOut.m_collision0 = this; + contactOut.m_collision1 = this; + } + } + return t; + } + + case m_nonUniform: + { + dgVector p0 (localP0 * m_invScale); + dgVector p1 (localP1 * m_invScale); + dgFloat32 t = m_childShape->RayCast (p0, p1, maxT, contactOut, body, userData, preFilter); + if (t <= maxT) { + if (!(m_childShape->IsType(dgCollision::dgCollisionMesh_RTTI) || m_childShape->IsType(dgCollision::dgCollisionCompound_RTTI))) { + contactOut.m_shapeId0 = GetUserDataID(); + contactOut.m_shapeId1 = GetUserDataID(); + dgVector n (m_invScale * contactOut.m_normal); + contactOut.m_normal = n.Normalize(); + } + if (!m_childShape->IsType(dgCollision::dgCollisionCompound_RTTI)) { + contactOut.m_collision0 = this; + contactOut.m_collision1 = this; + } + } + return t; + } + + case m_global: + default: + { + dgVector p0 (m_aligmentMatrix.UntransformVector (localP0 * m_invScale)); + dgVector p1 (m_aligmentMatrix.UntransformVector (localP1 * m_invScale)); + dgFloat32 t = m_childShape->RayCast (p0, p1, maxT, contactOut, body, userData, preFilter); + if (t <= maxT) { + if (!(m_childShape->IsType(dgCollision::dgCollisionMesh_RTTI) || m_childShape->IsType(dgCollision::dgCollisionCompound_RTTI))) { + contactOut.m_shapeId0 = GetUserDataID(); + contactOut.m_shapeId1 = GetUserDataID(); + dgVector n (m_aligmentMatrix.RotateVector(m_invScale * contactOut.m_normal)); + contactOut.m_normal = n.Normalize(); + } + if (!(m_childShape->IsType(dgCollision::dgCollisionCompound_RTTI))) { + contactOut.m_collision0 = this; + contactOut.m_collision1 = this; + } + } + return t; + } + } + } + return dgFloat32 (1.2f); +} + +void dgCollisionInstance::CalculateImplicitContacts(dgInt32 count, dgContactPoint* const contactPoints) const +{ + switch (m_scaleType) + { + case m_unit: + { + for (dgInt32 i = 0; i < count; i++) { + contactPoints[i].m_point = m_globalMatrix.UntransformVector(contactPoints[i].m_point); + } + m_childShape->CalculateImplicitContacts(count, contactPoints); + for (dgInt32 i = 0; i < count; i++) { + contactPoints[i].m_point = m_globalMatrix.TransformVector(contactPoints[i].m_point); + contactPoints[i].m_normal = m_globalMatrix.RotateVector(contactPoints[i].m_normal); + } + break; + } + + case m_uniform: + { + for (dgInt32 i = 0; i < count; i++) { + contactPoints[i].m_point = m_invScale * m_globalMatrix.UntransformVector(contactPoints[i].m_point); + } + m_childShape->CalculateImplicitContacts(count, contactPoints); + for (dgInt32 i = 0; i < count; i++) { + contactPoints[i].m_point = m_globalMatrix.TransformVector(contactPoints[i].m_point * m_scale); + contactPoints[i].m_normal = m_globalMatrix.RotateVector(contactPoints[i].m_normal); + } + break; + } + + case m_nonUniform: + { + for (dgInt32 i = 0; i < count; i++) { + contactPoints[i].m_point = m_invScale * m_globalMatrix.UntransformVector(contactPoints[i].m_point); + } + m_childShape->CalculateImplicitContacts(count, contactPoints); + for (dgInt32 i = 0; i < count; i++) { + contactPoints[i].m_point = m_globalMatrix.TransformVector(contactPoints[i].m_point * m_scale); + contactPoints[i].m_normal = m_globalMatrix.RotateVector(contactPoints[i].m_normal * m_invScale).Normalize(); + } + break; + } + + case m_global: + default: + { + for (dgInt32 i = 0; i < count; i++) { + contactPoints[i].m_point = m_invScale * m_globalMatrix.UntransformVector(m_aligmentMatrix.UntransformVector(contactPoints[i].m_point)); + } + m_childShape->CalculateImplicitContacts(count, contactPoints); + for (dgInt32 i = 0; i < count; i++) { + contactPoints[i].m_point = m_globalMatrix.TransformVector(m_aligmentMatrix.TransformVector(contactPoints[i].m_point) * m_scale); + contactPoints[i].m_normal = m_globalMatrix.RotateVector(m_aligmentMatrix.RotateVector(contactPoints[i].m_normal) * m_invScale).Normalize(); + } + } + } +} \ No newline at end of file diff --git a/thirdparty/src/newton/dgPhysics/dgCollisionInstance.h b/thirdparty/src/newton/dgPhysics/dgCollisionInstance.h new file mode 100644 index 000000000..64b033365 --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgCollisionInstance.h @@ -0,0 +1,589 @@ +/* Copyright (c) <2003-2019> +* +* 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_COLLISION_INSTANCE_H_ +#define _DG_COLLISION_INSTANCE_H_ + + +#define DG_MAX_COLLISION_AABB_PADDING dgFloat32 (1.0f / 16.0f) + +#include "dgCollision.h" + +class dgCollisionInstance +{ + public: + enum dgScaleType + { + m_unit, + m_uniform, + m_nonUniform, + m_global, + }; + + DG_CLASS_ALLOCATOR(allocator) + dgCollisionInstance(); + dgCollisionInstance(const dgCollisionInstance& instance); + dgCollisionInstance(const dgCollisionInstance& meshInstance, const dgCollision* const shape); + dgCollisionInstance(const dgWorld* const world, const dgCollision* const childCollision, dgInt32 shapeID, const dgMatrix& matrix); + dgCollisionInstance(const dgWorld* const world, dgDeserialize deserialization, void* const userData, dgInt32 revisionNumber); + ~dgCollisionInstance(); + + dgCollisionInstance* AddRef (); + dgInt32 Release (); + + void SetScale (const dgVector& scale); + const dgVector& GetScale () const; + const dgVector& GetInvScale () const; + + void SetGlobalScale (const dgVector& scale); + + dgScaleType GetScaleType() const; + dgScaleType GetCombinedScaleType(dgScaleType type) const; + + const dgMatrix& GetLocalMatrix () const; + const dgMatrix& GetGlobalMatrix () const; + const dgMatrix& GetAlignMatrix () const; + void SetLocalMatrix (const dgMatrix& matrix); + void SetGlobalMatrix (const dgMatrix& matrix); + + dgUnsigned64 GetUserDataID () const; + void SetUserDataID (dgUnsigned64 userData); + + void* GetUserData () const; + void SetUserData (void* const userData); + + dgCollisionInfo::dgInstanceMaterial GetMaterial () const; + void SetMaterial (const dgCollisionInfo::dgInstanceMaterial& userData); + + const void* GetCollisionHandle () const; + const dgCollisionInstance* GetParent () const; + + dgVector GetBoxSize() const; + dgVector GetBoxOrigin() const; + + dgFloat32 GetUmbraClipSize () const; + + const dgWorld* GetWorld() const; + const dgCollision* GetChildShape() const; + + void SetWorld (dgWorld* const world); + void SetChildShape (dgCollision* const shape); + + dgFloat32 GetVolume () const; + void GetCollisionInfo(dgCollisionInfo* const info) const; + + dgInt32 IsType (dgCollision::dgRTTI type) const; + dgMemoryAllocator* GetAllocator() const; + + bool GetCollisionMode() const; + void SetCollisionMode(bool mode); + + void SetBreakImpulse(dgFloat32 force); + dgFloat32 GetBreakImpulse() const; + + dgUnsigned32 GetSignature () const; + dgCollisionID GetCollisionPrimityType () const; + + void CalcObb (dgVector& origin, dgVector& size) const; + void CalcAABB (const dgMatrix& matrix, dgVector& p0, dgVector& p1) const; + dgFloat32 RayCast (const dgVector& localP0, const dgVector& localP1, dgFloat32 maxT, dgContactPoint& contactOut, OnRayPrecastAction preFilter, const dgBody* const body, void* const userData) const; + + dgFloat32 GetBoxMinRadius () const; + dgFloat32 GetBoxMaxRadius () const; + + dgMatrix CalculateInertia () const; + dgMatrix GetScaledTransform(const dgMatrix& matrix) const; + void DebugCollision (const dgMatrix& matrix, dgCollision::OnDebugCollisionMeshCallback callback, void* const userData) const; + + dgVector SupportVertex (const dgVector& dir) const; + dgInt32 CalculatePlaneIntersection (const dgVector& normal, const dgVector& point, dgVector* const contactsOut) const; + + dgInt32 CalculateSignature () const; + void SetCollisionBBox (const dgVector& p0, const dgVector& p1); + + dgInt32 GetConvexVertexCount() const; + + void Serialize(dgSerialize callback, void* const userData, bool saveShape = true) const; + dgVector CalculateBuoyancyVolume (const dgMatrix& matrix, const dgVector& fluidPlane) const; + + dgVector SupportVertexSpecial (const dgVector& dir, dgInt32* const vertexIndex) const; + dgVector SupportVertexSpecialProjectPoint (const dgVector& point, const dgVector& dir) const; + + dgFloat32 GetSkinThickness() const; + void SetSkinThickness(dgFloat32 thickness); + + void CalculateImplicitContacts(dgInt32 count, dgContactPoint* const contactPoints) const; + + dgMatrix m_globalMatrix; + dgMatrix m_localMatrix; + dgMatrix m_aligmentMatrix; + dgVector m_scale; + dgVector m_invScale; + dgVector m_maxScale; + dgCollisionInfo::dgInstanceMaterial m_material; + const dgWorld* m_world; + const dgCollision* m_childShape; + const void* m_subCollisionHandle; + const dgCollisionInstance* m_parent; + dgFloat32 m_skinThickness; + dgInt32 m_collisionMode; + dgInt32 m_refCount; + dgScaleType m_scaleType; + bool m_isExternal; + + static dgVector m_padding; +}; + +DG_INLINE dgCollisionInstance::dgCollisionInstance(const dgCollisionInstance& meshInstance, const dgCollision* const shape) + :m_globalMatrix(meshInstance.m_globalMatrix) + ,m_localMatrix (meshInstance.m_localMatrix) + ,m_aligmentMatrix (meshInstance.m_aligmentMatrix) + ,m_scale(meshInstance.m_scale) + ,m_invScale(meshInstance.m_invScale) + ,m_maxScale(meshInstance.m_maxScale) + ,m_material(meshInstance.m_material) + ,m_world(meshInstance.m_world) + ,m_childShape (shape) + ,m_subCollisionHandle(NULL) + ,m_parent(NULL) + ,m_skinThickness(meshInstance.m_skinThickness) + ,m_collisionMode(meshInstance.m_collisionMode) + ,m_refCount(1) + ,m_scaleType(meshInstance.m_scaleType) + ,m_isExternal(false) +{ + if (m_childShape) { + m_childShape->AddRef(); + } +} + +DG_INLINE dgCollisionInstance* dgCollisionInstance::AddRef () +{ + m_refCount ++; + return this; +} + +DG_INLINE dgInt32 dgCollisionInstance::Release () +{ + m_refCount --; + if (m_refCount) { + return m_refCount; + } + delete this; + return 0; +} + +DG_INLINE dgInt32 dgCollisionInstance::IsType (dgCollision::dgRTTI type) const +{ + return m_childShape->IsType (type); +} + +DG_INLINE const dgWorld* dgCollisionInstance::GetWorld() const +{ + return m_world; +} + +DG_INLINE const dgCollision* dgCollisionInstance::GetChildShape() const +{ + return m_childShape; +} + +DG_INLINE void dgCollisionInstance::SetWorld (dgWorld* const world) +{ + m_world = world; +} + +DG_INLINE void dgCollisionInstance::SetChildShape (dgCollision* const shape) +{ + shape->AddRef(); + if (m_childShape) { + m_childShape->Release(); + } + + m_childShape = shape; +} + +DG_INLINE void dgCollisionInstance::GetCollisionInfo(dgCollisionInfo* const info) const +{ + info->m_offsetMatrix = m_localMatrix; + info->m_collisionMaterial = m_material; + m_childShape->GetCollisionInfo(info); +} + +DG_INLINE const dgVector& dgCollisionInstance::GetScale () const +{ + return m_scale; +} + +DG_INLINE const dgVector& dgCollisionInstance::GetInvScale () const +{ + return m_invScale; +} + +DG_INLINE const dgMatrix& dgCollisionInstance::GetLocalMatrix () const +{ + return m_localMatrix; +} + +DG_INLINE const dgMatrix& dgCollisionInstance::GetGlobalMatrix () const +{ + return m_globalMatrix; +} + +DG_INLINE const dgMatrix& dgCollisionInstance::GetAlignMatrix () const +{ + return m_aligmentMatrix; +} + +DG_INLINE void dgCollisionInstance::SetGlobalMatrix (const dgMatrix& matrix) +{ + m_globalMatrix = matrix; +} + +DG_INLINE dgMemoryAllocator* dgCollisionInstance::GetAllocator() const +{ + return m_childShape->GetAllocator(); +} + +DG_INLINE dgFloat32 dgCollisionInstance::GetVolume () const +{ + return m_childShape->GetVolume() * m_scale.m_x * m_scale.m_y * m_scale.m_z; +} + +DG_INLINE bool dgCollisionInstance::GetCollisionMode() const +{ + return m_collisionMode ? true : false; +} + +DG_INLINE void dgCollisionInstance::SetCollisionMode(bool mode) +{ + m_collisionMode = mode ? 1 : 0; +} + + +DG_INLINE void dgCollisionInstance::SetBreakImpulse(dgFloat32 force) +{ + dgAssert (0); +// m_destructionImpulse = force; +} + +DG_INLINE dgFloat32 dgCollisionInstance::GetBreakImpulse() const +{ +// return m_destructionImpulse; + return dgFloat32 (1.0e20f); +} + +DG_INLINE const void* dgCollisionInstance::GetCollisionHandle () const +{ + return m_subCollisionHandle; +} + +DG_INLINE const dgCollisionInstance* dgCollisionInstance::GetParent () const +{ + return m_parent; +} + +DG_INLINE dgUnsigned64 dgCollisionInstance::GetUserDataID () const +{ + return m_material.m_userId; +} + +DG_INLINE void dgCollisionInstance::SetUserDataID (dgUnsigned64 userDataId) +{ + m_material.m_userId = userDataId; +} + +DG_INLINE void* dgCollisionInstance::GetUserData () const +{ + return m_material.m_userData; +} + +DG_INLINE void dgCollisionInstance::SetUserData (void* const userData) +{ + m_material.m_userData = userData; +} + +DG_INLINE dgCollisionInfo::dgInstanceMaterial dgCollisionInstance::GetMaterial () const +{ + return m_material; +} + +DG_INLINE void dgCollisionInstance::SetMaterial(const dgCollisionInfo::dgInstanceMaterial& userData) +{ + m_material = userData; +} + +DG_INLINE dgUnsigned32 dgCollisionInstance::GetSignature () const +{ + return m_childShape->GetSignature(); +} + +DG_INLINE dgCollisionID dgCollisionInstance::GetCollisionPrimityType () const +{ + return m_childShape->GetCollisionPrimityType(); +} + +DG_INLINE dgFloat32 dgCollisionInstance::GetBoxMinRadius () const +{ + return m_childShape->GetBoxMinRadius() * m_maxScale.m_x; +} + +DG_INLINE dgFloat32 dgCollisionInstance::GetBoxMaxRadius () const +{ + return m_childShape->GetBoxMaxRadius() * m_maxScale.m_x; +} + +DG_INLINE dgVector dgCollisionInstance::SupportVertex(const dgVector& dir) const +{ + dgAssert (dir.m_w == dgFloat32 (0.0f)); + dgAssert (dgAbs(dir.DotProduct(dir).GetScalar() - dgFloat32 (1.0f)) < dgFloat32 (1.0e-2f)); + dgAssert (dir.m_w == dgFloat32 (0.0f)); + switch (m_scaleType) + { + case m_unit: + { + return m_childShape->SupportVertex (dir, NULL); + } + case m_uniform: + { + return m_scale * m_childShape->SupportVertex (dir, NULL); + } + case m_nonUniform: + { + // support((p * S), n) = S * support (p, n * transp(S)) + dgVector dir1 ((m_scale * dir).Normalize()); + return m_scale * m_childShape->SupportVertex (dir1, NULL); + } + + case m_global: + default: + { + dgVector dir1 (m_aligmentMatrix.UnrotateVector((m_scale * dir).Normalize())); + return m_scale * m_aligmentMatrix.TransformVector (m_childShape->SupportVertex (dir1, NULL)); + } + } +} + +DG_INLINE dgVector dgCollisionInstance::SupportVertexSpecial (const dgVector& dir, dgInt32* const vertexIndex) const +{ + dgAssert (dir.m_w == dgFloat32 (0.0f)); + dgAssert(dgAbs(dir.DotProduct(dir).GetScalar() - dgFloat32(1.0f)) < dgFloat32(1.0e-2f)); + dgAssert(dir.m_w == dgFloat32(0.0f)); + switch (m_scaleType) + { + case m_unit: + { + return m_childShape->SupportVertexSpecial(dir, m_skinThickness, vertexIndex); + } + case m_uniform: + { + return m_scale * m_childShape->SupportVertexSpecial(dir, m_skinThickness, vertexIndex); + } + + default: + return SupportVertex(dir); + +/* + case m_nonUniform: + { + // support((p * S), n) = S * support (p, n * transp(S)) + dgVector dir1((m_scale * dir).Normalize()); + return m_scale * m_childShape->SupportVertexSpecial(dir1, m_skinThickness, vertexIndex); + } + + default: + { + dgVector dir1(m_aligmentMatrix.UnrotateVector((m_scale * dir).Normalize())); + return m_scale * m_aligmentMatrix.TransformVector(m_childShape->SupportVertexSpecial(dir1, vertexIndex)); + } +*/ + } +} + +DG_INLINE dgVector dgCollisionInstance::SupportVertexSpecialProjectPoint (const dgVector& point, const dgVector& dir) const +{ + dgAssert(dir.m_w == dgFloat32(0.0f)); + dgAssert(dgAbs(dir.DotProduct(dir).GetScalar() - dgFloat32(1.0f)) < dgFloat32(1.0e-2f)); + switch (m_scaleType) + { + case m_unit: + { + return m_childShape->SupportVertexSpecialProjectPoint(point, dir); + } + case m_uniform: + { + return m_scale * m_childShape->SupportVertexSpecialProjectPoint(point * m_invScale, dir); + } + + default: + return point; + +/* + case m_nonUniform: + { + // support((p * S), n) = S * support (p/S, n * transp(S)) + dgVector dir1((m_scale * dir).Normalize()); + return m_scale * m_childShape->SupportVertexSpecialProjectPoint(point * m_invScale, dir1); + } + + case m_global: + default: + { + dgVector dir1(m_aligmentMatrix.UnrotateVector((m_scale * dir).Normalize())); + return m_scale * m_aligmentMatrix.TransformVector(m_childShape->SupportVertexSpecialProjectPoint(m_aligmentMatrix.UntransformVector(point * m_invScale), dir1)); + } +*/ + } +} + +DG_INLINE void dgCollisionInstance::SetCollisionBBox (const dgVector& p0, const dgVector& p1) +{ + dgAssert (0); +} + +DG_INLINE dgInt32 dgCollisionInstance::CalculateSignature () const +{ + dgAssert (0); + return 0; +} + + +DG_INLINE dgInt32 dgCollisionInstance::GetConvexVertexCount() const +{ + return m_childShape->GetConvexVertexCount(); +} + + +DG_INLINE dgVector dgCollisionInstance::GetBoxSize() const +{ + switch (m_scaleType) + { + case m_unit: + case m_uniform: + case m_nonUniform: + return m_childShape->m_boxSize * m_scale; + + case m_global: + default: + return m_childShape->m_boxSize * m_maxScale; + } +} + +DG_INLINE dgVector dgCollisionInstance::GetBoxOrigin() const +{ + switch (m_scaleType) + { + case m_unit: + case m_uniform: + case m_nonUniform: + return m_childShape->m_boxOrigin * m_scale; + + case m_global: + default: + return m_aligmentMatrix.TransformVector(m_childShape->m_boxOrigin) * m_scale; + } +} + +DG_INLINE dgFloat32 dgCollisionInstance::GetUmbraClipSize () const +{ + return m_childShape->GetUmbraClipSize() * m_maxScale.m_x; +} + +DG_INLINE dgCollisionInstance::dgScaleType dgCollisionInstance::GetScaleType() const +{ + return m_scaleType; +} + +DG_INLINE dgCollisionInstance::dgScaleType dgCollisionInstance::GetCombinedScaleType(dgCollisionInstance::dgScaleType type) const +{ + dgAssert (0); + return dgMax(m_scaleType, type); +} + +DG_INLINE dgMatrix dgCollisionInstance::GetScaledTransform(const dgMatrix& matrix) const +{ + dgMatrix scaledMatrix(m_localMatrix * matrix); + scaledMatrix[0] = scaledMatrix[0].Scale(m_scale[0]); + scaledMatrix[1] = scaledMatrix[1].Scale(m_scale[1]); + scaledMatrix[2] = scaledMatrix[2].Scale(m_scale[2]); + return m_aligmentMatrix * scaledMatrix; +} + +DG_INLINE void dgCollisionInstance::CalcObb (dgVector& origin, dgVector& size) const +{ + size = m_childShape->GetObbSize(); + origin = m_childShape->GetObbOrigin(); + + switch (m_scaleType) + { + case m_unit: + { + size += m_padding; + break; + } + + case m_uniform: + case m_nonUniform: + { + size = size * m_scale + m_padding; + origin = origin * m_scale; + break; + } + case m_global: + { +// dgMatrix matrix1 (matrix); +// matrix1[0] = matrix1[0].Scale(m_scale.m_x); +// matrix1[1] = matrix1[1].Scale(m_scale.m_y); +// matrix1[2] = matrix1[2].Scale(m_scale.m_z); +// m_childShape->CalcAABB (m_aligmentMatrix * matrix1, p0, p1); +// p0 -= m_padding; +// p1 += m_padding; + + dgVector p0; + dgVector p1; + m_childShape->CalcAABB(m_aligmentMatrix, p0, p1); + size = (dgVector::m_half * (p1 - p0) * m_scale + m_padding) & dgVector::m_triplexMask; + origin = (dgVector::m_half * (p1 + p0) * m_scale) & dgVector::m_triplexMask;; + break; + } + } + + dgAssert (size.m_w == dgFloat32 (0.0f)); + dgAssert (origin.m_w == dgFloat32 (0.0f)); +} + +DG_INLINE dgFloat32 dgCollisionInstance::GetSkinThickness() const +{ + return m_skinThickness; +} + +DG_INLINE void dgCollisionInstance::SetSkinThickness(dgFloat32 thickness) +{ + m_skinThickness = dgAbs (thickness); +} + +DG_INLINE dgVector dgCollisionInstance::CalculateBuoyancyVolume(const dgMatrix& matrix, const dgVector& fluidPlane) const +{ + return m_childShape->CalculateVolumeIntegral(m_localMatrix * matrix, fluidPlane, *this); +} + +#endif + diff --git a/thirdparty/src/newton/dgPhysics/dgCollisionLumpedMassParticles.cpp b/thirdparty/src/newton/dgPhysics/dgCollisionLumpedMassParticles.cpp new file mode 100644 index 000000000..b51dd9f9a --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgCollisionLumpedMassParticles.cpp @@ -0,0 +1,345 @@ +/* Copyright (c) <2003-2019> +* +* 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 "dgPhysicsStdafx.h" +#include "dgWorld.h" +#include "dgContact.h" +#include "dgCollision.h" +#include "dgMeshEffect.h" +#include "dgDynamicBody.h" +#include "dgCollisionLumpedMassParticles.h" + + +#define DG_MINIMIM_ZERO_SPEED dgFloat32 (1.0e-3f) +#define DG_MINIMIM_PARTCLE_RADIUS dgFloat32 (1.0f/16.0f) +#define DG_MINIMIM_ZERO_SURFACE (DG_MINIMIM_PARTCLE_RADIUS * dgFloat32 (0.25f)) + + +dgCollisionLumpedMassParticles::dgCollisionLumpedMassParticles(dgWorld* const world, dgCollisionID collisionID) + :dgCollisionConvex(world->GetAllocator(), 0, collisionID) + ,m_posit(world->GetAllocator()) + ,m_veloc(world->GetAllocator()) + ,m_accel(world->GetAllocator()) + ,m_externalAccel(world->GetAllocator()) + ,m_mass(world->GetAllocator()) + ,m_invMass(world->GetAllocator()) + ,m_body(NULL) + ,m_totalMass(dgFloat32(1.0f)) + ,m_particleRadius(DG_MINIMIM_PARTCLE_RADIUS) + ,m_particlesCount(0) +{ + m_rtti |= dgCollisionLumpedMass_RTTI; +} + +dgCollisionLumpedMassParticles::dgCollisionLumpedMassParticles (const dgCollisionLumpedMassParticles& source) + :dgCollisionConvex(source) + ,m_posit(source.m_posit, source.m_particlesCount) + ,m_veloc(source.m_veloc, source.m_particlesCount) + ,m_accel(source.m_accel, source.m_particlesCount) + ,m_externalAccel(source.m_externalAccel, source.m_particlesCount) + ,m_mass(source.m_mass, source.m_particlesCount) + ,m_invMass(source.m_invMass, source.m_particlesCount) + ,m_body(NULL) + ,m_totalMass(source.m_totalMass) + ,m_particleRadius(source.m_particleRadius) + ,m_particlesCount(source.m_particlesCount) +{ + m_rtti |= dgCollisionLumpedMass_RTTI; +} + + +dgCollisionLumpedMassParticles::dgCollisionLumpedMassParticles (dgWorld* const world, dgDeserialize deserialization, void* const userData, dgInt32 revisionNumber) + :dgCollisionConvex(world, deserialization, userData, revisionNumber) + ,m_posit(world->GetAllocator()) + ,m_veloc(world->GetAllocator()) + ,m_accel(world->GetAllocator()) + ,m_externalAccel(world->GetAllocator()) + ,m_mass(world->GetAllocator()) + ,m_invMass(world->GetAllocator()) + ,m_body(NULL) + ,m_totalMass(dgFloat32(1.0f)) + ,m_particleRadius (DG_MINIMIM_PARTCLE_RADIUS) + ,m_particlesCount(0) +{ + m_rtti |= dgCollisionLumpedMass_RTTI; + dgAssert (0); +} + +dgCollisionLumpedMassParticles::~dgCollisionLumpedMassParticles(void) +{ +} + +void dgCollisionLumpedMassParticles::FinalizeBuild() +{ + m_veloc.Resize(m_particlesCount); + m_accel.Resize(m_particlesCount); + m_externalAccel.Resize(m_particlesCount); + + dgVector com(dgFloat32(0.0f)); + dgVector* const posit = &m_posit[0]; + + dgVector minp(dgFloat32 (1.0e10f)); + dgVector maxp(dgFloat32 (-1.0e10f)); + for (dgInt32 i = 0; i < m_particlesCount; i++) { + minp = minp.GetMin(posit[i]); + maxp = maxp.GetMax(posit[i]); + m_accel[i] = dgVector::m_zero; + m_veloc[i] = dgVector::m_zero; + m_externalAccel[i] = dgVector::m_zero; + } + + // for now use a fix size box + m_boxSize = dgVector::m_half * (maxp - minp); + m_boxOrigin = dgVector::m_half * (maxp + minp); +} + +void dgCollisionLumpedMassParticles::DebugCollision (const dgMatrix& matrix, dgCollision::OnDebugCollisionMeshCallback callback, void* const userData) const +{ + dgAssert (0); +} + +void dgCollisionLumpedMassParticles::SetOwnerAndMassPraperties (dgDynamicBody* const body) +{ + m_body = body; + + dgMatrix matrix (body->GetMatrix()); + dgVector position (matrix.m_posit); + matrix.m_posit = dgVector::m_wOne; + + dgVector* const posit = &m_posit[0]; + const dgFloat32* const mass = &m_mass[0]; + + dgVector xMassSum(dgFloat32(0.0f)); +// dgVector xyMassSum(dgFloat32(0.0f)); +// dgVector xxMassSum(dgFloat32(0.0f)); + dgFloat32 massSum = dgFloat32 (0.0f); +// dgFloat32 inertiaSum = dgFloat32 (0.0f); +// dgFloat32 radius2 = m_particleRadius * m_particleRadius * dgFloat32 (2.0f / 5.0f); + dgMatrix scaledTranform (body->m_collision->GetScaledTransform(matrix)); + for (dgInt32 i = 0; i < m_particlesCount; i++) { + massSum += mass[i]; + //inertiaSum += mass[i] * radius2; + posit[i] = scaledTranform.TransformVector(posit[i]) & dgVector::m_triplexMask; + xMassSum += posit[i].Scale (mass[i]); + //xxMassSum += posit[i] * posit[i].Scale (mass[i]); + //xyMassSum += posit[i] * (posit[i].ShiftTripleRight()).Scale (mass[i]); + } + m_totalMass = massSum; + dgFloat32 invMass = dgFloat32(1.0f) / massSum; + + body->m_collision->SetScale(dgVector (dgFloat32 (1.0f))); + body->m_collision->SetLocalMatrix (dgGetIdentityMatrix()); + matrix.m_posit = position; + body->m_matrix = matrix; + body->m_localCentreOfMass = xMassSum * invMass; + + //dgVector inertia (xxMassSum.Scale(invMass) - body->m_localCentreOfMass); + //inertia += dgVector (inertiaSum); + //inertia.m_w = massSum; + //body->m_mass = inertia; + body->m_mass = dgVector(dgFloat32(1.0f), dgFloat32(1.0f), dgFloat32(1.0f), m_totalMass); + body->m_invMass = dgVector(dgFloat32 (1.0f), dgFloat32 (1.0f), dgFloat32 (1.0f), invMass); + +// dgVector yySum(xxSum.ShiftTripleRight()); +// dgVector com(xxSum * den + origin); +// dgVector pxx0(origin - com); +// dgVector pxy0(pxx0.ShiftTripleRight()); +// dgVector Ixx(unitMass * (xxSum2 + xxSum * pxx0 * dgVector::m_two) + pxx0 * pxx0.Scale(m_body->m_mass.m_w)); +// dgVector Ixy(unitMass * (xySum + xxSum * pxy0 + yySum * pxx0) + pxx0 * pxy0.Scale(m_body->m_mass.m_w)); +// dgVector com2(body->m_localCentreOfMass * body->m_localCentreOfMass); +} + +int dgCollisionLumpedMassParticles::GetCount() const +{ + return m_particlesCount; +} + +dgInt32 dgCollisionLumpedMassParticles::GetStrideInByte() const +{ + return sizeof (dgVector); +} + +const dgVector* dgCollisionLumpedMassParticles::GetVelocity() const +{ + return &m_veloc[0]; +} + +const dgVector* dgCollisionLumpedMassParticles::GetPositions() const +{ + return &m_posit[0]; +} + +const dgVector* dgCollisionLumpedMassParticles::GetAcceleration() const +{ + return &m_accel[0]; +} + +dgInt32 dgCollisionLumpedMassParticles::CalculateSignature() const +{ + dgAssert (0); + return 0; +} + +void dgCollisionLumpedMassParticles::SetCollisionBBox(const dgVector& p0, const dgVector& p1) +{ + dgAssert (0); +} + +void dgCollisionLumpedMassParticles::Serialize(dgSerialize callback, void* const userData) const +{ + dgAssert (0); +} + +void dgCollisionLumpedMassParticles::RegisterCollision(const dgBody* const otherBody) +{ +// dgAssert (0); +} + +void dgCollisionLumpedMassParticles::CalcAABB(const dgMatrix& matrix, dgVector& p0, dgVector& p1) const +{ + dgVector origin(matrix.TransformVector(m_boxOrigin)); + dgVector size(matrix.m_front.Abs().Scale(m_boxSize.m_x) + matrix.m_up.Abs().Scale(m_boxSize.m_y) + matrix.m_right.Abs().Scale(m_boxSize.m_z)); + p0 = (origin - size) & dgVector::m_triplexMask; + p1 = (origin + size) & dgVector::m_triplexMask; +} + +dgFloat32 dgCollisionLumpedMassParticles::RayCast(const dgVector& localP0, const dgVector& localP1, dgFloat32 maxT, dgContactPoint& contactOut, const dgBody* const body, void* const userData, OnRayPrecastAction preFilter) const +{ + // for now brute force ray cast + dgVector p (m_posit[0]); + dgFloat32 distance2 = dgFloat32 (0.1f); + for (dgInt32 i = 0; i < m_particlesCount; i ++) { + dgVector posit (dgPointToRayDistance (m_posit[i], localP0, localP1)); + dgVector step (posit - m_posit[i]); + dgAssert (step.m_w == dgFloat32 (0.0f)); + dgFloat32 dist2 = step.DotProduct(step).GetScalar(); + if (dist2 < distance2) { + distance2 = dist2; + p = m_posit[i]; + } + } + + dgFloat32 dist = dgFloat32(1.0e10f); + if (distance2 < dgFloat32(0.1f)) { + contactOut.m_point = p; + contactOut.m_normal = dgVector(dgFloat32(0.0f), dgFloat32(1.0f), dgFloat32(0.0f), dgFloat32(0.0f)); + dgVector num(p - localP0); + dgVector den(localP1 - localP0); + dgAssert (den.m_w == dgFloat32 (0.0f)); + dist = num.DotProduct(den).GetScalar() / den.DotProduct(den).GetScalar(); + } + return dist; +} + +dgMatrix dgCollisionLumpedMassParticles::CalculateInertiaAndCenterOfMass(const dgMatrix& m_alignMatrix, const dgVector& localScale, const dgMatrix& matrix) const +{ +dgAssert (0); + dgVector com(dgFloat32(0.0f)); + for (dgInt32 i = 0; i < m_particlesCount; i++) { + com = matrix.RotateVector(m_posit[i] * localScale); + } + dgVector den(dgFloat32(1.0f / m_particlesCount)); + dgMatrix inertia(dgGetIdentityMatrix()); + inertia.m_posit = com * den; + inertia.m_posit.m_w = dgFloat32(1.0f); + return inertia; +} + +/* +dgFloat32 dgCollisionLumpedMassParticles::CalculaleContactPenetration(const dgVector& point, const dgVector& normal) const +{ + dgVector otherPoint (point); + otherPoint.m_y = dgFloat32 (0.0f); + dgFloat32 penetration = normal.DotProduct(point - otherPoint).GetScalar(); + return penetration; +} +*/ + +dgVector dgCollisionLumpedMassParticles::CalculateContactNormalAndPenetration(const dgVector& worldPosition) const +{ + dgVector contactNormal(dgFloat32(0.0f), dgFloat32(1.0f), dgFloat32(0.0f), dgFloat32(0.0f)); + dgVector otherPoint(worldPosition); + otherPoint.m_y = dgFloat32(0.0f); + dgVector point(worldPosition - contactNormal.Scale (m_particleRadius)); + contactNormal.m_w = contactNormal.DotProduct(otherPoint - point).GetScalar(); + + return contactNormal; +} + +void dgCollisionLumpedMassParticles::HandleCollision(dgFloat32 timestep, dgVector* const normalDir, dgVector* const normalAccel, dgFloat32* const frictionCoefficient) +{ + const dgMatrix& matrix = m_body->GetCollision()->GetGlobalMatrix(); + dgVector origin(matrix.m_posit); + + dgFloat32 coeficientOfFriction = dgFloat32(0.6f); + dgFloat32 coeficientOfPenetration = dgFloat32(0.1f); +coeficientOfPenetration *= 1.0f; + + dgVector timestepV(timestep); + dgFloat32 invTimeStep = dgFloat32 (1.0f) / timestep; + dgVector* const veloc = &m_veloc[0]; + const dgVector* const accel = &m_accel[0]; + const dgVector* const posit = &m_posit[0]; + const dgVector* const extAccel = &m_externalAccel[0]; + +static int xxx; +xxx ++; +//dgTrace (("%d %f %f\n", xxx, externAccel[0].m_y, veloc[0].m_y)); + + for (dgInt32 i = 0; i < m_particlesCount; i++) { + dgVector normal(dgVector::m_zero); + dgVector accel1(dgVector::m_zero); + dgVector tangent0(dgVector::m_zero); + dgVector tangent1(dgVector::m_zero); + + dgVector contactPosition(origin + posit[i]); + dgVector contactNormal(CalculateContactNormalAndPenetration (contactPosition)); + + dgFloat32 frictionCoef = dgFloat32(0.0f); + if (contactNormal.m_w > dgFloat32 (0.0f)) { + dgVector projectedVelocity(veloc[i] + timestepV * (accel[i] + extAccel[i])); + dgAssert (projectedVelocity.m_w == dgFloat32 (0.0f)); + dgFloat32 projectedNormalSpeed = contactNormal.DotProduct(projectedVelocity).GetScalar(); + if (projectedNormalSpeed < DG_MINIMIM_ZERO_SPEED) { + if (contactNormal.m_w > DG_MINIMIM_ZERO_SURFACE) { + normal = contactNormal & dgVector::m_triplexMask; + dgFloat32 penetration = dgClamp(contactNormal.m_w - DG_MINIMIM_ZERO_SURFACE, dgFloat32(0.0f), dgFloat32(0.125f)); + dgFloat32 s = dgFloat32 (2.0f) * invTimeStep * penetration; + dgFloat32 a = -s * invTimeStep; + veloc[i] += normal.Scale (s - veloc[i].DotProduct(normal).GetScalar()); + //dgVector penetrationSpeed(invTimeStep.Scale(coeficientOfPenetration * penetration)); + //dgVector normalSpeed(normal.DotProduct(veloc[i] * dgVector::m_negOne)); + //dgVector restoringSpeed(normalSpeed.GetMax(penetrationSpeed)); + //dgVector normalVelocity(normal * restoringSpeed); + //accel1 = invTimeStep * normalVelocity; + accel1 = normal.Scale(a); + frictionCoef = coeficientOfFriction; + } + } + } + + normalDir[i] = normal; + normalAccel[i] = accel1; + frictionCoefficient[i] = frictionCoef; + } +} + + diff --git a/thirdparty/src/newton/dgPhysics/dgCollisionLumpedMassParticles.h b/thirdparty/src/newton/dgPhysics/dgCollisionLumpedMassParticles.h new file mode 100644 index 000000000..7736ba5a6 --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgCollisionLumpedMassParticles.h @@ -0,0 +1,88 @@ +/* Copyright (c) <2003-2019> +* +* 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 __DGCOLLISION_LUMPED_MASS_PARTICLES_H__ +#define __DGCOLLISION_LUMPED_MASS_PARTICLES_H__ + + +#include "dgCollision.h" +#include "dgCollisionConvex.h" + + +class dgCollisionLumpedMassParticles: public dgCollisionConvex +{ + public: + dgCollisionLumpedMassParticles (const dgCollisionLumpedMassParticles& source); + dgCollisionLumpedMassParticles (dgWorld* const world, dgCollisionID collisionID); + dgCollisionLumpedMassParticles (dgWorld* const world, dgDeserialize deserialization, void* const userData, dgInt32 revisionNumber); + virtual ~dgCollisionLumpedMassParticles(void); + + dgInt32 GetCount() const; + dgInt32 GetStrideInByte() const; + const dgVector* GetVelocity() const; + const dgVector* GetPositions() const; + const dgVector* GetAcceleration() const; + + dgDynamicBody* GetOwner () const; + void SetOwnerAndMassPraperties (dgDynamicBody* const body); + virtual void IntegrateForces (dgFloat32 timestep) = 0; + + protected: + virtual void FinalizeBuild(); + virtual dgInt32 CalculateSignature() const; + virtual void RegisterCollision(const dgBody* const otherBody); + virtual void SetCollisionBBox(const dgVector& p0, const dgVector& p1); + virtual void Serialize(dgSerialize callback, void* const userData) const; + virtual void CalcAABB(const dgMatrix& matrix, dgVector& p0, dgVector& p1) const; + virtual dgMatrix CalculateInertiaAndCenterOfMass(const dgMatrix& m_alignMatrix, const dgVector& localScale, const dgMatrix& matrix) const; + + virtual void DebugCollision (const dgMatrix& matrix, dgCollision::OnDebugCollisionMeshCallback callback, void* const userData) const; + dgFloat32 RayCast(const dgVector& localP0, const dgVector& localP1, dgFloat32 maxT, dgContactPoint& contactOut, const dgBody* const body, void* const userData, OnRayPrecastAction preFilter) const; + + //dgFloat32 CalculaleContactPenetration(const dgVector& point, const dgVector& normal) const; + dgVector CalculateContactNormalAndPenetration(const dgVector& worldPosition) const; + virtual void HandleCollision (dgFloat32 timestep, dgVector* const normalDir, dgVector* const normalAccel, dgFloat32* const frictionCoefficient); + + virtual dgInt32 GetMemoryBufferSizeInBytes() const = 0; + + dgArray m_posit; + dgArray m_veloc; + dgArray m_accel; + dgArray m_externalAccel; + dgArray m_mass; + dgArray m_invMass; + dgDynamicBody* m_body; + dgFloat32 m_totalMass; + dgFloat32 m_particleRadius; + dgInt32 m_particlesCount; + + friend class dgBroadPhase; + friend class dgDynamicBody; + friend class dgWorldDynamicUpdate; +}; + +inline dgDynamicBody* dgCollisionLumpedMassParticles::GetOwner () const +{ + return m_body; +} +#endif + diff --git a/thirdparty/src/newton/dgPhysics/dgCollisionMassSpringDamperSystem.cpp b/thirdparty/src/newton/dgPhysics/dgCollisionMassSpringDamperSystem.cpp new file mode 100644 index 000000000..6e5ce0b44 --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgCollisionMassSpringDamperSystem.cpp @@ -0,0 +1,385 @@ +/* Copyright (c) <2003-2019> +* +* 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 "dgPhysicsStdafx.h" +#include "dgBody.h" +#include "dgWorld.h" +#include "dgContact.h" +#include "dgMeshEffect.h" +#include "dgDynamicBody.h" +#include "dgCollisionMassSpringDamperSystem.h" + + +dgCollisionMassSpringDamperSystem::dgCollisionMassSpringDamperSystem (dgWorld* const world, dgInt32 shapeID, dgInt32 pointCount, const dgFloat32* const points, dgInt32 strideInBytes, const dgFloat32* const pointsMasses, dgInt32 linksCount, const dgInt32* const links, const dgFloat32* const linksSpring, const dgFloat32* const LinksDamper) + :dgCollisionDeformableMesh(world, m_deformableSolidMesh) +{ + m_rtti |= dgCollisionMassSpringDamperSystem_RTTI; + + m_particlesCount = pointCount; + m_posit.Resize(m_particlesCount); + m_mass.Resize(m_particlesCount); + m_invMass.Resize(m_particlesCount); + const dgInt32 stride = strideInBytes / sizeof (dgFloat32); + m_totalMass = dgFloat32(0.0f); + for (dgInt32 i = 0; i < pointCount; i++) { + m_totalMass += m_totalMass; + m_mass[i] = pointsMasses[i]; + m_invMass[i] = dgFloat32(1.0f / pointsMasses[i]); + m_posit[i] = dgVector(points[i * stride + 0], points[i * stride + 1], points[i * stride + 2], dgFloat32(0.0f)); + } + + m_linksCount = linksCount; + m_linkList.Resize(linksCount); + for (dgInt32 i = 0; i < linksCount; i++) { + dgInt32 v0 = links[i * 2 + 0]; + dgInt32 v1 = links[i * 2 + 1]; + dgAssert(v0 != v1); + dgAssert(v0 >= 0); + dgAssert(v1 >= 0); + dgAssert(v0 < pointCount); + dgAssert(v1 < pointCount); + m_linkList[i].m_m0 = dgInt16(dgMin(v0, v1)); + m_linkList[i].m_m1 = dgInt16(dgMax(v0, v1)); + m_linkList[i].m_spring = linksSpring[i]; + m_linkList[i].m_damper = LinksDamper[i]; + + const dgVector& p0 = m_posit[v0]; + const dgVector& p1 = m_posit[v1]; + dgVector dp(p0 - p1); + dgAssert (dp.m_w == dgFloat32 (0.0f)); + m_linkList[i].m_restlength = dgSqrt(dp.DotProduct(dp).GetScalar()); + dgAssert(m_linkList[i].m_restlength > dgFloat32(1.0e-2f)); + } + + FinalizeBuild(); +} + + +dgCollisionMassSpringDamperSystem::dgCollisionMassSpringDamperSystem(const dgCollisionMassSpringDamperSystem& source) + :dgCollisionDeformableMesh(source) +{ + m_rtti |= source.m_rtti; +} + +dgCollisionMassSpringDamperSystem::dgCollisionMassSpringDamperSystem(dgWorld* const world, dgDeserialize deserialization, void* const userData, dgInt32 revisionNumber) + :dgCollisionDeformableMesh(world, deserialization, userData, revisionNumber) +{ +} + +dgCollisionMassSpringDamperSystem::~dgCollisionMassSpringDamperSystem(void) +{ +} + +dgInt32 dgCollisionMassSpringDamperSystem::GetMemoryBufferSizeInBytes() const +{ + dgInt32 sizeInByte = 0; + sizeInByte += 3 * m_particlesCount * sizeof (dgVector); + sizeInByte += 1 * m_particlesCount * sizeof (dgFloat32); + return sizeInByte; +} + +#if 0 +void dgCollisionMassSpringDamperSystem::CalculateAcceleration(dgFloat32 timestep) +{ + // Ks is in [sec^-2] a spring constant unit acceleration, not a spring force acceleration. + // Kc is in [sec^-1] a damper constant unit velocity, not a damper force acceleration. + + dgInt32 iter = 4; + dgVector* const accel = &m_accel[0]; + dgVector* const veloc = &m_veloc[0]; + dgVector* const posit = &m_posit[0]; + dgVector* const extAccel = &m_externalAccel[0]; + const dgSpringDamperLink* const links = &m_linkList[0]; + + dgVector* const dx = dgAlloca(dgVector, m_linksCount); + dgVector* const dv = dgAlloca(dgVector, m_linksCount); + dgVector* const dpdv = dgAlloca(dgVector, m_linksCount); + + dgVector* const normalDir = dgAlloca(dgVector, m_particlesCount); + dgVector* const normalAccel = dgAlloca(dgVector, m_particlesCount); + dgFloat32* const spring_A01 = dgAlloca(dgFloat32, m_linksCount); + dgFloat32* const spring_B01 = dgAlloca(dgFloat32, m_linksCount); + dgFloat32* const frictionCoeffecient = dgAlloca(dgFloat32, m_particlesCount); + + // dgFloat32* const damper_A01 = dgAlloca(dgFloat32, m_linksCount); + // dgFloat32* const damper_B01 = dgAlloca(dgFloat32, m_linksCount); + // dgFloat32* const damper_C01 = dgAlloca(dgFloat32, m_linksCount); + // dgVector* const collisionDir1 = dgAlloca(dgVector, m_particlesCount); + // dgVector* const collisionDir2 = dgAlloca(dgVector, m_particlesCount); + // dgVector* const tmp1 = dgAlloca(dgVector, m_particlesCount); + // dgVector* const tmp2 = dgAlloca(dgVector, m_particlesCount); + // dgVector* const diag = dgAlloca(dgVector, m_particlesCount); + // dgVector* const offDiag = dgAlloca(dgVector, m_particlesCount); + + dgVector unitAccel(m_body->m_externalForce * Scale (m_body->m_invMass.m_w)); + dgVector deltaOmega(m_body->m_invWorldInertiaMatrix.RotateVector(m_body->m_externalTorque.Scale(timestep))); + + m_body->m_alpha = dgVector::m_zero; + m_body->m_omega = dgVector::m_zero; + m_body->m_externalForce = dgVector::m_zero; + m_body->m_externalTorque = dgVector::m_zero; + + // here I need to add all other external acceleration like wind and pressure, friction and collision. + for (dgInt32 i = 0; i < m_particlesCount; i++) { + extAccel[i] = unitAccel; + } + + //dgFloat32 ks_dt = -timestep * kSpring; + //dgFloat32 kd_dt0 = -timestep * kDamper; + //dgFloat32 kd_dt1 = -timestep * kDamper * dgFloat32(2.0f); + + dgVector dtRK4(timestep / iter); + dgVector epsilon(dgFloat32(1.0e-14f)); + + dtRK4 = dtRK4 & dgVector::m_triplexMask; + HandleCollision(timestep, normalDir, normalAccel, frictionCoeffecient); + for (dgInt32 k = 0; k < iter; k++) { + + for (dgInt32 i = 0; i < m_particlesCount; i++) { + accel[i] = dgVector::m_zero; + } + + for (dgInt32 i = 0; i < m_linksCount; i++) { + const dgInt32 j0 = links[i].m_m0; + const dgInt32 j1 = links[i].m_m1; + dv[i] = veloc[j0] - veloc[j1]; + dx[i] = posit[j0] - posit[j1]; + + const dgVector p0p1(dx[i]); + const dgVector v0v1(dv[i]); + const dgVector length2(p0p1.DotProduct(p0p1)); + const dgVector mask(length2 > m_smallestLenght2); + + const dgVector lenght2((length2 & mask) | length2.And__Not(mask)); + const dgFloat32 length = (lenght2.Sqrt()).GetScalar(); + const dgFloat32 den = dgFloat32(1.0f) / length; + const dgFloat32 lenghtRatio = links[i].m_restlength * den; + const dgFloat32 compression = dgFloat32(1.0f) - lenghtRatio; + const dgVector fs(p0p1.Scale(links[i].m_spring * compression)); + const dgVector fd(p0p1.Scale(links[i].m_damper * den * den * (v0v1.DotProduct(p0p1)).GetScalar())); + + dgAssert(fs.m_w == dgFloat32(0.0f)); + dgAssert(fs.m_w == dgFloat32(0.0f)); + dgAssert(p0p1.m_w == dgFloat32(0.0f)); + + dpdv[i] = p0p1 * v0v1; + accel[j0] -= (fs + fd); + accel[j1] += (fs + fd); + + dgFloat32 ks_dt = -dtRK4.GetScalar() * links[i].m_spring; + spring_A01[i] = ks_dt * compression; + spring_B01[i] = ks_dt * lenghtRatio * den * den; + } + + for (dgInt32 i = 0; i < m_linksCount; i++) { + const dgVector dv0(dv[i]); + const dgVector A01(spring_A01[i]); + const dgVector B01(spring_B01[i]); + const dgVector dfdx(A01 * dv0 + B01 * dx[i] * dpdv[i]); + + const dgInt32 j0 = links[i].m_m0; + const dgInt32 j1 = links[i].m_m1; + + dgAssert(dfdx.m_w == dgFloat32(0.0f)); + accel[j0] += dfdx; + accel[j1] -= dfdx; + } + + for (dgInt32 i = 0; i < m_particlesCount; i++) { + //dgVector dirAccel1 (collisionDir1[i] * accel[i].DotProduct(collisionDir1[i])); + //dgVector dirAccel2 (collisionDir2[i] * accel[i].DotProduct(collisionDir2[i])); + //tmp0[i] = accel[i] + collidingAccel[i] - dirAccel0 - dirAccel1 - dirAccel2; + + dgVector netAccel (accel[i] + extAccel[i]); + dgVector tangentDir(veloc[i] - normalDir[i] * normalDir[i].DotProduct(veloc[i])); + dgVector mag(tangentDir.DotProduct(tangentDir) + epsilon); + + dgFloat32 tangentFrictionAccel = dgAbs(netAccel.DotProduct(normalDir[i]).GetScalar()); + dgVector friction(tangentDir.Scale(frictionCoeffecient[i] * tangentFrictionAccel / dgSqrt(mag.GetScalar()))); + + //dgVector particleAccel (accel[i] + normalAccel[i] - normalDirAccel); + dgVector normalDirAccel(normalDir[i] * netAccel.DotProduct(normalDir[i])); + //accel[i] = accel[i] + normalAccel[i] - normalDirAccel - friction; + netAccel = netAccel + normalAccel[i] - normalDirAccel - friction; + veloc[i] += netAccel * dtRK4; + posit[i] += veloc[i] * dtRK4; + } + } +} + + +#else +void dgCollisionMassSpringDamperSystem::CalculateAcceleration(dgFloat32 timestep) +{ + // Ks is in [sec^-2] a spring constant unit acceleration, not a spring force acceleration. + // Kc is in [sec^-1] a damper constant unit velocity, not a damper force acceleration. + + dgInt32 iter = 4; + dgVector* const accel = &m_accel[0]; + dgVector* const veloc = &m_veloc[0]; + dgVector* const posit = &m_posit[0]; + dgVector* const extAccel = &m_externalAccel[0]; + const dgSpringDamperLink* const links = &m_linkList[0]; +/* + + dgVector* const dx = dgAlloca(dgVector, m_linksCount); + dgVector* const dv = dgAlloca(dgVector, m_linksCount); + dgVector* const dpdv = dgAlloca(dgVector, m_linksCount); + + dgVector* const normalDir = dgAlloca(dgVector, m_particlesCount); + dgVector* const normalAccel = dgAlloca(dgVector, m_particlesCount); + dgFloat32* const spring_A01 = dgAlloca(dgFloat32, m_linksCount); + dgFloat32* const spring_B01 = dgAlloca(dgFloat32, m_linksCount); + dgFloat32* const frictionCoeffecient = dgAlloca(dgFloat32, m_particlesCount); +*/ + // dgFloat32* const damper_A01 = dgAlloca(dgFloat32, m_linksCount); + // dgFloat32* const damper_B01 = dgAlloca(dgFloat32, m_linksCount); + // dgFloat32* const damper_C01 = dgAlloca(dgFloat32, m_linksCount); + // dgVector* const collisionDir1 = dgAlloca(dgVector, m_particlesCount); + // dgVector* const collisionDir2 = dgAlloca(dgVector, m_particlesCount); + // dgVector* const tmp1 = dgAlloca(dgVector, m_particlesCount); + // dgVector* const tmp2 = dgAlloca(dgVector, m_particlesCount); + // dgVector* const diag = dgAlloca(dgVector, m_particlesCount); + // dgVector* const offDiag = dgAlloca(dgVector, m_particlesCount); + //dgVector deltaOmega(m_body->m_invWorldInertiaMatrix.RotateVector(m_body->m_externalTorque.Scale(timestep))); + + dgWorld* const world = m_body->GetWorld(); + world->m_solverJacobiansMemory.ResizeIfNecessary(GetMemoryBufferSizeInBytes() + 1024); + + dgVector* const normalAccel = (dgVector*)&world->m_solverJacobiansMemory[0]; + dgVector* const normalDir = &normalAccel[m_particlesCount]; + dgVector* const diagonal = &normalDir[m_particlesCount]; + dgFloat32* const frictionCoeffecient = (dgFloat32*)&diagonal[m_particlesCount]; + + dgVector unitAccel(m_body->m_externalForce * dgVector(m_body->m_invMass.m_w)); + + // here I need to add all other external acceleration like wind and pressure, friction and collision. + for (dgInt32 i = 0; i < m_particlesCount; i++) { + extAccel[i] = unitAccel; + } + + dgAssert(m_body->IsRTTIType(dgBody::m_dynamicBodyRTTI)); + m_body->m_alpha = dgVector::m_zero; + m_body->m_omega = dgVector::m_zero; + m_body->m_externalForce = dgVector::m_zero; + m_body->m_externalTorque = dgVector::m_zero; + + //dgFloat32 ks_dt = -timestep * kSpring; + //dgFloat32 kd_dt0 = -timestep * kDamper; + //dgFloat32 kd_dt1 = -timestep * kDamper * dgFloat32(2.0f); + + //dgFloat32 dtRK4__ = timestep / iter; + dgVector dtRK4 (timestep / iter); + dgVector epsilon(dgFloat32(1.0e-14f)); + +// dtRK4 = dtRK4 & dgVector::m_triplexMask; + HandleCollision(timestep, normalDir, normalAccel, frictionCoeffecient); + for (dgInt32 k = 0; k < iter; k++) { + for (dgInt32 i = 0; i < m_particlesCount; i++) { + accel[i] = dgVector::m_zero; + diagonal[i] = dgVector::m_zero; + } +/* + for (dgInt32 i = 0; i < m_linksCount; i++) { + const dgInt32 j0 = links[i].m_m0; + const dgInt32 j1 = links[i].m_m1; + + const dgVector p0p1(posit[j0] - posit[j1]); + const dgVector v0v1(veloc[j0] - veloc[j1]); + const dgVector dpdv(p0p1 * v0v1); + + const dgVector mag2(p0p1.DotProduct(p0p1)); + const dgVector mask(mag2 > m_smallestLenght2); + + const dgVector lenght2((mag2 & mask) | mag2.And__Not(mask)); + const dgFloat32 length = (lenght2.Sqrt()).GetScalar(); + const dgFloat32 invDen = dgFloat32(1.0f) / length; + const dgFloat32 lenghtRatio = restLenght[i] * invDen; + const dgFloat32 compression = dgFloat32(1.0f) - lenghtRatio; + const dgVector fs(p0p1.Scale(kSpring * compression)); + const dgVector fd(p0p1.Scale(kDamper * invDen * invDen * (v0v1.DotProduct(p0p1)).GetScalar())); + const dgVector dfsdx(v0v1.Scale (ks_dt * compression) + ((p0p1 * dpdv).Scale (ks_dt * lenghtRatio * invDen * invDen))); + dgAssert(fs.m_w == dgFloat32(0.0f)); + dgAssert(fs.m_w == dgFloat32(0.0f)); + dgAssert(p0p1.m_w == dgFloat32(0.0f)); + dgAssert(dfsdx.m_w == dgFloat32(0.0f)); + + const dgVector nextAccel(fs + fd - dfsdx); + accel[j0] -= nextAccel; + accel[j1] += nextAccel; + } +*/ + + for (dgInt32 i = 0; i < m_linksCount; i++) { + const dgInt32 j0 = links[i].m_m0; + const dgInt32 j1 = links[i].m_m1; + const dgVector p0p1(posit[j0] - posit[j1]); + const dgVector v0v1(veloc[j0] - veloc[j1]); + const dgVector dvdp(v0v1 * p0p1); + + const dgVector p0p1Mag2(p0p1.DotProduct(p0p1)); + const dgVector p0p1Mask(p0p1Mag2 > m_smallestLenght2); + //const dgVector p0p1Lenght2((p0p1Mag2 & p0p1Mask) | m_smallestLenght2.AndNot(p0p1Mask)); + const dgVector p0p1Lenght2(m_smallestLenght2.Select (p0p1Mag2, p0p1Mask)); + + const dgFloat32 p0p1Length = p0p1Lenght2.Sqrt().GetScalar(); + const dgFloat32 p0p1InvMag = dgFloat32 (1.0f) / p0p1Length; + + const dgFloat32 k01 = -links[i].m_spring * (p0p1Length - links[i].m_restlength) * p0p1InvMag; + const dgFloat32 d01 = -links[i].m_damper * dvdp.GetScalar() * p0p1InvMag * p0p1InvMag; + const dgFloat32 h01dt = - dtRK4.GetScalar() * links[i].m_spring * links[i].m_restlength * p0p1InvMag * p0p1InvMag * p0p1InvMag; + + const dgVector diag ((p0p1 * p0p1).Scale(h01dt * dtRK4.GetScalar())); + + const dgFloat32 dtdfp0dx0 = h01dt * v0v1.DotProduct(p0p1).GetScalar(); + const dgVector netForce (p0p1.Scale(k01 + d01 + dtdfp0dx0)); + + + diagonal[j0] -= diag; + diagonal[j1] -= diag; + accel[j0] += netForce; + accel[j1] -= netForce; + } + + + for (dgInt32 i = 0; i < m_particlesCount; i++) { + //dgVector dirAccel1 (collisionDir1[i] * accel[i].DotProduct(collisionDir1[i])); + //dgVector dirAccel2 (collisionDir2[i] * accel[i].DotProduct(collisionDir2[i])); + //tmp0[i] = accel[i] + collidingAccel[i] - dirAccel0 - dirAccel1 - dirAccel2; + + dgVector netAccel (accel[i] + extAccel[i]); + dgVector tangentDir(veloc[i] - normalDir[i] * (normalDir[i].DotProduct(veloc[i]))); + dgVector mag(tangentDir.DotProduct(tangentDir) + epsilon); + + dgFloat32 tangentFrictionAccel = dgAbs(netAccel.DotProduct(normalDir[i]).GetScalar()); + dgVector friction(tangentDir.Scale(frictionCoeffecient[i] * tangentFrictionAccel / dgSqrt(mag.GetScalar()))); + + //dgVector particleAccel (accel[i] + normalAccel[i] - normalDirAccel); + dgVector normalDirAccel(normalDir[i] * (netAccel.DotProduct(normalDir[i]))); + //accel[i] = accel[i] + normalAccel[i] - normalDirAccel - friction; + netAccel = netAccel + normalAccel[i] - normalDirAccel - friction; + veloc[i] += netAccel * dtRK4; + posit[i] += veloc[i] * dtRK4; + } + } +} + +#endif \ No newline at end of file diff --git a/thirdparty/src/newton/dgPhysics/dgCollisionMassSpringDamperSystem.h b/thirdparty/src/newton/dgPhysics/dgCollisionMassSpringDamperSystem.h new file mode 100644 index 000000000..324dc41e1 --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgCollisionMassSpringDamperSystem.h @@ -0,0 +1,50 @@ +/* Copyright (c) <2003-2019> +* +* 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 __DGCOLLISION_DEFORMABLE_CLOTH_PATCH_MESH_H__ +#define __DGCOLLISION_DEFORMABLE_CLOTH_PATCH_MESH_H__ + + +#include "dgCollision.h" +#include "dgCollisionConvex.h" +#include "dgCollisionDeformableMesh.h" + + +class dgCollisionMassSpringDamperSystem: public dgCollisionDeformableMesh +{ + public: + dgCollisionMassSpringDamperSystem (const dgCollisionMassSpringDamperSystem& source); + dgCollisionMassSpringDamperSystem (dgWorld* const world, dgInt32 shapeID, dgInt32 pointCount, const dgFloat32* const points, dgInt32 strideInBytes, const dgFloat32* const pointsMasses, dgInt32 linksCount, const dgInt32* const links, const dgFloat32* const linksSpring, const dgFloat32* const LinksDamper); + dgCollisionMassSpringDamperSystem (dgWorld* const world, dgDeserialize deserialization, void* const userData, dgInt32 revisionNumber); + + virtual ~dgCollisionMassSpringDamperSystem(void); + virtual void CalculateAcceleration(dgFloat32 timestep); + + dgInt32 GetMemoryBufferSizeInBytes() const; +}; + + + + + +#endif + diff --git a/thirdparty/src/newton/dgPhysics/dgCollisionMesh.cpp b/thirdparty/src/newton/dgPhysics/dgCollisionMesh.cpp new file mode 100644 index 000000000..177ecf8e2 --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgCollisionMesh.cpp @@ -0,0 +1,359 @@ +/* Copyright (c) <2003-2019> +* +* 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 "dgPhysicsStdafx.h" +#include "dgBody.h" +#include "dgWorld.h" +#include "dgContact.h" +#include "dgCollisionMesh.h" +#include "dgCollisionConvexPolygon.h" + +////////////////////////////////////////////////////////////////////// +// Construction/Destruction +////////////////////////////////////////////////////////////////////// + + +dgPolygonMeshDesc::dgPolygonMeshDesc(dgCollisionParamProxy& proxy, void* const userData) + :dgFastAABBInfo() + ,m_boxDistanceTravelInMeshSpace(dgFloat32 (0.0f)) + ,m_threadNumber(proxy.m_threadIndex) + ,m_faceCount(0) + ,m_vertexStrideInBytes(0) + ,m_skinThickness(proxy.m_skinThickness) + ,m_userData (userData) + ,m_objBody (proxy.m_body0) + ,m_polySoupBody(proxy.m_body1) + ,m_convexInstance(proxy.m_instance0) + ,m_polySoupInstance(proxy.m_instance1) + ,m_vertex(NULL) + ,m_faceIndexCount(NULL) + ,m_faceVertexIndex(NULL) + ,m_faceIndexStart(NULL) + ,m_hitDistance(NULL) + ,m_maxT(dgFloat32 (1.0f)) + ,m_doContinuesCollisionTest(proxy.m_continueCollision) +{ + dgAssert (m_polySoupInstance->IsType (dgCollision::dgCollisionMesh_RTTI)); + dgAssert (m_convexInstance->IsType (dgCollision::dgCollisionConvexShape_RTTI)); + + const dgMatrix& hullMatrix = m_convexInstance->GetGlobalMatrix(); + const dgMatrix& soupMatrix = m_polySoupInstance->GetGlobalMatrix(); + + dgMatrix& matrix = *this; + matrix = hullMatrix * soupMatrix.Inverse(); + dgMatrix convexMatrix (dgGetIdentityMatrix()); + + switch (m_polySoupInstance->GetScaleType()) + { + case dgCollisionInstance::m_unit: + { + break; + } + + case dgCollisionInstance::m_uniform: + { + const dgVector& invScale = m_polySoupInstance->GetInvScale(); + convexMatrix[0][0] = invScale.GetScalar(); + convexMatrix[1][1] = invScale.GetScalar(); + convexMatrix[2][2] = invScale.GetScalar(); + matrix.m_posit = matrix.m_posit * (invScale | dgVector::m_wOne); + break; + } + + case dgCollisionInstance::m_nonUniform: + { + const dgVector& invScale = m_polySoupInstance->GetInvScale(); + dgMatrix tmp (matrix[0] * invScale, matrix[1] * invScale, matrix[2] * invScale, dgVector::m_wOne); + convexMatrix = tmp * matrix.Inverse(); + convexMatrix.m_posit = dgVector::m_wOne; + matrix.m_posit = matrix.m_posit * (invScale | dgVector::m_wOne); + break; + } + + case dgCollisionInstance::m_global: + default: + { + dgAssert (0); + } + } + + dgMatrix fullMatrix (convexMatrix * matrix); + m_convexInstance->CalcAABB(fullMatrix, m_p0, m_p1); + + dgVector p0; + dgVector p1; + SetTransposeAbsMatrix(matrix); + m_convexInstance->CalcAABB(convexMatrix, p0, p1); + m_size = dgVector::m_half * (p1 - p0); + m_posit = matrix.TransformVector(dgVector::m_half * (p1 + p0)); + dgAssert (m_posit.m_w == dgFloat32 (1.0f)); +} + +void dgPolygonMeshDesc::SortFaceArray () +{ + dgInt32 stride = 8; + if (m_faceCount >= 8) { + dgInt32 stack[DG_MAX_COLLIDING_FACES][2]; + + stack[0][0] = 0; + stack[0][1] = m_faceCount - 1; + dgInt32 stackIndex = 1; + while (stackIndex) { + stackIndex --; + dgInt32 lo = stack[stackIndex][0]; + dgInt32 hi = stack[stackIndex][1]; + if ((hi - lo) > stride) { + dgInt32 i = lo; + dgInt32 j = hi; + dgFloat32 dist = m_hitDistance[(lo + hi) >> 1]; + do { + while (m_hitDistance[i] < dist) i ++; + while (m_hitDistance[j] > dist) j --; + + if (i <= j) { + dgSwap (m_hitDistance[i], m_hitDistance[j]); + dgSwap (m_faceIndexStart[i], m_faceIndexStart[j]); + dgSwap (m_faceIndexCount[i], m_faceIndexCount[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 ++; + } + dgAssert (stackIndex < dgInt32 (sizeof (stack) / (2 * sizeof (stack[0][0])))); + } + } + } + + stride = stride * 2; + if (m_faceCount < stride) { + stride = m_faceCount; + } + for (dgInt32 i = 1; i < stride; i ++) { + if (m_hitDistance[i] < m_hitDistance[0]) { + dgSwap (m_hitDistance[i], m_hitDistance[0]); + dgSwap (m_faceIndexStart[i], m_faceIndexStart[0]); + dgSwap (m_faceIndexCount[i], m_faceIndexCount[0]); + } + } + + for (dgInt32 i = 1; i < m_faceCount; i ++) { + dgInt32 j = i; + dgInt32 ptr = m_faceIndexStart[i]; + dgInt32 count = m_faceIndexCount[i]; + dgFloat32 dist = m_hitDistance[i]; + for ( ; dist < m_hitDistance[j - 1]; j --) { + dgAssert (j > 0); + m_hitDistance[j] = m_hitDistance [j-1]; + m_faceIndexStart[j] = m_faceIndexStart[j-1]; + m_faceIndexCount[j] = m_faceIndexCount[j-1]; + } + m_hitDistance[j] = dist; + m_faceIndexStart[j] = ptr; + m_faceIndexCount[j] = count; + } + +#ifdef _DEBUG + for (dgInt32 i = 0; i < m_faceCount - 1; i ++) { + dgAssert (m_hitDistance[i] <= m_hitDistance[i+1]); + } +#endif +} + + +dgCollisionMesh::dgCollisionMesh(dgWorld* const world, dgCollisionID type) + :dgCollision(world->GetAllocator(), 0, type) +{ + m_rtti |= dgCollisionMesh_RTTI; + m_debugCallback = NULL; + SetCollisionBBox (dgVector (dgFloat32 (0.0f)), dgVector (dgFloat32 (0.0f))); +} + +dgCollisionMesh::dgCollisionMesh (dgWorld* const world, dgDeserialize deserialization, void* const userData, dgInt32 revisionNumber) + :dgCollision(world, deserialization, userData, revisionNumber) +{ + dgAssert (m_rtti | dgCollisionMesh_RTTI); + + m_debugCallback = NULL; + SetCollisionBBox (dgVector (dgFloat32 (0.0f)), dgVector (dgFloat32 (0.0f))); +} + +dgCollisionMesh::~dgCollisionMesh() +{ +} + +void dgCollisionMesh::SetCollisionBBox (const dgVector& p0, const dgVector& p1) +{ + dgAssert (p0.m_x <= p1.m_x); + dgAssert (p0.m_y <= p1.m_y); + dgAssert (p0.m_z <= p1.m_z); + + m_boxSize = (p1 - p0).Scale (dgFloat32 (0.5f)) & dgVector::m_triplexMask; + m_boxOrigin = (p1 + p0).Scale (dgFloat32 (0.5f)) & dgVector::m_triplexMask; +} + +dgInt32 dgCollisionMesh::CalculateSignature () const +{ + dgAssert (0); + return 0; +} + + +dgInt32 dgCollisionMesh::CalculatePlaneIntersection (const dgVector& normal, const dgVector& point, dgVector* const contactsOut) const +{ + return 0; +} + +void dgCollisionMesh::SetDebugCollisionCallback (dgCollisionMeshCollisionCallback debugCallback) +{ + m_debugCallback = debugCallback; +} + + + + +#ifdef DG_DEBUG_AABB +dgVector dgCollisionMesh::BoxSupportMapping (const dgVector& dir) const +{ + return dgVector (dir.m_x < dgFloat32 (0.0f) ? m_p0.m_x : m_p1.m_x, + dir.m_y < dgFloat32 (0.0f) ? m_p0.m_y : m_p1.m_y, + dir.m_z < dgFloat32 (0.0f) ? m_p0.m_z : m_p1.m_z, dgFloat32 (0.0f)); +} +#endif + + + + +dgVector dgCollisionMesh::CalculateVolumeIntegral (const dgMatrix& globalMatrix, const dgVector& plane, const dgCollisionInstance& parentScale) const +{ + return dgVector (dgFloat32 (0.0f)); +} + + +void dgCollisionMesh::DebugCollision (const dgMatrix& matrixPtr, dgCollision::OnDebugCollisionMeshCallback callback, void* const userData) const +{ + dgAssert (0); +} + + +dgFloat32 dgCollisionMesh::GetVolume () const +{ +// dgAssert (0); + return dgFloat32 (0.0f); +} + +dgFloat32 dgCollisionMesh::GetBoxMinRadius () const +{ + return dgFloat32 (0.0f); +} + +dgFloat32 dgCollisionMesh::GetBoxMaxRadius () const +{ + return dgFloat32 (0.0f); +} + + + +void dgCollisionMesh::GetCollisionInfo(dgCollisionInfo* const info) const +{ + dgAssert (0); + dgCollision::GetCollisionInfo(info); +// info->m_offsetMatrix = GetLocalMatrix(); +} + +void dgCollisionMesh::Serialize(dgSerialize callback, void* const userData) const +{ + dgAssert (0); +} + +dgVector dgCollisionMesh::SupportVertex (const dgVector& dir, dgInt32* const vertexIndex) const +{ + dgAssert (0); + return dgVector (0, 0, 0, 0); +} + + +void dgCollisionMesh::CalcAABB(const dgMatrix& matrix, dgVector &p0, dgVector &p1) const +{ + dgVector origin (matrix.TransformVector(m_boxOrigin)); + dgVector size (matrix.m_front.Abs().Scale(m_boxSize.m_x) + matrix.m_up.Abs().Scale(m_boxSize.m_y) + matrix.m_right.Abs().Scale(m_boxSize.m_z)); + + p0 = (origin - size) & dgVector::m_triplexMask; + p1 = (origin + size) & dgVector::m_triplexMask; +} + + +dgInt32 dgCollisionMesh::CalculatePlaneIntersection (const dgFloat32* const vertex, const dgInt32* const index, dgInt32 indexCount, dgInt32 stride, const dgPlane& localPlane, dgVector* const contactsOut) const +{ + dgInt32 count = 0; + dgInt32 j = index[indexCount - 1] * stride; + dgVector p0 (&vertex[j]); + p0 = p0 & dgVector::m_triplexMask; + dgFloat32 side0 = localPlane.Evalue (p0); + for (dgInt32 i = 0; i < indexCount; i ++) { + j = index[i] * stride; + dgVector p1 (&vertex[j]); + p1 = p1 & dgVector::m_triplexMask; + dgFloat32 side1 = localPlane.Evalue (p1); + + if (side0 < dgFloat32 (0.0f)) { + if (side1 >= dgFloat32 (0.0f)) { + dgVector dp (p1 - p0); + dgAssert (dp.m_w == dgFloat32 (0.0f)); + dgFloat32 t = localPlane.DotProduct(dp).GetScalar(); + dgAssert (dgAbs (t) >= dgFloat32 (0.0f)); + if (dgAbs (t) < dgFloat32 (1.0e-8f)) { + t = dgSign(t) * dgFloat32 (1.0e-8f); + } + dgAssert (0); + contactsOut[count] = p0 - dp.Scale (side0 / t); + count ++; + + } + } else if (side1 <= dgFloat32 (0.0f)) { + dgVector dp (p1 - p0); + dgAssert (dp.m_w == dgFloat32 (0.0f)); + dgFloat32 t = localPlane.DotProduct(dp).GetScalar(); + dgAssert (dgAbs (t) >= dgFloat32 (0.0f)); + if (dgAbs (t) < dgFloat32 (1.0e-8f)) { + t = dgSign(t) * dgFloat32 (1.0e-8f); + } + dgAssert (0); + contactsOut[count] = p0 - dp.Scale (side0 / t); + count ++; + } + + side0 = side1; + p0 = p1; + } + + return count; +} + diff --git a/thirdparty/src/newton/dgPhysics/dgCollisionMesh.h b/thirdparty/src/newton/dgPhysics/dgCollisionMesh.h new file mode 100644 index 000000000..3d11dd2ea --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgCollisionMesh.h @@ -0,0 +1,220 @@ +/* Copyright (c) <2003-2019> +* +* 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 __DGCOLLISION_MESH_H__ +#define __DGCOLLISION_MESH_H__ + + +#include "dgCollision.h" +#include "dgCollisionConvex.h" +#include "dgCollisionInstance.h" + + +#define DG_MAX_COLLIDING_FACES 512 +#define DG_MAX_COLLIDING_INDICES (DG_MAX_COLLIDING_FACES * (4 * 2 + 3)) + + +class dgCollisionMesh; +typedef void (*dgCollisionMeshCollisionCallback) (const dgBody* const bodyWithTreeCollision, const dgBody* const body, dgInt32 faceID, + dgInt32 vertexCount, const dgFloat32* const vertex, dgInt32 vertexStrideInBytes); + + +DG_MSC_VECTOR_ALIGNMENT +class dgPolygonMeshDesc: public dgFastAABBInfo +{ + public: + class dgMesh + { + public: + dgInt32 m_globalFaceIndexCount[DG_MAX_COLLIDING_FACES]; + dgInt32 m_globalFaceIndexStart[DG_MAX_COLLIDING_FACES]; + dgFloat32 m_globalHitDistance[DG_MAX_COLLIDING_FACES]; + }; + + // colliding box in polygonSoup local space + DG_INLINE dgPolygonMeshDesc() + :dgFastAABBInfo() + ,m_boxDistanceTravelInMeshSpace(dgFloat32 (0.0f)) + ,m_maxT(dgFloat32 (1.0f)) + ,m_doContinuesCollisionTest(false) + { + } + + dgPolygonMeshDesc(dgCollisionParamProxy& proxy, void* const userData); + + DG_INLINE void SetDistanceTravel (const dgVector& distanceInGlobalSpace) + { + const dgMatrix& soupMatrix = m_polySoupInstance->GetGlobalMatrix(); + m_boxDistanceTravelInMeshSpace = m_polySoupInstance->GetInvScale() * soupMatrix.UnrotateVector(distanceInGlobalSpace * m_convexInstance->GetInvScale()); + if (m_boxDistanceTravelInMeshSpace.DotProduct(m_boxDistanceTravelInMeshSpace).GetScalar() < dgFloat32 (1.0e-2f)) { + m_doContinuesCollisionTest = false; + } + } + + DG_INLINE dgInt32 GetFaceIndexCount(dgInt32 indexCount) const + { + return indexCount * 2 + 3; + } + + DG_INLINE const dgInt32* GetAdjacentFaceEdgeNormalArray(const dgInt32* const faceIndexArray, dgInt32 indexCount) const + { + return &faceIndexArray[indexCount + 2]; + } + + + DG_INLINE dgInt32 GetNormalIndex(const dgInt32* const faceIndexArray, dgInt32 indexCount) const + { + return faceIndexArray[indexCount + 1]; + } + + DG_INLINE dgInt32 GetFaceId(const dgInt32* const faceIndexArray, dgInt32 indexCount) const + { + return faceIndexArray[indexCount]; + } + + DG_INLINE dgFloat32 GetFaceSize(const dgInt32* const faceIndexArray, dgInt32 indexCount) const + { + dgInt32 size = faceIndexArray[indexCount * 2 + 2]; + return dgFloat32 ((size >= 1) ? size : dgFloat32 (1.0f)); + } + + DG_INLINE dgFloat32 GetSeparetionDistance() const + { + return m_separationDistance[0] * m_polySoupInstance->GetScale().GetScalar(); + } + + + void SortFaceArray (); + + dgVector m_boxDistanceTravelInMeshSpace; + dgInt32 m_threadNumber; + dgInt32 m_faceCount; + dgInt32 m_vertexStrideInBytes; + dgFloat32 m_skinThickness; + void* m_userData; + dgBody *m_objBody; + dgBody *m_polySoupBody; + dgCollisionInstance* m_convexInstance; + dgCollisionInstance* m_polySoupInstance; + dgFloat32* m_vertex; + dgInt32* m_faceIndexCount; + dgInt32* m_faceVertexIndex; + + // private data; + dgInt32* m_faceIndexStart; + dgFloat32* m_hitDistance; + const dgCollisionMesh* m_me; + dgInt32 m_globalIndexCount; + dgFloat32 m_maxT; + bool m_doContinuesCollisionTest; + dgInt32 m_globalFaceVertexIndex[DG_MAX_COLLIDING_INDICES]; + dgMesh m_meshData; +} DG_GCC_VECTOR_ALIGNMENT; + +DG_MSC_VECTOR_ALIGNMENT +class dgCollisionMeshRayHitDesc +{ + public: + dgCollisionMeshRayHitDesc () + :m_matrix (dgGetIdentityMatrix()) + { + } + + dgVector m_localP0; + dgVector m_localP1; + dgVector m_normal; + dgUnsigned64 m_userId; + void* m_userData; + void* m_altenateUserData; + dgMatrix m_matrix; +}DG_GCC_VECTOR_ALIGNMENT; + + + +class dgCollisionMesh: public dgCollision +{ + public: + + DG_MSC_VECTOR_ALIGNMENT + class dgMeshVertexListIndexList + { + public: + dgInt32* m_indexList; + dgInt32* m_userDataList; + dgFloat32* m_veterxArray; + dgInt32 m_triangleCount; + dgInt32 m_maxIndexCount; + dgInt32 m_vertexCount; + dgInt32 m_vertexStrideInBytes; + }DG_GCC_VECTOR_ALIGNMENT; + + + dgCollisionMesh (dgWorld* const world, dgCollisionID type); + dgCollisionMesh (dgWorld* const world, dgDeserialize deserialization, void* const userData, dgInt32 revisionNumber); + virtual ~dgCollisionMesh(); + + virtual dgFloat32 GetVolume () const; + virtual dgFloat32 GetBoxMinRadius () const; + virtual dgFloat32 GetBoxMaxRadius () const; + virtual void GetVertexListIndexList (const dgVector& p0, const dgVector& p1, dgMeshVertexListIndexList &data) const = 0; + + virtual void GetCollidingFaces (dgPolygonMeshDesc* const data) const = 0; + + void SetDebugCollisionCallback (dgCollisionMeshCollisionCallback debugCallback); + dgCollisionMeshCollisionCallback GetDebugCollisionCallback() const { return m_debugCallback;} + + protected: + virtual void SetCollisionBBox (const dgVector& p0, const dgVector& p1); + + private: + virtual dgInt32 CalculateSignature () const; + virtual dgVector SupportVertex (const dgVector& dir, dgInt32* const vertexIndex) const; + + virtual void CalcAABB (const dgMatrix& matrix, dgVector& p0, dgVector& p1) const; + virtual void DebugCollision (const dgMatrix& matrix, dgCollision::OnDebugCollisionMeshCallback callback, void* const userData) const; + + virtual dgVector CalculateVolumeIntegral (const dgMatrix& globalMatrix, const dgVector& plane, const dgCollisionInstance& parentScale) const; + virtual dgFloat32 RayCast (const dgVector& localP0, const dgVector& localP1, dgFloat32 maxT, dgContactPoint& contactOut, const dgBody* const body, void* const userData, OnRayPrecastAction preFilter) const = 0; + + dgInt32 CalculatePlaneIntersection (const dgVector& normal, const dgVector& point, dgVector* const contactsOut) const; + dgInt32 CalculatePlaneIntersection (const dgFloat32* const vertex, const dgInt32* const index, dgInt32 indexCount, dgInt32 strideInFloat, const dgPlane& localPlane, dgVector* const contactsOut) const; + + + virtual void GetCollisionInfo(dgCollisionInfo* const info) const; + virtual void Serialize(dgSerialize callback, void* const userData) const; + + +#ifdef DG_DEBUG_AABB + dgVector BoxSupportMapping (const dgVector& dir) const; +#endif + + protected: + dgCollisionMeshCollisionCallback m_debugCallback; + + + friend class dgWorld; + friend class dgCollisionInstance; +}; + + + +#endif + diff --git a/thirdparty/src/newton/dgPhysics/dgCollisionNull.cpp b/thirdparty/src/newton/dgPhysics/dgCollisionNull.cpp new file mode 100644 index 000000000..1f9af6088 --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgCollisionNull.cpp @@ -0,0 +1,102 @@ +/* Copyright (c) <2003-2019> +* +* 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 "dgPhysicsStdafx.h" +#include "dgBody.h" +#include "dgContact.h" +#include "dgCollisionNull.h" + +////////////////////////////////////////////////////////////////////// +// Construction/Destruction +////////////////////////////////////////////////////////////////////// + + +dgCollisionNull::dgCollisionNull(dgMemoryAllocator* const allocator, dgUnsigned32 signature) + :dgCollisionConvex(allocator, signature, m_nullCollision) +{ + m_rtti |= dgCollisionNull_RTTI; + m_inertia = dgVector (dgFloat32 (1.0f), dgFloat32 (1.0f), dgFloat32 (1.0f), dgFloat32 (0.0f)); +} + +dgCollisionNull::dgCollisionNull(dgWorld* const world, dgDeserialize deserialization, void* const userData, dgInt32 revisionNumber) + :dgCollisionConvex (world, deserialization, userData, revisionNumber) +{ + m_rtti |= dgCollisionNull_RTTI; +} + +void dgCollisionNull::Serialize(dgSerialize callback, void* const userData) const +{ + SerializeLow(callback, userData); +} + +dgCollisionNull::~dgCollisionNull() +{ +} + +void dgCollisionNull::SetCollisionBBox (const dgVector& p0, const dgVector& p1) +{ + dgAssert (0); +} + +void dgCollisionNull::DebugCollision (const dgMatrix& matrixPtr, dgCollision::OnDebugCollisionMeshCallback callback, void* const userData) const +{ +} + +dgInt32 dgCollisionNull::CalculateSignature () const +{ + return dgInt32 (GetSignature()); +} + +void dgCollisionNull::CalcAABB (const dgMatrix& matrix, dgVector &p0, dgVector &p1) const +{ + p0 = matrix[3] & dgVector::m_triplexMask; + p1 = matrix[3] & dgVector::m_triplexMask; +} + +dgVector dgCollisionNull::SupportVertex (const dgVector& dir, dgInt32* const vertexIndex) const +{ + dgAssert (0); + return dgVector::m_zero; +} + +dgVector dgCollisionNull::SupportVertexSpecial (const dgVector& dir, dgFloat32 skinThickness, dgInt32* const vertexIndex) const +{ + dgAssert(0); + return dgVector::m_zero; +} + +dgFloat32 dgCollisionNull::GetVolume () const +{ + return dgFloat32 (0.0f); +} + +dgFloat32 dgCollisionNull::RayCast (const dgVector& localP0, const dgVector& localP1, dgFloat32 maxT, dgContactPoint& contactOut, const dgBody* const body, void* const userData, OnRayPrecastAction preFilter) const +{ + return dgFloat32 (1.2f); +} + + +dgVector dgCollisionNull::CalculateVolumeIntegral (const dgMatrix& globalMatrix, const dgVector& plane, const dgCollisionInstance& parentScale) const +{ + dgAssert (0); + return dgVector::m_zero; +} + diff --git a/thirdparty/src/newton/dgPhysics/dgCollisionNull.h b/thirdparty/src/newton/dgPhysics/dgCollisionNull.h new file mode 100644 index 000000000..ca527f10f --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgCollisionNull.h @@ -0,0 +1,60 @@ +/* Copyright (c) <2003-2019> +* +* 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 _dgCollisionNull_H__ +#define _dgCollisionNull_H__ + + +#include "dgCollisionConvex.h" + +class dgCollisionNull: public dgCollisionConvex +{ + public: + dgCollisionNull(dgMemoryAllocator* const allocator, dgUnsigned32 signature); + dgCollisionNull(dgWorld* const world, dgDeserialize deserialization, void* const userData, dgInt32 revisionNumber); + virtual ~dgCollisionNull(); + + + protected: + virtual dgFloat32 GetVolume () const; + virtual void CalcAABB (const dgMatrix& matrix, dgVector& p0, dgVector& p1) const; + + virtual dgVector SupportVertex (const dgVector& dir, dgInt32* const vertexIndex) const; + virtual dgVector SupportVertexSpecial (const dgVector& dir, dgFloat32 skinThickness, dgInt32* const vertexIndex) const; + + virtual void DebugCollision (const dgMatrix& matrix, dgCollision::OnDebugCollisionMeshCallback callback, void* const userData) const; + + virtual dgVector CalculateVolumeIntegral (const dgMatrix& globalMatrix, const dgVector& plane, const dgCollisionInstance& parentScale) const; + virtual dgFloat32 RayCast (const dgVector& localP0, const dgVector& localP1, dgFloat32 maxT, dgContactPoint& contactOut, const dgBody* const body, void* const userData, OnRayPrecastAction preFilter) const; + + private: + virtual dgInt32 CalculateSignature () const; + virtual void SetCollisionBBox (const dgVector& p0, const dgVector& p1); + + virtual void Serialize(dgSerialize callback, void* const userData) const; + + + friend class dgWorld; +}; + + +#endif + diff --git a/thirdparty/src/newton/dgPhysics/dgCollisionScene.cpp b/thirdparty/src/newton/dgPhysics/dgCollisionScene.cpp new file mode 100644 index 000000000..c6444f2ae --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgCollisionScene.cpp @@ -0,0 +1,399 @@ +/* Copyright (c) <2003-2019> +* +* 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 "dgPhysicsStdafx.h" +#include "dgBody.h" +#include "dgWorld.h" +#include "dgContact.h" +#include "dgIntersections.h" +#include "dgCollisionScene.h" +#include "dgCollisionInstance.h" + + +dgCollisionScene::dgCollisionScene (dgWorld* const world) + :dgCollisionCompound(world) +{ + m_collisionId = m_sceneCollision; + m_rtti |= dgCollisionScene_RTTI; +} + +dgCollisionScene::dgCollisionScene (const dgCollisionScene& source, const dgCollisionInstance* const myInstance) + :dgCollisionCompound(source, myInstance) +{ + m_rtti |= dgCollisionScene_RTTI; +} + +dgCollisionScene::dgCollisionScene (dgWorld* const world, dgDeserialize deserialization, void* const userData, const dgCollisionInstance* const myInstance, dgInt32 revisionNumber) + :dgCollisionCompound(world, deserialization, userData, myInstance, revisionNumber) +{ + dgAssert (m_rtti | dgCollisionScene_RTTI); +} + +dgCollisionScene::~dgCollisionScene() +{ +} + +dgFloat32 dgCollisionScene::GetBoxMinRadius () const +{ + return dgFloat32 (0.0f); +} + +dgFloat32 dgCollisionScene::GetBoxMaxRadius () const +{ + return dgFloat32 (0.0f); +} + + +void dgCollisionScene::MassProperties () +{ + m_inertia = dgVector::m_zero; + m_centerOfMass = dgVector::m_zero; + m_crossInertia = dgVector::m_zero; +} + +void dgCollisionScene::CollidePair (dgBroadPhase::dgPair* const pair, dgCollisionParamProxy& proxy) const +{ + const dgNodeBase* stackPool[DG_COMPOUND_STACK_DEPTH]; + + dgAssert (proxy.m_contactJoint == pair->m_contact); + dgContact* const contactJoint = pair->m_contact; + dgBody* const otherBody = contactJoint->GetBody0(); + dgBody* const sceneBody = contactJoint->GetBody1(); + dgAssert (sceneBody->GetCollision()->GetChildShape() == this); + dgAssert (sceneBody->GetCollision()->IsType(dgCollision::dgCollisionScene_RTTI)); + + dgCollisionInstance* const sceneInstance = sceneBody->m_collision; + dgCollisionInstance* const otherInstance = otherBody->m_collision; + + dgAssert (sceneInstance->GetChildShape() == this); + dgAssert (otherInstance->IsType (dgCollision::dgCollisionConvexShape_RTTI)); + + const dgContactMaterial* const material = contactJoint->GetMaterial(); + + const dgMatrix& myMatrix = sceneInstance->GetGlobalMatrix(); + const dgMatrix& otherMatrix = otherInstance->GetGlobalMatrix(); + dgMatrix matrix (otherMatrix * myMatrix.Inverse()); + + const dgVector& hullVeloc = otherBody->m_veloc; + dgAssert (hullVeloc.m_w == dgFloat32 (0.0f)); + dgFloat32 baseLinearSpeed = dgSqrt (hullVeloc.DotProduct(hullVeloc).GetScalar()); + + dgFloat32 timestep = pair->m_timestep; + dgFloat32 closestDist = dgFloat32 (1.0e10f); + dgFloat32 separatingDist = dgFloat32 (1.0e10f); + if (proxy.m_continueCollision && (baseLinearSpeed > dgFloat32 (1.0e-6f))) { + dgVector p0; + dgVector p1; + otherInstance->CalcAABB (matrix, p0, p1); + + const dgVector& hullOmega = otherBody->m_omega; + dgAssert (hullOmega.m_w == dgFloat32 (0.0f)); + + dgFloat32 minRadius = otherInstance->GetBoxMinRadius(); + dgFloat32 maxAngularSpeed = dgSqrt (hullOmega.DotProduct(hullOmega).GetScalar()); + dgFloat32 angularSpeedBound = maxAngularSpeed * (otherInstance->GetBoxMaxRadius() - minRadius); + + dgFloat32 upperBoundSpeed = baseLinearSpeed + dgSqrt (angularSpeedBound); + dgVector upperBoundVeloc (hullVeloc.Scale (proxy.m_timestep * upperBoundSpeed / baseLinearSpeed)); + + dgVector boxDistanceTravelInMeshSpace (myMatrix.UnrotateVector(upperBoundVeloc * otherInstance->m_invScale)); + + dgInt32 stack = 1; + stackPool[0] = m_root; + dgFastRayTest ray (dgVector (dgFloat32 (0.0f)), boxDistanceTravelInMeshSpace); + + dgFloat32 maxParam = proxy.m_timestep; + while (stack) { + stack--; + const dgNodeBase* const me = stackPool[stack]; + dgAssert (me); + + if (me->BoxIntersect (ray, p0, p1)) { + if (me->m_type == m_leaf) { + dgAssert (!me->m_right); + bool processContacts = true; + if (material->m_compoundAABBOverlap) { + //processContacts = material->m_compoundAABBOverlap (*material, sceneBody, me->m_myNode, otherBody, NULL, proxy.m_threadIndex); + processContacts = material->m_compoundAABBOverlap(*contactJoint, timestep, sceneBody, me->m_myNode, otherBody, NULL, proxy.m_threadIndex); + } + + if (processContacts) { + const dgCollisionInstance* const myInstance = me->GetShape(); + dgCollisionInstance childInstance (*myInstance, myInstance->GetChildShape()); + childInstance.SetGlobalMatrix(childInstance.GetLocalMatrix() * myMatrix); + proxy.m_instance1 = &childInstance; + dgInt32 count = pair->m_contactCount; + + proxy.m_timestep = maxParam; + m_world->SceneChildContacts (pair, proxy); + // remember to update separating distance + //data.m_separatingDistance = dgMin(proxy.m_contactJoint->m_separationDistance, data.m_separatingDistance); + separatingDist = dgMin(proxy.m_contactJoint->m_separationDistance, separatingDist); + dgFloat32 param = proxy.m_timestep; + dgAssert(param >= dgFloat32(0.0f)); + if (param < maxParam) { + maxParam = param; + } + + if (pair->m_contactCount > count) { + dgContactPoint* const buffer = proxy.m_contacts; + for (dgInt32 i = count; i < pair->m_contactCount; i ++) { + dgAssert (buffer[i].m_collision0 == proxy.m_instance0); + if (buffer[i].m_collision1->GetChildShape() == myInstance->GetChildShape()) { + buffer[i].m_collision1 = myInstance; + } + } + } + closestDist = dgMin(closestDist, contactJoint->m_closestDistance); + } + + } else { + dgAssert (me->m_type == m_node); + stackPool[stack] = me->m_left; + stack++; + dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgNodeBase*))); + + stackPool[stack] = me->m_right; + stack++; + dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgNodeBase*))); + } + } + } + + proxy.m_timestep = maxParam; + } else { + dgVector size; + dgVector origin; + + otherInstance->CalcObb(origin, size); + dgOOBBTestData data (matrix, origin, size); + dgInt32 stack = 1; + stackPool[0] = m_root; + while (stack) { + + stack --; + const dgNodeBase* const me = stackPool[stack]; + dgAssert (me); + + if (me->BoxTest (data)) { + if (me->m_type == m_leaf) { + dgAssert (!me->m_right); + bool processContacts = true; + if (material->m_compoundAABBOverlap) { + processContacts = material->m_compoundAABBOverlap (*contactJoint, timestep, sceneBody, me->m_myNode, otherBody, NULL, proxy.m_threadIndex); + } + + if (processContacts) { + const dgCollisionInstance* const myInstance = me->GetShape(); + dgCollisionInstance childInstance (*myInstance, myInstance->GetChildShape()); + childInstance.SetGlobalMatrix(childInstance.GetLocalMatrix() * myMatrix); + proxy.m_instance1 = &childInstance; + dgInt32 count = pair->m_contactCount; + m_world->SceneChildContacts (pair, proxy); + data.m_separatingDistance = dgMin(proxy.m_contactJoint->m_separationDistance, data.m_separatingDistance); + if (pair->m_contactCount > count) { + dgContactPoint* const buffer = proxy.m_contacts; + for (dgInt32 i = count; i < pair->m_contactCount; i ++) { + dgAssert (buffer[i].m_collision0 == proxy.m_instance0); + if (buffer[i].m_collision1->GetChildShape() == myInstance->GetChildShape()) { + buffer[i].m_collision1 = myInstance; + } + } + } else if (pair->m_contactCount == -1) { + break; + } + closestDist = dgMin(closestDist, contactJoint->m_closestDistance); + } + + } else { + dgAssert (me->m_type == m_node); + stackPool[stack] = me->m_left; + stack++; + dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgNodeBase*))); + + stackPool[stack] = me->m_right; + stack++; + dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgNodeBase*))); + } + } + } + separatingDist = dgMin (separatingDist, data.m_separatingDistance); + } + contactJoint->m_closestDistance = closestDist; + contactJoint->m_separationDistance = separatingDist; +} + + +void dgCollisionScene::CollideCompoundPair (dgBroadPhase::dgPair* const pair, dgCollisionParamProxy& proxy) const +{ + const dgNodeBase* stackPool[4 * DG_COMPOUND_STACK_DEPTH][2]; + + dgContact* const contactJoint = pair->m_contact; + dgBody* const myBody = contactJoint->GetBody1(); + dgBody* const otherBody = contactJoint->GetBody0(); + + dgAssert (myBody == proxy.m_body1); + dgAssert (otherBody == proxy.m_body0); + + dgCollisionInstance* const myCompoundInstance = myBody->m_collision; + dgCollisionInstance* const otherCompoundInstance = otherBody->m_collision; + + dgAssert (myCompoundInstance->GetChildShape() == this); + dgAssert (otherCompoundInstance->IsType (dgCollision::dgCollisionCompound_RTTI)); + dgCollisionCompound* const otherCompound = (dgCollisionCompound*)otherCompoundInstance->GetChildShape(); + + const dgContactMaterial* const material = contactJoint->GetMaterial(); + + dgMatrix myMatrix (myCompoundInstance->GetLocalMatrix() * myBody->m_matrix); + dgMatrix otherMatrix (otherCompoundInstance->GetLocalMatrix() * otherBody->m_matrix); + dgOOBBTestData data (otherMatrix * myMatrix.Inverse()); + + dgInt32 stack = 1; + stackPool[0][0] = m_root; + stackPool[0][1] = otherCompound->m_root; + + const dgVector& hullVeloc = otherBody->m_veloc; + dgAssert (hullVeloc.m_w == dgFloat32 (0.0f)); + dgFloat32 baseLinearSpeed = dgSqrt (hullVeloc.DotProduct(hullVeloc).GetScalar()); + + dgFloat32 timestep = pair->m_timestep; + dgFloat32 closestDist = dgFloat32 (1.0e10f); + dgFloat32 separatingDist = dgFloat32 (1.0e10f); +// if (proxy.m_continueCollision && (baseLinearSpeed > dgFloat32 (1.0e-6f))) { +// dgAssert (0); +// } else { +if (proxy.m_continueCollision && (baseLinearSpeed > dgFloat32 (1.0e-6f))) { +dgTrace (("Warning scene continue collision not implemented, using descrete collision instead\n")); +} + + + while (stack) { + stack --; + const dgNodeBase* const me = stackPool[stack][0]; + const dgNodeBase* const other = stackPool[stack][1]; + + dgAssert (me && other); + + if (me->BoxTest (data, other)) { + + if ((me->m_type == m_leaf) && (other->m_type == m_leaf)) { + dgAssert (!me->m_right); + + bool processContacts = true; + if (material->m_compoundAABBOverlap) { + processContacts = material->m_compoundAABBOverlap (*contactJoint, timestep, myBody, me->m_myNode, otherBody, other->m_myNode, proxy.m_threadIndex); + } + + if (processContacts) { + const dgCollisionInstance* const mySrcInstance = me->GetShape(); + const dgCollisionInstance* const otherSrcInstance = other->GetShape(); + dgCollisionInstance childInstance (*me->GetShape(), me->GetShape()->GetChildShape()); + dgCollisionInstance otherInstance (*other->GetShape(), other->GetShape()->GetChildShape()); + + childInstance.SetGlobalMatrix(childInstance.GetLocalMatrix() * myMatrix); + otherInstance.SetGlobalMatrix(otherInstance.GetLocalMatrix() * otherMatrix); + proxy.m_instance1 = &childInstance; + proxy.m_instance0 = &otherInstance; + + dgInt32 count = pair->m_contactCount; + m_world->SceneChildContacts (pair, proxy); + if (pair->m_contactCount > count) { + dgContactPoint* const buffer = proxy.m_contacts; + for (dgInt32 i = count; i < pair->m_contactCount; i ++) { + if (buffer[i].m_collision1->GetChildShape() == otherSrcInstance->GetChildShape()) { + dgAssert(buffer[i].m_collision0->GetChildShape() == mySrcInstance->GetChildShape()); + buffer[i].m_collision0 = mySrcInstance; + buffer[i].m_collision1 = otherSrcInstance; + } else { + dgAssert(buffer[i].m_collision1->GetChildShape() == mySrcInstance->GetChildShape()); + buffer[i].m_collision1 = mySrcInstance; + buffer[i].m_collision0 = otherSrcInstance; + } + } + } + + closestDist = dgMin(closestDist, contactJoint->m_closestDistance); + data.m_separatingDistance = dgMin(proxy.m_contactJoint->m_separationDistance, data.m_separatingDistance); + } + + } else if (me->m_type == m_leaf) { + dgAssert (other->m_type == m_node); + + stackPool[stack][0] = me; + stackPool[stack][1] = other->m_left; + stack++; + dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgNodeBase*))); + + stackPool[stack][0] = me; + stackPool[stack][1] = other->m_right; + stack++; + dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgNodeBase*))); + + + } else if (other->m_type == m_leaf) { + dgAssert (me->m_type == m_node); + + stackPool[stack][0] = me->m_left; + stackPool[stack][1] = other; + stack++; + dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgNodeBase*))); + + stackPool[stack][0] = me->m_right; + stackPool[stack][1] = other; + stack++; + dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgNodeBase*))); + } else { + dgAssert (me->m_type == m_node); + dgAssert (other->m_type == m_node); + + stackPool[stack][0] = me->m_left; + stackPool[stack][1] = other->m_left; + stack++; + dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgNodeBase*))); + + stackPool[stack][0] = me->m_left; + stackPool[stack][1] = other->m_right; + stack++; + dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgNodeBase*))); + + stackPool[stack][0] = me->m_right; + stackPool[stack][1] = other->m_left; + stack++; + dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgNodeBase*))); + + stackPool[stack][0] = me->m_right; + stackPool[stack][1] = other->m_right; + stack++; + dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgNodeBase*))); + } + } + } + separatingDist = dgMin (separatingDist, data.m_separatingDistance); +// } + contactJoint->m_closestDistance = closestDist; + contactJoint->m_separationDistance = separatingDist; +} + + +void dgCollisionScene::Serialize(dgSerialize callback, void* const userData) const +{ + dgCollisionCompound::Serialize(callback, userData); +} \ No newline at end of file diff --git a/thirdparty/src/newton/dgPhysics/dgCollisionScene.h b/thirdparty/src/newton/dgPhysics/dgCollisionScene.h new file mode 100644 index 000000000..1474f9480 --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgCollisionScene.h @@ -0,0 +1,48 @@ +/* Copyright (c) <2003-2019> +* +* 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 _DGCOLLISIONSCENE_H_ +#define _DGCOLLISIONSCENE_H_ + + +#include "dgCollision.h" +#include "dgCollisionCompound.h" + + +class dgCollisionScene: public dgCollisionCompound +{ + public: + dgCollisionScene(dgWorld* const world); + dgCollisionScene (const dgCollisionScene& source, const dgCollisionInstance* const myInstance); + dgCollisionScene(dgWorld* const world, dgDeserialize deserialization, void* const userData, const dgCollisionInstance* const myInstance, dgInt32 revisionNumber); + virtual ~dgCollisionScene(); + + void CollidePair (dgBroadPhase::dgPair* const pair, dgCollisionParamProxy& proxy) const; + void CollideCompoundPair (dgBroadPhase::dgPair* const pair, dgCollisionParamProxy& proxy) const; + virtual void MassProperties (); + virtual void Serialize(dgSerialize callback, void* const userData) const; + + dgFloat32 GetBoxMinRadius () const; + dgFloat32 GetBoxMaxRadius () const; +}; + +#endif diff --git a/thirdparty/src/newton/dgPhysics/dgCollisionSphere.cpp b/thirdparty/src/newton/dgPhysics/dgCollisionSphere.cpp new file mode 100644 index 000000000..0640beab0 --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgCollisionSphere.cpp @@ -0,0 +1,327 @@ +/* Copyright (c) <2003-2019> +* +* 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 "dgPhysicsStdafx.h" +#include "dgBody.h" +#include "dgWorld.h" +#include "dgContact.h" +#include "dgCollisionSphere.h" +#include "dgCollisionConvexPolygon.h" + + +////////////////////////////////////////////////////////////////////// +// Construction/Destruction +////////////////////////////////////////////////////////////////////// + + +#define DG_SPHERE_EDGE_COUNT 96 + +dgInt32 dgCollisionSphere::m_shapeRefCount = 0; +dgVector dgCollisionSphere::m_unitSphere[DG_SPHERE_VERTEX_COUNT]; +dgCollisionConvex::dgConvexSimplexEdge dgCollisionSphere::m_edgeArray[DG_SPHERE_EDGE_COUNT]; + +dgCollisionSphere::dgCollisionSphere(dgMemoryAllocator* const allocator, dgUnsigned32 signature, dgFloat32 radii) + :dgCollisionConvex(allocator, signature, m_sphereCollision) +{ + Init (radii, allocator); +} + +dgCollisionSphere::dgCollisionSphere(dgWorld* const world, dgDeserialize deserialization, void* const userData, dgInt32 revisionNumber) + :dgCollisionConvex (world, deserialization, userData, revisionNumber) +{ + dgFloat32 radios; + deserialization (userData, &radios, sizeof (radios)); + Init (radios, world->GetAllocator()); +} + +dgCollisionSphere::~dgCollisionSphere() +{ + m_shapeRefCount --; + dgAssert (m_shapeRefCount >= 0); + + dgCollisionConvex::m_simplex = NULL; + dgCollisionConvex::m_vertex = NULL; +} + + +void dgCollisionSphere::Init (dgFloat32 radius, dgMemoryAllocator* allocator) +{ + m_rtti |= dgCollisionSphere_RTTI; + m_radius = dgMax (dgAbs (radius), D_MIN_CONVEX_SHAPE_SIZE); + + m_edgeCount = DG_SPHERE_EDGE_COUNT; + m_vertexCount = DG_SPHERE_VERTEX_COUNT; + dgCollisionConvex::m_vertex = m_vertex; + + if (!m_shapeRefCount) { + + dgInt32 indexList[256]; + dgVector tmpVectex[256]; + + dgVector p0 ( dgFloat32 (1.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f)); + dgVector p1 (-dgFloat32 (1.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f)); + dgVector p2 ( dgFloat32 (0.0f), dgFloat32 (1.0f), dgFloat32 (0.0f), dgFloat32 (0.0f)); + dgVector p3 ( dgFloat32 (0.0f),-dgFloat32 (1.0f), dgFloat32 (0.0f), dgFloat32 (0.0f)); + dgVector p4 ( dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (1.0f), dgFloat32 (0.0f)); + dgVector p5 ( dgFloat32 (0.0f), dgFloat32 (0.0f),-dgFloat32 (1.0f), dgFloat32 (0.0f)); + + dgInt32 index = 1; + dgInt32 count = 0; + TesselateTriangle (index, p4, p0, p2, count, tmpVectex); + TesselateTriangle (index, p4, p2, p1, count, tmpVectex); + TesselateTriangle (index, p4, p1, p3, count, tmpVectex); + TesselateTriangle (index, p4, p3, p0, count, tmpVectex); + TesselateTriangle (index, p5, p2, p0, count, tmpVectex); + TesselateTriangle (index, p5, p1, p2, count, tmpVectex); + TesselateTriangle (index, p5, p3, p1, count, tmpVectex); + TesselateTriangle (index, p5, p0, p3, count, tmpVectex); + + //dgAssert (count == EDGE_COUNT); + dgInt32 vertexCount = dgVertexListToIndexList (&tmpVectex[0].m_x, sizeof (dgVector), 3 * sizeof (dgFloat32), 0, count, indexList, 0.001f); + + dgAssert (vertexCount == DG_SPHERE_VERTEX_COUNT); + for (dgInt32 i = 0; i < vertexCount; i ++) { + m_unitSphere[i] = tmpVectex[i]; + } + dgPolyhedra polyhedra(m_allocator); + + polyhedra.BeginFace(); + for (dgInt32 i = 0; i < count; i += 3) { +#ifdef _DEBUG + dgEdge* const edge = polyhedra.AddFace (indexList[i], indexList[i + 1], indexList[i + 2]); + dgAssert (edge); +#else + polyhedra.AddFace (indexList[i], indexList[i + 1], indexList[i + 2]); +#endif + } + polyhedra.EndFace(); + + dgUnsigned64 i1 = 0; + dgPolyhedra::Iterator iter (polyhedra); + for (iter.Begin(); iter; iter ++) { + dgEdge* const edge = &(*iter); + edge->m_userData = i1; + i1 ++; + } + + for (iter.Begin(); iter; iter ++) { + dgEdge* const edge = &(*iter); + + dgConvexSimplexEdge* const ptr = &m_edgeArray[edge->m_userData]; + + ptr->m_vertex = edge->m_incidentVertex; + ptr->m_next = &m_edgeArray[edge->m_next->m_userData]; + ptr->m_prev = &m_edgeArray[edge->m_prev->m_userData]; + ptr->m_twin = &m_edgeArray[edge->m_twin->m_userData]; + } + } + + for (dgInt32 i = 0; i < DG_SPHERE_VERTEX_COUNT; i ++) { + m_vertex[i] = m_unitSphere[i].Scale (m_radius); + } + + m_shapeRefCount ++; + dgCollisionConvex::m_simplex = m_edgeArray; + SetVolumeAndCG (); +} + +dgVector dgCollisionSphere::SupportVertex (const dgVector& dir, dgInt32* const vertexIndex) const +{ + dgAssert (dir.m_w == dgFloat32 (0.0f)); + dgAssert (dgAbs(dir.DotProduct(dir).GetScalar() - dgFloat32 (1.0f)) < dgFloat32 (1.0e-3f)); + dgAssert (dir.m_w == 0.0f); + return dir.Scale (m_radius); +} + +void dgCollisionSphere::TesselateTriangle (dgInt32 level, const dgVector& p0, const dgVector& p1, const dgVector& p2, dgInt32& count, dgVector* const ouput) const +{ + if (level) { + dgAssert (dgAbs (p0.DotProduct(p0).GetScalar() - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f)); + dgAssert (dgAbs (p1.DotProduct(p1).GetScalar() - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f)); + dgAssert (dgAbs (p2.DotProduct(p2).GetScalar() - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f)); + dgVector p01 (p0 + p1); + dgVector p12 (p1 + p2); + dgVector p20 (p2 + p0); + + //p01 = p01 * p01.InvMagSqrt(); + //p12 = p12 * p12.InvMagSqrt(); + //p20 = p20 * p20.InvMagSqrt(); + + p01 = p01.Normalize(); + p12 = p12.Normalize(); + p20 = p20.Normalize(); + + dgAssert (dgAbs (p01.DotProduct(p01).GetScalar() - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f)); + dgAssert (dgAbs (p12.DotProduct(p12).GetScalar() - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f)); + dgAssert (dgAbs (p20.DotProduct(p20).GetScalar() - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f)); + + TesselateTriangle (level - 1, p0, p01, p20, count, ouput); + TesselateTriangle (level - 1, p1, p12, p01, count, ouput); + TesselateTriangle (level - 1, p2, p20, p12, count, ouput); + TesselateTriangle (level - 1, p01, p12, p20, count, ouput); + + } else { + ouput[count ++] = p0; + ouput[count ++] = p1; + ouput[count ++] = p2; + } +} + +void dgCollisionSphere::SetCollisionBBox (const dgVector& p0__, const dgVector& p1__) +{ + dgAssert (0); +} + +dgInt32 dgCollisionSphere::CalculateSignature (dgFloat32 radius) +{ + dgUnsigned32 buffer[2]; + radius = dgAbs (radius); + + buffer[0] = m_sphereCollision; + buffer[1] = Quantize (radius); + return Quantize(buffer, sizeof (buffer)); +} + +dgInt32 dgCollisionSphere::CalculateSignature () const +{ + return CalculateSignature(m_radius); +} + +void dgCollisionSphere::CalcAABB (const dgMatrix& matrix, dgVector &p0, dgVector &p1) const +{ + dgVector size (matrix.m_front.Abs().Scale(m_radius) + matrix.m_up.Abs().Scale(m_radius) + matrix.m_right.Abs().Scale(m_radius)); + p0 = (matrix[3] - size) & dgVector::m_triplexMask; + p1 = (matrix[3] + size) & dgVector::m_triplexMask; +} + +dgInt32 dgCollisionSphere::CalculatePlaneIntersection (const dgVector& normal, const dgVector& point, dgVector* const contactsOut) const +{ + dgAssert (normal.m_w == 0.0f); + dgAssert (normal.DotProduct(normal).GetScalar() > dgFloat32 (0.999f)); + contactsOut[0] = normal * normal.DotProduct(point); + return 1; +} + +void dgCollisionSphere::DebugCollision (const dgMatrix& matrix, dgCollision::OnDebugCollisionMeshCallback callback, void* const userData) const +{ + dgTriplex pool[1024 * 2]; + dgVector tmpVectex[1024 * 2]; + + dgVector p0 ( dgFloat32 (1.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f)); + dgVector p1 (-dgFloat32 (1.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f)); + dgVector p2 ( dgFloat32 (0.0f), dgFloat32 (1.0f), dgFloat32 (0.0f), dgFloat32 (0.0f)); + dgVector p3 ( dgFloat32 (0.0f),-dgFloat32 (1.0f), dgFloat32 (0.0f), dgFloat32 (0.0f)); + dgVector p4 ( dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (1.0f), dgFloat32 (0.0f)); + dgVector p5 ( dgFloat32 (0.0f), dgFloat32 (0.0f),-dgFloat32 (1.0f), dgFloat32 (0.0f)); + + dgInt32 index = 3; + dgInt32 count = 0; + TesselateTriangle (index, p4, p0, p2, count, tmpVectex); + TesselateTriangle (index, p4, p2, p1, count, tmpVectex); + TesselateTriangle (index, p4, p1, p3, count, tmpVectex); + TesselateTriangle (index, p4, p3, p0, count, tmpVectex); + TesselateTriangle (index, p5, p2, p0, count, tmpVectex); + TesselateTriangle (index, p5, p1, p2, count, tmpVectex); + TesselateTriangle (index, p5, p3, p1, count, tmpVectex); + TesselateTriangle (index, p5, p0, p3, count, tmpVectex); + + for (dgInt32 i = 0; i < count; i ++) { + tmpVectex[i] = tmpVectex[i].Scale (m_radius); + } + + //dgMatrix matrix (GetLocalMatrix() * matrixPtr); + matrix.TransformTriplex (&pool[0].m_x, sizeof (dgTriplex), &tmpVectex[0].m_x, sizeof (dgVector), count); + for (dgInt32 i = 0; i < count; i += 3) { + callback (userData, 3, &pool[i].m_x, 0); + } +} + +dgFloat32 dgCollisionPoint::GetVolume () const +{ + dgAssert (0); + return dgFloat32 (0.0f); +} + +dgVector dgCollisionPoint::SupportVertex (const dgVector& dir, dgInt32* const vertexIndex) const +{ + return dgVector (dgFloat32 (0.0f)); +} + +dgVector dgCollisionPoint::SupportVertexSpecial (const dgVector& dir, dgFloat32 skinThickness, dgInt32* const vertexIndex) const +{ + return dgVector (dgFloat32 (0.0f)); +} + +dgFloat32 dgCollisionSphere::RayCast (const dgVector& p0, const dgVector& p1, dgFloat32 maxT, dgContactPoint& contactOut, const dgBody* const body, void* const userData, OnRayPrecastAction preFilter) const +{ + dgFloat32 t = dgRayCastSphere (p0, p1, dgVector (dgFloat32 (0.0f)), m_radius); + if (t < maxT) { + dgVector contact (p0 + (p1 - p0).Scale (t)); + dgAssert (contact.m_w == dgFloat32 (0.0f)); + //contactOut.m_normal = contact.Scale (dgRsqrt (contact.DotProduct(contact).GetScalar())); + contactOut.m_normal = contact.Normalize(); + //contactOut.m_userId = SetUserDataID(); + } + return t; +} + +void dgCollisionSphere::MassProperties () +{ + m_centerOfMass = dgVector::m_zero; + m_crossInertia = dgVector::m_zero; + dgFloat32 volume = dgFloat32 (4.0f * dgPi / 3.0f) * m_radius * m_radius * m_radius; + dgFloat32 II = dgFloat32 (2.0f / 5.0f) * m_radius * m_radius; + m_inertia = dgVector (II, II, II, dgFloat32 (0.0f)); + m_centerOfMass.m_w = volume; +} + +void dgCollisionSphere::GetCollisionInfo(dgCollisionInfo* const info) const +{ + dgCollisionConvex::GetCollisionInfo(info); + info->m_sphere.m_radius = m_radius; +} + +void dgCollisionSphere::Serialize(dgSerialize callback, void* const userData) const +{ + SerializeLow(callback, userData); + callback (userData, &m_radius, sizeof (m_radius)); +} + +dgVector dgCollisionSphere::SupportVertexSpecial (const dgVector& dir, dgFloat32 skinThickness, dgInt32* const vertexIndex) const +{ + return dgVector (dgFloat32 (0.0f)); +} + +dgVector dgCollisionSphere::SupportVertexSpecialProjectPoint (const dgVector& point, const dgVector& dir) const +{ + return dir.Scale (m_radius - DG_PENETRATION_TOL); +} + +void dgCollisionSphere::CalculateImplicitContacts(dgInt32 count, dgContactPoint* const contactPoints) const +{ + for (dgInt32 i = 0; i < count; i++) { + dgVector normal(contactPoints[i].m_point & dgVector::m_triplexMask); + dgAssert(normal.DotProduct(normal).GetScalar() > dgFloat32(0.0f)); + normal = normal.Normalize(); + contactPoints[i].m_normal = normal * dgVector::m_negOne; + contactPoints[i].m_point = normal.Scale(m_radius); + } +} \ No newline at end of file diff --git a/thirdparty/src/newton/dgPhysics/dgCollisionSphere.h b/thirdparty/src/newton/dgPhysics/dgCollisionSphere.h new file mode 100644 index 000000000..94496c56f --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgCollisionSphere.h @@ -0,0 +1,87 @@ +/* Copyright (c) <2003-2019> +* +* 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_COLLISION_SPHERE_H__ +#define _DG_COLLISION_SPHERE_H__ + + +#include "dgCollisionConvex.h" + +#define DG_SPHERE_VERTEX_COUNT 18 + +class dgCollisionSphere: public dgCollisionConvex +{ + public: + dgCollisionSphere(dgMemoryAllocator* allocator, dgUnsigned32 signature, dgFloat32 radius); + dgCollisionSphere(dgWorld* const world, dgDeserialize deserialization, void* const userData, dgInt32 revisionNumber); + virtual ~dgCollisionSphere(); + + protected: + void Init (dgFloat32 radius, dgMemoryAllocator* allocator); + virtual dgVector SupportVertex (const dgVector& dir, dgInt32* const vertexIndex) const; + + virtual void CalcAABB (const dgMatrix& matrix, dgVector& p0, dgVector& p1) const; + virtual void DebugCollision (const dgMatrix& matrix, dgCollision::OnDebugCollisionMeshCallback callback, void* const userData) const; + virtual dgFloat32 RayCast (const dgVector& localP0, const dgVector& localP1, dgFloat32 maxT, dgContactPoint& contactOut, const dgBody* const body, void* const userData, OnRayPrecastAction preFilter) const; + + void TesselateTriangle (dgInt32 level, const dgVector& p0, const dgVector& p1, const dgVector& p2, dgInt32& count, dgVector* const ouput) const; + static dgInt32 CalculateSignature (dgFloat32 radius); + + virtual dgInt32 CalculateSignature () const; + virtual void SetCollisionBBox (const dgVector& p0, const dgVector& p1); + + virtual void MassProperties (); + + virtual dgInt32 CalculatePlaneIntersection (const dgVector& normal, const dgVector& point, dgVector* const contactsOut) const; + + virtual void GetCollisionInfo(dgCollisionInfo* const info) const; + virtual void Serialize(dgSerialize callback, void* const userData) const; + + virtual void CalculateImplicitContacts(dgInt32 count, dgContactPoint* const contactPoints) const; + virtual dgVector SupportVertexSpecialProjectPoint(const dgVector& point, const dgVector& dir) const; + virtual dgVector SupportVertexSpecial (const dgVector& dir, dgFloat32 skinThickness, dgInt32* const vertexIndex) const; + + dgFloat32 m_radius; + dgVector m_vertex[DG_SPHERE_VERTEX_COUNT]; + + static dgInt32 m_shapeRefCount; + static dgVector m_unitSphere[]; + static dgConvexSimplexEdge m_edgeArray[]; + + friend class dgWorld; +}; + + +class dgCollisionPoint: public dgCollisionSphere +{ + public: + dgCollisionPoint (dgMemoryAllocator* const allocator) + :dgCollisionSphere(allocator, 0x12344321, dgFloat32 (0.25f)) + { + } + + virtual dgFloat32 GetVolume () const; + virtual dgVector SupportVertex (const dgVector& dir, dgInt32* const vertexIndex) const; + virtual dgVector SupportVertexSpecial (const dgVector& dir, dgFloat32 skinThickness, dgInt32* const vertexIndex) const; +}; + +#endif + diff --git a/thirdparty/src/newton/dgPhysics/dgCollisionUserMesh.cpp b/thirdparty/src/newton/dgPhysics/dgCollisionUserMesh.cpp new file mode 100644 index 000000000..52fc1c4ea --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgCollisionUserMesh.cpp @@ -0,0 +1,315 @@ +/* Copyright (c) <2003-2019> +* +* 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 "dgPhysicsStdafx.h" +#include "dgBody.h" +#include "dgWorld.h" +#include "dgCollisionUserMesh.h" + + +dgCollisionUserMesh::dgCollisionUserMesh(dgWorld* const world, const dgVector& boxP0, const dgVector& boxP1, const dgUserMeshCreation& data) + :dgCollisionMesh (world, m_userMesh) +{ + m_rtti |= dgCollisionUserMesh_RTTI; + + m_userData = data.m_userData; + m_getInfoCallback = data.m_getInfoCallback; + m_faceInAABBCalback = data.m_faceInAABBCallback; + m_rayHitCallback = data.m_rayHitCallback; + m_collideCallback = data.m_collideCallback; + m_destroyCallback = data.m_destroyCallback; + m_serializeCallback = data.m_serializeCallback; + m_getAABBOvelapTestCallback = data.m_getAABBOvelapTestCallback; + + SetCollisionBBox(boxP0, boxP1); +} + +dgCollisionUserMesh::dgCollisionUserMesh (dgWorld* const world, dgDeserialize deserialization, void* const userData, dgInt32 revisionNumber) + :dgCollisionMesh (world, deserialization, userData, revisionNumber) +{ +dgAssert (0); + m_rtti |= dgCollisionUserMesh_RTTI; + +/* + dgAABBPolygonSoup::Deserialize (deserialization, userData); + + dgVector p0; + dgVector p1; + GetAABB (p0, p1); + SetCollisionBBox(p0, p1); +*/ +} + +dgCollisionUserMesh::~dgCollisionUserMesh(void) +{ + if (m_destroyCallback) { + m_destroyCallback (m_userData); + } +} + +void dgCollisionUserMesh::Serialize(dgSerialize callback, void* const userData) const +{ + SerializeLow(callback, userData); + if (m_serializeCallback) { + m_serializeCallback (m_userData, callback, userData); + } +} + + +void dgCollisionUserMesh::GetVertexListIndexList (const dgVector& p0, const dgVector& p1, dgMeshVertexListIndexList &data) const +{ + if (m_faceInAABBCalback) { + return m_faceInAABBCalback (m_userData, &p0[0], &p1[0], (const dgFloat32**) &data.m_veterxArray, &data.m_vertexCount, &data.m_vertexStrideInBytes, + data.m_indexList, data.m_maxIndexCount, data.m_userDataList); + + } else { + data.m_triangleCount = 0; + } +} + +void dgCollisionUserMesh::GetCollisionInfo(dgCollisionInfo* const info) const +{ + dgCollision::GetCollisionInfo(info); + if (m_getInfoCallback) { + m_getInfoCallback (m_userData, info); + } +} + +bool dgCollisionUserMesh::AABBOvelapTest (const dgVector& boxP0, const dgVector& boxP1) const +{ + return (m_getAABBOvelapTestCallback && m_getAABBOvelapTestCallback (m_userData, boxP0, boxP1)) ? true : false; +} + + +dgFloat32 dgCollisionUserMesh::RayCast (const dgVector& localP0, const dgVector& localP1, dgFloat32 maxT, dgContactPoint& contactOut, const dgBody* const body, void* const userData, OnRayPrecastAction preFilter) const +{ + dgFloat32 param = dgFloat32 (1.2f); + if (m_rayHitCallback) { + dgCollisionMeshRayHitDesc data; + data.m_localP0 = localP0; + data.m_localP1 = localP1; + data.m_userId = body->m_collision->GetUserDataID(); + data.m_userData = m_userData; + data.m_altenateUserData = userData; + if (body) { + data.m_matrix = body->m_collision->GetGlobalMatrix(); + } + + dgFloat32 t = m_rayHitCallback (data); + if ((t < dgFloat32 (1.0f)) && (t > dgFloat32 (0.0f))) { + param = t; + contactOut.m_normal = data.m_normal; +// contactOut.m_userId = data.m_userId; + contactOut.m_shapeId0 = dgInt64 (data.m_userId); + } + } + return param; +} + + +dgVector dgCollisionUserMesh::SupportVertex (const dgVector& dir, dgInt32* const vertexIndex) const +{ + dgAssert (0); + return dgVector::m_zero; +} + +dgVector dgCollisionUserMesh::SupportVertexSpecial (const dgVector& dir, dgFloat32 skinThickness, dgInt32* const vertexIndex) const +{ + dgAssert(0); + return dgVector::m_zero; +} + +void dgCollisionUserMesh::GetCollidingFacesContinue(dgPolygonMeshDesc* const data) const +{ + data->m_me = this; + data->m_faceCount = 0; + data->m_userData = m_userData; + data->m_separationDistance = dgFloat32(0.0f); + + dgFastRayTest ray(dgVector(dgFloat32(0.0f)), data->m_boxDistanceTravelInMeshSpace); + m_collideCallback(&data->m_p0, &ray); + + dgInt32 faceCount0 = 0; + dgInt32 faceIndexCount0 = 0; + dgInt32 faceIndexCount1 = 0; + dgInt32 stride = data->m_vertexStrideInBytes / sizeof(dgFloat32); + dgFloat32* const vertex = data->m_vertex; + dgInt32* const address = data->m_meshData.m_globalFaceIndexStart; + dgFloat32* const hitDistance = data->m_meshData.m_globalHitDistance; + const dgInt32* const srcIndices = data->m_faceVertexIndex; + dgInt32* const dstIndices = data->m_globalFaceVertexIndex; + dgInt32* const faceIndexCountArray = data->m_faceIndexCount; + + for (dgInt32 i = 0; (i < data->m_faceCount) && (faceIndexCount0 < (DG_MAX_COLLIDING_INDICES - 32)); i++) { + dgInt32 indexCount = faceIndexCountArray[i]; + const dgInt32* const indexArray = &srcIndices[faceIndexCount1]; + + dgInt32 normalIndex = data->GetNormalIndex(indexArray, indexCount); + dgVector faceNormal(&vertex[normalIndex * stride]); + faceNormal = faceNormal & dgVector::m_triplexMask; + dgFloat32 dist = data->PolygonBoxRayDistance(faceNormal, indexCount, indexArray, stride, vertex, ray); + + const dgInt32 faceIndexCount = data->GetFaceIndexCount(indexCount); + if (dist < dgFloat32(1.0f)) { + hitDistance[faceCount0] = dist; + address[faceCount0] = faceIndexCount0; + faceIndexCountArray[faceCount0] = indexCount; + memcpy(&dstIndices[faceIndexCount0], indexArray, faceIndexCount * sizeof(dgInt32)); + faceCount0++; + faceIndexCount0 += faceIndexCount; + } + faceIndexCount1 += faceIndexCount; + } + + data->m_faceCount = 0; + if (faceCount0) { + data->m_faceCount = faceCount0; + data->m_faceIndexStart = address; + data->m_hitDistance = hitDistance; + data->m_faceVertexIndex = dstIndices; + + if (GetDebugCollisionCallback()) { + dgTriplex triplex[32]; + //const dgMatrix& matrix = data->m_polySoupInstance->GetGlobalMatrix(); + const dgVector scale = data->m_polySoupInstance->GetScale(); + dgMatrix matrix(data->m_polySoupInstance->GetLocalMatrix() * data->m_polySoupBody->GetMatrix()); + + for (dgInt32 i = 0; i < data->m_faceCount; i++) { + dgInt32 base = address[i]; + dgInt32 indexCount = faceIndexCountArray[i]; + const dgInt32* const vertexFaceIndex = &data->m_faceVertexIndex[base]; + for (dgInt32 j = 0; j < indexCount; j++) { + dgInt32 index = vertexFaceIndex[j]; + dgVector q(&vertex[index * stride]); + q = q & dgVector::m_triplexMask; + //dgVector p(matrix.TransformVector(scale * dgVector(&vertex[index * stride]))); + dgVector p(matrix.TransformVector(scale * q)); + triplex[j].m_x = p.m_x; + triplex[j].m_y = p.m_y; + triplex[j].m_z = p.m_z; + } + dgInt32 faceId = data->GetFaceId(vertexFaceIndex, indexCount); + GetDebugCollisionCallback() (data->m_polySoupBody, data->m_objBody, faceId, indexCount, &triplex[0].m_x, sizeof(dgTriplex)); + } + } + } +} + +void dgCollisionUserMesh::GetCollidingFacesDescrete(dgPolygonMeshDesc* const data) const +{ + data->m_me = this; + data->m_faceCount = 0; + data->m_userData = m_userData; + data->m_separationDistance = dgFloat32(0.0f); + + m_collideCallback(&data->m_p0, NULL); + + dgInt32 faceCount0 = 0; + dgInt32 faceIndexCount0 = 0; + dgInt32 faceIndexCount1 = 0; + dgInt32 stride = data->m_vertexStrideInBytes / sizeof(dgFloat32); + dgFloat32* const vertex = data->m_vertex; + dgInt32* const address = data->m_meshData.m_globalFaceIndexStart; + dgFloat32* const hitDistance = data->m_meshData.m_globalHitDistance; + const dgInt32* const srcIndices = data->m_faceVertexIndex; + dgInt32* const dstIndices = data->m_globalFaceVertexIndex; + dgInt32* const faceIndexCountArray = data->m_faceIndexCount; + + for (dgInt32 i = 0; (i < data->m_faceCount) && (faceIndexCount0 < (DG_MAX_COLLIDING_INDICES - 32)); i++) { + dgInt32 indexCount = faceIndexCountArray[i]; + const dgInt32* const indexArray = &srcIndices[faceIndexCount1]; + + dgInt32 normalIndex = data->GetNormalIndex(indexArray, indexCount); + dgVector faceNormal(&vertex[normalIndex * stride]); + faceNormal = faceNormal & dgVector::m_triplexMask; + dgFloat32 dist = data->PolygonBoxDistance(faceNormal, indexCount, indexArray, stride, vertex); + + const dgInt32 faceIndexCount = data->GetFaceIndexCount(indexCount); + if (dist > dgFloat32(0.0f)) { + hitDistance[faceCount0] = dist; + address[faceCount0] = faceIndexCount0; + faceIndexCountArray[faceCount0] = indexCount; + memcpy(&dstIndices[faceIndexCount0], indexArray, faceIndexCount * sizeof(dgInt32)); + faceCount0++; + faceIndexCount0 += faceIndexCount; + } + faceIndexCount1 += faceIndexCount; + } + + data->m_faceCount = 0; + if (faceCount0) { + data->m_faceCount = faceCount0; + data->m_faceIndexStart = address; + data->m_hitDistance = hitDistance; + data->m_faceVertexIndex = dstIndices; + + if (GetDebugCollisionCallback()) { + dgTriplex triplex[32]; + //const dgMatrix& matrix = data->m_polySoupInstance->GetGlobalMatrix(); + const dgVector scale = data->m_polySoupInstance->GetScale(); + dgMatrix matrix(data->m_polySoupInstance->GetLocalMatrix() * data->m_polySoupBody->GetMatrix()); + + for (dgInt32 i = 0; i < data->m_faceCount; i++) { + dgInt32 base = address[i]; + dgInt32 indexCount = faceIndexCountArray[i]; + const dgInt32* const vertexFaceIndex = &data->m_faceVertexIndex[base]; + for (dgInt32 j = 0; j < indexCount; j++) { + dgInt32 index = vertexFaceIndex[j]; + dgVector q(&vertex[index * stride]); + q = q & dgVector::m_triplexMask; + //dgVector p(matrix.TransformVector(scale * dgVector(&vertex[index * stride]))); + dgVector p(matrix.TransformVector(scale * q)); + triplex[j].m_x = p.m_x; + triplex[j].m_y = p.m_y; + triplex[j].m_z = p.m_z; + } + dgInt32 faceId = data->GetFaceId(vertexFaceIndex, indexCount); + GetDebugCollisionCallback() (data->m_polySoupBody, data->m_objBody, faceId, indexCount, &triplex[0].m_x, sizeof(dgTriplex)); + } + } + } +} + +void dgCollisionUserMesh::GetCollidingFaces (dgPolygonMeshDesc* const data) const +{ + if (m_collideCallback) { + if (data->m_doContinuesCollisionTest) { + GetCollidingFacesContinue(data); + } else { + GetCollidingFacesDescrete(data); + } + } +} + +void dgCollisionUserMesh::DebugCollision (const dgMatrix& matrixPtr, dgCollision::OnDebugCollisionMeshCallback callback, void* const userData) const +{ +/* + dgCollisionUserMeshShowPolyContext context; + + context.m_matrix = matrixPtr; + context.m_userData = userData;; + context.m_callback = callback; + + dgVector p0 (dgFloat32 (-1.0e20f), dgFloat32 (-1.0e20f), dgFloat32 (-1.0e20f), dgFloat32 (0.0f)); + dgVector p1 (dgFloat32 ( 1.0e20f), dgFloat32 ( 1.0e20f), dgFloat32 ( 1.0e20f), dgFloat32 (0.0f)); + ForAllSectors (p0, p1, ShowDebugPolygon, &context); +*/ +} diff --git a/thirdparty/src/newton/dgPhysics/dgCollisionUserMesh.h b/thirdparty/src/newton/dgPhysics/dgCollisionUserMesh.h new file mode 100644 index 000000000..29bba4d0d --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgCollisionUserMesh.h @@ -0,0 +1,88 @@ +/* Copyright (c) <2003-2019> +* +* 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 __DGCOLLISION_USERMESH__ +#define __DGCOLLISION_USERMESH__ + +#include "dgCollision.h" +#include "dgCollisionMesh.h" + + + +class dgCollisionUserMesh: public dgCollisionMesh +{ + public: + typedef void (dgApi *OnUserMeshDestroyCallback) (void* const userData); + typedef dgFloat32 (dgApi *OnUserMeshRayHitCallback) (dgCollisionMeshRayHitDesc& rayHitdata); + typedef void (dgApi *OnUserMeshCollisionInfo) (void* userData, dgCollisionInfo* infoRecord); + typedef void (dgApi *OnUserMeshCollideCallback) (void* const collideData, const void* const continueHandle); + typedef dgInt32 (dgApi *OnUserMeshAABBOverlapTest) (void* const userData, const dgVector& boxP0, const dgVector& boxP1); + typedef void (dgApi *OnUserMeshSerialize) (void* const userSerializeData, dgSerialize function, void* const serilalizeObject); + typedef void (dgApi *OnUserMeshFacesInAABB) (void* userData, const dgFloat32* p0, const dgFloat32* p1, const dgFloat32** vertexArray, dgInt32* vertexCount, dgInt32* vertexStrideInBytes, const dgInt32* indexList, dgInt32 maxIndexCount, const dgInt32* faceAttribute); + + dgCollisionUserMesh(dgWorld* const world, const dgVector& boxP0, const dgVector& boxP1, const dgUserMeshCreation& data); + dgCollisionUserMesh (dgWorld* const world, dgDeserialize deserialization, void* const userData, dgInt32 revisionNumber); + virtual ~dgCollisionUserMesh(void); + + void GetVertexListIndexList (const dgVector& p0, const dgVector& p1, dgMeshVertexListIndexList &data) const; + + bool AABBOvelapTest (const dgVector& boxP0, const dgVector& boxP1) const; + + private: + void Serialize(dgSerialize callback, void* const userData) const; + + dgVector SupportVertex (const dgVector& dir, dgInt32* const vertexIndex) const; + dgVector SupportVertexSpecial (const dgVector& dir, dgFloat32 skinThickness, dgInt32* const vertexIndex) const; + dgVector SupportVertexSpecialProjectPoint (const dgVector& point, const dgVector& dir) const {return point;} + + virtual void GetCollisionInfo(dgCollisionInfo* const info) const; + virtual dgFloat32 RayCast (const dgVector& localP0, const dgVector& localP1, dgFloat32 maxT, dgContactPoint& contactOut, const dgBody* const body, void* const userData, OnRayPrecastAction preFilter) const; + virtual void GetCollidingFaces (dgPolygonMeshDesc* const data) const; + virtual void DebugCollision (const dgMatrix& matrixPtr, dgCollision::OnDebugCollisionMeshCallback callback, void* const userData) const; + + void GetCollidingFacesContinue(dgPolygonMeshDesc* const data) const; + void GetCollidingFacesDescrete(dgPolygonMeshDesc* const data) const; + + void* m_userData; + OnUserMeshSerialize m_serializeCallback; + OnUserMeshCollisionInfo m_getInfoCallback; + OnUserMeshFacesInAABB m_faceInAABBCalback; + OnUserMeshRayHitCallback m_rayHitCallback; + OnUserMeshCollideCallback m_collideCallback; + OnUserMeshDestroyCallback m_destroyCallback; + OnUserMeshAABBOverlapTest m_getAABBOvelapTestCallback; +}; + +class dgUserMeshCreation +{ + public: + void* m_userData; + dgCollisionUserMesh::OnUserMeshSerialize m_serializeCallback; + dgCollisionUserMesh::OnUserMeshCollideCallback m_collideCallback; + dgCollisionUserMesh::OnUserMeshRayHitCallback m_rayHitCallback; + dgCollisionUserMesh::OnUserMeshDestroyCallback m_destroyCallback; + dgCollisionUserMesh::OnUserMeshCollisionInfo m_getInfoCallback; + dgCollisionUserMesh::OnUserMeshFacesInAABB m_faceInAABBCallback; + dgCollisionUserMesh::OnUserMeshAABBOverlapTest m_getAABBOvelapTestCallback; +}; + + +#endif diff --git a/thirdparty/src/newton/dgPhysics/dgConstraint.cpp b/thirdparty/src/newton/dgPhysics/dgConstraint.cpp new file mode 100644 index 000000000..5b16bba14 --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgConstraint.cpp @@ -0,0 +1,86 @@ +/* Copyright (c) <2003-2019> +* +* 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 "dgPhysicsStdafx.h" +#include "dgBody.h" +#include "dgWorld.h" +#include "dgConstraint.h" + +////////////////////////////////////////////////////////////////////// +// Construction/Destruction +////////////////////////////////////////////////////////////////////// + + +void* dgConstraint::GetUserData () const +{ + return m_userData; +} + +void dgConstraint::SetUserData (void *userData) +{ + m_userData = userData; +} + +dgFloat32 dgConstraint::GetMassScaleBody0() const +{ + return dgFloat32(1.0f); +} + +dgFloat32 dgConstraint::GetMassScaleBody1() const +{ + return dgFloat32(1.0f); +} + +void dgConstraint::InitPointParam (dgPointParam& param, dgFloat32 stiffness, const dgVector& p0Global, const dgVector& p1Global) const +{ + dgAssert (m_body0); + dgAssert (m_body1); + param.m_defualtDiagonalRegularizer = stiffness; + + param.m_posit0 = p0Global; + param.m_posit1 = p1Global; + + param.m_r0 = (p0Global - m_body0->m_globalCentreOfMass) & dgVector::m_triplexMask; + param.m_r1 = (p1Global - m_body1->m_globalCentreOfMass) & dgVector::m_triplexMask; +} + +void dgConstraint::InitInfo (dgConstraintInfo* const info) const +{ + info->m_attachBody_0 = GetBody0(); + dgAssert (info->m_attachBody_0); + dgWorld* const world = info->m_attachBody_0->GetWorld(); + if (info->m_attachBody_0 == (dgBody*)world->GetSentinelBody()) { + info->m_attachBody_0 = NULL; + } + + info->m_attachBody_1 = GetBody1(); + if (info->m_attachBody_1 == (dgBody*)world->GetSentinelBody()) { + info->m_attachBody_1 = NULL; + } + + info->m_attachMatrix_0 = dgGetIdentityMatrix(); + info->m_attachMatrix_1 = dgGetIdentityMatrix(); + + info->m_discriptionType[0] = 0; + +} + + diff --git a/thirdparty/src/newton/dgPhysics/dgConstraint.h b/thirdparty/src/newton/dgPhysics/dgConstraint.h new file mode 100644 index 000000000..345ede45d --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgConstraint.h @@ -0,0 +1,423 @@ +/* Copyright (c) <2003-2019> +* +* 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 __DGCONSTRAINT_H__ +#define __DGCONSTRAINT_H__ + +#include "dgBodyMasterList.h" + +#define DG_MAX_BOUND dgFloat32 (1.0e15f) +#define DG_MIN_BOUND (-DG_MAX_BOUND) +#define DG_INDEPENDENT_ROW -1 +#define DG_CONSTRAINT_MAX_ROWS (3 * 16) +#define MIN_JOINT_PIN_LENGTH dgFloat32 (50.0f) + +class dgBody; +class dgWorld; +class dgConstraint; +class dgBilateralBounds; + +typedef void (dgApi *ConstraintsForceFeeback) (const dgConstraint& me, dgFloat32 timestep, dgInt32 threadIndex); + +class dgConstraintInfo +{ + public: + dgMatrix m_attachMatrix_0; + dgMatrix m_attachMatrix_1; + dgFloat32 m_minLinearDof[3]; + dgFloat32 m_maxLinearDof[3]; + dgFloat32 m_minAngularDof[3]; + dgFloat32 m_maxAngularDof[3]; + dgBody* m_attachBody_0; + dgBody* m_attachBody_1; + dgFloat32 m_extraParameters[64]; + dgInt32 m_collideCollisionOn; + char m_discriptionType[64]; +}; + + +class dgJointCallbackParam +{ + public: + dgFloat32 m_accel; + dgFloat32 m_minFriction; + dgFloat32 m_maxFriction; + dgFloat32 m_timestep; +}; + +class dgForceImpactPair +{ + public: + void Clear() + { + m_force = dgFloat32 (0.0f); + m_impact = dgFloat32(0.0f); + for (dgInt32 i = 0; i < sizeof(m_initialGuess) / sizeof(m_initialGuess[0]); i++) { + m_initialGuess[i] = dgFloat32(0.0f); + } + } + + void Push(dgFloat32 val) + { + for (dgInt32 i = 1; i < sizeof(m_initialGuess) / sizeof(m_initialGuess[0]); i++) { + m_initialGuess[i - 1] = m_initialGuess[i]; + } + m_initialGuess[sizeof(m_initialGuess) / sizeof(m_initialGuess[0]) - 1] = val; + } + + dgFloat32 GetInitiailGuess() const + { + dgFloat32 value = dgFloat32(0.0f); + dgFloat32 smallest = dgFloat32(1.0e15f); + for (dgInt32 i = 0; i < sizeof(m_initialGuess) / sizeof(m_initialGuess[0]); i++) + { + dgFloat32 mag = dgAbs(m_initialGuess[i]); + if (mag < smallest) { + smallest = mag; + value = m_initialGuess[i]; + } + } + return value; + } + + dgFloat32 m_force; + dgFloat32 m_impact; + dgFloat32 m_initialGuess[4]; +}; + +class dgBilateralBounds +{ + public: + dgForceImpactPair* m_jointForce; + dgFloat32 m_low; + dgFloat32 m_upper; + dgInt32 m_normalIndex; +}; + +DG_MSC_VECTOR_ALIGNMENT +class dgJacobian +{ + public: + dgVector m_linear; + dgVector m_angular; +} DG_GCC_VECTOR_ALIGNMENT; + +DG_MSC_VECTOR_ALIGNMENT +class dgJacobianPair +{ + public: + dgJacobian m_jacobianM0; + dgJacobian m_jacobianM1; +} DG_GCC_VECTOR_ALIGNMENT; + +class dgLeftHandSide; +class dgRightHandSide; + +class dgJointAccelerationDecriptor +{ + public: + dgInt32 m_rowsCount; + dgFloat32 m_timeStep; + dgFloat32 m_invTimeStep; + dgFloat32 m_firstPassCoefFlag; + dgRightHandSide* m_rightHandSide; + const dgLeftHandSide* m_leftHandSide; +}; + +DG_MSC_VECTOR_ALIGNMENT +class dgContraintDescritor +{ + public: + dgJacobianPair m_jacobian[DG_CONSTRAINT_MAX_ROWS]; + dgBilateralBounds m_forceBounds[DG_CONSTRAINT_MAX_ROWS]; + dgFloat32 m_jointAccel[DG_CONSTRAINT_MAX_ROWS]; + dgFloat32 m_restitution[DG_CONSTRAINT_MAX_ROWS]; + dgFloat32 m_penetration[DG_CONSTRAINT_MAX_ROWS]; + dgFloat32 m_diagonalRegularizer[DG_CONSTRAINT_MAX_ROWS]; + dgFloat32 m_penetrationStiffness[DG_CONSTRAINT_MAX_ROWS]; + dgFloat32 m_zeroRowAcceleration[DG_CONSTRAINT_MAX_ROWS]; + dgInt8 m_flags[DG_CONSTRAINT_MAX_ROWS]; + dgWorld* m_world; + dgInt32 m_threadIndex; + dgFloat32 m_timestep; + dgFloat32 m_invTimestep; +} DG_GCC_VECTOR_ALIGNMENT; + + +typedef void (dgApi *OnConstraintDestroy) (dgConstraint& me); + +DG_MSC_VECTOR_ALIGNMENT +class dgConstraint +{ + public: + DG_CLASS_ALLOCATOR(allocator) + + enum dgConstraintID + { + m_ballConstraint, + m_hingeConstraint, + m_sliderConstraint, + m_contactConstraint, + m_upVectorConstraint, + m_universalConstraint, + m_corkScrewConstraint, + m_unknownConstraint + }; + + dgUnsigned32 GetId () const; + dgBody* GetBody0 () const; + dgBody* GetBody1 () const; + + dgBodyMasterListRow::dgListNode* GetLink0() const; + dgBodyMasterListRow::dgListNode* GetLink1() const; + void* GetUserData () const; + + bool IsActive() const; + bool IsCollidable () const; + bool IsBilateral () const; + bool IsSkeleton () const; + bool IsSkeletonLoop () const; + + dgInt32 GetMaxDOF() const; + virtual void ResetMaxDOF(); + virtual dgFloat32 GetImpulseContactSpeed() const; + virtual void SetImpulseContactSpeed(dgFloat32 speed); + + virtual dgFloat32 GetMassScaleBody0 () const; + virtual dgFloat32 GetMassScaleBody1 () const; + + void SetUserData (void *userData); + void SetCollidable (bool state); + virtual void SetDestructorCallback (OnConstraintDestroy destructor) = 0; + + virtual dgFloat32 GetStiffness() const; + virtual void SetStiffness(dgFloat32 stiffness); + + virtual dgInt32 GetSolverModel() const; + virtual void SetSolverModel(dgInt32 model); + + virtual void GetInfo (dgConstraintInfo* const info) const; + + ConstraintsForceFeeback GetUpdateFeedbackFunction (); + virtual void JointAccelerations(dgJointAccelerationDecriptor* const params) = 0; + + void SetIndex (dgInt32 index); + + class dgPointParam + { + public: + dgVector m_r0; + dgVector m_r1; + dgVector m_posit0; + dgVector m_posit1; + dgFloat32 m_defualtDiagonalRegularizer; + }; + + protected: + dgConstraint(); + virtual ~dgConstraint(); + + virtual dgUnsigned32 JacobianDerivative (dgContraintDescritor& params) = 0; + + void SetUpdateFeedbackFunction (ConstraintsForceFeeback function); + void InitPointParam (dgPointParam& param, dgFloat32 stiffness, const dgVector& p0Global, const dgVector& p1Global) const; + void InitInfo (dgConstraintInfo* const info) const; + + void* m_userData; + dgBody* m_body0; + dgBody* m_body1; + dgBodyMasterListRow::dgListNode* m_link0; + dgBodyMasterListRow::dgListNode* m_link1; + ConstraintsForceFeeback m_updaFeedbackCallback; + dgInt32 m_clusterLRU; + dgUnsigned32 m_index; + dgUnsigned32 m_impulseLru; + dgUnsigned32 m_dynamicsLru; + dgUnsigned32 m_maxDOF : 6; + dgUnsigned32 m_constId : 6; + dgUnsigned32 m_solverModel : 2; + dgUnsigned32 m_enableCollision : 1; + dgUnsigned32 m_isActive : 1; + dgUnsigned32 m_isBilateral : 1; + dgUnsigned32 m_graphTagged : 1; + dgUnsigned32 m_isInSkeleton : 1; + dgUnsigned32 m_isInSkeletonLoop : 1; + + friend class dgWorld; + friend class dgJacobianMemory; + friend class dgBodyMasterList; + friend class dgSkeletonContainer; + friend class dgWorldDynamicUpdate; + friend class dgParallelBodySolver; + friend class dgParallelSolverJointAcceleration; + friend class dgParallelSolverInitFeedbackUpdate; + friend class dgParallelSolverBuildJacobianMatrix; + friend class dgBroadPhaseMaterialCallbackWorkerThread; +} DG_GCC_VECTOR_ALIGNMENT; + +DG_INLINE dgConstraint::dgConstraint() + :m_userData(NULL) + ,m_body0(NULL) + ,m_body1(NULL) + ,m_link0(NULL) + ,m_link1(NULL) + ,m_updaFeedbackCallback(NULL) + ,m_clusterLRU(-1) + ,m_index(0) + ,m_impulseLru(0) + ,m_dynamicsLru(0) + ,m_maxDOF(6) + ,m_constId(m_unknownConstraint) + ,m_solverModel(2) + ,m_enableCollision(false) + ,m_isActive(false) + ,m_isBilateral(false) + ,m_graphTagged(false) + ,m_isInSkeleton(false) + ,m_isInSkeletonLoop(false) +{ + dgAssert ((((dgUnsigned64) this) & 15) == 0); +} + +DG_INLINE dgConstraint::~dgConstraint() +{ +} + +DG_INLINE ConstraintsForceFeeback dgConstraint::GetUpdateFeedbackFunction () +{ + return m_updaFeedbackCallback; +} + +DG_INLINE void dgConstraint::SetUpdateFeedbackFunction (ConstraintsForceFeeback function) +{ + m_updaFeedbackCallback = function; +} + +DG_INLINE bool dgConstraint::IsBilateral() const +{ + return m_isBilateral ? true : false; +} + +DG_INLINE bool dgConstraint::IsSkeleton () const +{ + return m_isInSkeleton ? true : false; +} + +DG_INLINE bool dgConstraint::IsSkeletonLoop () const +{ + return m_isInSkeletonLoop ? true : false; +} + +DG_INLINE bool dgConstraint::IsCollidable () const +{ + return m_enableCollision ? true : false; +} + +DG_INLINE void dgConstraint::SetCollidable (bool state) +{ + m_enableCollision = dgUnsigned32 (state); +} + +DG_INLINE dgUnsigned32 dgConstraint::GetId () const +{ + return m_constId; +} + +DG_INLINE dgBody* dgConstraint::GetBody0 () const +{ + return m_body0; +} + +DG_INLINE dgBody* dgConstraint::GetBody1 () const +{ + return m_body1; +} + +//DG_INLINE void dgConstraint::SetBodies (dgBody* const body0, dgBody* const body1) +//{ +// m_body0 = body0; +// m_body1 = body1; +//} + +DG_INLINE dgBodyMasterListRow::dgListNode* dgConstraint::GetLink0() const +{ + return m_link0; +} +DG_INLINE dgBodyMasterListRow::dgListNode* dgConstraint::GetLink1() const +{ + return m_link1; +} + + +DG_INLINE dgFloat32 dgConstraint::GetStiffness() const +{ + return dgFloat32 (1.0f); +} + +DG_INLINE void dgConstraint::SetStiffness(dgFloat32 stiffness) +{ +} + +DG_INLINE dgInt32 dgConstraint::GetSolverModel() const +{ + return m_solverModel; +} + +DG_INLINE void dgConstraint::SetSolverModel(dgInt32 model) +{ + m_solverModel = dgClamp(model, 0, 2); +} + +DG_INLINE void dgConstraint::ResetMaxDOF() +{ +} + +DG_INLINE void dgConstraint::SetImpulseContactSpeed(dgFloat32 speed) +{ +} + +DG_INLINE dgFloat32 dgConstraint::GetImpulseContactSpeed() const +{ + return dgFloat32 (0.0f); +} + +DG_INLINE dgInt32 dgConstraint::GetMaxDOF() const +{ + return dgInt32 (m_maxDOF); +} + +DG_INLINE bool dgConstraint::IsActive() const +{ + return m_isActive ? true : false; +} + +DG_INLINE void dgConstraint::SetIndex (dgInt32 index) +{ + m_index = index; +} + +DG_INLINE void dgConstraint::GetInfo(dgConstraintInfo* const info) const +{ + dgAssert(0); +} + +#endif + diff --git a/thirdparty/src/newton/dgPhysics/dgContact.cpp b/thirdparty/src/newton/dgPhysics/dgContact.cpp new file mode 100644 index 000000000..3f487bed5 --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgContact.cpp @@ -0,0 +1,495 @@ +/* Copyright (c) <2003-2019> +* +* 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 "dgPhysicsStdafx.h" +#include "dgBody.h" +#include "dgWorld.h" +#include "dgContact.h" +#include "dgCollisionInstance.h" +#include "dgWorldDynamicUpdate.h" + +#define REST_RELATIVE_VELOCITY dgFloat32 (1.0e-3f) +#define MAX_DYNAMIC_FRICTION_SPEED dgFloat32 (0.3f) +#define MAX_PENETRATION_STIFFNESS dgFloat32 (50.0f) +#define DG_DIAGONAL_REGULARIZER dgFloat32 (1.0e-3f) + +//#define DG_NEW_RESTITUTION_METHOD + +////////////////////////////////////////////////////////////////////// +// Construction/Destruction +////////////////////////////////////////////////////////////////////// +dgContactMaterial::dgContactMaterial() + :m_dir0 (dgFloat32 (0.0f)) + ,m_dir1 (dgFloat32 (0.0f)) + ,m_userData(NULL) + ,m_aabbOverlap(NULL) + ,m_processContactPoint(NULL) + ,m_contactGeneration(NULL) + ,m_compoundAABBOverlap(NULL) +{ + // dgAssert ( dgInt32 (sizeof (dgContactMaterial) & 15) == 0); + dgAssert ((((dgUnsigned64) this) & 15) == 0); + m_point = dgVector (dgFloat32 (0.0f)); + m_softness = dgFloat32 (0.1f); + m_restitution = dgFloat32 (0.4f); + m_skinThickness = dgFloat32 (0.0f); + + m_staticFriction0 = dgFloat32 (0.9f); + m_staticFriction1 = dgFloat32 (0.9f); + m_dynamicFriction0 = dgFloat32 (0.5f); + m_dynamicFriction1 = dgFloat32 (0.5f); + m_dir0_Force.Clear(); + m_dir1_Force.Clear(); + m_normal_Force.Clear(); + m_flags = m_collisionEnable | m_friction0Enable | m_friction1Enable; +} + +dgContact::dgContact(dgWorld* const world, const dgContactMaterial* const material, dgBody* const body0, dgBody* const body1) + :dgConstraint() + ,dgList(world->GetAllocator()) + ,m_positAcc(dgFloat32 (10.0f)) + ,m_rotationAcc () + ,m_material(material) + ,m_closestDistance (dgFloat32 (0.0f)) + ,m_separationDistance(dgFloat32 (0.0f)) + ,m_timeOfImpact(dgFloat32 (1.0e10f)) + ,m_impulseSpeed (dgFloat32 (0.0f)) + ,m_contactPruningTolereance(world->GetContactMergeTolerance()) + ,m_broadphaseLru(0) + ,m_killContact(0) + ,m_isNewContact(1) + ,m_skeletonIntraCollision(1) + ,m_skeletonSelftCollision(1) +{ + dgAssert ((((dgUnsigned64) this) & 15) == 0); + m_maxDOF = 0; + m_isActive = 0; + m_enableCollision = true; + m_constId = m_contactConstraint; + + dgAssert (!body0->m_isdead); + dgAssert (!body1->m_isdead); + + if (body0->m_invMass.m_w > dgFloat32(0.0f)) { + m_body0 = body0; + m_body1 = body1; + } else { + m_body0 = body1; + m_body1 = body0; + } + + if (world->m_onCreateContact) { + world->m_onCreateContact(world, this); + } +} + +dgContact::dgContact(dgContact* const clone) + :dgConstraint(*clone) + ,dgList(clone->GetAllocator()) + ,m_positAcc(clone->m_positAcc) + ,m_rotationAcc(clone->m_rotationAcc) + ,m_separtingVector (clone->m_separtingVector) + ,m_material(clone->m_material) + ,m_closestDistance(clone->m_closestDistance) + ,m_separationDistance(clone->m_separationDistance) + ,m_timeOfImpact(clone->m_timeOfImpact) + ,m_impulseSpeed (clone->m_impulseSpeed) + ,m_contactPruningTolereance(clone->m_contactPruningTolereance) + ,m_broadphaseLru(clone->m_broadphaseLru) + ,m_killContact(clone->m_killContact) + ,m_isNewContact(clone->m_isNewContact) + ,m_skeletonIntraCollision(clone->m_skeletonIntraCollision) + ,m_skeletonSelftCollision(clone->m_skeletonSelftCollision) +{ + dgAssert((((dgUnsigned64) this) & 15) == 0); + m_body0 = clone->m_body0; + m_body1 = clone->m_body1; + m_maxDOF = clone->m_maxDOF; + m_constId = m_contactConstraint; + m_isActive = clone->m_isActive; + m_enableCollision = clone->m_enableCollision; + Merge (*clone); + + dgAssert(!m_body0->m_isdead); + dgAssert(!m_body1->m_isdead); + + if (m_body0->m_world->m_onCreateContact) { + dgAssert(clone->m_body0); + m_body0->m_world->m_onCreateContact(clone->m_body0->m_world, this); + } +} + +dgContact::~dgContact() +{ + dgAssert(m_body0); + if (m_body0->m_world && m_body0->m_world->m_onDestroyContact) { + m_body0->m_world->m_onDestroyContact(m_body0->m_world, this); + } + + dgList::RemoveAll(); +} + +void dgContact::SwapBodies() +{ + dgSwap (m_body0, m_body1); + dgSwap (m_link0, m_link1); +} + +void dgContact::GetInfo (dgConstraintInfo* const info) const +{ + memset (info, 0, sizeof (dgConstraintInfo)); + InitInfo (info); + info->m_collideCollisionOn = GetCount(); + strcpy (info->m_discriptionType, "contact"); +} + +void dgContact::CalculatePointDerivative (dgInt32 index, dgContraintDescritor& desc, const dgVector& dir, const dgPointParam& param) const +{ + dgAssert (m_body0); + dgAssert (m_body1); + + dgJacobian &jacobian0 = desc.m_jacobian[index].m_jacobianM0; + dgJacobian &jacobian1 = desc.m_jacobian[index].m_jacobianM1; + jacobian0.m_linear = dir; + jacobian1.m_linear = dir * dgVector::m_negOne; + + jacobian0.m_angular = param.m_r0.CrossProduct(dir); + jacobian1.m_angular = dir.CrossProduct(param.m_r1); + + dgAssert(jacobian0.m_linear.m_w == dgFloat32(0.0f)); + dgAssert(jacobian0.m_angular.m_w == dgFloat32(0.0f)); + dgAssert(jacobian1.m_linear.m_w == dgFloat32(0.0f)); + dgAssert(jacobian1.m_angular.m_w == dgFloat32(0.0f)); +} + +bool dgContact::EstimateCCD (dgFloat32 timestep) const +{ +//return false; + dgAssert (m_body0->m_continueCollisionMode | m_body1->m_continueCollisionMode); + const dgVector& veloc0 = m_body0->m_veloc; + const dgVector& veloc1 = m_body1->m_veloc; + const dgVector& omega0 = m_body0->m_omega; + const dgVector& omega1 = m_body1->m_omega; + const dgVector& com0 = m_body0->m_globalCentreOfMass; + const dgVector& com1 = m_body1->m_globalCentreOfMass; + const dgCollisionInstance* const collision0 = m_body0->m_collision; + const dgCollisionInstance* const collision1 = m_body1->m_collision; + const dgFloat32 dist = dgMax(m_body0->m_collision->GetBoxMinRadius(), m_body1->m_collision->GetBoxMinRadius()) * dgFloat32(0.25f); + + const dgVector relVeloc(veloc1 - veloc0); + const dgVector relOmega(omega1 - omega0); + const dgFloat32 relVelocMag2(relVeloc.DotProduct(relVeloc).GetScalar()); + const dgFloat32 relOmegaMag2(relOmega.DotProduct(relOmega).GetScalar()); + + if ((relOmegaMag2 > dgFloat32(1.0f)) || ((relVelocMag2 * timestep * timestep) > (dist * dist))) { + dgWorld* const world = m_body0->GetWorld(); + dgTriplex normals[16]; + dgTriplex points[16]; + dgInt64 attrib0[16]; + dgInt64 attrib1[16]; + dgFloat32 penetrations[16]; + dgFloat32 timeToImpact = timestep; + dgTrace (("function %s on file %s is too slow, consider using supsteps\n", __FILE__, __FUNCTION__)); + + const dgInt32 ccdContactCount = world->CollideContinue( + collision0, m_body0->m_matrix, veloc0, omega0, collision1, m_body1->m_matrix, veloc1, omega1, + timeToImpact, points, normals, penetrations, attrib0, attrib1, 6, 0); + + for (dgInt32 j = 0; j < ccdContactCount; j++) { + dgVector point(&points[j].m_x); + dgVector normal(&normals[j].m_x); + point = point & dgVector::m_triplexMask; + normal = normal & dgVector::m_triplexMask; + dgVector vel0(veloc0 + omega0 * (point - com0)); + dgVector vel1(veloc1 + omega1 * (point - com1)); + dgVector vRel(vel1 - vel0); + dgFloat32 contactDistTravel = vRel.DotProduct(normal).GetScalar() * timestep; + if (contactDistTravel > dist) { + return true; + } + } + } + + return false; +} + +dgUnsigned32 dgContact::JacobianDerivative (dgContraintDescritor& params) +{ + dgInt32 frictionIndex = 0; + m_impulseSpeed = dgFloat32 (0.0f); + if (m_maxDOF) { + dgInt32 i = 0; + frictionIndex = GetCount(); + for (dgList::dgListNode* node = GetFirst(); node; node = node->GetNext()) { + const dgContactMaterial& contact = node->GetInfo(); + JacobianContactDerivative (params, contact, i, frictionIndex); + i ++; + } + } + + return dgUnsigned32 (frictionIndex); +} + +void dgContact::JacobianContactDerivative (dgContraintDescritor& params, const dgContactMaterial& contact, dgInt32 normalIndex, dgInt32& frictionIndex) +{ + dgPointParam pointData; + InitPointParam (pointData, dgFloat32 (1.0f), contact.m_point, contact.m_point); + CalculatePointDerivative (normalIndex, params, contact.m_normal, pointData); + + const dgVector veloc0 = m_body0->m_veloc; + const dgVector omega0 = m_body0->m_omega; + const dgVector veloc1 = m_body1->m_veloc; + const dgVector omega1 = m_body1->m_omega; + + const dgVector gyroAlpha0(m_body0->m_gyroAlpha); + const dgVector gyroAlpha1(m_body1->m_gyroAlpha); + + dgAssert(contact.m_normal.m_w == dgFloat32(0.0f)); + const dgJacobian &normalJacobian0 = params.m_jacobian[normalIndex].m_jacobianM0; + const dgJacobian &normalJacobian1 = params.m_jacobian[normalIndex].m_jacobianM1; + + const dgFloat32 impulseOrForceScale = (params.m_timestep > dgFloat32(0.0f)) ? params.m_invTimestep : dgFloat32(1.0f); + const dgFloat32 restitutionCoefficient = contact.m_restitution; + +#ifdef DG_NEW_RESTITUTION_METHOD + const dgFloat32 relSpeed = -(normalJacobian0.m_linear * veloc0 + normalJacobian0.m_angular * omega0 + normalJacobian1.m_linear * veloc1 + normalJacobian1.m_angular * omega1).AddHorizontal().GetScalar(); + const dgFloat32 penetrationStiffness = MAX_PENETRATION_STIFFNESS * contact.m_softness; + const dgFloat32 penetration = dgClamp(contact.m_penetration - DG_RESTING_CONTACT_PENETRATION, dgFloat32(0.0f), dgFloat32(0.5f)); + + const dgFloat32 penetrationSpeed = penetration * penetrationStiffness; + + dgFloat32 jointSpeed = dgFloat32(0.0f); + if (relSpeed >= dgFloat32(0.0f)) { + const dgFloat32 restitutionSpeed = restitutionCoefficient * relSpeed; + const dgFloat32 bounceSpeed = dgMax(restitutionSpeed, penetrationSpeed); + jointSpeed = bounceSpeed + dgMax(relSpeed, dgFloat32(0.0f)); + } else { + const dgFloat32 restitutionSpeed = relSpeed + penetrationSpeed; + jointSpeed = dgMax(restitutionSpeed, dgFloat32(0.0f)); + } + + const dgFloat32 relGyro = (normalJacobian0.m_angular * m_body0->m_gyroAlpha + normalJacobian1.m_angular * m_body1->m_gyroAlpha).AddHorizontal().GetScalar(); + + params.m_jointAccel[normalIndex] = relGyro + jointSpeed * impulseOrForceScale; + if (contact.m_flags & dgContactMaterial::m_overrideNormalAccel) { + params.m_jointAccel[normalIndex] += contact.m_normal_Force.m_force; + } + + const bool isHardContact = !(contact.m_flags & dgContactMaterial::m_isSoftContact); + + params.m_flags[normalIndex] = contact.m_flags & dgContactMaterial::m_isSoftContact; + params.m_penetration[normalIndex] = penetration; + params.m_restitution[normalIndex] = restitutionCoefficient; + params.m_penetrationStiffness[normalIndex] = penetrationStiffness; + params.m_forceBounds[normalIndex].m_low = dgFloat32(0.0f); + params.m_forceBounds[normalIndex].m_normalIndex = DG_INDEPENDENT_ROW; + params.m_forceBounds[normalIndex].m_jointForce = (dgForceImpactPair*)&contact.m_normal_Force; + params.m_diagonalRegularizer[normalIndex] = isHardContact ? DG_DIAGONAL_REGULARIZER : dgMax(DG_DIAGONAL_REGULARIZER, contact.m_skinThickness); + +#else + + dgFloat32 relSpeed = -(normalJacobian0.m_linear * veloc0 + normalJacobian0.m_angular * omega0 + normalJacobian1.m_linear * veloc1 + normalJacobian1.m_angular * omega1).AddHorizontal().GetScalar(); + dgFloat32 penetration = dgClamp(contact.m_penetration - DG_RESTING_CONTACT_PENETRATION, dgFloat32(0.0f), dgFloat32(0.5f)); + params.m_flags[normalIndex] = contact.m_flags & dgContactMaterial::m_isSoftContact; + params.m_penetration[normalIndex] = penetration; + params.m_restitution[normalIndex] = restitutionCoefficient; + params.m_forceBounds[normalIndex].m_low = dgFloat32(0.0f); + params.m_forceBounds[normalIndex].m_normalIndex = DG_INDEPENDENT_ROW; + params.m_forceBounds[normalIndex].m_jointForce = (dgForceImpactPair*)&contact.m_normal_Force; + + const dgFloat32 restitutionVelocity = (relSpeed > REST_RELATIVE_VELOCITY) ? relSpeed * restitutionCoefficient : dgFloat32(0.0f); + m_impulseSpeed = dgMax(m_impulseSpeed, restitutionVelocity); + + dgFloat32 penetrationStiffness = MAX_PENETRATION_STIFFNESS * contact.m_softness; + dgFloat32 penetrationVeloc = penetration * penetrationStiffness; + dgAssert(dgAbs(penetrationVeloc - MAX_PENETRATION_STIFFNESS * contact.m_softness * penetration) < dgFloat32(1.0e-6f)); + params.m_penetrationStiffness[normalIndex] = penetrationStiffness; + relSpeed += dgMax(restitutionVelocity, penetrationVeloc); + + const bool isHardContact = !(contact.m_flags & dgContactMaterial::m_isSoftContact); + params.m_diagonalRegularizer[normalIndex] = isHardContact ? DG_DIAGONAL_REGULARIZER : dgMax (DG_DIAGONAL_REGULARIZER, contact.m_skinThickness); + const dgFloat32 relGyro = (normalJacobian0.m_angular * m_body0->m_gyroAlpha + normalJacobian1.m_angular * m_body1->m_gyroAlpha).AddHorizontal().GetScalar(); + + params.m_jointAccel[normalIndex] = relGyro + relSpeed * impulseOrForceScale; + if (contact.m_flags & dgContactMaterial::m_overrideNormalAccel) { + params.m_jointAccel[normalIndex] += contact.m_normal_Force.m_force; + } +#endif + + // first dir friction force + if (contact.m_flags & dgContactMaterial::m_friction0Enable) { + dgInt32 jacobIndex = frictionIndex; + frictionIndex += 1; + dgAssert (contact.m_dir0.m_w == dgFloat32 (0.0f)); + CalculatePointDerivative (jacobIndex, params, contact.m_dir0, pointData); + + const dgJacobian &jacobian0 = params.m_jacobian[jacobIndex].m_jacobianM0; + const dgJacobian &jacobian1 = params.m_jacobian[jacobIndex].m_jacobianM1; + dgFloat32 relVelocErr = -(jacobian0.m_linear * veloc0 + jacobian0.m_angular * omega0 + jacobian1.m_linear * veloc1 + jacobian1.m_angular * omega1).AddHorizontal().GetScalar(); + + params.m_flags[jacobIndex] = 0; + params.m_forceBounds[jacobIndex].m_normalIndex = dgInt16 ((contact.m_flags & dgContactMaterial::m_override0Friction) ? DG_INDEPENDENT_ROW : normalIndex); + params.m_diagonalRegularizer[jacobIndex] = DG_DIAGONAL_REGULARIZER; + + params.m_restitution[jacobIndex] = dgFloat32(0.0f); + params.m_penetration[jacobIndex] = dgFloat32(0.0f); + + params.m_penetrationStiffness[jacobIndex] = dgFloat32 (0.0f); + if (contact.m_flags & dgContactMaterial::m_override0Accel) { + // note: using restitution been negative to indicate that the acceleration was override + params.m_restitution[jacobIndex] = dgFloat32 (-1.0f); + params.m_jointAccel[jacobIndex] = contact.m_dir0_Force.m_force; + } else { + const dgFloat32 relFrictionGyro = (jacobian0.m_angular * gyroAlpha0 + jacobian1.m_angular * gyroAlpha1).AddHorizontal().GetScalar(); + params.m_restitution[jacobIndex] = dgFloat32 (0.0f); + params.m_jointAccel[jacobIndex] = relFrictionGyro + relVelocErr * impulseOrForceScale; + } + if (dgAbs (relVelocErr) > MAX_DYNAMIC_FRICTION_SPEED) { + params.m_forceBounds[jacobIndex].m_low = -contact.m_dynamicFriction0; + params.m_forceBounds[jacobIndex].m_upper = contact.m_dynamicFriction0; + } else { + params.m_forceBounds[jacobIndex].m_low = -contact.m_staticFriction0; + params.m_forceBounds[jacobIndex].m_upper = contact.m_staticFriction0; + } + params.m_forceBounds[jacobIndex].m_jointForce = (dgForceImpactPair*)&contact.m_dir0_Force; + } + + if (contact.m_flags & dgContactMaterial::m_friction1Enable) { + dgInt32 jacobIndex = frictionIndex; + frictionIndex += 1; + dgAssert (contact.m_dir1.m_w == dgFloat32 (0.0f)); + CalculatePointDerivative (jacobIndex, params, contact.m_dir1, pointData); + + const dgJacobian &jacobian0 = params.m_jacobian[jacobIndex].m_jacobianM0; + const dgJacobian &jacobian1 = params.m_jacobian[jacobIndex].m_jacobianM1; + dgFloat32 relVelocErr = -(jacobian0.m_linear * veloc0 + jacobian0.m_angular * omega0 + jacobian1.m_linear * veloc1 + jacobian1.m_angular * omega1).AddHorizontal().GetScalar(); + + params.m_flags[jacobIndex] = 0; + params.m_forceBounds[jacobIndex].m_normalIndex = dgInt16 ((contact.m_flags & dgContactMaterial::m_override1Friction) ? DG_INDEPENDENT_ROW : normalIndex); + params.m_diagonalRegularizer[jacobIndex] = DG_DIAGONAL_REGULARIZER; + + params.m_restitution[jacobIndex] = dgFloat32 (0.0f); + params.m_penetration[jacobIndex] = dgFloat32 (0.0f); + params.m_penetrationStiffness[jacobIndex] = dgFloat32 (0.0f); + if (contact.m_flags & dgContactMaterial::m_override1Accel) { + // note: using restitution been negative to indicate that the acceleration was override + params.m_restitution[jacobIndex] = dgFloat32 (-1.0f); + params.m_jointAccel[jacobIndex] = contact.m_dir1_Force.m_force; + } else { + const dgFloat32 relFrictionGyro = (jacobian0.m_angular * gyroAlpha0 + jacobian1.m_angular * gyroAlpha1).AddHorizontal().GetScalar(); + params.m_restitution[jacobIndex] = dgFloat32 (0.0f); + params.m_jointAccel[jacobIndex] = relFrictionGyro + relVelocErr * impulseOrForceScale; + } + if (dgAbs (relVelocErr) > MAX_DYNAMIC_FRICTION_SPEED) { + params.m_forceBounds[jacobIndex].m_low = - contact.m_dynamicFriction1; + params.m_forceBounds[jacobIndex].m_upper = contact.m_dynamicFriction1; + } else { + params.m_forceBounds[jacobIndex].m_low = - contact.m_staticFriction1; + params.m_forceBounds[jacobIndex].m_upper = contact.m_staticFriction1; + } + params.m_forceBounds[jacobIndex].m_jointForce = (dgForceImpactPair*)&contact.m_dir1_Force; + } +} + +void dgContact::JointAccelerations(dgJointAccelerationDecriptor* const params) +{ + const dgVector& bodyVeloc0 = m_body0->m_veloc; + const dgVector& bodyOmega0 = m_body0->m_omega; + const dgVector& bodyVeloc1 = m_body1->m_veloc; + const dgVector& bodyOmega1 = m_body1->m_omega; + + const dgInt32 count = params->m_rowsCount; + + dgFloat32 timestep = dgFloat32 (1.0f); + dgFloat32 invTimestep = dgFloat32 (1.0f); + if (params->m_timeStep > dgFloat32 (0.0f)) { + timestep = params->m_timeStep; + invTimestep = params->m_invTimeStep; + } + + dgRightHandSide* const rightHandSide = params->m_rightHandSide; + const dgLeftHandSide* const leftHandSide = params->m_leftHandSide; + + const dgVector gyroAlpha0(m_body0->m_gyroAlpha); + const dgVector gyroAlpha1(m_body1->m_gyroAlpha); + + for (dgInt32 k = 0; k < count; k ++) { + // note: using restitution been negative to indicate that the acceleration was override + dgRightHandSide* const rhs = &rightHandSide[k]; + if (rhs->m_restitution >= dgFloat32 (0.0f)) { + const dgLeftHandSide* const row = &leftHandSide[k]; + const dgJacobian &jacobian0 = row->m_Jt.m_jacobianM0; + const dgJacobian &jacobian1 = row->m_Jt.m_jacobianM1; + + dgVector relVeloc (jacobian0.m_linear * bodyVeloc0 + jacobian0.m_angular * bodyOmega0 + jacobian1.m_linear * bodyVeloc1 + jacobian1.m_angular * bodyOmega1); + dgFloat32 vRel = relVeloc.AddHorizontal().GetScalar(); + dgFloat32 aRel = rhs->m_deltaAccel; + + if (rhs->m_normalForceIndex == DG_INDEPENDENT_ROW) { + dgAssert (rhs->m_restitution >= 0.0f); + dgAssert (rhs->m_restitution <= 2.0f); + + #ifdef DG_NEW_RESTITUTION_METHOD + //const dgFloat32 penetration = rhs->m_penetration; + if (vRel < dgFloat32(0.0f)) { + const dgFloat32 restitutionSpeed = rhs->m_restitution * vRel; + const dgFloat32 penetrationSpeed = - rhs->m_penetration * rhs->m_penetrationStiffness; + const dgFloat32 penetrationCorrection = -vRel * timestep * rhs->m_restitution; + if (penetrationCorrection > rhs->m_penetration) { + rhs->m_penetration = dgFloat32(0.001f); + } + const dgFloat32 bounceSpeed = dgMin(restitutionSpeed, penetrationSpeed); + vRel += bounceSpeed; + } else { + const dgFloat32 penetrationSpeed = rhs->m_penetration * rhs->m_penetrationStiffness; + const dgFloat32 penetrationCorrection = vRel * timestep * rhs->m_restitution; + if (penetrationCorrection > rhs->m_penetration) { + rhs->m_penetration = dgFloat32(0.001f); + } + vRel = (vRel < penetrationSpeed) ? penetrationSpeed : dgFloat32 (0.0f); + } + + #else + dgFloat32 penetrationVeloc = dgFloat32 (0.0f); + dgFloat32 restitution = (vRel <= dgFloat32 (0.0f)) ? (dgFloat32 (1.0f) + rhs->m_restitution) : dgFloat32 (1.0f); + if (rhs->m_penetration > DG_RESTING_CONTACT_PENETRATION * dgFloat32 (0.125f)) { + if (vRel > dgFloat32 (0.0f)) { + dgFloat32 penetrationCorrection = vRel * timestep; + dgAssert (penetrationCorrection >= dgFloat32 (0.0f)); + rhs->m_penetration = dgMax (dgFloat32 (0.0f), rhs->m_penetration - penetrationCorrection); + } else { + dgFloat32 penetrationCorrection = -vRel * timestep * rhs->m_restitution * dgFloat32 (8.0f); + if (penetrationCorrection > rhs->m_penetration) { + rhs->m_penetration = dgFloat32 (0.001f); + } + } + penetrationVeloc = -(rhs->m_penetration * rhs->m_penetrationStiffness); + } + vRel = vRel * restitution + penetrationVeloc; + #endif + } + + const dgFloat32 relGyro = (jacobian0.m_angular * gyroAlpha0 + jacobian1.m_angular * gyroAlpha1).AddHorizontal().GetScalar(); + rhs->m_coordenateAccel = relGyro + aRel - vRel * invTimestep; + } + } +} + diff --git a/thirdparty/src/newton/dgPhysics/dgContact.h b/thirdparty/src/newton/dgPhysics/dgContact.h new file mode 100644 index 000000000..e01d6bb68 --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgContact.h @@ -0,0 +1,380 @@ +/* Copyright (c) <2003-2019> +* +* 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 __DGCONTACT_H__ +#define __DGCONTACT_H__ + +#include "dgConstraint.h" +#include "dgContactSolver.h" + +class dgBody; +class dgWorld; +class dgContact; +class dgContactPoint; +class dgContactMaterial; +class dgPolygonMeshDesc; +class dgCollisionInstance; + + +#define DG_MAX_CONTATCS 128 +#define DG_RESTING_CONTACT_PENETRATION (DG_PENETRATION_TOL + dgFloat32 (1.0f / 1024.0f)) +#define DG_DIAGONAL_PRECONDITIONER dgFloat32 (25.0f) + +class dgContactList: public dgArray +{ + public: + dgContactList(dgMemoryAllocator* const allocator) + :dgArray(allocator) + ,m_contactCount(0) + ,m_contactCountReset(0) + ,m_activeContactCount(0) + { + Resize (1024 * 32); + } + + ~dgContactList() + { + } + + void Push (dgContact* const contact) + { + dgInt32 index = dgAtomicExchangeAndAdd(&m_contactCount, 1); + dgAssert (index < GetElementsCapacity()); + (*this)[index] = contact; + } + + dgInt32 m_contactCount; + dgInt32 m_contactCountReset; + dgInt32 m_activeContactCount; +}; + +DG_MSC_VECTOR_ALIGNMENT +class dgCollisionParamProxy +{ + public: + dgCollisionParamProxy(dgContact* const contact, dgContactPoint* const contactBuffer, dgInt32 threadIndex, bool ccdMode, bool intersectionTestOnly) + :m_normal(dgVector::m_zero) + ,m_closestPointBody0(dgVector::m_zero) + ,m_closestPointBody1(dgVector::m_zero) + ,m_contactJoint(contact) + ,m_contacts(contactBuffer) + ,m_polyMeshData(NULL) + ,m_threadIndex(threadIndex) + ,m_continueCollision(ccdMode) + ,m_intersectionTestOnly(intersectionTestOnly) + { + } + + dgVector m_normal; + dgVector m_closestPointBody0; + dgVector m_closestPointBody1; + dgContact* m_contactJoint; + dgBody* m_body0; + dgBody* m_body1; + dgCollisionInstance* m_instance0; + dgCollisionInstance* m_instance1; + dgContactPoint* m_contacts; + dgPolygonMeshDesc* m_polyMeshData; + + dgFloat32 m_timestep; + dgFloat32 m_skinThickness; + dgInt32 m_threadIndex; + dgInt32 m_maxContacts; + bool m_continueCollision; + bool m_intersectionTestOnly; +} DG_GCC_VECTOR_ALIGNMENT; + + +DG_MSC_VECTOR_ALIGNMENT +class dgContactPoint +{ + public: + dgVector m_point; + dgVector m_normal; + const dgBody* m_body0; + const dgBody* m_body1; + const dgCollisionInstance* m_collision0; + const dgCollisionInstance* m_collision1; + dgInt64 m_shapeId0; + dgInt64 m_shapeId1; + dgFloat32 m_penetration; +}DG_GCC_VECTOR_ALIGNMENT; + + +DG_MSC_VECTOR_ALIGNMENT +class dgContactMaterial: public dgContactPoint +{ + public: + enum { + m_isSoftContact = 1<<0, + m_collisionEnable = 1<<1, + m_friction0Enable = 1<<2, + m_friction1Enable = 1<<3, + m_override0Accel = 1<<4, + m_override1Accel = 1<<5, + m_override0Friction = 1<<6, + m_override1Friction = 1<<7, + m_overrideNormalAccel = 1<<8, + m_resetSkeletonSelfCollision = 1<<9, + m_resetSkeletonIntraCollision = 1<<10, + }; + + DG_MSC_VECTOR_ALIGNMENT + class dgUserContactPoint + { + public: + dgVector m_point; + dgVector m_normal; + dgUnsigned64 m_shapeId0; + dgUnsigned64 m_shapeId1; + dgFloat32 m_penetration; + dgUnsigned32 m_unused[3]; + } DG_GCC_VECTOR_ALIGNMENT; + + typedef bool (dgApi *OnAABBOverlap) (dgContact& contactJoint, dgFloat32 timestep, dgInt32 threadIndex); + typedef void (dgApi *OnContactCallback) (dgContact& contactJoint, dgFloat32 timestep, dgInt32 threadIndex); + typedef bool (dgApi *OnCompoundCollisionPrefilter) (dgContact& contactJoint, dgFloat32 timestep, const dgBody* bodyA, const void* collisionNodeA, const dgBody* bodyB, const void* collisionNodeB, dgInt32 threadIndex); + typedef bool (dgApi *OnContactGeneration) (const dgContactMaterial& material, const dgBody& body0, const dgCollisionInstance* collisionIntance0, const dgBody& body1, const dgCollisionInstance* collisionIntance1, dgUserContactPoint* const contacts, dgInt32 maxCount, dgInt32 threadIndex); + + dgContactMaterial(); + void* GetUserData () const; + void SetUserData (void* const userData); + void SetAsSoftContact (dgFloat32 regularizer); + void SetCollisionGenerationCallback (OnContactGeneration contactGeneration); + void SetCollisionCallback (OnAABBOverlap abbOvelap, OnContactCallback callback); + void SetCompoundCollisionCallback (OnCompoundCollisionPrefilter abbCompounndOvelap); + + dgVector m_dir0; + dgVector m_dir1; + dgForceImpactPair m_normal_Force; + dgForceImpactPair m_dir0_Force; + dgForceImpactPair m_dir1_Force; + dgFloat32 m_restitution; + dgFloat32 m_staticFriction0; + dgFloat32 m_staticFriction1; + dgFloat32 m_dynamicFriction0; + dgFloat32 m_dynamicFriction1; + dgFloat32 m_softness; + dgFloat32 m_skinThickness; + dgInt32 m_flags; + + private: + void *m_userData; + OnAABBOverlap m_aabbOverlap; + OnContactCallback m_processContactPoint; + OnContactGeneration m_contactGeneration; + OnCompoundCollisionPrefilter m_compoundAABBOverlap; + + friend class dgWorld; + friend class dgBroadPhase; + friend class dgCollisionScene; + friend class dgCollisionCompound; + friend class dgWorldDynamicUpdate; + friend class dgSolverWorlkerThreads; + friend class dgCollidingPairCollector; + friend class dgBroadPhaseMaterialCallbackWorkerThread; + +}DG_GCC_VECTOR_ALIGNMENT; + + +DG_MSC_VECTOR_ALIGNMENT +class dgContact: public dgConstraint, public dgList +{ + public: + void ResetSkeletonSelftCollision(); + void ResetSkeletonIntraCollision(); + dgFloat32 GetTimeOfImpact() const; + dgFloat32 GetClosestDistance() const; + void SetTimeOfImpact(dgFloat32 timetoImpact); + const dgContactMaterial* GetMaterial() const; + + dgFloat32 GetPruningTolerance() const; + void SetPruningTolerance(dgFloat32 tolerance); + + protected: + dgContact(dgContact* const clone); + dgContact(dgWorld* const world, const dgContactMaterial* const material, dgBody* const body0, dgBody* const body1); + virtual ~dgContact(); + + DG_CLASS_ALLOCATOR(allocator) + + virtual void ResetMaxDOF(); + virtual dgFloat32 GetImpulseContactSpeed() const; + virtual void SetImpulseContactSpeed(dgFloat32 speed); + virtual void GetInfo (dgConstraintInfo* const info) const; + virtual dgUnsigned32 JacobianDerivative (dgContraintDescritor& params); + virtual void JointAccelerations (dgJointAccelerationDecriptor* const params); + virtual bool IsDeformable() const ; + virtual void SetDestructorCallback (OnConstraintDestroy destructor); + + bool IsSkeletonIntraCollision() const; + bool IsSkeletonSelftCollision() const; + + void JacobianContactDerivative (dgContraintDescritor& params, const dgContactMaterial& contact, dgInt32 normalIndex, dgInt32& frictionIndex); + void CalculatePointDerivative (dgInt32 index, dgContraintDescritor& desc, const dgVector& dir, const dgPointParam& param) const; + + void SwapBodies(); + bool EstimateCCD (dgFloat32 timestep) const; + + dgVector m_positAcc; + dgQuaternion m_rotationAcc; + dgVector m_separtingVector; + const dgContactMaterial* m_material; + dgFloat32 m_closestDistance; + dgFloat32 m_separationDistance; + dgFloat32 m_timeOfImpact; + dgFloat32 m_impulseSpeed; + dgFloat32 m_contactPruningTolereance; + dgUnsigned32 m_broadphaseLru; + dgUnsigned32 m_killContact : 1; + dgUnsigned32 m_isNewContact : 1; + dgUnsigned32 m_skeletonIntraCollision : 1; + dgUnsigned32 m_skeletonSelftCollision : 1; + + friend class dgBody; + friend class dgWorld; + friend class dgDeadBodies; + friend class dgBroadPhase; + friend class dgContactList; + friend class dgContactSolver; + friend class dgCollisionScene; + friend class dgCollisionConvex; + friend class dgCollisionCompound; + friend class dgBodyMasterListRow; + friend class dgWorldDynamicUpdate; + friend class dgSolverWorlkerThreads; + friend class dgCollisionConvexPolygon; + friend class dgCollidingPairCollector; + +}DG_GCC_VECTOR_ALIGNMENT; + +DG_INLINE void dgContactMaterial::SetCollisionCallback (OnAABBOverlap aabbOverlap, OnContactCallback contact) +{ + m_aabbOverlap = aabbOverlap; + m_processContactPoint = contact; +} + +DG_INLINE void dgContactMaterial::SetCollisionGenerationCallback (OnContactGeneration contactGeneration) +{ + m_contactGeneration = contactGeneration; +} + +DG_INLINE void dgContactMaterial::SetCompoundCollisionCallback (OnCompoundCollisionPrefilter aabbOverlap) +{ + m_compoundAABBOverlap = aabbOverlap; +} + +DG_INLINE void* dgContactMaterial::GetUserData () const +{ + return m_userData; +} + +DG_INLINE void dgContactMaterial::SetUserData (void* const userData) +{ + m_userData = userData; +} + +DG_INLINE void dgContactMaterial::SetAsSoftContact(dgFloat32 regularizer) +{ + dgAssert(regularizer >= dgFloat32 (0.0f)); + dgAssert(regularizer <= dgFloat32 (1.0f)); + // re purpose some of the variable to store parameter for soft contact + m_flags |= m_isSoftContact; + m_skinThickness = regularizer; +} + +DG_INLINE const dgContactMaterial* dgContact::GetMaterial() const +{ + return m_material; +} + +DG_INLINE bool dgContact::IsDeformable() const +{ + return false; +} + +DG_INLINE void dgContact::SetDestructorCallback (OnConstraintDestroy destructor) +{ +} + +DG_INLINE void dgContact::SetTimeOfImpact(dgFloat32 timetoImpact) +{ + m_timeOfImpact = timetoImpact; +} + +DG_INLINE dgFloat32 dgContact::GetTimeOfImpact() const +{ + return m_timeOfImpact; +} + +DG_INLINE dgFloat32 dgContact::GetClosestDistance() const +{ + return m_closestDistance; +} + +DG_INLINE void dgContact::ResetMaxDOF() +{ + m_maxDOF = 0; +} + +DG_INLINE dgFloat32 dgContact::GetPruningTolerance() const +{ + return m_contactPruningTolereance; +} + +DG_INLINE void dgContact::SetPruningTolerance(dgFloat32 tolerance) +{ + m_contactPruningTolereance = dgAbs (tolerance); +} + +DG_INLINE void dgContact::ResetSkeletonIntraCollision() +{ + m_skeletonIntraCollision = 0; +} + +DG_INLINE bool dgContact::IsSkeletonIntraCollision() const +{ + return m_skeletonIntraCollision; +} + +DG_INLINE void dgContact::ResetSkeletonSelftCollision() +{ + m_skeletonSelftCollision = 0; +} + +DG_INLINE bool dgContact::IsSkeletonSelftCollision() const +{ + return m_skeletonSelftCollision; +} + +DG_INLINE dgFloat32 dgContact::GetImpulseContactSpeed() const +{ + return m_impulseSpeed; +} + +DG_INLINE void dgContact::SetImpulseContactSpeed(dgFloat32 speed) +{ + m_impulseSpeed = speed; +} + + +#endif + diff --git a/thirdparty/src/newton/dgPhysics/dgContactSolver.cpp b/thirdparty/src/newton/dgPhysics/dgContactSolver.cpp new file mode 100644 index 000000000..277f0402a --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgContactSolver.cpp @@ -0,0 +1,1511 @@ +/* Copyright (c) <2003-2019> +* +* 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 "dgPhysicsStdafx.h" +#include "dgBody.h" +#include "dgWorld.h" +#include "dgContact.h" +#include "dgContactSolver.h" +#include "dgCollisionMesh.h" +#include "dgCollisionConvex.h" +#include "dgCollisionInstance.h" +#include "dgCollisionConvexHull.h" +#include "dgCollisionConvexPolygon.h" + +dgVector dgContactSolver::m_hullDirs[] = +{ + dgVector(dgFloat32(0.577350f), dgFloat32(-0.577350f), dgFloat32(0.577350f), dgFloat32(0.0f)), + dgVector(dgFloat32(-0.577350f), dgFloat32(-0.577350f), dgFloat32(-0.577350f), dgFloat32(0.0f)), + dgVector(dgFloat32(0.577350f), dgFloat32(-0.577350f), dgFloat32(-0.577350f), dgFloat32(0.0f)), + dgVector(dgFloat32(-0.577350f), dgFloat32(0.577350f), dgFloat32(0.577350f), dgFloat32(0.0f)), + dgVector(dgFloat32(0.577350f), dgFloat32(0.577350f), dgFloat32(-0.577350f), dgFloat32(0.0f)), + dgVector(dgFloat32(-0.577350f), dgFloat32(0.577350f), dgFloat32(-0.577350f), dgFloat32(0.0f)), + dgVector(dgFloat32(-0.577350f), dgFloat32(-0.577350f), dgFloat32(0.577350f), dgFloat32(0.0f)), + dgVector(dgFloat32(0.577350f), dgFloat32(0.577350f), dgFloat32(0.577350f), dgFloat32(0.0f)), + dgVector(dgFloat32(0.000000f), dgFloat32(-1.000000f), dgFloat32(0.000000f), dgFloat32(0.0f)), + dgVector(dgFloat32(0.000000f), dgFloat32(1.000000f), dgFloat32(0.000000f), dgFloat32(0.0f)), + dgVector(dgFloat32(1.000000f), dgFloat32(0.000000f), dgFloat32(0.000000f), dgFloat32(0.0f)), + dgVector(dgFloat32(-1.000000f), dgFloat32(0.000000f), dgFloat32(0.000000f), dgFloat32(0.0f)), + dgVector(dgFloat32(0.000000f), dgFloat32(0.000000f), dgFloat32(1.000000f), dgFloat32(0.0f)), + dgVector(dgFloat32(0.000000f), dgFloat32(0.000000f), dgFloat32(-1.000000f), dgFloat32(0.0f)), +}; + +dgInt32 dgContactSolver::m_rayCastSimplex[4][4] = +{ + { 0, 1, 2, 3 }, + { 0, 2, 3, 1 }, + { 2, 1, 3, 0 }, + { 1, 0, 3, 2 }, +}; + +dgContactSolver::dgContactSolver(dgCollisionInstance* const instance) + :dgDownHeap(m_heapBuffer, sizeof (m_heapBuffer)) + ,m_proxy (NULL) + ,m_instance0(instance) + ,m_instance1(instance) + ,m_vertexIndex(0) +{ +} + +dgContactSolver::dgContactSolver(dgCollisionParamProxy* const proxy) + :dgDownHeap(m_heapBuffer, sizeof (m_heapBuffer)) + ,m_normal (proxy->m_contactJoint->m_separtingVector) + ,m_proxy (proxy) + ,m_instance0(proxy->m_instance0) + ,m_instance1(proxy->m_instance1) + ,m_vertexIndex(0) +{ +} + +DG_INLINE void dgContactSolver::SupportVertex(const dgVector& dir0, dgInt32 vertexIndex) +{ + dgAssert(dir0.m_w == dgFloat32(0.0f)); + dgAssert(dgAbs(dir0.DotProduct(dir0).GetScalar() - dgFloat32(1.0f)) < dgFloat32(1.0e-3f)); + dgVector dir1 (dir0.Scale(dgFloat32 (-1.0f))); + + const dgMatrix& matrix0 = m_instance0->m_globalMatrix; + const dgMatrix& matrix1 = m_instance1->m_globalMatrix; + dgVector p(matrix0.TransformVector(m_instance0->SupportVertexSpecial(matrix0.UnrotateVector (dir0), NULL)) & dgVector::m_triplexMask); + dgVector q(matrix1.TransformVector(m_instance1->SupportVertexSpecial(matrix1.UnrotateVector (dir1), NULL)) & dgVector::m_triplexMask); + m_hullDiff[vertexIndex] = p - q; + m_hullSum[vertexIndex] = p + q; +} + + +DG_INLINE dgBigVector dgContactSolver::ReduceLine(dgInt32& indexOut) +{ + const dgBigVector p0(m_hullDiff[0]); + const dgBigVector p1(m_hullDiff[1]); + const dgBigVector dp(p1 - p0); + dgBigVector v; + + const dgFloat64 mag2 = dp.DotProduct(dp).GetScalar(); + dgAssert (mag2 > dgFloat64 (0.0f)); + if (mag2 < dgFloat32(1.0e-24f)) { + v = p0; + indexOut = 1; + } else { + const dgFloat64 alpha0 = - p0.DotProduct(dp).GetScalar(); + if (alpha0 > mag2) { + v = p1; + indexOut = 1; + m_hullSum[0] = m_hullSum[1]; + m_hullDiff[0] = m_hullDiff[1]; + } else if (alpha0 < dgFloat64(0.0f)) { + v = p0; + indexOut = 1; + } else { + v = p0 + dp.Scale(alpha0 / mag2); + } + } + return v; +} + +DG_INLINE dgBigVector dgContactSolver::ReduceTriangle (dgInt32& indexOut) +{ + const dgBigVector p0(m_hullDiff[0]); + const dgBigVector p1(m_hullDiff[1]); + const dgBigVector p2(m_hullDiff[2]); + 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)) { + const dgFloat64 b0 = -e10.DotProduct(p0).GetScalar(); + const dgFloat64 b1 = -e20.DotProduct(p0).GetScalar(); + + const dgFloat64 u2 = b1 * a00 - a01 * b0; + const dgFloat64 u1 = b0 * a11 - a01 * b1; + + if (u2 < dgFloat32(0.0f)) { + // this looks funny but it is correct + } else if (u1 < dgFloat32(0.0f)) { + m_hullSum[1] = m_hullSum[2]; + m_hullDiff[1] = m_hullDiff[2]; + } else if ((u1 + u2) > det) { + m_hullSum[0] = m_hullSum[2]; + m_hullDiff[0] = m_hullDiff[2]; + } else { + return p0 + (e10.Scale(u1) + e20.Scale(u2)).Scale(dgFloat64(1.0f) / det); + } + indexOut = 2; + return ReduceLine(indexOut); + } + // this is a degenerated triangle. this should never happens + dgAssert(0); + return dgBigVector::m_zero; +} + +DG_INLINE dgBigVector dgContactSolver::ReduceTetrahedrum (dgInt32& indexOut) +{ + const dgBigVector p0(m_hullDiff[0]); + const dgBigVector p1(m_hullDiff[1]); + const dgBigVector p2(m_hullDiff[2]); + const dgBigVector p3(m_hullDiff[3]); + 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)) { + const dgFloat64 d2 = sqrt(desc22); + const dgFloat64 invd2 = dgFloat64(1.0f) / d2; + const dgFloat64 b0 = -e10.DotProduct(p0).GetScalar(); + const dgFloat64 b1 = -e20.DotProduct(p0).GetScalar(); + const dgFloat64 b2 = -e30.DotProduct(p0).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 + } else if (u2 < dgFloat64(0.0f)) { + m_hullSum[2] = m_hullSum[3]; + m_hullDiff[2] = m_hullDiff[3]; + } else if (u1 < dgFloat64(0.0f)) { + m_hullSum[1] = m_hullSum[3]; + m_hullDiff[1] = m_hullDiff[3]; + } else if (u1 + u2 + u3 > dgFloat64(1.0f)) { + m_hullSum[0] = m_hullSum[3]; + m_hullDiff[0] = m_hullDiff[3]; + } else { + return dgBigVector::m_zero; + } + indexOut = 3; + return ReduceTriangle(indexOut); + } + } + } + // this is a degenerated tetra. this should never happens. + // it seems this does happens about once per several millions calls, + // I will assume is acceptable. No fall back needed + //dgAssert (0); + return dgBigVector::m_zero; +} + + +//DG_INLINE dgInt32 dgContactSolver::CalculateClosestSimplex () +dgInt32 dgContactSolver::CalculateClosestSimplex() +{ + dgBigVector v(dgFloat32 (0.0f)); + dgInt32 index = 1; + if (m_vertexIndex <= 0) { + SupportVertex (m_proxy->m_contactJoint->m_separtingVector, 0); + v = m_hullDiff[0]; + } else { + switch (m_vertexIndex) + { + case 1: + { + v = m_hullDiff[0]; + break; + } + + case 2: + { + v = ReduceLine (m_vertexIndex); + break; + } + + case 3: + { + v = ReduceTriangle (m_vertexIndex); + break; + } + + case 4: + { + v = ReduceTetrahedrum (m_vertexIndex); + break; + } + } + index = m_vertexIndex; + } + + dgVector bestNormal (m_normal); + + dgInt32 iter = 0; + dgInt32 cycling = 0; + dgFloat64 minDist = dgFloat32 (1.0e20f); + dgFloat64 bestNormalDist = dgFloat32 (1.0e20f); + do { + dgFloat64 dist = v.DotProduct(v).GetScalar(); + if (dist < dgFloat32 (1.0e-9f)) { + // very deep penetration, resolve with generic Minkowsky solver + return -index; + } + + if (dist < minDist) { + minDist = dist; + cycling = -1; + } + cycling ++; + if (cycling > 4) { + // for now return -1 + return -index; + } + + const dgVector dir (v.Scale (-dgRsqrt(dist))); + dgAssert (dir.m_w == dgFloat32 (0.0f)); + SupportVertex (dir, index); + + const dgBigVector w (m_hullDiff[index]); + const dgVector wv (w - v); + dgAssert (wv.m_w == dgFloat32 (0.0f)); + const dgFloat64 dist1 = dir.DotProduct(wv).GetScalar(); + if (dist1 < dgFloat64 (1.0e-3f)) { + m_normal = dir; + return index; + } + + if (dist1 < bestNormalDist) { + bestNormal = dir; + bestNormalDist = dist1; + } + + index ++; + switch (index) + { + case 2: + { + v = ReduceLine (index); + break; + } + + case 3: + { + v = ReduceTriangle (index); + break; + } + + case 4: + { + v = ReduceTetrahedrum (index); + break; + } + } + + iter ++; + } while (iter < DG_CONNICS_CONTATS_ITERATIONS); + m_normal = bestNormal; + return (index < 4) ? index : -4; +} + + +DG_INLINE dgMinkFace* dgContactSolver::NewFace() +{ + dgMinkFace* face = (dgMinkFace*)m_freeFace; + if (m_freeFace) { + m_freeFace = m_freeFace->m_next; + } else { + face = &m_facePool[m_faceIndex]; + m_faceIndex++; + if (m_faceIndex >= DG_CONVEX_MINK_MAX_FACES) { + return NULL; + } + } + +#ifdef _DEBUG + memset(face, 0, sizeof (dgMinkFace)); +#endif + return face; +} + +DG_INLINE dgMinkFace* dgContactSolver::AddFace(dgInt32 v0, dgInt32 v1, dgInt32 v2) +{ + dgMinkFace* const face = NewFace(); + face->m_mark = 0; + face->m_vertex[0] = dgInt16(v0); + face->m_vertex[1] = dgInt16(v1); + face->m_vertex[2] = dgInt16(v2); + return face; +} + +DG_INLINE void dgContactSolver::DeleteFace(dgMinkFace* const face) +{ + dgFaceFreeList* const freeFace = (dgFaceFreeList*)face; + freeFace->m_next = m_freeFace; + m_freeFace = freeFace; +} + + +DG_INLINE void dgContactSolver::PushFace(dgMinkFace* const face) +{ + dgInt32 i0 = face->m_vertex[0]; + dgInt32 i1 = face->m_vertex[1]; + dgInt32 i2 = face->m_vertex[2]; + + dgPlane plane(m_hullDiff[i0], m_hullDiff[i1], m_hullDiff[i2]); + dgFloat32 mag2 = plane.DotProduct(plane & dgVector::m_triplexMask).GetScalar(); + face->m_alive = 1; + if (mag2 > dgFloat32(1.0e-16f)) { + face->m_plane = plane.Scale(dgRsqrt(mag2)); + dgMinkFace* face1 = face; + Push(face1, face->m_plane.m_w); + } else { + face->m_plane = dgPlane(dgVector::m_zero); + } +} + +dgInt32 dgContactSolver::CalculateIntersectingPlane(dgInt32 count) +{ + dgAssert(count >= 1); + if (count == 1) { + SupportVertex(m_proxy->m_contactJoint->m_separtingVector.Scale(dgFloat32(-1.0f)), 1); + dgVector err(m_hullDiff[1] - m_hullDiff[0]); + dgAssert (err.m_w == dgFloat32 (0.0f)); + if (err.DotProduct(err).GetScalar() < dgFloat32(1.0e-8f)) { + return -1; + } + count = 2; + } + + if (count == 2) { + dgVector e0(m_hullDiff[1] - m_hullDiff[0]); + dgAssert (e0.m_w == dgFloat32 (0.0f)); + dgAssert(e0.DotProduct(e0).GetScalar() > dgFloat32(0.0f)); + dgMatrix matrix(e0.Scale(dgRsqrt(e0.DotProduct(e0).GetScalar()))); + dgMatrix rotation(dgPitchMatrix(dgFloat32(45.0f * dgDegreeToRad))); + dgFloat32 maxArea = dgFloat32(0.0f); + for (dgInt32 i = 0; i < 8; i++) { + SupportVertex(matrix[1], 3); + dgVector e1(m_hullDiff[3] - m_hullDiff[0]); + dgAssert (e1.m_w == dgFloat32 (0.0f)); + dgVector area(e0.CrossProduct(e1)); + dgFloat32 area2 = area.DotProduct(area).GetScalar(); + if (area2 > maxArea) { + m_hullSum[2] = m_hullSum[3]; + m_hullDiff[2] = m_hullDiff[3]; + maxArea = area2; + } + matrix = rotation * matrix; + } + if (dgAbs (maxArea) < dgFloat32(1e-15f)) { + return -1; + } + dgAssert(maxArea > dgFloat32(0.0f)); + count++; + } + + dgFloat32 volume = dgFloat32(0.0f); + if (count == 3) { + dgVector e10(m_hullDiff[1] - m_hullDiff[0]); + dgVector e20(m_hullDiff[2] - m_hullDiff[0]); + dgVector normal(e10.CrossProduct(e20)); + dgAssert (normal.m_w == dgFloat32 (0.0f)); + dgFloat32 mag2 = normal.DotProduct(normal).GetScalar(); + dgAssert(mag2 > dgFloat32(0.0f)); + normal = normal.Scale(dgRsqrt(mag2)); + SupportVertex(normal, 3); + volume = normal.DotProduct(m_hullDiff[3] - m_hullDiff[0]).GetScalar(); + if (dgAbs(volume) < dgFloat32(1.0e-10f)) { + normal = normal.Scale(dgFloat32(-1.0f)); + SupportVertex(normal, 3); + volume = - normal.DotProduct(m_hullDiff[3] - m_hullDiff[0]).GetScalar(); + if (dgAbs(volume) < dgFloat32(1.0e-10f)) { + volume = dgFloat32(0.0f); + } + } + count = 4; + } else if (count == 4) { + dgVector e0(m_hullDiff[1] - m_hullDiff[0]); + dgVector e1(m_hullDiff[2] - m_hullDiff[0]); + dgVector e2(m_hullDiff[3] - m_hullDiff[0]); + dgVector n(e1.CrossProduct(e2)); + dgAssert (n.m_w == dgFloat32 (0.0f)); + volume = e0.DotProduct(n).GetScalar(); + } + + + dgAssert(count == 4); + if (volume > dgFloat32(0.0f)) { + dgSwap(m_hullSum[1], m_hullSum[0]); + dgSwap(m_hullDiff[1], m_hullDiff[0]); + } + + if (dgAbs(volume) < dgFloat32(1e-15f)) { + + // this volume is unrealizable, let us build a different tetrahedron using the method of core 200 + dgVector e1; + dgVector e2; + dgVector e3; + dgVector normal(dgFloat32(0.0f)); + + const dgInt32 nCount = dgInt32(sizeof(m_hullDirs) / sizeof(m_hullDirs[0])); + const dgFloat32 DG_CALCULATE_SEPARATING_PLANE_ERROR = dgFloat32(1.0f / 1024.0f); + + dgFloat32 error2 = dgFloat32(0.0f); + SupportVertex(m_hullDirs[0], 0); + + dgInt32 i = 1; + for (; i < nCount; i++) { + SupportVertex(m_hullDirs[i], 1); + e1 = m_hullDiff[1] - m_hullDiff[0]; + dgAssert (e1.m_w == dgFloat32 (0.0f)); + error2 = e1.DotProduct(e1).GetScalar(); + if (error2 > DG_CALCULATE_SEPARATING_PLANE_ERROR) { + break; + } + } + + for (i++; i < nCount; i++) { + SupportVertex(m_hullDirs[i], 2); + e2 = m_hullDiff[2] - m_hullDiff[0]; + normal = e1.CrossProduct(e2); + dgAssert (normal.m_w == dgFloat32 (0.0f)); + error2 = normal.DotProduct(normal).GetScalar(); + if (error2 > DG_CALCULATE_SEPARATING_PLANE_ERROR) { + break; + } + } + + error2 = dgFloat32(0.0f); + for (i++; i < nCount; i++) { + SupportVertex(m_hullDirs[i], 3); + e3 = m_hullDiff[3] - m_hullDiff[0]; + dgAssert (normal.m_w == dgFloat32 (0.0f)); + error2 = normal.DotProduct(e3).GetScalar(); + if (dgAbs(error2) > DG_CALCULATE_SEPARATING_PLANE_ERROR) { + break; + } + } + + if (i >= nCount) { +// dgAssert(0); + return -1; + } + + if (error2 > dgFloat32(0.0f)) { + dgSwap(m_hullSum[1], m_hullSum[2]); + dgSwap(m_hullDiff[1], m_hullDiff[2]); + } + +#ifdef _DEBUG + { + dgVector f0(m_hullDiff[1] - m_hullDiff[0]); + dgVector f1(m_hullDiff[2] - m_hullDiff[0]); + dgVector f2(m_hullDiff[3] - m_hullDiff[0]); + dgVector n(f1.CrossProduct(f2)); + dgAssert (n.m_w == dgFloat32 (0.0f)); + dgFloat32 volume1 = f0.DotProduct(n).GetScalar(); + dgAssert(volume1 < dgFloat32(0.0f)); + } +#endif + } + + // clear the face cache!! + Flush(); + m_faceIndex = 0; + m_vertexIndex = 4; + m_freeFace = NULL; + + dgMinkFace* const f0 = AddFace(0, 1, 2); + dgMinkFace* const f1 = AddFace(0, 2, 3); + dgMinkFace* const f2 = AddFace(2, 1, 3); + dgMinkFace* const f3 = AddFace(1, 0, 3); + + f0->m_twin[0] = f3; + f0->m_twin[1] = f2; + f0->m_twin[2] = f1; + + f1->m_twin[0] = f0; + f1->m_twin[1] = f2; + f1->m_twin[2] = f3; + + f2->m_twin[0] = f0; + f2->m_twin[1] = f3; + f2->m_twin[2] = f1; + + f3->m_twin[0] = f0; + f3->m_twin[1] = f1; + f3->m_twin[2] = f2; + + PushFace(f0); + PushFace(f1); + PushFace(f2); + PushFace(f3); + + dgInt32 cycling = 0; + dgInt32 iterCount = 0; + dgFloat32 cyclingMem[4]; + cyclingMem[0] = dgFloat32(1.0e10f); + cyclingMem[1] = dgFloat32(1.0e10f); + cyclingMem[2] = dgFloat32(1.0e10f); + cyclingMem[3] = dgFloat32(1.0e10f); + + const dgFloat32 resolutionScale = dgFloat32(0.125f); + const dgFloat32 minTolerance = DG_PENETRATION_TOL; + + while (GetCount()) { + dgMinkFace* const faceNode = (*this)[0]; + Pop(); + + if (faceNode->m_alive) { + SupportVertex(faceNode->m_plane & dgVector::m_triplexMask, m_vertexIndex); + const dgVector& p = m_hullDiff[m_vertexIndex]; + dgFloat32 dist = faceNode->m_plane.Evalue(p); + dgFloat32 distTolerance = dgMax(dgAbs(faceNode->m_plane.m_w) * resolutionScale, minTolerance); + + if (dist < distTolerance) { + dgVector sum[3]; + dgVector diff[3]; + m_normal = faceNode->m_plane & dgVector::m_triplexMask; + for (dgInt32 i = 0; i < 3; i++) { + dgInt32 j = faceNode->m_vertex[i]; + sum[i] = m_hullSum[j]; + diff[i] = m_hullDiff[j]; + } + for (dgInt32 i = 0; i < 3; i++) { + m_hullSum[i] = sum[i]; + m_hullDiff[i] = diff[i]; + } + return 3; + } + + iterCount++; + bool isCycling = false; + cyclingMem[cycling] = dist; + if (iterCount > 10) { + dgInt32 cyclingIndex = cycling; + for (dgInt32 i = 0; i < 3; i++) { + dgInt32 cyclingIndex0 = (cyclingIndex - 1) & 3; + if (((cyclingMem[cyclingIndex0] - cyclingMem[cyclingIndex]) < dgFloat32(-1.0e-5f))) { + isCycling = true; + cyclingMem[0] = dgFloat32(1.0e10f); + cyclingMem[1] = dgFloat32(1.0e10f); + cyclingMem[2] = dgFloat32(1.0e10f); + cyclingMem[3] = dgFloat32(1.0e10f); + break; + } + cyclingIndex = cyclingIndex0; + } + } + cycling = (cycling + 1) & 3; + + if (!isCycling) { + m_faceStack[0] = faceNode; + dgInt32 stackIndex = 1; + dgInt32 deletedCount = 0; + + while (stackIndex) { + stackIndex--; + dgMinkFace* const face = m_faceStack[stackIndex]; + + if (!face->m_mark && (face->m_plane.Evalue(p) > dgFloat32(0.0f))) { +#ifdef _DEBUG + for (dgInt32 i = 0; i < deletedCount; i++) { + dgAssert(m_deletedFaceList[i] != face); + } +#endif + + m_deletedFaceList[deletedCount] = face; + deletedCount++; + dgAssert(deletedCount < sizeof (m_deletedFaceList) / sizeof (m_deletedFaceList[0])); + face->m_mark = 1; + + for (dgInt32 i = 0; i < 3; i++) { + dgMinkFace* const twinFace = face->m_twin[i]; + if (twinFace && !twinFace->m_mark) { + m_faceStack[stackIndex] = twinFace; + stackIndex++; + dgAssert(stackIndex < sizeof (m_faceStack) / sizeof (m_faceStack[0])); + } + } + } + } + + //dgAssert (SanityCheck()); + dgInt32 newCount = 0; + for (dgInt32 i = 0; i < deletedCount; i++) { + dgMinkFace* const face = m_deletedFaceList[i]; + face->m_alive = 0; + dgAssert(face->m_mark == 1); + dgInt32 j0 = 2; + for (dgInt32 j1 = 0; j1 < 3; j1++) { + dgMinkFace* const twinFace = face->m_twin[j0]; + if (twinFace && !twinFace->m_mark) { + //dgMinkFace* const newFace = AddFace(m_vertexIndex, face->m_vertex[j0], face->m_vertex[j1]); + dgMinkFace* const newFace = NewFace(); + if (newFace) { + newFace->m_mark = 0; + newFace->m_vertex[0] = dgInt16(m_vertexIndex); + newFace->m_vertex[1] = dgInt16(face->m_vertex[j0]); + newFace->m_vertex[2] = dgInt16(face->m_vertex[j1]); + PushFace(newFace); + + newFace->m_twin[1] = twinFace; + dgInt32 index = (twinFace->m_twin[0] == face) ? 0 : ((twinFace->m_twin[1] == face) ? 1 : 2); + twinFace->m_twin[index] = newFace; + + m_coneFaceList[newCount] = newFace; + newCount++; + dgAssert(newCount < sizeof(m_coneFaceList) / sizeof(m_coneFaceList[0])); + } else { + // this is very rare but is does happend with some degenerated faces. + return -1; + } + } + j0 = j1; + } + } + + dgInt32 i0 = newCount - 1; + for (dgInt32 i1 = 0; i1 < newCount; i1++) { + dgMinkFace* const faceA = m_coneFaceList[i0]; + dgAssert(faceA->m_mark == 0); + + dgInt32 j0 = newCount - 1; + for (dgInt32 j1 = 0; j1 < newCount; j1++) { + if (i0 != j0) { + dgMinkFace* const faceB = m_coneFaceList[j0]; + dgAssert(faceB->m_mark == 0); + if (faceA->m_vertex[2] == faceB->m_vertex[1]) { + faceA->m_twin[2] = faceB; + faceB->m_twin[0] = faceA; + break; + } + } + j0 = j1; + } + i0 = i1; + } + + m_vertexIndex++; + dgAssert(m_vertexIndex < sizeof (m_hullDiff) / sizeof (m_hullDiff[0])); + + //dgAssert(SanityCheck()); + } + } else { + DeleteFace(faceNode); + } + } + + return -1; +} + +DG_INLINE void dgContactSolver::CalculateContactFromFeacture(dgInt32 featureType) +{ + dgVector d; + dgVector s; + switch (featureType) + { + case 1: + { + s = m_hullSum[0]; + d = m_hullDiff[0]; + break; + } + case 2: + { + const dgVector& p0 = m_hullDiff[0]; + const dgVector& p1 = m_hullDiff[1]; + dgVector dp(p1 - p0); + dgAssert(dp.m_w == dgFloat32 (0.0f)); + dgAssert(dp.DotProduct(dp).GetScalar() > dgFloat32(0.0f)); + dgFloat32 alpha0 = - p0.DotProduct(dp).GetScalar() / dp.DotProduct(dp).GetScalar(); + dgAssert(alpha0 <= dgFloat32(1.01f)); + dgAssert(alpha0 >= dgFloat32(-0.01f)); + d = p0 + dp.Scale(alpha0); + s = m_hullSum[0] + (m_hullSum[1] - m_hullSum[0]).Scale(alpha0); + break; + } + + case 3: + default: + { + dgVector e10(m_hullDiff[1] - m_hullDiff[0]); + dgVector e20(m_hullDiff[2] - m_hullDiff[0]); + dgVector normal(e10.CrossProduct(e20)); + dgAssert(normal.m_w == dgFloat32 (0.0f)); + dgAssert(normal.DotProduct(normal).GetScalar() > dgFloat32(0.0f)); + + dgFloat32 alphas[3]; + for (dgInt32 i0 = 2, i1 = 0; i1 < 3; i1++) { + const dgVector& p1p0 = m_hullDiff[i0]; + const dgVector& p2p0 = m_hullDiff[i1]; + alphas[i0] = normal.DotProduct(p1p0.CrossProduct(p2p0)).GetScalar(); + i0 = i1; + } + + dgFloat32 alphaDen = alphas[0] + alphas[1] + alphas[2]; + if (alphaDen > dgFloat32(1.0e-16f)) { + dgAssert(alphaDen > dgFloat32(0.0f)); + + alphaDen = dgFloat32(1.0f / alphaDen); + alphas[0] *= alphaDen; + alphas[1] *= alphaDen; + alphas[2] *= alphaDen; + s = m_hullSum[0].Scale(alphas[1]) + m_hullSum[1].Scale(alphas[2]) + m_hullSum[2].Scale(alphas[0]); + d = m_hullDiff[0].Scale(alphas[1]) + m_hullDiff[1].Scale(alphas[2]) + m_hullDiff[2].Scale(alphas[0]); + } else { + // this is a degenerated face that is so small that lose accuracy in 32 bit floats + // get the closest point from the longest edge + + dgAssert(e10.m_w == dgFloat32 (0.0f)); + dgAssert(e20.m_w == dgFloat32 (0.0f)); + dgVector dir((e10.DotProduct(e10).GetScalar() > e20.DotProduct(e20).GetScalar()) ? e10 : e20); + dgInt32 i0 = 0; + dgInt32 i1 = 0; + dgFloat32 dist0 = dir.DotProduct(m_hullDiff[0]).GetScalar(); + dgFloat32 dist1 = -dist0; + for (dgInt32 i = 1; i < 3; i++) { + dgFloat32 test = dir.DotProduct(m_hullDiff[i]).GetScalar(); + if (test > dist0) { + i0 = i; + dist0 = test; + } + test *= dgFloat32(-1.0f); + if (test > dist1) { + i1 = i; + dist1 = test; + } + } + + if (i0 != i1) { + const dgVector& p0 = m_hullDiff[i0]; + const dgVector& p1 = m_hullDiff[i1]; + dgVector dp(p1 - p0); + dgAssert(dp.m_w == dgFloat32 (0.0f)); + dgAssert(dp.DotProduct(dp).GetScalar() > dgFloat32(0.0f)); + dgFloat32 alpha0 = - p0.DotProduct(dp).GetScalar() / dp.DotProduct(dp).GetScalar(); + dgAssert(alpha0 <= dgFloat32(1.01f)); + dgAssert(alpha0 >= dgFloat32(-0.01f)); + d = p0 + dp.Scale(alpha0); + s = m_hullSum[0] + (m_hullSum[i1] - m_hullSum[i0]).Scale(alpha0); + } else { + s = m_hullSum[i0]; + d = m_hullDiff[i0]; + } + } + } + } + + m_closestPoint0 = dgVector::m_half * (s + d); + m_closestPoint1 = dgVector::m_half * (s - d); + dgAssert(m_normal.m_w == dgFloat32 (0.0f)); + dgAssert(dgAbs(m_normal.DotProduct(m_normal).GetScalar() - dgFloat32(1.0f)) < dgFloat32(1.0e-4f)); + m_proxy->m_contactJoint->m_separtingVector = m_normal; +} + + +bool dgContactSolver::SanityCheck() const +{ + for (dgInt32 i = 0; i < m_faceIndex; i++) { + const dgMinkFace* const face = &m_facePool[i]; + if (face->m_alive) { + for (dgInt32 j = 0; j < 3; j++) { + dgMinkFace* const twin = face->m_twin[j]; + if (!twin) { + return false; + } + + if (!twin->m_alive) { + return false; + } + + bool pass = false; + for (dgInt32 k = 0; k < 3; k++) { + if (twin->m_twin[k] == face) { + pass = true; + break; + } + } + if (!pass) { + return pass; + } + } + } + } + return true; +} + + +bool dgContactSolver::CalculateClosestPoints() +{ + dgInt32 simplexPointCount = CalculateClosestSimplex(); + if (simplexPointCount < 0) { + simplexPointCount = CalculateIntersectingPlane(-simplexPointCount); + } + + if (simplexPointCount > 0) { + dgAssert((simplexPointCount > 0) && (simplexPointCount <= 3)); + CalculateContactFromFeacture(simplexPointCount); + + const dgMatrix& matrix0 = m_instance0->m_globalMatrix; + const dgMatrix& matrix1 = m_instance1->m_globalMatrix; + m_closestPoint0 = matrix0.TransformVector(m_instance0->SupportVertexSpecialProjectPoint(matrix0.UntransformVector(m_closestPoint0), matrix0.UnrotateVector(m_normal))); + m_closestPoint1 = matrix1.TransformVector(m_instance1->SupportVertexSpecialProjectPoint(matrix1.UntransformVector(m_closestPoint1), matrix1.UnrotateVector(m_normal.Scale(-1.0f)))); + m_vertexIndex = simplexPointCount; + } + return simplexPointCount >= 0; +} + +dgInt32 dgContactSolver::ConvexPolygonToLineIntersection(const dgVector& normal, dgInt32 count1, dgVector* const shape1, dgInt32 count2, dgVector* const shape2, dgVector* const contactOut, dgVector* const mem) const +{ + dgInt32 count = 0; + dgVector* output = mem; + + dgAssert(count1 >= 3); + dgAssert(count2 <= 2); + dgAssert(normal.m_w == dgFloat32 (0.0f)); + + dgVector* ptr = NULL; + // face line intersection + if (count2 == 2) { + ptr = (dgVector*)&shape2[0]; + dgInt32 i0 = count1 - 1; + for (dgInt32 i1 = 0; i1 < count1; i1++) { + dgVector n(normal.CrossProduct(shape1[i1] - shape1[i0])); + dgAssert(n.m_w == dgFloat32 (0.0f)); + dgAssert(n.DotProduct(n).GetScalar() > dgFloat32(0.0f)); + dgPlane plane(n, - n.DotProduct(shape1[i0]).GetScalar()); + + dgFloat32 test0 = plane.Evalue(ptr[0]); + dgFloat32 test1 = plane.Evalue(ptr[1]); + if (test0 >= dgFloat32(0.0f)) { + if (test1 >= dgFloat32(0.0f)) { + output[count + 0] = ptr[0]; + output[count + 1] = ptr[1]; + count += 2; + } else { + dgVector dp(ptr[1] - ptr[0]); + dgAssert (dp.m_w == dgFloat32 (0.0f)); + dgFloat32 den = plane.DotProduct(dp).GetScalar(); + if (dgAbs(den) < 1.0e-10f) { + den = 1.0e-10f; + } + output[count + 0] = ptr[0]; + dgAssert (dp.m_w == dgFloat32 (0.0f)); + output[count + 1] = ptr[0] - dp.Scale(test0 / den); + count += 2; + } + } else if (test1 >= dgFloat32(0.0f)) { + dgVector dp(ptr[1] - ptr[0]); + dgAssert (dp.m_w == dgFloat32 (0.0f)); + dgFloat32 den = plane.DotProduct(dp).GetScalar(); + if (dgAbs(den) < 1.0e-10f) { + den = 1.0e-10f; + } + dgAssert (dp.m_w == dgFloat32 (0.0f)); + output[count] = ptr[0] - dp.Scale(test0 / den); + count++; + output[count] = ptr[1]; + count++; + } else { + return 0; + } + + + count2 = count; + ptr = output; + output = &output[count]; + count = 0; + i0 = i1; + //dgAssert (output < &pool[sizeof (pool)/sizeof (pool[0])]); + } + } else if (count2 == 1) { + const dgVector& p = shape2[0]; + dgInt32 i0 = count1 - 1; + for (dgInt32 i1 = 0; i1 < count1; i1++) { + dgVector n(normal.CrossProduct(shape1[i1] - shape1[i0])); + dgAssert(n.m_w == dgFloat32 (0.0f)); + dgAssert(n.DotProduct(n).GetScalar() > dgFloat32(0.0f)); + dgPlane plane(n, - n.DotProduct(shape1[i0]).GetScalar()); + dgFloat32 test0 = plane.Evalue(p); + if (test0 < dgFloat32(-1.e-3f)) { + return 0; + } + i0 = i1; + } + ptr = output; + output[count] = p; + count++; + + } else { + count2 = 0; + } + + for (dgInt32 i0 = 0; i0 < count2; i0++) { + contactOut[i0] = ptr[i0]; + } + return count2; +} + +dgInt32 dgContactSolver::ConvexPolygonsIntersection(const dgVector& normal, dgInt32 count0, dgVector* const shape0, dgInt32 count1, dgVector* const shape1, dgVector* const contactOut, dgInt32 maxContacts) const +{ + dgInt32 count = 0; + if (count1 <= 2) { + count = ConvexPolygonToLineIntersection(normal.Scale (dgFloat32 (-1.0f)), count0, shape0, count1, shape1, contactOut, &contactOut[count0 + count1 + maxContacts]); + } else if (count0 <= 2) { + count = ConvexPolygonToLineIntersection(normal, count1, shape1, count0, shape0, contactOut, &contactOut[count0 + count1 + maxContacts]); + } else { + dgAssert(count0 >= 3); + dgAssert(count1 >= 3); + + dgPerimenterEdge subdivision[128]; + dgAssert((2 * (count0 + count1)) < dgInt32(sizeof (subdivision) / sizeof (subdivision[0]))); + + for (dgInt32 i0 = 1; i0 < count1; i0++) { + subdivision[i0].m_vertex = &shape1[i0]; + subdivision[i0].m_prev = &subdivision[i0 - 1]; + subdivision[i0].m_next = &subdivision[i0 + 1]; + } + subdivision[0].m_vertex = &shape1[0]; + subdivision[0].m_prev = &subdivision[count1 - 1]; + subdivision[0].m_next = &subdivision[1]; + + subdivision[count1 - 1].m_next = &subdivision[0]; + + dgPerimenterEdge* edgeClipped[2]; + dgVector* output = &contactOut[count0 + count1 + maxContacts]; + + edgeClipped[0] = NULL; + edgeClipped[1] = NULL; + dgInt32 j0 = 0; + dgInt32 edgeIndex = count1; + dgPerimenterEdge* poly = &subdivision[0]; + for (dgInt32 j1 = count0 - 1; j1 >= 0; j1--) { + dgVector n(normal.CrossProduct(shape0[j1] - shape0[j0])); + dgAssert(n.m_w == 0.0f); + dgPlane plane(n, - n.DotProduct(shape0[j0]).GetScalar()); + j0 = j1; + count = 0; + dgPerimenterEdge* tmp = poly; + dgInt32 isInside = 0; + dgFloat32 test0 = plane.Evalue(*tmp->m_vertex); + do { + dgFloat32 test1 = plane.Evalue(*tmp->m_next->m_vertex); + + if (test0 >= dgFloat32(0.0f)) { + isInside |= 1; + if (test1 < dgFloat32(0.0f)) { + const dgVector& p0 = *tmp->m_vertex; + const dgVector& p1 = *tmp->m_next->m_vertex; + dgVector dp(p1 - p0); + dgAssert(dp.m_w == 0.0f); + dgFloat32 den = plane.DotProduct(dp).GetScalar(); + if (dgAbs(den) < dgFloat32(1.0e-24f)) { + den = (den >= dgFloat32(0.0f)) ? dgFloat32(1.0e-24f) : dgFloat32(-1.0e-24f); + } + + den = test0 / den; + if (den >= dgFloat32(0.0f)) { + den = dgFloat32(0.0f); + } else if (den <= -1.0f) { + den = dgFloat32(-1.0f); + } + dgAssert (dp.m_w == dgFloat32 (0.0f)); + output[0] = p0 - dp.Scale(den); + edgeClipped[0] = tmp; + count++; + } + } else if (test1 >= dgFloat32(0.0f)) { + const dgVector& p0 = *tmp->m_vertex; + const dgVector& p1 = *tmp->m_next->m_vertex; + isInside |= 1; + dgVector dp(p1 - p0); + dgAssert(dp.m_w == 0.0f); + dgFloat32 den = plane.DotProduct(dp).GetScalar(); + if (dgAbs(den) < dgFloat32(1.0e-24f)) { + den = (den >= dgFloat32(0.0f)) ? dgFloat32(1.0e-24f) : dgFloat32(-1.0e-24f); + } + den = test0 / den; + if (den >= dgFloat32(0.0f)) { + den = dgFloat32(0.0f); + } else if (den <= -1.0f) { + den = dgFloat32(-1.0f); + } + dgAssert (dp.m_w == dgFloat32 (0.0f)); + output[1] = p0 - dp.Scale(den); + edgeClipped[1] = tmp; + count++; + } + + test0 = test1; + tmp = tmp->m_next; + } while (tmp != poly && (count < 2)); + + if (!isInside) { + return 0; + } + + if (count == 2) { + dgPerimenterEdge* const newEdge = &subdivision[edgeIndex]; + newEdge->m_next = edgeClipped[1]; + newEdge->m_prev = edgeClipped[0]; + edgeClipped[0]->m_next = newEdge; + edgeClipped[1]->m_prev = newEdge; + + newEdge->m_vertex = &output[0]; + edgeClipped[1]->m_vertex = &output[1]; + poly = newEdge; + + output += 2; + edgeIndex++; + //dgAssert (output < &pool[sizeof (pool)/sizeof (pool[0])]); + dgAssert(edgeIndex < dgInt32(sizeof (subdivision) / sizeof (subdivision[0]))); + } + } + + dgAssert(poly); + count = 0; + dgPerimenterEdge* intersection = poly; + do { + contactOut[count] = *intersection->m_vertex; + count++; + intersection = intersection->m_next; + } while (intersection != poly); + } + return count; +} + +dgFloat32 dgContactSolver::RayCast (const dgVector& localP0, const dgVector& localP1, dgFloat32 maxT, dgContactPoint& contactOut) +{ + dgVector normal(dgFloat32 (0.0f)); + dgVector point (localP0); + dgVector point0 (localP0); + dgVector p0p1 (localP0 - localP1); + + // avoid NaN as a result of a division by zero + if ((p0p1.TestZero().GetSignMask() & 7) == 7) { + return dgFloat32(1.2f); + } + + dgFloat32 param = dgFloat32 (0.0f); + + dgInt32 index = 0; + memset (m_hullSum, 0, 4 * sizeof (m_hullSum[0])); + const dgCollisionConvex* const collision = (dgCollisionConvex*)m_instance0->GetChildShape(); + + dgVector dir1 (p0p1.Normalize()); + m_hullDiff[0] = collision->SupportVertex (dir1, NULL) - point; + dgBigVector v (m_hullDiff[0]); + index = 1; + do { + dgInt32 iter = 0; + dgInt32 cycling = 0; + dgFloat64 minDist = dgFloat32 (1.0e20f); + + do { + dgAssert (v.m_w == dgFloat32 (0.0f)); + const dgFloat64 distance = v.DotProduct(v).GetScalar(); + if (distance < dgFloat32 (1.0e-9f)) { + index = -1; + break; + } + + if (distance < minDist) { + minDist = distance; + cycling = -1; + } + cycling ++; + if (cycling > 4) { + //dgAssert (0); + index = -1; + break; + } + + dgVector dir (v.Scale (-dgRsqrt(dgFloat32 (distance)))); + dgAssert (dir.m_w == dgFloat32 (0.0f)); + m_hullDiff[index] = collision->SupportVertex (dir, NULL) - point; + const dgBigVector w (m_hullDiff[index]); + const dgVector wv (w - v); + dgAssert (wv.m_w == dgFloat32 (0.0f)); + const dgFloat32 distance1 = dir.DotProduct(wv).GetScalar(); + if (distance1 < dgFloat64 (1.0e-3f)) { + normal = dir; + break; + } + + index ++; + switch (index) + { + case 2: + { + v = ReduceLine (index); + break; + } + + case 3: + { + v = ReduceTriangle (index); + break; + } + + case 4: + { + v = ReduceTetrahedrum (index); + break; + } + } + + iter ++; + } while (iter < DG_CONNICS_CONTATS_ITERATIONS); + + dgAssert (index); + if (index > 0) { + dgVector q (v + point); + dgFloat32 den = normal.DotProduct(p0p1).GetScalar(); + if (dgAbs (den) < dgFloat32(1.0e-12f)) { + den = dgSign(den) * dgFloat32(1.0e-12f); + } + dgAssert (normal.m_w == dgFloat32 (0.0f)); + dgFloat32 t1 = normal.DotProduct(localP0 - q).GetScalar() / den; + + if (t1 < param) { + index = -1; + t1 = dgFloat32 (0.0f); + } else if (t1 > maxT) { + index = -1; + t1 = dgFloat32 (1.0f); + } + param = t1; + + point = localP0 - p0p1.Scale (param); + dgVector step (point0 - point); + point0 = point; + for(dgInt32 i = 0; i < index; i ++) { + m_hullDiff[i] += step; + } + + switch (index) + { + case 1: + { + v = m_hullDiff[0]; + break; + } + + case 2: + { + v = ReduceLine (index); + break; + } + + case 3: + { + v = ReduceTriangle (index); + break; + } + + case 4: + { + v = ReduceTetrahedrum (index); + break; + } + } + } + } while (index >= 0); + + if ((param > dgFloat32 (0.0f)) && (param < maxT)) { + contactOut.m_normal = normal; + } else { + param = dgFloat32 (1.2f); + } + + return param; +} + + +DG_INLINE void dgContactSolver::TranslateSimplex(const dgVector& step) +{ + m_instance1->m_globalMatrix.m_posit -= step; + for (dgInt32 i = 0; i < m_vertexIndex; i++) { + m_hullSum[i] -= step; + m_hullDiff[i] += step; + } +} + +dgInt32 dgContactSolver::CalculateConvexCastContacts() +{ + dgInt32 iter = 0; + dgInt32 count = 0; + + dgFloat32 tacc = dgFloat32(0.0f); + dgFloat32 timestep = m_proxy->m_timestep; + m_proxy->m_contactJoint->m_closestDistance = dgFloat32(1.0e10f); + + dgVector savedPosition1 (m_instance1->m_globalMatrix.m_posit); + + dgVector relVeloc (m_proxy->m_body0->GetVelocity() - m_proxy->m_body1->GetVelocity()); + do { + bool state = CalculateClosestPoints(); + if (!state) { + break; + } + dgAssert(m_normal.m_w == dgFloat32(0.0f)); + dgFloat32 den = m_normal.DotProduct(relVeloc).GetScalar(); + if (den <= dgFloat32(1.0e-6f)) { + // bodies are residing from each other, even if they are touching they are not considered to be colliding because the motion will move them apart + // get the closet point and the normal at contact point + m_proxy->m_timestep = dgFloat32 (1.0e10f); + m_proxy->m_normal = m_normal.Scale(-1.0f); + m_proxy->m_closestPointBody0 = m_closestPoint0; + m_proxy->m_closestPointBody1 = m_closestPoint1; + break; + } + + dgFloat32 num = m_normal.DotProduct(m_closestPoint1 - m_closestPoint0).GetScalar() - m_proxy->m_skinThickness; + if ((num <= dgFloat32(1.0e-5f)) && (tacc <= timestep)) { + // bodies collide at time tacc, but we do not set it yet + dgVector step(relVeloc.Scale(tacc)); + m_proxy->m_timestep = tacc; + m_proxy->m_closestPointBody0 = m_closestPoint0 + step; + m_proxy->m_closestPointBody1 = m_closestPoint1 + step; + m_proxy->m_normal = m_normal.Scale (dgFloat32 (-1.0f)); + m_proxy->m_contactJoint->m_closestDistance = m_proxy->m_normal.DotProduct(m_closestPoint0 - m_closestPoint1).GetScalar(); + dgFloat32 penetration = dgMax(num * dgFloat32(-1.0f) + DG_PENETRATION_TOL, dgFloat32(0.0f)); + m_proxy->m_contactJoint->m_closestDistance = penetration; + if (m_proxy->m_contacts && !m_proxy->m_intersectionTestOnly) { + if (m_proxy->m_instance0->GetCollisionMode() & m_proxy->m_instance1->GetCollisionMode()) { + + m_normal = m_normal.Scale (dgFloat32 (-1.0f)); + m_proxy->m_contactJoint->m_isActive = 1; + count = CalculateContacts(m_closestPoint0, m_closestPoint1, m_normal); + if (count) { + count = dgMin(m_proxy->m_maxContacts, count); + dgContactPoint* const contactOut = m_proxy->m_contacts; + + for (int i = 0; i < count; i++) { + contactOut[i].m_point = m_hullDiff[i] + step; + contactOut[i].m_normal = m_normal; + contactOut[i].m_penetration = penetration; + } + } + } + } + break; + } + + dgAssert (den > dgFloat32 (0.0f)); + dgFloat32 dt = num / den; + if ((tacc + dt) >= timestep) { + // object do not collide on this timestep + m_proxy->m_timestep = tacc + dt; + m_proxy->m_normal = m_normal.Scale (dgFloat32 (-1.0f)); + m_proxy->m_closestPointBody0 = m_closestPoint0; + m_proxy->m_closestPointBody1 = m_closestPoint1; + break; + } + + tacc += dt; + dgVector step(relVeloc.Scale(dt)); + TranslateSimplex(step); + + iter++; + } while (iter < DG_SEPARATION_PLANES_ITERATIONS); + + m_instance1->m_globalMatrix.m_posit = savedPosition1; + return count; +} + + +dgInt32 dgContactSolver::CalculateConvexToConvexContacts () +{ + dgInt32 count = 0; + if (m_proxy->m_intersectionTestOnly) { + CalculateClosestPoints(); + dgFloat32 penetration = m_normal.DotProduct(m_closestPoint1 - m_closestPoint0).GetScalar() - m_proxy->m_skinThickness - DG_PENETRATION_TOL; + dgInt32 retVal = (penetration <= dgFloat32(0.0f)) ? -1 : 0; + m_proxy->m_contactJoint->m_isActive = retVal; + return retVal; + } else { + bool colliding = CalculateClosestPoints(); + if (colliding) { + dgFloat32 penetration = m_normal.DotProduct(m_closestPoint1 - m_closestPoint0).GetScalar() - m_proxy->m_skinThickness - DG_PENETRATION_TOL; + if (penetration <= dgFloat32(1.0e-5f)) { + m_proxy->m_contactJoint->m_isActive = 1; + if (m_instance0->GetCollisionMode() & m_instance1->GetCollisionMode()) { + count = CalculateContacts(m_closestPoint0, m_closestPoint1, m_normal.Scale(-1.0f)); + } + } + + m_proxy->m_closestPointBody0 = m_closestPoint0; + m_proxy->m_closestPointBody1 = m_closestPoint1; + m_proxy->m_contactJoint->m_closestDistance = penetration; + m_proxy->m_contactJoint->m_separationDistance = penetration; + + m_normal = m_normal.Scale (dgFloat32 (-1.0f)); + penetration = -penetration; + m_proxy->m_normal = m_normal; + count = dgMin(m_proxy->m_maxContacts, count); + dgContactPoint* const contactOut = m_proxy->m_contacts; + + for (int i = 0; i < count; i ++) { + contactOut[i].m_point = m_hullDiff[i]; + contactOut[i].m_normal = m_normal; + contactOut[i].m_penetration = penetration; + } + } + } + + return count; +} + +dgInt32 dgContactSolver::CalculateContacts(const dgVector& point0, const dgVector& point1, const dgVector& normal) +{ + dgInt32 count = 0; + + const dgInt32 baseCount = 16; + + dgVector* const contactsOut = &m_hullDiff[0]; + dgAssert(m_instance1->IsType(dgCollision::dgCollisionConvexShape_RTTI)); + dgAssert(m_instance0->IsType(dgCollision::dgCollisionConvexShape_RTTI)); + + dgInt32 count1 = 0; + dgVector* const shape1 = &contactsOut[baseCount]; + + dgAssert(normal.m_w == dgFloat32(0.0f)); + + dgVector origin((point0 + point1).Scale(dgFloat32(0.5f))); + const dgMatrix& matrix1 = m_instance1->m_globalMatrix; + dgVector ponintOnInstance1(matrix1.UntransformVector(origin)); + dgVector normalOnInstance1(matrix1.UnrotateVector(normal)); + dgFloat32 dist = (normal.DotProduct(point0 - point1)).GetScalar(); + if (dist < dgFloat32(0.0f)) { + count1 = m_instance1->CalculatePlaneIntersection(normalOnInstance1, ponintOnInstance1, shape1); + } + if (!count1) { + dgVector step(normal.Scale(DG_PENETRATION_TOL * dgFloat32(2.0f))); + dgVector alternatePoint(point1); + for (dgInt32 i = 0; (i < 3) && !count1; i++) { + alternatePoint -= step; + dgVector alternatePointOnInstance1(matrix1.UntransformVector(alternatePoint)); + count1 = m_instance1->CalculatePlaneIntersection(normalOnInstance1, alternatePointOnInstance1, shape1); + } + //dgAssert(count1); + step = matrix1.UnrotateVector(normal * ((alternatePoint - origin).DotProduct(normal))); + for (dgInt32 i = 0; i < count1; i++) { + shape1[i] -= step; + } + } + + if (count1) { + for (int i = 0; i < count1; i++) { + shape1[i] = matrix1.TransformVector(shape1[i]); + } + + dgInt32 count0 = 0; + dgVector* const shape0 = &contactsOut[baseCount + count1]; + + const dgMatrix& matrix0 = m_instance0->m_globalMatrix; + dgVector pointOnInstance0(matrix0.UntransformVector(origin)); + dgVector normalOnInstance0(matrix0.UnrotateVector(normal.Scale(dgFloat32(-1.0f)))); + if (dist < dgFloat32(0.0f)) { + count0 = m_instance0->CalculatePlaneIntersection(normalOnInstance0, pointOnInstance0, shape0); + } + if (!count0) { + dgVector step(normal.Scale(DG_PENETRATION_TOL * dgFloat32(2.0f))); + dgVector alternatePoint(point0); + for (dgInt32 i = 0; (i < 3) && !count0; i++) { + alternatePoint += step; + dgVector alternatePointOnInstance0(matrix0.UntransformVector(alternatePoint)); + count0 = m_instance0->CalculatePlaneIntersection(normalOnInstance0, alternatePointOnInstance0, shape0); + } + //dgTrace (("If this is a frequent event, this routine should return the translation distance as the contact point\n")) + //dgAssert(count0); + step = matrix0.UnrotateVector(normal * ((alternatePoint - origin).DotProduct(normal))); + for (dgInt32 i = 0; i < count0; i++) { + shape0[i] -= step; + } + } + + if (count0) { + for (dgInt32 i = 0; i < count0; i++) { + shape0[i] = matrix0.TransformVector(shape0[i]); + } + + if (count1 == 1) { + count = 1; + contactsOut[0] = shape1[0]; + } else if (count0 == 1) { + count = 1; + contactsOut[0] = shape0[0]; + } else if ((count1 == 2) && (count0 == 2)) { + dgVector p0(shape1[0]); + dgVector p1(shape1[1]); + const dgVector& q0 = shape0[0]; + const dgVector& q1 = shape0[1]; + dgVector p10(p1 - p0); + dgVector q10(q1 - q0); + dgAssert(p10.m_w == dgFloat32(0.0f)); + dgAssert(q10.m_w == dgFloat32(0.0f)); + p10 = p10.Scale(dgRsqrt(p10.DotProduct(p10).GetScalar() + dgFloat32(1.0e-8f))); + q10 = q10.Scale(dgRsqrt(q10.DotProduct(q10).GetScalar() + dgFloat32(1.0e-8f))); + dgFloat32 dot = q10.DotProduct(p10).GetScalar(); + if (dgAbs(dot) > dgFloat32(0.998f)) { + dgFloat32 pl0 = p0.DotProduct(p10).GetScalar(); + dgFloat32 pl1 = p1.DotProduct(p10).GetScalar(); + dgFloat32 ql0 = q0.DotProduct(p10).GetScalar(); + dgFloat32 ql1 = q1.DotProduct(p10).GetScalar(); + if (pl0 > pl1) { + dgSwap(pl0, pl1); + dgSwap(p0, p1); + p10 = p10.Scale(dgFloat32(-1.0f)); + } + if (ql0 > ql1) { + dgSwap(ql0, ql1); + } + if (!((ql0 > pl1) && (ql1 < pl0))) { + dgFloat32 clip0 = (ql0 > pl0) ? ql0 : pl0; + dgFloat32 clip1 = (ql1 < pl1) ? ql1 : pl1; + + count = 2; + contactsOut[0] = p0 + p10.Scale(clip0 - pl0); + contactsOut[1] = p0 + p10.Scale(clip1 - pl0); + } + } else { + count = 1; + dgVector c0; + dgVector c1; + dgRayToRayDistance(p0, p1, q0, q1, c0, c1); + contactsOut[0] = (c0 + c1).Scale(dgFloat32(0.5f)); + } + } else { + dgAssert((count1 >= 2) && (count0 >= 2)); + count = ConvexPolygonsIntersection(normal, count0, shape0, count1, shape1, contactsOut, baseCount); + } + } + } + + if (!count && m_proxy->m_continueCollision) { + count = 1; + contactsOut[0] = origin; + } + + return count; +} diff --git a/thirdparty/src/newton/dgPhysics/dgContactSolver.h b/thirdparty/src/newton/dgPhysics/dgContactSolver.h new file mode 100644 index 000000000..0b9e20d4f --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgContactSolver.h @@ -0,0 +1,132 @@ +/* Copyright (c) <2003-2019> +* +* 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_CONTACT_SOLVER_H__ +#define _DG_CONTACT_SOLVER_H__ + +#include "dgCollision.h" +#include "dgCollisionInstance.h" + +DG_MSC_VECTOR_ALIGNMENT +class dgMinkFace +{ + public: + dgPlane m_plane; + dgMinkFace* m_twin[3]; + dgInt16 m_vertex[3]; + dgInt8 m_mark; + dgInt8 m_alive; +} DG_GCC_VECTOR_ALIGNMENT; + +#define DG_SEPARATION_PLANES_ITERATIONS 8 +#define DG_CONVEX_MINK_STACK_SIZE 64 +#define DG_CONNICS_CONTATS_ITERATIONS 32 +#define DG_CONVEX_MINK_MAX_FACES 512 +#define DG_CONVEX_MINK_MAX_POINTS 256 +#define DG_MAX_EDGE_COUNT 2048 +#define DG_PENETRATION_TOL dgFloat32 (1.0f / 1024.0f) +#define DG_MINK_VERTEX_ERR (dgFloat32 (1.0e-3f)) +#define DG_MINK_VERTEX_ERR2 (DG_MINK_VERTEX_ERR * DG_MINK_VERTEX_ERR) + + +class dgCollisionParamProxy; + +DG_MSC_VECTOR_ALIGNMENT +class dgContactSolver: public dgDownHeap +{ + public: + dgContactSolver(dgCollisionParamProxy* const proxy); + dgContactSolver(dgCollisionInstance* const instance0); + + bool CalculateClosestPoints(); + dgInt32 CalculateConvexCastContacts(); + dgInt32 CalculateConvexToConvexContacts(); + dgFloat32 RayCast (const dgVector& localP0, const dgVector& localP1, dgFloat32 maxT, dgContactPoint& contactOut); + + const dgVector& GetNormal() const {return m_normal;} + const dgVector& GetPoint0() const {return m_closestPoint0;} + const dgVector& GetPoint1() const {return m_closestPoint1;} + + private: + class dgPerimenterEdge + { + public: + const dgVector* m_vertex; + dgPerimenterEdge* m_next; + dgPerimenterEdge* m_prev; + }; + + class dgFaceFreeList + { + public: + dgFaceFreeList* m_next; + }; + + DG_INLINE dgMinkFace* NewFace(); + DG_INLINE void PushFace(dgMinkFace* const face); + DG_INLINE void DeleteFace(dgMinkFace* const face); + DG_INLINE dgMinkFace* AddFace(dgInt32 v0, dgInt32 v1, dgInt32 v2); + DG_INLINE void SupportVertex(const dgVector& dir, dgInt32 vertexIndex); + + DG_INLINE void TranslateSimplex(const dgVector& step); + + DG_INLINE void CalculateContactFromFeacture(dgInt32 featureType); + DG_INLINE dgBigVector ReduceLine(dgInt32& indexOut); + DG_INLINE dgBigVector ReduceTriangle (dgInt32& indexOut); + DG_INLINE dgBigVector ReduceTetrahedrum (dgInt32& indexOut); + + DG_INLINE dgPerimenterEdge* OldReduceContacts(dgPerimenterEdge* poly, dgInt32 maxCount) const; + + + bool SanityCheck() const; + dgInt32 ConvexPolygonsIntersection(const dgVector& normal, dgInt32 count1, dgVector* const shape1, dgInt32 count2, dgVector* const shape2, dgVector* const contactOut, dgInt32 maxContacts) const; + dgInt32 ConvexPolygonToLineIntersection(const dgVector& normal, dgInt32 count1, dgVector* const shape1, dgInt32 count2, dgVector* const shape2, dgVector* const contactOut, dgVector* const mem) const; + dgInt32 CalculateContacts (const dgVector& point0, const dgVector& point1, const dgVector& normal); + dgInt32 CalculateClosestSimplex (); + dgInt32 CalculateIntersectingPlane(dgInt32 count); + + dgVector m_normal; + dgVector m_closestPoint0; + dgVector m_closestPoint1; + dgCollisionParamProxy* m_proxy; + dgCollisionInstance* m_instance0; + dgCollisionInstance* m_instance1; + + dgFaceFreeList* m_freeFace; + dgInt32 m_vertexIndex; + dgInt32 m_faceIndex; + + dgVector m_hullDiff[DG_CONVEX_MINK_MAX_POINTS]; + dgVector m_hullSum[DG_CONVEX_MINK_MAX_POINTS]; + dgMinkFace* m_faceStack[DG_CONVEX_MINK_STACK_SIZE]; + dgMinkFace* m_coneFaceList[DG_CONVEX_MINK_STACK_SIZE]; + dgMinkFace* m_deletedFaceList[DG_CONVEX_MINK_STACK_SIZE]; + dgMinkFace m_facePool[DG_CONVEX_MINK_MAX_FACES]; + dgInt8 m_heapBuffer[DG_CONVEX_MINK_MAX_FACES * (sizeof (dgFloat32) + sizeof (dgMinkFace *))]; + + static dgVector m_hullDirs[14]; + static dgInt32 m_rayCastSimplex[4][4]; +}DG_GCC_VECTOR_ALIGNMENT; + + +#endif + + diff --git a/thirdparty/src/newton/dgPhysics/dgCorkscrewConstraint.cpp b/thirdparty/src/newton/dgPhysics/dgCorkscrewConstraint.cpp new file mode 100644 index 000000000..5ccabff7d --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgCorkscrewConstraint.cpp @@ -0,0 +1,282 @@ +/* Copyright (c) <2003-2019> +* +* 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 "dgPhysicsStdafx.h" +#include "dgBody.h" +#include "dgWorld.h" +#include "dgCorkscrewConstraint.h" + + + +////////////////////////////////////////////////////////////////////// +// Construction/Destruction +////////////////////////////////////////////////////////////////////// + +dgCorkscrewConstraint::dgCorkscrewConstraint () + :dgBilateralConstraint() +{ + dgAssert ((((dgUnsigned64) &m_localMatrix0) & 15) == 0); + + m_localMatrix0 = dgGetIdentityMatrix(); + m_localMatrix1 = dgGetIdentityMatrix(); + + m_maxDOF = 6; + m_constId = m_corkScrewConstraint; + m_posit = dgFloat32 (0.0f); + m_angle = dgFloat32 (0.0f); + m_jointAccelFnt = NULL; +} + +dgCorkscrewConstraint::~dgCorkscrewConstraint () +{ +} + +/* +dgCorkscrewConstraint* dgCorkscrewConstraint::Create(dgWorld* world) +{ + dgCorkscrewConstraint* constraint; + +// constraint = dgCorkscrewConstraintArray::GetPool().GetElement(); + dgCorkscrewConstraintArray& array = * world; + constraint = array.GetElement(); + + dgAssert ((((dgUnsigned64) &constraint->m_localMatrix0) & 15) == 0); + constraint->Init (); + constraint->m_maxDOF = 6; + constraint->m_constId = dgCorkscrewConstraintId; + + constraint->m_posit = dgFloat32 (0.0f); + constraint->m_angle = dgFloat32 (0.0f); + constraint->m_jointAccelFnt = NULL; + return constraint; +} + +void dgCorkscrewConstraint::Remove(dgWorld* world) +{ + dgCorkscrewConstraintArray& array = * world; + + dgBilateralConstraint::Remove (world); +// dgCorkscrewConstraintArray::GetPool().RemoveElement (this); + array.RemoveElement (this); +} +*/ + +void dgCorkscrewConstraint::SetJointParameterCallback (dgCorkscrewJointAcceleration callback) +{ + m_jointAccelFnt = callback; +} + +dgFloat32 dgCorkscrewConstraint::GetJointAngle () const +{ + return m_angle; +} + +dgFloat32 dgCorkscrewConstraint::GetJointPosit () const +{ + return m_posit; +} + + +dgFloat32 dgCorkscrewConstraint::GetJointOmega () const +{ + dgAssert (m_body0); + dgAssert (m_body1); + dgVector dir (m_body0->GetMatrix().RotateVector (m_localMatrix0[0])); + const dgVector& omega0 = m_body0->GetOmega(); + const dgVector& omega1 = m_body1->GetOmega(); + +// dgVector omega1 (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f)); +// if (m_body1) { +// omega1 = m_body1->GetOmega(); +// } + return dir.DotProduct(omega0 - omega1).GetScalar(); +} + +dgFloat32 dgCorkscrewConstraint::GetJointVeloc () const +{ + dgAssert (m_body0); + dgAssert (m_body1); + dgVector dir (m_body0->GetMatrix().RotateVector (m_localMatrix0[0])); + const dgVector& veloc0 = m_body0->GetVelocity(); + const dgVector& veloc1 = m_body1->GetVelocity(); + +// dgVector veloc1 (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f)); +// if (m_body1) { +// veloc1 = m_body1->GetVelocity(); +// } + return dir.DotProduct(veloc0 - veloc1).GetScalar(); +} + + +dgFloat32 dgCorkscrewConstraint::CalculateStopAlpha (dgFloat32 angle, const dgJointCallbackParam* param) const +{ + dgFloat32 alpha; + dgFloat32 omega; + dgFloat32 penetrationErr; + + alpha = dgFloat32 (0.0f); + if (m_angle > angle) { + omega = GetJointOmega (); + if (omega < dgFloat32 (0.0f)) { + omega = dgFloat32 (0.0f); + } + penetrationErr = angle - m_angle; + alpha = dgFloat32 (100.0f) * penetrationErr - omega * dgFloat32 (1.01f) / param->m_timestep; + + } else if (m_angle < angle) { + omega = GetJointOmega (); + if (omega > dgFloat32 (0.0f)) { + omega = dgFloat32 (0.0f); + } + + penetrationErr = angle - m_angle; + alpha = dgFloat32 (100.0f) * penetrationErr - omega * dgFloat32 (1.01f) / param->m_timestep; + } + return alpha; +} + +dgFloat32 dgCorkscrewConstraint::CalculateStopAccel (dgFloat32 distance, const dgJointCallbackParam* param) const +{ + dgFloat32 accel; + dgFloat32 speed; + dgFloat32 penetrationErr; + + accel = dgFloat32 (0.0f); + if (m_posit > distance) { + speed = GetJointVeloc (); + if (speed < dgFloat32 (0.0f)) { + speed = dgFloat32 (0.0f); + } + penetrationErr = (distance - m_posit); + accel = dgFloat32 (100.0f) * penetrationErr - speed * dgFloat32 (1.01f) / param->m_timestep; + + } else if (m_posit < distance) { + speed = GetJointVeloc (); + if (speed > dgFloat32 (0.0f)) { + speed = dgFloat32 (0.0f); + } + penetrationErr = distance - m_posit; + dgAssert (penetrationErr >= dgFloat32 (0.0f)); + accel = dgFloat32 (100.0f) * penetrationErr - speed * dgFloat32 (1.01f) / param->m_timestep; + } + return accel; +} + + +dgVector dgCorkscrewConstraint::GetJointForce () const +{ + dgMatrix matrix0; + dgMatrix matrix1; + + CalculateGlobalMatrixAndAngle (m_localMatrix0, m_localMatrix1, matrix0, matrix1); + + return dgVector (matrix0.m_up.Scale (m_jointForce[0].m_force) + + matrix0.m_right.Scale (m_jointForce[1].m_force) + + matrix0.m_up.Scale (m_jointForce[2].m_force) + + matrix0.m_right.Scale (m_jointForce[3].m_force)); + +} + +dgUnsigned32 dgCorkscrewConstraint::JacobianDerivative (dgContraintDescritor& params) +{ + dgMatrix matrix0; + dgMatrix matrix1; + + dgVector angle (CalculateGlobalMatrixAndAngle (m_localMatrix0, m_localMatrix1, matrix0, matrix1)); + + m_angle = -angle.m_x; + m_posit = matrix0.m_front.DotProduct(matrix0.m_posit - matrix1.m_posit).GetScalar(); + matrix1.m_posit += matrix1.m_front.Scale (m_posit); + + dgAssert (dgAbs (dgFloat32 (1.0f) - matrix0.m_front.DotProduct(matrix0.m_front).GetScalar()) < dgFloat32 (1.0e-5f)); + dgAssert (dgAbs (dgFloat32 (1.0f) - matrix0.m_up.DotProduct(matrix0.m_up).GetScalar()) < dgFloat32 (1.0e-5f)); + dgAssert (dgAbs (dgFloat32 (1.0f) - matrix0.m_right.DotProduct(matrix0.m_right).GetScalar()) < dgFloat32 (1.0e-5f)); + + const dgVector& dir1 = matrix0.m_up; + const dgVector& dir2 = matrix0.m_right; + +// const dgVector& p0 = matrix0.m_posit; +// const dgVector& p1 = matrix1.m_posit; + dgVector p0 (matrix0.m_posit); + dgVector p1 (matrix1.m_posit + matrix1.m_front.Scale (matrix1.m_front.DotProduct(p0 - matrix1.m_posit).GetScalar())); + + dgVector q0 (p0 + matrix0.m_front.Scale(MIN_JOINT_PIN_LENGTH)); + dgVector q1 (p1 + matrix1.m_front.Scale(MIN_JOINT_PIN_LENGTH)); + + dgPointParam pointDataP; + dgPointParam pointDataQ; + InitPointParam (pointDataP, m_defualtDiagonalRegularizer, p0, p1); + InitPointParam (pointDataQ, m_defualtDiagonalRegularizer, q0, q1); + + CalculatePointDerivative (0, params, dir1, pointDataP, &m_jointForce[0]); + CalculatePointDerivative (1, params, dir2, pointDataP, &m_jointForce[1]); + CalculatePointDerivative (2, params, dir1, pointDataQ, &m_jointForce[2]); + CalculatePointDerivative (3, params, dir2, pointDataQ, &m_jointForce[3]); + + dgInt32 ret = 4; + if (m_jointAccelFnt) { + dgUnsigned32 code; + dgJointCallbackParam axisParam[2]; + + // linear acceleration + axisParam[0].m_accel = dgFloat32 (0.0f); + axisParam[0].m_timestep = params.m_timestep; + axisParam[0].m_minFriction = DG_MIN_BOUND; + axisParam[0].m_maxFriction = DG_MAX_BOUND; + + // angular acceleration + axisParam[1].m_accel = dgFloat32 (0.0f); + axisParam[1].m_timestep = params.m_timestep; + axisParam[1].m_minFriction = DG_MIN_BOUND; + axisParam[1].m_maxFriction = DG_MAX_BOUND; + + code = m_jointAccelFnt (*this, axisParam); + if (code & 1) { + if ((axisParam[0].m_minFriction > DG_MIN_BOUND) || (axisParam[0].m_maxFriction < DG_MAX_BOUND)) { + params.m_forceBounds[ret].m_low = axisParam[0].m_minFriction; + params.m_forceBounds[ret].m_upper = axisParam[0].m_maxFriction; + params.m_forceBounds[ret].m_normalIndex = DG_INDEPENDENT_ROW; + } + + CalculatePointDerivative (ret, params, matrix0.m_front, pointDataP, &m_jointForce[ret]); + SetMotorAcceleration (ret, axisParam[0].m_accel, params); + ret ++; + } + + + if (code & 2) { + if ((axisParam[1].m_minFriction > DG_MIN_BOUND) || (axisParam[1].m_maxFriction < DG_MAX_BOUND)) { + params.m_forceBounds[ret].m_low = axisParam[1].m_minFriction; + params.m_forceBounds[ret].m_upper = axisParam[1].m_maxFriction; + params.m_forceBounds[ret].m_normalIndex = DG_INDEPENDENT_ROW; + } + + CalculateAngularDerivative (ret, params, matrix0.m_front, m_defualtDiagonalRegularizer, dgFloat32 (0.0f), &m_jointForce[ret]); + SetMotorAcceleration (ret, axisParam[1].m_accel, params); + ret ++; + } + } + + return dgUnsigned32 (ret); +} + + + diff --git a/thirdparty/src/newton/dgPhysics/dgCorkscrewConstraint.h b/thirdparty/src/newton/dgPhysics/dgCorkscrewConstraint.h new file mode 100644 index 000000000..d3b68dbfd --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgCorkscrewConstraint.h @@ -0,0 +1,77 @@ +/* Copyright (c) <2003-2019> +* +* 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_DGCORKSCREWCONSTRAINT_H__GRT672DF293FR_H) +#define AFX_DGCORKSCREWCONSTRAINT_H__GRT672DF293FR_H +#include "dgBilateralConstraint.h" + +//template class dgPool; + +class dgCorkscrewConstraint; + + +typedef dgUnsigned32 (dgApi *dgCorkscrewJointAcceleration) (const dgCorkscrewConstraint& hinge, dgJointCallbackParam* param); + +class dgCorkscrewConstraint: public dgBilateralConstraint +{ + public: + dgFloat32 GetJointAngle () const; + dgFloat32 GetJointOmega () const; + dgFloat32 GetJointPosit () const; + dgFloat32 GetJointVeloc () const; + + dgVector GetJointForce () const; + dgFloat32 CalculateStopAlpha (dgFloat32 angle, const dgJointCallbackParam* param) const; + dgFloat32 CalculateStopAccel (dgFloat32 distance, const dgJointCallbackParam* param) const; + void SetJointParameterCallback (dgCorkscrewJointAcceleration callback); + + + private: + virtual dgUnsigned32 JacobianDerivative (dgContraintDescritor& params); + virtual void Serialize (dgSerialize serializeCallback, void* const userData) {dgAssert (0);} + + dgCorkscrewConstraint(); + virtual ~dgCorkscrewConstraint(); + + dgMatrix m_localMatrix0; + dgMatrix m_localMatrix1; + + dgFloat32 m_angle; + dgFloat32 m_posit; + dgCorkscrewJointAcceleration m_jointAccelFnt; +// dgUnsigned32 m_reserve[1]; + +//#ifdef _NEWTON_USE_DOUBLE +// dgUnsigned32 m_reserve[3]; +//#else +// dgUnsigned32 m_reserve[1]; +//#endif + + friend class dgWorld; +// friend class dgPool; +}; + +//class dgCorkscrewConstraintArray: public dgPoolContainer +//{ +//}; + +#endif // !defined(AFX_DGCORKSCREWCONSTRAINT_H__GRT672DF293FR_H) + diff --git a/thirdparty/src/newton/dgPhysics/dgDynamicBody.cpp b/thirdparty/src/newton/dgPhysics/dgDynamicBody.cpp new file mode 100644 index 000000000..e24fb7d2c --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgDynamicBody.cpp @@ -0,0 +1,410 @@ +/* Copyright (c) <2003-2019> +* +* 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 "dgPhysicsStdafx.h" +#include "dgWorld.h" +#include "dgDynamicBody.h" +#include "dgCollisionInstance.h" +#include "dgCollisionLumpedMassParticles.h" + +////////////////////////////////////////////////////////////////////// +// Construction/Destruction +////////////////////////////////////////////////////////////////////// + +dgVector dgDynamicBody::m_equilibriumError2 (DG_ERR_TOLERANCE2); + + +dgDynamicBodyAsymetric::dgDynamicBodyAsymetric() + :dgDynamicBody() + , m_principalAxis(dgGetIdentityMatrix()) +{ + m_type = m_dynamicBody; + m_rtti |= m_dynamicBodyAsymentricRTTI; + dgAssert(dgInt32(sizeof(dgDynamicBody) & 0x0f) == 0); + +} + +dgDynamicBodyAsymetric::dgDynamicBodyAsymetric(dgWorld* const world, const dgTree* const collisionNode, dgDeserialize serializeCallback, void* const userData, dgInt32 revisionNumber) + :dgDynamicBody(world, collisionNode, serializeCallback, userData, revisionNumber) + , m_principalAxis(dgGetIdentityMatrix()) +{ + m_type = m_dynamicBody; + m_rtti |= m_dynamicBodyRTTI; + serializeCallback(userData, &m_principalAxis, sizeof(m_principalAxis)); +} + +void dgDynamicBodyAsymetric::Serialize(const dgTree& collisionRemapId, dgSerialize serializeCallback, void* const userData) +{ + dgDynamicBody::Serialize(collisionRemapId, serializeCallback, userData); + serializeCallback(userData, &m_principalAxis, sizeof(m_principalAxis)); +} + + +void dgDynamicBodyAsymetric::SetMassMatrix(dgFloat32 mass, const dgMatrix& inertia) +{ + //dgVector II; + m_principalAxis = inertia; + dgVector II (m_principalAxis.EigenVectors()); + dgMatrix massMatrix(dgGetIdentityMatrix()); + massMatrix[0][0] = II[0]; + massMatrix[1][1] = II[1]; + massMatrix[2][2] = II[2]; + dgBody::SetMassMatrix(mass, massMatrix); +} + +dgMatrix dgDynamicBodyAsymetric::CalculateLocalInertiaMatrix() const +{ + dgMatrix matrix(m_principalAxis); + matrix.m_posit = dgVector::m_wOne; + dgMatrix diagonal(dgGetIdentityMatrix()); + diagonal[0][0] = m_mass[0]; + diagonal[1][1] = m_mass[1]; + diagonal[2][2] = m_mass[2]; + return matrix * diagonal * matrix.Inverse(); +} + +dgMatrix dgDynamicBodyAsymetric::CalculateInertiaMatrix() const +{ + dgMatrix matrix(m_principalAxis * m_matrix); + matrix.m_posit = dgVector::m_wOne; + dgMatrix diagonal(dgGetIdentityMatrix()); + diagonal[0][0] = m_mass[0]; + diagonal[1][1] = m_mass[1]; + diagonal[2][2] = m_mass[2]; + return matrix * diagonal * matrix.Inverse(); +} + +dgMatrix dgDynamicBodyAsymetric::CalculateInvInertiaMatrix() const +{ + dgMatrix matrix(m_principalAxis * m_matrix); + matrix.m_posit = dgVector::m_wOne; + dgMatrix diagonal(dgGetIdentityMatrix()); + diagonal[0][0] = m_invMass[0]; + diagonal[1][1] = m_invMass[1]; + diagonal[2][2] = m_invMass[2]; + return matrix * diagonal * matrix.Inverse(); +} + +void dgDynamicBodyAsymetric::IntegrateOpenLoopExternalForce(dgFloat32 timestep) +{ + dgDynamicBody::IntegrateOpenLoopExternalForce(timestep); +} + + +dgDynamicBody::dgDynamicBody() + :dgBody() + ,m_externalForce(dgVector::m_zero) + ,m_externalTorque(dgVector::m_zero) + ,m_savedExternalForce(dgVector::m_zero) + ,m_savedExternalTorque(dgVector::m_zero) + ,m_dampCoef(dgVector::m_zero) + ,m_cachedDampCoef(dgVector::m_zero) + ,m_cachedTimeStep(dgFloat32(0.0f)) + ,m_sleepingCounter(0) + ,m_isInDestructionArrayLRU(0) + ,m_skeleton(NULL) + ,m_applyExtForces(NULL) +{ + m_type = m_dynamicBody; + m_rtti |= m_dynamicBodyRTTI; + dgAssert ( dgInt32 (sizeof (dgDynamicBody) & 0x0f) == 0); +} + +dgDynamicBody::dgDynamicBody (dgWorld* const world, const dgTree* const collisionCashe, dgDeserialize serializeCallback, void* const userData, dgInt32 revisionNumber) + :dgBody(world, collisionCashe, serializeCallback, userData, revisionNumber) + ,m_externalForce(dgVector::m_zero) + ,m_externalTorque(dgVector::m_zero) + ,m_savedExternalForce(dgVector::m_zero) + ,m_savedExternalTorque(dgVector::m_zero) + ,m_dampCoef(dgVector::m_zero) + ,m_cachedDampCoef(dgVector::m_zero) + ,m_cachedTimeStep(dgFloat32(0.0f)) + ,m_sleepingCounter(0) + ,m_isInDestructionArrayLRU(0) + ,m_skeleton(NULL) + ,m_applyExtForces(NULL) +{ + m_type = m_dynamicBody; + m_rtti |= m_dynamicBodyRTTI; + + m_invWorldInertiaMatrix[3][3] = dgFloat32 (1.0f); + serializeCallback (userData, &m_mass, sizeof (m_mass)); + serializeCallback (userData, &m_invMass, sizeof (m_invMass)); + serializeCallback (userData, &m_dampCoef, sizeof (m_dampCoef)); +} + +dgDynamicBody::~dgDynamicBody() +{ + if (m_skeleton) { + dgSkeletonContainer* const skel = m_skeleton; + m_skeleton = NULL; + m_world->DestroySkeletonContainer(skel); + } +} + +void dgDynamicBody::Serialize (const dgTree& collisionRemapId, dgSerialize serializeCallback, void* const userData) +{ + dgBody::Serialize (collisionRemapId, serializeCallback, userData); + + serializeCallback (userData, &m_mass, sizeof (m_mass)); + serializeCallback (userData, &m_invMass, sizeof (m_invMass)); + serializeCallback (userData, &m_dampCoef, sizeof (m_dampCoef)); +} + + +void dgDynamicBody::SetMatrixResetSleep(const dgMatrix& matrix) +{ + dgBody::SetMatrixResetSleep(matrix); + m_savedExternalForce = dgVector::m_zero; + m_savedExternalTorque = dgVector::m_zero; + CalcInvInertiaMatrix(); +} + +void dgDynamicBody::SetMatrixNoSleep(const dgMatrix& matrix) +{ + dgBody::SetMatrixNoSleep(matrix); + CalcInvInertiaMatrix(); +} + +void dgDynamicBody::AttachCollision (dgCollisionInstance* const collision) +{ + dgBody::AttachCollision(collision); + if (m_collision->IsType(dgCollision::dgCollisionMesh_RTTI) || m_collision->IsType(dgCollision::dgCollisionScene_RTTI)) { + //SetMassMatrix (m_mass.m_w, m_mass.m_x, m_mass.m_y, m_mass.m_z); + SetMassMatrix (m_mass.m_w, CalculateLocalInertiaMatrix()); + } +} + + +dgVector dgDynamicBody::GetAlpha() const +{ + return m_alpha; +} + +dgVector dgDynamicBody::GetAccel() const +{ + return m_accel; +} + +void dgDynamicBody::SetAlpha(const dgVector& alpha) +{ + m_alpha = alpha; +} + +void dgDynamicBody::SetAccel(const dgVector& accel) +{ + m_accel = accel; +} + + +bool dgDynamicBody::IsInEquilibrium() const +{ + if (m_equilibrium) { + dgVector deltaAccel((m_externalForce - m_savedExternalForce).Scale(m_invMass.m_w)); + dgAssert(deltaAccel.m_w == dgFloat32 (0.0f)); + dgFloat32 deltaAccel2 = deltaAccel.DotProduct(deltaAccel).GetScalar(); + if (deltaAccel2 > DG_ERR_TOLERANCE2) { + return false; + } + dgVector deltaAlpha(m_matrix.UnrotateVector(m_externalTorque - m_savedExternalTorque) * m_invMass); + dgAssert(deltaAlpha.m_w == dgFloat32 (0.0f)); + dgFloat32 deltaAlpha2 = deltaAlpha.DotProduct(deltaAlpha).GetScalar(); + if (deltaAlpha2 > DG_ERR_TOLERANCE2) { + return false; + } + return true; + } + return false; +} + +void dgDynamicBody::ApplyExtenalForces (dgFloat32 timestep, dgInt32 threadIndex) +{ + m_externalForce = dgVector::m_zero; + m_externalTorque = dgVector::m_zero; + if (m_applyExtForces) { + m_applyExtForces(*this, timestep, threadIndex); + } + + m_gyroRotation = m_rotation; + m_gyroTorque = dgVector::m_zero; + + m_externalForce += m_impulseForce; + m_externalTorque += m_impulseTorque; + m_impulseForce = dgVector::m_zero; + m_impulseTorque = dgVector::m_zero; +} + +void dgDynamicBody::AddDampingAcceleration(dgFloat32 timestep) +{ + dgVector damp (GetDampCoeffcient (timestep)); + dgVector omegaDamp(damp & dgVector::m_triplexMask); + dgVector omega(m_matrix.UnrotateVector(m_omega) * omegaDamp); + + m_veloc = m_veloc.Scale(damp.m_w); + m_omega = m_matrix.RotateVector(omega); +} + +void dgDynamicBody::InvalidateCache () +{ + m_sleepingCounter = 0; + m_savedExternalForce = dgVector::m_zero; + m_savedExternalTorque = dgVector::m_zero; + dgBody::InvalidateCache (); +} + +void dgDynamicBody::IntegrateImplicit(dgFloat32 timestep) +{ + // using simple backward Euler or implicit integration, this is. + // f'(t + dt) = (f(t + dt) - f(t)) / dt + + // therefore: + // f(t + dt) = f(t) + f'(t + dt) * dt + + // approximate f'(t + dt) by expanding the Taylor of f(w + dt) + // f(w + dt) = f(w) + f'(w) * dt + f''(w) * dt^2 / 2! + .... + + // assume dt^2 is negligible, therefore we can truncate the expansion to + // f(w + dt) ~= f(w) + f'(w) * dt + + // calculating dw as the f'(w) = d(wx, wy, wz) | dt + // dw/dt = a = (Tl - (wl x (wl * Il)) * Il^1 + + // expanding f(w) + // f'(wx) = Ix * ax = Tx - (Iz - Iy) * wy * wz + // f'(wy) = Iy * ay = Ty - (Ix - Iz) * wz * wx + // f'(wz) = Iz * az = Tz - (Iy - Ix) * wx * wy + // + // calculation the expansion + // Ix * ax = (Tx - (Iz - Iy) * wy * wz) - ((Iz - Iy) * wy * az + (Iz - Iy) * ay * wz) * dt + // Iy * ay = (Ty - (Ix - Iz) * wz * wx) - ((Ix - Iz) * wz * ax + (Ix - Iz) * az * wx) * dt + // Iz * az = (Tz - (Iy - Ix) * wx * wy) - ((Iy - Ix) * wx * ay + (Iy - Ix) * ax * wy) * dt + // + // factorizing a we get + // Ix * ax + (Iz - Iy) * dwy * az + (Iz - Iy) * dwz * ay = Tx - (Iz - Iy) * wy * wz + // Iy * ay + (Ix - Iz) * dwz * ax + (Ix - Iz) * dwx * az = Ty - (Ix - Iz) * wz * wx + // Iz * az + (Iy - Ix) * dwx * ay + (Iy - Ix) * dwy * ax = Tz - (Iy - Ix) * wx * wy + + dgVector localOmega(m_matrix.UnrotateVector(m_omega)); + dgVector localTorque(m_matrix.UnrotateVector(m_externalTorque - m_gyroTorque)); + + // and solving for alpha we get the angular acceleration at t + dt + // calculate gradient at a full time step + dgVector gradientStep(localTorque.Scale(timestep)); + + // derivative at half time step. (similar to midpoint Euler so that it does not loses too much energy) + dgVector dw(localOmega.Scale(dgFloat32(0.5f) * timestep)); + //dgVector dw(localOmega.Scale(dgFloat32(1.0f) * timestep)); + + // calculates Jacobian matrix ( + // dWx / dwx, dWx / dwy, dWx / dwz + // dWy / dwx, dWy / dwy, dWy / dwz + // dWz / dwx, dWz / dwy, dWz / dwz + // + // dWx / dwx = Ix, dWx / dwy = (Iz - Iy) * wz * dt, dWx / dwz = (Iz - Iy) * wy * dt) + // dWy / dwx = (Ix - Iz) * wz * dt, dWy / dwy = Iy, dWy / dwz = (Ix - Iz) * wx * dt + // dWz / dwx = (Iy - Ix) * wy * dt, dWz / dwy = (Iy - Ix) * wx * dt, dWz / dwz = Iz + dgMatrix jacobianMatrix( + dgVector(m_mass[0], (m_mass[2] - m_mass[1]) * dw[2], (m_mass[2] - m_mass[1]) * dw[1], dgFloat32(0.0f)), + dgVector((m_mass[0] - m_mass[2]) * dw[2], m_mass[1], (m_mass[0] - m_mass[2]) * dw[0], dgFloat32(0.0f)), + dgVector((m_mass[1] - m_mass[0]) * dw[1], (m_mass[1] - m_mass[0]) * dw[0], m_mass[2], dgFloat32(0.0f)), + dgVector::m_wOne); + + gradientStep = jacobianMatrix.SolveByGaussianElimination(gradientStep); + + localOmega += gradientStep; + + m_accel = m_externalForce.Scale(m_invMass.m_w); + m_alpha = m_matrix.RotateVector(localTorque * m_invMass); + + m_veloc += m_accel.Scale(timestep); + m_omega = m_matrix.RotateVector(localOmega); +} + +dgJacobian dgDynamicBody::IntegrateForceAndToque(const dgVector& force, const dgVector& torque, const dgVector& timestep) +{ + dgJacobian velocStep; + if (m_gyroTorqueOn) { + dgVector dtHalf(timestep * dgVector::m_half); + dgMatrix matrix(m_gyroRotation, dgVector::m_wOne); + + dgVector localOmega(matrix.UnrotateVector(m_omega)); + dgVector localTorque(matrix.UnrotateVector(torque - m_gyroTorque)); + + // derivative at half time step. (similar to midpoint Euler so that it does not loses too much energy) + dgVector dw(localOmega * dtHalf); + dgMatrix jacobianMatrix( + dgVector(m_mass[0], (m_mass[2] - m_mass[1]) * dw[2], (m_mass[2] - m_mass[1]) * dw[1], dgFloat32(0.0f)), + dgVector((m_mass[0] - m_mass[2]) * dw[2], m_mass[1], (m_mass[0] - m_mass[2]) * dw[0], dgFloat32(1.0f)), + dgVector((m_mass[1] - m_mass[0]) * dw[1], (m_mass[1] - m_mass[0]) * dw[0], m_mass[2], dgFloat32(1.0f)), + dgVector::m_wOne); + + // and solving for alpha we get the angular acceleration at t + dt + // calculate gradient at a full time step + //dgVector gradientStep(localTorque * timestep); + dgVector gradientStep(jacobianMatrix.SolveByGaussianElimination(localTorque * timestep)); + + dgVector omega(matrix.RotateVector(localOmega + gradientStep)); + dgAssert(omega.m_w == dgFloat32(0.0f)); + + // integrate rotation here + dgFloat32 omegaMag2 = omega.DotProduct(omega).GetScalar() + dgFloat32(1.0e-12f); + dgFloat32 invOmegaMag = dgRsqrt(omegaMag2); + dgVector omegaAxis(omega.Scale(invOmegaMag)); + dgFloat32 omegaAngle = invOmegaMag * omegaMag2 * timestep.GetScalar(); + dgQuaternion deltaRotation(omegaAxis, omegaAngle); + m_gyroRotation = m_gyroRotation * deltaRotation; + dgAssert((m_gyroRotation.DotProduct(m_gyroRotation) - dgFloat32(1.0f)) < dgFloat32(1.0e-5f)); + + matrix = dgMatrix(m_gyroRotation, dgVector::m_wOne); + localOmega = matrix.UnrotateVector(omega); + //dgVector angularMomentum(inertia * localOmega); + //body->m_gyroTorque = matrix.RotateVector(localOmega.CrossProduct(angularMomentum)); + //body->m_gyroAlpha = body->m_invWorldInertiaMatrix.RotateVector(body->m_gyroTorque); + dgVector localGyroTorque(localOmega.CrossProduct(m_mass * localOmega)); + m_gyroTorque = matrix.RotateVector(localGyroTorque); + m_gyroAlpha = matrix.RotateVector(localGyroTorque * m_invMass); + + velocStep.m_angular = matrix.RotateVector(gradientStep); + } else { + velocStep.m_angular = m_invWorldInertiaMatrix.RotateVector(torque) * timestep; + //velocStep.m_angular = velocStep.m_angular * dgVector::m_half; + } + + velocStep.m_linear = force.Scale(m_invMass.m_w) * timestep; + return velocStep; +} + +void dgDynamicBody::IntegrateOpenLoopExternalForce(dgFloat32 timestep) +{ + if (!m_equilibrium) { + if (!m_collision->IsType(dgCollision::dgCollisionLumpedMass_RTTI)) { + IntegrateImplicit(timestep); + } else { + dgCollisionLumpedMassParticles* const lumpedMassShape = (dgCollisionLumpedMassParticles*)m_collision->m_childShape; + lumpedMassShape->IntegrateForces(timestep); + } + } else { + m_accel = dgVector::m_zero; + m_alpha = dgVector::m_zero; + } +} + + diff --git a/thirdparty/src/newton/dgPhysics/dgDynamicBody.h b/thirdparty/src/newton/dgPhysics/dgDynamicBody.h new file mode 100644 index 000000000..724e131f2 --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgDynamicBody.h @@ -0,0 +1,258 @@ +/* Copyright (c) <2003-2019> +* +* 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_DYNAMIC_BODY_H_ +#define _DG_DYNAMIC_BODY_H_ + +#include "dgPhysicsStdafx.h" +#include "dgBody.h" + +#define DG_MAX_SPEED_ATT dgFloat32(0.02f) +//#define DG_FREEZE_ACCEL dgFloat32(0.1f) +#define DG_FREEZE_ACCEL dgFloat32(1.0f) +#define DG_FREEZE_SPEED dgFloat32(0.032f) + +#define DG_FREEZE_ACCEL2 (DG_FREEZE_ACCEL * DG_FREEZE_ACCEL) +#define DG_FREEZE_SPEED2 (DG_FREEZE_SPEED * DG_FREEZE_SPEED) + +#define DG_FREEZE_MAG DG_FREEZE_ACCEL +#define DG_FREEZE_MAG2 (DG_FREEZE_MAG * DG_FREEZE_MAG) + +#define DG_ERR_TOLERANCE dgFloat32(1.0e-2f) +#define DG_ERR_TOLERANCE2 (DG_ERR_TOLERANCE * DG_ERR_TOLERANCE) + +class dgSkeletonContainer; + +DG_MSC_VECTOR_ALIGNMENT +class dgDynamicBody : public dgBody +{ + public: + dgDynamicBody(); + dgDynamicBody (dgWorld* const world, const dgTree* const collisionNode, dgDeserialize serializeCallback, void* const userData, dgInt32 revisionNumber); + virtual ~dgDynamicBody (); + + virtual const dgVector& GetForce() const; + virtual const dgVector& GetTorque() const; + + virtual void AddForce (const dgVector& force); + virtual void AddTorque (const dgVector& torque); + virtual void SetForce (const dgVector& force); + virtual void SetTorque (const dgVector& torque); + + virtual dgFloat32 GetLinearDamping () const; + virtual dgVector GetAngularDamping () const; + virtual void SetLinearDamping (dgFloat32 linearDamp); + virtual void SetAngularDamping (const dgVector& angularDamp); + + virtual void AttachCollision (dgCollisionInstance* const collision); + virtual dgVector PredictLinearVelocity(dgFloat32 timestep) const; + virtual dgVector PredictAngularVelocity(dgFloat32 timestep) const; + + virtual void InvalidateCache(); + virtual void SetMatrixResetSleep(const dgMatrix& matrix); + virtual void SetMatrixNoSleep(const dgMatrix& matrix); + + virtual bool IsInEquilibrium () const; + virtual void SetCollidable (bool state) {} + + virtual void ApplyExtenalForces (dgFloat32 timestep, dgInt32 threadIndex); + virtual OnApplyExtForceAndTorque GetExtForceAndTorqueCallback () const; + virtual void SetExtForceAndTorqueCallback (OnApplyExtForceAndTorque callback); + virtual void Serialize (const dgTree& collisionRemapId, dgSerialize serializeCallback, void* const userData); + + virtual dgSkeletonContainer* GetSkeleton() const; + void SetSkeleton(dgSkeletonContainer* const skeleton); + + void IntegrateImplicit(dgFloat32 timeStep); + virtual void IntegrateOpenLoopExternalForce(dgFloat32 timeStep); + const dgVector& GetDampCoeffcient (dgFloat32 timestep); + + virtual dgJacobian IntegrateForceAndToque(const dgVector& force, const dgVector& torque, const dgVector& timestep); + + private: + virtual void AddDampingAcceleration(dgFloat32 timestep); + + virtual dgVector GetAlpha() const; + virtual dgVector GetAccel() const; + virtual void SetAlpha(const dgVector& alpha); + virtual void SetAccel(const dgVector& accel); + + dgVector m_externalForce; + dgVector m_externalTorque; + dgVector m_savedExternalForce; + dgVector m_savedExternalTorque; + dgVector m_dampCoef; + dgVector m_cachedDampCoef; + dgFloat32 m_cachedTimeStep; + dgInt32 m_sleepingCounter; + dgUnsigned32 m_isInDestructionArrayLRU; + dgSkeletonContainer* m_skeleton; + OnApplyExtForceAndTorque m_applyExtForces; + static dgVector m_equilibriumError2; + + friend class dgWorld; + friend class dgSolver; + friend class dgBroadPhase; + friend class dgBodyMasterList; + friend class dgSkeletonContainer; + friend class dgWorldDynamicUpdate; + friend class dgParallelBodySolver; + friend class dgCollisionDeformableMesh; + friend class dgCollisionDeformableSolidMesh; + friend class dgCollisionMassSpringDamperSystem; +} DG_GCC_VECTOR_ALIGNMENT; + + +DG_MSC_VECTOR_ALIGNMENT +class dgDynamicBodyAsymetric: public dgDynamicBody +{ + public: + dgDynamicBodyAsymetric(); + dgDynamicBodyAsymetric(dgWorld* const world, const dgTree* const collisionNode, dgDeserialize serializeCallback, void* const userData, dgInt32 revisionNumber); + + virtual void Serialize(const dgTree& collisionRemapId, dgSerialize serializeCallback, void* const userData); + + virtual dgMatrix CalculateInertiaMatrix() const; + virtual dgMatrix CalculateLocalInertiaMatrix() const; + virtual dgMatrix CalculateInvInertiaMatrix() const; + virtual void SetMassMatrix(dgFloat32 mass, const dgMatrix& inertia); + virtual void IntegrateOpenLoopExternalForce(dgFloat32 timestep); + + dgMatrix m_principalAxis; +} DG_GCC_VECTOR_ALIGNMENT; + + + +DG_INLINE const dgVector& dgDynamicBody::GetForce() const +{ + return m_externalForce; +} + +DG_INLINE const dgVector& dgDynamicBody::GetTorque() const +{ + return m_externalTorque; +} + + +DG_INLINE dgFloat32 dgDynamicBody::GetLinearDamping () const +{ + return m_dampCoef.m_w / DG_MAX_SPEED_ATT; +} + +DG_INLINE dgVector dgDynamicBody::GetAngularDamping () const +{ + return dgVector (m_dampCoef.m_x / DG_MAX_SPEED_ATT, + m_dampCoef.m_y / DG_MAX_SPEED_ATT, + m_dampCoef.m_z / DG_MAX_SPEED_ATT, dgFloat32 (0.0f)); +} + +DG_INLINE void dgDynamicBody::SetLinearDamping (dgFloat32 linearDamp) +{ + linearDamp = dgClamp (linearDamp, dgFloat32(0.0f), dgFloat32(1.0f)); + m_dampCoef.m_w = DG_MAX_SPEED_ATT * linearDamp; + m_cachedTimeStep = dgFloat32(0.0f); +} + +DG_INLINE void dgDynamicBody::SetAngularDamping (const dgVector& angularDamp) +{ + dgFloat32 tmp = dgClamp (angularDamp.m_x, dgFloat32(0.0f), dgFloat32(1.0f)); + m_dampCoef.m_x = DG_MAX_SPEED_ATT * tmp; + + tmp = dgClamp (angularDamp.m_y, dgFloat32(0.0f), dgFloat32(1.0f)); + m_dampCoef.m_y = DG_MAX_SPEED_ATT * tmp; + + tmp = dgClamp (angularDamp.m_z, dgFloat32(0.0f), dgFloat32(1.0f)); + m_dampCoef.m_z = DG_MAX_SPEED_ATT * tmp; + + m_cachedTimeStep = dgFloat32(0.0f); +} + +DG_INLINE void dgDynamicBody::AddForce (const dgVector& force) +{ + SetForce (m_externalForce + force); +} + +DG_INLINE void dgDynamicBody::AddTorque (const dgVector& torque) +{ + SetTorque (torque + m_externalTorque); +} + + +DG_INLINE void dgDynamicBody::SetForce (const dgVector& force) +{ + m_externalForce = force; +} + +DG_INLINE void dgDynamicBody::SetTorque (const dgVector& torque) +{ + m_externalTorque = torque; +} + + +DG_INLINE dgVector dgDynamicBody::PredictLinearVelocity(dgFloat32 timestep) const +{ + return m_veloc + m_externalForce.Scale (timestep * m_invMass.m_w); +} + +DG_INLINE dgVector dgDynamicBody::PredictAngularVelocity(dgFloat32 timestep) const +{ + return m_omega + m_invWorldInertiaMatrix.RotateVector(m_externalTorque).Scale (timestep); +} + + +DG_INLINE dgBody::OnApplyExtForceAndTorque dgDynamicBody::GetExtForceAndTorqueCallback () const +{ + return m_applyExtForces; +} + +DG_INLINE void dgDynamicBody::SetExtForceAndTorqueCallback (OnApplyExtForceAndTorque callback) +{ + m_applyExtForces = callback; +} + +DG_INLINE dgSkeletonContainer* dgDynamicBody::GetSkeleton() const +{ + return m_skeleton; +} + +DG_INLINE void dgDynamicBody::SetSkeleton(dgSkeletonContainer* const skeleton) +{ + dgAssert (!(m_skeleton && skeleton)); + m_skeleton = skeleton; +} + +DG_INLINE const dgVector& dgDynamicBody::GetDampCoeffcient (dgFloat32 timestep) +{ + if (dgAbs(m_cachedTimeStep - timestep) > dgFloat32(1.0e-6f)) { + m_cachedTimeStep = timestep; + const dgFloat32 tau = dgFloat32(60.0f) * timestep; + m_cachedDampCoef.m_x = dgPow(dgFloat32(1.0f) - m_dampCoef.m_x, tau); + m_cachedDampCoef.m_y = dgPow(dgFloat32(1.0f) - m_dampCoef.m_y, tau); + m_cachedDampCoef.m_z = dgPow(dgFloat32(1.0f) - m_dampCoef.m_z, tau); + m_cachedDampCoef.m_w = dgPow(dgFloat32(1.0f) - m_dampCoef.m_w, tau); + } + return m_cachedDampCoef; +} + + +#endif + + diff --git a/thirdparty/src/newton/dgPhysics/dgHingeConstraint.cpp b/thirdparty/src/newton/dgPhysics/dgHingeConstraint.cpp new file mode 100644 index 000000000..f9f8ada0a --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgHingeConstraint.cpp @@ -0,0 +1,199 @@ +/* Copyright (c) <2003-2019> +* +* 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 "dgPhysicsStdafx.h" +#include "dgBody.h" +#include "dgWorld.h" +#include "dgHingeConstraint.h" + + +////////////////////////////////////////////////////////////////////// +// Construction/Destruction +////////////////////////////////////////////////////////////////////// + +dgHingeConstraint::dgHingeConstraint () + :dgBilateralConstraint() +{ + dgAssert ((((dgUnsigned64) &m_localMatrix0) & 15) == 0); +// constraint->Init (); + + m_localMatrix0 = dgGetIdentityMatrix(); + m_localMatrix1 = dgGetIdentityMatrix(); + + m_maxDOF = 6; + m_jointAccelFnt = NULL; + m_constId = m_hingeConstraint; + m_angle = dgFloat32 (dgFloat32 (0.0f)); +} + +dgHingeConstraint::~dgHingeConstraint () +{ +} + +/* +dgHingeConstraint* dgHingeConstraint::Create(dgWorld* world) +{ + dgHingeConstraint* constraint; + + //constraint = dgHingeConstraintArray::GetPool().GetElement(); + dgHingeConstraintArray& array = *world; + constraint = array.GetElement(); + + dgAssert ((((dgUnsigned64) &constraint->m_localMatrix0) & 15) == 0); + constraint->Init (); + constraint->m_maxDOF = 6; + constraint->m_constId = dgHingeConstraintId; + + constraint->m_angle = dgFloat32 (0.0f); + constraint->m_jointAccelFnt = NULL; + return constraint; +} + +void dgHingeConstraint::Remove(dgWorld* world) +{ + dgHingeConstraintArray& array = *world; + dgBilateralConstraint::Remove (world); + //dgHingeConstraintArray::GetPool().RemoveElement (this); + array.RemoveElement (this); +} +*/ + +void dgHingeConstraint::SetJointParameterCallback (dgHingeJointAcceleration callback) +{ + m_jointAccelFnt = callback; +} + +dgFloat32 dgHingeConstraint::GetJointAngle () const +{ + return m_angle; +} + +dgFloat32 dgHingeConstraint::GetJointOmega () const +{ + dgAssert (m_body0); + dgAssert (m_body1); + dgVector dir (m_body0->GetMatrix().RotateVector (m_localMatrix0[0])); + const dgVector& omega0 = m_body0->GetOmega(); + const dgVector& omega1 = m_body1->GetOmega(); + return dir.DotProduct(omega0 - omega1).GetScalar(); +} + +dgFloat32 dgHingeConstraint::CalculateStopAlpha (dgFloat32 angle, const dgJointCallbackParam* param) const +{ + dgFloat32 alpha; + dgFloat32 omega; + dgFloat32 penetrationErr; + + alpha = dgFloat32 (0.0f); + if (m_angle > angle) { + omega = GetJointOmega (); + if (omega < dgFloat32 (0.0f)) { + omega = dgFloat32 (0.0f); + } + penetrationErr = (angle - m_angle); + alpha = dgFloat32 (100.0f) * penetrationErr - omega * dgFloat32 (1.01f) / param->m_timestep; + + } else if (m_angle < angle) { + omega = GetJointOmega (); + if (omega > dgFloat32 (0.0f)) { + omega = dgFloat32 (0.0f); + } + + penetrationErr = MIN_JOINT_PIN_LENGTH * (angle - m_angle); + alpha = dgFloat32 (100.0f) * penetrationErr - omega * dgFloat32 (1.01f) / param->m_timestep; + } + return alpha; +} + +dgVector dgHingeConstraint::GetJointForce () const +{ + dgMatrix matrix0; + dgMatrix matrix1; + + CalculateGlobalMatrixAndAngle (m_localMatrix0, m_localMatrix1, matrix0, matrix1); + return dgVector (matrix0.m_front.Scale (m_jointForce[0].m_force) + + matrix0.m_up.Scale (m_jointForce[1].m_force) + + matrix0.m_right.Scale (m_jointForce[2].m_force) + + matrix0.m_up.Scale (m_jointForce[3].m_force) + + matrix0.m_right.Scale (m_jointForce[4].m_force)); +} + + +dgUnsigned32 dgHingeConstraint::JacobianDerivative (dgContraintDescritor& params) +{ + dgMatrix matrix0; + dgMatrix matrix1; + dgVector angle (CalculateGlobalMatrixAndAngle (m_localMatrix0, m_localMatrix1, matrix0, matrix1)); + + m_angle = -angle.m_x; + + dgAssert (dgAbs (1.0f - matrix0.m_front.DotProduct(matrix0.m_front).GetScalar()) < dgFloat32 (1.0e-5f)); + dgAssert (dgAbs (1.0f - matrix0.m_up.DotProduct(matrix0.m_up).GetScalar()) < dgFloat32 (1.0e-5f)); + dgAssert (dgAbs (1.0f - matrix0.m_right.DotProduct(matrix0.m_right).GetScalar()) < dgFloat32 (1.0e-5f)); + + const dgVector& dir0 = matrix0.m_front; + const dgVector& dir1 = matrix0.m_up; + const dgVector& dir2 = matrix0.m_right; + + const dgVector& p0 = matrix0.m_posit; + const dgVector& p1 = matrix1.m_posit; + dgVector q0 (p0 + matrix0.m_front.Scale(MIN_JOINT_PIN_LENGTH)); + dgVector q1 (p1 + matrix1.m_front.Scale(MIN_JOINT_PIN_LENGTH)); + +// dgAssert (((p1 - p0) % (p1 - p0)) < 1.0e-2f); + + dgPointParam pointDataP; + dgPointParam pointDataQ; + InitPointParam (pointDataP, m_defualtDiagonalRegularizer, p0, p1); + InitPointParam (pointDataQ, m_defualtDiagonalRegularizer, q0, q1); + + CalculatePointDerivative (0, params, dir0, pointDataP, &m_jointForce[0]); + CalculatePointDerivative (1, params, dir1, pointDataP, &m_jointForce[1]); + CalculatePointDerivative (2, params, dir2, pointDataP, &m_jointForce[2]); + CalculatePointDerivative (3, params, dir1, pointDataQ, &m_jointForce[3]); + CalculatePointDerivative (4, params, dir2, pointDataQ, &m_jointForce[4]); + + dgInt32 ret = 5; + if (m_jointAccelFnt) { + dgJointCallbackParam axisParam; + axisParam.m_accel = dgFloat32 (0.0f); + axisParam.m_timestep = params.m_timestep; + axisParam.m_minFriction = DG_MIN_BOUND; + axisParam.m_maxFriction = DG_MAX_BOUND; + + if (m_jointAccelFnt (*this, &axisParam)) { + if ((axisParam.m_minFriction > DG_MIN_BOUND) || (axisParam.m_maxFriction < DG_MAX_BOUND)) { + params.m_forceBounds[5].m_low = axisParam.m_minFriction; + params.m_forceBounds[5].m_upper = axisParam.m_maxFriction; + params.m_forceBounds[5].m_normalIndex = DG_INDEPENDENT_ROW; + } + + CalculateAngularDerivative (5, params, dir0, m_defualtDiagonalRegularizer, dgFloat32 (0.0f), &m_jointForce[5]); + SetMotorAcceleration (5, axisParam.m_accel, params); + ret = 6; + } + } + + return dgUnsigned32 (ret); +} + + + diff --git a/thirdparty/src/newton/dgPhysics/dgHingeConstraint.h b/thirdparty/src/newton/dgPhysics/dgHingeConstraint.h new file mode 100644 index 000000000..4fb1ef969 --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgHingeConstraint.h @@ -0,0 +1,72 @@ +/* Copyright (c) <2003-2019> +* +* 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_DGHINGECONSTRAINT_H__3FF7D7AA_90CC_4BA5_B2A4_D3BA51AD2FFD_H) +#define AFX_DGHINGECONSTRAINT_H__3FF7D7AA_90CC_4BA5_B2A4_D3BA51AD2FFD_H +#include "dgBilateralConstraint.h" + +//template class dgPool; + +class dgHingeConstraint; + + +typedef dgUnsigned32 (dgApi *dgHingeJointAcceleration) (const dgHingeConstraint& hinge, dgJointCallbackParam* param); + +class dgHingeConstraint: public dgBilateralConstraint +{ + public: + dgFloat32 GetJointAngle () const; + dgFloat32 GetJointOmega () const; + dgVector GetJointForce () const; + dgFloat32 CalculateStopAlpha (dgFloat32 angle, const dgJointCallbackParam* param) const; + void SetJointParameterCallback (dgHingeJointAcceleration callback); + + private: + virtual dgUnsigned32 JacobianDerivative (dgContraintDescritor& params); + virtual void Serialize (dgSerialize serializeCallback, void* const userData) {dgAssert (0);} + + dgHingeConstraint(); + virtual ~dgHingeConstraint(); + + dgMatrix m_localMatrix0; + dgMatrix m_localMatrix1; + + dgFloat32 m_angle; + dgHingeJointAcceleration m_jointAccelFnt; + + +//#ifdef _NEWTON_USE_DOUBLE +// dgUnsigned32 m_reserve[1]; +//#else +// dgUnsigned32 m_reserve[2]; +//#endif + + friend class dgWorld; +// friend class dgPool; +}; + +//class dgHingeConstraintArray: public dgPoolContainer +//{ +//}; + + +#endif // !defined(AFX_DGHINGECONSTRAINT_H__3FF7D7AA_90CC_4BA5_B2A4_D3BA51AD2FFD_H) + diff --git a/thirdparty/src/newton/dgPhysics/dgKinematicBody.cpp b/thirdparty/src/newton/dgPhysics/dgKinematicBody.cpp new file mode 100644 index 000000000..1ca81df3a --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgKinematicBody.cpp @@ -0,0 +1,53 @@ +/* Copyright (c) <2003-2019> +* +* 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 "dgPhysicsStdafx.h" +#include "dgKinematicBody.h" + + +dgVector dgKinematicBody::m_dummy (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f)); + + +dgKinematicBody::dgKinematicBody() + :dgBody() +{ + m_collidable = false; + m_type = m_kinematicBody; + m_rtti |= m_kinematicBodyRTTI; +} + +dgKinematicBody::dgKinematicBody (dgWorld* const world, const dgTree* const collisionNode, dgDeserialize serializeCallback, void* const userData, dgInt32 revisionNumber) + :dgBody (world, collisionNode, serializeCallback, userData, revisionNumber) +{ + m_collidable = false; + m_type = m_kinematicBody; + m_rtti |= m_kinematicBodyRTTI; +} + +dgKinematicBody::~dgKinematicBody () +{ +} + +void dgKinematicBody::Serialize (const dgTree& collisionRemapId, dgSerialize serializeCallback, void* const userData) +{ + dgBody::Serialize (collisionRemapId, serializeCallback, userData); +} + diff --git a/thirdparty/src/newton/dgPhysics/dgKinematicBody.h b/thirdparty/src/newton/dgPhysics/dgKinematicBody.h new file mode 100644 index 000000000..0ffafe814 --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgKinematicBody.h @@ -0,0 +1,71 @@ +/* Copyright (c) <2003-2019> +* +* 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_KINEMATIC_BODY_H_ +#define _DG_KINEMATIC_BODY_H_ + +#include "dgPhysicsStdafx.h" +#include "dgBody.h" + + +DG_MSC_VECTOR_ALIGNMENT +class dgKinematicBody: public dgBody +{ + public: + dgKinematicBody(); + dgKinematicBody (dgWorld* const world, const dgTree* const collisionNode, dgDeserialize serializeCallback, void* const userData, dgInt32 revisionNumber); + virtual ~dgKinematicBody (); + + virtual const dgVector& GetForce() const {return m_dummy;} + virtual const dgVector& GetTorque() const {return m_dummy;} + + virtual void AddForce (const dgVector& force) {} + virtual void AddTorque (const dgVector& torque) {} + virtual void SetForce (const dgVector& force) {} + virtual void SetTorque (const dgVector& torque) {} + virtual void ApplyExtenalForces (dgFloat32 timestep, dgInt32 threadIndex) {} + virtual OnApplyExtForceAndTorque GetExtForceAndTorqueCallback () const {return NULL;} + virtual void SetExtForceAndTorqueCallback (OnApplyExtForceAndTorque callback) {} + + virtual dgFloat32 GetLinearDamping () const {return dgFloat32 (0.0f);} + virtual dgVector GetAngularDamping () const {return m_dummy;} + virtual void SetLinearDamping (dgFloat32 linearDamp) {} + virtual void SetAngularDamping (const dgVector& angularDamp) {} + + virtual dgVector PredictLinearVelocity(dgFloat32 timestep) const {return m_veloc;} + virtual dgVector PredictAngularVelocity(dgFloat32 timestep) const {return m_omega;} + + virtual bool IsInEquilibrium () const {return true;} + virtual void SetCollidable (bool state) {m_collidable = state;} + virtual void Serialize (const dgTree& collisionRemapId, dgSerialize serializeCallback, void* const userData); + virtual void AddDampingAcceleration(dgFloat32 timestep) {} + + static dgVector m_dummy; + + friend class dgWorld; + friend class dgBroadPhase; + friend class dgWorldDynamicUpdate; +} DG_GCC_VECTOR_ALIGNMENT; + + + +#endif + diff --git a/thirdparty/src/newton/dgPhysics/dgNarrowPhaseCollision.cpp b/thirdparty/src/newton/dgPhysics/dgNarrowPhaseCollision.cpp new file mode 100644 index 000000000..e441ee1ee --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgNarrowPhaseCollision.cpp @@ -0,0 +1,2077 @@ +/* Copyright (c) <2003-2019> +* +* 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 "dgPhysicsStdafx.h" + +#include "dgWorld.h" +#include "dgCollision.h" +#include "dgMeshEffect.h" +#include "dgDynamicBody.h" +#include "dgCollisionBVH.h" +#include "dgCollisionBox.h" +#include "dgContactSolver.h" +#include "dgKinematicBody.h" +#include "dgCollisionCone.h" +#include "dgCollisionNull.h" +#include "dgBodyMasterList.h" +#include "dgCollisionScene.h" +#include "dgCollisionSphere.h" +#include "dgCollisionCapsule.h" +#include "dgCollisionCylinder.h" +#include "dgCollisionCompound.h" +#include "dgCollisionUserMesh.h" +#include "dgCollisionInstance.h" +#include "dgWorldDynamicUpdate.h" +#include "dgCollisionConvexHull.h" +#include "dgCollisionHeightField.h" +#include "dgCollisionConvexPolygon.h" +#include "dgCollisionDeformableMesh.h" +#include "dgCollisionChamferCylinder.h" +#include "dgCollisionCompoundFractured.h" +#include "dgCollisionDeformableSolidMesh.h" +#include "dgCollisionMassSpringDamperSystem.h" +#include "dgCollisionIncompressibleParticles.h" + + +DG_MSC_VECTOR_ALIGNMENT +class dgCollisionContactCloud: public dgCollisionConvex +{ + public: + dgCollisionContactCloud(dgMemoryAllocator* const allocator, dgInt32 count, const dgContactPoint* const contacts) + :dgCollisionConvex(allocator, 0, m_contactCloud) + ,m_contacts(contacts) + ,m_count(count) + { + } + ~dgCollisionContactCloud() + { + } + + dgInt32 CalculateSignature() const + { + dgAssert (0); + return 0; + } + void Serialize(dgSerialize callback, void* const userData) const + { + dgAssert (0); + } + void SetCollisionBBox(const dgVector& p0, const dgVector& p1) + { + dgAssert (0); + } + dgFloat32 RayCast(const dgVector& localP0, const dgVector& localP1, dgFloat32 maxT, dgContactPoint& contactOut, const dgBody* const body, void* const userData, OnRayPrecastAction preFilter) const + { + dgAssert (0); + return 0; + } + + dgVector SupportVertex(const dgVector& dir, dgInt32* const vertexIndex) const + { + dgInt32 index = 0; + dgFloat32 dist = dgFloat32 (-1.0e10f); + for (dgInt32 i = 0; i < m_count; i ++) { + dgFloat32 dist1 = dir.DotProduct(m_contacts[i].m_point).GetScalar(); + if (dist1 > dist) { + index = i; + dist = dist1; + } + } + return m_contacts[index].m_point; + } + + dgInt32 CalculatePlaneIntersection(const dgVector& normal, const dgVector& point, dgVector* const contactsOut) const + { + *contactsOut = point; + return 1; + } + + dgFloat32 GetVolume() const + { + dgAssert (0); + return 0; + } + dgFloat32 GetBoxMinRadius() const + { + dgAssert (0); + return 0; + } + dgFloat32 GetBoxMaxRadius() const + { + dgAssert(0); + return 0; + } + + const dgContactPoint* m_contacts; + dgInt32 m_count; + + static dgVector m_pruneUpDir; + static dgVector m_pruneSupportX; + +} DG_GCC_VECTOR_ALIGNMENT; + +dgVector dgCollisionContactCloud::m_pruneUpDir(dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(1.0f), dgFloat32(0.0f)); +dgVector dgCollisionContactCloud::m_pruneSupportX(dgFloat32(1.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f)); + + +dgCollisionInstance* dgWorld::CreateNull () +{ + dgUnsigned32 crc = dgCollision::dgCollisionNull_RTTI; + dgBodyCollisionList::dgTreeNode* node = dgBodyCollisionList::Find (crc); + if (!node) { + dgCollision* const collision = new (m_allocator) dgCollisionNull (m_allocator, crc); + node = dgBodyCollisionList::Insert (collision, crc); + } + return CreateInstance (node->GetInfo(), 0, dgGetIdentityMatrix()); +} + +dgCollisionInstance* dgWorld::CreateSphere(dgFloat32 radii, dgInt32 shapeID, const dgMatrix& offsetMatrix) +{ + dgUnsigned32 crc = dgCollisionSphere::CalculateSignature (radii); + dgBodyCollisionList::dgTreeNode* node = dgBodyCollisionList::Find (crc); + if (!node) { + dgCollision* const collision = new (m_allocator) dgCollisionSphere (m_allocator, crc, dgAbs(radii)); + node = dgBodyCollisionList::Insert (collision, crc); + } + return CreateInstance (node->GetInfo(), shapeID, offsetMatrix); +} + +dgCollisionInstance* dgWorld::CreateBox(dgFloat32 dx, dgFloat32 dy, dgFloat32 dz, dgInt32 shapeID, const dgMatrix& offsetMatrix) +{ + dgUnsigned32 crc = dgCollisionBox::CalculateSignature(dx, dy, dz); + dgBodyCollisionList::dgTreeNode* node = dgBodyCollisionList::Find (crc); + if (!node) { + dgCollision* const collision = new (m_allocator) dgCollisionBox (m_allocator, crc, dx, dy, dz); + node = dgBodyCollisionList::Insert (collision, crc); + } + return CreateInstance (node->GetInfo(), shapeID, offsetMatrix); +} + +dgCollisionInstance* dgWorld::CreateCapsule (dgFloat32 radio0, dgFloat32 radio1, dgFloat32 height, dgInt32 shapeID, const dgMatrix& offsetMatrix) +{ + dgUnsigned32 crc = dgCollisionCapsule::CalculateSignature(dgAbs (radio0), dgAbs (radio1), dgAbs (height) * dgFloat32 (0.5f)); + + dgBodyCollisionList::dgTreeNode* node = dgBodyCollisionList::Find (crc); + if (!node) { + dgCollision* collision = new (m_allocator) dgCollisionCapsule (m_allocator, crc, radio0, radio1, height); + node = dgBodyCollisionList::Insert (collision, crc); + } + return CreateInstance (node->GetInfo(), shapeID, offsetMatrix); +} + +dgCollisionInstance* dgWorld::CreateCylinder (dgFloat32 radio0, dgFloat32 radio1, dgFloat32 height, dgInt32 shapeID, const dgMatrix& offsetMatrix) +{ + dgUnsigned32 crc = dgCollisionCylinder::CalculateSignature(dgAbs (radio0), dgAbs (radio1), dgAbs (height) * dgFloat32 (0.5f)); + + dgBodyCollisionList::dgTreeNode* node = dgBodyCollisionList::Find (crc); + if (!node) { + dgCollision* collision = new (m_allocator) dgCollisionCylinder (m_allocator, crc, radio0, radio1, height); + node = dgBodyCollisionList::Insert (collision, crc); + } + return CreateInstance (node->GetInfo(), shapeID, offsetMatrix); +} + +dgCollisionInstance* dgWorld::CreateChamferCylinder (dgFloat32 radius, dgFloat32 height, dgInt32 shapeID, const dgMatrix& offsetMatrix) +{ + dgUnsigned32 crc = dgCollisionChamferCylinder::CalculateSignature(dgAbs (radius), dgAbs (height) * dgFloat32 (0.5f)); + + dgBodyCollisionList::dgTreeNode* node = dgBodyCollisionList::Find (crc); + if (!node) { + dgCollision* collision = new (m_allocator) dgCollisionChamferCylinder (m_allocator, crc, radius, height); + node = dgBodyCollisionList::Insert (collision, crc); + } + return CreateInstance (node->GetInfo(), shapeID, offsetMatrix); +} + +dgCollisionInstance* dgWorld::CreateCone (dgFloat32 radius, dgFloat32 height, dgInt32 shapeID, const dgMatrix& offsetMatrix) +{ + dgUnsigned32 crc = dgCollisionCone::CalculateSignature (dgAbs (radius), dgAbs (height) * dgFloat32 (0.5f)); + dgBodyCollisionList::dgTreeNode* node = dgBodyCollisionList::Find (crc); + if (!node) { + dgCollision* const collision = new (m_allocator) dgCollisionCone (m_allocator, crc, radius, height); + node = dgBodyCollisionList::Insert (collision, crc); + } + return CreateInstance (node->GetInfo(), shapeID, offsetMatrix); +} + +dgCollisionInstance* dgWorld::CreateConvexHull (dgInt32 count, const dgFloat32* const vertexArray, dgInt32 strideInBytes, dgFloat32 tolerance, dgInt32 shapeID, const dgMatrix& offsetMatrix) +{ + dgUnsigned32 crc = dgCollisionConvexHull::CalculateSignature (count, vertexArray, strideInBytes); + + dgBodyCollisionList::dgTreeNode* node = dgBodyCollisionList::Find (crc); + + if (!node) { + // shape not found create a new one and add to the cache + dgCollisionConvexHull* const collision = new (m_allocator) dgCollisionConvexHull (m_allocator, crc, count, strideInBytes, tolerance, vertexArray); + if (collision->GetConvexVertexCount()) { + node = dgBodyCollisionList::Insert (collision, crc); + } else { + //most likely the point cloud is a plane or a line + //could not make the shape destroy the shell and return NULL + //note this is the only newton shape that can return NULL; + collision->Release(); + return NULL; + } + } + + // add reference to the shape and return the collision pointer + return CreateInstance (node->GetInfo(), shapeID, offsetMatrix); +} + +dgCollisionInstance* dgWorld::CreateCompound () +{ + // compound collision are not cached + dgCollisionCompound* const collision = new (m_allocator) dgCollisionCompound (this); + dgCollisionInstance* const instance = CreateInstance (collision, 0, dgGetIdentityMatrix()); + collision->SetParent(instance); + collision->Release(); + return instance; +} + +dgCollisionInstance* dgWorld::CreateScene () +{ + dgCollisionScene* const collision = new (m_allocator) dgCollisionScene(this); + dgCollisionInstance* const instance = CreateInstance (collision, 0, dgGetIdentityMatrix()); + collision->SetParent(instance); + collision->Release(); + return instance; +} + +dgCollisionInstance* dgWorld::CreateFracturedCompound (dgMeshEffect* const solidMesh, int shapeID, int fracturePhysicsMaterialID, int pointcloudCount, const dgFloat32* const vertexCloud, int strideInBytes, int materialID, const dgMatrix& textureMatrix, + dgCollisionCompoundFractured::OnEmitFractureChunkCallBack emitFracfuredChunk, + dgCollisionCompoundFractured::OnEmitNewCompundFractureCallBack emitFracturedCompound, + dgCollisionCompoundFractured::OnReconstructFractureMainMeshCallBack reconstructMainMesh) +{ + dgCollisionCompoundFractured* const collision = new (m_allocator) dgCollisionCompoundFractured (this, solidMesh, fracturePhysicsMaterialID, pointcloudCount, vertexCloud, strideInBytes, materialID, textureMatrix, emitFracfuredChunk, emitFracturedCompound, reconstructMainMesh); + dgCollisionInstance* const instance = CreateInstance (collision, shapeID, dgGetIdentityMatrix()); + collision->SetParent(instance); + collision->Release(); + return instance; +} + +dgCollisionInstance* dgWorld::CreateMassSpringDamperSystem (dgInt32 shapeID, dgInt32 pointCount, const dgFloat32* const points, dgInt32 strideInBytes, const dgFloat32* const pointsMass, dgInt32 linksCount, const dgInt32* const links, const dgFloat32* const linksSpring, const dgFloat32* const LinksDamper) +{ + dgCollision* const collision = new (m_allocator)dgCollisionMassSpringDamperSystem(this, shapeID, pointCount, points, strideInBytes, pointsMass, linksCount, links, linksSpring, LinksDamper); + dgCollisionInstance* const instance = CreateInstance(collision, shapeID, dgGetIdentityMatrix()); + collision->Release(); + return instance; +} + +dgCollisionInstance* dgWorld::CreateDeformableSolid (dgMeshEffect* const mesh, dgInt32 shapeID) +{ + dgAssert (m_allocator == mesh->GetAllocator()); + dgCollision* const collision = new (m_allocator) dgCollisionDeformableSolidMesh (this, mesh); + dgCollisionInstance* const instance = CreateInstance (collision, shapeID, dgGetIdentityMatrix()); + collision->Release(); + return instance; +} + +dgCollisionInstance* dgWorld::CreateBVH () +{ + // collision tree are not cached + dgCollision* const collision = new (m_allocator) dgCollisionBVH (this); + dgCollisionInstance* const instance = CreateInstance (collision, 0, dgGetIdentityMatrix()); + collision->Release(); + return instance; +} + +dgCollisionInstance* dgWorld::CreateStaticUserMesh (const dgVector& boxP0, const dgVector& boxP1, const dgUserMeshCreation& data) +{ + dgCollision* const collision = new (m_allocator) dgCollisionUserMesh(this, boxP0, boxP1, data); + dgCollisionInstance* const instance = CreateInstance (collision, 0, dgGetIdentityMatrix()); + collision->Release(); + return instance; + +} + +dgCollisionInstance* dgWorld::CreateHeightField( + dgInt32 width, dgInt32 height, dgInt32 contructionMode, dgInt32 elevationDataType, + const void* const elevationMap, const dgInt8* const atributeMap, + dgFloat32 verticalScale, dgFloat32 horizontalScale_x, dgFloat32 horizontalScale_z) +{ + dgCollision* const collision = new (m_allocator) dgCollisionHeightField (this, width, height, contructionMode, elevationMap, + elevationDataType ? dgCollisionHeightField::m_unsigned16Bit : dgCollisionHeightField::m_float32Bit, + verticalScale, atributeMap, horizontalScale_x, horizontalScale_z); + dgCollisionInstance* const instance = CreateInstance (collision, 0, dgGetIdentityMatrix()); + collision->Release(); + return instance; +} + +dgCollisionInstance* dgWorld::CreateInstance (const dgCollision* const child, dgInt32 shapeID, const dgMatrix& offsetMatrix) +{ + dgAssert (dgAbs (offsetMatrix[0].DotProduct(offsetMatrix[0]).GetScalar() - dgFloat32 (1.0f)) < dgFloat32 (1.0e-5f)); + dgAssert (dgAbs (offsetMatrix[1].DotProduct(offsetMatrix[1]).GetScalar() - dgFloat32 (1.0f)) < dgFloat32 (1.0e-5f)); + dgAssert (dgAbs (offsetMatrix[2].DotProduct(offsetMatrix[2]).GetScalar() - dgFloat32 (1.0f)) < dgFloat32 (1.0e-5f)); + dgAssert (dgAbs (offsetMatrix[2].DotProduct(offsetMatrix[0].CrossProduct(offsetMatrix[1])).GetScalar() - dgFloat32 (1.0f)) < dgFloat32 (1.0e-5f)); + + dgAssert (offsetMatrix[0][3] == dgFloat32 (0.0f)); + dgAssert (offsetMatrix[1][3] == dgFloat32 (0.0f)); + dgAssert (offsetMatrix[2][3] == dgFloat32 (0.0f)); + dgAssert (offsetMatrix[3][3] == dgFloat32 (1.0f)); + dgCollisionInstance* const instance = new (m_allocator) dgCollisionInstance (this, child, shapeID, offsetMatrix); + return instance; +} + +void dgWorld::SerializeCollision(dgCollisionInstance* const shape, dgSerialize serialization, void* const userData) const +{ + dgSerializeMarker(serialization, userData); + shape->Serialize(serialization, userData); +} + +dgCollisionInstance* dgWorld::CreateCollisionFromSerialization (dgDeserialize deserialization, void* const userData) +{ + dgInt32 revision = dgDeserializeMarker (deserialization, userData); + dgCollisionInstance* const instance = new (m_allocator) dgCollisionInstance (this, deserialization, userData, revision); + return instance; +} + +dgContactMaterial* dgWorld::GetMaterial (dgUnsigned32 bodyGroupId0, dgUnsigned32 bodyGroupId1) const +{ + if (bodyGroupId0 > bodyGroupId1) { + dgSwap (bodyGroupId0, bodyGroupId1); + } + + dgUnsigned32 key = (bodyGroupId1 << 16) + bodyGroupId0; + dgBodyMaterialList::dgTreeNode *const node = dgBodyMaterialList::Find (key); + + return node ? &node->GetInfo() : NULL; +} + +dgContactMaterial* dgWorld::GetFirstMaterial () const +{ + dgBodyMaterialList::dgTreeNode *const node = dgBodyMaterialList::Minimum(); + dgAssert (node); + return &node->GetInfo(); +} + +dgContactMaterial* dgWorld::GetNextMaterial (dgContactMaterial* material) const +{ + dgBodyMaterialList::dgTreeNode *const thisNode = dgBodyMaterialList::GetNodeFromInfo (*material); + dgAssert (thisNode); + dgBodyMaterialList::dgTreeNode *const node = (dgBodyMaterialList::dgTreeNode *)thisNode->Next(); + if (node) { + return &node->GetInfo(); + } + + return NULL; +} + +dgUnsigned32 dgWorld::GetDefualtBodyGroupID() const +{ + return m_defualtBodyGroupID; +} + +dgUnsigned32 dgWorld::CreateBodyGroupID() +{ + dgContactMaterial pairMaterial; + + pairMaterial.m_aabbOverlap = NULL; + pairMaterial.m_processContactPoint = NULL; + pairMaterial.m_compoundAABBOverlap = NULL; + + dgUnsigned32 newId = m_bodyGroupID; + m_bodyGroupID += 1; + for (dgUnsigned32 i = 0; i < m_bodyGroupID ; i ++) { + dgUnsigned32 key = (newId << 16) + i; + + dgBodyMaterialList::Insert(pairMaterial, key); + } + + return newId; +} + +void dgWorld::ReleaseCollision(const dgCollision* const collision) +{ + dgInt32 ref = collision->Release(); + if (ref == 1) { + dgBodyCollisionList::dgTreeNode* const node = dgBodyCollisionList::Find (collision->m_signature); + if (node) { + dgAssert (node->GetInfo() == collision); + collision->Release(); + dgBodyCollisionList::Remove (node); + } + } +} + +// ******************************************************************************** +// +// separate collision system +// +// ******************************************************************************** +dgInt32 dgWorld::ClosestPoint (dgTriplex& point, const dgCollisionInstance* const collision, const dgMatrix& matrix, dgTriplex& contact, dgTriplex& normal, dgInt32 threadIndex) +{ + dgTriplex contactA; + dgMatrix pointMatrix (dgGetIdentityMatrix()); + + contact = point; + pointMatrix.m_posit.m_x = point.m_x; + pointMatrix.m_posit.m_y = point.m_y; + pointMatrix.m_posit.m_z = point.m_z; + return ClosestPoint(collision, matrix, m_pointCollision, pointMatrix, contact, contactA, normal, threadIndex); +} + +dgInt32 dgWorld::ClosestPoint(const dgCollisionInstance* const collisionSrcA, const dgMatrix& matrixA, + const dgCollisionInstance* const collisionSrcB, const dgMatrix& matrixB, + dgTriplex& contactA, dgTriplex& contactB, dgTriplex& normalAB,dgInt32 threadIndex) +{ + dgKinematicBody collideBodyA; + dgKinematicBody collideBodyB; + dgContactPoint contacts[DG_MAX_CONTATCS]; + + dgCollisionInstance collisionA(*collisionSrcA, collisionSrcA->GetChildShape()); + dgCollisionInstance collisionB(*collisionSrcB, collisionSrcB->GetChildShape()); + + collideBodyA.m_matrix = matrixA; + collideBodyA.m_collision = &collisionA; + collisionA.SetGlobalMatrix(collisionA.GetLocalMatrix() * matrixA); + + collideBodyB.m_matrix = matrixB; + collideBodyB.m_collision = &collisionB; + collisionB.SetGlobalMatrix (collisionB.GetLocalMatrix() * matrixB); + + dgContactMaterial material; + material.m_penetration = dgFloat32 (0.0f); + dgContact contactJoint (this, &material, &collideBodyB, &collideBodyA); + dgCollisionParamProxy proxy(&contactJoint, contacts, threadIndex, false, false); + + proxy.m_body0 = &collideBodyA; + proxy.m_instance0 = collideBodyA.m_collision; + proxy.m_body1 = &collideBodyB; + proxy.m_instance1 = collideBodyB.m_collision; + proxy.m_timestep = dgFloat32 (0.0f); + proxy.m_skinThickness = dgFloat32 (0.0f); + proxy.m_maxContacts = 16; + + dgInt32 flag = 0; + if (collisionA.IsType (dgCollision::dgCollisionCompound_RTTI)) { + flag = ClosestCompoundPoint (proxy); + } else if (collisionB.IsType (dgCollision::dgCollisionCompound_RTTI)) { + dgSwap (proxy.m_body0, proxy.m_body1); + dgSwap (proxy.m_instance0, proxy.m_instance1); + flag = ClosestCompoundPoint (proxy); + normalAB.m_x *= dgFloat32 (-1.0f); + normalAB.m_y *= dgFloat32 (-1.0f); + normalAB.m_z *= dgFloat32 (-1.0f); + } else if (collisionA.IsType (dgCollision::dgCollisionConvexShape_RTTI) && collisionB.IsType (dgCollision::dgCollisionConvexShape_RTTI)) { + flag = ClosestPoint (proxy); + } + + if (flag) { + contactA.m_x = contacts[0].m_point.m_x; + contactA.m_y = contacts[0].m_point.m_y; + contactA.m_z = contacts[0].m_point.m_z; + contactB.m_x = contacts[1].m_point.m_x; + contactB.m_y = contacts[1].m_point.m_y; + contactB.m_z = contacts[1].m_point.m_z; + normalAB.m_x = contacts[0].m_normal.m_x; + normalAB.m_y = contacts[0].m_normal.m_y; + normalAB.m_z = contacts[0].m_normal.m_z; + } + return flag; +} + + +bool dgWorld::IntersectionTest (const dgCollisionInstance* const collisionSrcA, const dgMatrix& matrixA, + const dgCollisionInstance* const collisionSrcB, const dgMatrix& matrixB, + dgInt32 threadIndex) +{ + dgKinematicBody collideBodyA; + dgKinematicBody collideBodyB; + dgCollisionInstance collisionA(*collisionSrcA, collisionSrcA->GetChildShape()); + dgCollisionInstance collisionB(*collisionSrcB, collisionSrcB->GetChildShape()); + + collideBodyA.m_world = this; + collideBodyA.SetContinueCollisionMode(false); + collideBodyA.m_matrix = matrixA; + collideBodyA.m_collision = &collisionA; + collideBodyA.UpdateCollisionMatrix(dgFloat32 (0.0f), 0); + + collideBodyB.m_world = this; + collideBodyB.SetContinueCollisionMode(false); + collideBodyB.m_matrix = matrixB; + collideBodyB.m_collision = &collisionB; + collideBodyB.UpdateCollisionMatrix(dgFloat32 (0.0f), 0); + + dgContactMaterial material; + material.m_penetration = dgFloat32 (0.0f); + dgContact contactJoint (this, &material, &collideBodyB, &collideBodyA); + + dgBroadPhase::dgPair pair; + pair.m_contactCount = 0; + pair.m_contact = &contactJoint; + pair.m_contactBuffer = NULL; + pair.m_timestep = dgFloat32 (0.0f); + pair.m_cacheIsValid = 0; + CalculateContacts (&pair, threadIndex, false, true); + return (pair.m_contactCount == -1) ? true : false; +} + +dgInt32 dgWorld::ClosestCompoundPoint (dgCollisionParamProxy& proxy) const +{ + dgCollisionInstance* const instance = proxy.m_instance0; + dgAssert (instance->IsType(dgCollision::dgCollisionCompound_RTTI)); + dgCollisionCompound* const collision = (dgCollisionCompound*) instance->GetChildShape(); + return collision->ClosestDistance (proxy); +} + +// ********************************************************************************** +// +// dynamics collision system +// +// ********************************************************************************** +static inline dgInt32 CompareContact (const dgContactPoint* const contactA, const dgContactPoint* 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; + } +} + +DG_INLINE dgInt32 dgWorld::PruneSupport(dgInt32 count, const dgVector& dir, const dgVector* points) const +{ + dgInt32 index = 0; + dgFloat32 maxVal = dgFloat32(-1.0e20f); + for (dgInt32 i = 0; i < count; i++) { + dgFloat32 dist = dir.DotProduct(points[i]).GetScalar(); + if (dist > maxVal) { + index = i; + maxVal = dist; + } + } + return index; +} + +dgInt32 dgWorld::Prune2dContacts(const dgMatrix& matrix, dgInt32 count, dgContactPoint* const contactArray, int maxCount, dgFloat32 tol) const +{ + class dgConvexFaceNode + { + public: + dgVector m_point2d; + dgContactPoint m_contact; + dgConvexFaceNode* m_next; + dgConvexFaceNode* m_prev; + dgInt32 m_mask; + }; + + class dgHullStackSegment + { + public: + dgVector m_p0; + dgVector m_p1; + dgConvexFaceNode* m_edgeP0; + }; + + dgVector xyMask(dgVector::m_xMask | dgVector::m_yMask); + + dgVector array[DG_MAX_CONTATCS]; + dgHullStackSegment stack[DG_MAX_CONTATCS]; + + dgConvexFaceNode convexHull[32]; + dgContactPoint buffer[32]; + + // it is a big mistake to set contact to deepest penetration because si cause unwanted pops. + // is better to present the original contact penetrations + //dgFloat32 maxPenetration = dgFloat32(0.0f); + for (dgInt32 i = 0; i < count; i++) { + array[i] = matrix.UntransformVector(contactArray[i].m_point) & xyMask; + } + + dgInt32 i0 = PruneSupport(count, dgCollisionContactCloud::m_pruneSupportX, array); + count--; + convexHull[0].m_point2d = array[i0]; + convexHull[0].m_contact = contactArray[i0]; + stack[0].m_p0 = array[i0]; + array[i0] = array[count]; + contactArray[i0] = contactArray[count]; + + dgInt32 i1 = PruneSupport(count, dgCollisionContactCloud::m_pruneSupportX.Scale(dgFloat32(-1.0f)), array); + count--; + convexHull[1].m_point2d = array[i1]; + convexHull[1].m_contact = contactArray[i1]; + stack[0].m_p1 = array[i1]; + array[i1] = array[count]; + contactArray[i1] = contactArray[count]; + + stack[0].m_edgeP0 = &convexHull[0]; + convexHull[0].m_next = &convexHull[1]; + convexHull[0].m_prev = &convexHull[1]; + convexHull[1].m_next = &convexHull[0]; + convexHull[1].m_prev = &convexHull[0]; + + stack[1].m_edgeP0 = &convexHull[1]; + stack[1].m_p0 = stack[0].m_p1; + stack[1].m_p1 = stack[0].m_p0; + + int hullCount = 2; + dgInt32 stackIndex = 2; + dgFloat32 totalArea = dgFloat32 (0.0f); + while (stackIndex && count && (hullCount < sizeof (convexHull)/sizeof (convexHull[0]))) { + stackIndex--; + + dgHullStackSegment segment(stack[stackIndex]); + dgVector p1p0((segment.m_p1 - segment.m_p0)); + dgFloat32 mag2 = p1p0.DotProduct(p1p0).GetScalar(); + if (mag2 > dgFloat32 (1.0e-5f)) { + dgVector dir(dgCollisionContactCloud::m_pruneUpDir.CrossProduct(p1p0)); + dgInt32 newIndex = PruneSupport(count, dir, array); + + dgVector edge(array[newIndex] - segment.m_p0); + dgVector normal(p1p0.CrossProduct(edge)); + if (normal.m_z > dgFloat32(1.e-4f)) { + totalArea += normal.m_z; + dgAssert(stackIndex < (DG_MAX_CONTATCS - 2)); + convexHull[hullCount].m_point2d = array[newIndex]; + convexHull[hullCount].m_contact = contactArray[newIndex]; + convexHull[hullCount].m_next = segment.m_edgeP0->m_next; + segment.m_edgeP0->m_next->m_prev = &convexHull[hullCount]; + + convexHull[hullCount].m_prev = segment.m_edgeP0; + segment.m_edgeP0->m_next = &convexHull[hullCount]; + + stack[stackIndex + 0].m_p0 = segment.m_p0; + stack[stackIndex + 0].m_p1 = array[newIndex]; + stack[stackIndex + 0].m_edgeP0 = segment.m_edgeP0; + + stack[stackIndex + 1].m_p0 = array[newIndex]; + stack[stackIndex + 1].m_p1 = segment.m_p1; + stack[stackIndex + 1].m_edgeP0 = &convexHull[hullCount]; + + hullCount++; + stackIndex += 2; + count--; + array[newIndex] = array[count]; + contactArray[newIndex] = contactArray[count]; + } + } + } + dgAssert (hullCount < sizeof (convexHull)/sizeof (convexHull[0])); + + dgUpHeap sortHeap(array, sizeof (array)); + dgConvexFaceNode* hullPoint = &convexHull[0]; + + bool hasLinearCombination = true; + while (hasLinearCombination) { + sortHeap.Flush(); + hasLinearCombination = false; + dgConvexFaceNode* ptr = hullPoint; + dgVector e0 (ptr->m_next->m_point2d - ptr->m_point2d); + do { + dgVector e1(ptr->m_next->m_next->m_point2d - ptr->m_next->m_point2d); + dgFloat32 area = e0.m_y * e1.m_x - e0.m_x * e1.m_y; + sortHeap.Push(ptr->m_next, area); + e0 = e1; + ptr->m_mask = 1; + ptr = ptr->m_next; + } while (ptr != hullPoint); + + while (sortHeap.GetCount() && (sortHeap.Value() * dgFloat32 (16.0f) < totalArea)) { + dgConvexFaceNode* const corner = sortHeap[0]; + if (corner->m_mask && corner->m_prev->m_mask) { + if (hullPoint == corner) { + hullPoint = corner->m_prev; + } + hullCount --; + hasLinearCombination = true; + corner->m_prev->m_mask = 0; + corner->m_next->m_prev = corner->m_prev; + corner->m_prev->m_next = corner->m_next; + } + sortHeap.Pop(); + } + } + + while (hullCount > maxCount) { + sortHeap.Flush(); + dgConvexFaceNode* ptr = hullPoint; + dgVector e0(ptr->m_next->m_point2d - ptr->m_point2d); + do { + dgVector e1(ptr->m_next->m_next->m_point2d - ptr->m_next->m_point2d); + dgFloat32 area = e0.m_y * e1.m_x - e0.m_x * e1.m_y; + sortHeap.Push(ptr->m_next, area); + e0 = e1; + ptr->m_mask = 1; + ptr = ptr->m_next; + } while (ptr != hullPoint); + + while (sortHeap.GetCount() && (hullCount > maxCount)) { + dgConvexFaceNode* const corner = sortHeap[0]; + if (corner->m_mask && corner->m_prev->m_mask) { + if (hullPoint == corner) { + hullPoint = corner->m_prev; + } + hullCount--; + hasLinearCombination = true; + corner->m_prev->m_mask = 0; + corner->m_next->m_prev = corner->m_prev; + corner->m_prev->m_next = corner->m_next; + } + sortHeap.Pop(); + } + } + + hullCount = 0; + dgConvexFaceNode* ptr = hullPoint; + do { + contactArray[hullCount] = ptr->m_contact; + hullCount ++; + ptr = ptr->m_next; + } while (ptr != hullPoint); + return hullCount; +} + +dgInt32 dgWorld::Prune3dContacts(const dgMatrix& matrix, dgInt32 count, dgContactPoint* const contactArray, int maxCount, dgFloat32 distTol) const +{ + dgVector array[DG_MAX_CONTATCS]; + dgFloat32 max_x = dgFloat32 (1.0e20f); + dgInt32 maxIndex = 0; + for (dgInt32 i = 0; i < count; i++) { + array[i] = matrix.UntransformVector(contactArray[i].m_point); + array[i].m_w = dgFloat32 (i); + if (array[i].m_x < max_x) { + maxIndex = i; + max_x = array[i].m_x; + } + } + dgSwap (array[0], array[maxIndex]); + + for (dgInt32 i = 2; i < count; i++) { + dgInt32 j = i; + dgVector tmp (array[i]); + for (; array[j - 1].m_x > tmp.m_x; j--) { + dgAssert(j > 0); + array[j] = array[j - 1]; + } + array[j] = tmp; + } + + dgFloat32 window = dgFloat32 (2.5e-3f); + do { + dgInt32 packContacts = 0; + window *= dgFloat32 (2.0f); + const dgFloat32 window2 = window * window; + + dgUnsigned8 mask[DG_MAX_CONTATCS]; + memset (mask, 0, count * sizeof (dgUnsigned8)); + for (dgInt32 i = 0; i < count; i++) { + if (!mask[i]) { + const dgFloat32 val = array[i].m_x + window; + for (dgInt32 j = i + 1; (j < count) && (array[j].m_x < val); j++) { + if (!mask[j]) { + dgVector dp((array[j] - array[i]) & dgVector::m_triplexMask); + dgAssert(dp.m_w == dgFloat32(0.0f)); + dgFloat32 dist2 = dp.DotProduct(dp).GetScalar(); + if (dist2 < window2) { + mask[j] = 1; + packContacts = 1; + } + } + } + } + } + + if (packContacts) { + dgInt32 j = 0; + for (dgInt32 i = 0; i < count; i++) { + if (!mask[i]) { + array[j] = array[i]; + j++; + } + } + count = j; + } + } while (count > maxCount); + + dgContactPoint tmpContact [16]; + for (dgInt32 i = 0; i < count; i++) { + dgInt32 index = dgInt32 (array[i].m_w); + tmpContact[i] = contactArray[index]; + } + + memcpy (contactArray, tmpContact, count * sizeof (dgContactPoint)); + return count; +} + +dgInt32 dgWorld::PruneContacts (dgInt32 count, dgContactPoint* const contactArray, dgFloat32 distTolerenace, dgInt32 maxCount) const +{ + dgVector origin(dgVector::m_zero); + for (dgInt32 i = 0; i < count; i++) { + origin += contactArray[i].m_point; + } + dgVector scale (dgFloat32(1.0f) / count); + origin = origin * scale; + origin.m_w = dgFloat32 (1.0f); + + dgMatrix covariance(dgGetZeroMatrix()); + for (dgInt32 i = 0; i < count; i++) { + dgVector p (contactArray[i].m_point - origin); + dgAssert (p.m_w == dgFloat32 (0.0f)); + dgMatrix matrix(p, p); + covariance.m_front += matrix.m_front; + covariance.m_up += matrix.m_up; + covariance.m_right += matrix.m_right; + } + + for (dgInt32 i = 0; i < 3; i++) { + if (dgAbs(covariance[i][i]) < (1.0e-6f)) { + for (dgInt32 j = 0; j < 3; j++) { + covariance[i][j] = dgFloat32(0.0f); + covariance[j][i] = dgFloat32(0.0f); + } + } + } + dgVector eigen (covariance.EigenVectors()); + covariance.m_posit = origin; + + if (eigen[1] < eigen[2]) { + dgSwap(eigen[1], eigen[2]); + dgSwap(covariance[1], covariance[2]); + } + if (eigen[0] < eigen[1]) { + dgSwap(eigen[0], eigen[1]); + dgSwap(covariance[0], covariance[1]); + } + if (eigen[1] < eigen[2]) { + dgSwap(eigen[1], eigen[2]); + dgSwap(covariance[1], covariance[2]); + } + + const dgFloat32 eigenValueError = dgFloat32 (1.0e-4f); + if (eigen[2] > eigenValueError) { + // 3d convex Hull + return Prune3dContacts(covariance, count, contactArray, maxCount, distTolerenace); + } else if (eigen[1] > eigenValueError) { + // is a 2d or 1d convex hull + return Prune2dContacts(covariance, count, contactArray, maxCount, distTolerenace); + } else if (eigen[0] > eigenValueError) { + // is a 1d or 1d convex hull + if (count > 2) { + dgFloat32 maxValue = dgFloat32(-1.0e10f); + dgFloat32 minValue = dgFloat32(-1.0e10f); + dgInt32 j0 = 0; + dgInt32 j1 = 0; + for (dgInt32 i = 0; i < count; i++) { + dgFloat32 dist = contactArray[i].m_point.DotProduct(covariance.m_front).GetScalar(); + if (dist > maxValue) { + j0 = i; + maxValue = dist; + } + if (-dist > minValue) { + j1 = i; + minValue = -dist; + } + } + dgContactPoint c0(contactArray[j0]); + dgContactPoint c1(contactArray[j1]); + contactArray[0] = c0; + contactArray[1] = c1; + } + return 2; + } + return 1; +} + +dgInt32 dgWorld::PruneContactsByRank(dgInt32 count, dgCollisionParamProxy& proxy, dgInt32 maxCount) const +{ + dgJacobian jt[DG_CONSTRAINT_MAX_ROWS / 3]; + dgFloat32 massMatrix[DG_CONSTRAINT_MAX_ROWS * DG_CONSTRAINT_MAX_ROWS / 9]; + + dgAssert (count > maxCount); + const dgBody* const body = proxy.m_body0; + dgVector com(body->m_globalCentreOfMass); + for (dgInt32 i = 0; i < count; i++) { + jt[i].m_linear = proxy.m_contacts[i].m_normal; + jt[i].m_angular = (proxy.m_contacts[i].m_point - com).CrossProduct(proxy.m_contacts[i].m_normal); + } + + + dgInt32 index = 0; + for (dgInt32 i = 0; i < count; i++) { + dgFloat32* const row = &massMatrix[index]; + const dgJacobian& gInvMass = jt[i]; + dgVector aii(gInvMass.m_linear * jt[i].m_linear + gInvMass.m_angular * jt[i].m_angular); + row[i] = aii.AddHorizontal().GetScalar() * dgFloat32(1.0001f); + for (dgInt32 j = i + 1; j < count; j++) { + dgVector aij(gInvMass.m_linear * jt[j].m_linear + gInvMass.m_angular * jt[j].m_angular); + dgFloat32 b = aij.AddHorizontal().GetScalar(); + row[j] = b; + massMatrix[j * count + i] = b; + } + index += count; + } + + dgFloat32 eigenValues[DG_CONSTRAINT_MAX_ROWS / 3]; + dgEigenValues(count, count, massMatrix, eigenValues); + + for (dgInt32 i = 1; i < count; i++) { + dgFloat32 value = eigenValues[i]; + dgContactPoint point (proxy.m_contacts[i]); + + dgInt32 j = i - 1; + for (; (j >= 0) && (eigenValues[j] < value); j--) { + eigenValues[j + 1] = eigenValues[j]; + proxy.m_contacts[j + 1] = proxy.m_contacts[j]; + } + eigenValues[j + 1] = value; + proxy.m_contacts[j + 1] = point; + } + + return maxCount; +} + +void dgWorld::PopulateContacts (dgBroadPhase::dgPair* const pair, dgInt32 threadIndex) +{ + dgContact* const contact = pair->m_contact; + dgBody* const body0 = contact->m_body0; + dgBody* const body1 = contact->m_body1; + dgAssert (body0 != body1); + dgAssert (body0); + dgAssert (body1); + dgAssert (contact->m_body0 == body0); + dgAssert (contact->m_body1 == body1); + + const dgContactMaterial* const material = contact->m_material; + const dgContactPoint* const contactArray = pair->m_contactBuffer; + + if (material->m_flags & dgContactMaterial::m_resetSkeletonSelfCollision) { + contact->ResetSkeletonSelftCollision(); + } + + if (material->m_flags & dgContactMaterial::m_resetSkeletonIntraCollision) { + contact->ResetSkeletonIntraCollision(); + } + + dgInt32 contactCount = pair->m_contactCount; + dgList& list = *contact; + + contact->m_timeOfImpact = pair->m_timestep; + + dgInt32 count = 0; + dgVector cachePosition [DG_MAX_CONTATCS]; + dgList::dgListNode *nodes[DG_MAX_CONTATCS]; + + for (dgList::dgListNode *contactNode = list.GetFirst(); contactNode; contactNode = contactNode->GetNext()) { + + nodes[count] = contactNode; + cachePosition[count] = contactNode->GetInfo().m_point; + count ++; + } + + const dgVector& v0 = body0->m_veloc; + const dgVector& w0 = body0->m_omega; + const dgVector& com0 = body0->m_globalCentreOfMass; + + const dgVector& v1 = body1->m_veloc; + const dgVector& w1 = body1->m_omega; + const dgVector& com1 = body1->m_globalCentreOfMass; + + dgVector controlDir0 (dgFloat32 (0.0f)); + dgVector controlDir1 (dgFloat32 (0.0f)); + dgVector controlNormal (contactArray[0].m_normal); + dgVector vel0 (v0 + w0.CrossProduct(contactArray[0].m_point - com0)); + dgVector vel1 (v1 + w1.CrossProduct(contactArray[0].m_point - com1)); + dgVector vRel (vel1 - vel0); + dgAssert (controlNormal.m_w == dgFloat32 (0.0f)); + dgVector tangDir (vRel - controlNormal * vRel.DotProduct(controlNormal)); + dgAssert (tangDir.m_w == dgFloat32 (0.0f)); + dgFloat32 diff = tangDir.DotProduct(tangDir).GetScalar(); + + dgInt32 staticMotion = 0; + if (diff <= dgFloat32 (1.0e-2f)) { + staticMotion = 1; + if (dgAbs (controlNormal.m_z) > dgFloat32 (0.577f)) { + tangDir = dgVector (-controlNormal.m_y, controlNormal.m_z, dgFloat32 (0.0f), dgFloat32 (0.0f)); + } else { + tangDir = dgVector (-controlNormal.m_y, controlNormal.m_x, dgFloat32 (0.0f), dgFloat32 (0.0f)); + } + controlDir0 = controlNormal.CrossProduct(tangDir); + dgAssert (controlDir0.m_w == dgFloat32 (0.0f)); + dgAssert (controlDir0.DotProduct(controlDir0).GetScalar() > dgFloat32 (1.0e-8f)); + controlDir0 = controlDir0.Normalize(); + controlDir1 = controlNormal.CrossProduct(controlDir0); + dgAssert (dgAbs(controlNormal.DotProduct(controlDir0.CrossProduct(controlDir1)).GetScalar() - dgFloat32 (1.0f)) < dgFloat32 (1.0e-3f)); + } + +//if (body0->m_uniqueID == 7) +//dgTrace (("frame:%d body:%d\n", GetFrameNumber(), body0->m_uniqueID)); + + dgFloat32 maxImpulse = dgFloat32 (-1.0f); + for (dgInt32 i = 0; i < contactCount; i ++) { + dgList::dgListNode* contactNode = NULL; + dgFloat32 min = dgFloat32 (1.0e20f); + dgInt32 index = -1; + for (dgInt32 j = 0; j < count; j ++) { + dgVector v (cachePosition[j] - contactArray[i].m_point); + dgAssert (v.m_w == dgFloat32 (0.0f)); + diff = v.DotProduct(v).GetScalar(); + if (diff < min) { + min = diff; + index = j; + contactNode = nodes[j]; + } + } + + if (contactNode) { + count --; + dgAssert (index != -1); + nodes[index] = nodes[count]; + cachePosition[index] = cachePosition[count]; + } else { + GlobalLock(); + contactNode = list.Append (); + GlobalUnlock(); + } + + dgContactMaterial* const contactMaterial = &contactNode->GetInfo(); + + dgAssert (dgCheckFloat(contactArray[i].m_point.m_x)); + dgAssert (dgCheckFloat(contactArray[i].m_point.m_y)); + dgAssert (dgCheckFloat(contactArray[i].m_point.m_z)); + dgAssert (contactArray[i].m_body0); + dgAssert (contactArray[i].m_body1); + dgAssert (contactArray[i].m_collision0); + dgAssert (contactArray[i].m_collision1); + dgAssert (contactArray[i].m_body0 == body0); + dgAssert (contactArray[i].m_body1 == body1); + +//if (body0->m_uniqueID == 7) +//dgTrace(("penetration(%f) point(%f %f %f)\n", +//contactArray[i].m_penetration, +//contactArray[i].m_point.m_x, contactArray[i].m_point.m_y, contactArray[i].m_point.m_z)); + + contactMaterial->m_point = contactArray[i].m_point; + contactMaterial->m_normal = contactArray[i].m_normal; + contactMaterial->m_penetration = contactArray[i].m_penetration; + contactMaterial->m_body0 = contactArray[i].m_body0; + contactMaterial->m_body1 = contactArray[i].m_body1; + contactMaterial->m_collision0 = contactArray[i].m_collision0; + contactMaterial->m_collision1 = contactArray[i].m_collision1; + contactMaterial->m_shapeId0 = contactArray[i].m_shapeId0; + contactMaterial->m_shapeId1 = contactArray[i].m_shapeId1; + contactMaterial->m_softness = material->m_softness; + contactMaterial->m_skinThickness = material->m_skinThickness; + contactMaterial->m_restitution = material->m_restitution; + contactMaterial->m_staticFriction0 = material->m_staticFriction0; + contactMaterial->m_staticFriction1 = material->m_staticFriction1; + contactMaterial->m_dynamicFriction0 = material->m_dynamicFriction0; + contactMaterial->m_dynamicFriction1 = material->m_dynamicFriction1; + + dgAssert (dgAbs(contactMaterial->m_normal.DotProduct(contactMaterial->m_normal).GetScalar() - dgFloat32 (1.0f)) < dgFloat32 (1.0e-1f)); + + contactMaterial->m_flags = dgContactMaterial::m_collisionEnable | (material->m_flags & (dgContactMaterial::m_friction0Enable | dgContactMaterial::m_friction1Enable)); + contactMaterial->m_userData = material->m_userData; + + if (staticMotion) { + if (contactMaterial->m_normal.DotProduct(controlNormal).GetScalar() > dgFloat32 (0.9995f)) { + contactMaterial->m_dir0 = controlDir0; + contactMaterial->m_dir1 = controlDir1; + } else { + if (dgAbs (contactMaterial->m_normal.m_z) > dgFloat32 (0.577f)) { + tangDir = dgVector (-contactMaterial->m_normal.m_y, contactMaterial->m_normal.m_z, dgFloat32 (0.0f), dgFloat32 (0.0f)); + } else { + tangDir = dgVector (-contactMaterial->m_normal.m_y, contactMaterial->m_normal.m_x, dgFloat32 (0.0f), dgFloat32 (0.0f)); + } + contactMaterial->m_dir0 = contactMaterial->m_normal.CrossProduct(tangDir); + dgAssert (contactMaterial->m_dir0.m_w == dgFloat32 (0.0f)); + dgAssert (contactMaterial->m_dir0.DotProduct(contactMaterial->m_dir0).GetScalar() > dgFloat32 (1.0e-8f)); + contactMaterial->m_dir0 = contactMaterial->m_dir0.Normalize(); + contactMaterial->m_dir1 = contactMaterial->m_normal.CrossProduct(contactMaterial->m_dir0); + dgAssert (dgAbs(contactMaterial->m_normal.DotProduct(contactMaterial->m_dir0.CrossProduct(contactMaterial->m_dir1)).GetScalar() - dgFloat32 (1.0f)) < dgFloat32 (1.0e-3f)); + } + } else { + dgVector veloc0 (v0 + w0.CrossProduct(contactMaterial->m_point - com0)); + dgVector veloc1 (v1 + w1.CrossProduct(contactMaterial->m_point - com1)); + dgVector relReloc (veloc1 - veloc0); + + dgAssert (contactMaterial->m_normal.m_w == dgFloat32 (0.0f)); + dgFloat32 impulse = relReloc.DotProduct(contactMaterial->m_normal).GetScalar(); + if (dgAbs (impulse) > maxImpulse) { + maxImpulse = dgAbs (impulse); + } + + dgVector tangentDir (relReloc - contactMaterial->m_normal.Scale (impulse)); + dgAssert (tangentDir.m_w == dgFloat32 (0.0f)); + diff = tangentDir.DotProduct(tangentDir).GetScalar(); + if (diff > dgFloat32 (1.0e-2f)) { + dgAssert (tangentDir.m_w == dgFloat32 (0.0f)); + //contactMaterial->m_dir0 = tangentDir.Scale (dgRsqrt (diff)); + contactMaterial->m_dir0 = tangentDir.Normalize(); + } else { + if (dgAbs (contactMaterial->m_normal.m_z) > dgFloat32 (0.577f)) { + tangentDir = dgVector (-contactMaterial->m_normal.m_y, contactMaterial->m_normal.m_z, dgFloat32 (0.0f), dgFloat32 (0.0f)); + } else { + tangentDir = dgVector (-contactMaterial->m_normal.m_y, contactMaterial->m_normal.m_x, dgFloat32 (0.0f), dgFloat32 (0.0f)); + } + contactMaterial->m_dir0 = contactMaterial->m_normal.CrossProduct(tangentDir); + dgAssert (contactMaterial->m_dir0.m_w == dgFloat32 (0.0f)); + dgAssert (contactMaterial->m_dir0.DotProduct(contactMaterial->m_dir0).GetScalar() > dgFloat32 (1.0e-8f)); + contactMaterial->m_dir0 = contactMaterial->m_dir0.Normalize(); + } + contactMaterial->m_dir1 = contactMaterial->m_normal.CrossProduct(contactMaterial->m_dir0); + dgAssert (dgAbs(contactMaterial->m_normal.DotProduct(contactMaterial->m_dir0.CrossProduct(contactMaterial->m_dir1)).GetScalar() - dgFloat32 (1.0f)) < dgFloat32 (1.0e-3f)); + } + dgAssert (contactMaterial->m_dir0.m_w == dgFloat32 (0.0f)); + dgAssert (contactMaterial->m_dir0.m_w == dgFloat32 (0.0f)); + dgAssert (contactMaterial->m_normal.m_w == dgFloat32 (0.0f)); + } + + if (count) { + GlobalLock(); + for (dgInt32 i = 0; i < count; i ++) { + list.Remove(nodes[i]); + } + GlobalUnlock(); + } + + contact->m_maxDOF = dgUnsigned32 (3 * contact->GetCount()); + if (material->m_processContactPoint) { + material->m_processContactPoint(*contact, pair->m_timestep, threadIndex); + } +} + +void dgWorld::ProcessContacts (dgBroadPhase::dgPair* const pair, dgInt32 threadIndex) +{ + dgAssert (pair->m_contact); + dgAssert (pair->m_contact->m_body0); + dgAssert (pair->m_contact->m_body1); + dgAssert (pair->m_contact->m_body0 != pair->m_contact->m_body1); + + pair->m_contact->m_positAcc = dgVector::m_zero; + pair->m_contact->m_rotationAcc = dgQuaternion(); + PopulateContacts (pair, threadIndex); +} + +void dgWorld::ConvexContacts (dgBroadPhase::dgPair* const pair, dgCollisionParamProxy& proxy) const +{ + dgContact* const constraint = pair->m_contact; + + dgBody* const otherBody = constraint->m_body1; + dgBody* const convexBody = constraint->m_body0; + + proxy.m_body1 = otherBody; + proxy.m_body0 = convexBody; + proxy.m_instance0 = proxy.m_body0->m_collision; + proxy.m_instance1 = proxy.m_body1->m_collision; + dgAssert (proxy.m_instance0->IsType (dgCollision::dgCollisionConvexShape_RTTI)); + if (proxy.m_instance1->IsType (dgCollision::dgCollisionConvexShape_RTTI)) { + dgAssert (convexBody->m_collision->IsType (dgCollision::dgCollisionConvexShape_RTTI)); + dgAssert (otherBody->m_collision->IsType (dgCollision::dgCollisionConvexShape_RTTI)); + pair->m_contactCount = CalculateConvexToConvexContacts (proxy); + } else { + dgAssert (constraint->m_body0->m_collision->IsType (dgCollision::dgCollisionConvexShape_RTTI)); + dgAssert (convexBody->m_collision->IsType (dgCollision::dgCollisionConvexShape_RTTI)); + pair->m_contactCount = CalculateConvexToNonConvexContacts (proxy); + } +} + +void dgWorld::CompoundContacts (dgBroadPhase::dgPair* const pair, dgCollisionParamProxy& proxy) const +{ + dgContact* const constraint = pair->m_contact; + pair->m_contactCount = 0; + + dgCollisionInstance* const instance = constraint->m_body0->GetCollision(); + dgCollisionCompound* const compound = (dgCollisionCompound*) instance->GetChildShape(); + dgAssert (compound->IsType(dgCollision::dgCollisionCompound_RTTI)); + compound->CalculateContacts (pair, proxy); + + proxy.m_contactJoint->m_separationDistance = dgFloat32 (0.0f); +} + +void dgWorld::SceneChildContacts (dgBroadPhase::dgPair* const pair, dgCollisionParamProxy& proxy) const +{ + dgAssert (pair->m_contact->GetBody1()->GetCollision()->IsType(dgCollision::dgCollisionScene_RTTI)); + dgContactPoint* const savedBuffer = proxy.m_contacts; + + proxy.m_maxContacts = ((DG_MAX_CONTATCS - pair->m_contactCount) > 32) ? 32 : DG_MAX_CONTATCS - pair->m_contactCount; + proxy.m_contacts = &savedBuffer[pair->m_contactCount]; + + if (proxy.m_instance1->IsType (dgCollision::dgCollisionConvexShape_RTTI)) { + pair->m_contactCount += CalculateConvexToConvexContacts (proxy); + } else { + pair->m_contactCount += CalculateConvexToNonConvexContacts (proxy); + } + + proxy.m_contacts = savedBuffer; + if (pair->m_contactCount > (DG_MAX_CONTATCS - 2 * (DG_CONSTRAINT_MAX_ROWS / 3))) { + pair->m_contactCount = PruneContacts(pair->m_contactCount, proxy.m_contacts, proxy.m_contactJoint->GetPruningTolerance(), 16); + } +} + +void dgWorld::SceneContacts (dgBroadPhase::dgPair* const pair, dgCollisionParamProxy& proxy) const +{ + dgContact* const constraint = pair->m_contact; + pair->m_contactCount = 0; + + dgBody* const sceneBody = constraint->m_body1; + dgBody* const otherBody = constraint->m_body0; + + dgCollisionInstance* const sceneInstance = sceneBody->GetCollision(); + dgCollisionInstance* const otherInstance = otherBody->GetCollision(); + dgAssert (sceneInstance->IsType(dgCollision::dgCollisionScene_RTTI)); + dgAssert (!otherInstance->IsType(dgCollision::dgCollisionScene_RTTI)); + if (otherInstance->IsType (dgCollision::dgCollisionConvexShape_RTTI)) { + proxy.m_body0 = otherBody; + proxy.m_body1 = sceneBody; + proxy.m_instance0 = otherBody->m_collision; + proxy.m_instance1 = NULL; + + dgCollisionScene* const scene = (dgCollisionScene*)sceneInstance->GetChildShape(); + scene->CollidePair (pair, proxy); + } else if (otherInstance->IsType (dgCollision::dgCollisionCompound_RTTI) & ~otherInstance->IsType (dgCollision::dgCollisionScene_RTTI)) { + proxy.m_body0 = otherBody; + proxy.m_body1 = sceneBody; + proxy.m_instance0 = NULL; + proxy.m_instance1 = NULL; + + dgCollisionScene* const scene = (dgCollisionScene*)sceneInstance->GetChildShape(); + scene->CollideCompoundPair (pair, proxy); + } else { + dgAssert (0); + } +} + + +void dgWorld::CalculateContacts (dgBroadPhase::dgPair* const pair, dgInt32 threadIndex, bool ccdMode, bool intersectionTestOnly) +{ + dgContact* const contact = pair->m_contact; + dgBody* const body0 = contact->m_body0; + dgBody* const body1 = contact->m_body1; + const dgContactMaterial* const material = contact->m_material; + dgCollisionParamProxy proxy(contact, pair->m_contactBuffer, threadIndex, ccdMode, intersectionTestOnly); + + pair->m_flipContacts = false; + proxy.m_timestep = pair->m_timestep; + proxy.m_maxContacts = DG_MAX_CONTATCS; + proxy.m_skinThickness = material->m_skinThickness; + + if (body1->m_collision->IsType(dgCollision::dgCollisionScene_RTTI)) { + dgAssert(contact->m_body1->GetInvMass().m_w == dgFloat32(0.0f)); + SceneContacts(pair, proxy); + } else if (body0->m_collision->IsType (dgCollision::dgCollisionScene_RTTI)) { + contact->SwapBodies(); + pair->m_flipContacts = -1; + dgAssert(contact->m_body1->GetInvMass().m_w == dgFloat32(0.0f)); + SceneContacts (pair, proxy); + } else if (body0->m_collision->IsType (dgCollision::dgCollisionCompound_RTTI)) { + CompoundContacts (pair, proxy); + } else if (body1->m_collision->IsType (dgCollision::dgCollisionCompound_RTTI)) { + contact->SwapBodies(); + pair->m_flipContacts = -1; + CompoundContacts (pair, proxy); + } else if (body0->m_collision->IsType (dgCollision::dgCollisionConvexShape_RTTI)) { + ConvexContacts (pair, proxy); + } else if (body1->m_collision->IsType (dgCollision::dgCollisionConvexShape_RTTI)) { + contact->SwapBodies(); + pair->m_flipContacts = -1; + ConvexContacts (pair, proxy); + } + + if (pair->m_contactCount > 1) { + pair->m_contactCount = PruneContacts (pair->m_contactCount, pair->m_contactBuffer, contact->GetPruningTolerance(), 16); + } + pair->m_timestep = proxy.m_timestep; +} + +dgFloat32 dgWorld::CalculateTimeToImpact (dgContact* const contact, dgFloat32 timestep, dgInt32 threadIndex, dgVector& p, dgVector& q, dgVector& normal, dgFloat32 dist) const +{ + dgBroadPhase::dgPair pair; + + dgInt32 isActive = contact->m_isActive; + dgInt32 contactCount = contact->m_maxDOF; + dgFloat32 separationDistance = contact->m_separationDistance; + + contact->m_maxDOF = 0; + pair.m_contact = contact; + pair.m_cacheIsValid = false; + pair.m_contactBuffer = NULL; + + dgBody* const body0 = contact->m_body0; + dgBody* const body1 = contact->m_body1; +// const dgContactMaterial* const material = contact->m_material; + dgCollisionParamProxy proxy(contact, NULL, threadIndex, true, true); + + proxy.m_maxContacts = 0; + proxy.m_timestep = timestep; +// proxy.m_skinThickness = material->m_skinThickness; + proxy.m_skinThickness = dist; + + if (body0->m_collision->IsType (dgCollision::dgCollisionScene_RTTI)) { + contact->SwapBodies(); + SceneContacts (&pair, proxy); + } else if (body1->m_collision->IsType (dgCollision::dgCollisionScene_RTTI)) { + SceneContacts (&pair, proxy); + } else if (body0->m_collision->IsType (dgCollision::dgCollisionCompound_RTTI)) { + CompoundContacts (&pair, proxy); + } else if (body1->m_collision->IsType (dgCollision::dgCollisionCompound_RTTI)) { + contact->SwapBodies(); + CompoundContacts (&pair, proxy); + } else if (body0->m_collision->IsType (dgCollision::dgCollisionConvexShape_RTTI)) { + ConvexContacts (&pair, proxy); + } else if (body1->m_collision->IsType (dgCollision::dgCollisionConvexShape_RTTI)) { + contact->SwapBodies(); + ConvexContacts (&pair, proxy); + } + + if (contact->m_body0 == body0) { + normal = proxy.m_normal; + p = proxy.m_closestPointBody0; + q = proxy.m_closestPointBody1; + } else { + contact->m_body0 = body0; + contact->m_body1 = body1; + //normal = proxy.m_normal.Scale(dgFloat32 (-1.0f)); + normal = proxy.m_normal * dgVector::m_negOne; + p = proxy.m_closestPointBody1; + q = proxy.m_closestPointBody0; + } + + contact->m_maxDOF = contactCount; + contact->m_isActive = isActive; + contact->m_separationDistance = separationDistance; + return proxy.m_timestep; +} + + +dgInt32 dgWorld::CollideContinue ( + const dgCollisionInstance* const collisionSrcA, const dgMatrix& matrixA, const dgVector& velocA, const dgVector& omegaA, + const dgCollisionInstance* const collisionSrcB, const dgMatrix& matrixB, const dgVector& velocB, const dgVector& omegaB, + dgFloat32& retTimeStep, dgTriplex* const points, dgTriplex* const normals, dgFloat32* const penetration, + dgInt64* const attibuteA, dgInt64* const attibuteB, dgInt32 maxContacts, dgInt32 threadIndex) +{ + dgKinematicBody collideBodyA; + dgKinematicBody collideBodyB; + dgCollisionInstance collisionA(*collisionSrcA, collisionSrcA->GetChildShape()); + dgCollisionInstance collisionB(*collisionSrcB, collisionSrcB->GetChildShape()); + + dgContactPoint contacts[DG_MAX_CONTATCS]; + + dgInt32 count = 0; + maxContacts = dgMin (DG_MAX_CONTATCS, maxContacts); + + collideBodyA.m_world = this; + collideBodyA.SetContinueCollisionMode(true); + collideBodyA.m_matrix = matrixA; + collideBodyA.m_collision = &collisionA; + collideBodyA.m_masterNode = NULL; + collideBodyA.m_broadPhaseNode = NULL; + collideBodyA.m_veloc = dgVector (velocA[0], velocA[1], velocA[2], dgFloat32 (0.0f)); + collideBodyA.m_omega = dgVector (omegaA[0], omegaA[1], omegaA[2], dgFloat32 (0.0f)); + collisionA.SetGlobalMatrix(collisionA.GetLocalMatrix() * matrixA); + + collideBodyB.m_world = this; + collideBodyB.SetContinueCollisionMode(true); + collideBodyB.m_matrix = matrixB; + collideBodyB.m_collision = &collisionB; + collideBodyB.m_masterNode = NULL; + collideBodyB.m_broadPhaseNode = NULL; + collideBodyB.m_veloc = dgVector (velocB[0], velocB[1], velocB[2], dgFloat32 (0.0f)); + collideBodyB.m_omega = dgVector (omegaB[0], omegaB[1], omegaB[2], dgFloat32 (0.0f)); + collisionB.SetGlobalMatrix(collisionB.GetLocalMatrix() * matrixB); + + dgContactMaterial material; + material.m_penetration = dgFloat32 (0.0f); + dgContact contactJoint (this, &material, &collideBodyB, &collideBodyA); + + dgBroadPhase::dgPair pair; + pair.m_contact = &contactJoint; + pair.m_contactBuffer = contacts; + pair.m_timestep = retTimeStep; + pair.m_contactCount = 0; + pair.m_cacheIsValid = 0; + CalculateContacts (&pair, threadIndex, true, maxContacts ? false : true); + + if (pair.m_timestep < retTimeStep) { + retTimeStep = pair.m_timestep; + } + + count = pair.m_contactCount; + if (count) { + if (count > maxContacts) { + count = PruneContacts (count, contacts, contactJoint.GetPruningTolerance(), maxContacts); + } + count = dgMin(count, maxContacts); + + if (pair.m_flipContacts) { + for (dgInt32 i = 0; i < count; i++) { + dgVector step ((collideBodyA.m_veloc - collideBodyB.m_veloc).Scale (pair.m_timestep)); + points[i].m_x = contacts[i].m_point.m_x + step.m_x; + points[i].m_y = contacts[i].m_point.m_y + step.m_y; + points[i].m_z = contacts[i].m_point.m_z + step.m_z; + normals[i].m_x = -contacts[i].m_normal.m_x; + normals[i].m_y = -contacts[i].m_normal.m_y; + normals[i].m_z = -contacts[i].m_normal.m_z; + penetration[i] = contacts[i].m_penetration; + attibuteA[i] = contacts[i].m_shapeId1; + attibuteB[i] = contacts[i].m_shapeId0; + } + } else { + for (dgInt32 i = 0; i < count; i ++) { + points[i].m_x = contacts[i].m_point.m_x; + points[i].m_y = contacts[i].m_point.m_y; + points[i].m_z = contacts[i].m_point.m_z; + normals[i].m_x = contacts[i].m_normal.m_x; + normals[i].m_y = contacts[i].m_normal.m_y; + normals[i].m_z = contacts[i].m_normal.m_z; + penetration[i] = contacts[i].m_penetration; + attibuteA[i] = contacts[i].m_shapeId0; + attibuteB[i] = contacts[i].m_shapeId1; + } + } + } + return count; +} + +dgInt32 dgWorld::Collide ( + const dgCollisionInstance* const collisionSrcA, const dgMatrix& matrixA, + const dgCollisionInstance* const collisionSrcB, const dgMatrix& matrixB, + dgTriplex* const points, dgTriplex* const normals, dgFloat32* const penetration, + dgInt64* const attibuteA, dgInt64* const attibuteB, dgInt32 maxContacts, dgInt32 threadIndex) +{ + dgKinematicBody collideBodyA; + dgKinematicBody collideBodyB; + dgCollisionInstance collisionA(*collisionSrcA, collisionSrcA->GetChildShape()); + dgCollisionInstance collisionB(*collisionSrcB, collisionSrcB->GetChildShape()); + + collisionA.SetCollisionMode(true); + collisionB.SetCollisionMode(true); + + dgContactPoint contacts[DG_MAX_CONTATCS]; + + dgInt32 count = 0; + maxContacts = dgMin (DG_MAX_CONTATCS, maxContacts); + + collideBodyA.m_world = this; + collideBodyA.SetContinueCollisionMode(false); + collideBodyA.m_matrix = matrixA; + collideBodyA.m_collision = &collisionA; + collideBodyA.UpdateCollisionMatrix(dgFloat32 (0.0f), 0); + + collideBodyB.m_world = this; + collideBodyB.SetContinueCollisionMode(false); + collideBodyB.m_matrix = matrixB; + collideBodyB.m_collision = &collisionB; + collideBodyB.UpdateCollisionMatrix(dgFloat32 (0.0f), 0); + + dgContactMaterial material; + material.m_penetration = dgFloat32 (0.0f); + + dgContact contactJoint (this, &material, &collideBodyB, &collideBodyA); +// contactJoint.SetBodies (&collideBodyA, &collideBodyB); + + dgBroadPhase::dgPair pair; + pair.m_contactCount = 0; + pair.m_contact = &contactJoint; + pair.m_contactBuffer = contacts; + pair.m_timestep = dgFloat32 (0.0f); + pair.m_cacheIsValid = 0; + CalculateContacts (&pair, threadIndex, false, false); + + count = pair.m_contactCount; + if (count > maxContacts) { + count = PruneContacts(count, contacts, contactJoint.GetPruningTolerance(), maxContacts); + } + count = dgMin(count, maxContacts); + + dgFloat32 swapContactScale = (contactJoint.GetBody0() != &collideBodyA) ? dgFloat32 (-1.0f) : dgFloat32 (1.0f); + for (dgInt32 i = 0; i < count; i ++) { + points[i].m_x = contacts[i].m_point.m_x; + points[i].m_y = contacts[i].m_point.m_y; + points[i].m_z = contacts[i].m_point.m_z; + normals[i].m_x = contacts[i].m_normal.m_x * swapContactScale; + normals[i].m_y = contacts[i].m_normal.m_y * swapContactScale; + normals[i].m_z = contacts[i].m_normal.m_z * swapContactScale; + penetration[i] = contacts[i].m_penetration; + attibuteA[i] = contacts[i].m_shapeId0; + attibuteB[i] = contacts[i].m_shapeId1; + } + + return count; +} + + + + +// ************************************************************************* +// +// +// ************************************************************************* + +dgInt32 dgWorld::ClosestPoint (dgCollisionParamProxy& proxy) const +{ + dgCollisionInstance* const collision0 = proxy.m_instance0; + dgCollisionInstance* const collision1 = proxy.m_instance1; + + if (!(collision0->GetConvexVertexCount() && collision1->GetConvexVertexCount())) { + return 0; + } + + dgAssert(collision0->GetCollisionPrimityType() < m_nullCollision); + dgAssert(collision1->GetCollisionPrimityType() < m_nullCollision); + dgAssert(proxy.m_instance1->IsType(dgCollision::dgCollisionConvexShape_RTTI)); + + dgContact* const contactJoint = proxy.m_contactJoint; + dgAssert(contactJoint); + contactJoint->m_closestDistance = dgFloat32(1.0e10f); + contactJoint->m_separationDistance = dgFloat32(0.0f); + + dgCollisionInstance instance0(*collision0, collision0->m_childShape); + dgCollisionInstance instance1(*collision1, collision1->m_childShape); + proxy.m_instance0 = &instance0; + proxy.m_instance1 = &instance1; + + dgVector origin(instance0.m_globalMatrix.m_posit & dgVector::m_triplexMask); + instance0.m_globalMatrix.m_posit = dgVector::m_wOne; + instance1.m_globalMatrix.m_posit -= origin; + + contactJoint->m_separtingVector = collision0->GetGlobalMatrix().m_up; + + dgContactSolver contactSolver(&proxy); + bool retVal = contactSolver.CalculateClosestPoints(); + if (retVal) { + proxy.m_closestPointBody0 = contactSolver.GetPoint0() + origin; + proxy.m_closestPointBody1 = contactSolver.GetPoint1() + origin; + proxy.m_normal = contactSolver.GetNormal().Scale(-1.0f); + + dgContactPoint* const contactOut = proxy.m_contacts; + contactOut[0].m_normal = proxy.m_normal; + contactOut[0].m_point = proxy.m_closestPointBody0; + + contactOut[1].m_normal = contactSolver.GetNormal(); + contactOut[1].m_point = proxy.m_closestPointBody1; + + contactJoint->m_closestDistance = (contactOut[1].m_point - contactOut[0].m_point).DotProduct(contactSolver.GetNormal()).GetScalar(); + contactJoint->m_separationDistance = dgFloat32(0.0f); + + instance0.m_material.m_userData = NULL; + instance1.m_material.m_userData = NULL; + proxy.m_instance0 = collision0; + proxy.m_instance1 = collision1; + retVal = contactJoint->m_closestDistance >= dgFloat32(0.0f); + } + return retVal ? 1 : 0; +} + +dgInt32 dgWorld::CalculateUserContacts(dgCollisionParamProxy& proxy) const +{ + dgContactMaterial::dgUserContactPoint buffer[16]; + dgContact* const contactJoint = proxy.m_contactJoint; + int count = contactJoint->m_material->m_contactGeneration(*contactJoint->m_material, *proxy.m_body0, proxy.m_instance0, *proxy.m_body1, proxy.m_instance1, buffer, sizeof (buffer) / sizeof (buffer[0]), proxy.m_threadIndex); + if (count) { + proxy.m_contactJoint->m_isActive = 1; + dgContactPoint* const contactOut = proxy.m_contacts; + for (dgInt32 i = 0; i < count; i++) { + dgAssert (buffer[i].m_normal.m_w == dgFloat32 (0.0f)); + dgAssert(dgAbs(buffer[i].m_normal.DotProduct(buffer[i].m_normal).GetScalar() - dgFloat32(1.0f)) < dgFloat32(1.0e-4f)); + contactOut[i].m_point = buffer[i].m_point; + contactOut[i].m_normal = buffer[i].m_normal; + contactOut[i].m_penetration = buffer[i].m_penetration; + contactOut[i].m_shapeId0 = buffer[i].m_shapeId0; + contactOut[i].m_shapeId1 = buffer[i].m_shapeId1; + contactOut[i].m_body0 = proxy.m_body0; + contactOut[i].m_body1 = proxy.m_body1; + contactOut[i].m_collision0 = proxy.m_instance0; + contactOut[i].m_collision1 = proxy.m_instance1; + } + } + return count; +} + + +dgInt32 dgWorld::CalculateConvexToConvexContacts(dgCollisionParamProxy& proxy) const +{ + dgInt32 count = 0; + dgContact* const contactJoint = proxy.m_contactJoint; + dgAssert(contactJoint); + + dgCollisionInstance* const collision0 = proxy.m_instance0; + dgCollisionInstance* const collision1 = proxy.m_instance1; + dgAssert(collision0->IsType(dgCollision::dgCollisionConvexShape_RTTI)); + dgAssert(collision1->IsType(dgCollision::dgCollisionConvexShape_RTTI)); + contactJoint->m_closestDistance = dgFloat32(1.0e10f); + contactJoint->m_separationDistance = dgFloat32(0.0f); + + if (!(collision0->GetConvexVertexCount() && collision1->GetConvexVertexCount())) { + return count; + } + + dgAssert(collision0->GetCollisionPrimityType() != m_nullCollision); + dgAssert(collision1->GetCollisionPrimityType() != m_nullCollision); + dgAssert(proxy.m_instance1->IsType(dgCollision::dgCollisionConvexShape_RTTI)); + + if (!contactJoint->m_material->m_contactGeneration) { + dgCollisionInstance instance0(*collision0, collision0->m_childShape); + dgCollisionInstance instance1(*collision1, collision1->m_childShape); + + proxy.m_instance0 = &instance0; + proxy.m_instance1 = &instance1; + + dgVector origin(instance0.m_globalMatrix.m_posit & dgVector::m_triplexMask); + instance0.m_globalMatrix.m_posit = dgVector::m_wOne; + instance1.m_globalMatrix.m_posit -= origin; + + if (instance0.GetCollisionPrimityType() == instance1.GetCollisionPrimityType()) { + switch (instance0.GetCollisionPrimityType()) + { + case m_sphereCollision: + { + dgVector diff(instance1.GetGlobalMatrix().m_posit - instance0.GetGlobalMatrix().m_posit); + dgFloat32 mag2 = diff.DotProduct(diff).GetScalar(); + if (mag2 < dgFloat32(1.0e-6f)) { + dgMatrix tmp(instance1.GetGlobalMatrix()); + tmp.m_posit.m_x += dgFloat32(1.0e-3f); + instance1.SetGlobalMatrix(tmp); + } + break; + } + case m_capsuleCollision: + { + dgMatrix diff(instance1.GetGlobalMatrix() * instance0.GetGlobalMatrix().Inverse()); + if (dgAbs(diff[0][0]) > dgFloat32(0.9999f)) { + if (dgAbs(diff.m_posit.m_x) < dgFloat32(1.0e-3f)) { + diff.m_posit.m_y += dgFloat32(1.0e-3f); + instance1.SetGlobalMatrix(diff * instance0.GetGlobalMatrix()); + } + } + break; + } + case m_chamferCylinderCollision: + { + dgMatrix diff(instance1.GetGlobalMatrix() * instance0.GetGlobalMatrix().Inverse()); + if (dgAbs(diff[0][0]) > dgFloat32(0.9999f)) { + if (dgAbs(diff.m_posit.m_x) < dgFloat32(1.0e-3f)) { + diff.m_posit.m_x += dgFloat32(1.0e-3f); + instance1.SetGlobalMatrix(diff * instance0.GetGlobalMatrix()); + } + } + break; + } + default:; + } + } + if (contactJoint->m_isNewContact) { + contactJoint->m_isNewContact = false; + dgVector v((proxy.m_instance0->m_globalMatrix.m_posit - proxy.m_instance1->m_globalMatrix.m_posit) & dgVector::m_triplexMask); + dgFloat32 mag2 = v.DotProduct(v).m_x; + if (mag2 > dgFloat32(0.0f)) { + contactJoint->m_separtingVector = v.Scale(dgRsqrt(mag2)); + } else { + contactJoint->m_separtingVector = proxy.m_instance0->m_globalMatrix.m_up; + } + } + + dgContactSolver contactSolver(&proxy); + if (proxy.m_continueCollision) { + count = contactSolver.CalculateConvexCastContacts(); + } else { + count = contactSolver.CalculateConvexToConvexContacts(); + } + + proxy.m_closestPointBody0 += origin; + proxy.m_closestPointBody1 += origin; + dgContactPoint* const contactOut = proxy.m_contacts; + for (dgInt32 i = 0; i < count; i++) { + contactOut[i].m_point += origin; + contactOut[i].m_body0 = proxy.m_body0; + contactOut[i].m_body1 = proxy.m_body1; + contactOut[i].m_collision0 = collision0; + contactOut[i].m_collision1 = collision1; + contactOut[i].m_shapeId0 = collision0->GetUserDataID(); + contactOut[i].m_shapeId1 = collision1->GetUserDataID(); + } + + instance0.m_material.m_userData = NULL; + instance1.m_material.m_userData = NULL; + proxy.m_instance0 = collision0; + proxy.m_instance1 = collision1; + + } else { + count = CalculateUserContacts(proxy); + dgContactPoint* const contactOut = proxy.m_contacts; + for (dgInt32 i = 0; i < count; i++) { + contactOut[i].m_body0 = proxy.m_body0; + contactOut[i].m_body1 = proxy.m_body1; + contactOut[i].m_collision0 = collision0; + contactOut[i].m_collision1 = collision1; + contactOut[i].m_shapeId0 = collision0->GetUserDataID(); + contactOut[i].m_shapeId1 = collision1->GetUserDataID(); + } + } + + return count; +} + +dgInt32 dgWorld::CalculateConvexToNonConvexContacts(dgCollisionParamProxy& proxy) const +{ + dgInt32 count = 0; + dgContact* const contactJoint = proxy.m_contactJoint; + dgAssert(contactJoint); + + dgCollisionInstance* const collision0 = proxy.m_instance0; + dgCollisionInstance* const collision1 = proxy.m_instance1; + dgAssert(collision1->IsType(dgCollision::dgCollisionMesh_RTTI)); + dgAssert(collision0->IsType(dgCollision::dgCollisionConvexShape_RTTI)); + contactJoint->m_closestDistance = dgFloat32(1.0e10f); + if (!collision0->GetConvexVertexCount()) { + return count; + } + + dgFloat32 separationDistance = dgFloat32 (0.0f); + if (!contactJoint->m_material->m_contactGeneration) { + dgCollisionInstance instance0(*collision0, collision0->m_childShape); + dgCollisionInstance instance1(*collision1, collision1->m_childShape); + proxy.m_instance0 = &instance0; + proxy.m_instance1 = &instance1; + + dgVector origin(instance0.m_globalMatrix.m_posit & dgVector::m_triplexMask); + instance0.m_globalMatrix.m_posit = dgVector::m_wOne; + instance1.m_globalMatrix.m_posit -= origin; + + dgAssert(proxy.m_timestep <= dgFloat32(2.0f)); + dgAssert(proxy.m_timestep >= dgFloat32(0.0f)); + + dgPolygonMeshDesc data(proxy, NULL); + if (proxy.m_continueCollision) { + data.m_doContinuesCollisionTest = true; + + const dgVector& hullVeloc = data.m_objBody->m_veloc; + const dgVector& hullOmega = data.m_objBody->m_omega; + + dgFloat32 baseLinearSpeed = dgSqrt(hullVeloc.DotProduct(hullVeloc).GetScalar()); + if (baseLinearSpeed > dgFloat32(1.0e-6f)) { + const dgFloat32 minRadius = instance0.GetBoxMinRadius(); + const dgFloat32 maxRadius = instance0.GetBoxMaxRadius(); + dgFloat32 maxAngularSpeed = dgSqrt(hullOmega.DotProduct(hullOmega).GetScalar()); + dgFloat32 angularSpeedBound = maxAngularSpeed * (maxRadius - minRadius); + + dgFloat32 upperBoundSpeed = baseLinearSpeed + dgSqrt(angularSpeedBound); + dgVector upperBoundVeloc(hullVeloc.Scale(proxy.m_timestep * upperBoundSpeed / baseLinearSpeed)); + data.SetDistanceTravel(upperBoundVeloc); + } + } + + dgCollisionMesh* const polysoup = (dgCollisionMesh *)data.m_polySoupInstance->GetChildShape(); + polysoup->GetCollidingFaces(&data); + + if (data.m_faceCount) { + proxy.m_polyMeshData = &data; + + if (proxy.m_continueCollision) { + count = CalculateConvexToNonConvexContactsContinue(proxy); + } else { + count = CalculatePolySoupToHullContactsDescrete(proxy); + } + + if (count > 0) { + proxy.m_contactJoint->m_isActive = 1; + } + } + + proxy.m_closestPointBody0 += origin; + proxy.m_closestPointBody1 += origin; + separationDistance = data.GetSeparetionDistance(); + dgContactPoint* const contactOut = proxy.m_contacts; + for (dgInt32 i = 0; i < count; i++) { + contactOut[i].m_point += origin; + contactOut[i].m_body0 = proxy.m_body0; + contactOut[i].m_body1 = proxy.m_body1; + contactOut[i].m_collision0 = collision0; + contactOut[i].m_collision1 = collision1; + contactOut[i].m_shapeId0 = collision0->GetUserDataID(); + //contactOut[i].m_shapeId1 = collision1->GetUserDataID(); + } + + instance0.m_material.m_userData = NULL; + instance1.m_material.m_userData = NULL; + proxy.m_instance0 = collision0; + proxy.m_instance1 = collision1; + } else { + count = CalculateUserContacts(proxy); + + dgContactPoint* const contactOut = proxy.m_contacts; + for (dgInt32 i = 0; i < count; i++) { + contactOut[i].m_body0 = proxy.m_body0; + contactOut[i].m_body1 = proxy.m_body1; + contactOut[i].m_collision0 = collision0; + contactOut[i].m_collision1 = collision1; + contactOut[i].m_shapeId0 = collision0->GetUserDataID(); + //contactOut[i].m_shapeId1 = collision1->GetUserDataID(); + } + } + + contactJoint->m_separationDistance = separationDistance; + return count; +} + +dgInt32 dgWorld::CalculatePolySoupToHullContactsDescrete (dgCollisionParamProxy& proxy) const +{ + dgAssert (proxy.m_instance1->IsType (dgCollision::dgCollisionMesh_RTTI)); + dgAssert (proxy.m_instance0->IsType (dgCollision::dgCollisionConvexShape_RTTI)); + + dgCollisionInstance* const polySoupInstance = proxy.m_instance1; + dgPolygonMeshDesc& data = *proxy.m_polyMeshData; + + dgAssert (data.m_faceCount); + + dgCollisionConvexPolygon polygon (m_allocator); + dgCollisionInstance polyInstance (*polySoupInstance, &polygon); + polyInstance.SetScale(dgVector (dgFloat32 (1.0f))); + polyInstance.m_localMatrix = dgGetIdentityMatrix(); + polyInstance.m_globalMatrix = dgGetIdentityMatrix(); + + proxy.m_instance1 = &polyInstance; + polygon.m_vertex = data.m_vertex; + polygon.m_stride = dgInt32 (data.m_vertexStrideInBytes / sizeof (dgFloat32)); + + dgInt32 count = 0; + dgInt32 maxContacts = proxy.m_maxContacts; + dgInt32 maxReduceLimit = maxContacts - 16; + dgInt32 countleft = maxContacts; + + const dgVector& polygonInstanceScale = polySoupInstance->GetScale(); + const dgMatrix polySoupGlobalMatrix = polySoupInstance->m_globalMatrix; + const dgMatrix polySoupGlobalAligmentMatrix = polySoupInstance->m_aligmentMatrix; + + dgMatrix polySoupScaledMatrix (polySoupGlobalAligmentMatrix[0] * polygonInstanceScale, + polySoupGlobalAligmentMatrix[1] * polygonInstanceScale, + polySoupGlobalAligmentMatrix[2] * polygonInstanceScale, + polySoupGlobalAligmentMatrix[3]); + polySoupScaledMatrix = polySoupScaledMatrix * polySoupGlobalMatrix; + + dgAssert (proxy.m_contactJoint); + dgVector separatingVector (proxy.m_instance0->m_globalMatrix.m_up); + + const dgInt32 stride = polygon.m_stride; + const dgFloat32* const vertex = polygon.m_vertex; + dgAssert (polyInstance.m_scaleType == dgCollisionInstance::m_unit); + dgFloat32 closestDist = dgFloat32 (1.0e10f); + dgContactPoint* const contactOut = proxy.m_contacts; + dgContact* const contactJoint = proxy.m_contactJoint; + dgInt32* const indexArray = (dgInt32*)data.m_faceVertexIndex; + data.SortFaceArray(); + + for (dgInt32 i = data.m_faceCount - 1; (i >= 0) && (count < 32); i --) { + dgInt32 address = data.m_faceIndexStart[i]; + const dgInt32* const localIndexArray = &indexArray[address]; + polygon.m_vertexIndex = localIndexArray; + polygon.m_count = data.m_faceIndexCount[i]; + polygon.m_paddedCount = polygon.m_count; + polygon.m_adjacentFaceEdgeNormalIndex = data.GetAdjacentFaceEdgeNormalArray (localIndexArray, polygon.m_count); + polygon.m_faceId = data.GetFaceId (localIndexArray, polygon.m_count); + polygon.m_faceClipSize = data.GetFaceSize (localIndexArray, polygon.m_count); + polygon.m_faceNormalIndex = data.GetNormalIndex (localIndexArray, polygon.m_count); + polygon.m_normal = polygon.CalculateGlobalNormal (polySoupInstance, dgVector (&vertex[polygon.m_faceNormalIndex * stride]) & dgVector::m_triplexMask); + dgAssert (polygon.m_normal.m_w == dgFloat32 (0.0f)); + for (dgInt32 j = 0; j < polygon.m_count; j++) { + polygon.m_localPoly[j] = polySoupScaledMatrix.TransformVector(dgVector(&vertex[localIndexArray[j] * stride]) & dgVector::m_triplexMask); + } + contactJoint->m_separtingVector = separatingVector; + proxy.m_maxContacts = countleft; + proxy.m_contacts = &contactOut[count]; + dgInt32 count1 = polygon.CalculateContactToConvexHullDescrete (this, polySoupInstance, proxy); + closestDist = dgMin(closestDist, contactJoint->m_closestDistance); + + if (count1 > 0) { + count += count1; + countleft -= count1; + dgAssert (countleft >= 0); + if (count >= maxReduceLimit) { + count = PruneContacts(count, contactOut, dgFloat32 (1.0e-2f), 16); + countleft = maxContacts - count; + dgAssert (countleft >= 0); + proxy.m_maxContacts = countleft; + } + } else if (count1 == -1) { + count = -1; + break; + } + } + + contactJoint->m_closestDistance = closestDist; + + if (count) { + switch (proxy.m_instance0->GetCollisionPrimityType()) + { + case m_sphereCollision: + case m_capsuleCollision: + case m_chamferCylinderCollision: + proxy.m_instance0->CalculateImplicitContacts(count, contactOut); + break; + + default: + { + bool contactsValid = true; + + for (dgInt32 i = 0; (i < count) && contactsValid; i++) { + const dgVector& normal = contactOut[i].m_normal; + for (dgInt32 j = i + 1; (j < count) && contactsValid; j++) { + const dgFloat32 project = (normal.DotProduct(contactOut[j].m_normal)).GetScalar(); + contactsValid = contactsValid && (project > dgFloat32(-0.1f)); + } + } + + if (!contactsValid) { + dgCollisionContactCloud contactCloud(GetAllocator(), count, contactOut); + dgCollisionInstance cloudInstance(*polySoupInstance, &contactCloud); + cloudInstance.m_globalMatrix = dgGetIdentityMatrix(); + cloudInstance.SetScale(dgVector::m_one); + bool saveintersectionTestOnly = proxy.m_intersectionTestOnly; + proxy.m_instance1 = &cloudInstance; + proxy.m_intersectionTestOnly = true; + dgContactSolver contactSolver(&proxy); + contactSolver.CalculateConvexToConvexContacts(); + dgVector normal(contactSolver.GetNormal() * dgVector::m_negOne); + for (dgInt32 i = 0; i < count; i++) { + contactOut[i].m_normal = normal; + } + + proxy.m_intersectionTestOnly = saveintersectionTestOnly; + cloudInstance.m_material.m_userData = NULL; + } + } + } + } + + proxy.m_contacts = contactOut; + + // restore the pointer + polyInstance.m_material.m_userData = NULL; + proxy.m_instance1 = polySoupInstance; + return count; +} + +dgInt32 dgWorld::CalculateConvexToNonConvexContactsContinue(dgCollisionParamProxy& proxy) const +{ + dgAssert(proxy.m_instance1->IsType(dgCollision::dgCollisionMesh_RTTI)); + dgAssert(proxy.m_instance0->IsType(dgCollision::dgCollisionConvexShape_RTTI)); + + dgCollisionInstance* const polySoupInstance = proxy.m_instance1; + dgPolygonMeshDesc& data = *proxy.m_polyMeshData; + + dgAssert(data.m_faceCount); + + dgCollisionConvexPolygon polygon(m_allocator); + dgCollisionInstance polyInstance(*polySoupInstance, &polygon); + polyInstance.SetScale(dgVector(dgFloat32(1.0f))); + polyInstance.m_localMatrix = dgGetIdentityMatrix(); + polyInstance.m_globalMatrix = dgGetIdentityMatrix(); + + proxy.m_instance1 = &polyInstance; + polygon.m_vertex = data.m_vertex; + polygon.m_stride = dgInt32(data.m_vertexStrideInBytes / sizeof (dgFloat32)); + + dgInt32 count = 0; + dgInt32 maxContacts = proxy.m_maxContacts; + dgInt32 maxReduceLimit = maxContacts >> 2; + dgInt32 countleft = maxContacts; + + const dgVector& polygonInstanceScale = polySoupInstance->GetScale(); + const dgMatrix polySoupGlobalMatrix = polySoupInstance->m_globalMatrix; + const dgMatrix polySoupGlobalAligmentMatrix = polySoupInstance->m_aligmentMatrix; + + dgMatrix polySoupScaledMatrix (polySoupGlobalAligmentMatrix[0] * polygonInstanceScale, + polySoupGlobalAligmentMatrix[1] * polygonInstanceScale, + polySoupGlobalAligmentMatrix[2] * polygonInstanceScale, + polySoupGlobalAligmentMatrix[3]); + polySoupScaledMatrix = polySoupScaledMatrix * polySoupGlobalMatrix; + + dgAssert (proxy.m_contactJoint); + dgVector separatingVector (proxy.m_instance0->m_globalMatrix.m_up); + + const dgInt32 stride = polygon.m_stride; + const dgFloat32* const vertex = polygon.m_vertex; + dgAssert(polyInstance.m_scaleType == dgCollisionInstance::m_unit); + + dgContactPoint* const contactOut = proxy.m_contacts; + dgContact* const contactJoint = proxy.m_contactJoint; + dgInt32* const indexArray = (dgInt32*)data.m_faceVertexIndex; + data.SortFaceArray(); + + dgVector n(dgFloat32(0.0f), dgFloat32(1.0f), dgFloat32(0.0f), dgFloat32(0.0f)); + dgVector p(dgFloat32(0.0f)); + dgVector q(dgFloat32(0.0f)); + dgFloat32 closestDist = dgFloat32(1.0e10f); + dgFloat32 minTimeStep = proxy.m_timestep; + + dgFloat32 timeNormalizer = proxy.m_timestep; + dgFloat32 epsilon = dgFloat32(-1.0e-3f) * proxy.m_timestep; + + for (dgInt32 i = 0; (i < data.m_faceCount) && (proxy.m_timestep >= (data.m_hitDistance[i] * timeNormalizer)); i++) { + dgInt32 address = data.m_faceIndexStart[i]; + const dgInt32* const localIndexArray = &indexArray[address]; + polygon.m_vertexIndex = localIndexArray; + polygon.m_count = data.m_faceIndexCount[i]; + polygon.m_paddedCount = polygon.m_count; + polygon.m_adjacentFaceEdgeNormalIndex = data.GetAdjacentFaceEdgeNormalArray(localIndexArray, polygon.m_count); + polygon.m_faceId = data.GetFaceId(localIndexArray, polygon.m_count); + polygon.m_faceClipSize = data.GetFaceSize(localIndexArray, polygon.m_count); + polygon.m_faceNormalIndex = data.GetNormalIndex(localIndexArray, polygon.m_count); + polygon.m_normal = polygon.CalculateGlobalNormal(polySoupInstance, dgVector(&vertex[polygon.m_faceNormalIndex * stride]) & dgVector::m_triplexMask); + dgAssert(polygon.m_normal.m_w == dgFloat32(0.0f)); + for (dgInt32 j = 0; j < polygon.m_count; j++) { + polygon.m_localPoly[j] = polySoupScaledMatrix.TransformVector(dgVector(&vertex[localIndexArray[j] * stride]) & dgVector::m_triplexMask); + } + contactJoint->m_separtingVector = separatingVector; + proxy.m_timestep = minTimeStep; + proxy.m_maxContacts = countleft; + proxy.m_contacts = &contactOut[count]; + + dgInt32 count1 = polygon.CalculateContactToConvexHullContinue(this, polySoupInstance, proxy); + + if (count1 > 0) { + dgFloat32 error = proxy.m_timestep - minTimeStep; + if (error < epsilon) { + count = 0; + countleft = maxContacts; + for (dgInt32 j = 0; j < count1; j++) { + contactOut[j] = proxy.m_contacts[j]; + } + } + count += count1; + countleft -= count1; + dgAssert(countleft >= 0); + if (count >= maxReduceLimit) { + count = PruneContacts(count, contactOut, dgFloat32(1.0e-2f), 16); + countleft = maxContacts - count; + dgAssert(countleft >= 0); + } + } + + closestDist = dgMin(closestDist, contactJoint->m_closestDistance); + if (proxy.m_timestep <= minTimeStep) { + minTimeStep = proxy.m_timestep; + n = proxy.m_normal; + p = proxy.m_closestPointBody0; + q = proxy.m_closestPointBody1; + } + } + + polyInstance.m_material.m_userData = NULL; + proxy.m_contacts = contactOut; + + proxy.m_normal = n; + proxy.m_closestPointBody0 = p; + proxy.m_closestPointBody1 = q; + proxy.m_timestep = minTimeStep; + return count; +} + diff --git a/thirdparty/src/newton/dgPhysics/dgPhysics.h b/thirdparty/src/newton/dgPhysics/dgPhysics.h new file mode 100644 index 000000000..30b46fb10 --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgPhysics.h @@ -0,0 +1,59 @@ +/* Copyright (c) <2003-2019> +* +* 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 __DGPHYSICS_H__ +#define __DGPHYSICS_H__ + +#include +#include "dgBody.h" +#include "dgWorld.h" +#include "dgContact.h" +#include "dgCollision.h" +#include "dgMeshEffect.h" +#include "dgConstraint.h" +#include "dgDynamicBody.h" +#include "dgWorldPlugins.h" +#include "dgCollisionBVH.h" +#include "dgContactSolver.h" +#include "dgKinematicBody.h" +#include "dgCollisionMesh.h" +#include "dgCollisionScene.h" +#include "dgBodyMasterList.h" +#include "dgUserConstraint.h" +#include "dgBallConstraint.h" +#include "dgHingeConstraint.h" +#include "dgSkeletonContainer.h" +#include "dgCollisionInstance.h" +#include "dgCollisionUserMesh.h" +#include "dgSlidingConstraint.h" +#include "dgUpVectorConstraint.h" +#include "dgCollisionConvexHull.h" +#include "dgUniversalConstraint.h" +#include "dgCorkscrewConstraint.h" +#include "dgBroadPhaseAggregate.h" +#include "dgCollisionHeightField.h" +#include "dgCollisionConvexPolygon.h" +#include "dgCollisionDeformableMesh.h" +#include "dgCollisionCompoundFractured.h" +#include "dgCollisionLumpedMassParticles.h" + +#endif + diff --git a/thirdparty/src/newton/dgPhysics/dgPhysicsStdafx.h b/thirdparty/src/newton/dgPhysics/dgPhysicsStdafx.h new file mode 100644 index 000000000..23fc3a4b9 --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgPhysicsStdafx.h @@ -0,0 +1,46 @@ +/* Copyright (c) <2003-2019> +* +* 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. +*/ + +// stdafx.h : include file for standard system include files, +// or project specific include files that are used frequently, but +// are changed infrequently +// + +#ifndef _DG_PHYSICS_STDAFX_H__ +#define _DG_PHYSICS_STDAFX_H__ + + +#ifndef WIN32_LEAN_AND_MEAN +# define WIN32_LEAN_AND_MEAN // Exclude rarely-used stuff from Windows headers +#endif + +#include + + + +//#define DG_PROFILE_PHYSICS + + + + + +#endif + diff --git a/thirdparty/src/newton/dgPhysics/dgSkeletonContainer.cpp b/thirdparty/src/newton/dgPhysics/dgSkeletonContainer.cpp new file mode 100644 index 000000000..d3c9fba7d --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgSkeletonContainer.cpp @@ -0,0 +1,1385 @@ +/* Copyright (c) <2003-2019> +* +* 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 "dgPhysicsStdafx.h" +#include "dgBody.h" +#include "dgWorld.h" +#include "dgConstraint.h" +#include "dgDynamicBody.h" +#include "dgSkeletonContainer.h" +#include "dgWorldDynamicUpdate.h" +#include "dgBilateralConstraint.h" + + + +class dgSkeletonContainer::dgNodePair +{ + public: + dgInt32 m_m0; + dgInt32 m_m1; +}; + +DG_MSC_VECTOR_ALIGNMENT +class dgSkeletonContainer::dgForcePair +{ + public: + dgSpatialVector m_joint; + dgSpatialVector m_body; +} DG_GCC_VECTOR_ALIGNMENT; + +DG_MSC_VECTOR_ALIGNMENT +class dgSkeletonContainer::dgMatriData +{ + public: + dgSpatialMatrix m_jt; + dgSpatialMatrix m_invMass; +} DG_GCC_VECTOR_ALIGNMENT; + +DG_MSC_VECTOR_ALIGNMENT +class dgSkeletonContainer::dgBodyJointMatrixDataPair +{ + public: + dgMatriData m_body; + dgMatriData m_joint; +} DG_GCC_VECTOR_ALIGNMENT; + + +class dgSkeletonContainer::dgNode +{ + public: + + DG_CLASS_ALLOCATOR(allocator) + dgNode(dgDynamicBody* const body) + :m_body (body) + ,m_joint(NULL) + ,m_parent(NULL) + ,m_child(NULL) + ,m_sibling(NULL) + ,m_index(0) + ,m_dof(0) + ,m_swapJacobianBodiesIndex(0) + { + } + + dgNode (dgBilateralConstraint* const joint, dgNode* const parent) + :m_body ((dgDynamicBody*) ((joint->GetBody0() == parent->m_body) ? joint->GetBody1() : joint->GetBody0())) + ,m_joint (joint) + ,m_parent(parent) + ,m_child(NULL) + ,m_sibling(NULL) + ,m_index(0) + ,m_dof(0) + ,m_swapJacobianBodiesIndex(joint->GetBody0() == parent->m_body) + { + dgAssert (m_parent); + dgAssert (m_body->GetInvMass().m_w != dgFloat32 (0.0f)); + if (m_parent->m_child) { + m_sibling = m_parent->m_child; + } + m_parent->m_child = this; + } + + DG_INLINE ~dgNode() + { + m_body->SetSkeleton(NULL); + + dgNode* next; + for (dgNode* ptr = m_child; ptr; ptr = next) { + next = ptr->m_sibling; + delete ptr; + } + } + + DG_INLINE void CalculateInertiaMatrix(dgSpatialMatrix* const bodyMassArray) const + { + dgSpatialMatrix& bodyMass = bodyMassArray[m_index]; + + bodyMass = dgSpatialMatrix(dgFloat32(0.0f)); + if (m_body->GetInvMass().m_w != dgFloat32(0.0f)) { + const dgFloat32 mass = m_body->GetMass().m_w; + dgMatrix inertia(m_body->CalculateInertiaMatrix()); + for (dgInt32 i = 0; i < 3; i++) { + bodyMass[i][i] = mass; + for (dgInt32 j = 0; j < 3; j++) { + bodyMass[i + 3][j + 3] = inertia[i][j]; + } + } + } + } + + DG_INLINE void GetJacobians(const dgJointInfo* const jointInfo, const dgLeftHandSide* const leftHandSide, const dgRightHandSide* const rightHandSide, dgSpatialMatrix* const jointMassArray) + { + dgAssert(m_parent); + dgAssert(jointInfo->m_joint == m_joint); + + dgSpatialMatrix& bodyJt = m_data.m_body.m_jt; + dgSpatialMatrix& jointJ = m_data.m_joint.m_jt; + dgSpatialMatrix& jointMass = jointMassArray[m_index]; + + const dgInt32 start = jointInfo->m_pairStart; + const dgSpatialVector zero (dgSpatialVector::m_zero); + if (!m_swapJacobianBodiesIndex) { + for (dgInt32 i = 0; i < m_dof; i++) { + const dgInt32 k = m_sourceJacobianIndex[i]; + const dgRightHandSide* const rhs = &rightHandSide[start + k]; + const dgLeftHandSide* const row = &leftHandSide[start + k]; + jointMass[i] = zero; + jointMass[i][i] = -rhs->m_diagDamp; + bodyJt[i] = dgSpatialVector (row->m_Jt.m_jacobianM0.m_linear * dgVector::m_negOne, row->m_Jt.m_jacobianM0.m_angular * dgVector::m_negOne); + jointJ[i] = dgSpatialVector (row->m_Jt.m_jacobianM1.m_linear * dgVector::m_negOne, row->m_Jt.m_jacobianM1.m_angular * dgVector::m_negOne); + } + } else { + for (dgInt32 i = 0; i < m_dof; i++) { + const dgInt32 k = m_sourceJacobianIndex[i]; + const dgRightHandSide* const rhs = &rightHandSide[start + k]; + const dgLeftHandSide* const row = &leftHandSide[start + k]; + jointMass[i] = zero; + jointMass[i][i] = -rhs->m_diagDamp; + bodyJt[i] = dgSpatialVector(row->m_Jt.m_jacobianM1.m_linear * dgVector::m_negOne, row->m_Jt.m_jacobianM1.m_angular * dgVector::m_negOne); + jointJ[i] = dgSpatialVector(row->m_Jt.m_jacobianM0.m_linear * dgVector::m_negOne, row->m_Jt.m_jacobianM0.m_angular * dgVector::m_negOne); + } + } + } + + dgInt32 Factorize(const dgJointInfo* const jointInfoArray, const dgLeftHandSide* const leftHandSide, const dgRightHandSide* const rightHandSide, dgSpatialMatrix* const bodyMassArray, dgSpatialMatrix* const jointMassArray) + { + CalculateInertiaMatrix(bodyMassArray); + + m_ordinals = m_ordinalInit; + dgInt32 boundedDof = 0; + if (m_joint) { + dgAssert (m_parent); + const dgJointInfo* const jointInfo = &jointInfoArray[m_joint->m_index]; + dgAssert(jointInfo->m_joint == m_joint); + + m_dof = 0; + dgInt32 count = jointInfo->m_pairCount; + const dgInt32 first = jointInfo->m_pairStart; + for (dgInt32 i = 0; i < count; i++) { + dgInt32 k = m_sourceJacobianIndex[i]; + const dgRightHandSide* const rhs = &rightHandSide[k + first]; + if ((rhs->m_lowerBoundFrictionCoefficent <= dgFloat32 (-DG_LCP_MAX_VALUE)) && (rhs->m_upperBoundFrictionCoefficent >= dgFloat32 (DG_LCP_MAX_VALUE))) { + m_dof ++; + } else { + dgSwap(m_sourceJacobianIndex[i], m_sourceJacobianIndex[count - 1]); + i--; + count--; + } + } + dgAssert (m_dof > 0); + dgAssert (m_dof <= 6); + boundedDof += jointInfo->m_pairCount - count; + GetJacobians(jointInfo, leftHandSide, rightHandSide, jointMassArray); + } + + dgSpatialMatrix& bodyInvMass = m_data.m_body.m_invMass; + const dgSpatialMatrix& bodyMass = bodyMassArray[m_index]; + if (m_body->GetInvMass().m_w != dgFloat32(0.0f)) { + for (dgNode* child = m_child; child; child = child->m_sibling) { + CalculateBodyDiagonal(child, bodyMassArray, jointMassArray); + } + bodyInvMass = bodyMass.Inverse(6); + } else { + bodyInvMass = dgSpatialMatrix(dgFloat32(0.0f)); + } + + if (m_joint) { + dgSpatialMatrix& bodyJt = m_data.m_body.m_jt; + dgAssert(m_parent); + for (dgInt32 i = 0; i < m_dof; i++) { + bodyJt[i] = bodyInvMass.VectorTimeMatrix(bodyJt[i]); + } + CalculateJointDiagonal(bodyMassArray, jointMassArray); + CalculateJacobianBlock(); + } + return boundedDof; + } + + DG_INLINE void CalculateBodyDiagonal(dgNode* const child, dgSpatialMatrix* const bodyMassArray, const dgSpatialMatrix* const jointMassArray) + { + dgAssert(child->m_joint); + + dgSpatialMatrix copy (dgFloat32(0.0f)); + const dgInt32 dof = child->m_dof; + const dgSpatialMatrix& jacobianMatrix = child->m_data.m_joint.m_jt; + const dgSpatialMatrix& childDiagonal = jointMassArray[child->m_index]; + for (dgInt32 i = 0; i < dof ; i++) { + const dgSpatialVector& jacobian = jacobianMatrix[i]; + for (dgInt32 j = 0; j < dof ; j++) { + dgAssert(dgAreEqual (dgFloat64(childDiagonal[i][j]), dgFloat64(childDiagonal[j][i]), dgFloat64(1.0e-5f))); + dgFloat64 val = childDiagonal[i][j]; + copy[j] = copy[j] + jacobian.Scale(val); + } + } + + dgSpatialMatrix& bodyMass = bodyMassArray[m_index]; + for (dgInt32 i = 0; i < dof; i++) { + const dgSpatialVector& Jacobian = copy[i]; + const dgSpatialVector& JacobianTranspose = jacobianMatrix[i]; + for (dgInt32 j = 0; j < 6; j++) { + dgFloat64 val = -Jacobian[j]; + bodyMass[j] = bodyMass[j] + JacobianTranspose.Scale(val); + } + } + } + + DG_INLINE void CalculateJointDiagonal (const dgSpatialMatrix* const bodyMassArray, dgSpatialMatrix* const jointMassArray) + { + const dgSpatialMatrix& bodyMass = bodyMassArray[m_index]; + const dgSpatialMatrix& bodyJt = m_data.m_body.m_jt; + + dgSpatialMatrix tmp; + for (dgInt32 i = 0; i < m_dof; i++) { + tmp[i] = bodyMass.VectorTimeMatrix(bodyJt[i]); + } + + dgSpatialMatrix& jointMass = jointMassArray[m_index]; + for (dgInt32 i = 0; i < m_dof; i++) { + dgFloat64 a = bodyJt[i].DotProduct(tmp[i]); + jointMass[i][i] -= a; + for (dgInt32 j = i + 1; j < m_dof; j++) { + a = - bodyJt[i].DotProduct(tmp[j]); + jointMass[i][j] = a; + jointMass[j][i] = a; + } + } + + dgSpatialMatrix& jointInvMass = m_data.m_joint.m_invMass; + jointInvMass = jointMass.Inverse(m_dof); + } + + DG_INLINE void CalculateJacobianBlock() + { + dgSpatialMatrix& jointJ = m_data.m_joint.m_jt; + + dgSpatialMatrix copy; + const dgSpatialVector zero (dgSpatialVector::m_zero); + for (dgInt32 i = 0; i < m_dof; i++) { + copy[i] = jointJ[i]; + jointJ[i] = zero; + } + + const dgSpatialMatrix& jointInvMass = m_data.m_joint.m_invMass; + for (dgInt32 i = 0; i < m_dof; i++) { + const dgSpatialVector& jacobian = copy[i]; + const dgSpatialVector& invDiagonalRow = jointInvMass[i]; + for (dgInt32 j = 0; j < m_dof; j++) { + dgFloat64 val = invDiagonalRow[j]; + jointJ[j] = jointJ[j] + jacobian.Scale(val); + } + } + } + + DG_INLINE void JointJacobianTimeMassForward (dgForcePair& force) + { + const dgSpatialMatrix& bodyJt = m_data.m_body.m_jt; + for (dgInt32 i = 0; i < m_dof; i++) { + force.m_joint[i] -= bodyJt[i].DotProduct(force.m_body); + } + } + + DG_INLINE void BodyJacobianTimeMassForward(const dgForcePair& force, dgForcePair& parentForce) const + { + const dgSpatialMatrix& jointJ = m_data.m_joint.m_jt; + for (dgInt32 i = 0; i < m_dof; i++) { + parentForce.m_body = parentForce.m_body + jointJ[i].Scale(-force.m_joint[i]); + } + } + + DG_INLINE void JointJacobianTimeSolutionBackward(dgForcePair& force, const dgForcePair& parentForce) + { + const dgSpatialMatrix& jointJ = m_data.m_joint.m_jt; + const dgSpatialVector& f = parentForce.m_body; + for (dgInt32 i = 0; i < m_dof; i++) { + force.m_joint[i] -= f.DotProduct(jointJ[i]); + } + } + + DG_INLINE void BodyJacobianTimeSolutionBackward(dgForcePair& force) + { + const dgSpatialMatrix& bodyJt = m_data.m_body.m_jt; + for (dgInt32 i = 0; i < m_dof; i++) { + force.m_body = force.m_body + bodyJt[i].Scale(-force.m_joint[i]); + } + } + + DG_INLINE void BodyDiagInvTimeSolution(dgForcePair& force) + { + const dgSpatialMatrix& bodyInvMass = m_data.m_body.m_invMass; + force.m_body = bodyInvMass.VectorTimeMatrix(force.m_body); + } + + DG_INLINE void JointDiagInvTimeSolution(dgForcePair& force) + { + const dgSpatialMatrix& jointInvMass = m_data.m_joint.m_invMass; + force.m_joint = jointInvMass.VectorTimeMatrix(force.m_joint, m_dof); + } + + DG_INLINE dgInt32 GetAuxiliaryRows(const dgJointInfo* const jointInfoArray, const dgRightHandSide* const rightHandSide) const + { + dgInt32 rowCount = 0; + if (m_joint) { + dgAssert(m_parent); + const dgJointInfo* const jointInfo = &jointInfoArray[m_joint->m_index]; + dgAssert(jointInfo->m_joint == m_joint); + dgInt32 count = jointInfo->m_pairCount; + const dgInt32 first = jointInfo->m_pairStart; + for (dgInt32 i = 0; i < count; i++) { + const dgRightHandSide* const rhs = &rightHandSide[i + first]; + if (!((rhs->m_lowerBoundFrictionCoefficent <= dgFloat32 (-DG_LCP_MAX_VALUE)) && (rhs->m_upperBoundFrictionCoefficent >= dgFloat32 (DG_LCP_MAX_VALUE)))) { + rowCount++; + } + } + } + return rowCount; + } + + dgBodyJointMatrixDataPair m_data; + dgDynamicBody* m_body; + dgBilateralConstraint* m_joint; + dgNode* m_parent; + dgNode* m_child; + dgNode* m_sibling; + union + { + dgInt64 m_ordinals; + dgInt8 m_sourceJacobianIndex[8]; + }; + dgInt16 m_index; + dgInt8 m_dof; + dgInt8 m_swapJacobianBodiesIndex; + static dgInt64 m_ordinalInit; +}; + +dgInt64 dgSkeletonContainer::dgNode::m_ordinalInit = 0x050403020100ll; + +dgSkeletonContainer::dgSkeletonContainer(dgWorld* const world, dgDynamicBody* const rootBody) + :m_world(world) + ,m_skeleton(new (world->GetAllocator()) dgNode(rootBody)) + ,m_nodesOrder(NULL) + ,m_pairs(NULL) + ,m_deltaForce(NULL) + ,m_massMatrix11(NULL) + ,m_massMatrix10(NULL) + ,m_rightHandSide(NULL) + ,m_leftHandSide(NULL) + ,m_frictionIndex(NULL) + ,m_matrixRowsIndex(NULL) + ,m_listNode(NULL) + ,m_loopingJoints(world->GetAllocator()) + ,m_auxiliaryMemoryBuffer(world->GetAllocator()) + ,m_lru(0) + ,m_blockSize(0) + ,m_nodeCount(1) + ,m_loopCount(0) + ,m_dynamicsLoopCount(0) + ,m_rowCount(0) + ,m_loopRowCount(0) + ,m_auxiliaryRowCount(0) + ,m_consideredCloseLoop(1) +{ + if (rootBody->GetInvMass().m_w != dgFloat32 (0.0f)) { + rootBody->SetSkeleton(this); + } +} + +dgSkeletonContainer::~dgSkeletonContainer() +{ + for (dgInt32 i = 0; i < m_loopCount; i++) { + dgConstraint* const joint = m_loopingJoints[i]; + joint->m_isInSkeleton = false; + } + + for (dgInt32 i = 0; i < m_nodeCount - 1; i ++) { + m_nodesOrder[i]->m_joint->m_isInSkeleton = false; + } + + dgMemoryAllocator* const allocator = m_world->GetAllocator(); + if (m_nodesOrder) { + allocator->Free(m_nodesOrder); + } + + delete m_skeleton; +} + +dgSkeletonContainer::dgNode* dgSkeletonContainer::GetRoot () const +{ + return m_skeleton; +} + +dgSkeletonContainer::dgNode* dgSkeletonContainer::GetParent (dgNode* const node) const +{ + return node->m_parent; +} + +dgDynamicBody* dgSkeletonContainer::GetBody(dgSkeletonContainer::dgNode* const node) const +{ + return node->m_body; +} + +dgBilateralConstraint* dgSkeletonContainer::GetJoint(dgSkeletonContainer::dgNode* const node) const +{ + return node->m_joint; +} + +dgSkeletonContainer::dgNode* dgSkeletonContainer::GetFirstChild(dgSkeletonContainer::dgNode* const parent) const +{ + return parent->m_child; +} + +dgSkeletonContainer::dgNode* dgSkeletonContainer::GetNextSiblingChild(dgSkeletonContainer::dgNode* const sibling) const +{ + return sibling->m_sibling; +} + +dgWorld* dgSkeletonContainer::GetWorld() const +{ + return m_world; +} + +void dgSkeletonContainer::ClearSelfCollision() +{ + m_dynamicsLoopCount = 0; +} + +void dgSkeletonContainer::AddSelfCollisionJoint(dgConstraint* const joint) +{ + m_world->GlobalLock(); + m_loopingJoints[m_loopCount + m_dynamicsLoopCount] = joint; + m_dynamicsLoopCount++; + m_world->GlobalUnlock(); +} + +void dgSkeletonContainer::SortGraph(dgNode* const root, dgInt32& index) +{ + for (dgNode* node = root->m_child; node; node = node->m_sibling) { + SortGraph(node, index); + } + + dgAssert((m_nodeCount - index - 1) >= 0); + m_nodesOrder[index] = root; + root->m_index = dgInt16(index); + index++; + dgAssert(index <= m_nodeCount); +} + +dgSkeletonContainer::dgNode* dgSkeletonContainer::FindNode(dgDynamicBody* const body) const +{ + dgInt32 stack = 1; + dgNode* stackPool[1024]; + + stackPool[0] = m_skeleton; + while (stack) { + stack--; + dgNode* const node = stackPool[stack]; + if (node->m_body == body) { + return node; + } + + for (dgNode* ptr = node->m_child; ptr; ptr = ptr->m_sibling) { + stackPool[stack] = ptr; + stack++; + dgAssert(stack < dgInt32(sizeof (stackPool) / sizeof (stackPool[0]))); + } + } + return NULL; +} + +dgSkeletonContainer::dgNode* dgSkeletonContainer::AddChild(dgBilateralConstraint* const joint, dgNode* const parent) +{ + dgAssert (m_skeleton->m_body); + dgMemoryAllocator* const allocator = m_world->GetAllocator(); + dgNode* const node = new (allocator)dgNode(joint, parent); + m_nodeCount ++; + + joint->m_isInSkeleton = true; + dgAssert (m_world->GetSentinelBody() != node->m_body); + node->m_body->SetSkeleton(this); + return node; +} + +void dgSkeletonContainer::RemoveLoopJoint(dgBilateralConstraint* const joint) +{ + for (dgInt32 i = 0; i < m_loopCount; i++) { + if (m_loopingJoints[i] == joint) { + joint->m_isInSkeleton = false; + m_loopCount--; + m_loopingJoints[i] = m_loopingJoints[m_loopCount]; + break; + } + } +} + +void dgSkeletonContainer::Finalize(dgInt32 loopJointsCount, dgBilateralConstraint** const loopJointArray) +{ + dgAssert(m_nodeCount >= 1); + + const dgDynamicBody* const rootBody = m_skeleton->m_body; + dgAssert (((rootBody->GetInvMass().m_w == dgFloat32 (0.0f)) && (m_skeleton->m_child->m_sibling == NULL)) || (m_skeleton->m_body->GetInvMass().m_w != dgFloat32 (0.0f))); + + dgMemoryAllocator* const allocator = rootBody->GetWorld()->GetAllocator(); + m_nodesOrder = (dgNode**)allocator->Malloc(m_nodeCount * sizeof (dgNode*)); + + dgInt32 index = 0; + SortGraph(m_skeleton, index); + dgAssert(index == m_nodeCount); + + if (loopJointsCount) { + for (dgInt32 i = 0; i < loopJointsCount; i++) { + dgBilateralConstraint* const joint = loopJointArray[i]; + dgAssert(joint->GetBody0()->IsRTTIType(dgBody::m_dynamicBodyRTTI)); + dgAssert(joint->GetBody1()->IsRTTIType(dgBody::m_dynamicBodyRTTI)); + dgAssert((FindNode((dgDynamicBody*)joint->GetBody0()) || FindNode((dgDynamicBody*)joint->GetBody1()))); + joint->m_isInSkeleton = true; + + m_loopingJoints[m_loopCount] = joint; + m_loopCount++; + } + } +} + +void dgSkeletonContainer::FactorizeMatrix(dgInt32 size, dgInt32 stride, dgFloat32* const matrix, dgFloat32* const diagDamp) const +{ + DG_TRACKTIME(); + bool isPsdMatrix = false; + dgFloat32* const backupMatrix = dgAlloca(dgFloat32, size * stride); + do { + { + dgInt32 srcLine = 0; + dgInt32 dstLine = 0; + for (dgInt32 i = 0; i < size; i++) { + memcpy(&backupMatrix[dstLine], &matrix[srcLine], size * sizeof(dgFloat32)); + srcLine += size; + dstLine += stride; + } + } + isPsdMatrix = dgCholeskyFactorization(size, stride, matrix); + if (!isPsdMatrix) { + dgInt32 srcLine = 0; + dgInt32 dstLine = 0; + for (dgInt32 i = 0; i < size; i++) { + memcpy(&matrix[dstLine], &backupMatrix[srcLine], size * sizeof(dgFloat32)); + diagDamp[i] *= dgFloat32(4.0f); + matrix[dstLine + i] += diagDamp[i]; + dstLine += size; + srcLine += stride; + } + } + } while (!isPsdMatrix); +} +void dgSkeletonContainer::CalculateLoopMassMatrixCoefficients(dgFloat32* const diagDamp) +{ + DG_TRACKTIME(); + const dgInt32 primaryCount = m_rowCount - m_auxiliaryRowCount; + for (dgInt32 index = 0; index < m_auxiliaryRowCount; index ++) { + const dgInt32 ii = m_matrixRowsIndex[primaryCount + index]; + const dgLeftHandSide* const row_i = &m_leftHandSide[ii]; + const dgRightHandSide* const rhs_i = &m_rightHandSide[ii]; + const dgJacobian JMinvM0(row_i->m_JMinv.m_jacobianM0); + const dgJacobian JMinvM1(row_i->m_JMinv.m_jacobianM1); + const dgVector element( + JMinvM0.m_linear * row_i->m_Jt.m_jacobianM0.m_linear + JMinvM0.m_angular * row_i->m_Jt.m_jacobianM0.m_angular + + JMinvM1.m_linear * row_i->m_Jt.m_jacobianM1.m_linear + JMinvM1.m_angular * row_i->m_Jt.m_jacobianM1.m_angular); + + // I know I am doubling the matrix regularizer, but this makes the solution more robust. + dgFloat32* const matrixRow11 = &m_massMatrix11[m_auxiliaryRowCount * index]; + dgFloat32 diagonal = element.AddHorizontal().GetScalar() + rhs_i->m_diagDamp; + matrixRow11[index] = diagonal + rhs_i->m_diagDamp; + //diagDamp[index] = matrixRow11[index] * (DG_PSD_DAMP_TOL * dgFloat32(4.0f)); + diagDamp[index] = matrixRow11[index] * dgFloat32(4.0e-3f); + + const dgInt32 m0_i = m_pairs[primaryCount + index].m_m0; + const dgInt32 m1_i = m_pairs[primaryCount + index].m_m1; + for (dgInt32 j = index + 1; j < m_auxiliaryRowCount; j++) { + const dgInt32 jj = m_matrixRowsIndex[primaryCount + j]; + const dgLeftHandSide* const row_j = &m_leftHandSide[jj]; + + const dgInt32 k = primaryCount + j; + dgVector acc(dgVector::m_zero); + const dgInt32 m0_j = m_pairs[k].m_m0; + const dgInt32 m1_j = m_pairs[k].m_m1; + bool hasEffect = false; + if (m0_i == m0_j) { + hasEffect = true; + acc += JMinvM0.m_linear * row_j->m_Jt.m_jacobianM0.m_linear + JMinvM0.m_angular * row_j->m_Jt.m_jacobianM0.m_angular; + } else if (m0_i == m1_j) { + hasEffect = true; + acc += JMinvM0.m_linear * row_j->m_Jt.m_jacobianM1.m_linear + JMinvM0.m_angular * row_j->m_Jt.m_jacobianM1.m_angular; + } + + if (m1_i == m1_j) { + hasEffect = true; + acc += JMinvM1.m_linear * row_j->m_Jt.m_jacobianM1.m_linear + JMinvM1.m_angular * row_j->m_Jt.m_jacobianM1.m_angular; + } else if (m1_i == m0_j) { + hasEffect = true; + acc += JMinvM1.m_linear * row_j->m_Jt.m_jacobianM0.m_linear + JMinvM1.m_angular * row_j->m_Jt.m_jacobianM0.m_angular; + } + + if (hasEffect) { + acc = acc.AddHorizontal(); + dgFloat32 offDiagValue = acc.GetScalar(); + matrixRow11[j] = offDiagValue; + m_massMatrix11[j * m_auxiliaryRowCount + index] = offDiagValue; + } + } + + dgFloat32* const matrixRow10 = &m_massMatrix10[primaryCount * index]; + for (dgInt32 j = 0; j < primaryCount; j++) { + const dgInt32 jj = m_matrixRowsIndex[j]; + const dgLeftHandSide* const row_j = &m_leftHandSide[jj]; + + const dgInt32 m0_j = m_pairs[j].m_m0; + const dgInt32 m1_j = m_pairs[j].m_m1; + dgVector acc(dgVector::m_zero); + bool hasEffect = false; + if (m0_i == m0_j) { + hasEffect = true; + acc += JMinvM0.m_linear * row_j->m_Jt.m_jacobianM0.m_linear + JMinvM0.m_angular * row_j->m_Jt.m_jacobianM0.m_angular; + } else if (m0_i == m1_j) { + hasEffect = true; + acc += JMinvM0.m_linear * row_j->m_Jt.m_jacobianM1.m_linear + JMinvM0.m_angular * row_j->m_Jt.m_jacobianM1.m_angular; + } + + if (m1_i == m1_j) { + hasEffect = true; + acc += JMinvM1.m_linear * row_j->m_Jt.m_jacobianM1.m_linear + JMinvM1.m_angular * row_j->m_Jt.m_jacobianM1.m_angular; + } else if (m1_i == m0_j) { + hasEffect = true; + acc += JMinvM1.m_linear * row_j->m_Jt.m_jacobianM0.m_linear + JMinvM1.m_angular * row_j->m_Jt.m_jacobianM0.m_angular; + } + + if (hasEffect) { + acc = acc.AddHorizontal(); + dgFloat32 val = acc.GetScalar(); + matrixRow10[j] = val; + } + } + } +} + +void dgSkeletonContainer::ConditionMassMatrix () const +{ + DG_TRACKTIME(); + dgForcePair* const forcePair = dgAlloca(dgForcePair, m_nodeCount); +// dgForcePair* const accelPair = dgAlloca(dgForcePair, m_nodeCount); + const dgSpatialVector zero(dgSpatialVector::m_zero); +// accelPair[m_nodeCount - 1].m_body = zero; +// accelPair[m_nodeCount - 1].m_joint = zero; + + const dgInt32 primaryCount = m_rowCount - m_auxiliaryRowCount; + for (dgInt32 i = 0; i < m_auxiliaryRowCount; i ++) { + dgInt32 entry0 = 0; + dgInt32 startjoint = m_nodeCount; + const dgFloat32* const matrixRow10 = &m_massMatrix10[i * primaryCount]; + for (dgInt32 j = 0; j < m_nodeCount - 1; j++) { + const dgNode* const node = m_nodesOrder[j]; + const dgInt32 index = node->m_index; + //accelPair[index].m_body = zero; + //dgSpatialVector& a = accelPair[index].m_joint; + forcePair[index].m_body = zero; + dgSpatialVector& a = forcePair[index].m_joint; + + const int count = node->m_dof; + for (dgInt32 k = 0; k < count; k++) { + const dgFloat32 value = matrixRow10[entry0]; + a[k] = value; + startjoint = (value == 0.0f) ? startjoint : dgMin(startjoint, index); + entry0++; + } + } + + startjoint = (startjoint == m_nodeCount) ? 0 : startjoint; + dgAssert(startjoint < m_nodeCount); + //SolveForward(forcePair, accelPair, startjoint); + forcePair[m_nodeCount - 1].m_body = zero; + forcePair[m_nodeCount - 1].m_joint = zero; + SolveForward(forcePair, forcePair, startjoint); + SolveBackward(forcePair); + + dgInt32 entry1 = 0; + dgFloat32* const deltaForcePtr = &m_deltaForce[i * primaryCount]; + for (dgInt32 j = 0; j < m_nodeCount - 1; j++) { + const dgNode* const node = m_nodesOrder[j]; + const dgInt32 index = node->m_index; + const dgSpatialVector& f = forcePair[index].m_joint; + const int count = node->m_dof; + for (dgInt32 k = 0; k < count; k++) { + deltaForcePtr[entry1] = dgFloat32(f[k]); + entry1++; + } + } + } +} + +void dgSkeletonContainer::RebuildMassMatrix(const dgFloat32* const diagDamp) const +{ + DG_TRACKTIME(); + const dgInt32 primaryCount = m_rowCount - m_auxiliaryRowCount; + dgInt16* const indexList = dgAlloca(dgInt16, primaryCount); + for (dgInt32 i = 0; i < m_auxiliaryRowCount; i ++) { + const dgFloat32* const matrixRow10 = &m_massMatrix10[i * primaryCount]; + dgFloat32* const matrixRow11 = &m_massMatrix11[i * m_auxiliaryRowCount]; + + dgInt32 indexCount = 0; + for (dgInt32 k = 0; k < primaryCount; k++) { + indexList[indexCount] = dgInt16(k); + indexCount += (matrixRow10[k] != dgFloat32(0.0f)) ? 1 : 0; + } + + for (dgInt32 j = i; j < m_auxiliaryRowCount; j++) { + dgFloat32 offDiagonal = matrixRow11[j]; + const dgFloat32* const row10 = &m_deltaForce[j * primaryCount]; + for (dgInt32 k = 0; k < indexCount; k++) { + dgInt32 index = indexList[k]; + offDiagonal += matrixRow10[index] * row10[index]; + } + matrixRow11[j] = offDiagonal; + m_massMatrix11[j * m_auxiliaryRowCount + i] = offDiagonal; + } + + matrixRow11[i] = dgMax(matrixRow11[i], diagDamp[i]); + } +} + +void dgSkeletonContainer::InitLoopMassMatrix(const dgJointInfo* const jointInfoArray) +{ + const dgInt32 primaryCount = m_rowCount - m_auxiliaryRowCount; + dgInt8* const memoryBuffer = CalculateBufferSizeInBytes(jointInfoArray); + + m_frictionIndex = (dgInt32*)memoryBuffer; + m_matrixRowsIndex = (dgInt32*)&m_frictionIndex[m_rowCount]; + m_pairs = (dgNodePair*)&m_matrixRowsIndex[m_rowCount]; + m_massMatrix11 = (dgFloat32*)&m_pairs[m_rowCount]; + m_massMatrix10 = (dgFloat32*)&m_massMatrix11[m_auxiliaryRowCount * m_auxiliaryRowCount]; + m_deltaForce = &m_massMatrix10[m_auxiliaryRowCount * primaryCount]; + + dgFloat32* const diagDamp = dgAlloca(dgFloat32, m_auxiliaryRowCount); + dgInt32* const boundRow = dgAlloca(dgInt32, m_auxiliaryRowCount); + + m_blockSize = 0; + dgInt32 primaryIndex = 0; + dgInt32 auxiliaryIndex = 0; + for (dgInt32 i = 0; i < m_nodeCount - 1; i++) { + const dgNode* const node = m_nodesOrder[i]; + const dgJointInfo* const jointInfo = &jointInfoArray[node->m_joint->m_index]; + + const dgInt32 m0 = jointInfo->m_m0; + const dgInt32 m1 = jointInfo->m_m1; + + const dgInt32 primaryDof = node->m_dof; + const dgInt32 first = jointInfo->m_pairStart; + + for (dgInt32 j = 0; j < primaryDof; j++) { + const dgInt32 index = node->m_sourceJacobianIndex[j]; + m_pairs[primaryIndex].m_m0 = m0; + m_pairs[primaryIndex].m_m1 = m1; + m_frictionIndex[primaryIndex] = 0; + m_matrixRowsIndex[primaryIndex] = first + index; + primaryIndex++; + } + + const dgInt32 auxiliaryDof = jointInfo->m_pairCount - primaryDof; + for (dgInt32 j = 0; j < auxiliaryDof; j++) { + const dgInt32 index = node->m_sourceJacobianIndex[primaryDof + j]; + const dgRightHandSide* const rhs = &m_rightHandSide[first + index]; + + m_pairs[auxiliaryIndex + primaryCount].m_m0 = m0; + m_pairs[auxiliaryIndex + primaryCount].m_m1 = m1; + m_frictionIndex[auxiliaryIndex + primaryCount] = 0; + m_matrixRowsIndex[auxiliaryIndex + primaryCount] = first + index; + const dgInt32 boundIndex = (rhs->m_lowerBoundFrictionCoefficent <= dgFloat32(-DG_LCP_MAX_VALUE)) && (rhs->m_upperBoundFrictionCoefficent >= dgFloat32(DG_LCP_MAX_VALUE)) ? 1 : 0; + boundRow[auxiliaryIndex] = boundIndex; + m_blockSize += boundIndex; + auxiliaryIndex++; + } + } + dgAssert (m_loopRowCount == (m_auxiliaryRowCount - auxiliaryIndex)); + + const dgInt32 loopCount = m_loopCount + m_dynamicsLoopCount; + for (dgInt32 j = 0; j < loopCount; j ++) { + const dgConstraint* const joint = m_loopingJoints[j]; + const dgJointInfo* const jointInfo = &jointInfoArray[joint->m_index]; + + const dgInt32 m0 = jointInfo->m_m0; + const dgInt32 m1 = jointInfo->m_m1; + const dgInt32 first = jointInfo->m_pairStart; + const dgInt32 auxiliaryDof = jointInfo->m_pairCount; + + for (dgInt32 i = 0; i < auxiliaryDof; i++) { + const dgRightHandSide* const rhs = &m_rightHandSide[first + i]; + m_pairs[auxiliaryIndex + primaryCount].m_m0 = m0; + m_pairs[auxiliaryIndex + primaryCount].m_m1 = m1; + m_frictionIndex[auxiliaryIndex + primaryCount] = (rhs->m_normalForceIndex < 0) ? 0 : rhs->m_normalForceIndex - i; + m_matrixRowsIndex[auxiliaryIndex + primaryCount] = first + i; + const dgInt32 boundIndex = (rhs->m_lowerBoundFrictionCoefficent <= dgFloat32(-DG_LCP_MAX_VALUE)) && (rhs->m_upperBoundFrictionCoefficent >= dgFloat32(DG_LCP_MAX_VALUE)) ? 1 : 0; + boundRow[auxiliaryIndex] = boundIndex; + m_blockSize += boundIndex; + auxiliaryIndex++; + } + } + + dgAssert(primaryIndex == primaryCount); + dgAssert(auxiliaryIndex == m_auxiliaryRowCount); + + for (dgInt32 i = 1; i < auxiliaryIndex; i++) { + dgInt32 j = i; + dgInt32 tmpBoundRow = boundRow[j]; + dgNodePair tmpPair(m_pairs[primaryCount + j]); + dgInt32 tmpFrictionIndex = m_frictionIndex[primaryCount + j]; + dgInt32 tmpMatrixRowsIndex = m_matrixRowsIndex[primaryCount + j]; + + for (; j && (boundRow[j - 1] < tmpBoundRow); j--) { + dgAssert(j > 0); + boundRow[j] = boundRow[j - 1]; + m_pairs[primaryCount + j] = m_pairs[primaryCount + j - 1]; + m_frictionIndex[primaryCount + j] = m_frictionIndex[primaryCount + j - 1]; + m_matrixRowsIndex[primaryCount + j] = m_matrixRowsIndex[primaryCount + j - 1]; + } + boundRow[j] = tmpBoundRow; + m_pairs[primaryCount + j] = tmpPair; + m_frictionIndex[primaryCount + j] = tmpFrictionIndex; + m_matrixRowsIndex[primaryCount + j] = tmpMatrixRowsIndex; + } + + memset(m_massMatrix10, 0, primaryCount * m_auxiliaryRowCount * sizeof(dgFloat32)); + memset(m_massMatrix11, 0, m_auxiliaryRowCount * m_auxiliaryRowCount * sizeof(dgFloat32)); + + CalculateLoopMassMatrixCoefficients(diagDamp); + ConditionMassMatrix (); + RebuildMassMatrix(diagDamp); + + if (m_blockSize) { + FactorizeMatrix(m_blockSize, m_auxiliaryRowCount, m_massMatrix11, diagDamp); + const int boundedSize = m_auxiliaryRowCount - m_blockSize; + dgFloat32* const acc = dgAlloca(dgFloat32, m_auxiliaryRowCount); + dgInt32 rowStart = 0; + for (dgInt32 i = 0; i < m_blockSize; i++) { + memset(acc, 0, boundedSize * sizeof (dgFloat32)); + const dgFloat32* const row = &m_massMatrix11[rowStart]; + for (dgInt32 j = 0; j < i; j++) { + const dgFloat32 s = row[j]; + const dgFloat32* const x = &m_massMatrix11[j * m_auxiliaryRowCount + m_blockSize]; + for (dgInt32 k = 0; k < boundedSize; k++) { + acc[k] += s * x[k]; + } + } + + dgFloat32* const x = &m_massMatrix11[rowStart + m_blockSize]; + const dgFloat32 den = -dgFloat32(1.0f) / row[i]; + for (dgInt32 j = 0; j < boundedSize; j++) { + x[j] = (x[j] + acc[j]) * den; + } + rowStart += m_auxiliaryRowCount; + } + + for (dgInt32 i = m_blockSize - 1; i >= 0; i--) { + memset(acc, 0, boundedSize * sizeof (dgFloat32)); + for (dgInt32 j = i + 1; j < m_blockSize; j++) { + const dgFloat32 s = m_massMatrix11[j * m_auxiliaryRowCount + i]; + const dgFloat32* const x = &m_massMatrix11[j * m_auxiliaryRowCount + m_blockSize]; + for (dgInt32 k = 0; k < boundedSize; k++) { + acc[k] += s * x[k]; + } + } + + dgFloat32* const x = &m_massMatrix11[i * m_auxiliaryRowCount + m_blockSize]; + const dgFloat32 den = dgFloat32(1.0f) / m_massMatrix11[i * m_auxiliaryRowCount + i]; + for (dgInt32 j = 0; j < boundedSize; j++) { + x[j] = (x[j] - acc[j]) * den; + } + } + + for (dgInt32 i = 0; i < boundedSize; i++) { + for (int j = 0; j < m_blockSize; j++) { + acc[j] = m_massMatrix11[j * m_auxiliaryRowCount + m_blockSize + i]; + } + + dgFloat32* const arow = &m_massMatrix11[(m_blockSize + i) * m_auxiliaryRowCount + m_blockSize]; + for (int j = i; j < boundedSize; j++) { + const dgFloat32* const row1 = &m_massMatrix11[(m_blockSize + j) * m_auxiliaryRowCount]; + dgFloat32 elem = row1[m_blockSize + i] + dgDotProduct(m_blockSize, acc, row1); + arow[j] = elem; + m_massMatrix11[(m_blockSize + j) * m_auxiliaryRowCount + m_blockSize + i] = elem; + } + arow[i] += diagDamp[m_blockSize + i]; + } + dgAssert (dgTestPSDmatrix(m_auxiliaryRowCount - m_blockSize, m_auxiliaryRowCount, &m_massMatrix11[m_auxiliaryRowCount * m_blockSize + m_blockSize])); + } +} + +bool dgSkeletonContainer::SanityCheck(const dgForcePair* const force, const dgForcePair* const accel) const +{ + return true; +} + +DG_INLINE void dgSkeletonContainer::SolveForward(dgForcePair* const force, const dgForcePair* const accel, dgInt32 startNode) const +{ + dgSpatialVector zero (dgSpatialVector::m_zero); + for (dgInt32 i = 0; i < startNode; i++) { + force[i].m_body = zero; + force[i].m_joint = zero; + } + for (dgInt32 i = startNode; i < m_nodeCount - 1; i++) { + dgNode* const node = m_nodesOrder[i]; + dgAssert(node->m_joint); + dgAssert(node->m_index == i); + dgForcePair& f = force[i]; + const dgForcePair& a = accel[i]; + f.m_body = a.m_body; + f.m_joint = a.m_joint; + for (dgNode* child = node->m_child; child; child = child->m_sibling) { + dgAssert(child->m_joint); + dgAssert(child->m_parent->m_index == i); + child->BodyJacobianTimeMassForward(force[child->m_index], f); + } + node->JointJacobianTimeMassForward(f); + } + + force[m_nodeCount - 1] = accel[m_nodeCount - 1]; + for (dgNode* child = m_nodesOrder[m_nodeCount - 1]->m_child; child; child = child->m_sibling) { + child->BodyJacobianTimeMassForward(force[child->m_index], force[child->m_parent->m_index]); + } + + for (dgInt32 i = startNode; i < m_nodeCount - 1; i++) { + dgNode* const node = m_nodesOrder[i]; + dgForcePair& f = force[i]; + node->BodyDiagInvTimeSolution(f); + node->JointDiagInvTimeSolution(f); + } + m_nodesOrder[m_nodeCount - 1]->BodyDiagInvTimeSolution(force[m_nodeCount - 1]); +} + +DG_INLINE void dgSkeletonContainer::SolveBackward(dgForcePair* const force) const +{ + for (dgInt32 i = m_nodeCount - 2; i >= 0; i--) { + dgNode* const node = m_nodesOrder[i]; + dgAssert(node->m_index == i); + dgForcePair& f = force[i]; + node->JointJacobianTimeSolutionBackward(f, force[node->m_parent->m_index]); + node->BodyJacobianTimeSolutionBackward(f); + } +} + +DG_INLINE void dgSkeletonContainer::CalculateForce (dgForcePair* const force, const dgForcePair* const accel) const +{ + SolveForward(force, accel); + SolveBackward(force); +} + +DG_INLINE void dgSkeletonContainer::UpdateForces (dgJointInfo* const jointInfoArray, dgJacobian* const internalForces, const dgForcePair* const force) const +{ + dgVector zero (dgVector::m_zero); + for (dgInt32 i = 0; i < (m_nodeCount - 1) ; i ++) { + dgNode* const node = m_nodesOrder[i]; + const dgJointInfo* const jointInfo = &jointInfoArray[node->m_joint->m_index]; + + dgJacobian y0; + dgJacobian y1; + y0.m_linear = zero; + y0.m_angular = zero; + y1.m_linear = zero; + y1.m_angular = zero; + dgAssert(i == node->m_index); + + const dgSpatialVector& f = force[i].m_joint; + dgAssert(jointInfo->m_joint == node->m_joint); + const dgInt32 first = jointInfo->m_pairStart; + const dgInt32 count = node->m_dof; + for (dgInt32 j = 0; j < count; j ++) { + const dgInt32 k = node->m_sourceJacobianIndex[j]; + dgRightHandSide* const rhs = &m_rightHandSide[first + k]; + const dgLeftHandSide* const row = &m_leftHandSide[first + k]; + + rhs->m_force += dgFloat32(f[j]); + dgVector jointForce = dgFloat32 (f[j]); + y0.m_linear += row->m_Jt.m_jacobianM0.m_linear * jointForce; + y0.m_angular += row->m_Jt.m_jacobianM0.m_angular * jointForce; + y1.m_linear += row->m_Jt.m_jacobianM1.m_linear * jointForce; + y1.m_angular += row->m_Jt.m_jacobianM1.m_angular * jointForce; + } + + const dgInt32 m0 = jointInfo->m_m0; + const dgInt32 m1 = jointInfo->m_m1; + 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; + } +} + +DG_INLINE void dgSkeletonContainer::CalculateJointAccel(dgJointInfo* const jointInfoArray, const dgJacobian* const internalForces, dgForcePair* const accel) const +{ + const dgSpatialVector zero (dgSpatialVector::m_zero); + for (dgInt32 i = 0; i < m_nodeCount - 1; i++) { + dgNode* const node = m_nodesOrder[i]; + dgAssert(i == node->m_index); + + dgForcePair& a = accel[i]; + dgAssert(node->m_body); + a.m_body = zero; + a.m_joint = zero; + + dgAssert(node->m_joint); + const dgJointInfo* const jointInfo = &jointInfoArray[node->m_joint->m_index]; + dgAssert(jointInfo->m_joint == node->m_joint); + + const dgInt32 first = jointInfo->m_pairStart; + const dgInt32 dof = jointInfo->m_pairCount; + const dgInt32 m0 = jointInfo->m_m0; + const dgInt32 m1 = jointInfo->m_m1; + const dgJacobian& y0 = internalForces[m0]; + const dgJacobian& y1 = internalForces[m1]; + + for (dgInt32 j = 0; j < dof; j++) { + const dgInt32 k = node->m_sourceJacobianIndex[j]; + const dgLeftHandSide* const row = &m_leftHandSide[first + k]; + const dgRightHandSide* const rhs = &m_rightHandSide[first + k]; + dgVector diag(row->m_JMinv.m_jacobianM0.m_linear * y0.m_linear + row->m_JMinv.m_jacobianM0.m_angular * y0.m_angular + + row->m_JMinv.m_jacobianM1.m_linear * y1.m_linear + row->m_JMinv.m_jacobianM1.m_angular * y1.m_angular); + a.m_joint[j] = -(rhs->m_coordenateAccel - rhs->m_force * rhs->m_diagDamp - diag.AddHorizontal().GetScalar()); + } + } + dgAssert((m_nodeCount - 1) == m_nodesOrder[m_nodeCount - 1]->m_index); + accel[m_nodeCount - 1].m_body = zero; + accel[m_nodeCount - 1].m_joint = zero; +} + +void dgSkeletonContainer::SolveLcp(dgInt32 stride, dgInt32 size, const dgFloat32* const matrix, const dgFloat32* const x0, dgFloat32* const x, const dgFloat32* const b, const dgFloat32* const low, const dgFloat32* const high, const dgInt32* const normalIndex) const +{ + D_TRACKTIME(); + if (m_world->GetCurrentPlugin()) { + dgWorldPlugin* const plugin = m_world->GetCurrentPlugin()->GetInfo().m_plugin; + plugin->SolveDenseLcp(stride, size, matrix, x0, x, b, low, high, normalIndex); + } else { +#if 0 + // sequential Sidle iteration + const dgFloat32 sor = dgFloat32(1.125f); + const dgFloat32 tol2 = dgFloat32(0.25f); + const dgInt32 maxIterCount = 64; + + dgFloat32* const invDiag1 = dgAlloca(dgFloat32, size); + + int rowStart = 0; + for (dgInt32 i = 0; i < size; i++) { + const int index = normalIndex[i]; + const dgFloat32 coefficient = index ? (x[i + index] + x0[i + index]) : 1.0f; + const dgFloat32 l = low[i] * coefficient - x0[i]; + const dgFloat32 h = high[i] * coefficient - x0[i];; + x[i] = dgClamp(dgFloat32(0.0f), l, h); + invDiag1[i] = dgFloat32(1.0f) / matrix[rowStart + i]; + rowStart += stride; + } + + dgFloat32 tolerance(tol2 * dgFloat32(2.0f)); + const dgFloat32* const invDiag = invDiag1; + + dgInt32 iterCount = 0; + for (dgInt32 k = 0; (k < maxIterCount) && (tolerance > tol2); k++) { + iterCount++; + dgInt32 base = 0; + tolerance = dgFloat32(0.0f); + for (dgInt32 i = 0; i < size; i++) { + const dgFloat32* const row = &matrix[base]; + dgFloat32 r = b[i] - dgDotProduct(size, row, x); + + const int index = normalIndex[i]; + const dgFloat32 coefficient = index ? (x[i + index] + x0[i + index]) : dgFloat32(1.0f); + const dgFloat32 l = low[i] * coefficient - x0[i]; + const dgFloat32 h = high[i] * coefficient - x0[i]; + + dgFloat32 f = x[i] + ((r + row[i] * x[i]) * invDiag[i] - x[i]) * sor; + if (f > h) { + f = h; + } else if (f < l) { + f = l; + } else { + tolerance += r * r; + } + x[i] = f; + base += stride; + } + } +#else + // ready for parallelization + const dgFloat32 sor = dgFloat32(1.125f); + const dgFloat32 tol2 = dgFloat32(0.25f); + const dgInt32 maxIterCount = 64; + + dgFloat32* const invDiag1 = dgAlloca(dgFloat32, size); + dgFloat32* const residual = dgAlloca(dgFloat32, size); + + dgInt32 rowStart = 0; + for (dgInt32 i = 0; i < size; i++) { + const int index = normalIndex[i]; + const dgFloat32 coefficient = index ? (x[i + index] + x0[i + index]) : 1.0f; + const dgFloat32 l = low[i] * coefficient - x0[i]; + const dgFloat32 h = high[i] * coefficient - x0[i];; + x[i] = dgClamp(dgFloat32(0.0f), l, h); + invDiag1[i] = dgFloat32(1.0f) / matrix[rowStart + i]; + rowStart += stride; + } + + dgInt32 base = 0; + for (dgInt32 i = 0; i < size; i++) { + const dgFloat32* const row = &matrix[base]; + residual[i] = b[i] - dgDotProduct(size, row, x); + base += stride; + } + + dgInt32 iterCount = 0; + dgFloat32 tolerance(tol2 * dgFloat32(2.0f)); + const dgFloat32* const invDiag = invDiag1; + const dgFloat32 one = dgFloat32(1.0f); + for (dgInt32 k = 0; (k < maxIterCount) && (tolerance > tol2); k++) { + base = 0; + iterCount++; + tolerance = dgFloat32(0.0f); + for (dgInt32 i = 0; i < size; i++) { + const dgFloat32 r = residual[i]; + const int index = normalIndex[i]; + const dgFloat32 coefficient = index ? x[i + index] + x0[i + index] : one; + const dgFloat32 l = low[i] * coefficient - x0[i]; + const dgFloat32 h = high[i] * coefficient - x0[i]; + + const dgFloat32* const row = &matrix[base]; +#if 0 + dgFloat32 f = x[i] + ((r + row[i] * x[i]) * invDiag[i] - x[i]) * sor; + if (f > h) { + f = h; + } else if (f < l) { + f = l; + } else { + tolerance += r * r; + } + const dgFloat32 dx = f - x[i]; +#else + const dgFloat32 f = dgClamp(x[i] + ((r + row[i] * x[i]) * invDiag[i] - x[i]) * sor, l, h); + const dgFloat32 dx = f - x[i]; + const dgFloat32 dr = dx * row[i]; + tolerance += dr * dr; +#endif + x[i] = f; + if (dgAbs (dx) > dgFloat32 (1.0e-6f)) { + for (dgInt32 j = 0; j < size; j++) { + residual[j] -= row[j] * dx; + } + } + base += stride; + } + } +#endif + } +} + +void dgSkeletonContainer::SolveBlockLcp(dgInt32 size, dgInt32 blockSize, const dgFloat32* const x0, dgFloat32* const x, dgFloat32* const b, const dgFloat32* const low, const dgFloat32* const high, const dgInt32* const normalIndex) const +{ + if (blockSize) { + dgSolveCholesky(blockSize, size, m_massMatrix11, x, b); + if (blockSize != size) { + + dgInt32 base = blockSize * size; + for (dgInt32 i = blockSize; i < size; i++) { + b[i] -= dgDotProduct(blockSize, &m_massMatrix11[base], x); + base += size; + } + + const int boundedSize = size - blockSize; + SolveLcp( + size, boundedSize, &m_massMatrix11[blockSize * size + blockSize], + &x0[blockSize], &x[blockSize], &b[blockSize], &low[blockSize], &high[blockSize], &normalIndex[blockSize]); + + for (dgInt32 j = 0; j < blockSize; j++) { + const dgFloat32* const row = &m_massMatrix11[j * size + blockSize]; + dgFloat32 acc = dgFloat32 (0.0f); + for (dgInt32 i = 0; i < boundedSize; i++) { + acc += x[blockSize + i] * row[i]; + } + x[j] += acc; + } + } + } else { + SolveLcp(size, size, m_massMatrix11, x0, x, b, low, high, normalIndex); + } +} + +void dgSkeletonContainer::SolveAuxiliary(const dgJointInfo* const jointInfoArray, dgJacobian* const internalForces, const dgForcePair* const accel, dgForcePair* const force) const +{ + dgFloat32* const f = dgAlloca(dgFloat32, m_rowCount); + dgFloat32* const u = dgAlloca(dgFloat32, m_auxiliaryRowCount); + dgFloat32* const b = dgAlloca(dgFloat32, m_auxiliaryRowCount); + dgFloat32* const u0 = dgAlloca(dgFloat32, m_auxiliaryRowCount); + dgFloat32* const low = dgAlloca(dgFloat32, m_auxiliaryRowCount); + dgFloat32* const high = dgAlloca(dgFloat32, m_auxiliaryRowCount); + dgInt32* const normalIndex = dgAlloca(dgInt32, m_auxiliaryRowCount); + + dgInt32 primaryIndex = 0; + const dgInt32 primaryCount = m_rowCount - m_auxiliaryRowCount; + + for (dgInt32 i = 0; i < m_nodeCount - 1; i++) { + const dgNode* const node = m_nodesOrder[i]; + const dgInt32 primaryDof = node->m_dof; + const dgSpatialVector& forceSpatial = force[i].m_joint; + + for (dgInt32 j = 0; j < primaryDof; j++) { + f[primaryIndex] = dgFloat32(forceSpatial[j]); + primaryIndex++; + } + } + + dgAssert (primaryIndex == primaryCount); + for (dgInt32 i = 0; i < m_auxiliaryRowCount; i++) { + const int index = m_matrixRowsIndex[primaryCount + i]; + const dgLeftHandSide* const row = &m_leftHandSide[index]; + const dgRightHandSide* const rhs = &m_rightHandSide[index]; + + const int m0 = m_pairs[primaryCount + i].m_m0; + const int m1 = m_pairs[primaryCount + i].m_m1; + + const dgJacobian& y0 = internalForces[m0]; + const dgJacobian& y1 = internalForces[m1]; + + f[primaryCount + i] = dgFloat32(0.0f); + + dgVector acc(row->m_JMinv.m_jacobianM0.m_linear * y0.m_linear + row->m_JMinv.m_jacobianM0.m_angular * y0.m_angular + + row->m_JMinv.m_jacobianM1.m_linear * y1.m_linear + row->m_JMinv.m_jacobianM1.m_angular * y1.m_angular); + b[i] = rhs->m_coordenateAccel - acc.AddHorizontal().GetScalar(); + + normalIndex[i] = m_frictionIndex[primaryCount + i]; + u0[i] = rhs->m_force; + low[i] = rhs->m_lowerBoundFrictionCoefficent; + high[i] = rhs->m_upperBoundFrictionCoefficent; + } + + for (dgInt32 i = 0; i < m_auxiliaryRowCount; i++) { + dgFloat32* const matrixRow10 = &m_massMatrix10[i * primaryCount]; + b[i] -= dgDotProduct(primaryCount, matrixRow10, f); + } + + SolveBlockLcp(m_auxiliaryRowCount, m_blockSize, u0, u, b, low, high, normalIndex); + + for (dgInt32 i = 0; i < m_auxiliaryRowCount; i++) { + const dgFloat32 s = u[i]; + f[primaryCount + i] = s; + dgMulAdd(primaryCount, f, f, &m_deltaForce[i * primaryCount], s); + } + + for (dgInt32 i = 0; i < m_rowCount; i++) { + dgInt32 index = m_matrixRowsIndex[i]; + dgRightHandSide* const rhs = &m_rightHandSide[index]; + const dgLeftHandSide* const row = &m_leftHandSide[index]; + const dgInt32 m0 = m_pairs[i].m_m0; + const dgInt32 m1 = m_pairs[i].m_m1; + + rhs->m_force += f[i]; + dgVector jointForce(f[i]); + internalForces[m0].m_linear += row->m_Jt.m_jacobianM0.m_linear * jointForce; + internalForces[m0].m_angular += row->m_Jt.m_jacobianM0.m_angular * jointForce; + internalForces[m1].m_linear += row->m_Jt.m_jacobianM1.m_linear * jointForce; + internalForces[m1].m_angular += row->m_Jt.m_jacobianM1.m_angular * jointForce; + } +} + +dgInt8* dgSkeletonContainer::CalculateBufferSizeInBytes (const dgJointInfo* const jointInfoArray) +{ + dgInt32 rowCount = 0; + dgInt32 auxiliaryRowCount = 0; + if (m_nodesOrder) { + for (dgInt32 i = 0; i < m_nodeCount - 1; i++) { + dgNode* const node = m_nodesOrder[i]; + rowCount += jointInfoArray[node->m_joint->m_index].m_pairCount; + auxiliaryRowCount += node->GetAuxiliaryRows(jointInfoArray, m_rightHandSide); + } + } + + dgInt32 extraAuxiliaryRows = 0; + const dgInt32 loopCount = m_loopCount + m_dynamicsLoopCount; + for (dgInt32 j = 0; j < loopCount; j++) { + const dgConstraint* const joint = m_loopingJoints[j]; + extraAuxiliaryRows += jointInfoArray[joint->m_index].m_pairCount; + } + rowCount += extraAuxiliaryRows; + auxiliaryRowCount+= extraAuxiliaryRows; + + dgInt32 size = sizeof (dgInt32) * rowCount; + size += sizeof (dgInt32) * rowCount; + size += sizeof (dgNodePair) * rowCount; + size += sizeof(dgFloat32) * auxiliaryRowCount * auxiliaryRowCount; // matrix11[auxiliaryRowCount * auxiliaryRowCount] + size += sizeof (dgFloat32) * auxiliaryRowCount * (rowCount - auxiliaryRowCount); + size += sizeof (dgFloat32) * auxiliaryRowCount * (rowCount - auxiliaryRowCount); + size = (size + 1024) & -0x10; + m_auxiliaryMemoryBuffer.ResizeIfNecessary((size + 1024) & -0x10); + return &m_auxiliaryMemoryBuffer[0]; +} + +void dgSkeletonContainer::InitMassMatrix(const dgJointInfo* const jointInfoArray, const dgLeftHandSide* const leftHandSide, dgRightHandSide* const rightHandSide, bool consideredCloseLoop) +{ + D_TRACKTIME(); + dgInt32 rowCount = 0; + dgInt32 auxiliaryCount = 0; + m_leftHandSide = leftHandSide; + m_rightHandSide = rightHandSide; + m_consideredCloseLoop = consideredCloseLoop ? 1 : 0; + + dgSpatialMatrix* const bodyMassArray = dgAlloca (dgSpatialMatrix, m_nodeCount); + dgSpatialMatrix* const jointMassArray = dgAlloca (dgSpatialMatrix, m_nodeCount); + + if (m_nodesOrder) { + for (dgInt32 i = 0; i < m_nodeCount - 1; i++) { + dgNode* const node = m_nodesOrder[i]; + const dgJointInfo& info = jointInfoArray[node->m_joint->m_index]; + rowCount += info.m_pairCount; + auxiliaryCount += node->Factorize(jointInfoArray, leftHandSide, rightHandSide, bodyMassArray, jointMassArray); + } + m_nodesOrder[m_nodeCount - 1]->Factorize(jointInfoArray, leftHandSide, rightHandSide, bodyMassArray, jointMassArray); + } + m_rowCount = dgInt16 (rowCount); + m_auxiliaryRowCount = dgInt16 (auxiliaryCount); + + dgInt32 loopRowCount = 0; + const dgInt32 loopCount = m_loopCount + m_dynamicsLoopCount; + for (dgInt32 j = 0; j < loopCount; j++) { + const dgConstraint* const joint = m_loopingJoints[j]; + loopRowCount += jointInfoArray[joint->m_index].m_pairCount; + } + m_loopRowCount = dgInt16 (loopRowCount); + m_rowCount += m_loopRowCount; + m_auxiliaryRowCount += m_loopRowCount; + + if (m_auxiliaryRowCount && m_consideredCloseLoop) { + InitLoopMassMatrix(jointInfoArray); + } +} + +void dgSkeletonContainer::CalculateJointForce(dgJointInfo* const jointInfoArray, const dgBodyInfo* const bodyArray, dgJacobian* const internalForces) +{ + D_TRACKTIME(); + dgForcePair* const force = dgAlloca(dgForcePair, m_nodeCount); + dgForcePair* const accel = dgAlloca(dgForcePair, m_nodeCount); + + CalculateJointAccel(jointInfoArray, internalForces, accel); + CalculateForce(force, accel); + if (m_auxiliaryRowCount && m_consideredCloseLoop) { + SolveAuxiliary (jointInfoArray, internalForces, accel, force); + } else { + UpdateForces(jointInfoArray, internalForces, force); + } +} + diff --git a/thirdparty/src/newton/dgPhysics/dgSkeletonContainer.h b/thirdparty/src/newton/dgPhysics/dgSkeletonContainer.h new file mode 100644 index 000000000..5d59d1768 --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgSkeletonContainer.h @@ -0,0 +1,119 @@ +/* Copyright (c) <2003-2019> +* +* 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_SKELETON_CONTAINER_H__ +#define _DG_SKELETON_CONTAINER_H__ + +#include "dgConstraint.h" +#include "dgContact.h" +#include "dgBilateralConstraint.h" + +class dgDynamicBody; + +class dgSkeletonContainer +{ + public: + class dgNode; + class dgNodePair; + class dgForcePair; + class dgMatriData; + class dgBodyJointMatrixDataPair; + + DG_CLASS_ALLOCATOR(allocator) + dgSkeletonContainer(dgWorld* const world, dgDynamicBody* const rootBody); + virtual ~dgSkeletonContainer(); + + dgWorld* GetWorld() const; + dgInt32 GetJointCount () const {return m_nodeCount - 1;} + dgNode* AddChild (dgBilateralConstraint* const joint, dgNode* const parent); + void RemoveLoopJoint(dgBilateralConstraint* const joint); + void Finalize (dgInt32 loopJoints, dgBilateralConstraint** const loopJointArray); + + dgNode* GetRoot () const; + dgDynamicBody* GetBody(dgNode* const node) const; + dgBilateralConstraint* GetJoint(dgNode* const node) const; + dgNode* GetParent (dgNode* const node) const; + dgNode* GetFirstChild (dgNode* const parent) const; + dgNode* GetNextSiblingChild (dgNode* const sibling) const; + + void ClearSelfCollision(); + void AddSelfCollisionJoint(dgConstraint* const joint); + + dgInt32 GetLru() const { return m_lru; } + void SetLru(dgInt32 lru) { m_lru = lru; } + + virtual void CalculateJointForce (dgJointInfo* const jointInfoArray, const dgBodyInfo* const bodyArray, dgJacobian* const internalForces); + virtual void InitMassMatrix (const dgJointInfo* const jointInfoArray, const dgLeftHandSide* const matrixRow, dgRightHandSide* const rightHandSide, bool m_consideredCloseLoop = true); + + private: + bool SanityCheck(const dgForcePair* const force, const dgForcePair* const accel) const; + void ConditionMassMatrix() const; + void RebuildMassMatrix(const dgFloat32* const diagDamp) const; + void CalculateLoopMassMatrixCoefficients(dgFloat32* const diagDamp); + + DG_INLINE void CalculateForce(dgForcePair* const force, const dgForcePair* const accel) const; + DG_INLINE void SolveBackward(dgForcePair* const force) const; + DG_INLINE void SolveForward(dgForcePair* const force, const dgForcePair* const accel, dgInt32 startNode = 0) const; + DG_INLINE void UpdateForces(dgJointInfo* const jointInfoArray, dgJacobian* const internalForces, const dgForcePair* const force) const; + DG_INLINE void CalculateJointAccel (dgJointInfo* const jointInfoArray, const dgJacobian* const internalForces, dgForcePair* const accel) const; + + dgNode* FindNode(dgDynamicBody* const node) const; + void SortGraph(dgNode* const root, dgInt32& index); + + void InitLoopMassMatrix (const dgJointInfo* const jointInfoArray); + dgInt8* CalculateBufferSizeInBytes (const dgJointInfo* const jointInfoArray); + void SolveAuxiliary (const dgJointInfo* const jointInfoArray, dgJacobian* const internalForces, const dgForcePair* const accel, dgForcePair* const force) const; + void SolveLcp(dgInt32 stride, dgInt32 size, const dgFloat32* const matrix, const dgFloat32* const x0, dgFloat32* const x, const dgFloat32* const b, const dgFloat32* const low, const dgFloat32* const high, const dgInt32* const normalIndex) const; + void SolveBlockLcp(dgInt32 size, dgInt32 blockSize, const dgFloat32* const x0, dgFloat32* const x, dgFloat32* const b, const dgFloat32* const low, const dgFloat32* const high, const dgInt32* const normalIndex) const; + void FactorizeMatrix(dgInt32 size, dgInt32 stride, dgFloat32* const matrix, dgFloat32* const diagDamp) const; + + dgWorld* m_world; + dgNode* m_skeleton; + dgNode** m_nodesOrder; + dgNodePair* m_pairs; + dgFloat32* m_deltaForce; + dgFloat32* m_massMatrix11; + dgFloat32* m_massMatrix10; + + dgRightHandSide* m_rightHandSide; + const dgLeftHandSide* m_leftHandSide; + dgInt32* m_frictionIndex; + dgInt32* m_matrixRowsIndex; + dgSkeletonList::dgListNode* m_listNode; + dgArray m_loopingJoints; + dgArray m_auxiliaryMemoryBuffer; + dgInt32 m_lru; + dgInt32 m_blockSize; + dgInt16 m_nodeCount; + dgInt16 m_loopCount; + dgInt16 m_dynamicsLoopCount; + dgInt16 m_rowCount; + dgInt16 m_loopRowCount; + dgInt16 m_auxiliaryRowCount; + dgInt16 m_consideredCloseLoop; + + friend class dgWorld; + friend class dgParallelBodySolver; + friend class dgWorldDynamicUpdate; +}; + +#endif + diff --git a/thirdparty/src/newton/dgPhysics/dgSlidingConstraint.cpp b/thirdparty/src/newton/dgPhysics/dgSlidingConstraint.cpp new file mode 100644 index 000000000..5ca6e510a --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgSlidingConstraint.cpp @@ -0,0 +1,204 @@ +/* Copyright (c) <2003-2019> +* +* 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 "dgPhysicsStdafx.h" +#include "dgBody.h" +#include "dgWorld.h" +#include "dgSlidingConstraint.h" + + + +////////////////////////////////////////////////////////////////////// +// Construction/Destruction +////////////////////////////////////////////////////////////////////// + +dgSlidingConstraint::dgSlidingConstraint () + :dgBilateralConstraint() +{ + dgAssert ((((dgUnsigned64) &m_localMatrix0) & 15) == 0); + + m_localMatrix0 = dgGetIdentityMatrix(); + m_localMatrix1 = dgGetIdentityMatrix(); + + m_maxDOF = 6; + m_constId = m_sliderConstraint; + m_posit = dgFloat32 (0.0f); + m_jointAccelFnt = NULL; +} + +dgSlidingConstraint::~dgSlidingConstraint () +{ +} + +/* +dgSlidingConstraint* dgSlidingConstraint::Create(dgWorld* world) +{ + dgSlidingConstraint* constraint; + + dgSlidingConstraintArray& array = *world; +// constraint = dgSlidingConstraintArray::GetPool().GetElement(); + constraint = array.GetElement(); + + dgAssert ((((dgUnsigned64) &constraint->m_localMatrix0) & 15) == 0); + + constraint->Init (); + constraint->m_maxDOF = 6; + constraint->m_constId = dgSliderConstraintId; + constraint->m_posit = dgFloat32 (0.0f); + constraint->m_jointAccelFnt = NULL; + return constraint; +} + +void dgSlidingConstraint::Remove(dgWorld* world) +{ + dgSlidingConstraintArray& array = *world; + dgBilateralConstraint::Remove (world); +// dgSlidingConstraintArray::GetPool().RemoveElement (this); + array.RemoveElement (this); +} +*/ + +void dgSlidingConstraint::SetJointParameterCallback (dgSlidingJointAcceleration callback) +{ + m_jointAccelFnt = callback; +} + +dgFloat32 dgSlidingConstraint::GetJointPosit () const +{ + return m_posit; +} + +dgFloat32 dgSlidingConstraint::GetJointVeloc () const +{ + dgAssert (m_body0); + dgAssert (m_body1); + dgVector dir (m_body0->GetMatrix().RotateVector (m_localMatrix0[0])); + const dgVector& veloc0 = m_body0->GetVelocity(); + const dgVector& veloc1 = m_body1->GetVelocity(); + return dir.DotProduct(veloc0 - veloc1).GetScalar(); +} + +dgFloat32 dgSlidingConstraint::CalculateStopAccel (dgFloat32 distance, const dgJointCallbackParam* param) const +{ + dgFloat32 accel; + dgFloat32 speed; + dgFloat32 penetrationErr; + + accel = dgFloat32 (0.0f); + if (m_posit > distance) { + speed = GetJointVeloc (); + if (speed < dgFloat32 (0.0f)) { + speed = dgFloat32 (0.0f); + } + penetrationErr = (distance - m_posit); + accel = dgFloat32 (100.0f) * penetrationErr - speed * dgFloat32 (1.01f) / param->m_timestep; + + } else if (m_posit < distance) { + speed = GetJointVeloc (); + if (speed > dgFloat32 (0.0f)) { + speed = dgFloat32 (0.0f); + } + penetrationErr = distance - m_posit; + dgAssert (penetrationErr >= dgFloat32 (0.0f)); + accel = dgFloat32 (100.0f) * penetrationErr - speed * dgFloat32 (1.01f) / param->m_timestep; + } + return accel; +} + +dgVector dgSlidingConstraint::GetJointForce () const +{ + dgMatrix matrix0; + dgMatrix matrix1; + + CalculateGlobalMatrixAndAngle (m_localMatrix0, m_localMatrix1, matrix0, matrix1); + return dgVector (matrix0.m_up.Scale (m_jointForce[0].m_force) + + matrix0.m_right.Scale (m_jointForce[1].m_force) + + matrix0.m_up.Scale (m_jointForce[2].m_force) + + matrix0.m_right.Scale (m_jointForce[3].m_force) + + matrix0.m_right.Scale (m_jointForce[4].m_force)); +} + + +dgUnsigned32 dgSlidingConstraint::JacobianDerivative (dgContraintDescritor& params) +{ + dgMatrix matrix0; + dgMatrix matrix1; + + //dgVector angle (CalculateGlobalMatrixAndAngle (matrix0, matrix1)); + CalculateGlobalMatrixAndAngle (m_localMatrix0, m_localMatrix1, matrix0, matrix1); + m_posit = matrix0.m_front.DotProduct(matrix0.m_posit - matrix1.m_posit).GetScalar(); + matrix1.m_posit += matrix1.m_front.Scale (m_posit); + + dgAssert (dgAbs (dgFloat32 (1.0f) - matrix0.m_front.DotProduct(matrix0.m_front).GetScalar()) < dgFloat32 (1.0e-5f)); + dgAssert (dgAbs (dgFloat32 (1.0f) - matrix0.m_up.DotProduct(matrix0.m_up).GetScalar()) < dgFloat32 (1.0e-5f)); + dgAssert (dgAbs (dgFloat32 (1.0f) - matrix0.m_right.DotProduct(matrix0.m_right).GetScalar()) < dgFloat32 (1.0e-5f)); + + const dgVector& dir1 = matrix0.m_up; + const dgVector& dir2 = matrix0.m_right; + + dgVector p0 (matrix0.m_posit); + dgVector p1 (matrix1.m_posit + matrix1.m_front.Scale (matrix1.m_front.DotProduct(p0 - matrix1.m_posit).GetScalar())); + + dgVector q0 (p0 + matrix0.m_front.Scale(MIN_JOINT_PIN_LENGTH)); + dgVector q1 (p1 + matrix1.m_front.Scale(MIN_JOINT_PIN_LENGTH)); + + dgVector r0 (p0 + matrix0.m_up.Scale(MIN_JOINT_PIN_LENGTH)); + dgVector r1 (p1 + matrix1.m_up.Scale(MIN_JOINT_PIN_LENGTH)); + + dgPointParam pointDataP; + dgPointParam pointDataQ; + dgPointParam pointDataR; + InitPointParam (pointDataP, m_defualtDiagonalRegularizer, p0, p1); + InitPointParam (pointDataQ, m_defualtDiagonalRegularizer, q0, q1); + InitPointParam (pointDataR, m_defualtDiagonalRegularizer, r0, r1); + + CalculatePointDerivative (0, params, dir1, pointDataP, &m_jointForce[0]); + CalculatePointDerivative (1, params, dir2, pointDataP, &m_jointForce[1]); + CalculatePointDerivative (2, params, dir1, pointDataQ, &m_jointForce[2]); + CalculatePointDerivative (3, params, dir2, pointDataQ, &m_jointForce[3]); + CalculatePointDerivative (4, params, dir2, pointDataR, &m_jointForce[4]); + + dgInt32 ret = 5; + if (m_jointAccelFnt) { + dgJointCallbackParam axisParam; + axisParam.m_accel = dgFloat32 (0.0f); + axisParam.m_timestep = params.m_timestep; + axisParam.m_minFriction = DG_MIN_BOUND; + axisParam.m_maxFriction = DG_MAX_BOUND; + + if (m_jointAccelFnt (*this, &axisParam)) { + if ((axisParam.m_minFriction > DG_MIN_BOUND) || (axisParam.m_maxFriction < DG_MAX_BOUND)) { + params.m_forceBounds[5].m_low = axisParam.m_minFriction; + params.m_forceBounds[5].m_upper = axisParam.m_maxFriction; + params.m_forceBounds[5].m_normalIndex = DG_INDEPENDENT_ROW; + } + + CalculatePointDerivative (5, params, matrix0.m_front, pointDataP, &m_jointForce[5]); + SetMotorAcceleration (5, axisParam.m_accel, params); + ret = 6; + } + } + + return dgUnsigned32 (ret); +} + + + diff --git a/thirdparty/src/newton/dgPhysics/dgSlidingConstraint.h b/thirdparty/src/newton/dgPhysics/dgSlidingConstraint.h new file mode 100644 index 000000000..4e41c455c --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgSlidingConstraint.h @@ -0,0 +1,70 @@ +/* Copyright (c) <2003-2019> +* +* 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_DGSLIDINGCONSTRAINT_H__3FF7D7AA_90CC_4BA5_B2A4_D3BA51AD2FFD_H) +#define AFX_DGSLIDINGCONSTRAINT_H__3FF7D7AA_90CC_4BA5_B2A4_D3BA51AD2FFD_H +#include "dgBilateralConstraint.h" + +//template class dgPool; + +class dgSlidingConstraint; +typedef dgUnsigned32 (dgApi *dgSlidingJointAcceleration) (const dgSlidingConstraint& hinge, dgJointCallbackParam* param); + +class dgSlidingConstraint: public dgBilateralConstraint +{ + public: + dgFloat32 GetJointPosit () const; + dgFloat32 GetJointVeloc () const; + dgVector GetJointForce () const; + dgFloat32 CalculateStopAccel (dgFloat32 distance, const dgJointCallbackParam* param) const; + void SetJointParameterCallback (dgSlidingJointAcceleration callback); + + private: + dgSlidingConstraint(); + virtual ~dgSlidingConstraint(); + + virtual dgUnsigned32 JacobianDerivative (dgContraintDescritor& params); + virtual void Serialize (dgSerialize serializeCallback, void* const userData) {dgAssert (0);} + + dgMatrix m_localMatrix0; + dgMatrix m_localMatrix1; + + dgFloat32 m_posit; + dgSlidingJointAcceleration m_jointAccelFnt; + +//#ifdef _NEWTON_USE_DOUBLE +// dgUnsigned32 m_reserve[1]; +//#else +// dgUnsigned32 m_reserve[2]; +//#endif + + friend class dgWorld; +// friend class dgPool; +}; + +/* +class dgSlidingConstraintArray: public dgPoolContainer +{ +}; +*/ + +#endif // !defined(AFX_DGSLIDINGCONSTRAINT_H__3FF7D7AA_90CC_4BA5_B2A4_D3BA51AD2FFD_H) + diff --git a/thirdparty/src/newton/dgPhysics/dgUniversalConstraint.cpp b/thirdparty/src/newton/dgPhysics/dgUniversalConstraint.cpp new file mode 100644 index 000000000..467c4e482 --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgUniversalConstraint.cpp @@ -0,0 +1,309 @@ +/* Copyright (c) <2003-2019> +* +* 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 "dgPhysicsStdafx.h" +#include "dgBody.h" +#include "dgWorld.h" +#include "dgUniversalConstraint.h" + + + +////////////////////////////////////////////////////////////////////// +// Construction/Destruction +////////////////////////////////////////////////////////////////////// + +dgUniversalConstraint::dgUniversalConstraint () + :dgBilateralConstraint() +{ + dgAssert ((((dgUnsigned64) &m_localMatrix0) & 15) == 0); + + m_localMatrix0 = dgGetIdentityMatrix(); + m_localMatrix1 = dgGetIdentityMatrix(); + + m_maxDOF = 6; + m_constId = m_universalConstraint; + + m_angle0 = dgFloat32 (0.0f); + m_angle1 = dgFloat32 (0.0f); + m_jointAccelFnt = NULL; +} + +dgUniversalConstraint::~dgUniversalConstraint () +{ +} + +/* +dgUniversalConstraint* dgUniversalConstraint::Create(dgWorld* world) +{ + dgUniversalConstraint* constraint; + +// constraint = dgUniversalConstraintArray::GetPool().GetElement(); + + dgUniversalConstraintArray& array = * world; + constraint = array.GetElement(); + + dgAssert ((((dgUnsigned64) &constraint->m_localMatrix0) & 15) == 0); + constraint->Init (); + constraint->m_maxDOF = 6; + constraint->m_constId = dgUniversalConstraintId; + + constraint->m_angle0 = dgFloat32 (0.0f); + constraint->m_angle1 = dgFloat32 (0.0f); + constraint->m_jointAccelFnt = NULL; + return constraint; +} + +void dgUniversalConstraint::Remove(dgWorld* world) +{ + dgUniversalConstraintArray& array = * world; + + dgBilateralConstraint::Remove (world); +// dgUniversalConstraintArray::GetPool().RemoveElement (this); + array.RemoveElement (this); +} +*/ + +void dgUniversalConstraint::SetJointParameterCallback (dgUniversalJointAcceleration callback) +{ + m_jointAccelFnt = callback; +} + + +dgFloat32 dgUniversalConstraint::GetJointAngle0 () const +{ + return m_angle0; +} + +dgFloat32 dgUniversalConstraint::GetJointAngle1 () const +{ + return m_angle1; +} + + +dgFloat32 dgUniversalConstraint::GetJointOmega0 () const +{ + dgAssert (m_body0); + dgAssert (m_body1); + + dgVector dir (m_body0->GetMatrix().RotateVector (m_localMatrix0[0])); + const dgVector& omega0 = m_body0->GetOmega(); + const dgVector& omega1 = m_body1->GetOmega(); + +// dgVector omega1 (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f)); +// if (m_body1) { +// omega1 = m_body1->GetOmega(); +// } + return dir.DotProduct(omega0 - omega1).GetScalar(); +} + +dgFloat32 dgUniversalConstraint::GetJointOmega1 () const +{ + dgAssert (m_body0); + dgAssert (m_body1); + + dgVector dir (m_body1->GetMatrix().RotateVector (m_localMatrix1[1])); + const dgVector& omega0 = m_body0->GetOmega(); + const dgVector& omega1 = m_body1->GetOmega(); + +// dgMatrix matrix (dgGetIdentityMatrix()); +// if (m_body1) { +// matrix = m_body1->GetMatrix(); +// omega1 = m_body1->GetOmega(); +// } +// dgVector dir (matrix.RotateVector (m_localMatrix1[1])); + + return dir.DotProduct(omega0 - omega1).GetScalar(); +} + +dgFloat32 dgUniversalConstraint::CalculateStopAlpha0 (dgFloat32 angle, const dgJointCallbackParam* param) const +{ + dgFloat32 alpha; + dgFloat32 omega; + dgFloat32 penetrationErr; + + alpha = dgFloat32 (0.0f); + if (m_angle0 > angle) { + omega = GetJointOmega0 (); + if (omega < dgFloat32 (0.0f)) { + omega = dgFloat32 (0.0f); + } + penetrationErr = angle - m_angle0; + alpha = 100.0f * penetrationErr - omega * 1.01f / param->m_timestep; + + } else if (m_angle0 < angle) { + omega = GetJointOmega0 (); + if (omega > dgFloat32 (0.0f)) { + omega = dgFloat32 (0.0f); + } + + penetrationErr = angle - m_angle0; + alpha = 100.0f * penetrationErr - omega * 1.01f / param->m_timestep; + } + return alpha; + +} + +dgFloat32 dgUniversalConstraint::CalculateStopAlpha1 (dgFloat32 angle, const dgJointCallbackParam* param) const +{ + dgFloat32 alpha; + dgFloat32 omega; + dgFloat32 penetrationErr; + + alpha = dgFloat32 (0.0f); + if (m_angle1 > angle) { + omega = GetJointOmega1 (); + if (omega > dgFloat32 (0.0f)) { + omega = dgFloat32 (0.0f); + } + penetrationErr = m_angle1 - angle; +// alpha = (penetrationErr + omega * 1.01f) / param->m_timestep; + alpha = penetrationErr * 100.0f - omega * 1.01f / param->m_timestep; + } else if (m_angle1 < angle) { + omega = GetJointOmega1 (); + if (omega < dgFloat32 (0.0f)) { + omega = dgFloat32 (0.0f); + } + + penetrationErr = m_angle1 - angle; +// alpha = (penetrationErr + omega * 1.01f) / param->m_timestep; + alpha = penetrationErr * 100.0f - omega * 1.01f / param->m_timestep; + } + return alpha; +} + + +dgVector dgUniversalConstraint::GetJointForce () const +{ + dgMatrix matrix0; + dgMatrix matrix1; + + CalculateGlobalMatrixAndAngle (m_localMatrix0, m_localMatrix1, matrix0, matrix1); + + return dgVector (matrix0.m_up.Scale (m_jointForce[0].m_force) + + matrix0.m_right.Scale (m_jointForce[1].m_force) + + matrix0.m_up.Scale (m_jointForce[2].m_force) + + matrix0.m_right.Scale (m_jointForce[3].m_force)); +} + +dgUnsigned32 dgUniversalConstraint::JacobianDerivative (dgContraintDescritor& params) +{ + dgMatrix matrix0; + dgMatrix matrix1; + dgFloat32 sinAngle; + dgFloat32 cosAngle; + dgInt32 ret; + + CalculateGlobalMatrixAndAngle (m_localMatrix0, m_localMatrix1, matrix0, matrix1); + + const dgVector& dir0 = matrix0.m_front; + const dgVector& dir1 = matrix1.m_up; + dgVector dir2 (dir0.CrossProduct(dir1)); + + dgVector dir3 (dir2.CrossProduct(dir0)); + dir3 = dir3.Normalize(); + + const dgVector& p0 = matrix0.m_posit; + const dgVector& p1 = matrix1.m_posit; + + dgVector q0 (p0 + dir3.Scale(MIN_JOINT_PIN_LENGTH)); + dgVector q1 (p1 + dir1.Scale(MIN_JOINT_PIN_LENGTH)); + + dgPointParam pointDataP; + dgPointParam pointDataQ; + InitPointParam (pointDataP, m_defualtDiagonalRegularizer, p0, p1); + InitPointParam (pointDataQ, m_defualtDiagonalRegularizer, q0, q1); + + CalculatePointDerivative (0, params, dir0, pointDataP, &m_jointForce[0]); + CalculatePointDerivative (1, params, dir1, pointDataP, &m_jointForce[1]); + CalculatePointDerivative (2, params, dir2, pointDataP, &m_jointForce[2]); + CalculatePointDerivative (3, params, dir0, pointDataQ, &m_jointForce[3]); + ret = 4; + + +// dgVector sinAngle0 (matrix1.m_up * matrix0.m_up); +// m_angle0 = dgAsin (ClampValue (sinAngle0 % dir0, -0.9999999f, 0.9999999f)); +// if ((matrix0.m_up % matrix1.m_up) < dgFloat32 (0.0f)) { +// m_angle0 = (m_angle0 >= dgFloat32 (0.0f)) ? dgPI - m_angle0 : dgPI + m_angle0; +// } + + sinAngle = matrix0.m_front.DotProduct(matrix1.m_up.CrossProduct(matrix0.m_up)).GetScalar(); + cosAngle = matrix0.m_up.DotProduct(matrix1.m_up).GetScalar(); +// dgAssert (dgAbs (m_angle0 - dgAtan2 (sinAngle, cosAngle)) < 1.0e-1f); + m_angle0 = dgAtan2 (sinAngle, cosAngle); + +// dgVector sinAngle1 (matrix0.m_front * matrix1.m_front); +// m_angle1 = dgAsin (ClampValue (sinAngle1 % dir1, -0.9999999f, 0.9999999f)); +// if ((matrix0.m_front % matrix1.m_front) < dgFloat32 (0.0f)) { +// m_angle1 = (m_angle1 >= dgFloat32 (0.0f)) ? dgPI - m_angle1 : dgPI + m_angle1; +// } + + sinAngle = matrix1.m_up.DotProduct(matrix0.m_front.CrossProduct(matrix1.m_front)).GetScalar(); + cosAngle = matrix0.m_front.DotProduct(matrix1.m_front).GetScalar(); +// dgAssert (dgAbs (m_angle1 - dgAtan2 (sinAngle, cosAngle)) < 1.0e-1f); + m_angle1 = dgAtan2 (sinAngle, cosAngle); + + if (m_jointAccelFnt) { + dgUnsigned32 code; + dgJointCallbackParam axisParam[2]; + + // linear acceleration + axisParam[0].m_accel = dgFloat32 (0.0f); + axisParam[0].m_timestep = params.m_timestep; + axisParam[0].m_minFriction = DG_MIN_BOUND; + axisParam[0].m_maxFriction = DG_MAX_BOUND; + + // angular acceleration + axisParam[1].m_accel = dgFloat32 (0.0f); + axisParam[1].m_timestep = params.m_timestep; + axisParam[1].m_minFriction = DG_MIN_BOUND; + axisParam[1].m_maxFriction = DG_MAX_BOUND; + + code = m_jointAccelFnt (*this, axisParam); + if (code & 1) { + if ((axisParam[0].m_minFriction > DG_MIN_BOUND) || (axisParam[0].m_maxFriction < DG_MAX_BOUND)) { + params.m_forceBounds[ret].m_low = axisParam[0].m_minFriction; + params.m_forceBounds[ret].m_upper = axisParam[0].m_maxFriction; + params.m_forceBounds[ret].m_normalIndex = DG_INDEPENDENT_ROW; + } + +// CalculatePointDerivative (ret, params, dir0, pointDataP, &m_jointForce[ret]); + CalculateAngularDerivative (ret, params, dir0, m_defualtDiagonalRegularizer, dgFloat32 (0.0f), &m_jointForce[ret]); + SetMotorAcceleration (ret, axisParam[0].m_accel, params); + ret ++; + } + + if (code & 2) { + if ((axisParam[1].m_minFriction > DG_MIN_BOUND) || (axisParam[1].m_maxFriction < DG_MAX_BOUND)) { + params.m_forceBounds[ret].m_low = axisParam[1].m_minFriction; + params.m_forceBounds[ret].m_upper = axisParam[1].m_maxFriction; + params.m_forceBounds[ret].m_normalIndex = DG_INDEPENDENT_ROW; + } + CalculateAngularDerivative (ret, params, dir1, m_defualtDiagonalRegularizer, dgFloat32 (0.0f), &m_jointForce[ret]); + SetMotorAcceleration (ret, axisParam[1].m_accel, params); + ret ++; + + } + } + return dgUnsigned32 (ret); +} + + + diff --git a/thirdparty/src/newton/dgPhysics/dgUniversalConstraint.h b/thirdparty/src/newton/dgPhysics/dgUniversalConstraint.h new file mode 100644 index 000000000..96437aba6 --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgUniversalConstraint.h @@ -0,0 +1,76 @@ +/* Copyright (c) <2003-2019> +* +* 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_DGUNIVERSALCONSTRAINT_H__3FA456TR_INCLUDED) +#define AFX_DGUNIVERSALCONSTRAINT_H__3FA456TR_INCLUDED +#include "dgBilateralConstraint.h" + +//template class dgPool; + +class dgUniversalConstraint; + + +typedef dgUnsigned32 (dgApi *dgUniversalJointAcceleration) (const dgUniversalConstraint& hinge, dgJointCallbackParam* param); + +class dgUniversalConstraint: public dgBilateralConstraint +{ + public: + dgFloat32 GetJointAngle0 () const; + dgFloat32 GetJointAngle1 () const; + dgFloat32 GetJointOmega0 () const; + dgFloat32 GetJointOmega1 () const; + + dgVector GetJointForce () const; + dgFloat32 CalculateStopAlpha0 (dgFloat32 angle, const dgJointCallbackParam* param) const; + dgFloat32 CalculateStopAlpha1 (dgFloat32 angle, const dgJointCallbackParam* param) const; + void SetJointParameterCallback (dgUniversalJointAcceleration callback); + + private: + virtual dgUnsigned32 JacobianDerivative (dgContraintDescritor& params); + virtual void Serialize (dgSerialize serializeCallback, void* const userData) {dgAssert (0);} + + dgUniversalConstraint(); + virtual ~dgUniversalConstraint(); + + dgMatrix m_localMatrix0; + dgMatrix m_localMatrix1; + + dgFloat32 m_angle0; + dgFloat32 m_angle1; + dgUniversalJointAcceleration m_jointAccelFnt; + +//#ifdef _NEWTON_USE_DOUBLE +// dgUnsigned32 m_reserve[3]; +//#else +// dgUnsigned32 m_reserve[1]; +//#endif + + + friend class dgWorld; +// friend class dgPool; +}; + +//class dgUniversalConstraintArray: public dgPoolContainer +//{ +//}; + +#endif // !defined(AFX_DGUNIVERSALCONSTRAINT_H__3FA456TR_INCLUDED) + diff --git a/thirdparty/src/newton/dgPhysics/dgUpVectorConstraint.cpp b/thirdparty/src/newton/dgPhysics/dgUpVectorConstraint.cpp new file mode 100644 index 000000000..b0f6f4bf6 --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgUpVectorConstraint.cpp @@ -0,0 +1,106 @@ +/* Copyright (c) <2003-2019> +* +* 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 "dgPhysicsStdafx.h" +#include "dgBody.h" +#include "dgWorld.h" +#include "dgUpVectorConstraint.h" + + +////////////////////////////////////////////////////////////////////// +// Construction/Destruction +////////////////////////////////////////////////////////////////////// + +dgUpVectorConstraint::dgUpVectorConstraint () + :dgBilateralConstraint() +{ + dgAssert ( dgInt32 (sizeof (dgUpVectorConstraint) & 15) == 0); + dgAssert ((((dgUnsigned64) &m_localMatrix0) & 15) == 0); + + m_localMatrix0 = dgGetIdentityMatrix(); + m_localMatrix1 = dgGetIdentityMatrix(); + + SetStiffness (dgFloat32 (0.0f)); + m_maxDOF = 2; + m_constId = m_upVectorConstraint; + m_callBack = NULL; +} + +dgUpVectorConstraint::~dgUpVectorConstraint () +{ +} + +void dgUpVectorConstraint::InitPinDir (const dgVector& pin) +{ + + const dgMatrix& matrix = m_body0->GetMatrix(); + + dgVector pivot (matrix.m_posit); + SetPivotAndPinDir (pivot, pin, m_localMatrix0, m_localMatrix1); + +} + +void dgUpVectorConstraint::SetPinDir (const dgVector& pin) +{ + m_localMatrix1 = dgMatrix (pin); +} + +dgVector dgUpVectorConstraint::GetPinDir () const +{ + return m_localMatrix1.m_front; +} + + +void dgUpVectorConstraint::SetJointParameterCallback (dgUpVectorJointCallback callback) +{ + m_callBack = callback; +} + + +dgUnsigned32 dgUpVectorConstraint::JacobianDerivative (dgContraintDescritor& params) +{ + dgMatrix matrix0; + dgMatrix matrix1; + CalculateGlobalMatrixAndAngle (m_localMatrix0, m_localMatrix1, matrix0, matrix1); + + dgVector lateralDir (matrix0.m_front.CrossProduct(matrix1.m_front)); + + dgInt32 ret = 0; + dgFloat32 mag = lateralDir.DotProduct(lateralDir).GetScalar(); + if (mag > dgFloat32 (1.0e-6f)) { + mag = dgSqrt (mag); + lateralDir = lateralDir.Scale (dgFloat32 (1.0f) / mag); + dgFloat32 angle = dgAsin (mag); + CalculateAngularDerivative (0, params, lateralDir, m_defualtDiagonalRegularizer, angle, &m_jointForce[0]); + + dgVector frontDir (lateralDir.CrossProduct(matrix1.m_front)); + CalculateAngularDerivative (1, params, frontDir, m_defualtDiagonalRegularizer, dgFloat32 (0.0f), &m_jointForce[1]); + ret = 2; + } else { + CalculateAngularDerivative (0, params, matrix0.m_up, m_defualtDiagonalRegularizer, 0.0, &m_jointForce[0]); + CalculateAngularDerivative (1, params, matrix0.m_right, m_defualtDiagonalRegularizer, dgFloat32 (0.0f), &m_jointForce[1]); + ret = 2; + } + return dgUnsigned32 (ret); +} + + + diff --git a/thirdparty/src/newton/dgPhysics/dgUpVectorConstraint.h b/thirdparty/src/newton/dgPhysics/dgUpVectorConstraint.h new file mode 100644 index 000000000..cc2923d81 --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgUpVectorConstraint.h @@ -0,0 +1,65 @@ +/* Copyright (c) <2003-2019> +* +* 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 __DGUPVECTORCONSTRAINT__ +#define __DGUPVECTORCONSTRAINT__ +#include "dgBilateralConstraint.h" + +//template class dgPool; + +class dgUpVectorConstraint; + +typedef dgUnsigned32 (dgApi *dgUpVectorJointCallback) (const dgUpVectorConstraint& upVector); + +class dgUpVectorConstraint: public dgBilateralConstraint +{ + public: + void SetJointParameterCallback (dgUpVectorJointCallback callback); + void InitPinDir (const dgVector& pin); + + dgVector GetPinDir () const; + void SetPinDir (const dgVector& pin); + + private: + dgUpVectorConstraint(); + virtual ~dgUpVectorConstraint(); + + virtual dgUnsigned32 JacobianDerivative (dgContraintDescritor& params); + virtual void Serialize (dgSerialize serializeCallback, void* const userData) {dgAssert (0);} + + dgUpVectorJointCallback m_callBack; + + dgMatrix m_localMatrix0; + dgMatrix m_localMatrix1; +// dgUnsigned32 m_reserve[3]; + + friend class dgWorld; +// friend class dgPool; +}; + +/* +class dgUpVectorConstraintArray: public dgPoolContainer +{ +}; +*/ + +#endif // !defined(__DGUPVECTORCONSTRAINT_563GFT35684GT_H) + diff --git a/thirdparty/src/newton/dgPhysics/dgUserConstraint.cpp b/thirdparty/src/newton/dgPhysics/dgUserConstraint.cpp new file mode 100644 index 000000000..4c8d6a079 --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgUserConstraint.cpp @@ -0,0 +1,54 @@ +/* Copyright (c) <2003-2019> +* +* 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 "dgPhysicsStdafx.h" +#include "dgWorld.h" +#include "dgUserConstraint.h" + +////////////////////////////////////////////////////////////////////// +// Construction/Destruction +////////////////////////////////////////////////////////////////////// + +dgUserConstraint::dgUserConstraint(dgWorld* const world, dgBody* const body0, dgBody* const body1, dgInt32 constraintID) + :dgBilateralConstraint() +{ + m_maxDOF = 6; + m_enableCollision = false; + m_constId = dgUnsigned32 (m_unknownConstraint + constraintID); + m_body0 = body0; + m_body1 = body1; + m_userData = NULL; + m_destructor = NULL; + if (world) { + world->AttachConstraint(this, body0, body1); + } +} + +dgUserConstraint::~dgUserConstraint() +{ +} + +void dgUserConstraint::GetInfo (dgConstraintInfo* const info) const +{ + InitInfo (info); +} + + diff --git a/thirdparty/src/newton/dgPhysics/dgUserConstraint.h b/thirdparty/src/newton/dgPhysics/dgUserConstraint.h new file mode 100644 index 000000000..d1b516628 --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgUserConstraint.h @@ -0,0 +1,45 @@ +/* Copyright (c) <2003-2019> +* +* 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_DGUSERCONSTRAINT_H__97A0E0E3_C511_4761_9346_BAEC6E6322BA_H) +#define AFX_DGUSERCONSTRAINT_H__97A0E0E3_C511_4761_9346_BAEC6E6322BA_H + + +#include "dgBilateralConstraint.h" + +//class dgWorld; + +class dgUserConstraint: public dgBilateralConstraint +{ + public: + + protected: + virtual dgUnsigned32 JacobianDerivative (dgContraintDescritor& params) = 0; + virtual void Serialize (dgSerialize serializeCallback, void* const userData) = 0; + dgUserConstraint (dgWorld* const manager, dgBody* const dyn0, dgBody* const dyn1, dgInt32 constraintID); + + virtual void GetInfo (dgConstraintInfo* const info) const; + + virtual ~dgUserConstraint(); +}; + +#endif // !defined(AFX_DGUSERCONSTRAINT_H__97A0E0E3_C511_4761_9346_BAEC6E6322BA_H) + diff --git a/thirdparty/src/newton/dgPhysics/dgWorld.cpp b/thirdparty/src/newton/dgPhysics/dgWorld.cpp new file mode 100644 index 000000000..9074b2aa3 --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgWorld.cpp @@ -0,0 +1,1859 @@ +/* Copyright (c) <2003-2019> +* +* 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 "dgPhysicsStdafx.h" +#include "dgWorld.h" + +#include "dgDynamicBody.h" +#include "dgKinematicBody.h" +#include "dgCollisionBox.h" +#include "dgKinematicBody.h" +#include "dgCollisionNull.h" +#include "dgCollisionCone.h" +#include "dgCollisionScene.h" +#include "dgCollisionSphere.h" +#include "dgBroadPhaseMixed.h" +#include "dgCollisionCapsule.h" +#include "dgCollisionInstance.h" +#include "dgCollisionCompound.h" +#include "dgWorldDynamicUpdate.h" +#include "dgCollisionConvexHull.h" +#include "dgBroadPhaseSegregated.h" +#include "dgCollisionChamferCylinder.h" + +#include "dgUserConstraint.h" +#include "dgBallConstraint.h" +#include "dgHingeConstraint.h" +#include "dgSkeletonContainer.h" +#include "dgSlidingConstraint.h" +#include "dgUpVectorConstraint.h" +#include "dgUniversalConstraint.h" +#include "dgCorkscrewConstraint.h" + +#define DG_DEFAULT_SOLVER_ITERATION_COUNT 4 +#define DG_SYNC_THREAD 1 +#define DG_ASYNC_THREAD 2 + +/* +static dgInt32 TestSort(const dgInt32* const A, const dgInt32* const B, void* const) +{ + if (*A < *B) { + return -1; + } else if (*A > *B) { + return 1; + } + return 0; +} + +static void TestSort() +{ + int array[] = { 10, 10, 10, 10, 10, 5, 10, 10, 10, 10, 10, 10 }; + dgSort(array, sizeof(array) / sizeof(array[0]), TestSort); +} + + +static char *xxx[10] = {"bbbbbbbbbb", + "baaaaaaaab", + "babbbbbaab", + "babaaabaab", + "baaababaab", + "bbbaaabaab", + "bbbbaabaab", + "babbbbbaab", + "baaaaaaaab", + "bbbbbbbbbb"}; + +struct myPath: public dgPathFinder +{ + dgInt32 goalx; + dgInt32 goaly; + myPath () + :dgPathFinder(1024, 10) + { + for (int i = 0; i < 10; i ++) { + strcpy (&m_map[i][0], xxx[i]); + } + } + + const dgPathNode* CalCulatePath (dgInt32 source, dgInt32 goal) + { + goalx = goal % 10; + goaly = goal / 10; + return dgPathFinder::CalCulatePath (source, goal) ; + } + + + float GetCostFromParent(const dgPathNode& node) const + { + dgInt32 x; + dgInt32 y; + dgInt32 x0; + dgInt32 y0; + dgInt32 x1; + dgInt32 y1; + dgInt32 id; + + id = node.GetId(); + x = id % 10; + y = id / 10; + + const dgPathNode* parent = node.GetParent(); + id = parent->GetId(); + x0 = id % 10; + y0 = id / 10; + + const dgPathNode* grandParent = parent->GetParent(); + x1 = 2 * x0 - x; + y1 = 2 * y0 - y; + if (grandParent) { + id = grandParent->GetId(); + x1 = id % 10; + y1 = id / 10; + } + + dgInt32 dx0; + dgInt32 dy0; + dgInt32 dx1; + dgInt32 dy1; + float penalty; + + dx0 = x0 - x; + dy0 = y0 - y; + dx1 = x1 - x0; + dy1 = y1 - y0; + penalty = 0.0f; + if (dx1 * dy0 - dx0 * dy1) { + penalty = dgFloat32(1.0f); + } + + static dgInt32 xxxx; + if (!xxxx){ + xxxx = 1; + penalty = 9.1f; + } + + return (xxx[y][x] == 'a') ? (dgFloat32(1.0f) + penalty): 50.0f; + } + + float GetEstimatedCostToGoal(dgInt32 id) const + { + dgInt32 x; + dgInt32 y; + + x = id % 10 - goalx; + y = id / 10 - goaly; + return dgSqrt ((float)(x * x + y * y)); + } + + dgInt32 EnumerateChildren(dgInt32 parent, dgInt32 array[]) const + { + dgInt32 x; + dgInt32 y; + + x = parent % 10; + y = parent / 10; + + array[0] = (y - 1) * 10 + x; + array[1] = (y - 0) * 10 + x - 1; + array[2] = (y + 1) * 10 + x; + array[3] = (y + 0) * 10 + x + 1; + return 4; + } + + char m_map[20][20]; +}; + + +static void TestAStart() +{ + myPath path; + const dgPathNode* firtNode; + for (firtNode = path.CalCulatePath (5 * 10 + 3, 4 * 10 + 8); firtNode; firtNode= firtNode->GetNext()) { + dgInt32 id; + dgInt32 x; + dgInt32 y; + + id = firtNode->GetId(); + x = id % 10; + y = id / 10; + path.m_map[y][x] = '_'; + } +} + + +class TestGradient: public dgSymmetricConjugateGradientSolver +{ + public: + TestGradient() + :dgSymmetricConjugateGradientSolver() + { + dgFloat32 xxx[4]; + dgFloat32 b[4]; + xxx[0] = 1.0f; xxx[1] = 2.0f; xxx[2] = 3.0f;; xxx[3] = 4.0f; + dgCovarianceMatrix(4, &A[0][0], xxx, xxx); + A[0][0] = 10.0f; A[1][1] = 10.0f; A[2][2] = 10.0f; + dgAssert(dgTestPSDmatrix(4, 4, &A[0][0])); + dgMatrixTimeVector(4, &A[0][0], xxx, b); + + dgFloat32 x[4]; + x[0] = 0.0f; x[1] = 0.0f; x[2] = 0.0f;; x[3] = 0.0f; + Solve(4, 1.0e-6f, x, b); + } + + virtual void MatrixTimeVector(dgFloat32* const out, const dgFloat32* const v) const + { + dgMatrixTimeVector(4, &A[0][0], v, out); + } + + virtual void InversePrecoditionerTimeVector(dgFloat32* const out, const dgFloat32* const v) const + { + for (int i = 0; i < 4; i++) { + out[i] = v[i] / A[i][i]; + } + } + + + dgFloat32 A[4][4]; +}; +*/ + +////////////////////////////////////////////////////////////////////// +// Construction/Destruction +////////////////////////////////////////////////////////////////////// + +dgWorld::dgWorld(dgMemoryAllocator* const allocator) + :dgBodyMasterList(allocator) + ,dgBodyMaterialList(allocator) + ,dgBodyCollisionList(allocator) + ,dgSkeletonList(allocator) + ,dgContactList(allocator) + ,dgBilateralConstraintList(allocator) + ,dgWorldDynamicUpdate(allocator) + ,dgMutexThread("newtonMainThread", DG_SYNC_THREAD) + ,dgAsyncThread("newtonAsyncThread", DG_ASYNC_THREAD) + ,dgWorldThreadPool(allocator) + ,dgDeadBodies(allocator) + ,dgDeadJoints(allocator) + ,dgWorldPluginList(allocator) + ,m_broadPhase(NULL) + ,m_sentinelBody(NULL) + ,m_pointCollision(NULL) + ,m_userData(NULL) + ,m_allocator (allocator) + ,m_onPostUpdateCallback(NULL) + ,m_listeners(allocator) + ,m_perInstanceData(allocator) + ,m_bodiesMemory (allocator, 64) + ,m_jointsMemory (allocator, 64) + ,m_clusterMemory (allocator, 64) + ,m_solverJacobiansMemory (allocator, 64) + ,m_solverRightHandSideMemory (allocator, 64) + ,m_solverForceAccumulatorMemory (allocator, 64) +{ + //TestAStart(); + //TestSort(); + //TestGradient(); + + dgMutexThread* const myThread = this; + SetParentThread (myThread); + + // avoid small memory fragmentations on initialization + m_bodiesMemory.Resize(1024); + m_clusterMemory.Resize(1024); + m_jointsMemory.Resize(1024 * 2); + m_solverJacobiansMemory.Resize(1024 * 64); + m_solverRightHandSideMemory.Resize(1024 * 64); + m_solverForceAccumulatorMemory.Resize(1024 * 32); + + m_savetimestep = dgFloat32 (0.0f); + m_allocator = allocator; + + m_onCollisionInstanceDestruction = NULL; + m_onCollisionInstanceCopyConstrutor = NULL; + + m_onSerializeJointCallback = NULL; + m_onDeserializeJointCallback = NULL; + + m_inUpdate = 0; + m_clusterLRU = 0; + m_bodyGroupID = 0; + m_frameNumber = 0; + m_dynamicsLru = 0; + m_genericLRUMark = 0; + m_bodiesUniqueID = 0; + m_numberOfSubsteps = 1; + m_useParallelSolver = 1; + m_lastExecutionTime = 0; + m_defualtBodyGroupID = CreateBodyGroupID(); + m_solverIterations = DG_DEFAULT_SOLVER_ITERATION_COUNT; + + m_frictiomTheshold = dgFloat32 (0.25f); + + m_userData = NULL; + m_onClusterUpdate = NULL; + m_onCreateContact = NULL; + m_onDestroyContact = NULL; + + + m_freezeAccel2 = DG_FREEZE_ACCEL2; + m_freezeAlpha2 = DG_FREEZE_ACCEL2; + m_freezeSpeed2 = DG_FREEZE_SPEED2; + m_freezeOmega2 = DG_FREEZE_SPEED2; + + m_contactTolerance = DG_PRUNE_CONTACT_TOLERANCE; + + dgInt32 steps = 1; + dgFloat32 freezeAccel2 = m_freezeAccel2; + dgFloat32 freezeAlpha2 = m_freezeAlpha2; + dgFloat32 freezeSpeed2 = m_freezeSpeed2; + dgFloat32 freezeOmega2 = m_freezeOmega2; + for (dgInt32 i = 0; i < DG_SLEEP_ENTRIES; i ++) { + m_sleepTable[i].m_maxAccel = freezeAccel2; + m_sleepTable[i].m_maxAlpha = freezeAlpha2; + m_sleepTable[i].m_maxVeloc = freezeSpeed2; + m_sleepTable[i].m_maxOmega = freezeOmega2; + m_sleepTable[i].m_steps = steps; + steps += 7; + freezeAccel2 *= dgFloat32 (1.5f); + freezeAlpha2 *= dgFloat32 (1.5f); + freezeSpeed2 *= dgFloat32 (1.5f); + freezeOmega2 *= dgFloat32 (1.5f); + } + + m_sleepTable[0].m_maxAccel *= dgFloat32(0.009f); + m_sleepTable[0].m_maxAlpha *= dgFloat32(0.009f); + + steps += 300; + m_sleepTable[DG_SLEEP_ENTRIES - 1].m_maxAccel *= dgFloat32 (100.0f); + m_sleepTable[DG_SLEEP_ENTRIES - 1].m_maxAlpha *= dgFloat32 (100.0f); + m_sleepTable[DG_SLEEP_ENTRIES - 1].m_maxVeloc = 0.25f; + m_sleepTable[DG_SLEEP_ENTRIES - 1].m_maxOmega = 0.1f; + m_sleepTable[DG_SLEEP_ENTRIES - 1].m_steps = steps; + + SetThreadsCount (0); + + m_broadPhase = new (allocator) dgBroadPhaseMixed(this); + //m_broadPhase = new (allocator) dgBroadPhaseSegregated (this); + + //m_pointCollision = new (m_allocator) dgCollisionPoint(m_allocator); + dgCollision* const pointCollison = new (m_allocator) dgCollisionPoint(m_allocator); + m_pointCollision = CreateInstance(pointCollison, 0, dgGetIdentityMatrix()); + pointCollison->Release(); + + AddSentinelBody(); +} + +dgWorld::~dgWorld() +{ + Sync(); + dgAsyncThread::Terminate(); + dgMutexThread::Terminate(); + + UnloadPlugins(); + m_listeners.RemoveAll(); + + DestroyAllBodies(); + RemoveAllGroupID(); + m_pointCollision->Release(); + DestroyBody (m_sentinelBody); + + dgDeadBodies& bodyList = *this; + dgDeadJoints& jointList = *this; + + jointList.DestroyJoints(*this); + bodyList.DestroyBodies(*this); + + delete m_broadPhase; +} + +void dgWorld::DestroyAllBodies() +{ + dgBodyMasterList& me = *this; + + Sync(); + + dgSkeletonList& skelList = *this; + dgSkeletonList::Iterator iter(skelList); + for (iter.Begin(); iter; iter++) { + dgSkeletonContainer* const skeleton = iter.GetNode()->GetInfo(); + delete skeleton; + } + skelList.RemoveAll(); + + while (m_disableBodies.GetRoot()) { + dgBody* const body = m_disableBodies.GetRoot()->GetKey(); + BodyEnableSimulation(body); + } + + dgAssert(dgBodyMasterList::GetFirst()->GetInfo().GetBody() == m_sentinelBody); + for (dgBodyMasterList::dgListNode* node = me.GetFirst()->GetNext(); node;) { + dgBody* const body = node->GetInfo().GetBody(); + node = node->GetNext(); + DestroyBody(body); + } + + dgDeadBodies& bodyList = *this; + dgDeadJoints& jointList = *this; + + jointList.DestroyJoints(*this); + bodyList.DestroyBodies(*this); + + dgAssert(me.GetFirst()->GetInfo().GetCount() == 0); + dgAssert(dgBodyCollisionList::GetCount() == 0); +} + +void dgWorld::SetThreadsCount (dgInt32 count) +{ + dgThreadHive::SetThreadsCount(count); +} + +dgUnsigned32 dgWorld::GetPerformanceCount () +{ + return 0; +} + +void dgWorld::AddSentinelBody() +{ + dgCollision* const collision = new (m_allocator) dgCollisionNull (m_allocator, 0x4352fe67); + dgCollisionInstance* const instance = CreateInstance(collision, 0, dgGetIdentityMatrix()); + collision->Release(); + m_sentinelBody = CreateDynamicBody(instance, dgGetIdentityMatrix()); + instance->Release(); +} + +dgDynamicBody* dgWorld::GetSentinelBody() const +{ + return m_sentinelBody; +} + + +dgFloat32 dgWorld::GetContactMergeTolerance() const +{ + return m_contactTolerance; +} + +void dgWorld::SetContactMergeTolerance(dgFloat32 tolerenace) +{ + m_contactTolerance = dgMax (tolerenace, dgFloat32 (1.e-3f)); +} + +void dgWorld::EnableParallelSolverOnLargeIsland(dgInt32 mode) +{ + m_useParallelSolver = mode ? 1 : 0; +} + +dgInt32 dgWorld::GetParallelSolverOnLargeIsland() const +{ + return m_useParallelSolver ? 1 : 0; +} + + +void dgWorld::SetFrictionThreshold (dgFloat32 acceleration) +{ + m_frictiomTheshold = dgMax (dgFloat32(1.0e-2f), acceleration); +} + + +void dgWorld::RemoveAllGroupID() +{ + while (dgBodyMaterialList::GetCount()) { + dgBodyMaterialList::Remove (dgBodyMaterialList::GetRoot()); + } + m_bodyGroupID = 0; + m_defualtBodyGroupID = CreateBodyGroupID(); +} + +void dgWorld::InitBody (dgBody* const body, dgCollisionInstance* const collision, const dgMatrix& matrix) +{ + dgAssert (collision); + + m_bodiesUniqueID ++; + body->m_world = this; + + body->m_spawnnedFromCallback = dgUnsigned32 (m_inUpdate ? true : false); + body->m_uniqueID = dgInt32 (m_bodiesUniqueID); + + dgBodyMasterList::AddBody(body); + + body->SetLinearDamping(dgFloat32(0.1045f)); + body->SetCentreOfMass (dgVector (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (1.0f))); + body->SetAngularDamping (dgVector (dgFloat32 (0.1045f), dgFloat32 (0.1045f), dgFloat32 (0.1045f), dgFloat32 (0.0f))); + body->AttachCollision(collision); + body->m_bodyGroupId = dgInt32 (m_defualtBodyGroupID); + + dgMatrix inertia(dgGetIdentityMatrix()); + if (!body->GetCollision()->IsType(dgCollision::dgCollisionLumpedMass_RTTI)) { + inertia[0][0] = DG_INFINITE_MASS; + inertia[1][1] = DG_INFINITE_MASS; + inertia[2][2] = DG_INFINITE_MASS; + body->SetMassMatrix(DG_INFINITE_MASS * dgFloat32(2.0f), inertia); + } else { + dgBodyMasterList::RemoveBody(body); + body->UpdateLumpedMatrix(); + dgBodyMasterList::AddBody(body); + inertia[0][0] = body->m_mass.m_x; + inertia[1][1] = body->m_mass.m_y; + inertia[2][2] = body->m_mass.m_z; + body->SetMassMatrix(body->m_mass.m_w, inertia); + } + body->SetMatrix(matrix); + if (!body->GetCollision()->IsType (dgCollision::dgCollisionNull_RTTI)) { + m_broadPhase->Add (body); + } +} + +void dgWorld::BodyEnableSimulation (dgBody* const body) +{ + if (!body->m_masterNode) { + m_disableBodies.Remove(body); + dgBodyMasterList::AddBody(body); + body->SetMassMatrix(body->m_mass.m_w, body->CalculateLocalInertiaMatrix()); + m_broadPhase->Add (body); + dgAssert (body->m_masterNode); + } +} + +void dgWorld::BodyDisableSimulation(dgBody* const body) +{ + if (body->m_masterNode) { + m_broadPhase->Remove(body); + dgBodyMasterList::RemoveBody(body); + m_disableBodies.Insert(0, body); + dgAssert (!body->m_masterNode); + } +} + +bool dgWorld::GetBodyEnableDisableSimulationState (dgBody* const body) const +{ + return body->m_masterNode ? true : false; +} + +dgDynamicBody* dgWorld::CreateDynamicBody(dgCollisionInstance* const collision, const dgMatrix& matrix) +{ + dgDynamicBody* const body = new (m_allocator) dgDynamicBody(); + dgAssert (dgInt32 (sizeof (dgBody) & 0xf) == 0); + dgAssert ((dgUnsigned64 (body) & 0xf) == 0); + + InitBody (body, collision, matrix); + return body; +} + +dgDynamicBody* dgWorld::CreateDynamicBodyAsymetric(dgCollisionInstance* const collision, const dgMatrix& matrix) +{ + dgDynamicBody* const body = new (m_allocator) dgDynamicBodyAsymetric(); + dgAssert(dgInt32(sizeof(dgBody) & 0xf) == 0); + dgAssert((dgUnsigned64(body) & 0xf) == 0); + + InitBody(body, collision, matrix); + return body; +} + +dgKinematicBody* dgWorld::CreateKinematicBody (dgCollisionInstance* const collision, const dgMatrix& matrix) +{ + dgKinematicBody* const body = new (m_allocator) dgKinematicBody(); + dgAssert (dgInt32 (sizeof (dgBody) & 0xf) == 0); + dgAssert ((dgUnsigned64 (body) & 0xf) == 0); + + InitBody (body, collision, matrix); + return body; +} + + +void dgWorld::DestroyBody(dgBody* const body) +{ + for (dgListenerList::dgListNode* node = m_listeners.GetLast(); node; node = node->GetPrev()) { + dgListener& listener = node->GetInfo(); + if (listener.m_onBodyDestroy) { + listener.m_onBodyDestroy(this, node, body); + } + } + + dgDeadBodies& deadBodyList = *this; + deadBodyList.DestroyBody(body); +} + +void dgWorld::DestroyConstraint(dgConstraint* const constraint) +{ + RemoveConstraint (constraint); + delete constraint; +} + +void dgWorld::ExecuteUserJob (dgWorkerThreadTaskCallback userJobKernel, void* const userJobKernelContext, const char* const functionName) +{ + QueueJob (userJobKernel, this, userJobKernelContext, functionName); +} + +void dgWorld::SetUserData (void* const userData) +{ + m_userData = userData; +} + +void* dgWorld::GetUserData() const +{ + return m_userData; +} + +void dgWorld::SetIslandUpdateCallback (OnClusterUpdate callback) +{ + m_onClusterUpdate = callback; +} + +void dgWorld::SetCreateDestroyContactCallback(OnCreateContact createContactCallback, OnDestroyContact destroyContactCallback) +{ + m_onCreateContact = createContactCallback; + m_onDestroyContact = destroyContactCallback; +} + +void* dgWorld::AddListener (const char* const nameid, void* const userData) +{ + dgListenerList::dgListNode* const node = m_listeners.Append(); + dgListener& listener = node->GetInfo(); + strncpy (listener.m_name, nameid, sizeof (listener.m_name)); + listener.m_world = this; + listener.m_userData = userData; + return node; +} + +void dgWorld::ListenerSetDestroyCallback(void* const listenerNode, OnListenerDestroyCallback destroyCallback) +{ + dgListener& listener = ((dgListenerList::dgListNode*) listenerNode)->GetInfo(); + listener.m_onListenerDestroy = destroyCallback; +} + +void dgWorld::ListenerSetPreUpdate(void* const listenerNode, OnListenerUpdateCallback updateCallback) +{ + dgListener& listener = ((dgListenerList::dgListNode*) listenerNode)->GetInfo(); + listener.m_onPreUpdate = updateCallback; +} + +void dgWorld::ListenerSetPostUpdate(void* const listenerNode, OnListenerUpdateCallback updateCallback) +{ + dgListener& listener = ((dgListenerList::dgListNode*) listenerNode)->GetInfo(); + listener.m_onPostUpdate = updateCallback; +} + +void dgWorld::ListenerSetPostStep(void* const listenerNode, OnListenerUpdateCallback updateCallback) +{ + dgListener& listener = ((dgListenerList::dgListNode*) listenerNode)->GetInfo(); + listener.m_onPostStep = updateCallback; +} + +void* dgWorld::GetListenerUserData (void* const listenerNode) const +{ + dgListener& listener = ((dgListenerList::dgListNode*) listenerNode)->GetInfo(); + return listener.m_userData; +} + +void dgWorld::SetListenerBodyDestroyCallback (void* const listenerNode, OnListenerBodyDestroyCallback callback) +{ + dgListener& listener = ((dgListenerList::dgListNode*) listenerNode)->GetInfo(); + listener.m_onBodyDestroy = callback; +} + +void dgWorld::SetListenerBodyDebugCallback (void* const listenerNode, OnListenerDebugCallback callback) +{ + dgListener& listener = ((dgListenerList::dgListNode*) listenerNode)->GetInfo(); + listener.m_onDebugCallback = callback; +} + +dgWorld::OnListenerBodyDestroyCallback dgWorld::GetListenerBodyDestroyCallback (void* const listenerNode) const +{ + dgListener& listener = ((dgListenerList::dgListNode*) listenerNode)->GetInfo(); + return listener.m_onBodyDestroy; +} + +void dgWorld::ListenersDebug(void* const debugContext) +{ + for (dgListenerList::dgListNode* node = m_listeners.GetFirst(); node; node = node->GetNext()) { + dgListener& listener = node->GetInfo(); + if (listener.m_onDebugCallback) { + listener.m_onDebugCallback (this, listener.m_userData, debugContext); + } + } +} + +void* dgWorld::FindListener (const char* const nameid) const +{ + for (dgListenerList::dgListNode* node = m_listeners.GetFirst(); node; node = node->GetNext()) { + dgListener& listener = node->GetInfo(); + if (!strcmp (nameid, listener.m_name)) { + return node; + } + } + return NULL; +} + + +dgBallConstraint* dgWorld::CreateBallConstraint ( + const dgVector& pivot, + dgBody* const body0, + dgBody* const body1) +{ + + dgAssert (body0); + dgAssert (body0 != body1); + dgBallConstraint* const constraint = new (m_allocator) dgBallConstraint; + + AttachConstraint (constraint, body0, body1); + constraint->SetPivotPoint (pivot); + return constraint; +} + + +dgHingeConstraint* dgWorld::CreateHingeConstraint ( + const dgVector& pivot, + const dgVector& pinDir, + dgBody* const body0, + dgBody* const body1) +{ + dgAssert (body0); + dgAssert (body0 != body1); + dgHingeConstraint* const constraint = new (m_allocator) dgHingeConstraint; + + AttachConstraint (constraint, body0, body1); + constraint->SetPivotAndPinDir (pivot, pinDir, constraint->m_localMatrix0, constraint->m_localMatrix1); + return constraint; +} + + +dgUpVectorConstraint* dgWorld::CreateUpVectorConstraint (const dgVector& pin, dgBody *body) +{ + dgAssert (body); + dgUpVectorConstraint* const constraint = new (m_allocator) dgUpVectorConstraint; + + AttachConstraint (constraint, body, NULL); + constraint->InitPinDir (pin); + return constraint; +} + + + +dgSlidingConstraint* dgWorld::CreateSlidingConstraint ( + const dgVector& pivot, + const dgVector& pinDir, + dgBody* const body0, + dgBody* const body1) +{ + dgAssert (body0); + dgAssert (body0 != body1); + dgSlidingConstraint* const constraint = new (m_allocator) dgSlidingConstraint; + + AttachConstraint (constraint, body0, body1); + constraint->SetPivotAndPinDir (pivot, pinDir, constraint->m_localMatrix0, constraint->m_localMatrix1); + return constraint; +} + + +dgCorkscrewConstraint* dgWorld::CreateCorkscrewConstraint ( + const dgVector& pivot, + const dgVector& pinDir, + dgBody* const body0, + dgBody* const body1) +{ + dgAssert (body0); + dgAssert (body0 != body1); + dgCorkscrewConstraint* const constraint = new (m_allocator) dgCorkscrewConstraint; + + AttachConstraint (constraint, body0, body1); + constraint->SetPivotAndPinDir (pivot, pinDir, constraint->m_localMatrix0, constraint->m_localMatrix1); + return constraint; +} + + +dgUniversalConstraint* dgWorld::CreateUniversalConstraint ( + const dgVector& pivot, + const dgVector& pin0, + const dgVector& pin1, + dgBody* const body0, + dgBody* const body1) +{ + dgAssert (body0); + dgAssert (body0 != body1); + dgUniversalConstraint* const constraint = new (m_allocator) dgUniversalConstraint; + + AttachConstraint (constraint, body0, body1); + constraint->SetPivotAndPinDir(pivot, pin0, pin1, constraint->m_localMatrix0, constraint->m_localMatrix1); + return constraint; +} + +dgInt32 dgWorld::GetBodiesCount() const +{ + const dgBodyMasterList& list = *this; + return list.GetCount() - 1; +} + +dgInt32 dgWorld::GetConstraintsCount() const +{ + const dgBodyMasterList& list = *this; + return dgInt32 (list.m_constraintCount); +} + + +void dgWorld::BodySetMatrix (dgBody* const body, const dgMatrix& matrix) +{ + #define DG_RECURSIVE_SIZE 1024 + + dgBody* queue[DG_RECURSIVE_SIZE]; + + dgInt32 index = 1; + queue[0] = body; + m_genericLRUMark ++; + body->m_genericLRUMark = m_genericLRUMark; + dgMatrix relMatrix (body->GetMatrix().Inverse() * matrix); + while (index) { + index --; + dgBody* body1 = queue[index]; + dgAssert (body1 != m_sentinelBody); + + // why should I do this? I do no remember the reason + //m_broadPhase->Remove (body); + //m_broadPhase->Add (body); + + dgMatrix matrix1 (body1->GetMatrix() * relMatrix); + //body1->SetOmega (dgVector (dgFloat32 (0.0f))); + //body1->SetVelocity (dgVector (dgFloat32 (0.0f))); + body1->SetOmega (matrix1.RotateVector (body1->GetOmega())); + body1->SetVelocity (matrix1.RotateVector (body1->GetVelocity())); + + body1->SetMatrix (matrix1); + body1->UpdateCollisionMatrix (dgFloat32 (0.0f), 0); + body1->SetSleepState(false); + + for (dgBodyMasterListRow::dgListNode* jointNode = body1->m_masterNode->GetInfo().GetFirst(); jointNode; jointNode = jointNode->GetNext()) { + dgBodyMasterListCell& cell = jointNode->GetInfo(); + body1 = cell.m_bodyNode; + if (body1 != m_sentinelBody) { + if (body1->m_genericLRUMark != m_genericLRUMark) { + dgConstraint* constraint; + constraint = cell.m_joint; + if (constraint->GetId() != dgConstraint::m_contactConstraint) { + body1->m_genericLRUMark = m_genericLRUMark; + queue[index] = body1; + index ++; + dgAssert (index < DG_RECURSIVE_SIZE); + } + } + } + } + } +} + + +bool dgWorld::AreBodyConnectedByJoints (dgBody* const originSrc, dgBody* const targetSrc) +{ + #define DG_QEUEU_SIZE 1024 + dgBody* queue[DG_QEUEU_SIZE]; + + m_genericLRUMark ++; + + dgBody* origin1 = originSrc; + dgBody* target1 = targetSrc; + if (origin1->GetInvMass().m_w == dgFloat32 (0.0f)) { + dgSwap (origin1, target1); + } + + dgAssert (origin1->GetInvMass().m_w != dgFloat32 (0.0f)); + dgBody* const origin = origin1; + dgBody* const target = target1; + + dgInt32 end = 1; + dgInt32 start = 0; + queue[0] = origin; + origin->m_genericLRUMark = m_genericLRUMark; + + while (start != end) { + dgBody* const originVar = queue[start]; + start ++; + start &= (DG_QEUEU_SIZE - 1); + + for (dgBodyMasterListRow::dgListNode* jointNode = originVar->m_masterNode->GetInfo().GetFirst(); jointNode; jointNode = jointNode->GetNext()) { + dgBodyMasterListCell& cell = jointNode->GetInfo(); + + dgBody* const body = cell.m_bodyNode; + if (body->m_genericLRUMark != m_genericLRUMark) { + dgConstraint* const constraint = cell.m_joint; + if (constraint->GetId() != dgConstraint::m_contactConstraint) { + if (body == target) { + return true; + } + body->m_genericLRUMark = m_genericLRUMark; + queue[end] = body; + end ++; + end &= (DG_QEUEU_SIZE - 1); + } + } + } + } + return false; +} + +void dgWorld::FlushCache() +{ + // delete all contacts + dgContactList& contactList = *this; + for (dgInt32 i = contactList.m_contactCount - 1; i >= 0; i--) { + dgContact* const contact = contactList[i]; + contact->m_killContact = 1; + } + m_broadPhase->DeleteDeadContact(0.0f); + + // clean up memory in bradPhase + m_broadPhase->InvalidateCache (); + + // sort body list + SortMasterList(); +} + +void dgWorld::StepDynamics (dgFloat32 timestep) +{ + //SerializeToFile ("xxx.bin"); + + dgAssert (m_inUpdate == 0); + dgAssert (GetThreadCount() >= 1); + + m_inUpdate ++; + m_frameNumber ++; + + D_TRACKTIME(); + UpdateSkeletons(); + UpdateBroadphase(timestep); + UpdateDynamics (timestep); + + if (m_listeners.GetCount()) { + for (dgListenerList::dgListNode* node = m_listeners.GetFirst(); node; node = node->GetNext()) { + dgListener& listener = node->GetInfo(); + if (listener.m_onPostUpdate) { + listener.m_onPostUpdate(this, listener.m_userData, timestep); + } + } + } + + m_inUpdate --; +} + +void dgWorldThreadPool::OnBeginWorkerThread (dgInt32 threadId) +{ +} + +void dgWorldThreadPool::OnEndWorkerThread (dgInt32 threadId) +{ +} + +void dgWorld::Execute (dgInt32 threadID) +{ + dgMutexThread::Execute (threadID); +} + +void dgWorld::UpdateTransforms(dgBodyMasterList::dgListNode* node, dgInt32 threadID) +{ + const dgInt32 threadsCount = GetThreadCount(); + while (node) { + dgBody* const body = node->GetInfo().GetBody(); + if (body->m_transformIsDirty && body->m_matrixUpdate) { + body->m_matrixUpdate (*body, body->m_matrix, threadID); + } + body->m_transformIsDirty = false; + + for (dgInt32 i = 0; i < threadsCount; i++) { + node = node ? node->GetNext() : NULL; + } + } +} + +void dgWorld::UpdateTransforms(void* const context, void* const nodePtr, dgInt32 threadID) +{ + dgWorld* const world = (dgWorld*)context; + dgBodyMasterList::dgListNode* node = (dgBodyMasterList::dgListNode*) nodePtr; + world->UpdateTransforms(node, threadID); +} + +void dgWorld::RunStep () +{ + D_TRACKTIME(); + + BeginSection(); + dgUnsigned64 timeAcc = dgGetTimeInMicrosenconds(); + + dgFloat32 step = m_savetimestep / m_numberOfSubsteps; + for (dgUnsigned32 i = 0; i < m_numberOfSubsteps; i ++) { + StepDynamics (step); + + dgDeadBodies& bodyList = *this; + dgDeadJoints& jointList = *this; + + jointList.DestroyJoints (*this); + bodyList.DestroyBodies (*this); + } + + const dgBodyMasterList* const masterList = this; + dgBodyMasterList::dgListNode* threadNode = masterList->GetFirst(); + const dgInt32 threadsCount = GetThreadCount(); + for (dgInt32 i = 0; i < threadsCount; i++) { + QueueJob(UpdateTransforms, this, threadNode, "dgWorld::UpdateTransforms"); + threadNode = threadNode ? threadNode->GetNext() : NULL; + } + SynchronizationBarrier(); + + if (m_listeners.GetCount()) { + for (dgListenerList::dgListNode* node = m_listeners.GetFirst(); node; node = node->GetNext()) { + dgListener& listener = node->GetInfo(); + if (listener.m_onPostStep) { + listener.m_onPostStep(this, listener.m_userData, m_savetimestep); + } + } + } + + if (m_onPostUpdateCallback) { + m_onPostUpdateCallback (this, m_savetimestep); + } + + m_lastExecutionTime = (dgGetTimeInMicrosenconds() - timeAcc) * dgFloat32 (1.0e-6f); + EndSection(); +} + +void dgWorld::TickCallback(dgInt32 threadID) +{ + RunStep(); +} + +void dgWorld::Update (dgFloat32 timestep) +{ + m_savetimestep = timestep; + #ifdef DG_USE_THREAD_EMULATION + dgFloatExceptions exception; + dgSetPrecisionDouble precision; + RunStep (); + #else + // runs the update in a separate thread and wait until the update is completed before it returns. + // this will run well on single core systems, since the two thread are mutually exclusive + dgMutexThread::Tick(); + #endif +} + +void dgWorld::UpdateAsync (dgFloat32 timestep) +{ + + #ifdef DG_USE_THREAD_EMULATION + Update(timestep); + #else + m_savetimestep = timestep; + dgAsyncThread::Tick(); + //Update(timestep); + #endif +} + +void dgWorld::SetCollisionInstanceConstructorDestructor (OnCollisionInstanceDuplicate constructor, OnCollisionInstanceDestroy destructor) +{ + m_onCollisionInstanceDestruction = destructor; + m_onCollisionInstanceCopyConstrutor = constructor; +} + +dgInt32 dgWorld::GetBroadPhaseType() const +{ + return dgInt32 (m_broadPhase->GetType()); +} + +void dgWorld::SetBroadPhaseType(dgInt32 type) +{ + if (type != GetBroadPhaseType()) { + dgBroadPhase* newBroadPhase = NULL; + + switch (type) + { + case m_broadphaseSegregated: + newBroadPhase = new (m_allocator) dgBroadPhaseSegregated (this); + break; + + case m_broadphaseMixed: + default: + newBroadPhase = new (m_allocator) dgBroadPhaseMixed(this); + break; + } + + m_broadPhase->MoveNodes(newBroadPhase); + delete m_broadPhase; + m_broadPhase = newBroadPhase; + } +} + +void dgWorld::ResetBroadPhase() +{ + dgBroadPhase* newBroadPhase = NULL; + + switch (GetBroadPhaseType()) + { + case m_broadphaseSegregated: + newBroadPhase = new (m_allocator) dgBroadPhaseSegregated (this); + break; + + case m_broadphaseMixed: + default: + newBroadPhase = new (m_allocator) dgBroadPhaseMixed(this); + break; + } + + m_broadPhase->MoveNodes(newBroadPhase); + delete m_broadPhase; + m_broadPhase = newBroadPhase; +} + +dgContact* dgWorld::FindContactJoint (const dgBody* body0, const dgBody* body1) const +{ + dgAssert (m_broadPhase); + return m_broadPhase->m_contactCache.FindContactJoint(body0, body1); +} + +dgSkeletonContainer* dgWorld::CreateNewtonSkeletonContainer (dgBody* const rootBone) +{ + dgAssert (rootBone); + dgSkeletonList* const list = this; + dgAssert (rootBone->GetType() == dgBody::m_dynamicBody); + dgSkeletonContainer* const container = new (m_allocator) dgSkeletonContainer(this, (dgDynamicBody*)rootBone); + + container->m_listNode = list->Append(container); + return container; +} + +void dgWorld::DestroySkeletonContainer (dgSkeletonContainer* const container) +{ + dgSkeletonList* const list = this; + dgAssert(container->m_listNode); + list->Remove(container->m_listNode); + delete container; +} + +dgBroadPhaseAggregate* dgWorld::CreateAggreGate() const +{ + return m_broadPhase->CreateAggregate(); +} + +void dgWorld::DestroyAggregate(dgBroadPhaseAggregate* const aggregate) const +{ + m_broadPhase->DestroyAggregate((dgBroadPhaseAggregate*) aggregate); +} + +dgDeadJoints::dgDeadJoints(dgMemoryAllocator* const allocator) + :dgTree(allocator) + ,m_lock(0) +{ +} + +void dgDeadJoints::DestroyJoint(dgConstraint* const joint) +{ + dgScopeSpinLock lock(&m_lock); + dgWorld& me = *((dgWorld*)this); + me.DestroyConstraint(joint); +} + +void dgDeadJoints::DestroyJoints(dgWorld& world) +{ + dgScopeSpinLock lock(&m_lock); + Iterator iter (*this); + for (iter.Begin(); iter; iter++) { + dgTreeNode* const node = iter.GetNode(); + dgConstraint* const joint = node->GetInfo(); + world.DestroyConstraint (joint); + } + RemoveAll (); +} + +dgDeadBodies::dgDeadBodies(dgMemoryAllocator* const allocator) + :dgTree(allocator) + ,m_lock(0) +{ +} + +void dgDeadBodies::DestroyBody(dgBody* const body) +{ + if (body->m_destructor) { + body->m_destructor(*body); + } + body->m_isdead = 1; + body->SetDestructorCallback (NULL); + body->SetMatrixUpdateCallback (NULL); + body->SetExtForceAndTorqueCallback (NULL); + + for (dgBodyMasterListRow::dgListNode* node = body->GetMasterList()->GetInfo().GetLast(); node; node = node->GetPrev()) { + dgConstraint* const joint = node->GetInfo().m_joint; + if (joint && (joint->GetId() == dgConstraint::m_contactConstraint)) { + dgContact* const contactJoint = (dgContact*)joint; + contactJoint->m_killContact = 1; + } + } + + dgScopeSpinLock lock(&m_lock); + Insert (body, body); +} + +void dgDeadBodies::DestroyBodies(dgWorld& world) +{ +// dgScopeSpinLock lock(&m_lock); + if (GetCount()) { + world.m_broadPhase->DeleteDeadContact(0.0f); + Iterator iter(*this); + for (iter.Begin(); iter; iter++) { + dgTreeNode* const bodyNode = iter.GetNode(); + dgBody* const body = bodyNode->GetInfo(); + + if (world.m_disableBodies.Find(body)) { + world.m_disableBodies.Remove(body); + } else { + world.m_broadPhase->Remove(body); + world.dgBodyMasterList::RemoveBody(body); + } + + dgAssert(body->m_collision); + body->m_collision->Release(); + delete body; + } + RemoveAll(); + } +} + +void dgWorld::UpdateBroadphase(dgFloat32 timestep) +{ + m_broadPhase->UpdateContacts (timestep); +} + +#if 1 +dgInt32 dgWorld::CompareJointByInvMass(const dgBilateralConstraint* const jointA, const dgBilateralConstraint* const jointB, void* notUsed) +{ + dgInt32 modeA = jointA->m_solverModel; + dgInt32 modeB = jointB->m_solverModel; + + if (modeA < modeB) { + return -1; + } else if (modeA > modeB) { + return 1; + } else { + dgFloat32 invMassA = dgMin(jointA->GetBody0()->m_invMass.m_w, jointA->GetBody1()->m_invMass.m_w); + dgFloat32 invMassB = dgMin(jointB->GetBody0()->m_invMass.m_w, jointB->GetBody1()->m_invMass.m_w); + if (invMassA < invMassB) { + return -1; + } else if (invMassA > invMassB) { + return 1; + } + } + return 0; +} + +void dgWorld::UpdateSkeletons() +{ + D_TRACKTIME(); + dgSkeletonList& skelManager = *this; + + if (skelManager.m_skelListIsDirty) { + skelManager.m_skelListIsDirty = false; + dgSkeletonList::Iterator iter(skelManager); + for (iter.Begin(); iter; iter++) { + dgSkeletonContainer* const skeleton = iter.GetNode()->GetInfo(); + delete skeleton; + } + skelManager.RemoveAll(); + + m_dynamicsLru = m_dynamicsLru + 1; + dgUnsigned32 lru = m_dynamicsLru; + + dgBodyMasterList& masterList = *this; + m_solverJacobiansMemory.ResizeIfNecessary((2 * (masterList.m_constraintCount + 1024)) * sizeof (dgBilateralConstraint*)); + dgBilateralConstraint** const jointList = (dgBilateralConstraint**)&m_solverJacobiansMemory[0]; + + dgInt32 jointCount = 0; + for (dgBodyMasterList::dgListNode* node = masterList.GetFirst(); node; node = node->GetNext()) { + const dgBodyMasterListRow& graphNode = node->GetInfo(); + dgBody* const srcBody = graphNode.GetBody(); + + for (dgBodyMasterListRow::dgListNode* jointNode = srcBody->m_masterNode->GetInfo().GetLast(); jointNode; jointNode = jointNode->GetPrev()) { + dgBodyMasterListCell* const cell = &jointNode->GetInfo(); + dgConstraint* const constraint = cell->m_joint; + dgAssert(constraint); + dgAssert((constraint->m_body0 == srcBody) || (constraint->m_body1 == srcBody)); + dgAssert((constraint->m_body0 == cell->m_bodyNode) || (constraint->m_body1 == cell->m_bodyNode)); + bool test = constraint->IsBilateral(); + test = test && (constraint->m_solverModel < 2); + test = test && (constraint->m_dynamicsLru != lru); + test = test && (constraint->GetMassScaleBody0() == dgFloat32 (1.0f)); + test = test && (constraint->GetMassScaleBody1() == dgFloat32 (1.0f)); + //if (constraint->IsBilateral() && (constraint->m_solverModel < 2) && (constraint->m_dynamicsLru != lru)) { + if (test) { + constraint->m_dynamicsLru = lru; + jointList[jointCount] = (dgBilateralConstraint*)constraint; + jointCount++; + } + } + } + + dgSortIndirect(jointList, jointCount, CompareJointByInvMass); + + const dgInt32 poolSize = 1024 * 4; + dgBilateralConstraint* loopJoints[64]; + dgSkeletonContainer::dgNode* queuePool[poolSize]; + + m_dynamicsLru = m_dynamicsLru + 1; + lru = m_dynamicsLru; + for (dgInt32 i = 0; i < jointCount; i++) { + dgBilateralConstraint* const constraint = jointList[i]; + if (constraint->m_dynamicsLru != lru) { + dgQueue queue(queuePool, poolSize); + + dgInt32 loopCount = 0; + dgDynamicBody* const rootBody = (dgDynamicBody*)((constraint->GetBody0()->GetInvMass().m_w < constraint->GetBody1()->GetInvMass().m_w) ? constraint->GetBody0() : constraint->GetBody1()); + dgSkeletonContainer* const skeleton = CreateNewtonSkeletonContainer(rootBody); + dgSkeletonContainer::dgNode* const rootNode = skeleton->GetRoot(); + if (rootBody->GetInvMass().m_w == dgFloat32 (0.0f)) { + if (constraint->IsBilateral() && (constraint->m_dynamicsLru != lru)) { + constraint->m_dynamicsLru = lru; + dgDynamicBody* const childBody = (dgDynamicBody*)((constraint->GetBody0() == rootBody) ? constraint->GetBody1() : constraint->GetBody0()); + if (!constraint->m_solverModel) { + if ((childBody->m_dynamicsLru != lru) && (childBody->GetInvMass().m_w != dgFloat32(0.0f))) { + childBody->m_dynamicsLru = lru; + dgSkeletonContainer::dgNode* const node = skeleton->AddChild((dgBilateralConstraint*)constraint, rootNode); + queue.Insert(node); + } + } + } + } else { + queue.Insert(rootNode); + rootBody->m_dynamicsLru = lru; + } + + while (!queue.IsEmpty()) { + dgInt32 count = queue.m_firstIndex - queue.m_lastIndex; + if (count < 0) { + count += queue.m_mod; + } + + dgInt32 index = queue.m_lastIndex; + queue.Reset(); + + for (dgInt32 j = 0; j < count; j++) { + dgSkeletonContainer::dgNode* const parentNode = queue.m_pool[index]; + dgDynamicBody* const parentBody = skeleton->GetBody(parentNode); + + for (dgBodyMasterListRow::dgListNode* jointNode1 = parentBody->m_masterNode->GetInfo().GetFirst(); jointNode1; jointNode1 = jointNode1->GetNext()) { + dgBodyMasterListCell* const cell1 = &jointNode1->GetInfo(); + dgConstraint* const constraint1 = cell1->m_joint; + if (constraint1->IsBilateral() && (constraint1->m_dynamicsLru != lru)) { + constraint1->m_dynamicsLru = lru; + + dgDynamicBody* const childBody = (dgDynamicBody*)((constraint1->GetBody0() == parentBody) ? constraint1->GetBody1() : constraint1->GetBody0()); + if (!constraint1->m_solverModel) { + if ((childBody->m_dynamicsLru != lru) && (childBody->GetInvMass().m_w != dgFloat32(0.0f))) { + childBody->m_dynamicsLru = lru; + dgSkeletonContainer::dgNode* const childNode = skeleton->AddChild((dgBilateralConstraint*)constraint1, parentNode); + queue.Insert(childNode); + } else if (loopCount < (sizeof (loopJoints) / sizeof(loopJoints[0]))) { + loopJoints[loopCount] = (dgBilateralConstraint*)constraint1; + loopCount++; + } + + //} else if ((constraint1->m_solverModel != 2) && loopCount < (sizeof (loopJoints) / sizeof(loopJoints[0]))) { + } else if ((constraint1->m_solverModel == 1) && (loopCount < (sizeof (loopJoints) / sizeof(loopJoints[0])))) { + dgAssert (constraint1->m_solverModel != 0); + loopJoints[loopCount] = (dgBilateralConstraint*)constraint1; + loopCount++; + } + } + } + index++; + if (index >= queue.m_mod) { + index = 0; + } + } + } + + skeleton->Finalize(loopCount, loopJoints); + } + } + } + + dgSkeletonList::Iterator iter(skelManager); + for (iter.Begin(); iter; iter++) { + dgSkeletonContainer* const skeleton = iter.GetNode()->GetInfo(); + skeleton->ClearSelfCollision(); + } +} + +#else + +dgInt32 dgWorld::CompareJointByInvMass(const dgBilateralConstraint* const jointA, const dgBilateralConstraint* const jointB, void* notUsed) +{ + dgAssert (jointA->m_solverModel < 2); + dgAssert (jointB->m_solverModel < 2); + + dgWorld* const world = jointA->GetBody0()->GetWorld(); + dgBody* const rootA = world->FindRoot (jointA->GetBody0()); + dgBody* const rootB = world->FindRoot (jointB->GetBody0()); + + if (rootA->m_uniqueID < rootB->m_uniqueID) { + return -1; + } else if (rootA->m_uniqueID > rootB->m_uniqueID) { + return 1; + } + + dgFloat32 invMassA[2]; + dgFloat32 invMassB[2]; + invMassA[0] = jointA->GetBody0()->m_invMass.m_w; + invMassA[1] = jointA->GetBody1()->m_invMass.m_w; + + invMassB[0] = jointB->GetBody0()->m_invMass.m_w; + invMassB[1] = jointB->GetBody1()->m_invMass.m_w; + + if (invMassA[0] < invMassA[1]) { + dgSwap(invMassA[0], invMassA[1]); + } + if (invMassB[0] < invMassB[1]) { + dgSwap(invMassA[0], invMassA[1]); + } + + if (invMassA[1] < invMassB[1]) { + return -1; + } else if (invMassA[1] > invMassB[1]) { + return 1; + } + + if (invMassA[0] < invMassB[0]) { + return -1; + } else if (invMassA[0] > invMassB[0]) { + return 1; + } + + return 0; +} + +void dgWorld::UpdateSkeletons() +{ + D_TRACKTIME(); + dgSkeletonList& skelManager = *this; +skelManager.m_skelListIsDirty = true; + + if (skelManager.m_skelListIsDirty) { + skelManager.m_skelListIsDirty = false; + dgSkeletonList::Iterator iter(skelManager); + for (iter.Begin(); iter; iter++) { + dgSkeletonContainer* const skeleton = iter.GetNode()->GetInfo(); + delete skeleton; + } + skelManager.RemoveAll(); + + const dgBilateralConstraintList& jointList = *this; + m_solverJacobiansMemory.ResizeIfNecessary((jointList.GetCount() + 1024) * sizeof (dgBilateralConstraint*)); + dgBilateralConstraint** const jointArray = (dgBilateralConstraint**)&m_solverJacobiansMemory[0]; + + dgInt32 jointCount = 0; + for (dgBilateralConstraintList::dgListNode* node = jointList.GetFirst(); node; node = node->GetNext()) { + dgBilateralConstraint* const joint = node->GetInfo(); + if (joint->m_solverModel < 2) { + joint->m_body0->InitJointSet(); + joint->m_body1->InitJointSet(); + jointArray[jointCount] = joint; + jointCount++; + } + } + + for (dgInt32 i = 0; i < jointCount; i++) { + dgBilateralConstraint* const joint = jointArray[i]; + dgAssert(joint->GetBody0()->m_invMass.m_w > dgFloat32(0.0f)); + if (joint->GetBody1()->m_invMass.m_w > dgFloat32(0.0f)) { + UnionSet(joint); + } else { + dgBody* const root = FindRootAndSplit(joint->GetBody0()); + root->m_disjointInfo.m_jointCount += 1; + } + } + + dgSortIndirect(jointArray, jointCount, CompareJointByInvMass); + + for (dgInt32 i = 0; i < jointCount; i++) { + dgBilateralConstraint* const joint = jointArray[i]; + dgBody* const root = FindRoot (joint->GetBody0()); + root->m_disjointInfo.m_rank = -1; + } + + int skelCount = 0; + for (dgInt32 i = 0; i < jointCount; i++) { + dgBilateralConstraint* const joint = jointArray[i]; + dgBody* const root = FindRoot(joint->GetBody0()); + if (root->m_disjointInfo.m_rank == -1) { + root->m_disjointInfo.m_rank = skelCount; + skelCount ++; + } + } + + dgInt32* batchStart = dgAlloca (dgInt32, skelCount + 1); + memset (batchStart, 0, sizeof (dgInt32) *(skelCount + 1)); + for (dgInt32 i = 0; i < jointCount; i++) { + dgBilateralConstraint* const joint = jointArray[i]; + dgBody* const root = FindRoot(joint->GetBody0()); + batchStart[root->m_disjointInfo.m_rank] += 1; + } + + dgInt32 acc = 0; + for (dgInt32 i = 0; i <= skelCount ; i++) { + dgInt32 count = batchStart[i]; + batchStart[i] = acc; + acc += count; + } + + for (dgInt32 i = 0; i < skelCount ; i++) { + dgInt32 index = batchStart[i]; + dgInt32 count = batchStart[i + 1] - i; + dgBilateralConstraint** const constraint = &jointArray[index]; + + for (dgInt32 j = 0; j < count; j ++) { + dgBilateralConstraint* const joint = constraint[j]; + joint->m_body0->InitJointSet(); + joint->m_body1->InitJointSet(); + } + + dgInt32 loopCount = 0; + dgInt32 spanningCount = 0; + dgBilateralConstraint** const loopJoints = dgAlloca (dgBilateralConstraint*, count); + dgBilateralConstraint** const spanningTree = dgAlloca (dgBilateralConstraint*, count); + for (dgInt32 j = 0; j < count; j ++) { + dgBilateralConstraint* const joint = constraint[j]; + dgBody* const root0 = FindRoot(joint->GetBody0()); + dgBody* const root1 = FindRoot(joint->GetBody1()); + if (root0 != root1) { + UnionSet(joint); + spanningTree[spanningCount] = joint; + spanningCount ++; + } else { + loopJoints[loopCount] = joint; + loopCount ++; + } + } + //skeleton->Finalize(loopCount, loopJoints); + + } + } + + dgSkeletonList::Iterator iter(skelManager); + for (iter.Begin(); iter; iter++) { + dgSkeletonContainer* const skeleton = iter.GetNode()->GetInfo(); + skeleton->ClearSelfCollision(); + } +} +#endif + +void dgWorld::OnSerializeToFile(void* const fileHandle, const void* const buffer, dgInt32 size) +{ + dgAssert((size & 0x03) == 0); + size_t bytes = fwrite(buffer, size, 1, (FILE*)fileHandle); + bytes=0; +} + +void dgWorld::OnDeserializeFromFile(void* const fileHandle, void* const buffer, dgInt32 size) +{ + dgAssert((size & 0x03) == 0); + FILE* const file = (FILE*)fileHandle; + size_t bytes = fread(buffer, size, 1, file); + bytes=0; +} + +void dgWorld::OnBodySerializeToFile(dgBody& body, void* const userData, dgSerialize serializeCallback, void* const serializeHandle) +{ + const char* const bodyIndentification = "NewtonGravityBody\0\0\0\0"; + int size = (dgInt32(strlen(bodyIndentification)) + 3) & -4; + serializeCallback(serializeHandle, &size, sizeof (size)); + serializeCallback(serializeHandle, bodyIndentification, size); +} + +void dgWorld::SetJointSerializationCallbacks(OnJointSerializationCallback serializeJoint, OnJointDeserializationCallback deserializeJoint) +{ + m_onSerializeJointCallback = serializeJoint; + m_onDeserializeJointCallback = deserializeJoint; +} + +void dgWorld::GetJointSerializationCallbacks(OnJointSerializationCallback* const serializeJoint, OnJointDeserializationCallback* const deserializeJoint) const +{ + *serializeJoint = m_onSerializeJointCallback; + *deserializeJoint = m_onDeserializeJointCallback; +} + +dgBody* dgWorld::FindBodyFromSerializedID(dgInt32 serializedID) const +{ + const dgBodyMasterList& me = *this; + for (dgBodyMasterList::dgListNode* node = me.GetFirst()->GetNext(); node; node = node->GetNext()) { + const dgBodyMasterListRow& graphNode = node->GetInfo(); + if (graphNode.GetBody()->m_serializedEnum == serializedID) { + return graphNode.GetBody(); + } + } + return NULL; +} + +dgInt32 dgWorld::SerializeToFileSort(const dgBody* const body0, const dgBody* const body1, void* const context) +{ + if (body0->m_uniqueID < body1->m_uniqueID) { + return -1; + } else if (body0->m_uniqueID > body1->m_uniqueID) { + return 1; + } + return 0; +} + +void dgWorld::SerializeScene(void* const userData, OnBodySerialize bodyCallback, dgSerialize serializeCallback, void* const serializeHandle) const +{ + dgBody** const array = new dgBody*[GetBodiesCount()]; + + dgInt32 count = 0; + const dgBodyMasterList& me = *this; + for (dgBodyMasterList::dgListNode* node = me.GetFirst()->GetNext(); node; node = node->GetNext()) { + const dgBodyMasterListRow& graphNode = node->GetInfo(); + array[count] = graphNode.GetBody(); + array[count]->m_serializedEnum = count; + count++; + dgAssert(count <= GetBodiesCount()); + } + + dgSortIndirect(array, count, SerializeToFileSort); + SerializeBodyArray(userData, bodyCallback ? bodyCallback : OnBodySerializeToFile, array, count, serializeCallback, serializeHandle); + SerializeJointArray(count, OnSerializeToFile, serializeHandle); + + for (dgBodyMasterList::dgListNode* node = me.GetFirst()->GetNext(); node; node = node->GetNext()) { + const dgBodyMasterListRow& graphNode = node->GetInfo(); + graphNode.GetBody()->m_serializedEnum = -1; + } + + delete[] array; +} + +void dgWorld::DeserializeScene(void* const userData, OnBodyDeserialize bodyCallback, dgDeserialize deserializeCallback, void* const serializeHandle) +{ + dgTree bodyMap(GetAllocator()); + DeserializeBodyArray(userData, bodyCallback ? bodyCallback : OnBodyDeserializeFromFile, bodyMap, deserializeCallback, serializeHandle); + DeserializeJointArray(bodyMap, deserializeCallback, serializeHandle); + + const dgBodyMasterList& me = *this; + for (dgBodyMasterList::dgListNode* node = me.GetFirst()->GetNext(); node; node = node->GetNext()) { + const dgBodyMasterListRow& graphNode = node->GetInfo(); + graphNode.GetBody()->m_serializedEnum = -1; + } +} + +void dgWorld::OnBodyDeserializeFromFile(dgBody& body, void* const userData, dgDeserialize deserializeCallback, void* const fileHandle) +{ +} + +void dgWorld::DeserializeBodyArray (void* const userData, OnBodyDeserialize bodyCallback, dgTree&bodyMap, dgDeserialize deserializeCallback, void* const serializeHandle) +{ + dgInt32 revision = dgDeserializeMarker(deserializeCallback, serializeHandle); + + dgTree shapeMap(GetAllocator()); + + dgInt32 uniqueShapes; + deserializeCallback(serializeHandle, &uniqueShapes, sizeof (uniqueShapes)); + for (dgInt32 i = 0; i < uniqueShapes; i++) { + dgInt32 id; + + deserializeCallback(serializeHandle, &id, sizeof (id)); + dgCollisionInstance instance(this, deserializeCallback, serializeHandle, revision); + dgDeserializeMarker(deserializeCallback, serializeHandle); + + const dgCollision* const shape = instance.GetChildShape(); + shapeMap.Insert(shape, id); + shape->AddRef(); + } + + dgInt32 bodyCount; + deserializeCallback(serializeHandle, &bodyCount, sizeof (bodyCount)); + for (dgInt32 i = 0; i < bodyCount; i++) { + dgInt32 bodyType; + deserializeCallback(serializeHandle, &bodyType, sizeof (bodyType)); + dgBody* body = NULL; + + switch (bodyType) + { + case dgBody::m_dynamicBody: + { + body = new (m_allocator)dgDynamicBody(this, &shapeMap, deserializeCallback, serializeHandle, revision); + break; + } + case dgBody::m_kinematicBody: + { + body = new (m_allocator)dgKinematicBody(this, &shapeMap, deserializeCallback, serializeHandle, revision); + break; + } + + case dgBody::m_dynamicBodyAsymatric: + { + body = new (m_allocator)dgDynamicBodyAsymetric(this, &shapeMap, deserializeCallback, serializeHandle, revision); + break; + } + + } + + dgAssert(body); + m_bodiesUniqueID++; + body->m_freeze = false; + body->m_sleeping = false; + body->m_equilibrium = false; + body->m_spawnnedFromCallback = false; + body->m_uniqueID = dgInt32(m_bodiesUniqueID); + + dgBodyMasterList::AddBody(body); + body->SetMatrix(body->GetMatrix()); + m_broadPhase->Add(body); + if (body->IsRTTIType(dgBody::m_dynamicBodyRTTI)) { + dgDynamicBody* const dynBody = (dgDynamicBody*)body; + dynBody->SetMassMatrix(dynBody->m_mass.m_w, dynBody->CalculateLocalInertiaMatrix()); + } + + // load user related data + bodyCallback(*body, userData, deserializeCallback, serializeHandle); + + bodyMap.Insert(body, body->m_serializedEnum); + + // sync to next body + dgDeserializeMarker(deserializeCallback, serializeHandle); + } + + dgTree::Iterator iter(shapeMap); + for (iter.Begin(); iter; iter++) { + const dgCollision* const collision = iter.GetNode()->GetInfo(); + collision->Release(); + } +} + +void dgWorld::DeserializeJointArray (const dgTree&bodyMap, dgDeserialize serializeCallback, void* const userData) +{ + dgInt32 count = 0; + + dgDeserializeMarker (serializeCallback, userData); + serializeCallback(userData, &count, sizeof (count)); + + for (dgInt32 i = 0; i < count; i ++) { + if (m_onDeserializeJointCallback) { + dgInt32 bodyIndex0; + dgInt32 bodyIndex1; + + serializeCallback(userData, &bodyIndex0, sizeof (bodyIndex0)); + serializeCallback(userData, &bodyIndex1, sizeof (bodyIndex1)); + + dgBody* const body0 = (bodyIndex0 != -1) ? bodyMap.Find (bodyIndex0)->GetInfo() : NULL; + dgBody* const body1 = (bodyIndex1 != -1) ? bodyMap.Find (bodyIndex1)->GetInfo() : NULL; + m_onDeserializeJointCallback (body0, body1, serializeCallback, userData); + } + dgDeserializeMarker(serializeCallback, userData); + } + + dgDeserializeMarker(serializeCallback, userData); +} + +void dgWorld::SerializeBodyArray(void* const userData, OnBodySerialize bodyCallback, dgBody** const array, dgInt32 count, dgSerialize serializeCallback, void* const fileHandle) const +{ + dgSerializeMarker(serializeCallback, fileHandle); + + // serialize all collisions + dgInt32 uniqueShapes = 0; + dgTree shapeMap(GetAllocator()); + for (dgInt32 i = 0; i < count; i++) { + dgBody* const body = array[i]; + dgAssert(body->m_world == this); + dgCollisionInstance* const instance = body->GetCollision(); + const dgCollision* const collision = instance->GetChildShape(); + dgTree::dgTreeNode* const shapeNode = shapeMap.Insert(uniqueShapes, collision); + if (shapeNode) { + uniqueShapes++; + } + } + + serializeCallback(fileHandle, &uniqueShapes, sizeof (uniqueShapes)); + dgTree::Iterator iter(shapeMap); + for (iter.Begin(); iter; iter++) { + dgInt32 id = iter.GetNode()->GetInfo(); + const dgCollision* const collision = iter.GetKey(); + dgCollisionInstance instance(this, collision, 0, dgMatrix(dgGetIdentityMatrix())); + serializeCallback(fileHandle, &id, sizeof (id)); + instance.Serialize(serializeCallback, fileHandle); + dgSerializeMarker(serializeCallback, fileHandle); + } + + serializeCallback(fileHandle, &count, sizeof (count)); + for (dgInt32 i = 0; i < count; i++) { + dgBody* const body = array[i]; + + dgInt32 bodyType = body->GetType(); + serializeCallback(fileHandle, &bodyType, sizeof (bodyType)); + + // serialize the body + body->Serialize(shapeMap, serializeCallback, fileHandle); + + // serialize body custom data + bodyCallback(*body, userData, serializeCallback, fileHandle); + + dgSerializeMarker(serializeCallback, fileHandle); + } +} + +void dgWorld::SerializeJointArray(dgInt32 bodyCount, dgSerialize serializeCallback, void* const userData) const +{ + dgInt32 count = 0; + const dgBodyMasterList* me = this; + for (dgBodyMasterList::dgListNode* node = me->GetFirst(); node; node = node->GetNext()) { + const dgBodyMasterListRow& info = node->GetInfo(); + for (dgBodyMasterListRow::dgListNode *jointNode = info.GetFirst(); jointNode; jointNode = jointNode->GetNext()) { + const dgBodyMasterListCell& cell = jointNode->GetInfo(); + + dgConstraint* const joint = cell.m_joint; + count += joint->IsBilateral() ? 1 : 0; + } + } + + // dgTree bodyMap (GetAllocator()); + // for (dgInt32 i = 0; i < bodyCount; i ++) { + // bodyMap.Insert (i, bodyArray[i]); + // } + + count /= 2; + dgSerializeMarker(serializeCallback, userData); + serializeCallback(userData, &count, sizeof (count)); + + dgTree map(GetAllocator()); + for (dgBodyMasterList::dgListNode* node = me->GetFirst(); node; node = node->GetNext()) { + dgBodyMasterListRow& info = node->GetInfo(); + for (dgBodyMasterListRow::dgListNode *jointNode = info.GetFirst(); jointNode; jointNode = jointNode->GetNext()) { + const dgBodyMasterListCell& cell = jointNode->GetInfo(); + dgConstraint* const joint = cell.m_joint; + if (joint->IsBilateral()) { + if (!map.Find(joint)) { + map.Insert(0, joint); + dgAssert(joint->GetBody0()); + dgAssert(joint->GetBody1()); + const dgInt32 body0 = (joint->GetBody0() != m_sentinelBody) ? joint->GetBody0()->m_serializedEnum : -1; + const dgInt32 body1 = (joint->GetBody1() != m_sentinelBody) ? joint->GetBody1()->m_serializedEnum : -1; + + serializeCallback(userData, &body0, sizeof (dgInt32)); + serializeCallback(userData, &body1, sizeof (dgInt32)); + + dgBilateralConstraint* const bilateralJoint = (dgBilateralConstraint*)joint; + bilateralJoint->Serialize(serializeCallback, userData); + + dgSerializeMarker(serializeCallback, userData); + } + } + } + } + + dgSerializeMarker(serializeCallback, userData); +} + diff --git a/thirdparty/src/newton/dgPhysics/dgWorld.h b/thirdparty/src/newton/dgPhysics/dgWorld.h new file mode 100644 index 000000000..692c29db9 --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgWorld.h @@ -0,0 +1,654 @@ +/* Copyright (c) <2003-2019> +* +* 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_WORLD_H_ +#define _DG_WORLD_H_ + +#include "dgBody.h" +#include "dgContact.h" +#include "dgCollision.h" +#include "dgBroadPhase.h" +#include "dgWorldPlugins.h" +#include "dgCollisionScene.h" +#include "dgBodyMasterList.h" +#include "dgWorldDynamicUpdate.h" +#include "dgBilateralConstraint.h" +//#include "dgDeformableBodiesUpdate.h" +#include "dgCollisionCompoundFractured.h" + +#define DG_REDUCE_CONTACT_TOLERANCE dgFloat32 (5.0e-2f) +#define DG_PRUNE_CONTACT_TOLERANCE dgFloat32 (5.0e-2f) + +#define DG_SLEEP_ENTRIES 8 +#define DG_MAX_DESTROYED_BODIES_BY_FORCE 8 + +class dgBody; +class dgDynamicBody; +class dgKinematicBody; +class dgCollisionPoint; +class dgUserConstraint; +class dgBallConstraint; +class dgHingeConstraint; +class dgUserMeshCreation; +class dgSlidingConstraint; +class dgCollisionInstance; +class dgSkeletonContainer; +class dgUpVectorConstraint; +class dgUniversalConstraint; +class dgCorkscrewConstraint; +class dgCollisionDeformableMesh; + + +class dgBodyCollisionList: public dgTree +{ + public: + dgBodyCollisionList (dgMemoryAllocator* const allocator) + :dgTree(allocator) + { + } +}; + +class dgBodyMaterialList: public dgTree +{ + public: + dgBodyMaterialList (dgMemoryAllocator* const allocator) + :dgTree(allocator) + { + } +}; + +class dgSkeletonList: public dgList +{ + public: + dgSkeletonList(dgMemoryAllocator* const allocator) + :dgList(allocator) + ,m_lruMarker(1) + ,m_skelListIsDirty(true) + { + } + + dgInt32 m_lruMarker; + bool m_skelListIsDirty; +}; + +class dgWorld; +class dgCollisionInstance; +class dgCollisionParamProxy; + +class dgSolverProgressiveSleepEntry +{ + public: + dgFloat32 m_maxAccel; + dgFloat32 m_maxAlpha; + dgFloat32 m_maxVeloc; + dgFloat32 m_maxOmega; + dgInt32 m_steps; +}; + +class dgWorldThreadPool: public dgThreadHive +{ + public: + dgWorldThreadPool(dgMemoryAllocator* const allocator) + :dgThreadHive(allocator) + { + } + + virtual void OnBeginWorkerThread (dgInt32 threadId); + virtual void OnEndWorkerThread (dgInt32 threadId); +}; + +class dgDeadJoints: public dgTree +{ + public: + dgDeadJoints(dgMemoryAllocator* const allocator); + void DestroyJoints(dgWorld& world); + void DestroyJoint(dgConstraint* const joint); + private: + dgInt32 m_lock; +}; + +class dgDeadBodies: public dgTree +{ + public: + dgDeadBodies(dgMemoryAllocator* const allocator); + void DestroyBody(dgBody* const body); + void DestroyBodies(dgWorld& world); + + private: + dgInt32 m_lock; +}; + +typedef void (*OnPostUpdateCallback) (const dgWorld* const world, dgFloat32 timestep); + +DG_MSC_VECTOR_ALIGNMENT +class dgWorld + :public dgBodyMasterList + ,public dgBodyMaterialList + ,public dgBodyCollisionList + ,public dgSkeletonList + ,public dgContactList + ,public dgBilateralConstraintList + ,public dgWorldDynamicUpdate + ,public dgMutexThread + ,public dgAsyncThread + ,public dgWorldThreadPool + ,public dgDeadBodies + ,public dgDeadJoints + ,public dgWorldPluginList +{ + public: + typedef void (dgApi *OnCreateContact) (const dgWorld* const world, const dgContact* const contact); + typedef void (dgApi *OnDestroyContact) (const dgWorld* const world, const dgContact* const contact); + typedef dgUnsigned32 (dgApi *OnClusterUpdate) (const dgWorld* const world, void* island, dgInt32 bodyCount); + typedef void (dgApi *OnListenerBodyDestroyCallback) (const dgWorld* const world, void* const listener, dgBody* const body); + typedef void (dgApi *OnListenerUpdateCallback) (const dgWorld* const world, void* const listener, dgFloat32 timestep); + typedef void (dgApi *OnListenerDestroyCallback) (const dgWorld* const world, void* const listener); + typedef void (dgApi *OnListenerDebugCallback) (const dgWorld* const world, void* const listener, void* const debugContext); + typedef void (dgApi *OnBodySerialize) (dgBody& me, void* const userData, dgSerialize funt, void* const serilalizeObject); + typedef void (dgApi *OnBodyDeserialize) (dgBody& me, void* const userData, dgDeserialize funt, void* const serilalizeObject); + typedef void (dgApi *OnCollisionInstanceDestroy) (const dgWorld* const world, const dgCollisionInstance* const collision); + typedef void (dgApi *OnCollisionInstanceDuplicate) (const dgWorld* const world, dgCollisionInstance* const collision, const dgCollisionInstance* const sourceCollision); + + typedef void (dgApi *OnJointSerializationCallback) (const dgUserConstraint* const joint, dgSerialize funt, void* const serilalizeObject); + typedef void (dgApi *OnJointDeserializationCallback) (const dgBody* const body0, const dgBody* const body1, dgDeserialize funt, void* const serilalizeObject); + + enum dgBroadPhaseType + { + m_broadphaseMixed, + m_broadphaseSegregated, + }; + + class dgListener + { + public: + dgListener() + :m_world(NULL) + ,m_userData(NULL) + ,m_onPostStep(NULL) + ,m_onPreUpdate(NULL) + ,m_onPostUpdate(NULL) + ,m_onDebugCallback(NULL) + ,m_onListenerDestroy(NULL) + ,m_onBodyDestroy(NULL) + { + } + + ~dgListener() + { + if (m_onListenerDestroy) { + m_onListenerDestroy(m_world, m_userData); + } + } + + char m_name[32]; + dgWorld* m_world; + void* m_userData; + OnListenerUpdateCallback m_onPostStep; + OnListenerUpdateCallback m_onPreUpdate; + OnListenerUpdateCallback m_onPostUpdate; + OnListenerDebugCallback m_onDebugCallback; + OnListenerDestroyCallback m_onListenerDestroy; + OnListenerBodyDestroyCallback m_onBodyDestroy; + }; + + class dgListenerList: public dgList + { + public: + dgListenerList (dgMemoryAllocator* const allocator) + :dgList (allocator) + { + } + + ~dgListenerList() + { + } + }; + + DG_CLASS_ALLOCATOR(allocator) + + dgWorld(dgMemoryAllocator* const allocator); + ~dgWorld(); + + dgFloat32 GetUpdateTime() const; + dgBroadPhase* GetBroadPhase() const; + + dgInt32 GetSolverIterations() const; + void SetSolverIterations (dgInt32 mode); + + OnPostUpdateCallback GetPostUpdateCallback() const; + void SetPostUpdateCallback (OnPostUpdateCallback callback); + + void EnableParallelSolverOnLargeIsland(dgInt32 mode); + dgInt32 GetParallelSolverOnLargeIsland() const; + + void FlushCache(); + + virtual dgUnsigned64 GetTimeInMicrosenconds() const; + + void* GetUserData() const; + void SetUserData (void* const userData); + + void Update (dgFloat32 timestep); + void UpdateAsync (dgFloat32 timestep); + void StepDynamics (dgFloat32 timestep); + + dgInt32 Collide (const dgCollisionInstance* const collisionA, const dgMatrix& matrixA, + const dgCollisionInstance* const collisionB, const dgMatrix& matrixB, + dgTriplex* const points, dgTriplex* const normals, dgFloat32* const penetration, + dgInt64* const attibuteA, dgInt64* const attibuteB, dgInt32 maxContacts, dgInt32 threadIndex); + + dgInt32 CollideContinue (const dgCollisionInstance* const collisionA, const dgMatrix& matrixA, const dgVector& velocA, const dgVector& omegaA, + const dgCollisionInstance* const collisionB, const dgMatrix& matrixB, const dgVector& velocB, const dgVector& omegaB, + dgFloat32& timeStep, dgTriplex* const points, dgTriplex* const normals, dgFloat32* const penetration, + dgInt64* const attibuteA, dgInt64* const attibuteB, dgInt32 maxContacts, dgInt32 threadIndex); + + bool IntersectionTest (const dgCollisionInstance* const collisionA, const dgMatrix& matrixA, + const dgCollisionInstance* const collisionB, const dgMatrix& matrixB, + dgInt32 threadIndex); + + dgInt32 ClosestPoint (dgTriplex& point, const dgCollisionInstance* const collision, const dgMatrix& matrix, dgTriplex& contact, dgTriplex& normal, dgInt32 threadIndex); + dgInt32 ClosestPoint (const dgCollisionInstance* const collisionA, const dgMatrix& matrixA, + const dgCollisionInstance* const collisionB, const dgMatrix& matrixB, + dgTriplex& contactA, dgTriplex& contactB, dgTriplex& normalAB, dgInt32 threadIndex); + + void SetFrictionThreshold (dgFloat32 acceletion); + + void ListenersDebug(void* const debugContext); + void* GetListenerUserData (void* const listener) const; + void* FindListener (const char* const nameid) const; + + void* AddListener (const char* const nameid, void* const userData); + void ListenerSetPostStep (void* const listener, OnListenerUpdateCallback updateCallback); + void ListenerSetPostUpdate (void* const listener, OnListenerUpdateCallback updateCallback); + void ListenerSetPreUpdate (void* const listener, OnListenerUpdateCallback updateCallback); + void ListenerSetDestroyCallback (void* const listener, OnListenerDestroyCallback destroyCallback); + + void SetListenerBodyDebugCallback (void* const listener, OnListenerDebugCallback callback); + void SetListenerBodyDestroyCallback (void* const listener, OnListenerBodyDestroyCallback callback); + OnListenerBodyDestroyCallback GetListenerBodyDestroyCallback (void* const listener) const; + + void SetIslandUpdateCallback (OnClusterUpdate callback); + void SetCreateDestroyContactCallback(OnCreateContact createContactCallback, OnDestroyContact destroyContactCallback); + + void InitBody (dgBody* const body, dgCollisionInstance* const collision, const dgMatrix& matrix); + dgDynamicBody* CreateDynamicBody (dgCollisionInstance* const collision, const dgMatrix& matrix); + dgKinematicBody* CreateKinematicBody (dgCollisionInstance* const collision, const dgMatrix& matrix); + dgDynamicBody* CreateDynamicBodyAsymetric(dgCollisionInstance* const collision, const dgMatrix& matrix); + void DestroyBody(dgBody* const body); + void DestroyAllBodies (); + + // apply the transform matrix to the body and recurse trough all bodies attached to this body with a + // bilateral joint contact joint are ignored. + void BodySetMatrix (dgBody* const body, const dgMatrix& matrix); + + dgInt32 GetBodiesCount() const; + dgInt32 GetConstraintsCount() const; + dgUnsigned32 GetFrameNumber() const; + + dgCollisionInstance* CreateInstance (const dgCollision* const child, dgInt32 shapeID, const dgMatrix& offsetMatrix); + + dgCollisionInstance* CreateNull (); + dgCollisionInstance* CreateSphere (dgFloat32 radiusdg, dgInt32 shapeID, const dgMatrix& offsetMatrix = dgGetIdentityMatrix()); + dgCollisionInstance* CreateCone (dgFloat32 radius, dgFloat32 height, dgInt32 shapeID, const dgMatrix& offsetMatrix = dgGetIdentityMatrix()); + dgCollisionInstance* CreateCapsule (dgFloat32 radio0, dgFloat32 radio1, dgFloat32 height, dgInt32 shapeID, const dgMatrix& offsetMatrix = dgGetIdentityMatrix()); + dgCollisionInstance* CreateCylinder (dgFloat32 radio0, dgFloat32 radio1, dgFloat32 height, dgInt32 shapeID, const dgMatrix& offsetMatrix = dgGetIdentityMatrix()); + dgCollisionInstance* CreateBox (dgFloat32 dx, dgFloat32 dy, dgFloat32 dz, dgInt32 shapeID, const dgMatrix& offsetMatrix = dgGetIdentityMatrix()); + dgCollisionInstance* CreateConvexHull (dgInt32 count, const dgFloat32* const points, dgInt32 strideInBytes, dgFloat32 thickness, dgInt32 shapeID, const dgMatrix& offsetMatrix = dgGetIdentityMatrix()); + dgCollisionInstance* CreateChamferCylinder (dgFloat32 radius, dgFloat32 height, dgInt32 shapeID, const dgMatrix& offsetMatrix = dgGetIdentityMatrix()); + dgCollisionInstance* CreateCompound (); + dgCollisionInstance* CreateFracturedCompound (dgMeshEffect* const solidMesh, int shapeID, int fracturePhysicsMaterialID, int pointcloudCount, const dgFloat32* const vertexCloud, int strideInBytes, int materialID, const dgMatrix& textureMatrix, + dgCollisionCompoundFractured::OnEmitFractureChunkCallBack emitFrafuredChunk, dgCollisionCompoundFractured::OnEmitNewCompundFractureCallBack emitFracturedCompound, dgCollisionCompoundFractured::OnReconstructFractureMainMeshCallBack reconstructMainMesh); + + dgCollisionInstance* CreateDeformableSolid (dgMeshEffect* const mesh, dgInt32 shapeID); + dgCollisionInstance* CreateMassSpringDamperSystem (dgInt32 shapeID, dgInt32 pointCount, const dgFloat32* const points, dgInt32 srideInBytes, const dgFloat32* const pointsMass, dgInt32 linksCount, const dgInt32* const links, const dgFloat32* const linksSpring, const dgFloat32* const LinksDamper); + + dgCollisionInstance* CreateBVH (); + dgCollisionInstance* CreateStaticUserMesh (const dgVector& boxP0, const dgVector& boxP1, const dgUserMeshCreation& data); + dgCollisionInstance* CreateHeightField (dgInt32 width, dgInt32 height, dgInt32 contructionMode, dgInt32 elevationDataType, const void* const elevationMap, const dgInt8* const atributeMap, dgFloat32 verticalScale, dgFloat32 horizontalScale_x, dgFloat32 horizontalScale_z); + dgCollisionInstance* CreateScene (); + + dgBroadPhaseAggregate* CreateAggreGate() const; + void DestroyAggregate(dgBroadPhaseAggregate* const aggregate) const; + + void SetCollisionInstanceConstructorDestructor (OnCollisionInstanceDuplicate constructor, OnCollisionInstanceDestroy destructor); + + static void OnDeserializeFromFile(void* const userData, void* const buffer, dgInt32 size); + static void OnSerializeToFile(void* const userData, const void* const buffer, dgInt32 size); + static dgInt32 SerializeToFileSort (const dgBody* const body0, const dgBody* const body1, void* const context); + static void OnBodySerializeToFile (dgBody& body, void* const userData, dgSerialize serializeCallback, void* const serializeHandle); + static void OnBodyDeserializeFromFile (dgBody& body, void* const userData, dgDeserialize deserializeCallback, void* const serializeHandle); + + dgBody* FindBodyFromSerializedID(dgInt32 serializedID) const; + void SetJointSerializationCallbacks(OnJointSerializationCallback serializeJointCallback, OnJointDeserializationCallback deserializeJointCallback); + void GetJointSerializationCallbacks(OnJointSerializationCallback* const serializeJointCallback, OnJointDeserializationCallback* const deserializeJointCallback) const; + + void SerializeScene(void* const userData, OnBodySerialize bodyCallback, dgSerialize serializeCallback, void* const serializeHandle) const; + void DeserializeScene(void* const userData, OnBodyDeserialize bodyCallback, dgDeserialize deserializeCallback, void* const serializeHandle); + + void SerializeBodyArray (void* const userData, OnBodySerialize bodyCallback, dgBody** const array, dgInt32 count, dgSerialize serializeCallback, void* const serializeHandle) const; + void DeserializeBodyArray (void* const userData, OnBodyDeserialize bodyCallback, dgTree&bodyMap, dgDeserialize deserializeCallback, void* const serializeHandle); + + void SerializeJointArray (dgInt32 count, dgSerialize serializeCallback, void* const serializeHandle) const; + void DeserializeJointArray (const dgTree&bodyMap, dgDeserialize serializeCallback, void* const serializeHandle); + + void SerializeCollision (dgCollisionInstance* const shape, dgSerialize deserialization, void* const userData) const; + dgCollisionInstance* CreateCollisionFromSerialization (dgDeserialize deserialization, void* const userData); + void ReleaseCollision(const dgCollision* const collision); + + dgUpVectorConstraint* CreateUpVectorConstraint (const dgVector& pin, dgBody *body); + + dgBallConstraint* CreateBallConstraint (const dgVector& pivot, dgBody* const body0, dgBody *refBody = NULL); + dgHingeConstraint* CreateHingeConstraint (const dgVector& pivot, const dgVector& pin, dgBody* const body0, dgBody *refBody = NULL); + dgSlidingConstraint* CreateSlidingConstraint (const dgVector& pivot, const dgVector& pin, dgBody* const body0, dgBody *refBody = NULL); + dgCorkscrewConstraint* CreateCorkscrewConstraint (const dgVector& pivot, const dgVector& pin, dgBody* const body0, dgBody *refBody = NULL); + dgUniversalConstraint* CreateUniversalConstraint (const dgVector& pivot, const dgVector& pin0, const dgVector& pin1, dgBody* const body0, dgBody *body1 = NULL); + + void DestroyConstraint (dgConstraint* constraint); + + dgSkeletonContainer* CreateNewtonSkeletonContainer (dgBody* const rootBone); + void DestroySkeletonContainer (dgSkeletonContainer* const container); + + dgUnsigned32 CreateBodyGroupID(); + void RemoveAllGroupID(); + + dgUnsigned32 GetDefualtBodyGroupID() const; + dgContactMaterial* GetMaterial (dgUnsigned32 bodyGroupId0, dgUnsigned32 bodyGroupId1) const; + + dgContactMaterial* GetFirstMaterial () const; + dgContactMaterial* GetNextMaterial (dgContactMaterial* material) const; + dgContact* FindContactJoint (const dgBody* body0, const dgBody* body1) const; + + void SetThreadsCount (dgInt32 count); + + //Parallel Job dispatcher for user related stuff + void ExecuteUserJob (dgWorkerThreadTaskCallback userJobKernel, void* const userJobKernelContext, const char* const functionName); + + void BodyEnableSimulation (dgBody* const body); + void BodyDisableSimulation (dgBody* const body); + bool GetBodyEnableDisableSimulationState (dgBody* const body) const; + + dgDynamicBody* GetSentinelBody() const; + dgMemoryAllocator* GetAllocator() const; + + dgInt32 GetBroadPhaseType() const; + void SetBroadPhaseType (dgInt32 type); + void ResetBroadPhase(); + + dgFloat32 GetContactMergeTolerance() const; + void SetContactMergeTolerance(dgFloat32 tolerenace); + + void SetSubsteps (dgInt32 subSteps); + dgInt32 GetSubsteps () const; + void FlushRegisters() const; + + private: + class dgAdressDistPair + { + public: + dgInt32 m_adress; + dgFloat32 m_dist; + }; + + void RunStep (); + void CalculateContacts (dgBroadPhase::dgPair* const pair, dgInt32 threadIndex, bool ccdMode, bool intersectionTestOnly); + + dgInt32 PruneContacts (dgInt32 count, dgContactPoint* const contact, dgFloat32 distTolerenace, dgInt32 maxCount = (DG_CONSTRAINT_MAX_ROWS / 3)) const; + dgInt32 CalculateConvexPolygonToHullContactsDescrete (dgCollisionParamProxy& proxy) const; + dgInt32 CalculatePolySoupToHullContactsDescrete (dgCollisionParamProxy& proxy) const; + dgInt32 CalculateConvexToNonConvexContactsContinue (dgCollisionParamProxy& proxy) const; + dgInt32 CalculateUserContacts (dgCollisionParamProxy& proxy) const; + dgInt32 CalculateConvexToNonConvexContacts (dgCollisionParamProxy& proxy) const; + dgInt32 CalculateConvexToConvexContacts (dgCollisionParamProxy& proxy) const; + dgInt32 PruneContactsByRank(dgInt32 count, dgCollisionParamProxy& proxy, dgInt32 maxCount) const; + + void PopulateContacts (dgBroadPhase::dgPair* const pair, dgInt32 threadIndex); + void ProcessContacts (dgBroadPhase::dgPair* const pair, dgInt32 threadIndex); + + void ConvexContacts (dgBroadPhase::dgPair* const pair, dgCollisionParamProxy& proxy) const; + void CompoundContacts (dgBroadPhase::dgPair* const pair, dgCollisionParamProxy& proxy) const; + + void SceneContacts (dgBroadPhase::dgPair* const pair, dgCollisionParamProxy& proxy) const; + void SceneChildContacts (dgBroadPhase::dgPair* const pair, dgCollisionParamProxy& proxy) const; + + dgFloat32 CalculateTimeToImpact (dgContact* const contactJoint, dgFloat32 timestep, dgInt32 threadIndex, dgVector& p, dgVector& q, dgVector& normal, dgFloat32 dist) const; + dgInt32 ClosestPoint (dgCollisionParamProxy& proxy) const; + //dgInt32 ClosestCompoundPoint (dgBody* const compoundConvexA, dgBody* const collisionB, dgTriplex& contactA, dgTriplex& contactB, dgTriplex& normalAB, dgInt32 threadIndex) const; + dgInt32 ClosestCompoundPoint (dgCollisionParamProxy& proxy) const; + + bool AreBodyConnectedByJoints (dgBody* const origin, dgBody* const target); + + void UpdateSkeletons(); + void UpdateBroadphase(dgFloat32 timestep); + + void AddSentinelBody(); + void InitConvexCollision (); + + dgInt32 Prune3dContacts(const dgMatrix& matrix, dgInt32 count, dgContactPoint* const contact, int maxCount, dgFloat32 distTol) const; + dgInt32 Prune2dContacts(const dgMatrix& matrix, dgInt32 count, dgContactPoint* const contact, int maxCount, dgFloat32 distTol) const; + DG_INLINE dgInt32 PruneSupport(dgInt32 count, const dgVector& dir, const dgVector* points) const; + + DG_INLINE dgBody* FindRoot(dgBody* const body) const; + DG_INLINE dgBody* FindRootAndSplit(dgBody* const body) const; + DG_INLINE void UnionSet(const dgConstraint* const joint) const; + + virtual void Execute (dgInt32 threadID); + virtual void TickCallback (dgInt32 threadID); + void UpdateTransforms(dgBodyMasterList::dgListNode* node, dgInt32 threadID); + + static dgUnsigned32 dgApi GetPerformanceCount (); + static void UpdateTransforms(void* const context, void* const node, dgInt32 threadID); + static dgInt32 SortFaces (const dgAdressDistPair* const A, const dgAdressDistPair* const B, void* const context); + static dgInt32 CompareJointByInvMass (const dgBilateralConstraint* const jointA, const dgBilateralConstraint* const jointB, void* notUsed); + + dgUnsigned32 m_numberOfSubsteps; + dgUnsigned32 m_frameNumber; + dgUnsigned32 m_dynamicsLru; + dgUnsigned32 m_inUpdate; + dgUnsigned32 m_solverIterations; + dgUnsigned32 m_bodyGroupID; + dgUnsigned32 m_defualtBodyGroupID; + dgUnsigned32 m_bodiesUniqueID; + dgUnsigned32 m_useParallelSolver; + dgUnsigned32 m_genericLRUMark; + dgInt32 m_clusterLRU; + + dgFloat32 m_freezeAccel2; + dgFloat32 m_freezeAlpha2; + dgFloat32 m_freezeSpeed2; + dgFloat32 m_freezeOmega2; + dgFloat32 m_frictiomTheshold; + dgFloat32 m_savetimestep; + dgFloat32 m_contactTolerance; + dgFloat32 m_lastExecutionTime; + + dgSolverProgressiveSleepEntry m_sleepTable[DG_SLEEP_ENTRIES]; + + dgBroadPhase* m_broadPhase; + dgDynamicBody* m_sentinelBody; + dgCollisionInstance* m_pointCollision; + + void* m_userData; + dgMemoryAllocator* m_allocator; + + OnClusterUpdate m_onClusterUpdate; + OnCreateContact m_onCreateContact; + OnDestroyContact m_onDestroyContact; + OnCollisionInstanceDestroy m_onCollisionInstanceDestruction; + OnCollisionInstanceDuplicate m_onCollisionInstanceCopyConstrutor; + OnJointSerializationCallback m_onSerializeJointCallback; + OnJointDeserializationCallback m_onDeserializeJointCallback; + OnPostUpdateCallback m_onPostUpdateCallback; + + dgListenerList m_listeners; + dgTree m_perInstanceData; + dgArray m_bodiesMemory; + dgArray m_jointsMemory; + dgArray m_clusterMemory; + dgArray m_solverJacobiansMemory; + dgArray m_solverRightHandSideMemory; + dgArray m_solverForceAccumulatorMemory; + + friend class dgBody; + friend class dgSolver; + friend class dgContact; + friend class dgBroadPhase; + friend class dgDeadBodies; + friend class dgDeadJoints; + friend class dgWorldPlugin; + friend class dgContactList; + friend class dgUserConstraint; + friend class dgBodyMasterList; + friend class dgJacobianMemory; + friend class dgCollisionScene; + friend class dgCollisionConvex; + friend class dgBroadPhaseMixed; + friend class dgCollisionInstance; + friend class dgCollisionCompound; + friend class dgParallelBodySolver; + friend class dgWorldDynamicUpdate; + friend class dgParallelSolverClear; + friend class dgParallelSolverSolve; + friend class dgCollisionHeightField; + friend class dgSolverWorlkerThreads; + friend class dgBroadPhaseSegregated; + friend class dgCollisionConvexPolygon; + friend class dgCollidingPairCollector; + friend class dgCollisionDeformableMesh; + friend class dgParallelSolverUpdateForce; + friend class dgParallelSolverUpdateVeloc; + friend class dgParallelSolverBodyInertia; + friend class dgCollisionDeformableSolidMesh; + friend class dgBroadPhaseApplyExternalForce; + friend class dgParallelSolverCalculateForces; + friend class dgCollisionMassSpringDamperSystem; + friend class dgParallelSolverJointAcceleration; + friend class dgParallelSolverBuildJacobianRows; + friend class dgParallelSolverInitFeedbackUpdate; + friend class dgParallelSolverInitInternalForces; + friend class dgParallelSolverBuildJacobianMatrix; + + friend class dgBroadPhaseMaterialCallbackWorkerThread; + friend class dgBroadPhaseCalculateContactsWorkerThread; +} DG_GCC_VECTOR_ALIGNMENT; + + +inline dgMemoryAllocator* dgWorld::GetAllocator() const +{ + return m_allocator; +} + +inline dgBroadPhase* dgWorld::GetBroadPhase() const +{ + return m_broadPhase; +} + +inline void dgWorld::SetSubsteps (dgInt32 subSteps) +{ + m_numberOfSubsteps = dgClamp(subSteps, 1, 8); +} + +inline dgInt32 dgWorld::GetSubsteps () const +{ + return m_numberOfSubsteps; +} + +inline dgFloat32 dgWorld::GetUpdateTime() const +{ + return m_lastExecutionTime; +} + +inline OnPostUpdateCallback dgWorld::GetPostUpdateCallback() const +{ + return m_onPostUpdateCallback; +} + +inline void dgWorld::SetPostUpdateCallback(OnPostUpdateCallback callback) +{ + m_onPostUpdateCallback = callback; +} + +inline dgUnsigned64 dgWorld::GetTimeInMicrosenconds() const +{ + return dgGetTimeInMicrosenconds(); +} + +inline void dgWorld::SetSolverIterations(dgInt32 mode) +{ + m_solverIterations = dgUnsigned32(dgMax(1, mode)); +} + +inline dgInt32 dgWorld::GetSolverIterations() const +{ + return m_solverIterations; +} + +DG_INLINE dgBody* dgWorld::FindRoot(dgBody* const body) const +{ + dgBody* node = body; + for (; node->m_disjointInfo.m_parent != node; node = node->m_disjointInfo.m_parent); + return node; +} + +DG_INLINE dgBody* dgWorld::FindRootAndSplit(dgBody* const body) const +{ + dgBody* node = body; + while (node->m_disjointInfo.m_parent != node) { + dgBody* const prev = node; + node = node->m_disjointInfo.m_parent; + prev->m_disjointInfo.m_parent = node->m_disjointInfo.m_parent; + } + return node; +} + +DG_INLINE void dgWorld::UnionSet(const dgConstraint* const joint) const +{ + dgBody* const body0 = joint->GetBody0(); + dgBody* const body1 = joint->GetBody1(); + dgBody* root0 = FindRootAndSplit(body0); + dgBody* root1 = FindRootAndSplit(body1); + if (root0 != root1) { + if (root0->m_disjointInfo.m_rank < root1->m_disjointInfo.m_rank) { + dgSwap(root0, root1); + } + root1->m_disjointInfo.m_parent = root0; + if (root0->m_disjointInfo.m_rank == root1->m_disjointInfo.m_rank) { + root0->m_disjointInfo.m_rank += 1; + dgAssert(root0->m_disjointInfo.m_rank <= 6); + } + root0->m_disjointInfo.m_rowCount += root1->m_disjointInfo.m_rowCount; + root0->m_disjointInfo.m_bodyCount += root1->m_disjointInfo.m_bodyCount; + root0->m_disjointInfo.m_jointCount += root1->m_disjointInfo.m_jointCount; + } + root0->m_disjointInfo.m_jointCount++; + root0->m_disjointInfo.m_rowCount += joint->m_maxDOF; +} + +DG_INLINE dgUnsigned32 dgWorld::GetFrameNumber() const +{ + return m_frameNumber; +} + +DG_INLINE void dgWorld::FlushRegisters() const +{ + dgWorldPluginList::FlushRegisters(); +} + +#endif diff --git a/thirdparty/src/newton/dgPhysics/dgWorldDynamicUpdate.cpp b/thirdparty/src/newton/dgPhysics/dgWorldDynamicUpdate.cpp new file mode 100644 index 000000000..ad0767bbc --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgWorldDynamicUpdate.cpp @@ -0,0 +1,687 @@ +/* Copyright (c) <2003-2019> +* +* 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 "dgPhysicsStdafx.h" + +#include "dgBody.h" +#include "dgWorld.h" +#include "dgBroadPhase.h" +#include "dgConstraint.h" +#include "dgDynamicBody.h" +#include "dgCollisionInstance.h" +#include "dgSkeletonContainer.h" +#include "dgWorldDynamicUpdate.h" +#include "dgBilateralConstraint.h" +#include "dgCollisionDeformableMesh.h" + + +dgVector dgWorldDynamicUpdate::m_velocTol (dgFloat32 (1.0e-8f)); + +class dgWorldDynamicUpdateSyncDescriptor +{ + public: + dgWorldDynamicUpdateSyncDescriptor() + { + memset (this, 0, sizeof (dgWorldDynamicUpdateSyncDescriptor)); + } + + dgFloat32 m_timestep; + dgInt32 m_atomicCounter; + + dgInt32 m_clusterCount; + dgInt32 m_firstCluster; +}; + + +void dgJacobianMemory::Init(dgWorld* const world, dgInt32 rowsCount, dgInt32 bodyCount) +{ + world->m_solverJacobiansMemory.ResizeIfNecessary((rowsCount + 1) * sizeof(dgLeftHandSide)); + m_leftHandSizeBuffer = (dgLeftHandSide*)&world->m_solverJacobiansMemory[0]; + + world->m_solverRightHandSideMemory.ResizeIfNecessary((rowsCount + 1) * sizeof(dgRightHandSide)); + m_righHandSizeBuffer = (dgRightHandSide*)&world->m_solverRightHandSideMemory[0]; + + world->m_solverForceAccumulatorMemory.ResizeIfNecessary((bodyCount + 8) * sizeof(dgJacobian)); + m_internalForcesBuffer = (dgJacobian*)&world->m_solverForceAccumulatorMemory[0]; + dgAssert(bodyCount <= (((world->m_solverForceAccumulatorMemory.GetBytesCapacity() - 16) / dgInt32(sizeof(dgJacobian))) & (-8))); + + dgAssert((dgUnsigned64(m_leftHandSizeBuffer) & 0x01f) == 0); + dgAssert((dgUnsigned64(m_internalForcesBuffer) & 0x01f) == 0); +} + +////////////////////////////////////////////////////////////////////// +// Construction/Destruction +////////////////////////////////////////////////////////////////////// + +dgWorldDynamicUpdate::dgWorldDynamicUpdate(dgMemoryAllocator* const allocator) + :m_solverMemory() + ,m_parallelSolver(allocator) + ,m_clusterData(NULL) + ,m_bodies(0) + ,m_joints(0) + ,m_clusters(0) + ,m_markLru(0) + ,m_softBodiesCount(0) + ,m_impulseLru(0) + ,m_softBodyCriticalSectionLock(0) +{ + m_parallelSolver.m_world = (dgWorld*) this; +} + +void dgWorldDynamicUpdate::UpdateDynamics(dgFloat32 timestep) +{ + D_TRACKTIME(); + + m_bodies = 0; + m_joints = 0; + m_clusters = 0; + m_softBodiesCount = 0; + dgWorld* const world = (dgWorld*) this; + world->m_dynamicsLru = world->m_dynamicsLru + DG_BODY_LRU_STEP; + m_markLru = world->m_dynamicsLru; + + dgDynamicBody* const sentinelBody = world->m_sentinelBody; + sentinelBody->m_index = 0; + sentinelBody->m_resting = 1; + sentinelBody->m_sleeping = 1; + sentinelBody->m_autoSleep = 1; + sentinelBody->m_equilibrium = 1; + sentinelBody->m_dynamicsLru = m_markLru; + + BuildClusters(timestep); + const dgInt32 threadCount = world->GetThreadCount(); + + dgWorldDynamicUpdateSyncDescriptor descriptor; + descriptor.m_timestep = timestep; + + dgInt32 index = m_softBodiesCount; + descriptor.m_atomicCounter = 0; + descriptor.m_firstCluster = index; + descriptor.m_clusterCount = m_clusters - index; + + dgInt32 useParallelSolver = world->m_useParallelSolver; +//useParallelSolver = 0; + if (useParallelSolver) { + dgInt32 count = 0; + for (dgInt32 i = 0; (i < m_clusters) && (m_clusterData[index + i].m_jointCount >= DG_PARALLEL_JOINT_COUNT_CUT_OFF); i++) { + count++; + } + if (count) { + CalculateReactionForcesParallel(&m_clusterData[index], count, timestep); + index += count; + } + } + + if (index < m_clusters) { + descriptor.m_atomicCounter = 0; + descriptor.m_firstCluster = index; + descriptor.m_clusterCount = m_clusters - index; + for (dgInt32 i = 0; i < threadCount; i ++) { + world->QueueJob (CalculateClusterReactionForcesKernel, &descriptor, world, "dgWorldDynamicUpdate::CalculateClusterReactionForces"); + } + world->SynchronizationBarrier(); + } + + dgBodyInfo* const bodyArrayPtr = &world->m_bodiesMemory[0]; + for (dgInt32 i = 0; i < m_softBodiesCount; i++) { + dgBodyCluster* const cluster = &m_clusterData[i]; +// IntegrateExternalForce(cluster, timestep, 0); + dgBodyInfo* const bodyArray = &bodyArrayPtr[cluster->m_bodyStart]; + dgAssert (cluster->m_bodyCount == 2); + dgDynamicBody* const body = (dgDynamicBody*)bodyArray[1].m_body; + dgAssert (body->m_collision->IsType(dgCollision::dgCollisionLumpedMass_RTTI)); + body->IntegrateOpenLoopExternalForce(timestep); + IntegrateVelocity(cluster, DG_SOLVER_MAX_ERROR, timestep, 0); + } + + m_clusterData = NULL; +} + +dgInt32 dgWorldDynamicUpdate::CompareKey(dgInt32 highA, dgInt32 lowA, dgInt32 highB, dgInt32 lowB) +{ + if (highA < highB) { + return 1; + } else if (highA > highB) { + return -1; + } + if (lowA < lowB) { + return 1; + } else if (lowA > lowB) { + return -1; + } + return 0; +} + +dgInt32 dgWorldDynamicUpdate::CompareJointInfos(const dgJointInfo* const infoA, const dgJointInfo* const infoB, void*) +{ + return CompareKey(infoA->m_jointCount, infoA->m_setId, infoB->m_jointCount, infoB->m_setId); +} + +dgInt32 dgWorldDynamicUpdate::CompareClusterInfos(const dgBodyCluster* const clusterA, const dgBodyCluster* const clusterB, void* notUsed) +{ + return CompareKey(clusterA->m_jointCount, clusterA->m_bodyStart, clusterB->m_jointCount, clusterB->m_bodyStart); +} + +void dgWorldDynamicUpdate::BuildClusters(dgFloat32 timestep) +{ + D_TRACKTIME(); + dgWorld* const world = (dgWorld*) this; + dgContactList& contactList = *world; + dgBodyMasterList& masterList = *world; + const dgBilateralConstraintList& jointList = *world; + dgInt32 jointCount = contactList.m_activeContactCount; + + dgArray& jointArray = world->m_jointsMemory; + jointArray.ResizeIfNecessary(jointCount + jointList.GetCount()); + dgJointInfo* const baseJointArray = &jointArray[0]; + +#ifdef _DEBUG + for (dgBodyMasterList::dgListNode* node = masterList.GetLast(); node; node = node->GetPrev()) { + const dgBodyMasterListRow& graphNode = node->GetInfo(); + dgBody* const body = graphNode.GetBody(); + if (body->GetInvMass().m_w == dgFloat32(0.0f)) { + for (; node; node = node->GetPrev()) { + dgAssert(node->GetInfo().GetBody()->GetInvMass().m_w == dgFloat32(0.0f)); + } + break; + } + } +#endif + + // add bilateral joints to the joint array + for (dgBilateralConstraintList::dgListNode* node = jointList.GetFirst(); node; node = node->GetNext()) { + dgConstraint* const joint = node->GetInfo(); + if (joint->GetBody0()->m_invMass.m_w || joint->GetBody1()->m_invMass.m_w) { + baseJointArray[jointCount].m_joint = joint; + jointCount++; + } + } + + // form all disjoints sets + for (dgInt32 i = 0; i < jointCount; i ++) { + const dgConstraint* const joint = baseJointArray[i].m_joint; + dgBody* const body0 = joint->GetBody0(); + dgBody* const body1 = joint->GetBody1(); + const dgFloat32 invMass0 = body0->m_invMass.m_w; + const dgFloat32 invMass1 = body1->m_invMass.m_w; + + dgInt32 resting = body0->m_equilibrium & body1->m_equilibrium; + body0->m_resting = resting | (invMass0 == dgFloat32(0.0f)); + body1->m_resting = resting | (invMass1 == dgFloat32(0.0f)); + + if ((invMass0 > dgFloat32 (0.0f)) && (invMass1 > dgFloat32 (0.0f))) { + world->UnionSet(joint); + } else if (invMass1 == dgFloat32 (0.0f)) { + dgBody* const root = world->FindRootAndSplit(body0); + root->m_disjointInfo.m_jointCount += 1; + root->m_disjointInfo.m_rowCount += joint->m_maxDOF; + } else { + dgBody* const root = world->FindRootAndSplit(body1); + root->m_disjointInfo.m_jointCount += 1; + root->m_disjointInfo.m_rowCount += joint->m_maxDOF; + } + } + + // find and tag all sleeping disjoint sets, + // and add single bodies as a set of zero joints and one body + dgInt32 bodyInfoCount = 0; + dgInt32 clustersCount = 0; + dgInt32 augmentedJointCount = jointCount; + + dgArray& clusterMemory = world->m_clusterMemory; + for (dgBodyMasterList::dgListNode* node = masterList.GetLast(); node && (node->GetInfo().GetBody()->GetInvMass().m_w != dgFloat32(0.0f)); node = node->GetPrev()) { + dgBody* const body = node->GetInfo().GetBody(); + if (body->IsRTTIType(dgBody::m_dynamicBodyRTTI | dgBody::m_dynamicBodyAsymatric)) { + dgBody* root = body; + dgInt32 state = 1; + do { + state &= (root->m_equilibrium & root->m_autoSleep); + root = root->m_disjointInfo.m_parent; + } while (root->m_disjointInfo.m_parent != root); + root->m_jointSet &= state; + + if (!root->m_jointSet && !root->m_disjointInfo.m_jointCount) { + dgJointInfo& jointInfo = jointArray[augmentedJointCount]; + + dgAssert (root == body); + dgAssert (root->m_index == -1); + dgAssert (root->m_disjointInfo.m_bodyCount == 1); + + root->m_index = clustersCount; + jointInfo.m_body = body; + jointInfo.m_jointCount = 0; + jointInfo.m_setId = root->m_index; + jointInfo.m_bodyCount = root->m_disjointInfo.m_bodyCount; + jointInfo.m_pairCount = 0; + + dgBodyCluster& cluster = clusterMemory[clustersCount]; + cluster.m_bodyCount = 2; + cluster.m_jointCount = 0; + cluster.m_rowCount = 0; + cluster.m_hasSoftBodies = 0; + cluster.m_isContinueCollision = 0; + cluster.m_bodyStart = root->m_index; + + clustersCount ++; + bodyInfoCount += 2; + augmentedJointCount ++; + } + } + } + + // remove all sleeping joints sets + dgJointInfo* const augmentedJointArray = &jointArray[0]; + for (dgInt32 i = jointCount - 1; i >= 0; i --) { + dgJointInfo* const jointInfo = &augmentedJointArray[i]; + dgConstraint* const constraint = jointInfo->m_joint; + dgBody* const body = (constraint->GetBody0()->GetInvMass().m_w != dgFloat32 (0.0f)) ? constraint->GetBody0() : constraint->GetBody1(); + dgAssert (body->GetInvMass().m_w); + dgBody* const root = world->FindRoot (body); + if (root->m_jointSet) { + augmentedJointCount --; + augmentedJointArray[i] = augmentedJointArray[augmentedJointCount]; + } else { + if (root->m_index == -1) { + root->m_index = clustersCount; + + dgBodyCluster& cluster = clusterMemory[clustersCount]; + cluster.m_bodyCount = root->m_disjointInfo.m_bodyCount + 1; + cluster.m_jointCount = root->m_disjointInfo.m_jointCount; + cluster.m_rowCount = root->m_disjointInfo.m_rowCount; + cluster.m_hasSoftBodies = 0; + cluster.m_bodyStart = root->m_index; + cluster.m_isContinueCollision = 0; + + clustersCount++; + bodyInfoCount += root->m_disjointInfo.m_bodyCount + 1; + } + jointInfo->m_setId = root->m_index; + jointInfo->m_pairCount = constraint->m_maxDOF; + jointInfo->m_bodyCount = root->m_disjointInfo.m_bodyCount; + jointInfo->m_jointCount = root->m_disjointInfo.m_jointCount; + } + } + + m_clusterData = &world->m_clusterMemory[0]; +// dgSort(augmentedJointArray, augmentedJointCount, CompareJointInfos); +// dgSort(m_clusterData, clustersCount, CompareClusterInfos); + dgParallelSort(*world, augmentedJointArray, augmentedJointCount, CompareJointInfos); + dgParallelSort(*world, m_clusterData, clustersCount, CompareClusterInfos); + + dgInt32 bodyStart = 0; + dgInt32 jointStart = 0; + dgInt32 softBodiesCount = 0; + for (dgInt32 i = 0; i < clustersCount; i++) { + dgBodyCluster& cluster = m_clusterData[i]; + cluster.m_bodyStart = bodyStart; + cluster.m_jointStart = jointStart; + + bodyStart += cluster.m_bodyCount; + softBodiesCount += cluster.m_hasSoftBodies; + jointStart += cluster.m_jointCount ? cluster.m_jointCount : 1; + } + world->m_bodiesMemory.ResizeIfNecessary(bodyStart); + + dgInt32 rowStart = 0; + + for (dgInt32 i = 0; i < clustersCount; i++) { + dgBodyCluster& cluster = m_clusterData[i]; + dgBodyInfo* const bodyArray = &world->m_bodiesMemory[cluster.m_bodyStart]; + dgJointInfo* const jointSetArray = &augmentedJointArray[cluster.m_jointStart]; + bodyArray[0].m_body = world->GetSentinelBody(); + + cluster.m_rowStart = rowStart; + bool clusterIsContinueCollision = false; + if (cluster.m_jointCount) { + dgInt32 bodyIndex = 1; + for (dgInt32 j = 0; j < cluster.m_jointCount; j++) { + dgJointInfo* const jointInfo = &jointSetArray[j]; + dgConstraint* const joint = jointInfo->m_joint; + dgBody* const body0 = joint->m_body0; + dgBody* const body1 = joint->m_body1; + + dgInt32 m0 = 0; + if (body0->GetInvMass().m_w != dgFloat32(0.0f)) { + if (body0->m_disjointInfo.m_rank >= 0) { + body0->m_disjointInfo.m_rank = -1; + body0->m_index = bodyIndex; + bodyArray[bodyIndex].m_body = body0; + bodyIndex++; + dgAssert(bodyIndex <= cluster.m_bodyCount); + } + m0 = body0->m_index; + } + + dgInt32 m1 = 0; + if (body1->GetInvMass().m_w != dgFloat32(0.0f)) { + if (body1->m_disjointInfo.m_rank >= 0) { + body1->m_disjointInfo.m_rank = -1; + body1->m_index = bodyIndex; + bodyArray[bodyIndex].m_body = body1; + bodyIndex++; + dgAssert(bodyIndex <= cluster.m_bodyCount); + } + m1 = body1->m_index; + } + + if (joint->GetId() == dgConstraint::m_contactConstraint) { + // check for CCD mode + if (body0->m_continueCollisionMode | body1->m_continueCollisionMode) { + const dgContact* const contact = (dgContact*) joint; + clusterIsContinueCollision |= contact->EstimateCCD (timestep); + } + } + + jointInfo->m_m0 = m0; + jointInfo->m_m1 = m1; + jointInfo->m_pairStart = rowStart; + rowStart += jointInfo->m_pairCount; + } + + cluster.m_isContinueCollision = clusterIsContinueCollision; + if (cluster.m_isContinueCollision) { + cluster.m_rowCount += DG_CCD_EXTRA_CONTACT_COUNT * cluster.m_jointCount / 2; + if (cluster.m_rowCount < DG_CONSTRAINT_MAX_ROWS) { + cluster.m_rowCount = DG_CONSTRAINT_MAX_ROWS; + } + } + + } else { + dgAssert(cluster.m_bodyCount == 2); + bodyArray[1].m_body = jointSetArray[0].m_body; + } + } + + m_solverMemory.Init(world, rowStart, bodyStart); + + m_bodies = bodyStart; + m_joints = jointStart; + m_clusters = clustersCount; + m_softBodiesCount = softBodiesCount; +} + +dgInt32 dgWorldDynamicUpdate::CompareBodyJacobianPair(const dgBodyJacobianPair* const infoA, const dgBodyJacobianPair* const infoB, void* notUsed) +{ + if (infoA->m_bodyIndex < infoB->m_bodyIndex) { + return -1; + } else if (infoA->m_bodyIndex > infoB->m_bodyIndex) { + return 1; + } + return 0; +} + +dgBody* dgWorldDynamicUpdate::GetClusterBody(const void* const clusterPtr, dgInt32 index) const +{ + const dgClusterCallbackStruct* const cluster = (dgClusterCallbackStruct*)clusterPtr; + + char* const ptr = &((char*)cluster->m_bodyArray)[cluster->m_strideInByte * index]; + dgBody** const bodyPtr = (dgBody**)ptr; + return (index < cluster->m_count) ? ((index >= 0) ? *bodyPtr : NULL) : NULL; +} + +void dgWorldDynamicUpdate::CalculateClusterReactionForcesKernel (void* const context, void* const worldContext, dgInt32 threadID) +{ + D_TRACKTIME(); + dgWorldDynamicUpdateSyncDescriptor* const descriptor = (dgWorldDynamicUpdateSyncDescriptor*) context; + + dgFloat32 timestep = descriptor->m_timestep; + dgWorld* const world = (dgWorld*) worldContext; + dgInt32 count = descriptor->m_clusterCount; + dgBodyCluster* const clusters = &world->m_clusterData[descriptor->m_firstCluster]; + +//static int xxx; +//xxx++; +//if (xxx >= 59) +//xxx *=1; +//dgTrace(("\nframe %d\n", xxx)); + + for (dgInt32 i = dgAtomicExchangeAndAdd(&descriptor->m_atomicCounter, 1); i < count; i = dgAtomicExchangeAndAdd(&descriptor->m_atomicCounter, 1)) { + dgBodyCluster* const cluster = &clusters[i]; + world->ResolveClusterForces (cluster, threadID, timestep); + } +} + +dgInt32 dgWorldDynamicUpdate::GetJacobianDerivatives(dgContraintDescritor& constraintParam, dgJointInfo* const jointInfo, dgConstraint* const constraint, dgLeftHandSide* const leftHandSide, dgRightHandSide* const rightHandSide, dgInt32 rowCount) const +{ + dgInt32 dof = dgInt32(constraint->m_maxDOF); + dgAssert(dof <= DG_CONSTRAINT_MAX_ROWS); + for (dgInt32 i = 0; i < dof; i++) { + constraintParam.m_forceBounds[i].m_low = DG_MIN_BOUND; + constraintParam.m_forceBounds[i].m_upper = DG_MAX_BOUND; + constraintParam.m_forceBounds[i].m_jointForce = NULL; + constraintParam.m_forceBounds[i].m_normalIndex = DG_INDEPENDENT_ROW; + } + + dgAssert(constraint->m_body0); + dgAssert(constraint->m_body1); + + dgBody* const body0 = constraint->m_body0; + dgBody* const body1 = constraint->m_body1; + + dgAssert(body0->IsRTTIType(dgBody::m_dynamicBodyRTTI) || body0->IsRTTIType(dgBody::m_kinematicBodyRTTI)); + dgAssert(body1->IsRTTIType(dgBody::m_dynamicBodyRTTI) || body1->IsRTTIType(dgBody::m_kinematicBodyRTTI)); + + body0->m_inCallback = true; + body1->m_inCallback = true; + dof = constraint->JacobianDerivative(constraintParam); + body0->m_inCallback = false; + body1->m_inCallback = false; + + if (constraint->GetId() == dgConstraint::m_contactConstraint) { + dgContact* const contactJoint = (dgContact*)constraint; + contactJoint->m_isInSkeletonLoop = false; + dgSkeletonContainer* const skeleton0 = body0->GetSkeleton(); + dgSkeletonContainer* const skeleton1 = body1->GetSkeleton(); + if (skeleton0 && (skeleton0 == skeleton1)) { + if (contactJoint->IsSkeletonSelftCollision()) { + contactJoint->m_isInSkeletonLoop = true; + skeleton0->AddSelfCollisionJoint(contactJoint); + } + } else if (contactJoint->IsSkeletonIntraCollision()) { + if (skeleton0 && !skeleton1) { + contactJoint->m_isInSkeletonLoop = true; + skeleton0->AddSelfCollisionJoint(contactJoint); + } else if (skeleton1 && !skeleton0) { + contactJoint->m_isInSkeletonLoop = true; + skeleton1->AddSelfCollisionJoint(contactJoint); + } + } + } else if (constraint->IsBilateral() && !constraint->m_isInSkeleton && (constraint->m_solverModel == 3)) { + dgSkeletonContainer* const skeleton0 = body0->GetSkeleton(); + dgSkeletonContainer* const skeleton1 = body1->GetSkeleton(); + if (skeleton0 || skeleton1) { + if (skeleton0 && !skeleton1) { + constraint->m_isInSkeletonLoop = true; + skeleton0->AddSelfCollisionJoint(constraint); + } else if (skeleton1 && !skeleton0) { + constraint->m_isInSkeletonLoop = true; + skeleton1->AddSelfCollisionJoint(constraint); + } + } + } + + jointInfo->m_pairCount = dof; + jointInfo->m_pairStart = rowCount; + + for (dgInt32 i = 0; i < dof; i++) { + dgAssert(constraintParam.m_forceBounds[i].m_jointForce); + + dgLeftHandSide* const row = &leftHandSide[rowCount]; + dgRightHandSide* const rhs = &rightHandSide[rowCount]; + + row->m_Jt = constraintParam.m_jacobian[i]; + rhs->m_diagDamp = dgFloat32(0.0f); + rhs->m_diagonalRegularizer = dgClamp (constraintParam.m_diagonalRegularizer[i], dgFloat32(1.0e-5f), dgFloat32(1.0f)); + rhs->m_coordenateAccel = constraintParam.m_jointAccel[i]; + rhs->m_restitution = constraintParam.m_restitution[i]; + rhs->m_penetration = constraintParam.m_penetration[i]; + rhs->m_penetrationStiffness = constraintParam.m_penetrationStiffness[i]; + rhs->m_lowerBoundFrictionCoefficent = constraintParam.m_forceBounds[i].m_low; + rhs->m_upperBoundFrictionCoefficent = constraintParam.m_forceBounds[i].m_upper; + rhs->m_jointFeebackForce = constraintParam.m_forceBounds[i].m_jointForce; + + dgAssert (constraintParam.m_forceBounds[i].m_normalIndex >= -1); + rhs->m_normalForceIndex = constraintParam.m_forceBounds[i].m_normalIndex; + rowCount++; + } + + return rowCount; +} + +void dgWorldDynamicUpdate::IntegrateVelocity(const dgBodyCluster* const cluster, dgFloat32 accelTolerance, dgFloat32 timestep, dgInt32 threadID) const +{ + D_TRACKTIME(); + dgWorld* const world = (dgWorld*) this; + dgFloat32 velocityDragCoeff = DG_FREEZZING_VELOCITY_DRAG; + dgBodyInfo* const bodyArray = &world->m_bodiesMemory[cluster->m_bodyStart + 1]; + + dgInt32 count = cluster->m_bodyCount - 1; + if (count <= DG_SMALL_ISLAND_COUNT) { + velocityDragCoeff = dgFloat32(0.9999f); + } + + dgFloat32 maxAccel = dgFloat32(0.0f); + dgFloat32 maxAlpha = dgFloat32(0.0f); + dgFloat32 maxSpeed = dgFloat32(0.0f); + dgFloat32 maxOmega = dgFloat32(0.0f); + + const dgFloat32 speedFreeze = world->m_freezeSpeed2; + const dgFloat32 accelFreeze = world->m_freezeAccel2 * ((count <= DG_SMALL_ISLAND_COUNT) ? dgFloat32(0.01f) : dgFloat32(1.0f)); + //const dgFloat32 accelFreeze = world->m_freezeAccel2 * ((count <= DG_SMALL_ISLAND_COUNT) ? dgFloat32(0.0025f) : dgFloat32(1.0f)); + dgVector velocDragVect(velocityDragCoeff, velocityDragCoeff, velocityDragCoeff, dgFloat32(0.0f)); + + bool stackSleeping = true; + dgInt32 sleepCounter = 10000; + for (dgInt32 i = 0; i < count; i++) { + dgBody* const body = bodyArray[i].m_body; + dgAssert(body->IsRTTIType(dgBody::m_dynamicBodyRTTI) || body->IsRTTIType(dgBody::m_kinematicBody)); + + body->m_equilibrium = 1; + dgVector isMovingMask(body->m_veloc + body->m_omega + body->m_accel + body->m_alpha); + if ((isMovingMask.TestZero().GetSignMask() & 7) != 7) { + dgAssert(body->m_invMass.m_w); + if (body->IsRTTIType(dgBody::m_dynamicBodyRTTI)) { + body->IntegrateVelocity(timestep); + } + + dgAssert(body->m_accel.m_w == dgFloat32(0.0f)); + dgAssert(body->m_alpha.m_w == dgFloat32(0.0f)); + dgAssert(body->m_veloc.m_w == dgFloat32(0.0f)); + dgAssert(body->m_omega.m_w == dgFloat32(0.0f)); + dgFloat32 accel2 = body->m_accel.DotProduct(body->m_accel).GetScalar(); + dgFloat32 alpha2 = body->m_alpha.DotProduct(body->m_alpha).GetScalar(); + dgFloat32 speed2 = body->m_veloc.DotProduct(body->m_veloc).GetScalar(); + dgFloat32 omega2 = body->m_omega.DotProduct(body->m_omega).GetScalar(); + + maxAccel = dgMax(maxAccel, accel2); + maxAlpha = dgMax(maxAlpha, alpha2); + maxSpeed = dgMax(maxSpeed, speed2); + maxOmega = dgMax(maxOmega, omega2); + bool equilibrium = (accel2 < accelFreeze) && (alpha2 < accelFreeze) && (speed2 < speedFreeze) && (omega2 < speedFreeze); + if (equilibrium) { + dgVector veloc(body->m_veloc * velocDragVect); + dgVector omega(body->m_omega * velocDragVect); + body->m_veloc = (veloc.DotProduct(veloc) > m_velocTol) & veloc; + body->m_omega = (omega.DotProduct(omega) > m_velocTol) & omega; + } + + body->m_equilibrium = equilibrium ? 1 : 0; + stackSleeping &= equilibrium; + if (body->IsRTTIType(dgBody::m_dynamicBodyRTTI)) { + dgDynamicBody* const dynBody = (dgDynamicBody*)body; + sleepCounter = dgMin(sleepCounter, dynBody->m_sleepingCounter); + dynBody->m_sleepingCounter++; + } + + body->UpdateCollisionMatrix(timestep, threadID); + } + } + + if (cluster->m_jointCount) { + if (stackSleeping) { + for (dgInt32 i = 0; i < count; i++) { + dgBody* const body = bodyArray[i].m_body; + dgAssert(body->IsRTTIType(dgBody::m_dynamicBodyRTTI) || body->IsRTTIType(dgBody::m_kinematicBodyRTTI)); + body->m_accel = dgVector::m_zero; + body->m_alpha = dgVector::m_zero; + body->m_veloc = dgVector::m_zero; + body->m_omega = dgVector::m_zero; + body->m_sleeping = body->m_autoSleep; + // force entire island to equilibrium + body->m_equilibrium = 1; + } + } else { + bool state = (maxAccel > world->m_sleepTable[DG_SLEEP_ENTRIES - 1].m_maxAccel) || + (maxAlpha > world->m_sleepTable[DG_SLEEP_ENTRIES - 1].m_maxAlpha) || + (maxSpeed > world->m_sleepTable[DG_SLEEP_ENTRIES - 1].m_maxVeloc) || + (maxOmega > world->m_sleepTable[DG_SLEEP_ENTRIES - 1].m_maxOmega); + if (state) { + for (dgInt32 i = 0; i < count; i++) { + dgBody* const body = bodyArray[i].m_body; + if (body->IsRTTIType(dgBody::m_dynamicBodyRTTI)) { + dgDynamicBody* const dynBody = (dgDynamicBody*)body; + dynBody->m_sleepingCounter = 0; + } + } + } else { + if (count < DG_SMALL_ISLAND_COUNT) { + // delay small islands for about 10 seconds + sleepCounter >>= 8; + for (dgInt32 i = 0; i < count; i++) { + dgBody* const body = bodyArray[i].m_body; + body->m_equilibrium = 0; + } + } + dgInt32 timeScaleSleepCount = dgInt32(dgFloat32(60.0f) * sleepCounter * timestep); + + dgInt32 index = DG_SLEEP_ENTRIES; + for (dgInt32 i = 1; i < DG_SLEEP_ENTRIES; i ++) { + if (world->m_sleepTable[i].m_steps > timeScaleSleepCount) { + index = i; + break; + } + } + index --; + + bool state1 = (maxAccel < world->m_sleepTable[index].m_maxAccel) && + (maxAlpha < world->m_sleepTable[index].m_maxAlpha) && + (maxSpeed < world->m_sleepTable[index].m_maxVeloc) && + (maxOmega < world->m_sleepTable[index].m_maxOmega); + if (state1) { + for (dgInt32 i = 0; i < count; i++) { + dgBody* const body = bodyArray[i].m_body; + body->m_accel = dgVector::m_zero; + body->m_alpha = dgVector::m_zero; + body->m_veloc = dgVector::m_zero; + body->m_omega = dgVector::m_zero; + body->m_sleeping = body->m_autoSleep; + // force entire island to equilibrium + body->m_equilibrium = 1; + if (body->IsRTTIType(dgBody::m_dynamicBodyRTTI)) { + dgDynamicBody* const dynBody = (dgDynamicBody*)body; + dynBody->m_sleepingCounter = 0; + } + } + } + } + } + } +} diff --git a/thirdparty/src/newton/dgPhysics/dgWorldDynamicUpdate.h b/thirdparty/src/newton/dgPhysics/dgWorldDynamicUpdate.h new file mode 100644 index 000000000..6e98b76f4 --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgWorldDynamicUpdate.h @@ -0,0 +1,297 @@ +/* Copyright (c) <2003-2019> +* +* 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_WORLD_DYNAMICS_H_ +#define _DG_WORLD_DYNAMICS_H_ + + +#include "dgPhysicsStdafx.h" +#include "dgWorldDynamicsParallelSolver.h" + + +#define DG_BODY_LRU_STEP 2 +#define DG_MAX_SKELETON_JOINT_COUNT 256 +#define DG_MAX_CONTINUE_COLLISON_STEPS 8 +#define DG_SMALL_ISLAND_COUNT 32 + +#define DG_FREEZZING_VELOCITY_DRAG dgFloat32 (0.9f) +#define DG_SOLVER_MAX_ERROR (DG_FREEZE_MAG * dgFloat32 (0.5f)) + +#define DG_CCD_EXTRA_CONTACT_COUNT (8 * 3) +#define DG_PARALLEL_JOINT_COUNT_CUT_OFF (64) +//#define DG_PARALLEL_JOINT_COUNT_CUT_OFF (2) + + +// the solver is a RK order 4, but instead of weighting the intermediate derivative by the usual 1/6, 1/3, 1/3, 1/6 coefficients +// I am using 1/4, 1/4, 1/4, 1/4. +// This is correct.The weighting coefficients of any RK method comes from an arbitrary criteria +// solving for a set of linear equation on the coefficients. +// The standard coefficients just happen to lead to an accurate result because they are optimal +// with respect to a second order solution.For differential equations of higher order, then is not +// clear if the traditional weigh factors are any better than any other set of weighting factors. +// A different set of coefficient generates results that are not much different than the optimal set, +// but it allows for simpler calculation of the intermediate derivatives and also for less intermediate memory. +// For more detail on the derivation of the Runge Kutta coefficients you can go to: +// http://pathfinder.scar.utoronto.ca/~dyer/csca57/book_P/node51.html + +class dgBody; +class dgDynamicBody; +class dgWorldDynamicUpdateSyncDescriptor; + +class dgClusterCallbackStruct +{ + public: + dgWorld* m_world; + dgInt32 m_count; + dgInt32 m_strideInByte; + void* m_bodyArray; +}; + +class dgBodyInfo +{ + public: + dgBody* m_body; +}; + +class dgJointInfo +{ + public: + union + { + struct + { + dgBody* m_body; + dgInt32 m_bodyCount; + dgInt32 m_jointCount; + dgInt32 m_setId; + dgInt32 m_unUsed; + }; + struct + { + dgConstraint* m_joint; + dgInt32 m_m0; + dgInt32 m_m1; + dgInt32 m_pairStart; + dgInt32 m_pairCount; + }; + }; + dgFloat32 m_preconditioner0; + dgFloat32 m_preconditioner1; +}; + +class dgBodyJacobianPair +{ + public: + dgInt32 m_bodyIndex; + dgInt32 m_JointIndex; +}; + +class dgBodyCluster +{ + public: + dgInt32 m_bodyCount; + dgInt32 m_jointCount; + dgInt32 m_rowCount; + dgInt32 m_bodyStart; + dgInt32 m_jointStart; + dgInt32 m_rowStart; + dgInt16 m_hasSoftBodies; + dgInt16 m_isContinueCollision; +}; + +class dgJointImpulseInfo +{ + public: + dgContact* m_joint; + dgInt32 m_m0; + dgInt32 m_m1; + dgInt32 m_pairStart; + dgInt32 m_pairCount; + dgInt32 m_rhsStart; +}; + + +template +class dgQueue +{ + public: + dgQueue (T* const pool, dgInt32 size) + :m_pool (pool) + { + m_mod = size; + m_lastIndex = 0; + m_firstIndex = 0; + } + + void Insert (T info) + { + m_pool[m_firstIndex] = info; + m_firstIndex ++; + if (m_firstIndex >= m_mod) { + m_firstIndex = 0; + } + dgAssert (m_firstIndex != m_lastIndex); + } + + T Remove () + { + dgAssert (m_firstIndex != m_lastIndex); + + T element = m_pool[m_lastIndex]; + m_lastIndex ++; + if (m_lastIndex >= m_mod) { + m_lastIndex = 0; + } + + return element; + } + + void Reset () + { + m_lastIndex = m_firstIndex; + } + + bool IsEmpty () const + { + return (m_firstIndex == m_lastIndex); + } + + dgInt32 m_mod; + dgInt32 m_firstIndex; + dgInt32 m_lastIndex; + T* m_pool; +}; + +DG_MSC_VECTOR_ALIGNMENT +class dgLeftHandSide +{ + public: + dgJacobianPair m_Jt; + dgJacobianPair m_JMinv; +} DG_GCC_VECTOR_ALIGNMENT; + + +DG_MSC_VECTOR_ALIGNMENT +class dgRightHandSide +{ + public: + dgFloat32 m_force; + dgFloat32 m_diagDamp; + dgFloat32 m_invJinvMJt; + dgFloat32 m_coordenateAccel; + + dgFloat32 m_lowerBoundFrictionCoefficent; + dgFloat32 m_upperBoundFrictionCoefficent; + dgFloat32 m_deltaAccel; + dgFloat32 m_restitution; + + dgFloat32 m_maxImpact; + dgFloat32 m_penetration; + dgFloat32 m_diagonalRegularizer; + dgFloat32 m_penetrationStiffness; + + dgForceImpactPair* m_jointFeebackForce; + dgInt32 m_normalForceIndex; +} DG_GCC_VECTOR_ALIGNMENT; + +class dgJacobianMemory +{ + public: + dgJacobianMemory() {} + void Init (dgWorld* const world, dgInt32 rowsCount, dgInt32 bodyCount); + + dgJacobian* m_internalForcesBuffer; + dgLeftHandSide* m_leftHandSizeBuffer; + dgRightHandSide* m_righHandSizeBuffer; +}; + +class dgWorldDynamicUpdate +{ + public: + class dgParallelClusterArray; + + dgWorldDynamicUpdate(dgMemoryAllocator* const allocator); + ~dgWorldDynamicUpdate() {} + void UpdateDynamics (dgFloat32 timestep); + dgBody* GetClusterBody (const void* const cluster, dgInt32 index) const; + + dgJacobianMemory& GetSolverMemory() { return m_solverMemory; } + virtual dgInt32 GetJacobianDerivatives (dgContraintDescritor& constraintParamOut, dgJointInfo* const jointInfo, dgConstraint* const constraint, dgLeftHandSide* const matrixRow, dgRightHandSide* const rightHandSide, dgInt32 rowCount) const; + virtual void CalculateNetAcceleration (dgBody* const body, const dgVector& invTimeStep, const dgVector& accNorm) const; + + private: + static DG_INLINE dgInt32 CompareKey(dgInt32 highA, dgInt32 lowA, dgInt32 highB, dgInt32 lowB); + static dgInt32 CompareJointInfos(const dgJointInfo* const infoA, const dgJointInfo* const infoB, void* notUsed); + static dgInt32 CompareClusterInfos (const dgBodyCluster* const clusterA, const dgBodyCluster* const clusterB, void* notUsed); + + void BuildClusters(dgFloat32 timestep); + + dgBodyCluster MergeClusters(const dgBodyCluster* const clusterArray, dgInt32 clustersCount) const; + dgInt32 SortClusters(const dgBodyCluster* const cluster, dgFloat32 timestep, dgInt32 threadID) const; + + static dgInt32 CompareBodyJacobianPair(const dgBodyJacobianPair* const infoA, const dgBodyJacobianPair* const infoB, void* notUsed); + static void IntegrateClustersParallelKernel (void* const context, void* const worldContext, dgInt32 threadID); + static void CalculateClusterReactionForcesKernel (void* const context, void* const worldContext, dgInt32 threadID); + + void BuildJacobianMatrix (dgBodyCluster* const cluster, dgInt32 threadID, dgFloat32 timestep) const; + void ResolveClusterForces (dgBodyCluster* const cluster, dgInt32 threadID, dgFloat32 timestep) const; + void IntegrateReactionsForces(const dgBodyCluster* const cluster, dgInt32 threadID, dgFloat32 timestep) const; + void BuildJacobianMatrix (const dgBodyInfo* const bodyInfo, dgJointInfo* const jointInfo, dgJacobian* const internalForces, dgLeftHandSide* const matrixRow, dgRightHandSide* const rightHandSide, dgFloat32 forceImpulseScale) const; + void CalculateClusterReactionForces(const dgBodyCluster* const cluster, dgInt32 threadID, dgFloat32 timestep) const; + + void IntegrateInslandParallel(dgParallelClusterArray* const clusters, dgInt32 threadID); + void CalculateReactionForcesParallel(const dgBodyCluster* const clusters, dgInt32 clustersCount, dgFloat32 timestep); + + dgFloat32 CalculateJointForce(const dgJointInfo* const jointInfo, const dgBodyInfo* const bodyArray, dgJacobian* const internalForces, const dgLeftHandSide* const matrixRow, dgRightHandSide* const rightHandSide) const; + dgFloat32 CalculateJointForce_3_13(const dgJointInfo* const jointInfo, const dgBodyInfo* const bodyArray, dgJacobian* const internalForces, const dgLeftHandSide* const matrixRow, dgRightHandSide* const rightHandSide) const; + dgJacobian IntegrateForceAndToque(dgDynamicBody* const body, const dgVector& force, const dgVector& torque, const dgVector& timestep) const ; + + void IntegrateExternalForce(const dgBodyCluster* const cluster, dgFloat32 timestep, dgInt32 threadID) const; + void IntegrateVelocity (const dgBodyCluster* const cluster, dgFloat32 accelTolerance, dgFloat32 timestep, dgInt32 threadID) const; + void CalculateClusterContacts (dgBodyCluster* const cluster, dgFloat32 timestep, dgInt32 currLru, dgInt32 threadID) const; + + void CalculateImpulseVeloc(dgJointImpulseInfo* const jointInfo, const dgLeftHandSide* const leftHandSide, const dgRightHandSide* const rightHandSide, dgFloat32* const contactVeloc) const; + void ResolveImpulse(const dgJointInfo* const constraintArray, const dgLeftHandSide* const leftHandSide, dgRightHandSide* const rightHandSide, dgDownHeap& impactJoints) const; + dgFloat32 CalculateJointImpulse(const dgJointImpulseInfo* const jointInfo, const dgBodyInfo* const bodyArray, dgJacobian* const internalForces, const dgLeftHandSide* const matrixRow, const dgRightHandSide* const rightHandSide, dgFloat32* const relVel, dgFloat32* const outImpulse) const; + + dgJacobianMemory m_solverMemory; + dgParallelBodySolver m_parallelSolver; + dgBodyCluster* m_clusterData; + + dgInt32 m_bodies; + dgInt32 m_joints; + dgInt32 m_clusters; + dgInt32 m_markLru; + dgInt32 m_softBodiesCount; + mutable dgInt32 m_impulseLru; + mutable dgInt32 m_softBodyCriticalSectionLock; + + static dgVector m_velocTol; + + friend class dgWorld; + friend class dgJacobianMemory; + friend class dgSkeletonContainer; + friend class dgParallelBodySolver; + friend class dgSolverWorlkerThreads; +}; + +#endif + diff --git a/thirdparty/src/newton/dgPhysics/dgWorldDynamicsParallelSolver.cpp b/thirdparty/src/newton/dgPhysics/dgWorldDynamicsParallelSolver.cpp new file mode 100644 index 000000000..120b6170e --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgWorldDynamicsParallelSolver.cpp @@ -0,0 +1,1416 @@ +/* Copyright (c) <2003-2019> +* +* 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 "dgPhysicsStdafx.h" + +#include "dgBody.h" +#include "dgWorld.h" +#include "dgConstraint.h" +#include "dgDynamicBody.h" +#include "dgSkeletonContainer.h" +#include "dgWorldDynamicUpdate.h" +#include "dgWorldDynamicsParallelSolver.h" + +#define D_USE_SOA_SOLVER + + +class dgWorldDynamicUpdate::dgParallelClusterArray +{ + public: + dgParallelClusterArray(const dgBodyCluster* const clusterArray, dgInt32 clustersCount, dgFloat32 timestep) + :m_clusterArray(clusterArray) + ,m_clustersCount(clustersCount) + ,m_timestep(timestep) + ,m_atomicIndex(0) + { + } + + const dgBodyCluster* m_clusterArray; + dgInt32 m_clustersCount; + dgFloat32 m_timestep; + dgInt32 m_atomicIndex; +}; + +void dgWorldDynamicUpdate::CalculateReactionForcesParallel(const dgBodyCluster* const clusterArray, dgInt32 clustersCount, dgFloat32 timestep) +{ + DG_TRACKTIME(); + dgWorld* const world = (dgWorld*) this; + + dgBodyCluster cluster(MergeClusters(clusterArray, clustersCount)); + dgBodyInfo* const bodyArray = &world->m_bodiesMemory[m_bodies]; + dgJointInfo* const jointArray = &world->m_jointsMemory[m_joints]; + + if (world->GetCurrentPlugin()) { + dgWorldPlugin* const plugin = world->GetCurrentPlugin()->GetInfo().m_plugin; + plugin->CalculateJointForces(cluster, bodyArray, jointArray, timestep); + } else { + m_parallelSolver.CalculateJointForces(cluster, bodyArray, jointArray, timestep); + } + + dgParallelClusterArray integrateCluster(clusterArray, clustersCount, timestep); + const dgInt32 threadCounts = world->GetThreadCount(); + for (dgInt32 i = 0; i < threadCounts; i++) { + world->QueueJob(IntegrateClustersParallelKernel, &integrateCluster, world, "dgWorldDynamicUpdate::IntegrateClustersParallelKernel"); + } + world->SynchronizationBarrier(); +} + +void dgWorldDynamicUpdate::IntegrateClustersParallelKernel(void* const context, void* const worldPtr, dgInt32 threadID) +{ + DG_TRACKTIME(); + dgWorld* const world = (dgWorld*)worldPtr; + dgParallelClusterArray* const clusterArray = (dgParallelClusterArray*)context; + world->IntegrateInslandParallel(clusterArray, threadID); +} + +void dgWorldDynamicUpdate::IntegrateInslandParallel(dgParallelClusterArray* const clusterArray, dgInt32 threadID) +{ + DG_TRACKTIME(); + dgWorld* const world = (dgWorld*) this; + dgFloat32 timestep = clusterArray->m_timestep; + const dgInt32 clustersCount = clusterArray->m_clustersCount; + for (dgInt32 i = dgAtomicExchangeAndAdd(&clusterArray->m_atomicIndex, 1); i < clustersCount; i = dgAtomicExchangeAndAdd(&clusterArray->m_atomicIndex, 1)) { + world->IntegrateVelocity(&clusterArray->m_clusterArray[i], DG_SOLVER_MAX_ERROR, timestep, 0); + } +} + +dgBodyCluster dgWorldDynamicUpdate::MergeClusters(const dgBodyCluster* const clusterArray, dgInt32 clustersCount) const +{ + DG_TRACKTIME(); + dgBodyCluster cluster; + dgWorld* const world = (dgWorld*) this; + dgInt32 bodyCount = 0; + dgInt32 jointsCount = 0; + dgInt32 rowCount = 0; + for (dgInt32 i = 0; i < clustersCount; i++) { + const dgBodyCluster* const srcCluster = &clusterArray[i]; + bodyCount += srcCluster->m_bodyCount - 1; + jointsCount += srcCluster->m_jointCount; + rowCount += srcCluster->m_rowCount; + } + + world->m_solverMemory.Init(world, rowCount, 2 * bodyCount); + world->m_bodiesMemory.ResizeIfNecessary((m_bodies + bodyCount + 1) * sizeof(dgBodyInfo)); + world->m_jointsMemory.ResizeIfNecessary(m_joints + jointsCount + 32); + + dgBodyInfo* const bodyPtr = &world->m_bodiesMemory[0]; + dgJointInfo* const constraintPtr = &world->m_jointsMemory[0]; + + dgBodyInfo* const bodyArray = &bodyPtr[m_bodies]; + dgJointInfo* const jointArray = &constraintPtr[m_joints]; + + bodyArray[0].m_body = world->m_sentinelBody; + dgAssert(world->m_sentinelBody->m_index == 0); + + dgInt32 rowsCount = 0; + dgInt32 bodyIndex = 1; + dgInt32 jointIndex = 0; + for (dgInt32 i = 0; i < clustersCount; i++) { + const dgBodyCluster* const srcCluster = &clusterArray[i]; + rowsCount += srcCluster->m_rowCount; + + dgBodyInfo* const srcBodyArray = &bodyPtr[srcCluster->m_bodyStart]; + const dgInt32 count = srcCluster->m_bodyCount; + for (dgInt32 j = 1; j < count; j++) { + dgBody* const body = srcBodyArray[j].m_body; + bodyArray[bodyIndex].m_body = body; + body->m_index = bodyIndex; + bodyIndex++; + } + + dgJointInfo* const clusterJointArray = &constraintPtr[srcCluster->m_jointStart]; + const dgInt32 joints = srcCluster->m_jointCount; + for (dgInt32 j = 0; j < joints; j++) { + jointArray[jointIndex] = clusterJointArray[j]; + dgJointInfo* const jointInfo = &jointArray[jointIndex]; + + dgConstraint* const constraint = jointInfo->m_joint; + constraint->m_index = jointIndex; + const dgBody* const body0 = constraint->GetBody0(); + const dgBody* const body1 = constraint->GetBody1(); + + dgInt32 m0 = (body0->GetInvMass().m_w != dgFloat32(0.0f)) ? body0->m_index : 0; + dgInt32 m1 = (body1->GetInvMass().m_w != dgFloat32(0.0f)) ? body1->m_index : 0; + jointInfo->m_m0 = m0; + jointInfo->m_m1 = m1; + jointIndex++; + } + } + + cluster.m_bodyStart = 0; + cluster.m_jointStart = 0; + cluster.m_bodyCount = bodyIndex; + cluster.m_jointCount = jointsCount; + cluster.m_rowCount = rowsCount; + + cluster.m_rowStart = 0; + cluster.m_isContinueCollision = 0; + cluster.m_hasSoftBodies = 0; + + return cluster; +} + +void dgParallelBodySolver::InitWeights() +{ + DG_TRACKTIME(); + const dgJointInfo* const jointArray = m_jointArray; + const dgInt32 jointCount = m_cluster->m_jointCount; + dgBodyProxy* const weight = m_bodyProxyArray; + memset(m_bodyProxyArray, 0, m_cluster->m_bodyCount * sizeof(dgBodyProxy)); + for (dgInt32 i = 0; i < jointCount; i++) { + const dgJointInfo* const jointInfo = &jointArray[i]; + const dgInt32 m0 = jointInfo->m_m0; + const dgInt32 m1 = jointInfo->m_m1; + weight[m0].m_weight += dgFloat32(1.0f); + weight[m1].m_weight += dgFloat32(1.0f); + } + m_bodyProxyArray[0].m_weight = dgFloat32(1.0f); + + dgFloat32 extraPasses = dgFloat32(0.0f); + const dgInt32 bodyCount = m_cluster->m_bodyCount; + + dgSkeletonList& skeletonList = *m_world; + const dgInt32 lru = skeletonList.m_lruMarker; + skeletonList.m_lruMarker += 1; + + m_skeletonCount = 0; + for (dgInt32 i = 1; i < bodyCount; i++) { + extraPasses = dgMax(weight[i].m_weight, extraPasses); + + dgDynamicBody* const body = (dgDynamicBody*)m_bodyArray[i].m_body; + dgSkeletonContainer* const container = body->GetSkeleton(); + if (container && (container->m_lru != lru)) { + container->m_lru = lru; + m_skeletonArray[m_skeletonCount] = container; + m_skeletonCount ++; + } + } + const dgInt32 conectivity = 7; + m_solverPasses += 2 * dgInt32(extraPasses) / conectivity + 1; +} + +void dgParallelBodySolver::InitBodyArray() +{ + for (dgInt32 i = 0; i < m_threadCounts; i++) { + m_world->QueueJob(InitBodyArrayKernel, this, NULL, "dgParallelBodySolver::InitBodyArray"); + } + m_world->SynchronizationBarrier(); + m_bodyProxyArray->m_invWeight = dgFloat32(1.0f); +} + +void dgParallelBodySolver::InitBodyArrayKernel(void* const context, void* const, dgInt32 threadID) +{ + DG_TRACKTIME(); + dgParallelBodySolver* const me = (dgParallelBodySolver*)context; + me->InitBodyArray(threadID); +} + +void dgParallelBodySolver::InitJacobianMatrixKernel(void* const context, void* const, dgInt32 threadID) +{ + DG_TRACKTIME(); + dgParallelBodySolver* const me = (dgParallelBodySolver*)context; + me->InitJacobianMatrix(threadID); +} + +void dgParallelBodySolver::CalculateJointsForceKernel(void* const context, void* const, dgInt32 threadID) +{ + dgParallelBodySolver* const me = (dgParallelBodySolver*)context; + me->CalculateJointsForce(threadID); +} + +void dgParallelBodySolver::CalculateJointsAccelerationKernel(void* const context, void* const, dgInt32 threadID) +{ + DG_TRACKTIME(); + dgParallelBodySolver* const me = (dgParallelBodySolver*)context; + me->CalculateJointsAcceleration(threadID); +} + +void dgParallelBodySolver::IntegrateBodiesVelocityKernel(void* const context, void* const worldContext, dgInt32 threadID) +{ + DG_TRACKTIME(); + dgParallelBodySolver* const me = (dgParallelBodySolver*)context; + me->IntegrateBodiesVelocity(threadID); +} + +void dgParallelBodySolver::CalculateBodiesAccelerationKernel(void* const context, void* const, dgInt32 threadID) +{ + DG_TRACKTIME(); + dgParallelBodySolver* const me = (dgParallelBodySolver*)context; + me->CalculateBodiesAcceleration(threadID); +} + +void dgParallelBodySolver::UpdateForceFeedbackKernel(void* const context, void* const, dgInt32 threadID) +{ + DG_TRACKTIME(); + dgParallelBodySolver* const me = (dgParallelBodySolver*)context; + me->UpdateForceFeedback(threadID); +} + +void dgParallelBodySolver::UpdateKinematicFeedbackKernel(void* const context, void* const, dgInt32 threadID) +{ + DG_TRACKTIME(); + dgParallelBodySolver* const me = (dgParallelBodySolver*)context; + me->UpdateKinematicFeedback(threadID); +} + +void dgParallelBodySolver::TransposeMassMatrixKernel(void* const context, void* const, dgInt32 threadID) +{ + DG_TRACKTIME(); + dgParallelBodySolver* const me = (dgParallelBodySolver*)context; + me->TransposeMassMatrix(threadID); +} + +void dgParallelBodySolver::UpdateRowAccelerationKernel(void* const context, void* const, dgInt32 threadID) +{ + DG_TRACKTIME(); + dgParallelBodySolver* const me = (dgParallelBodySolver*)context; + me->UpdateRowAcceleration(threadID); +} + +DG_INLINE void dgParallelBodySolver::TransposeRow(dgSolverSoaElement* const row, const dgJointInfo* const jointInfoArray, dgInt32 index) +{ + const dgLeftHandSide* const leftHandSide = &m_world->m_solverMemory.m_leftHandSizeBuffer[0]; + const dgRightHandSide* const rightHandSide = &m_world->m_solverMemory.m_righHandSizeBuffer[0]; + dgInt32* const normalIndex = (dgInt32*) &row->m_normalForceIndex[0]; + if (jointInfoArray[0].m_pairCount == jointInfoArray[DG_WORK_GROUP_SIZE - 1].m_pairCount) { + for (dgInt32 i = 0; i < DG_WORK_GROUP_SIZE; i++) { + const dgJointInfo* const jointInfo = &jointInfoArray[i]; + const dgLeftHandSide* const lhs = &leftHandSide[jointInfo->m_pairStart + index]; + const dgRightHandSide* const rhs = &rightHandSide[jointInfo->m_pairStart + index]; + + row->m_Jt.m_jacobianM0.m_linear.m_x[i] = lhs->m_Jt.m_jacobianM0.m_linear.m_x; + row->m_Jt.m_jacobianM0.m_linear.m_y[i] = lhs->m_Jt.m_jacobianM0.m_linear.m_y; + row->m_Jt.m_jacobianM0.m_linear.m_z[i] = lhs->m_Jt.m_jacobianM0.m_linear.m_z; + row->m_Jt.m_jacobianM0.m_angular.m_x[i] = lhs->m_Jt.m_jacobianM0.m_angular.m_x; + row->m_Jt.m_jacobianM0.m_angular.m_y[i] = lhs->m_Jt.m_jacobianM0.m_angular.m_y; + row->m_Jt.m_jacobianM0.m_angular.m_z[i] = lhs->m_Jt.m_jacobianM0.m_angular.m_z; + row->m_Jt.m_jacobianM1.m_linear.m_x[i] = lhs->m_Jt.m_jacobianM1.m_linear.m_x; + row->m_Jt.m_jacobianM1.m_linear.m_y[i] = lhs->m_Jt.m_jacobianM1.m_linear.m_y; + row->m_Jt.m_jacobianM1.m_linear.m_z[i] = lhs->m_Jt.m_jacobianM1.m_linear.m_z; + row->m_Jt.m_jacobianM1.m_angular.m_x[i] = lhs->m_Jt.m_jacobianM1.m_angular.m_x; + row->m_Jt.m_jacobianM1.m_angular.m_y[i] = lhs->m_Jt.m_jacobianM1.m_angular.m_y; + row->m_Jt.m_jacobianM1.m_angular.m_z[i] = lhs->m_Jt.m_jacobianM1.m_angular.m_z; + + row->m_JMinv.m_jacobianM0.m_linear.m_x[i] = lhs->m_JMinv.m_jacobianM0.m_linear.m_x; + row->m_JMinv.m_jacobianM0.m_linear.m_y[i] = lhs->m_JMinv.m_jacobianM0.m_linear.m_y; + row->m_JMinv.m_jacobianM0.m_linear.m_z[i] = lhs->m_JMinv.m_jacobianM0.m_linear.m_z; + row->m_JMinv.m_jacobianM0.m_angular.m_x[i] = lhs->m_JMinv.m_jacobianM0.m_angular.m_x; + row->m_JMinv.m_jacobianM0.m_angular.m_y[i] = lhs->m_JMinv.m_jacobianM0.m_angular.m_y; + row->m_JMinv.m_jacobianM0.m_angular.m_z[i] = lhs->m_JMinv.m_jacobianM0.m_angular.m_z; + row->m_JMinv.m_jacobianM1.m_linear.m_x[i] = lhs->m_JMinv.m_jacobianM1.m_linear.m_x; + row->m_JMinv.m_jacobianM1.m_linear.m_y[i] = lhs->m_JMinv.m_jacobianM1.m_linear.m_y; + row->m_JMinv.m_jacobianM1.m_linear.m_z[i] = lhs->m_JMinv.m_jacobianM1.m_linear.m_z; + row->m_JMinv.m_jacobianM1.m_angular.m_x[i] = lhs->m_JMinv.m_jacobianM1.m_angular.m_x; + row->m_JMinv.m_jacobianM1.m_angular.m_y[i] = lhs->m_JMinv.m_jacobianM1.m_angular.m_y; + row->m_JMinv.m_jacobianM1.m_angular.m_z[i] = lhs->m_JMinv.m_jacobianM1.m_angular.m_z; + + row->m_force[i] = rhs->m_force; + row->m_diagDamp[i] = rhs->m_diagDamp; + row->m_invJinvMJt[i] = rhs->m_invJinvMJt; + row->m_coordenateAccel[i] = rhs->m_coordenateAccel; + row->m_lowerBoundFrictionCoefficent[i] = rhs->m_lowerBoundFrictionCoefficent; + row->m_upperBoundFrictionCoefficent[i] = rhs->m_upperBoundFrictionCoefficent; + //row->m_normalForceIndex.m_i[i] = (rhs->m_normalForceIndex + 1) * DG_WORK_GROUP_SIZE + i; + normalIndex[i] = (rhs->m_normalForceIndex + 1) * DG_WORK_GROUP_SIZE + i; + } + } else { + memset(row, 0, sizeof (dgSolverSoaElement)); + for (dgInt32 i = 0; i < DG_WORK_GROUP_SIZE; i++) { + if (index < jointInfoArray[i].m_pairCount) { + const dgJointInfo* const jointInfo = &jointInfoArray[i]; + const dgLeftHandSide* const lhs = &leftHandSide[jointInfo->m_pairStart + index]; + const dgRightHandSide* const rhs = &rightHandSide[jointInfo->m_pairStart + index]; + + row->m_Jt.m_jacobianM0.m_linear.m_x[i] = lhs->m_Jt.m_jacobianM0.m_linear.m_x; + row->m_Jt.m_jacobianM0.m_linear.m_y[i] = lhs->m_Jt.m_jacobianM0.m_linear.m_y; + row->m_Jt.m_jacobianM0.m_linear.m_z[i] = lhs->m_Jt.m_jacobianM0.m_linear.m_z; + row->m_Jt.m_jacobianM0.m_angular.m_x[i] = lhs->m_Jt.m_jacobianM0.m_angular.m_x; + row->m_Jt.m_jacobianM0.m_angular.m_y[i] = lhs->m_Jt.m_jacobianM0.m_angular.m_y; + row->m_Jt.m_jacobianM0.m_angular.m_z[i] = lhs->m_Jt.m_jacobianM0.m_angular.m_z; + row->m_Jt.m_jacobianM1.m_linear.m_x[i] = lhs->m_Jt.m_jacobianM1.m_linear.m_x; + row->m_Jt.m_jacobianM1.m_linear.m_y[i] = lhs->m_Jt.m_jacobianM1.m_linear.m_y; + row->m_Jt.m_jacobianM1.m_linear.m_z[i] = lhs->m_Jt.m_jacobianM1.m_linear.m_z; + row->m_Jt.m_jacobianM1.m_angular.m_x[i] = lhs->m_Jt.m_jacobianM1.m_angular.m_x; + row->m_Jt.m_jacobianM1.m_angular.m_y[i] = lhs->m_Jt.m_jacobianM1.m_angular.m_y; + row->m_Jt.m_jacobianM1.m_angular.m_z[i] = lhs->m_Jt.m_jacobianM1.m_angular.m_z; + + row->m_JMinv.m_jacobianM0.m_linear.m_x[i] = lhs->m_JMinv.m_jacobianM0.m_linear.m_x; + row->m_JMinv.m_jacobianM0.m_linear.m_y[i] = lhs->m_JMinv.m_jacobianM0.m_linear.m_y; + row->m_JMinv.m_jacobianM0.m_linear.m_z[i] = lhs->m_JMinv.m_jacobianM0.m_linear.m_z; + row->m_JMinv.m_jacobianM0.m_angular.m_x[i] = lhs->m_JMinv.m_jacobianM0.m_angular.m_x; + row->m_JMinv.m_jacobianM0.m_angular.m_y[i] = lhs->m_JMinv.m_jacobianM0.m_angular.m_y; + row->m_JMinv.m_jacobianM0.m_angular.m_z[i] = lhs->m_JMinv.m_jacobianM0.m_angular.m_z; + row->m_JMinv.m_jacobianM1.m_linear.m_x[i] = lhs->m_JMinv.m_jacobianM1.m_linear.m_x; + row->m_JMinv.m_jacobianM1.m_linear.m_y[i] = lhs->m_JMinv.m_jacobianM1.m_linear.m_y; + row->m_JMinv.m_jacobianM1.m_linear.m_z[i] = lhs->m_JMinv.m_jacobianM1.m_linear.m_z; + row->m_JMinv.m_jacobianM1.m_angular.m_x[i] = lhs->m_JMinv.m_jacobianM1.m_angular.m_x; + row->m_JMinv.m_jacobianM1.m_angular.m_y[i] = lhs->m_JMinv.m_jacobianM1.m_angular.m_y; + row->m_JMinv.m_jacobianM1.m_angular.m_z[i] = lhs->m_JMinv.m_jacobianM1.m_angular.m_z; + + row->m_force[i] = rhs->m_force; + row->m_diagDamp[i] = rhs->m_diagDamp; + row->m_invJinvMJt[i] = rhs->m_invJinvMJt; + row->m_coordenateAccel[i] = rhs->m_coordenateAccel; + row->m_lowerBoundFrictionCoefficent[i] = rhs->m_lowerBoundFrictionCoefficent; + row->m_upperBoundFrictionCoefficent[i] = rhs->m_upperBoundFrictionCoefficent; + //row->m_normalForceIndex.m_i[i] = (rhs->m_normalForceIndex + 1) * DG_WORK_GROUP_SIZE + i; + normalIndex[i] = (rhs->m_normalForceIndex + 1) * DG_WORK_GROUP_SIZE + i; + } else { + //row->m_normalForceIndex.m_i[i] = i; + normalIndex[i] = i; + } + } + } +} + +void dgParallelBodySolver::TransposeMassMatrix(dgInt32 threadID) +{ + const dgJointInfo* const jointInfoArray = m_jointArray; + dgSolverSoaElement* const massMatrixArray = &m_massMatrix[0]; + + const dgInt32 step = m_threadCounts; + const dgInt32 jointCount = m_jointCount; + for (dgInt32 i = threadID; i < jointCount; i += step) { + const dgInt32 index = i * DG_WORK_GROUP_SIZE; + const dgInt32 rowCount = jointInfoArray[index].m_pairCount; + const dgInt32 rowSoaStart = dgAtomicExchangeAndAdd(&m_soaRowsCount, rowCount); + m_soaRowStart[i] = rowSoaStart; + for (dgInt32 j = 0; j < rowCount; j++) { + dgSolverSoaElement* const row = &massMatrixArray[rowSoaStart + j]; + TransposeRow(row, &jointInfoArray[index], j); + } + } +} + +DG_INLINE void dgParallelBodySolver::BuildJacobianMatrix(dgJointInfo* const jointInfo, dgLeftHandSide* const leftHandSide, dgRightHandSide* const rightHandSide, dgJacobian* const internalForces) +{ + const dgInt32 m0 = jointInfo->m_m0; + const dgInt32 m1 = jointInfo->m_m1; + const dgInt32 index = jointInfo->m_pairStart; + const dgInt32 count = jointInfo->m_pairCount; + const dgDynamicBody* const body0 = (dgDynamicBody*)m_bodyArray[m0].m_body; + const dgDynamicBody* const body1 = (dgDynamicBody*)m_bodyArray[m1].m_body; + const bool isBilateral = jointInfo->m_joint->IsBilateral(); + + const dgMatrix invInertia0 = body0->m_invWorldInertiaMatrix; + const dgMatrix invInertia1 = body1->m_invWorldInertiaMatrix; + const dgVector invMass0(body0->m_invMass[3]); + const dgVector invMass1(body1->m_invMass[3]); + + dgWorkGroupFloat force0(dgVector::m_zero); + if (body0->IsRTTIType(dgBody::m_dynamicBodyRTTI)) { + force0 = dgWorkGroupFloat(body0->m_externalForce, body0->m_externalTorque); + } + + dgWorkGroupFloat force1(dgVector::m_zero); + if (body1->IsRTTIType(dgBody::m_dynamicBodyRTTI)) { + force1 = dgWorkGroupFloat(body1->m_externalForce, body1->m_externalTorque); + } + + jointInfo->m_preconditioner0 = dgFloat32(1.0f); + jointInfo->m_preconditioner1 = dgFloat32(1.0f); + if ((invMass0.GetScalar() > dgFloat32(0.0f)) && (invMass1.GetScalar() > dgFloat32(0.0f)) && !(body0->GetSkeleton() && body1->GetSkeleton())) { + const dgFloat32 mass0 = body0->GetMass().m_w; + const dgFloat32 mass1 = body1->GetMass().m_w; + if (mass0 > (DG_DIAGONAL_PRECONDITIONER * mass1)) { + jointInfo->m_preconditioner0 = mass0 / (mass1 * DG_DIAGONAL_PRECONDITIONER); + } else if (mass1 > (DG_DIAGONAL_PRECONDITIONER * mass0)) { + jointInfo->m_preconditioner1 = mass1 / (mass0 * DG_DIAGONAL_PRECONDITIONER); + } + } + + dgWorkGroupFloat forceAcc0(dgVector::m_zero); + dgWorkGroupFloat forceAcc1(dgVector::m_zero); + + const dgWorkGroupFloat weight0(m_bodyProxyArray[m0].m_weight * jointInfo->m_preconditioner0); + const dgWorkGroupFloat weight1(m_bodyProxyArray[m1].m_weight * jointInfo->m_preconditioner0); + + const dgFloat32 forceImpulseScale = dgFloat32(1.0f); + const dgFloat32 preconditioner0 = jointInfo->m_preconditioner0; + const dgFloat32 preconditioner1 = jointInfo->m_preconditioner1; + + for (dgInt32 i = 0; i < count; i++) { + dgLeftHandSide* const lhs = &leftHandSide[index + i]; + dgRightHandSide* const rhs = &rightHandSide[index + i]; + + lhs->m_JMinv.m_jacobianM0.m_linear = lhs->m_Jt.m_jacobianM0.m_linear * invMass0; + lhs->m_JMinv.m_jacobianM0.m_angular = invInertia0.RotateVector(lhs->m_Jt.m_jacobianM0.m_angular); + lhs->m_JMinv.m_jacobianM1.m_linear = lhs->m_Jt.m_jacobianM1.m_linear * invMass1; + lhs->m_JMinv.m_jacobianM1.m_angular = invInertia1.RotateVector(lhs->m_Jt.m_jacobianM1.m_angular); + + const dgWorkGroupFloat& JMinvM0 = (dgWorkGroupFloat&)lhs->m_JMinv.m_jacobianM0; + const dgWorkGroupFloat& JMinvM1 = (dgWorkGroupFloat&)lhs->m_JMinv.m_jacobianM1; + dgWorkGroupFloat tmpAccel((JMinvM0 * force0).MulAdd(JMinvM1, force1)); + + dgFloat32 extenalAcceleration = -tmpAccel.AddHorizontal(); + rhs->m_deltaAccel = extenalAcceleration * forceImpulseScale; + rhs->m_coordenateAccel += extenalAcceleration * forceImpulseScale; + dgAssert(rhs->m_jointFeebackForce); + const dgFloat32 force = rhs->m_jointFeebackForce->GetInitiailGuess() * forceImpulseScale; + //const dgFloat32 force = rhs->m_jointFeebackForce->m_force * forceImpulseScale; + + rhs->m_force = isBilateral ? dgClamp(force, rhs->m_lowerBoundFrictionCoefficent, rhs->m_upperBoundFrictionCoefficent) : force; + rhs->m_maxImpact = dgFloat32(0.0f); + + const dgWorkGroupFloat& JtM0 = (dgWorkGroupFloat&)lhs->m_Jt.m_jacobianM0; + const dgWorkGroupFloat& JtM1 = (dgWorkGroupFloat&)lhs->m_Jt.m_jacobianM1; + + dgWorkGroupFloat tmpDiag((weight0 * JMinvM0 * JtM0).MulAdd(weight1, JMinvM1 * JtM1)); + dgFloat32 diag = tmpDiag.AddHorizontal(); + dgAssert(diag > dgFloat32(0.0f)); + rhs->m_diagDamp = diag * rhs->m_diagonalRegularizer; + diag *= (dgFloat32(1.0f) + rhs->m_diagonalRegularizer); + rhs->m_invJinvMJt = dgFloat32(1.0f) / diag; + + dgWorkGroupFloat f0(rhs->m_force * preconditioner0); + dgWorkGroupFloat f1(rhs->m_force * preconditioner1); + forceAcc0 = forceAcc0.MulAdd(JtM0, f0); + forceAcc1 = forceAcc1.MulAdd(JtM1, f1); + } + + if (m0) { + dgWorkGroupFloat& out = (dgWorkGroupFloat&)internalForces[m0]; + dgScopeSpinPause lock(&m_bodyProxyArray[m0].m_lock); + out = out + forceAcc0; + } + if (m1) { + dgWorkGroupFloat& out = (dgWorkGroupFloat&)internalForces[m1]; + dgScopeSpinPause lock(&m_bodyProxyArray[m1].m_lock); + out = out + forceAcc1; + } +} + +DG_INLINE void dgParallelBodySolver::SortWorkGroup(dgInt32 base) const +{ + dgJointInfo* const jointArray = m_jointArray; + for (dgInt32 i = 1; i < DG_WORK_GROUP_SIZE; i++) { + dgInt32 index = base + i; + const dgJointInfo tmp(jointArray[index]); + for (; (index > base) && (jointArray[index - 1].m_pairCount < tmp.m_pairCount); index--) { + jointArray[index] = jointArray[index - 1]; + } + jointArray[index] = tmp; + } +} + +void dgParallelBodySolver::CalculateBodiesAcceleration() +{ + for (dgInt32 i = 0; i < m_threadCounts; i++) { + m_world->QueueJob(CalculateBodiesAccelerationKernel, this, NULL, "dgParallelBodySolver::CalculateBodiesAcceleration"); + } + m_world->SynchronizationBarrier(); +} + +void dgParallelBodySolver::UpdateForceFeedback() +{ + for (dgInt32 i = 0; i < m_threadCounts; i++) { + m_world->QueueJob(UpdateForceFeedbackKernel, this, NULL, "dgParallelBodySolver::UpdateForceFeedback"); + } + m_world->SynchronizationBarrier(); +} + +void dgParallelBodySolver::UpdateKinematicFeedback() +{ + for (dgInt32 i = 0; i < m_threadCounts; i++) { + m_world->QueueJob(UpdateKinematicFeedbackKernel, this, NULL, "dgParallelBodySolver::UpdateKinematicFeedback"); + } + m_world->SynchronizationBarrier(); +} + +void dgParallelBodySolver::InitJacobianMatrix(dgInt32 threadID) +{ + dgLeftHandSide* const leftHandSide = &m_world->m_solverMemory.m_leftHandSizeBuffer[0]; + dgRightHandSide* const rightHandSide = &m_world->m_solverMemory.m_righHandSizeBuffer[0]; + dgJacobian* const internalForces = &m_world->m_solverMemory.m_internalForcesBuffer[0]; + + dgContraintDescritor constraintParams; + constraintParams.m_world = m_world; + constraintParams.m_threadIndex = threadID; + constraintParams.m_timestep = m_timestep; + constraintParams.m_invTimestep = m_invTimestep; + + const dgInt32 step = m_threadCounts; + const dgInt32 jointCount = m_cluster->m_jointCount; + for (dgInt32 i = threadID; i < jointCount; i += step) { + dgJointInfo* const jointInfo = &m_jointArray[i]; + dgConstraint* const constraint = jointInfo->m_joint; + dgAssert(jointInfo->m_m0 >= 0); + dgAssert(jointInfo->m_m1 >= 0); + dgAssert(jointInfo->m_m0 != jointInfo->m_m1); + const dgInt32 rowBase = dgAtomicExchangeAndAdd(&m_jacobianMatrixRowAtomicIndex, jointInfo->m_pairCount); + m_world->GetJacobianDerivatives(constraintParams, jointInfo, constraint, leftHandSide, rightHandSide, rowBase); + BuildJacobianMatrix(jointInfo, leftHandSide, rightHandSide, internalForces); + } +} + +dgInt32 dgParallelBodySolver::CompareJointInfos(const dgJointInfo* const infoA, const dgJointInfo* const infoB, void* notUsed) +{ + const dgInt32 restingA = (infoA->m_joint->m_body0->m_resting & infoA->m_joint->m_body1->m_resting) ? 1 : 0; + const dgInt32 restingB = (infoB->m_joint->m_body0->m_resting & infoB->m_joint->m_body1->m_resting) ? 1 : 0; + + const dgInt32 countA = (restingA << 24) + infoA->m_pairCount; + const dgInt32 countB = (restingB << 24) + infoB->m_pairCount; + + if (countA < countB) { + return 1; + } + if (countA > countB) { + return -1; + } + return 0; +} + +void dgParallelBodySolver::InitJacobianMatrix() +{ + m_jacobianMatrixRowAtomicIndex = 0; + dgJacobian* const internalForces = &m_world->m_solverMemory.m_internalForcesBuffer[0]; + memset(internalForces, 0, m_cluster->m_bodyCount * sizeof (dgJacobian)); + + for (dgInt32 i = 0; i < m_threadCounts; i++) { + m_world->QueueJob(InitJacobianMatrixKernel, this, NULL, "dgParallelBodySolver::InitJacobianMatrix"); + } + m_world->SynchronizationBarrier(); + +#ifdef D_USE_SOA_SOLVER + dgJointInfo* const jointArray = m_jointArray; +// dgSort(jointArray, m_cluster->m_jointCount, CompareJointInfos); + dgParallelSort(*m_world, jointArray, m_cluster->m_jointCount, CompareJointInfos); + + const dgInt32 jointCount = m_jointCount * DG_WORK_GROUP_SIZE; + for (dgInt32 i = m_cluster->m_jointCount; i < jointCount; i++) { + memset(&jointArray[i], 0, sizeof(dgJointInfo)); + } + + dgInt32 size = 0; + for (dgInt32 i = 0; i < jointCount; i += DG_WORK_GROUP_SIZE) { + const dgConstraint* const joint1 = jointArray[i + DG_WORK_GROUP_SIZE - 1].m_joint; + if (joint1) { + if (!(joint1->m_body0->m_resting & joint1->m_body1->m_resting)) { + const dgConstraint* const joint0 = jointArray[i].m_joint; + if (joint0->m_body0->m_resting & joint0->m_body1->m_resting) { + SortWorkGroup(i); + } + } + for (dgInt32 j = 0; j < DG_WORK_GROUP_SIZE; j++) { + dgConstraint* const joint = jointArray[i + j].m_joint; + joint->m_index = i + j; + } + } else { + SortWorkGroup(i); + for (dgInt32 j = 0; j < DG_WORK_GROUP_SIZE; j ++) { + dgConstraint* const joint = jointArray[i + j].m_joint; + if (joint) { + joint->m_index = i + j; + } + } + } + size += jointArray[i].m_pairCount; + } + m_massMatrix.ResizeIfNecessary(size); + + m_soaRowsCount = 0; + for (dgInt32 i = 0; i < m_threadCounts; i++) { + m_world->QueueJob(TransposeMassMatrixKernel, this, NULL, "dgParallelBodySolver::TransposeMassMatrix"); + } + m_world->SynchronizationBarrier(); +#endif +} + +void dgParallelBodySolver::InitBodyArray(dgInt32 threadID) +{ + const dgBodyInfo* const bodyArray = m_bodyArray; + dgBodyProxy* const bodyProxyArray = m_bodyProxyArray; + + const dgInt32 step = m_threadCounts;; + const dgInt32 bodyCount = m_cluster->m_bodyCount; + for (dgInt32 i = threadID; i < bodyCount; i += step) { + const dgBodyInfo* const bodyInfo = &bodyArray[i]; + dgBody* const body = (dgDynamicBody*)bodyInfo->m_body; + body->AddDampingAcceleration(m_timestep); + body->CalcInvInertiaMatrix(); + + body->m_accel = body->m_veloc; + body->m_alpha = body->m_omega; + + const dgFloat32 w = bodyProxyArray[i].m_weight ? bodyProxyArray[i].m_weight : dgFloat32(1.0f); + bodyProxyArray[i].m_weight = w; + bodyProxyArray[i].m_invWeight = dgFloat32(1.0f) / w; + } +} + +void dgParallelBodySolver::CalculateJointsAcceleration(dgInt32 threadID) +{ + dgJointAccelerationDecriptor joindDesc; + joindDesc.m_timeStep = m_timestepRK; + joindDesc.m_invTimeStep = m_invTimestepRK; + joindDesc.m_firstPassCoefFlag = m_firstPassCoef; + dgRightHandSide* const rightHandSide = &m_world->m_solverMemory.m_righHandSizeBuffer[0]; + const dgLeftHandSide* const leftHandSide = &m_world->m_solverMemory.m_leftHandSizeBuffer[0]; + + const dgInt32 step = m_threadCounts; + const dgInt32 jointCount = m_cluster->m_jointCount; + for (dgInt32 i = threadID; i < jointCount; i += step) { + dgJointInfo* const jointInfo = &m_jointArray[i]; + dgConstraint* const constraint = jointInfo->m_joint; + const dgInt32 pairStart = jointInfo->m_pairStart; + joindDesc.m_rowsCount = jointInfo->m_pairCount; + joindDesc.m_leftHandSide = &leftHandSide[pairStart]; + joindDesc.m_rightHandSide = &rightHandSide[pairStart]; + + constraint->JointAccelerations(&joindDesc); + } +} + +void dgParallelBodySolver::CalculateBodiesAcceleration(dgInt32 threadID) +{ + dgVector invTime(m_invTimestep); + dgFloat32 maxAccNorm2 = DG_SOLVER_MAX_ERROR * DG_SOLVER_MAX_ERROR; + + const dgInt32 step = m_threadCounts; + const dgInt32 bodyCount = m_cluster->m_bodyCount; + for (dgInt32 i = threadID; i < bodyCount; i += step) { + dgDynamicBody* const body = (dgDynamicBody*)m_bodyArray[i].m_body; + m_world->CalculateNetAcceleration(body, invTime, maxAccNorm2); + } +} + +void dgParallelBodySolver::UpdateKinematicFeedback(dgInt32 threadID) +{ + const dgInt32 step = m_threadCounts; + const dgInt32 jointCount = m_cluster->m_jointCount; + for (dgInt32 i = threadID; i < jointCount; i += step) { + dgJointInfo* const jointInfo = &m_jointArray[i]; + if (jointInfo->m_joint->m_updaFeedbackCallback) { + jointInfo->m_joint->m_updaFeedbackCallback(*jointInfo->m_joint, m_timestep, threadID); + } + } +} + +void dgParallelBodySolver::UpdateRowAcceleration(dgInt32 threadID) +{ + dgSolverSoaElement* const massMatrix = &m_massMatrix[0]; + const dgRightHandSide* const rightHandSide = &m_world->m_solverMemory.m_righHandSizeBuffer[0]; + + const dgInt32* const soaRowStart = m_soaRowStart; + const dgJointInfo* const jointInfoArray = m_jointArray; + + const dgInt32 step = m_threadCounts; + const dgInt32 jointCount = m_jointCount; + for (dgInt32 i = threadID; i < jointCount; i += step) { + const dgInt32 rowStart = soaRowStart[i]; + const dgJointInfo* const jointInfoBase = &jointInfoArray[i * DG_WORK_GROUP_SIZE]; + + for (dgInt32 j = 0; j < DG_WORK_GROUP_SIZE; j++) { + const dgJointInfo* const jointInfo = &jointInfoBase[j]; + if (jointInfo->m_joint) { + dgInt32 const rowCount = jointInfo->m_pairCount; + dgInt32 const rowStartBase = jointInfo->m_pairStart; + for (dgInt32 k = 0; k < rowCount; k++) { + dgSolverSoaElement* const row = &massMatrix[rowStart + k]; + row->m_coordenateAccel[j] = rightHandSide[k + rowStartBase].m_coordenateAccel; + } + } + } + } +} + +void dgParallelBodySolver::UpdateForceFeedback(dgInt32 threadID) +{ + const dgRightHandSide* const rightHandSide = &m_world->m_solverMemory.m_righHandSizeBuffer[0]; + dgInt32 hasJointFeeback = 0; + + const dgInt32 step = m_threadCounts; + const dgInt32 jointCount = m_cluster->m_jointCount; + for (dgInt32 i = threadID; i < jointCount; i += step) { + dgJointInfo* const jointInfo = &m_jointArray[i]; + dgConstraint* const constraint = jointInfo->m_joint; + const dgInt32 first = jointInfo->m_pairStart; + const dgInt32 count = jointInfo->m_pairCount; + + for (dgInt32 j = 0; j < count; j++) { + const dgRightHandSide* const rhs = &rightHandSide[j + first]; + dgAssert(dgCheckFloat(rhs->m_force)); + rhs->m_jointFeebackForce->Push(rhs->m_force); + rhs->m_jointFeebackForce->m_force = rhs->m_force; + rhs->m_jointFeebackForce->m_impact = rhs->m_maxImpact * m_timestepRK; + } + hasJointFeeback |= (constraint->m_updaFeedbackCallback ? 1 : 0); + } + m_hasJointFeeback[threadID] = hasJointFeeback; +} + +void dgParallelBodySolver::IntegrateBodiesVelocity(dgInt32 threadID) +{ + dgVector speedFreeze2(m_world->m_freezeSpeed2 * dgFloat32(0.1f)); + dgVector freezeOmega2(m_world->m_freezeOmega2 * dgFloat32(0.1f)); + + dgVector timestep4(m_timestepRK); + dgJacobian* const internalForces = &m_world->m_solverMemory.m_internalForcesBuffer[0]; + + const dgInt32 step = m_threadCounts; + const dgInt32 bodyCount = m_cluster->m_bodyCount - 1; + for (dgInt32 j = threadID; j < bodyCount; j += step) { + const dgInt32 i = j + 1; + dgDynamicBody* const body = (dgDynamicBody*)m_bodyArray[i].m_body; + dgAssert(body->m_index == i); + + if (body->IsRTTIType(dgBody::m_dynamicBodyRTTI)) { + const dgJacobian& forceAndTorque = internalForces[i]; + const dgVector force(body->m_externalForce + forceAndTorque.m_linear); + const dgVector torque(body->m_externalTorque + forceAndTorque.m_angular); + + //const dgVector velocStep((force.Scale(body->m_invMass.m_w)) * timestep4); + //const dgVector omegaStep((body->m_invWorldInertiaMatrix.RotateVector(torque)) * timestep4); + const dgJacobian velocStep(body->IntegrateForceAndToque(force, torque, timestep4)); + + if (!body->m_resting) { + //body->m_veloc += velocStep; + //body->m_omega += omegaStep; + body->m_veloc += velocStep.m_linear; + body->m_omega += velocStep.m_angular; + } else { + //const dgVector velocStep2(velocStep.DotProduct(velocStep)); + //const dgVector omegaStep2(omegaStep.DotProduct(omegaStep)); + const dgVector velocStep2(velocStep.m_linear.DotProduct(velocStep.m_linear)); + const dgVector omegaStep2(velocStep.m_angular.DotProduct(velocStep.m_angular)); + const dgVector test(((velocStep2 > speedFreeze2) | (omegaStep2 > speedFreeze2)) & dgVector::m_negOne); + const dgInt32 equilibrium = test.GetSignMask() ? 0 : 1; + body->m_resting &= equilibrium; + } + dgAssert(body->m_veloc.m_w == dgFloat32(0.0f)); + dgAssert(body->m_omega.m_w == dgFloat32(0.0f)); + } + } +} + +void dgParallelBodySolver::IntegrateBodiesVelocity() +{ + DG_TRACKTIME(); + for (dgInt32 i = 0; i < m_threadCounts; i++) { + m_world->QueueJob(IntegrateBodiesVelocityKernel, this, NULL, "dgParallelBodySolver::IntegrateBodiesVelocity"); + } + m_world->SynchronizationBarrier(); +} + +void dgParallelBodySolver::CalculateJointsForce() +{ + DG_TRACKTIME(); + const dgInt32 bodyCount = m_cluster->m_bodyCount; + dgJacobian* const internalForces = &m_world->GetSolverMemory().m_internalForcesBuffer[0]; + dgJacobian* const tempInternalForces = &m_world->GetSolverMemory().m_internalForcesBuffer[bodyCount]; + const dgInt32 passes = m_solverPasses; + const dgInt32 threadCounts = m_world->GetThreadCount(); + + dgFloat32 accNorm = DG_SOLVER_MAX_ERROR * dgFloat32(2.0f); + for (dgInt32 k = 0; (k < passes) && (accNorm > DG_SOLVER_MAX_ERROR); k++) { + memset(tempInternalForces, 0, bodyCount * sizeof(dgJacobian)); + for (dgInt32 i = 0; i < threadCounts; i++) { + m_world->QueueJob(CalculateJointsForceKernel, this, NULL, "dgSolver::CalculateJointsForce"); + } + m_world->SynchronizationBarrier(); + memcpy(internalForces, tempInternalForces, bodyCount * sizeof(dgJacobian)); + for (dgInt32 i = 0; i < threadCounts; i++) { + accNorm = dgMax(accNorm, m_accelNorm[i]); + } + } +} + +void dgParallelBodySolver::CalculateJointsAccel() +{ + DG_TRACKTIME(); + for (dgInt32 i = 0; i < m_threadCounts; i++) { + m_world->QueueJob(CalculateJointsAccelerationKernel, this, NULL, "dgParallelBodySolver::CalculateJointsAcceleration"); + } + m_world->SynchronizationBarrier(); + m_firstPassCoef = dgFloat32(1.0f); + +#ifdef D_USE_SOA_SOLVER + for (dgInt32 i = 0; i < m_threadCounts; i++) { + m_world->QueueJob(UpdateRowAccelerationKernel, this, NULL, "dgParallelBodySolver::UpdateRowAcceleration"); + } + m_world->SynchronizationBarrier(); +#endif +} + +void dgParallelBodySolver::InitSkeletons(dgInt32 threadID) +{ + dgRightHandSide* const rightHandSide = &m_world->m_solverMemory.m_righHandSizeBuffer[0]; + const dgLeftHandSide* const leftHandSide = &m_world->m_solverMemory.m_leftHandSizeBuffer[0]; + + const dgInt32 count = m_skeletonCount; + const dgInt32 threadCounts = m_world->GetThreadCount(); + dgSkeletonContainer** const skeletonArray = &m_skeletonArray[0]; + + for (dgInt32 i = threadID; i < count; i += threadCounts) { + dgSkeletonContainer* const skeleton = skeletonArray[i]; + skeleton->InitMassMatrix(m_jointArray, leftHandSide, rightHandSide); + } +} + +void dgParallelBodySolver::UpdateSkeletons(dgInt32 threadID) +{ + const dgInt32 count = m_skeletonCount; + const dgInt32 threadCounts = m_world->GetThreadCount(); + dgSkeletonContainer** const skeletonArray = &m_skeletonArray[0]; + dgJacobian* const internalForces = &m_world->m_solverMemory.m_internalForcesBuffer[0]; + + for (dgInt32 i = threadID; i < count; i += threadCounts) { + dgSkeletonContainer* const skeleton = skeletonArray[i]; + skeleton->CalculateJointForce(m_jointArray, m_bodyArray, internalForces); + } +} + +void dgParallelBodySolver::InitSkeletonsKernel(void* const context, void* const, dgInt32 threadID) +{ + DG_TRACKTIME(); + dgParallelBodySolver* const me = (dgParallelBodySolver*)context; + me->InitSkeletons(threadID); +} + +void dgParallelBodySolver::UpdateSkeletonsKernel(void* const context, void* const, dgInt32 threadID) +{ + DG_TRACKTIME(); + dgParallelBodySolver* const me = (dgParallelBodySolver*)context; + me->UpdateSkeletons(threadID); +} + +void dgParallelBodySolver::InitSkeletons() +{ + DG_TRACKTIME(); + const dgInt32 threadCounts = m_world->GetThreadCount(); + for (dgInt32 i = 0; i < threadCounts; i++) { + m_world->QueueJob(InitSkeletonsKernel, this, NULL, "dgSolver::InitSkeletonsKernel"); + } + m_world->SynchronizationBarrier(); +} + +void dgParallelBodySolver::UpdateSkeletons() +{ + DG_TRACKTIME(); + const dgInt32 threadCounts = m_world->GetThreadCount(); + for (dgInt32 i = 0; i < threadCounts; i++) { + m_world->QueueJob(UpdateSkeletonsKernel, this, NULL, "dgParallelBodySolver::UpdateSkeletons"); + } + m_world->SynchronizationBarrier(); +} + + +#ifdef D_USE_SOA_SOLVER + +dgFloat32 dgParallelBodySolver::CalculateJointForce(const dgJointInfo* const jointInfo, dgSolverSoaElement* const massMatrix, const dgJacobian* const internalForcesPtr) const +{ + dgWorkGroupVector6 forceM0; + dgWorkGroupVector6 forceM1; + dgWorkGroupFloat weight0; + dgWorkGroupFloat weight1; + dgWorkGroupFloat preconditioner0; + dgWorkGroupFloat preconditioner1; + dgWorkGroupFloat accNorm(m_zero); + dgWorkGroupFloat normalForce[DG_CONSTRAINT_MAX_ROWS + 1]; + const dgBodyProxy* const bodyProxyArray = m_bodyProxyArray; + const dgWorkGroupFloat* const internalForces = (dgWorkGroupFloat*)internalForcesPtr; + + for (dgInt32 i = 0; i < DG_WORK_GROUP_SIZE; i++) { + const dgInt32 m0 = jointInfo[i].m_m0; + const dgInt32 m1 = jointInfo[i].m_m1; + + forceM0.m_linear.m_x[i] = internalForces[m0][0]; + forceM0.m_linear.m_y[i] = internalForces[m0][1]; + forceM0.m_linear.m_z[i] = internalForces[m0][2]; + forceM0.m_angular.m_x[i] = internalForces[m0][4]; + forceM0.m_angular.m_y[i] = internalForces[m0][5]; + forceM0.m_angular.m_z[i] = internalForces[m0][6]; + + forceM1.m_linear.m_x[i] = internalForces[m1][0]; + forceM1.m_linear.m_y[i] = internalForces[m1][1]; + forceM1.m_linear.m_z[i] = internalForces[m1][2]; + forceM1.m_angular.m_x[i] = internalForces[m1][4]; + forceM1.m_angular.m_y[i] = internalForces[m1][5]; + forceM1.m_angular.m_z[i] = internalForces[m1][6]; + + weight0[i] = bodyProxyArray[m0].m_weight; + weight1[i] = bodyProxyArray[m1].m_weight; + + preconditioner0[i] = jointInfo[i].m_preconditioner0; + preconditioner1[i] = jointInfo[i].m_preconditioner1; + } + + forceM0.m_linear.m_x = forceM0.m_linear.m_x * preconditioner0; + forceM0.m_linear.m_y = forceM0.m_linear.m_y * preconditioner0; + forceM0.m_linear.m_z = forceM0.m_linear.m_z * preconditioner0; + forceM0.m_angular.m_x = forceM0.m_angular.m_x * preconditioner0; + forceM0.m_angular.m_y = forceM0.m_angular.m_y * preconditioner0; + forceM0.m_angular.m_z = forceM0.m_angular.m_z * preconditioner0; + + forceM1.m_linear.m_x = forceM1.m_linear.m_x * preconditioner1; + forceM1.m_linear.m_y = forceM1.m_linear.m_y * preconditioner1; + forceM1.m_linear.m_z = forceM1.m_linear.m_z * preconditioner1; + forceM1.m_angular.m_x = forceM1.m_angular.m_x * preconditioner1; + forceM1.m_angular.m_y = forceM1.m_angular.m_y * preconditioner1; + forceM1.m_angular.m_z = forceM1.m_angular.m_z * preconditioner1; + + preconditioner0 = preconditioner0 * weight0; + preconditioner1 = preconditioner1 * weight1; + + const dgInt32 rowsCount = jointInfo->m_pairCount; + normalForce[0] = m_one; + for (dgInt32 i = 0; i < rowsCount; i++) { + dgSolverSoaElement* const row = &massMatrix[i]; + + dgWorkGroupFloat a; + a = row->m_coordenateAccel.MulSub(row->m_JMinv.m_jacobianM0.m_linear.m_x, forceM0.m_linear.m_x); + a = a.MulSub(row->m_JMinv.m_jacobianM0.m_linear.m_y, forceM0.m_linear.m_y); + a = a.MulSub(row->m_JMinv.m_jacobianM0.m_linear.m_z, forceM0.m_linear.m_z); + a = a.MulSub(row->m_JMinv.m_jacobianM0.m_angular.m_x, forceM0.m_angular.m_x); + a = a.MulSub(row->m_JMinv.m_jacobianM0.m_angular.m_y, forceM0.m_angular.m_y); + a = a.MulSub(row->m_JMinv.m_jacobianM0.m_angular.m_z, forceM0.m_angular.m_z); + + a = a.MulSub(row->m_JMinv.m_jacobianM1.m_linear.m_x, forceM1.m_linear.m_x); + a = a.MulSub(row->m_JMinv.m_jacobianM1.m_linear.m_y, forceM1.m_linear.m_y); + a = a.MulSub(row->m_JMinv.m_jacobianM1.m_linear.m_z, forceM1.m_linear.m_z); + a = a.MulSub(row->m_JMinv.m_jacobianM1.m_angular.m_x, forceM1.m_angular.m_x); + a = a.MulSub(row->m_JMinv.m_jacobianM1.m_angular.m_y, forceM1.m_angular.m_y); + a = a.MulSub(row->m_JMinv.m_jacobianM1.m_angular.m_z, forceM1.m_angular.m_z); + a = a.MulSub(row->m_force, row->m_diagDamp); + + dgWorkGroupFloat f(row->m_force.MulAdd(row->m_invJinvMJt, a)); + dgWorkGroupFloat frictionNormal(normalForce, row->m_normalForceIndex); + + dgWorkGroupFloat lowerFrictionForce(frictionNormal * row->m_lowerBoundFrictionCoefficent); + dgWorkGroupFloat upperFrictionForce(frictionNormal * row->m_upperBoundFrictionCoefficent); + + a = a & (f < upperFrictionForce) & (f > lowerFrictionForce); + f = f.GetMax(lowerFrictionForce).GetMin(upperFrictionForce); + accNorm = accNorm.MulAdd(a, a); + + dgWorkGroupFloat deltaForce(f - row->m_force); + + row->m_force = f; + normalForce[i + 1] = f; + + dgWorkGroupFloat deltaForce0(deltaForce * preconditioner0); + dgWorkGroupFloat deltaForce1(deltaForce * preconditioner1); + + forceM0.m_linear.m_x = forceM0.m_linear.m_x.MulAdd(row->m_Jt.m_jacobianM0.m_linear.m_x, deltaForce0); + forceM0.m_linear.m_y = forceM0.m_linear.m_y.MulAdd(row->m_Jt.m_jacobianM0.m_linear.m_y, deltaForce0); + forceM0.m_linear.m_z = forceM0.m_linear.m_z.MulAdd(row->m_Jt.m_jacobianM0.m_linear.m_z, deltaForce0); + forceM0.m_angular.m_x = forceM0.m_angular.m_x.MulAdd(row->m_Jt.m_jacobianM0.m_angular.m_x, deltaForce0); + forceM0.m_angular.m_y = forceM0.m_angular.m_y.MulAdd(row->m_Jt.m_jacobianM0.m_angular.m_y, deltaForce0); + forceM0.m_angular.m_z = forceM0.m_angular.m_z.MulAdd(row->m_Jt.m_jacobianM0.m_angular.m_z, deltaForce0); + + forceM1.m_linear.m_x = forceM1.m_linear.m_x.MulAdd(row->m_Jt.m_jacobianM1.m_linear.m_x, deltaForce1); + forceM1.m_linear.m_y = forceM1.m_linear.m_y.MulAdd(row->m_Jt.m_jacobianM1.m_linear.m_y, deltaForce1); + forceM1.m_linear.m_z = forceM1.m_linear.m_z.MulAdd(row->m_Jt.m_jacobianM1.m_linear.m_z, deltaForce1); + forceM1.m_angular.m_x = forceM1.m_angular.m_x.MulAdd(row->m_Jt.m_jacobianM1.m_angular.m_x, deltaForce1); + forceM1.m_angular.m_y = forceM1.m_angular.m_y.MulAdd(row->m_Jt.m_jacobianM1.m_angular.m_y, deltaForce1); + forceM1.m_angular.m_z = forceM1.m_angular.m_z.MulAdd(row->m_Jt.m_jacobianM1.m_angular.m_z, deltaForce1); + } + + const dgFloat32 tol = dgFloat32(0.5f); + const dgFloat32 tol2 = tol * tol; + dgWorkGroupFloat maxAccel(accNorm); + for (dgInt32 i = 0; (i < 4) && (maxAccel.AddHorizontal() > tol2); i++) { + maxAccel = m_zero; + for (dgInt32 j = 0; j < rowsCount; j++) { + dgSolverSoaElement* const row = &massMatrix[j]; + + dgWorkGroupFloat a; + a = row->m_coordenateAccel.MulSub(row->m_JMinv.m_jacobianM0.m_linear.m_x, forceM0.m_linear.m_x); + a = a.MulSub(row->m_JMinv.m_jacobianM0.m_linear.m_y, forceM0.m_linear.m_y); + a = a.MulSub(row->m_JMinv.m_jacobianM0.m_linear.m_z, forceM0.m_linear.m_z); + a = a.MulSub(row->m_JMinv.m_jacobianM0.m_angular.m_x, forceM0.m_angular.m_x); + a = a.MulSub(row->m_JMinv.m_jacobianM0.m_angular.m_y, forceM0.m_angular.m_y); + a = a.MulSub(row->m_JMinv.m_jacobianM0.m_angular.m_z, forceM0.m_angular.m_z); + + a = a.MulSub(row->m_JMinv.m_jacobianM1.m_linear.m_x, forceM1.m_linear.m_x); + a = a.MulSub(row->m_JMinv.m_jacobianM1.m_linear.m_y, forceM1.m_linear.m_y); + a = a.MulSub(row->m_JMinv.m_jacobianM1.m_linear.m_z, forceM1.m_linear.m_z); + a = a.MulSub(row->m_JMinv.m_jacobianM1.m_angular.m_x, forceM1.m_angular.m_x); + a = a.MulSub(row->m_JMinv.m_jacobianM1.m_angular.m_y, forceM1.m_angular.m_y); + a = a.MulSub(row->m_JMinv.m_jacobianM1.m_angular.m_z, forceM1.m_angular.m_z); + a = a.MulSub(row->m_force, row->m_diagDamp); + + dgWorkGroupFloat f(row->m_force.MulAdd(row->m_invJinvMJt, a)); + dgWorkGroupFloat frictionNormal(normalForce, row->m_normalForceIndex); + + dgWorkGroupFloat lowerFrictionForce(frictionNormal * row->m_lowerBoundFrictionCoefficent); + dgWorkGroupFloat upperFrictionForce(frictionNormal * row->m_upperBoundFrictionCoefficent); + + a = a & (f < upperFrictionForce) & (f > lowerFrictionForce); + f = f.GetMax(lowerFrictionForce).GetMin(upperFrictionForce); + maxAccel = maxAccel.MulAdd(a, a); + + dgWorkGroupFloat deltaForce(f - row->m_force); + + row->m_force = f; + normalForce[j + 1] = f; + + dgWorkGroupFloat deltaForce0(deltaForce * preconditioner0); + dgWorkGroupFloat deltaForce1(deltaForce * preconditioner1); + + forceM0.m_linear.m_x = forceM0.m_linear.m_x.MulAdd(row->m_Jt.m_jacobianM0.m_linear.m_x, deltaForce0); + forceM0.m_linear.m_y = forceM0.m_linear.m_y.MulAdd(row->m_Jt.m_jacobianM0.m_linear.m_y, deltaForce0); + forceM0.m_linear.m_z = forceM0.m_linear.m_z.MulAdd(row->m_Jt.m_jacobianM0.m_linear.m_z, deltaForce0); + forceM0.m_angular.m_x = forceM0.m_angular.m_x.MulAdd(row->m_Jt.m_jacobianM0.m_angular.m_x, deltaForce0); + forceM0.m_angular.m_y = forceM0.m_angular.m_y.MulAdd(row->m_Jt.m_jacobianM0.m_angular.m_y, deltaForce0); + forceM0.m_angular.m_z = forceM0.m_angular.m_z.MulAdd(row->m_Jt.m_jacobianM0.m_angular.m_z, deltaForce0); + + forceM1.m_linear.m_x = forceM1.m_linear.m_x.MulAdd(row->m_Jt.m_jacobianM1.m_linear.m_x, deltaForce1); + forceM1.m_linear.m_y = forceM1.m_linear.m_y.MulAdd(row->m_Jt.m_jacobianM1.m_linear.m_y, deltaForce1); + forceM1.m_linear.m_z = forceM1.m_linear.m_z.MulAdd(row->m_Jt.m_jacobianM1.m_linear.m_z, deltaForce1); + forceM1.m_angular.m_x = forceM1.m_angular.m_x.MulAdd(row->m_Jt.m_jacobianM1.m_angular.m_x, deltaForce1); + forceM1.m_angular.m_y = forceM1.m_angular.m_y.MulAdd(row->m_Jt.m_jacobianM1.m_angular.m_y, deltaForce1); + forceM1.m_angular.m_z = forceM1.m_angular.m_z.MulAdd(row->m_Jt.m_jacobianM1.m_angular.m_z, deltaForce1); + } + } + + return accNorm.AddHorizontal(); +} + +void dgParallelBodySolver::CalculateJointsForce(dgInt32 threadID) +{ + const dgInt32* const soaRowStart = m_soaRowStart; + const dgBodyInfo* const bodyArray = m_bodyArray; + dgJacobian* const internalForces = &m_world->m_solverMemory.m_internalForcesBuffer[0]; + dgRightHandSide* const rightHandSide = &m_world->m_solverMemory.m_righHandSizeBuffer[0]; + dgSolverSoaElement* const massMatrix = &m_massMatrix[0]; + dgFloat32 accNorm = dgFloat32(0.0f); + + const dgInt32 step = m_threadCounts; + const dgInt32 jointCount = m_jointCount; + for (dgInt32 i = threadID; i < jointCount; i += step) { + const dgInt32 rowStart = soaRowStart[i]; + dgJointInfo* const jointInfo = &m_jointArray[i * DG_WORK_GROUP_SIZE]; + + bool isSleeping = true; + dgFloat32 accel2 = dgFloat32 (0.0f); + for (dgInt32 j = 0; (j < DG_WORK_GROUP_SIZE) && isSleeping; j++) { + const dgInt32 m0 = jointInfo[j].m_m0; + const dgInt32 m1 = jointInfo[j].m_m1; + const dgBody* const body0 = bodyArray[m0].m_body; + const dgBody* const body1 = bodyArray[m1].m_body; + isSleeping &= body0->m_resting; + isSleeping &= body1->m_resting; + } + if (!isSleeping) { + accel2 = CalculateJointForce(jointInfo, &massMatrix[rowStart], internalForces); + for (dgInt32 j = 0; j < DG_WORK_GROUP_SIZE; j++) { + const dgJointInfo* const joint = &jointInfo[j]; + if (joint->m_joint) { + dgInt32 const rowCount = joint->m_pairCount; + dgInt32 const rowStartBase = joint->m_pairStart; + for (dgInt32 k = 0; k < rowCount; k++) { + const dgSolverSoaElement* const row = &massMatrix[rowStart + k]; + rightHandSide[k + rowStartBase].m_force = row->m_force[j]; + rightHandSide[k + rowStartBase].m_maxImpact = dgMax(dgAbs(row->m_force[j]), rightHandSide[k + rowStartBase].m_maxImpact); + } + } + } + } + + dgWorkGroupVector6 forceM0; + dgWorkGroupVector6 forceM1; + + forceM0.m_linear.m_x = m_zero; + forceM0.m_linear.m_y = m_zero; + forceM0.m_linear.m_z = m_zero; + forceM0.m_angular.m_x = m_zero; + forceM0.m_angular.m_y = m_zero; + forceM0.m_angular.m_z = m_zero; + + forceM1.m_linear.m_x = m_zero; + forceM1.m_linear.m_y = m_zero; + forceM1.m_linear.m_z = m_zero; + forceM1.m_angular.m_x = m_zero; + forceM1.m_angular.m_y = m_zero; + forceM1.m_angular.m_z = m_zero; + + const dgInt32 rowsCount = jointInfo->m_pairCount; + + for (dgInt32 j = 0; j < rowsCount; j++) { + dgSolverSoaElement* const row = &massMatrix[rowStart + j]; + + dgWorkGroupFloat f(row->m_force); + forceM0.m_linear.m_x = forceM0.m_linear.m_x.MulAdd(row->m_Jt.m_jacobianM0.m_linear.m_x, f); + forceM0.m_linear.m_y = forceM0.m_linear.m_y.MulAdd(row->m_Jt.m_jacobianM0.m_linear.m_y, f); + forceM0.m_linear.m_z = forceM0.m_linear.m_z.MulAdd(row->m_Jt.m_jacobianM0.m_linear.m_z, f); + forceM0.m_angular.m_x = forceM0.m_angular.m_x.MulAdd(row->m_Jt.m_jacobianM0.m_angular.m_x, f); + forceM0.m_angular.m_y = forceM0.m_angular.m_y.MulAdd(row->m_Jt.m_jacobianM0.m_angular.m_y, f); + forceM0.m_angular.m_z = forceM0.m_angular.m_z.MulAdd(row->m_Jt.m_jacobianM0.m_angular.m_z, f); + + forceM1.m_linear.m_x = forceM1.m_linear.m_x.MulAdd(row->m_Jt.m_jacobianM1.m_linear.m_x, f); + forceM1.m_linear.m_y = forceM1.m_linear.m_y.MulAdd(row->m_Jt.m_jacobianM1.m_linear.m_y, f); + forceM1.m_linear.m_z = forceM1.m_linear.m_z.MulAdd(row->m_Jt.m_jacobianM1.m_linear.m_z, f); + forceM1.m_angular.m_x = forceM1.m_angular.m_x.MulAdd(row->m_Jt.m_jacobianM1.m_angular.m_x, f); + forceM1.m_angular.m_y = forceM1.m_angular.m_y.MulAdd(row->m_Jt.m_jacobianM1.m_angular.m_y, f); + forceM1.m_angular.m_z = forceM1.m_angular.m_z.MulAdd(row->m_Jt.m_jacobianM1.m_angular.m_z, f); + } + + dgBodyProxy* const bodyProxyArray = m_bodyProxyArray; + dgJacobian* const tempInternalForces = &m_world->m_solverMemory.m_internalForcesBuffer[m_cluster->m_bodyCount]; + for (dgInt32 j = 0; j < DG_WORK_GROUP_SIZE; j++) { + const dgJointInfo* const joint = &jointInfo[j]; + if (joint->m_joint) { + dgJacobian m_body0Force; + dgJacobian m_body1Force; + + m_body0Force.m_linear = dgVector (forceM0.m_linear.m_x[j], forceM0.m_linear.m_y[j], forceM0.m_linear.m_z[j], dgFloat32 (0.0f)); + m_body0Force.m_angular = dgVector (forceM0.m_angular.m_x[j], forceM0.m_angular.m_y[j], forceM0.m_angular.m_z[j], dgFloat32 (0.0f)); + + m_body1Force.m_linear = dgVector(forceM1.m_linear.m_x[j], forceM1.m_linear.m_y[j], forceM1.m_linear.m_z[j], dgFloat32(0.0f)); + m_body1Force.m_angular = dgVector(forceM1.m_angular.m_x[j], forceM1.m_angular.m_y[j], forceM1.m_angular.m_z[j], dgFloat32(0.0f)); + + const dgInt32 m0 = jointInfo[j].m_m0; + const dgInt32 m1 = jointInfo[j].m_m1; + + if (m0) { + dgScopeSpinPause lock(&bodyProxyArray[m0].m_lock); + tempInternalForces[m0].m_linear += m_body0Force.m_linear; + tempInternalForces[m0].m_angular += m_body0Force.m_angular; + } + if (m1) { + dgScopeSpinPause lock(&bodyProxyArray[m1].m_lock); + tempInternalForces[m1].m_linear += m_body1Force.m_linear; + tempInternalForces[m1].m_angular += m_body1Force.m_angular; + } + } + } + accNorm += accel2; + } + m_accelNorm[threadID] = accNorm; +} + +#else + +void dgParallelBodySolver::CalculateJointsForce(dgInt32 threadID) +{ + dgVector accNorm(dgVector::m_zero); + const dgBodyInfo* const bodyArray = m_bodyArray; + dgBodyProxy* const bodyProxyArray = m_bodyProxyArray; + dgJacobian* const internalForces = &m_world->m_solverMemory.m_internalForcesBuffer[0]; + dgJacobian* const tempInternalForces = &m_world->m_solverMemory.m_internalForcesBuffer[m_cluster->m_bodyCount]; + + dgRightHandSide* const rightHandSide = &m_world->m_solverMemory.m_righHandSizeBuffer[0]; + const dgLeftHandSide* const leftHandSide = &m_world->m_solverMemory.m_leftHandSizeBuffer[0]; + + const dgInt32 step = m_threadCounts; + const dgInt32 jointCount = m_cluster->m_jointCount; + dgFloat32 normalForce[DG_CONSTRAINT_MAX_ROWS + 1]; + + for (dgInt32 i = threadID; i < jointCount; i += step) { + dgJointInfo* const jointInfo = &m_jointArray[i]; + dgFloat32 accel2 = dgFloat32(0.0f); + + const dgInt32 m0 = jointInfo->m_m0; + const dgInt32 m1 = jointInfo->m_m1; + const dgBody* const body0 = bodyArray[m0].m_body; + const dgBody* const body1 = bodyArray[m1].m_body; + dgInt32 isSleeping = body0->m_resting & body1->m_resting; + if (!isSleeping) { + + dgVector preconditioner0 (jointInfo->m_preconditioner0); + dgVector preconditioner1 (jointInfo->m_preconditioner1); + + dgVector forceM0 (internalForces[m0].m_linear * preconditioner0); + dgVector torqueM0 (internalForces[m0].m_angular * preconditioner0); + dgVector forceM1 (internalForces[m1].m_linear * preconditioner1); + dgVector torqueM1 (internalForces[m1].m_angular * preconditioner1); + + preconditioner0 = preconditioner0.Scale (bodyProxyArray[m0].m_weight); + preconditioner1 = preconditioner1.Scale (bodyProxyArray[m1].m_weight); + + normalForce[0] = dgFloat32 (1.0f); + const dgInt32 rowsCount = jointInfo->m_pairCount; + const dgInt32 rowStart = jointInfo->m_pairStart; + for (dgInt32 j = 0; j < rowsCount; j++) { + dgRightHandSide* const rhs = &rightHandSide[rowStart + j]; + const dgLeftHandSide* const lhs = &leftHandSide[rowStart + j]; + dgVector a(lhs->m_JMinv.m_jacobianM0.m_linear * forceM0); + a = a.MulAdd(lhs->m_JMinv.m_jacobianM0.m_angular, torqueM0); + a = a.MulAdd(lhs->m_JMinv.m_jacobianM1.m_linear, forceM1); + a = a.MulAdd(lhs->m_JMinv.m_jacobianM1.m_angular, torqueM1); + //a = dgVector(rhs->m_coordenateAccel + rhs->m_gyroAccel - rhs->m_force * rhs->m_diagDamp) - a.AddHorizontal(); + a = dgVector(rhs->m_coordenateAccel - rhs->m_force * rhs->m_diagDamp) - a.AddHorizontal(); + dgVector f(rhs->m_force + rhs->m_invJinvMJt * a.GetScalar()); + + dgAssert(rhs->m_normalForceIndex >= -1); + dgAssert(rhs->m_normalForceIndex <= rowsCount); + + const dgInt32 frictionIndex = rhs->m_normalForceIndex + 1; + const dgFloat32 frictionNormal = normalForce[frictionIndex]; + const dgVector lowerFrictionForce(frictionNormal * rhs->m_lowerBoundFrictionCoefficent); + const dgVector upperFrictionForce(frictionNormal * rhs->m_upperBoundFrictionCoefficent); + + a = a & (f < upperFrictionForce) & (f > lowerFrictionForce); + f = f.GetMax(lowerFrictionForce).GetMin(upperFrictionForce); + accNorm = accNorm.MulAdd(a, a); + + dgVector deltaForce(f - dgVector(rhs->m_force)); + + rhs->m_force = f.GetScalar(); + normalForce[j + 1] = f.GetScalar(); + + dgVector deltaForce0(deltaForce * preconditioner0); + dgVector deltaForce1(deltaForce * preconditioner1); + + forceM0 = forceM0.MulAdd(lhs->m_Jt.m_jacobianM0.m_linear, deltaForce0); + torqueM0 = torqueM0.MulAdd(lhs->m_Jt.m_jacobianM0.m_angular, deltaForce0); + forceM1 = forceM1.MulAdd(lhs->m_Jt.m_jacobianM1.m_linear, deltaForce1); + torqueM1 = torqueM1.MulAdd(lhs->m_Jt.m_jacobianM1.m_angular, deltaForce1); + } + + const dgFloat32 tol = dgFloat32(0.5f); + const dgFloat32 tol2 = tol * tol; + dgVector maxAccel(accNorm); + + for (dgInt32 k = 0; (k < 4) && (maxAccel.GetScalar() > tol2); k++) { + maxAccel = dgVector::m_zero; + for (dgInt32 j = 0; j < rowsCount; j++) { + dgRightHandSide* const rhs = &rightHandSide[rowStart + j]; + const dgLeftHandSide* const lhs = &leftHandSide[rowStart + j]; + + dgVector a(lhs->m_JMinv.m_jacobianM0.m_linear * forceM0); + a = a.MulAdd(lhs->m_JMinv.m_jacobianM0.m_angular, torqueM0); + a = a.MulAdd(lhs->m_JMinv.m_jacobianM1.m_linear, forceM1); + a = a.MulAdd(lhs->m_JMinv.m_jacobianM1.m_angular, torqueM1); + //a = dgVector(rhs->m_coordenateAccel + rhs->m_gyroAccel - rhs->m_force * rhs->m_diagDamp) - a.AddHorizontal(); + a = dgVector(rhs->m_coordenateAccel - rhs->m_force * rhs->m_diagDamp) - a.AddHorizontal(); + dgVector f(rhs->m_force + rhs->m_invJinvMJt * a.GetScalar()); + + dgAssert(rhs->m_normalForceIndex >= -1); + dgAssert(rhs->m_normalForceIndex <= rowsCount); + + const dgInt32 frictionIndex = rhs->m_normalForceIndex + 1; + const dgFloat32 frictionNormal = normalForce[frictionIndex]; + const dgVector lowerFrictionForce(frictionNormal * rhs->m_lowerBoundFrictionCoefficent); + const dgVector upperFrictionForce(frictionNormal * rhs->m_upperBoundFrictionCoefficent); + + a = a & (f < upperFrictionForce) & (f > lowerFrictionForce); + f = f.GetMax(lowerFrictionForce).GetMin(upperFrictionForce); + maxAccel = maxAccel.MulAdd(a, a); + + dgVector deltaForce(f - rhs->m_force); + + rhs->m_force = f.GetScalar(); + normalForce[j + 1] = f.GetScalar(); + + dgVector deltaForce0(deltaForce * preconditioner0); + dgVector deltaForce1(deltaForce * preconditioner1); + forceM0 = forceM0.MulAdd(lhs->m_Jt.m_jacobianM0.m_linear, deltaForce0); + torqueM0 = torqueM0.MulAdd(lhs->m_Jt.m_jacobianM0.m_angular, deltaForce0); + forceM1 = forceM1.MulAdd(lhs->m_Jt.m_jacobianM1.m_linear, deltaForce1); + torqueM1 = torqueM1.MulAdd(lhs->m_Jt.m_jacobianM1.m_angular, deltaForce1); + } + } + + } + + dgVector forceM0(dgVector::m_zero); + dgVector torqueM0(dgVector::m_zero); + dgVector forceM1(dgVector::m_zero); + dgVector torqueM1(dgVector::m_zero); + + const dgInt32 rowsCount = jointInfo->m_pairCount; + const dgInt32 rowStart = jointInfo->m_pairStart; + + for (dgInt32 j = 0; j < rowsCount; j++) { + const dgRightHandSide* const rhs = &rightHandSide[rowStart + j]; + const dgLeftHandSide* const lhs = &leftHandSide[rowStart + j]; + + dgVector f (rhs->m_force); + forceM0 = forceM0.MulAdd(lhs->m_Jt.m_jacobianM0.m_linear, f); + torqueM0 = torqueM0.MulAdd(lhs->m_Jt.m_jacobianM0.m_angular, f); + forceM1 = forceM1.MulAdd(lhs->m_Jt.m_jacobianM1.m_linear, f); + torqueM1 = torqueM1.MulAdd(lhs->m_Jt.m_jacobianM1.m_angular, f); + } + + if (m0) { + dgScopeSpinPause lock(&bodyProxyArray[m0].m_lock); + tempInternalForces[m0].m_linear += forceM0; + tempInternalForces[m0].m_angular += torqueM0; + } + if (m1) { + dgScopeSpinPause lock(&bodyProxyArray[m1].m_lock); + tempInternalForces[m1].m_linear += forceM1; + tempInternalForces[m1].m_angular += torqueM1; + } + + accNorm += accel2; + } + m_accelNorm[threadID] = accNorm.GetScalar(); +} +#endif + + +void dgParallelBodySolver::CalculateForces() +{ + m_firstPassCoef = dgFloat32(0.0f); + if (m_skeletonCount) { + InitSkeletons(); + } + for (dgInt32 step = 0; step < 4; step++) { + CalculateJointsAccel(); + CalculateJointsForce(); + if (m_skeletonCount) { + UpdateSkeletons(); + } + IntegrateBodiesVelocity(); + } + + UpdateForceFeedback(); + + dgInt32 hasJointFeeback = 0; + for (dgInt32 i = 0; i < DG_MAX_THREADS_HIVE_COUNT; i++) { + hasJointFeeback |= m_hasJointFeeback[i]; + } + CalculateBodiesAcceleration(); + + if (hasJointFeeback) { + UpdateKinematicFeedback(); + } +} + +void dgParallelBodySolver::CalculateJointForces(const dgBodyCluster& cluster, dgBodyInfo* const bodyArray, dgJointInfo* const jointArray, dgFloat32 timestep) +{ + DG_TRACKTIME(); + m_cluster = &cluster; + m_bodyArray = bodyArray; + m_jointArray = jointArray; + m_timestep = timestep; + m_invTimestep = (timestep > dgFloat32(0.0f)) ? dgFloat32(1.0f) / timestep : dgFloat32(0.0f); + + m_invStepRK = dgFloat32(0.25f); + m_timestepRK = m_timestep * m_invStepRK; + m_invTimestepRK = m_invTimestep * dgFloat32(4.0f); + + m_solverPasses = m_world->GetSolverIterations(); + m_threadCounts = m_world->GetThreadCount(); + m_jointCount = ((m_cluster->m_jointCount + DG_WORK_GROUP_SIZE - 1) & -dgInt32(DG_WORK_GROUP_SIZE - 1)) / DG_WORK_GROUP_SIZE; + + m_soaRowStart = dgAlloca(dgInt32, m_jointCount); + m_bodyProxyArray = dgAlloca(dgBodyProxy, cluster.m_bodyCount); + + InitWeights(); + InitBodyArray(); + InitJacobianMatrix(); + CalculateForces(); +} + diff --git a/thirdparty/src/newton/dgPhysics/dgWorldDynamicsParallelSolver.h b/thirdparty/src/newton/dgPhysics/dgWorldDynamicsParallelSolver.h new file mode 100644 index 000000000..74be108ac --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgWorldDynamicsParallelSolver.h @@ -0,0 +1,322 @@ +/* Copyright (c) <2003-2019> +* +* 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_PARALLEL_SOLVER_H_ +#define _DG_PARALLEL_SOLVER_H_ + +#include "dgPhysicsStdafx.h" + +class dgBodyInfo; +class dgJointInfo; +class dgBodyCluster; +class dgSkeletonContainer; + +#define DG_WORK_GROUP_SIZE 8 + +DG_MSC_VECTOR_ALIGNMENT +class dgWorkGroupFloat +{ + public: + DG_INLINE dgWorkGroupFloat() + { + } + + DG_INLINE dgWorkGroupFloat(const dgWorkGroupFloat& me) + :m_low(me.m_low) + ,m_high(me.m_high) + { + } + + DG_INLINE dgWorkGroupFloat(const dgVector& v) + :m_low(v) + ,m_high(v) + { + } + + DG_INLINE dgWorkGroupFloat(const dgVector& low, const dgVector& high) + :m_low(low) + ,m_high(high) + { + } + + DG_INLINE dgWorkGroupFloat(const dgWorkGroupFloat* const baseAddr, const dgWorkGroupFloat& index) + { + const dgInt32* const indirectIndex = (dgInt32*) &index[0]; + const dgFloat32* const src = &(*baseAddr)[0]; + dgFloat32* const dst = &(*this)[0]; + for (dgInt32 i = 0; i < DG_WORK_GROUP_SIZE; i++) { + dst[i] = src[indirectIndex[i]]; + } + } + + DG_INLINE dgFloat32& operator[] (dgInt32 i) + { + dgAssert(i >= 0); + dgAssert(i < DG_WORK_GROUP_SIZE); + dgFloat32* const ptr = &m_low[0]; + return ptr[i]; + } + + DG_INLINE const dgFloat32& operator[] (dgInt32 i) const + { + dgAssert(i >= 0); + dgAssert(i < DG_WORK_GROUP_SIZE); + const dgFloat32* const ptr = &m_low[0]; + return ptr[i]; + } + + DG_INLINE dgWorkGroupFloat operator+ (const dgWorkGroupFloat& A) const + { + return dgWorkGroupFloat(m_low + A.m_low, m_high + A.m_high); + } + + DG_INLINE dgWorkGroupFloat operator- (const dgWorkGroupFloat& A) const + { + return dgWorkGroupFloat(m_low - A.m_low, m_high - A.m_high); + } + + DG_INLINE dgWorkGroupFloat operator* (const dgWorkGroupFloat& A) const + { + return dgWorkGroupFloat(m_low * A.m_low, m_high * A.m_high); + } + + DG_INLINE dgWorkGroupFloat MulAdd(const dgWorkGroupFloat& A, const dgWorkGroupFloat& B) const + { + return dgWorkGroupFloat(m_low.MulAdd(A.m_low, B.m_low), m_high.MulAdd(A.m_high, B.m_high)); + } + + DG_INLINE dgWorkGroupFloat MulSub(const dgWorkGroupFloat& A, const dgWorkGroupFloat& B) const + { + return dgWorkGroupFloat(m_low.MulSub(A.m_low, B.m_low), m_high.MulSub(A.m_high, B.m_high)); + } + + DG_INLINE dgWorkGroupFloat operator> (const dgWorkGroupFloat& A) const + { + return dgWorkGroupFloat(m_low > A.m_low, m_high > A.m_high); + } + + DG_INLINE dgWorkGroupFloat operator< (const dgWorkGroupFloat& A) const + { + return dgWorkGroupFloat(m_low < A.m_low, m_high < A.m_high); + } + + DG_INLINE dgWorkGroupFloat operator| (const dgWorkGroupFloat& A) const + { + return dgWorkGroupFloat(m_low | A.m_low, m_high | A.m_high); + } + + DG_INLINE dgWorkGroupFloat operator& (const dgWorkGroupFloat& A) const + { + return dgWorkGroupFloat(m_low & A.m_low, m_high & A.m_high); + } + + DG_INLINE dgWorkGroupFloat GetMin(const dgWorkGroupFloat& A) const + { + return dgWorkGroupFloat(m_low.GetMin(A.m_low), m_high.GetMin(A.m_high)); + } + + DG_INLINE dgWorkGroupFloat GetMax(const dgWorkGroupFloat& A) const + { + return dgWorkGroupFloat(m_low.GetMax(A.m_low), m_high.GetMax(A.m_high)); + } + + DG_INLINE dgFloat32 AddHorizontal() const + { + return (m_low + m_high).AddHorizontal().GetScalar(); + } + + DG_INLINE dgFloat32 GetMax() const + { + return (m_low.GetMax(m_high)).GetMax(); + } + + dgVector m_low; + dgVector m_high; +} DG_GCC_VECTOR_ALIGNMENT; + +DG_MSC_VECTOR_ALIGNMENT +class dgWorkGroupVector3 +{ + public: + dgWorkGroupFloat m_x; + dgWorkGroupFloat m_y; + dgWorkGroupFloat m_z; +} DG_GCC_VECTOR_ALIGNMENT; + + +DG_MSC_VECTOR_ALIGNMENT +class dgWorkGroupVector6 +{ + public: + dgWorkGroupVector3 m_linear; + dgWorkGroupVector3 m_angular; +} DG_GCC_AVX_ALIGNMENT; + +DG_MSC_VECTOR_ALIGNMENT +class dgSolverSoaJacobianPair +{ + public: + dgWorkGroupVector6 m_jacobianM0; + dgWorkGroupVector6 m_jacobianM1; +} DG_GCC_VECTOR_ALIGNMENT; + +DG_MSC_VECTOR_ALIGNMENT +class dgSolverSoaElement +{ + public: + dgSolverSoaJacobianPair m_Jt; + dgSolverSoaJacobianPair m_JMinv; + + dgWorkGroupFloat m_force; + dgWorkGroupFloat m_diagDamp; + dgWorkGroupFloat m_invJinvMJt; + dgWorkGroupFloat m_coordenateAccel; + dgWorkGroupFloat m_normalForceIndex; + dgWorkGroupFloat m_lowerBoundFrictionCoefficent; + dgWorkGroupFloat m_upperBoundFrictionCoefficent; +} DG_GCC_VECTOR_ALIGNMENT; + +class dgParallelBodySolver +{ + public: + class dgBodyProxy + { + public: + dgFloat32 m_weight; + dgFloat32 m_invWeight; + dgInt32 m_lock; + }; + + ~dgParallelBodySolver() {} + dgParallelBodySolver(dgMemoryAllocator* const allocator); + + void CalculateJointForces(const dgBodyCluster& cluster, dgBodyInfo* const bodyArray, dgJointInfo* const jointArray, dgFloat32 timestep); + + private: + void InitWeights(); + void InitBodyArray(); + void InitSkeletons(); + void CalculateForces(); + void UpdateSkeletons(); + void InitJacobianMatrix(); + void UpdateForceFeedback(); + void CalculateJointsForce(); + void CalculateJointsAccel(); + void IntegrateBodiesVelocity(); + void UpdateKinematicFeedback(); + void CalculateBodiesAcceleration(); + + void InitBodyArray(dgInt32 threadID); + void InitSkeletons(dgInt32 threadID); + void UpdateSkeletons(dgInt32 threadID); + void InitJacobianMatrix(dgInt32 threadID); + void UpdateForceFeedback(dgInt32 threadID); + void TransposeMassMatrix(dgInt32 threadID); + void CalculateJointsForce(dgInt32 threadID); + void UpdateRowAcceleration(dgInt32 threadID); + void IntegrateBodiesVelocity(dgInt32 threadID); + void UpdateKinematicFeedback(dgInt32 threadID); + void CalculateJointsAcceleration(dgInt32 threadID); + void CalculateBodiesAcceleration(dgInt32 threadID); + + static void InitSkeletonsKernel(void* const context, void* const, dgInt32 threadID); + static void InitBodyArrayKernel(void* const context, void* const, dgInt32 threadID); + static void UpdateSkeletonsKernel(void* const context, void* const, dgInt32 threadID); + static void InitJacobianMatrixKernel(void* const context, void* const, dgInt32 threadID); + static void UpdateForceFeedbackKernel(void* const context, void* const, dgInt32 threadID); + static void TransposeMassMatrixKernel(void* const context, void* const, dgInt32 threadID); + static void CalculateJointsForceKernel(void* const context, void* const, dgInt32 threadID); + static void UpdateRowAccelerationKernel(void* const context, void* const, dgInt32 threadID); + static void IntegrateBodiesVelocityKernel(void* const context, void* const, dgInt32 threadID); + static void UpdateKinematicFeedbackKernel(void* const context, void* const, dgInt32 threadID); + static void CalculateBodiesAccelerationKernel(void* const context, void* const, dgInt32 threadID); + static void CalculateJointsAccelerationKernel(void* const context, void* const, dgInt32 threadID); + + static dgInt32 CompareJointInfos(const dgJointInfo* const infoA, const dgJointInfo* const infoB, void* notUsed); + + dgFloat32 CalculateJointForce(const dgJointInfo* const jointInfo, dgSolverSoaElement* const massMatrix, const dgJacobian* const internalForces) const; + DG_INLINE void SortWorkGroup (dgInt32 base) const; + DG_INLINE void TransposeRow (dgSolverSoaElement* const row, const dgJointInfo* const jointInfoArray, dgInt32 index); + DG_INLINE void BuildJacobianMatrix(dgJointInfo* const jointInfo, dgLeftHandSide* const leftHandSide, dgRightHandSide* const righHandSide, dgJacobian* const internalForces); + + protected: + dgWorld* m_world; + const dgBodyCluster* m_cluster; + dgBodyInfo* m_bodyArray; + dgJointInfo* m_jointArray; + dgBodyProxy* m_bodyProxyArray; + dgFloat32 m_timestep; + dgFloat32 m_invTimestep; + dgFloat32 m_invStepRK; + dgFloat32 m_timestepRK; + dgFloat32 m_invTimestepRK; + dgFloat32 m_firstPassCoef; + dgFloat32 m_accelNorm[DG_MAX_THREADS_HIVE_COUNT]; + dgInt32 m_hasJointFeeback[DG_MAX_THREADS_HIVE_COUNT]; + dgArray m_skeletonArray; + + dgInt32 m_jointCount; + dgInt32 m_solverPasses; + dgInt32 m_threadCounts; + dgInt32 m_soaRowsCount; + dgInt32 m_skeletonCount; + dgInt32 m_jacobianMatrixRowAtomicIndex; + dgInt32* m_soaRowStart; + dgInt32* m_bodyRowStart; + + private: + dgWorkGroupFloat m_one; + dgWorkGroupFloat m_zero; + + dgArray m_massMatrix; + friend class dgWorldDynamicUpdate; +}; + +DG_INLINE dgParallelBodySolver::dgParallelBodySolver(dgMemoryAllocator* const allocator) + :m_world(NULL) + ,m_cluster(NULL) + ,m_bodyArray(NULL) + ,m_jointArray(NULL) + ,m_bodyProxyArray(NULL) + ,m_timestep(dgFloat32(0.0f)) + ,m_invTimestep(dgFloat32(0.0f)) + ,m_invStepRK(dgFloat32(0.0f)) + ,m_timestepRK(dgFloat32(0.0f)) + ,m_invTimestepRK(dgFloat32(0.0f)) + ,m_firstPassCoef(dgFloat32(0.0f)) + ,m_skeletonArray(allocator) + ,m_jointCount(0) + ,m_solverPasses(0) + ,m_threadCounts(0) + ,m_soaRowsCount(0) + ,m_skeletonCount(0) + ,m_jacobianMatrixRowAtomicIndex(0) + ,m_soaRowStart(NULL) + ,m_bodyRowStart(NULL) + ,m_massMatrix(allocator) + ,m_one(dgFloat32 (1.0f)) + ,m_zero(dgFloat32 (0.0f)) +{ + m_skeletonArray[32] = NULL; +} + +#endif + diff --git a/thirdparty/src/newton/dgPhysics/dgWorldDynamicsSimpleSolver.cpp b/thirdparty/src/newton/dgPhysics/dgWorldDynamicsSimpleSolver.cpp new file mode 100644 index 000000000..1ffd2edb6 --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgWorldDynamicsSimpleSolver.cpp @@ -0,0 +1,1417 @@ +/* Copyright (c) <2003-2019> +* +* 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 "dgPhysicsStdafx.h" +#include "dgBody.h" +#include "dgWorld.h" +#include "dgConstraint.h" +#include "dgDynamicBody.h" +#include "dgDynamicBody.h" +#include "dgSkeletonContainer.h" +#include "dgCollisionInstance.h" +#include "dgWorldDynamicUpdate.h" +#include "dgBilateralConstraint.h" + +#define DG_IMPULSE_COUNT (DG_PARALLEL_JOINT_COUNT_CUT_OFF + 32) +#define DG_IMPULSE_CONTACT_SPEED dgFloat32(0.8f) + +void dgWorldDynamicUpdate::BuildJacobianMatrix(const dgBodyInfo* const bodyInfoArray, dgJointInfo* const jointInfo, dgJacobian* const internalForces, dgLeftHandSide* const leftHandSide, dgRightHandSide* const rightHandSide, dgFloat32 forceImpulseScale) const +{ + const dgInt32 index = jointInfo->m_pairStart; + const dgInt32 count = jointInfo->m_pairCount; + const dgInt32 m0 = jointInfo->m_m0; + const dgInt32 m1 = jointInfo->m_m1; + + const dgConstraint* const joint = jointInfo->m_joint; + const dgBody* const body0 = bodyInfoArray[m0].m_body; + const dgBody* const body1 = bodyInfoArray[m1].m_body; + const bool isBilateral = joint->IsBilateral(); + + const dgVector invMass0(body0->m_invMass[3]); + const dgMatrix& invInertia0 = body0->m_invWorldInertiaMatrix; + const dgVector invMass1(body1->m_invMass[3]); + const dgMatrix& invInertia1 = body1->m_invWorldInertiaMatrix; + + dgVector force0(dgVector::m_zero); + dgVector torque0(dgVector::m_zero); + if (body0->IsRTTIType(dgBody::m_dynamicBodyRTTI)) { + force0 = ((dgDynamicBody*)body0)->m_externalForce; + torque0 = ((dgDynamicBody*)body0)->m_externalTorque; + } + + dgVector force1(dgVector::m_zero); + dgVector torque1(dgVector::m_zero); + if (body1->IsRTTIType(dgBody::m_dynamicBodyRTTI)) { + force1 = ((dgDynamicBody*)body1)->m_externalForce; + torque1 = ((dgDynamicBody*)body1)->m_externalTorque; + } + + jointInfo->m_preconditioner0 = dgFloat32(1.0f); + jointInfo->m_preconditioner1 = dgFloat32(1.0f); + + if ((invMass0.GetScalar() > dgFloat32(0.0f)) && (invMass1.GetScalar() > dgFloat32(0.0f))) { + const dgFloat32 mass0 = body0->GetMass().m_w; + const dgFloat32 mass1 = body1->GetMass().m_w; + if (mass0 > (DG_DIAGONAL_PRECONDITIONER * mass1)) { + jointInfo->m_preconditioner0 = mass0 / (mass1 * DG_DIAGONAL_PRECONDITIONER); + } else if (mass1 > (DG_DIAGONAL_PRECONDITIONER * mass0)) { + jointInfo->m_preconditioner1 = mass1 / (mass0 * DG_DIAGONAL_PRECONDITIONER); + } + } + + if (joint->IsSkeletonLoop() || joint->IsSkeleton() || (body0->GetSkeleton() && body1->GetSkeleton())) { + jointInfo->m_preconditioner0 = dgFloat32(1.0f); + jointInfo->m_preconditioner1 = dgFloat32(1.0f); + } else { + const dgFloat32 scale0 = joint->GetMassScaleBody0(); + const dgFloat32 scale1 = joint->GetMassScaleBody1(); + if ((scale0 != dgFloat32 (1.0f)) || (scale1 != dgFloat32 (1.0f))) { + jointInfo->m_preconditioner0 = scale0; + jointInfo->m_preconditioner1 = scale1; + } + } + + dgJacobian forceAcc0; + dgJacobian forceAcc1; + const dgVector preconditioner0(jointInfo->m_preconditioner0); + const dgVector preconditioner1(jointInfo->m_preconditioner1); + forceAcc0.m_linear = dgVector::m_zero; + forceAcc0.m_angular = dgVector::m_zero; + forceAcc1.m_linear = dgVector::m_zero; + forceAcc1.m_angular = dgVector::m_zero; + + for (dgInt32 i = 0; i < count; i++) { + dgLeftHandSide* const row = &leftHandSide[index + i]; + dgRightHandSide* const rhs = &rightHandSide[index + i]; + + row->m_JMinv.m_jacobianM0.m_linear = row->m_Jt.m_jacobianM0.m_linear * invMass0; + row->m_JMinv.m_jacobianM0.m_angular = invInertia0.RotateVector(row->m_Jt.m_jacobianM0.m_angular); + row->m_JMinv.m_jacobianM1.m_linear = row->m_Jt.m_jacobianM1.m_linear * invMass1; + row->m_JMinv.m_jacobianM1.m_angular = invInertia1.RotateVector(row->m_Jt.m_jacobianM1.m_angular); + + dgVector tmpAccel(row->m_JMinv.m_jacobianM0.m_linear * force0 + row->m_JMinv.m_jacobianM0.m_angular * torque0 + + row->m_JMinv.m_jacobianM1.m_linear * force1 + row->m_JMinv.m_jacobianM1.m_angular * torque1); + + dgAssert(tmpAccel.m_w == dgFloat32(0.0f)); + dgFloat32 extenalAcceleration = -(tmpAccel.AddHorizontal()).GetScalar(); + rhs->m_deltaAccel = extenalAcceleration * forceImpulseScale; + rhs->m_coordenateAccel += extenalAcceleration * forceImpulseScale; + dgAssert(rhs->m_jointFeebackForce); + + const dgFloat32 force = rhs->m_jointFeebackForce->GetInitiailGuess() * forceImpulseScale; + //const dgFloat32 force = rhs->m_jointFeebackForce->m_force * forceImpulseScale; + + rhs->m_force = isBilateral ? dgClamp(force, rhs->m_lowerBoundFrictionCoefficent, rhs->m_upperBoundFrictionCoefficent) : force; + rhs->m_maxImpact = dgFloat32(0.0f); + + dgVector jMinvM0linear(preconditioner0 * row->m_JMinv.m_jacobianM0.m_linear); + dgVector jMinvM0angular(preconditioner0 * row->m_JMinv.m_jacobianM0.m_angular); + dgVector jMinvM1linear(preconditioner1 * row->m_JMinv.m_jacobianM1.m_linear); + dgVector jMinvM1angular(preconditioner1 * row->m_JMinv.m_jacobianM1.m_angular); + + dgVector tmpDiag(jMinvM0linear * row->m_Jt.m_jacobianM0.m_linear + jMinvM0angular * row->m_Jt.m_jacobianM0.m_angular + + jMinvM1linear * row->m_Jt.m_jacobianM1.m_linear + jMinvM1angular * row->m_Jt.m_jacobianM1.m_angular); + + dgAssert(tmpDiag.m_w == dgFloat32(0.0f)); + dgFloat32 diag = (tmpDiag.AddHorizontal()).GetScalar(); + dgAssert(diag > dgFloat32(0.0f)); + rhs->m_diagDamp = diag * rhs->m_diagonalRegularizer; + diag *= (dgFloat32(1.0f) + rhs->m_diagonalRegularizer); + rhs->m_invJinvMJt = dgFloat32(1.0f) / diag; + + dgAssert(dgCheckFloat(rhs->m_force)); + dgVector val(rhs->m_force); + forceAcc0.m_linear += row->m_Jt.m_jacobianM0.m_linear * val; + forceAcc0.m_angular += row->m_Jt.m_jacobianM0.m_angular * val; + forceAcc1.m_linear += row->m_Jt.m_jacobianM1.m_linear * val; + forceAcc1.m_angular += row->m_Jt.m_jacobianM1.m_angular * val; + } + + forceAcc0.m_linear = forceAcc0.m_linear * preconditioner0; + forceAcc0.m_angular = forceAcc0.m_angular * preconditioner0; + forceAcc1.m_linear = forceAcc1.m_linear * preconditioner1; + forceAcc1.m_angular = forceAcc1.m_angular * preconditioner1; + + internalForces[m0].m_linear += forceAcc0.m_linear; + internalForces[m0].m_angular += forceAcc0.m_angular; + internalForces[m1].m_linear += forceAcc1.m_linear; + internalForces[m1].m_angular += forceAcc1.m_angular; +} + +void dgWorldDynamicUpdate::BuildJacobianMatrix(dgBodyCluster* const cluster, dgInt32 threadID, dgFloat32 timestep) const +{ + D_TRACKTIME(); + dgAssert(cluster->m_bodyCount >= 2); + + dgWorld* const world = (dgWorld*) this; + const dgInt32 bodyCount = cluster->m_bodyCount; + + dgBodyInfo* const bodyArray = &world->m_bodiesMemory[cluster->m_bodyStart]; + dgJacobian* const internalForces = &m_solverMemory.m_internalForcesBuffer[cluster->m_bodyStart]; + + dgAssert(((dgDynamicBody*)bodyArray[0].m_body)->IsRTTIType(dgBody::m_dynamicBodyRTTI)); + dgAssert((((dgDynamicBody*)bodyArray[0].m_body)->m_accel.DotProduct(((dgDynamicBody*)bodyArray[0].m_body)->m_accel)).GetScalar() == dgFloat32(0.0f)); + dgAssert((((dgDynamicBody*)bodyArray[0].m_body)->m_alpha.DotProduct(((dgDynamicBody*)bodyArray[0].m_body)->m_alpha)).GetScalar() == dgFloat32(0.0f)); + dgAssert((((dgDynamicBody*)bodyArray[0].m_body)->m_externalForce.DotProduct(((dgDynamicBody*)bodyArray[0].m_body)->m_externalForce)).GetScalar() == dgFloat32(0.0f)); + dgAssert((((dgDynamicBody*)bodyArray[0].m_body)->m_externalTorque.DotProduct(((dgDynamicBody*)bodyArray[0].m_body)->m_externalTorque)).GetScalar() == dgFloat32(0.0f)); + + internalForces[0].m_linear = dgVector::m_zero; + internalForces[0].m_angular = dgVector::m_zero; + + if (timestep != dgFloat32(0.0f)) { + for (dgInt32 i = 1; i < bodyCount; i++) { + dgBody* const body = (dgDynamicBody*)bodyArray[i].m_body; + dgAssert(body->IsRTTIType(dgBody::m_dynamicBodyRTTI) || body->IsRTTIType(dgBody::m_kinematicBodyRTTI)); + if (!body->m_equilibrium) { + dgAssert(body->m_invMass.m_w > dgFloat32(0.0f)); + body->AddDampingAcceleration(timestep); + body->CalcInvInertiaMatrix(); + } + + // re use these variables for temp storage + body->m_accel = body->m_veloc; + body->m_alpha = body->m_omega; + + internalForces[i].m_linear = dgVector::m_zero; + internalForces[i].m_angular = dgVector::m_zero; + } + } else { + for (dgInt32 i = 1; i < bodyCount; i++) { + dgBody* const body = bodyArray[i].m_body; + dgAssert(body->IsRTTIType(dgBody::m_dynamicBodyRTTI) || body->IsRTTIType(dgBody::m_kinematicBodyRTTI)); + if (!body->m_equilibrium) { + dgAssert(body->m_invMass.m_w > dgFloat32(0.0f)); + body->CalcInvInertiaMatrix(); + } + + // re use these variables for temp storage + body->m_accel = body->m_veloc; + body->m_alpha = body->m_omega; + + internalForces[i].m_linear = dgVector::m_zero; + internalForces[i].m_angular = dgVector::m_zero; + } + } + + dgContraintDescritor constraintParams; + + constraintParams.m_world = world; + constraintParams.m_threadIndex = threadID; + constraintParams.m_timestep = timestep; + constraintParams.m_invTimestep = (timestep > dgFloat32(1.0e-5f)) ? dgFloat32(1.0f / timestep) : dgFloat32(0.0f); + const dgFloat32 forceOrImpulseScale = (timestep > dgFloat32(0.0f)) ? dgFloat32(1.0f) : dgFloat32(0.0f); + + dgJointInfo* const constraintArray = &world->m_jointsMemory[cluster->m_jointStart]; + dgLeftHandSide* const leftHandSide = &m_solverMemory.m_leftHandSizeBuffer[cluster->m_rowStart]; + dgRightHandSide* const rightHandSide = &m_solverMemory.m_righHandSizeBuffer[cluster->m_rowStart]; + + dgInt32 rowCount = 0; + const dgInt32 jointCount = cluster->m_jointCount; + + dgUnsigned8 impactBuffer[256 * (sizeof(dgContact*) + sizeof(dgFloat32))]; + dgDownHeap impactJoints(impactBuffer, sizeof(impactBuffer)); + + for (dgInt32 i = 0; i < jointCount; i++) { + dgJointInfo* const jointInfo = &constraintArray[i]; + dgConstraint* const constraint = jointInfo->m_joint; + + dgAssert(dgInt32(constraint->m_index) == i); + dgAssert(jointInfo->m_m0 < cluster->m_bodyCount); + dgAssert(jointInfo->m_m1 < cluster->m_bodyCount); + + rowCount = GetJacobianDerivatives(constraintParams, jointInfo, constraint, leftHandSide, rightHandSide, rowCount); + dgAssert(rowCount <= cluster->m_rowCount); + + dgAssert(jointInfo->m_m0 >= 0); + dgAssert(jointInfo->m_m0 < bodyCount); + dgAssert(jointInfo->m_m1 >= 0); + dgAssert(jointInfo->m_m1 < bodyCount); + BuildJacobianMatrix(bodyArray, jointInfo, internalForces, leftHandSide, rightHandSide, forceOrImpulseScale); + + dgFloat32 impulseSpeed = constraint->GetImpulseContactSpeed(); + if (impulseSpeed > DG_IMPULSE_CONTACT_SPEED) { + dgContact* contact = (dgContact*)constraint; + impactJoints.Push(contact, impulseSpeed); + } + } + + if (impactJoints.GetCount()) { +// ResolveImpulse(constraintArray, leftHandSide, rightHandSide, impactJoints); + } +} + +void dgWorldDynamicUpdate::ResolveImpulse(const dgJointInfo* const constraintArray, const dgLeftHandSide* const leftHandSide, dgRightHandSide* const rightHandSide, dgDownHeap& impactJoints) const +{ + dgJacobian impulse[DG_IMPULSE_COUNT]; + dgBodyInfo bodyArray[DG_IMPULSE_COUNT]; + dgJointImpulseInfo contactArray[DG_IMPULSE_COUNT]; + dgFloat32 relVeloc[DG_IMPULSE_COUNT]; + dgFloat32 outImpulse[DG_IMPULSE_COUNT]; + + dgAtomicExchangeAndAdd(&m_impulseLru, 1); + while (impactJoints.GetCount()) { + dgContact* const contact = impactJoints[0]; + impactJoints.Pop(); + if (contact->GetImpulseContactSpeed() > DG_IMPULSE_CONTACT_SPEED) { + dgBody* body = contact->GetBody0(); + dgBody* const otherBody = contact->GetBody1(); + bool test = otherBody->GetInvMass().m_w > dgFloat32(0.0f); + test = test && (otherBody->m_veloc.DotProduct(otherBody->m_veloc).GetScalar() > body->m_veloc.DotProduct(body->m_veloc).GetScalar()); + if (test) { + body = otherBody; + } + + dgInt32 bodyCount = 1; + dgInt32 rowsCount = 0; + dgInt32 contactCount = 0; + + bodyArray[0].m_body = body; + impulse[0].m_linear = dgVector::m_zero; + impulse[0].m_angular = dgVector::m_zero; + for (dgBodyMasterListRow::dgListNode* jointNode = body->m_masterNode->GetInfo().GetFirst(); jointNode; jointNode = jointNode->GetNext()) { + dgBodyMasterListCell* const cell = &jointNode->GetInfo(); + dgConstraint* const constraint = cell->m_joint; + + if (constraint->IsActive() && (constraint->GetId() == dgConstraint::m_contactConstraint)) { + dgContact* const contactJoint = (dgContact*)constraint; + if (contactJoint->GetImpulseContactSpeed() > DG_IMPULSE_CONTACT_SPEED) { + dgBody* const body0 = contactJoint->GetBody0(); + dgBody* const body1 = contactJoint->GetBody1(); + dgAssert ((body0 == body) || (body1 == body)); + + const dgJointInfo& jointInfo = constraintArray[contactJoint->m_index]; + dgJointImpulseInfo& contactInfo = contactArray[contactCount]; + contactInfo.m_joint = contactJoint; + dgAssert(jointInfo.m_joint == contactJoint); + + contactInfo.m_pairStart = jointInfo.m_pairStart; + contactInfo.m_pairCount = jointInfo.m_pairCount; + contactInfo.m_rhsStart = rowsCount; + + bodyArray[bodyCount].m_body = (body1 != body) ? body1 : body0; + contactArray[contactCount].m_m0 = (body1 != body) ? 0 : bodyCount; + contactArray[contactCount].m_m1 = (body1 != body) ? bodyCount : 0; + + impulse[bodyCount].m_linear = dgVector::m_zero; + impulse[bodyCount].m_angular = dgVector::m_zero; + + dgAssert (bodyArray[contactArray[contactCount].m_m0].m_body == contactJoint->GetBody0()); + dgAssert (bodyArray[contactArray[contactCount].m_m1].m_body == contactJoint->GetBody1()); + + bodyCount++; + contactCount++; + rowsCount += contactInfo.m_pairCount; + dgAssert(bodyCount < sizeof(bodyArray) / sizeof(bodyArray[0])); + } + } + } + + dgInt32 lru = dgAtomicExchangeAndAdd(&m_impulseLru, 1); + memset(outImpulse, 0, rowsCount * sizeof(dgFloat32)); + for (dgInt32 i = 0; i < contactCount; i++) { + dgJointImpulseInfo* const jointInfo = &contactArray[i]; + jointInfo->m_joint->m_impulseLru = lru; + jointInfo->m_joint->SetImpulseContactSpeed(dgFloat32 (0.0f)); + const dgInt32 index = jointInfo->m_pairStart; + CalculateImpulseVeloc(jointInfo, &leftHandSide[index], &rightHandSide[index], &relVeloc[jointInfo->m_rhsStart]); + } + + dgFloat32 impulseNorm = dgFloat32 (10.0f); + dgFloat32 maxImpulseNorm = DG_IMPULSE_CONTACT_SPEED; + for (dgInt32 j = 0; (j < 4) && (impulseNorm > maxImpulseNorm); j++) { + impulseNorm = dgFloat32 (0.0f); + for (dgInt32 i = 0; i < contactCount; i++) { + dgJointImpulseInfo* const jointInfo = &contactArray[i]; + const dgInt32 index = jointInfo->m_pairStart; + dgFloat32 impulse2 = CalculateJointImpulse(jointInfo, bodyArray, impulse, &leftHandSide[index], &rightHandSide[index], &relVeloc[jointInfo->m_rhsStart], &outImpulse[i]); + impulseNorm += impulse2; + } + } + + for (dgInt32 i = 0; i < bodyCount; i++) { + dgDynamicBody* const impulseBody = (dgDynamicBody*)bodyArray[i].m_body; + const dgVector velocStep(impulse[i].m_linear.Scale(impulseBody->m_invMass.m_w)); + const dgVector omegaStep(impulseBody->m_invWorldInertiaMatrix.RotateVector(impulse[i].m_angular)); + impulseBody->m_veloc += velocStep; + impulseBody->m_omega += omegaStep; + } + + for (dgInt32 i = 0; i < bodyCount; i ++) { + dgBody* const fanBody = bodyArray[i].m_body; + if (fanBody->GetInvMass().m_w > dgFloat32 (0.0f)) { + for (dgBodyMasterListRow::dgListNode* jointNode = fanBody->m_masterNode->GetInfo().GetFirst(); jointNode; jointNode = jointNode->GetNext()) { + dgBodyMasterListCell* const cell = &jointNode->GetInfo(); + dgConstraint* const constraint = cell->m_joint; + + if ((dgInt32 (constraint->m_index) != lru) && constraint->IsActive() && (constraint->GetId() == dgConstraint::m_contactConstraint)) { + dgAssert (0); + } + } + } + } + } + } +} + +dgFloat32 dgWorldDynamicUpdate::CalculateJointImpulse(const dgJointImpulseInfo* const jointInfo, const dgBodyInfo* const bodyArray, dgJacobian* const internalForces, const dgLeftHandSide* const matrixRow, const dgRightHandSide* const rightHandSide, dgFloat32* const relVel, dgFloat32* const out) const +{ + dgVector accNorm(dgVector::m_zero); + + dgFloat32 normalForce[DG_CONSTRAINT_MAX_ROWS + 4]; + + const dgInt32 m0 = jointInfo->m_m0; + const dgInt32 m1 = jointInfo->m_m1; + dgInt32 rowsCount = jointInfo->m_pairCount; + + dgVector linearM0(internalForces[m0].m_linear); + dgVector angularM0(internalForces[m0].m_angular); + dgVector linearM1(internalForces[m1].m_linear); + dgVector angularM1(internalForces[m1].m_angular); + + normalForce[0] = dgFloat32(1.0f); +// const dgInt32 rowStart = jointInfo->m_pairStart; + + for (dgInt32 j = 0; j < rowsCount; j++) { + const dgLeftHandSide* const row = &matrixRow[j]; + const dgRightHandSide* const rhs____ = &rightHandSide[j]; + dgVector a(row->m_JMinv.m_jacobianM0.m_linear * linearM0); + a = a.MulAdd(row->m_JMinv.m_jacobianM0.m_angular, angularM0); + a = a.MulAdd(row->m_JMinv.m_jacobianM1.m_linear, linearM1); + a = a.MulAdd(row->m_JMinv.m_jacobianM1.m_angular, angularM1); + //a = dgVector(rhs->m_coordenateAccel - rhs->m_force * rhs->m_diagDamp) - a.AddHorizontal(); + a = dgVector(relVel[j] - out[j] * rhs____->m_diagDamp) - a.AddHorizontal(); + + //dgVector f(rhs->m_force + rhs->m_invJinvMJt * a.GetScalar()); + dgVector f(out[j] + rhs____->m_invJinvMJt * a.GetScalar()); + dgAssert(rhs____->m_normalForceIndex >= -1); + dgAssert(rhs____->m_normalForceIndex <= rowsCount); + + dgInt32 frictionIndex = rhs____->m_normalForceIndex + 1; + dgFloat32 frictionNormal = normalForce[frictionIndex]; + dgVector lowerFrictionForce(frictionNormal * rhs____->m_lowerBoundFrictionCoefficent); + dgVector upperFrictionForce(frictionNormal * rhs____->m_upperBoundFrictionCoefficent); + + a = a & (f < upperFrictionForce) & (f > lowerFrictionForce); + f = f.GetMax(lowerFrictionForce).GetMin(upperFrictionForce); + accNorm = accNorm.MulAdd(a, a); + + //dgVector deltaForce(f - dgVector(rhs->m_force)); + dgVector deltaForce(f - dgVector(out[j])); + + //rhs->m_force = f.GetScalar(); + out[j] = f.GetScalar(); + normalForce[j + 1] = f.GetScalar(); + + linearM0 = linearM0.MulAdd(row->m_Jt.m_jacobianM0.m_linear, deltaForce); + angularM0 = angularM0.MulAdd(row->m_Jt.m_jacobianM0.m_angular, deltaForce); + linearM1 = linearM1.MulAdd(row->m_Jt.m_jacobianM1.m_linear, deltaForce); + angularM1 = angularM1.MulAdd(row->m_Jt.m_jacobianM1.m_angular, deltaForce); + } + + + dgVector maxAccel(accNorm); + const dgFloat32 tol = dgFloat32(0.5f); + const dgFloat32 tol2 = tol * tol; + for (dgInt32 i = 0; (i < 4) && (maxAccel.GetScalar() > tol2); i++) { + maxAccel = dgVector::m_zero; + for (dgInt32 j = 0; j < rowsCount; j++) { + const dgLeftHandSide* const row = &matrixRow[j]; + const dgRightHandSide* const rhs____ = &rightHandSide[j]; + dgVector a(row->m_JMinv.m_jacobianM0.m_linear * linearM0); + a = a.MulAdd(row->m_JMinv.m_jacobianM0.m_angular, angularM0); + a = a.MulAdd(row->m_JMinv.m_jacobianM1.m_linear, linearM1); + a = a.MulAdd(row->m_JMinv.m_jacobianM1.m_angular, angularM1); + //a = dgVector(rhs->m_coordenateAccel - rhs->m_force * rhs->m_diagDamp) - a.AddHorizontal(); + a = dgVector(relVel[j] - out[j] * rhs____->m_diagDamp) - a.AddHorizontal(); + + //dgVector f(rhs->m_force + rhs->m_invJinvMJt * a.GetScalar()); + dgVector f(out[j] + rhs____->m_invJinvMJt * a.GetScalar()); + dgAssert(rhs____->m_normalForceIndex >= -1); + dgAssert(rhs____->m_normalForceIndex <= rowsCount); + + dgInt32 frictionIndex = rhs____->m_normalForceIndex + 1; + dgFloat32 frictionNormal = normalForce[frictionIndex]; + dgVector lowerFrictionForce(frictionNormal * rhs____->m_lowerBoundFrictionCoefficent); + dgVector upperFrictionForce(frictionNormal * rhs____->m_upperBoundFrictionCoefficent); + + a = a & (f < upperFrictionForce) & (f > lowerFrictionForce); + f = f.GetMax(lowerFrictionForce).GetMin(upperFrictionForce); + maxAccel = maxAccel.MulAdd(a, a); + + //dgVector deltaForce(f - dgVector(rhs->m_force)); + dgVector deltaForce(f - dgVector(out[j])); + + //rhs->m_force = f.GetScalar(); + out[j] = f.GetScalar(); + normalForce[j + 1] = f.GetScalar(); + + linearM0 = linearM0.MulAdd(row->m_Jt.m_jacobianM0.m_linear, deltaForce); + angularM0 = angularM0.MulAdd(row->m_Jt.m_jacobianM0.m_angular, deltaForce); + linearM1 = linearM1.MulAdd(row->m_Jt.m_jacobianM1.m_linear, deltaForce); + angularM1 = angularM1.MulAdd(row->m_Jt.m_jacobianM1.m_angular, deltaForce); + } + } + + + internalForces[m0].m_linear = linearM0; + internalForces[m0].m_angular = angularM0; + internalForces[m1].m_linear = linearM1; + internalForces[m1].m_angular = angularM1; + + return accNorm.GetScalar(); +} + + +void dgWorldDynamicUpdate::CalculateImpulseVeloc(dgJointImpulseInfo* const jointInfo, const dgLeftHandSide* const leftHandSide, const dgRightHandSide* const rightHandSide, dgFloat32* const contactVeloc) const +{ + dgContact* const contact = (dgContact*)jointInfo->m_joint; + const dgVector& bodyVeloc0 = contact->GetBody0()->m_veloc; + const dgVector& bodyOmega0 = contact->GetBody0()->m_omega; + const dgVector& bodyVeloc1 = contact->GetBody1()->m_veloc; + const dgVector& bodyOmega1 = contact->GetBody1()->m_omega; + + const dgInt32 count = jointInfo->m_pairCount; + for (dgInt32 k = 0; k < count; k++) { + const dgRightHandSide* const rhs = &rightHandSide[k]; + const dgLeftHandSide* const row = &leftHandSide[k]; + const dgJacobian &jacobian0 = row->m_Jt.m_jacobianM0; + const dgJacobian &jacobian1 = row->m_Jt.m_jacobianM1; + + dgVector relVeloc(jacobian0.m_linear * bodyVeloc0 + jacobian0.m_angular * bodyOmega0 + jacobian1.m_linear * bodyVeloc1 + jacobian1.m_angular * bodyOmega1); + dgFloat32 vRel = relVeloc.AddHorizontal().GetScalar(); + if (rhs->m_normalForceIndex == DG_INDEPENDENT_ROW) { + dgAssert(rhs->m_restitution >= 0.0f); + dgAssert(rhs->m_restitution <= 2.0f); + dgFloat32 restitution = (vRel <= dgFloat32(0.0f)) ? (dgFloat32(1.0f) + rhs->m_restitution) : dgFloat32(1.0f); + vRel = vRel * restitution; + } + contactVeloc[k] = - vRel; + } +} + +dgInt32 dgWorldDynamicUpdate::SortClusters(const dgBodyCluster* const cluster, dgFloat32 timestep, dgInt32 threadID) const +{ + DG_TRACKTIME(); + dgWorld* const world = (dgWorld*) this; + dgBodyInfo* const bodyArray = &world->m_bodiesMemory[cluster->m_bodyStart]; + dgJointInfo* const constraintArray = &world->m_jointsMemory[cluster->m_jointStart]; + + const dgInt32 bodyCount = cluster->m_bodyCount; + const dgInt32 jointCount = cluster->m_jointCount; + + dgJointInfo* const tmpInfoList = dgAlloca(dgJointInfo, cluster->m_jointCount); + dgJointInfo** queueBuffer = dgAlloca(dgJointInfo*, cluster->m_jointCount * 2 + 1024 * 8); + dgBodyJacobianPair* const bodyJoint = dgAlloca(dgBodyJacobianPair, cluster->m_jointCount * 2); + dgInt32* const bodyJointList = dgAlloca(dgInt32, bodyCount + 1); + + dgQueue queue(queueBuffer, cluster->m_jointCount * 2 + 1024 * 8); + dgFloat32 heaviestMass = dgFloat32(1.0e20f); + dgInt32 infoIndex = 0; + dgInt32 activeJoints = 0; + dgJointInfo* heaviestBody = NULL; + + for (dgInt32 i = 0; i < jointCount; i++) { + dgJointInfo& jointInfo = constraintArray[i]; + tmpInfoList[i] = jointInfo; + tmpInfoList[i].m_preconditioner0 = dgFloat32(0.0f); + + jointInfo.m_joint->m_graphTagged = 0; + const dgInt32 m0 = jointInfo.m_m0; + const dgInt32 m1 = jointInfo.m_m1; + dgBody* const body0 = bodyArray[m0].m_body; + dgBody* const body1 = bodyArray[m1].m_body; + + const dgFloat32 invMass0 = body0->GetInvMass().m_w; + const dgFloat32 invMass1 = body1->GetInvMass().m_w; + + if ((invMass0 == dgFloat32(0.0f)) || (invMass1 == dgFloat32(0.0f))) { + queue.Insert(&tmpInfoList[i]); + tmpInfoList[i].m_preconditioner0 = dgFloat32(1.0f); + } else if (invMass0 && (heaviestMass > invMass0)) { + heaviestMass = invMass0; + heaviestBody = &tmpInfoList[i]; + } else if (invMass1 && (heaviestMass > invMass1)) { + heaviestMass = invMass1; + heaviestBody = &tmpInfoList[i]; + } + + bodyJoint[i * 2 + 0].m_bodyIndex = m0; + bodyJoint[i * 2 + 0].m_JointIndex = i; + bodyJoint[i * 2 + 1].m_bodyIndex = m1; + bodyJoint[i * 2 + 1].m_JointIndex = i; + } + + if (queue.IsEmpty()) { + dgAssert(heaviestBody); + queue.Insert(heaviestBody); + heaviestBody->m_preconditioner0 = dgFloat32(1.0f); + } + + dgSort(bodyJoint, jointCount * 2, CompareBodyJacobianPair); + memset(bodyJointList, 0, sizeof(dgInt32) * (cluster->m_bodyCount + 1)); + for (dgInt32 i = 0; i < jointCount * 2; i++) { + dgInt32 index = bodyJoint[i].m_bodyIndex; + bodyJointList[index] ++; + } + + dgInt32 startIndex = 0; + for (dgInt32 i = 0; i <= bodyCount; i++) { + dgInt32 count = bodyJointList[i]; + bodyJointList[i] = startIndex; + startIndex += count; + } + + while (!queue.IsEmpty()) { + dgInt32 count = queue.m_firstIndex - queue.m_lastIndex; + if (count < 0) { + count += queue.m_mod; + } + + dgInt32 index = queue.m_lastIndex; + queue.Reset(); + + for (dgInt32 i = 0; i < count; i++) { + dgJointInfo* const jointInfo = queue.m_pool[index]; + dgConstraint* const constraint = jointInfo->m_joint; + if (!constraint->m_graphTagged) { + constraint->m_index = infoIndex; + constraintArray[infoIndex] = *jointInfo; + constraint->m_graphTagged = 1; + infoIndex++; + dgAssert(infoIndex <= cluster->m_jointCount); + + const dgInt32 m0 = jointInfo->m_m0; + const dgInt32 m1 = jointInfo->m_m1; + const dgBody* const body0 = bodyArray[m0].m_body; + const dgBody* const body1 = bodyArray[m1].m_body; + + activeJoints += !(body0->m_resting & body1->m_resting); + + if (body0->GetInvMass().m_w > dgFloat32(0.0f)) { + const dgInt32 endJoint = bodyJointList[m0 + 1]; + for (dgInt32 j = bodyJointList[m0]; j < endJoint; j++) { + dgJointInfo* const info = &tmpInfoList[bodyJoint[j].m_JointIndex]; + dgConstraint* const nextConstraint = info->m_joint; + if (!nextConstraint->m_graphTagged) { + if (!info->m_preconditioner0) { + queue.Insert(info); + info->m_preconditioner0 = dgFloat32(1.0f); + } + } + } + } + + if (body1->GetInvMass().m_w > dgFloat32(0.0f)) { + const dgInt32 endJoint = bodyJointList[m1 + 1]; + for (dgInt32 j = bodyJointList[m1]; j < endJoint; j++) { + dgJointInfo* const info = &tmpInfoList[bodyJoint[j].m_JointIndex]; + dgConstraint* const nextConstraint = info->m_joint; + if (!nextConstraint->m_graphTagged) { + if (!info->m_preconditioner0) { + queue.Insert(info); + info->m_preconditioner0 = dgFloat32(1.0f); + } + } + } + } + + if (infoIndex == cluster->m_jointCount) { + queue.Reset(); + break; + } + } + index++; + if (index >= queue.m_mod) { + index = 0; + } + } + } + + dgAssert(infoIndex == cluster->m_jointCount); + return activeJoints; +} + +void dgWorldDynamicUpdate::ResolveClusterForces(dgBodyCluster* const cluster, dgInt32 threadID, dgFloat32 timestep) const +{ + dgInt32 activeJoint = cluster->m_jointCount; + if (activeJoint > 0) { + activeJoint = SortClusters(cluster, timestep, threadID); + } + + dgWorld* const world = (dgWorld*) this; + dgJointInfo* const constraintArray = &world->m_jointsMemory[cluster->m_jointStart]; + + if (!cluster->m_isContinueCollision) { + if (activeJoint >= 1) { + BuildJacobianMatrix(cluster, threadID, timestep); + CalculateClusterReactionForces(cluster, threadID, timestep); + } else if (cluster->m_jointCount == 0) { + IntegrateExternalForce(cluster, timestep, threadID); + } else { + dgAssert((activeJoint == 0) && cluster->m_jointCount); + dgBodyInfo* const bodyArray = &world->m_bodiesMemory[cluster->m_bodyStart]; + dgVector zero(dgVector::m_zero); + for (dgInt32 i = 1; i < cluster->m_bodyCount; i++) { + dgDynamicBody* const body = (dgDynamicBody*)bodyArray[i].m_body; + body->m_accel = zero; + body->m_alpha = zero; + } + } + + IntegrateVelocity (cluster, DG_SOLVER_MAX_ERROR, timestep, threadID); + } else { + // calculate reaction forces and new velocities + BuildJacobianMatrix (cluster, threadID, timestep); + IntegrateReactionsForces (cluster, threadID, timestep); + + // see if the cluster goes to sleep + bool isAutoSleep = true; + bool stackSleeping = true; + dgInt32 sleepCounter = 10000; + + const dgInt32 bodyCount = cluster->m_bodyCount; + dgBodyInfo* const bodyArray = &world->m_bodiesMemory[cluster->m_bodyStart]; + + const dgFloat32 forceDamp = DG_FREEZZING_VELOCITY_DRAG; + dgFloat32 maxAccel = dgFloat32 (0.0f); + dgFloat32 maxAlpha = dgFloat32 (0.0f); + dgFloat32 maxSpeed = dgFloat32 (0.0f); + dgFloat32 maxOmega = dgFloat32 (0.0f); + + const dgFloat32 speedFreeze = world->m_freezeSpeed2; + const dgFloat32 accelFreeze = world->m_freezeAccel2; + const dgVector forceDampVect (forceDamp, forceDamp, forceDamp, dgFloat32 (0.0f)); + for (dgInt32 i = 1; i < bodyCount; i ++) { + dgDynamicBody* const body = (dgDynamicBody*) bodyArray[i].m_body; + if (body->IsRTTIType (dgBody::m_dynamicBodyRTTI)) { + dgAssert (body->m_invMass.m_w); + + const dgFloat32 accel2 = body->m_accel.DotProduct(body->m_accel).GetScalar(); + const dgFloat32 alpha2 = body->m_alpha.DotProduct(body->m_alpha).GetScalar(); + const dgFloat32 speed2 = body->m_veloc.DotProduct(body->m_veloc).GetScalar(); + const dgFloat32 omega2 = body->m_omega.DotProduct(body->m_omega).GetScalar(); + + maxAccel = dgMax (maxAccel, accel2); + maxAlpha = dgMax (maxAlpha, alpha2); + maxSpeed = dgMax (maxSpeed, speed2); + maxOmega = dgMax (maxOmega, omega2); + + bool equilibrium = (accel2 < accelFreeze) && (alpha2 < accelFreeze) && (speed2 < speedFreeze) && (omega2 < speedFreeze); + if (equilibrium) { + dgVector veloc (body->m_veloc * forceDampVect); + dgVector omega = body->m_omega * forceDampVect; + body->m_veloc = (veloc.DotProduct(veloc) > m_velocTol) & veloc; + body->m_omega = (omega.DotProduct(omega) > m_velocTol) & omega; + + } + body->m_equilibrium = equilibrium ? 1 : 0; + stackSleeping &= equilibrium; + isAutoSleep &= body->m_autoSleep; + + sleepCounter = dgMin (sleepCounter, body->m_sleepingCounter); + } + // clear accel and angular acceleration + body->m_accel = dgVector::m_zero; + body->m_alpha = dgVector::m_zero; + } + + if (isAutoSleep) { + if (stackSleeping) { + // the cluster went to sleep mode, + for (dgInt32 i = 1; i < bodyCount; i ++) { + dgBody* const body = bodyArray[i].m_body; + dgAssert (body->IsRTTIType (dgBody::m_dynamicBodyRTTI) || body->IsRTTIType (dgBody::m_kinematicBodyRTTI)); + body->m_accel = dgVector::m_zero; + body->m_alpha = dgVector::m_zero; + body->m_veloc = dgVector::m_zero; + body->m_omega = dgVector::m_zero; + } + } else { + // cluster is not sleeping but may be resting with small residual velocity for a long time + // see if we can force to go to sleep + if ((maxAccel > world->m_sleepTable[DG_SLEEP_ENTRIES - 1].m_maxAccel) || + (maxAlpha > world->m_sleepTable[DG_SLEEP_ENTRIES - 1].m_maxAlpha) || + (maxSpeed > world->m_sleepTable[DG_SLEEP_ENTRIES - 1].m_maxVeloc) || + (maxOmega > world->m_sleepTable[DG_SLEEP_ENTRIES - 1].m_maxOmega)) { + for (dgInt32 i = 1; i < bodyCount; i ++) { + dgDynamicBody* const body = (dgDynamicBody*) bodyArray[i].m_body; + if (body->IsRTTIType (dgBody::m_dynamicBodyRTTI)) { + body->m_sleepingCounter = 0; + } + } + } else { + dgInt32 index = 0; + for (dgInt32 i = 0; i < DG_SLEEP_ENTRIES; i ++) { + if ((maxAccel <= world->m_sleepTable[i].m_maxAccel) && + (maxAlpha <= world->m_sleepTable[i].m_maxAlpha) && + (maxSpeed <= world->m_sleepTable[i].m_maxVeloc) && + (maxOmega <= world->m_sleepTable[i].m_maxOmega)) { + index = i; + break; + } + } + + dgInt32 timeScaleSleepCount = dgInt32 (dgFloat32 (60.0f) * sleepCounter * timestep); + if (timeScaleSleepCount > world->m_sleepTable[index].m_steps) { + // force cluster to sleep + stackSleeping = true; + for (dgInt32 i = 1; i < bodyCount; i ++) { + dgBody* const body = bodyArray[i].m_body; + dgAssert (body->IsRTTIType (dgBody::m_dynamicBodyRTTI) || body->IsRTTIType (dgBody::m_kinematicBodyRTTI)); + body->m_accel = dgVector::m_zero; + body->m_alpha = dgVector::m_zero; + body->m_veloc = dgVector::m_zero; + body->m_omega = dgVector::m_zero; + body->m_equilibrium = 1; + } + } else { + sleepCounter ++; + for (dgInt32 i = 1; i < bodyCount; i ++) { + dgDynamicBody* const body = (dgDynamicBody*) bodyArray[i].m_body; + if (body->IsRTTIType (dgBody::m_dynamicBodyRTTI)) { + body->m_sleepingCounter = sleepCounter; + } + } + } + } + } + } + + if (!(isAutoSleep & stackSleeping)) { + // cluster is not sleeping, need to integrate cluster velocity + const dgUnsigned32 lru = world->GetBroadPhase()->m_lru; + const dgInt32 jointCount = cluster->m_jointCount; + + dgFloat32 timeRemaining = timestep; + const dgFloat32 timeTol = dgFloat32 (0.01f) * timestep; + for (dgInt32 i = 0; (i < DG_MAX_CONTINUE_COLLISON_STEPS) && (timeRemaining > timeTol); i ++) { + // calculate the closest time to impact + dgFloat32 timeToImpact = timeRemaining; + for (dgInt32 j = 0; (j < jointCount) && (timeToImpact > timeTol); j ++) { + dgContact* const contact = (dgContact*) constraintArray[j].m_joint; + if (contact->GetId() == dgConstraint::m_contactConstraint) { + dgDynamicBody* const body0 = (dgDynamicBody*)contact->m_body0; + dgDynamicBody* const body1 = (dgDynamicBody*)contact->m_body1; + if (body0->m_continueCollisionMode | body1->m_continueCollisionMode) { + dgVector p; + dgVector q; + dgVector normal; + dgVector vrel(body0->m_veloc - body1->m_veloc); + dgFloat32 speed2 = vrel.DotProduct(vrel).GetScalar(); + if (speed2 < dgFloat32(1.0e-2f)) { + timeToImpact = dgFloat32(0.0f); + } else { + timeToImpact = dgMin(timeToImpact, world->CalculateTimeToImpact(contact, timeToImpact, threadID, p, q, normal, dgFloat32(-1.0f / 256.0f))); + } + } + } + } + + if (timeToImpact > timeTol) { + timeRemaining -= timeToImpact; + for (dgInt32 j = 1; j < bodyCount; j ++) { + dgDynamicBody* const body = (dgDynamicBody*) bodyArray[j].m_body; + if (body->IsRTTIType (dgBody::m_dynamicBodyRTTI)) { + body->IntegrateVelocity(timeToImpact); + body->UpdateWorlCollisionMatrix(); + } + } + } else { + if (timeToImpact >= dgFloat32 (-1.0e-5f)) { + for (dgInt32 j = 1; j < bodyCount; j++) { + dgDynamicBody* const body = (dgDynamicBody*)bodyArray[j].m_body; + if (body->IsRTTIType(dgBody::m_dynamicBodyRTTI)) { + body->IntegrateVelocity(timeToImpact); + body->UpdateWorlCollisionMatrix(); + } + } + } + + CalculateClusterContacts (cluster, timeRemaining, lru, threadID); + BuildJacobianMatrix (cluster, threadID, 0.0f); + IntegrateReactionsForces (cluster, threadID, 0.0f); + + bool clusterReceding = true; + const dgFloat32 step = timestep * dgFloat32 (1.0f / DG_MAX_CONTINUE_COLLISON_STEPS); + for (dgInt32 k = 0; (k < DG_MAX_CONTINUE_COLLISON_STEPS) && clusterReceding; k ++) { + dgFloat32 smallTimeStep = dgMin (step, timeRemaining); + timeRemaining -= smallTimeStep; + for (dgInt32 j = 1; j < bodyCount; j ++) { + dgDynamicBody* const body = (dgDynamicBody*) bodyArray[j].m_body; + if (body->IsRTTIType (dgBody::m_dynamicBodyRTTI)) { + body->IntegrateVelocity (smallTimeStep); + body->UpdateWorlCollisionMatrix(); + } + } + + clusterReceding = false; + if (timeRemaining > timeTol) { + CalculateClusterContacts (cluster, timeRemaining, lru, threadID); + + bool isColliding = false; + for (dgInt32 j = 0; (j < jointCount) && !isColliding; j ++) { + dgContact* const contact = (dgContact*) constraintArray[j].m_joint; + if (contact->GetId() == dgConstraint::m_contactConstraint) { + + const dgBody* const body0 = contact->m_body0; + const dgBody* const body1 = contact->m_body1; + + const dgVector& veloc0 = body0->m_veloc; + const dgVector& veloc1 = body1->m_veloc; + + const dgVector& omega0 = body0->m_omega; + const dgVector& omega1 = body1->m_omega; + + const dgVector& com0 = body0->m_globalCentreOfMass; + const dgVector& com1 = body1->m_globalCentreOfMass; + + for (dgList::dgListNode* node = contact->GetFirst(); node; node = node->GetNext()) { + const dgContactMaterial* const contactMaterial = &node->GetInfo(); + dgVector vel0 (veloc0 + omega0.CrossProduct(contactMaterial->m_point - com0)); + dgVector vel1 (veloc1 + omega1.CrossProduct(contactMaterial->m_point - com1)); + dgVector vRel (vel0 - vel1); + dgAssert (contactMaterial->m_normal.m_w == dgFloat32 (0.0f)); + dgFloat32 speed = vRel.DotProduct(contactMaterial->m_normal).m_w; + isColliding |= (speed < dgFloat32 (0.0f)); + } + } + } + clusterReceding = !isColliding; + } + } + } + } + + if (timeRemaining > dgFloat32 (0.0)) { + for (dgInt32 j = 1; j < bodyCount; j ++) { + dgDynamicBody* const body = (dgDynamicBody*) bodyArray[j].m_body; + if (body->IsRTTIType (dgBody::m_dynamicBodyRTTI)) { + body->IntegrateVelocity(timeRemaining); + body->UpdateCollisionMatrix (timeRemaining, threadID); + } + } + } else { + for (dgInt32 j = 1; j < bodyCount; j ++) { + dgDynamicBody* const body = (dgDynamicBody*) bodyArray[j].m_body; + if (body->IsRTTIType (dgBody::m_dynamicBodyRTTI)) { + body->UpdateCollisionMatrix (timestep, threadID); + } + } + } + } + } +} + +void dgWorldDynamicUpdate::CalculateClusterContacts(dgBodyCluster* const cluster, dgFloat32 timestep, dgInt32 currLru, dgInt32 threadID) const +{ + dgWorld* const world = (dgWorld*) this; + dgInt32 jointCount = cluster->m_jointCount; + dgJointInfo* const constraintArrayPtr = &world->m_jointsMemory[0]; + dgJointInfo* const constraintArray = &constraintArrayPtr[cluster->m_jointStart]; + + dgBroadPhase::dgPair pair; + dgContactPoint contactArray[DG_MAX_CONTATCS]; + for (dgInt32 j = 0; (j < jointCount); j ++) { + dgContact* const contact = (dgContact*) constraintArray[j].m_joint; + if (contact->GetId() == dgConstraint::m_contactConstraint) { + const dgContactMaterial* const material = contact->m_material; + if (material->m_flags & dgContactMaterial::m_collisionEnable) { + dgInt32 processContacts = 1; + if (material->m_aabbOverlap) { + //processContacts = material->m_aabbOverlap (*material, *contact->GetBody0(), *contact->GetBody1(), threadID); + processContacts = material->m_aabbOverlap(*contact, timestep, threadID); + } + + if (processContacts) { + contact->m_maxDOF = 0; + contact->m_broadphaseLru = currLru; + pair.m_contact = contact; + pair.m_cacheIsValid = false; + pair.m_timestep = timestep; + pair.m_contactBuffer = contactArray; + world->CalculateContacts (&pair, threadID, false, false); + if (pair.m_contactCount) { + dgAssert (pair.m_contactCount <= (DG_CONSTRAINT_MAX_ROWS / 3)); + world->ProcessContacts (&pair, threadID); + } + } + } + } + } +} + +void dgWorldDynamicUpdate::IntegrateExternalForce(const dgBodyCluster* const cluster, dgFloat32 timestep, dgInt32 threadID) const +{ + D_TRACKTIME(); + dgWorld* const world = (dgWorld*) this; + dgBodyInfo* const bodyArray = &world->m_bodiesMemory[cluster->m_bodyStart]; + + dgAssert (timestep > dgFloat32 (0.0f)); + const dgInt32 bodyCount = cluster->m_bodyCount; + for (dgInt32 i = 1; i < bodyCount; i ++) { + dgDynamicBody* const body = (dgDynamicBody*) bodyArray[i].m_body; + body->UpdateGyroData(); + body->AddDampingAcceleration(timestep); + body->IntegrateOpenLoopExternalForce(timestep); + } +} + +void dgWorldDynamicUpdate::CalculateNetAcceleration(dgBody* const body, const dgVector& invTimeStep, const dgVector& maxAccNorm2) const +{ + dgAssert(body->IsRTTIType(dgBody::m_dynamicBodyRTTI) || body->IsRTTIType(dgBody::m_kinematicBodyRTTI)); + // the initial velocity and angular velocity were stored in m_accel and body->m_alpha for memory saving + dgVector accel (invTimeStep * (body->m_veloc - body->m_accel)); + dgVector alpha (invTimeStep * (body->m_omega - body->m_alpha)); + dgVector accelTest((accel.DotProduct(accel) > maxAccNorm2) | (alpha.DotProduct(alpha) > maxAccNorm2)); + accel = accel & accelTest; + alpha = alpha & accelTest; + + body->m_accel = accel; + body->m_alpha = alpha; +} + + +void dgWorldDynamicUpdate::IntegrateReactionsForces(const dgBodyCluster* const cluster, dgInt32 threadID, dgFloat32 timestep) const +{ + if (cluster->m_jointCount == 0) { + IntegrateExternalForce(cluster, timestep, threadID); + } else { + CalculateClusterReactionForces(cluster, threadID, timestep); + } +} + +dgFloat32 dgWorldDynamicUpdate::CalculateJointForce_3_13(const dgJointInfo* const jointInfo, const dgBodyInfo* const bodyArray, dgJacobian* const internalForces, const dgLeftHandSide* const matrixRow, dgRightHandSide* const rightHandSide) const +{ + dgVector accNorm(dgVector::m_zero); + const dgInt32 m0 = jointInfo->m_m0; + const dgInt32 m1 = jointInfo->m_m1; + const dgBody* const body0 = bodyArray[m0].m_body; + const dgBody* const body1 = bodyArray[m1].m_body; + + if (!(body0->m_resting & body1->m_resting)) { + dgFloat32 normalForce[DG_CONSTRAINT_MAX_ROWS + 1]; + dgVector linearM0(internalForces[m0].m_linear); + dgVector angularM0(internalForces[m0].m_angular); + dgVector linearM1(internalForces[m1].m_linear); + dgVector angularM1(internalForces[m1].m_angular); + + const dgVector preconditioner0(jointInfo->m_preconditioner0); + const dgVector preconditioner1(jointInfo->m_preconditioner1); + + const dgInt32 index = jointInfo->m_pairStart; + const dgInt32 rowsCount = jointInfo->m_pairCount; + + normalForce[0] = dgFloat32 (1.0f); + dgVector firstPass(dgVector::m_one); + dgVector maxAccel(dgVector::m_three); + const dgFloat32 restAcceleration = DG_SOLVER_MAX_ERROR * DG_SOLVER_MAX_ERROR * dgFloat32(4.0f); + for (dgInt32 i = 0; (i < 4) && (maxAccel.GetScalar() > restAcceleration); i++) { + maxAccel = dgFloat32(0.0f); + for (dgInt32 j = 0; j < rowsCount; j++) { + dgRightHandSide* const rhs = &rightHandSide[index + j]; + const dgLeftHandSide* const row = &matrixRow[index + j]; + + dgAssert(row->m_Jt.m_jacobianM0.m_linear.m_w == dgFloat32(0.0f)); + dgAssert(row->m_Jt.m_jacobianM0.m_angular.m_w == dgFloat32(0.0f)); + dgAssert(row->m_Jt.m_jacobianM1.m_linear.m_w == dgFloat32(0.0f)); + dgAssert(row->m_Jt.m_jacobianM1.m_angular.m_w == dgFloat32(0.0f)); + + dgVector diag(row->m_JMinv.m_jacobianM0.m_linear * linearM0 + row->m_JMinv.m_jacobianM0.m_angular * angularM0 + + row->m_JMinv.m_jacobianM1.m_linear * linearM1 + row->m_JMinv.m_jacobianM1.m_angular * angularM1); + + dgVector accel(rhs->m_coordenateAccel - rhs->m_force * rhs->m_diagDamp - (diag.AddHorizontal()).GetScalar()); + dgVector force(rhs->m_force + rhs->m_invJinvMJt * accel.GetScalar()); + + dgAssert (rhs->m_normalForceIndex >= -1); + dgAssert (rhs->m_normalForceIndex <= rowsCount); + dgInt32 frictionIndex = rhs->m_normalForceIndex + 1; + + dgFloat32 frictionNormal = normalForce[frictionIndex]; + dgVector lowerFrictionForce(frictionNormal * rhs->m_lowerBoundFrictionCoefficent); + dgVector upperFrictionForce(frictionNormal * rhs->m_upperBoundFrictionCoefficent); + + accel = accel & (force < upperFrictionForce) & (force > lowerFrictionForce); + force = force.GetMax(lowerFrictionForce).GetMin(upperFrictionForce); + maxAccel = maxAccel.GetMax(accel.Abs()); + + dgAssert(maxAccel.m_x >= dgAbs(accel.m_x)); + + accNorm = accNorm.GetMax(maxAccel * firstPass); + + dgVector deltaForce(force - dgVector(rhs->m_force)); + rhs->m_force = force.GetScalar(); + normalForce[j + 1] = force.GetScalar(); + + dgVector deltaforce0(preconditioner0 * deltaForce); + dgVector deltaforce1(preconditioner1 * deltaForce); + + linearM0 += row->m_Jt.m_jacobianM0.m_linear * deltaforce0; + angularM0 += row->m_Jt.m_jacobianM0.m_angular * deltaforce0; + linearM1 += row->m_Jt.m_jacobianM1.m_linear * deltaforce1; + angularM1 += row->m_Jt.m_jacobianM1.m_angular * deltaforce1; + } + firstPass = dgVector::m_zero; + } + + for (dgInt32 i = 0; i < rowsCount; i++) { + //dgLeftHandSide* const row = &matrixRow[index + i]; + dgRightHandSide* const rhs = &rightHandSide[index + i]; + rhs->m_maxImpact = dgMax(dgAbs(rhs->m_force), rhs->m_maxImpact); + } + + internalForces[m0].m_linear = linearM0; + internalForces[m0].m_angular = angularM0; + internalForces[m1].m_linear = linearM1; + internalForces[m1].m_angular = angularM1; + } + + return accNorm.GetScalar() * accNorm.GetScalar(); +} + +dgFloat32 dgWorldDynamicUpdate::CalculateJointForce(const dgJointInfo* const jointInfo, const dgBodyInfo* const bodyArray, dgJacobian* const internalForces, const dgLeftHandSide* const matrixRow, dgRightHandSide* const rightHandSide) const +{ + dgVector accNorm(dgVector::m_zero); + dgFloat32 normalForce[DG_CONSTRAINT_MAX_ROWS + 4]; + + const dgInt32 m0 = jointInfo->m_m0; + const dgInt32 m1 = jointInfo->m_m1; + const dgBody* const body0 = bodyArray[m0].m_body; + const dgBody* const body1 = bodyArray[m1].m_body; + + if (!(body0->m_resting & body1->m_resting)) { + dgInt32 rowsCount = jointInfo->m_pairCount; + + dgVector linearM0(internalForces[m0].m_linear); + dgVector angularM0(internalForces[m0].m_angular); + dgVector linearM1(internalForces[m1].m_linear); + dgVector angularM1(internalForces[m1].m_angular); + + const dgVector preconditioner0(jointInfo->m_preconditioner0); + const dgVector preconditioner1(jointInfo->m_preconditioner1); + + normalForce[0] = dgFloat32(1.0f); + const dgInt32 rowStart = jointInfo->m_pairStart; + for (dgInt32 j = 0; j < rowsCount; j++) { + dgRightHandSide* const rhs = &rightHandSide[rowStart + j]; + const dgLeftHandSide* const row = &matrixRow[rowStart + j]; + dgVector a (row->m_JMinv.m_jacobianM0.m_linear * linearM0); + a = a.MulAdd(row->m_JMinv.m_jacobianM0.m_angular, angularM0); + a = a.MulAdd(row->m_JMinv.m_jacobianM1.m_linear, linearM1); + a = a.MulAdd(row->m_JMinv.m_jacobianM1.m_angular, angularM1); + a = dgVector(rhs->m_coordenateAccel - rhs->m_force * rhs->m_diagDamp) - a.AddHorizontal(); + + dgVector f(rhs->m_force + rhs->m_invJinvMJt * a.GetScalar()); + dgAssert(rhs->m_normalForceIndex >= -1); + dgAssert(rhs->m_normalForceIndex <= rowsCount); + dgInt32 frictionIndex = rhs->m_normalForceIndex + 1; + + dgFloat32 frictionNormal = normalForce[frictionIndex]; + dgVector lowerFrictionForce(frictionNormal * rhs->m_lowerBoundFrictionCoefficent); + dgVector upperFrictionForce(frictionNormal * rhs->m_upperBoundFrictionCoefficent); + + a = a & (f < upperFrictionForce) & (f > lowerFrictionForce); + f = f.GetMax(lowerFrictionForce).GetMin(upperFrictionForce); + accNorm = accNorm.MulAdd(a, a); + + dgVector deltaForce(f - dgVector(rhs->m_force)); + + rhs->m_force = f.GetScalar(); + normalForce[j + 1] = f.GetScalar(); + + dgVector deltaforce0(preconditioner0 * deltaForce); + dgVector deltaforce1(preconditioner1 * deltaForce); + linearM0 = linearM0.MulAdd(row->m_Jt.m_jacobianM0.m_linear, deltaforce0); + angularM0 = angularM0.MulAdd(row->m_Jt.m_jacobianM0.m_angular, deltaforce0); + linearM1 = linearM1.MulAdd(row->m_Jt.m_jacobianM1.m_linear, deltaforce1); + angularM1 = angularM1.MulAdd(row->m_Jt.m_jacobianM1.m_angular, deltaforce1); + } + + dgVector maxAccel(accNorm); + const dgFloat32 tol = dgFloat32(0.5f); + const dgFloat32 tol2 = tol * tol; + for (dgInt32 i = 0; (i < 4) && (maxAccel.GetScalar() > tol2); i++) { + maxAccel = dgVector::m_zero; + for (dgInt32 j = 0; j < rowsCount; j++) { + dgRightHandSide* const rhs = &rightHandSide[rowStart + j]; + const dgLeftHandSide* const row = &matrixRow[rowStart + j]; + dgVector a(row->m_JMinv.m_jacobianM0.m_linear * linearM0); + a = a.MulAdd(row->m_JMinv.m_jacobianM0.m_angular, angularM0); + a = a.MulAdd(row->m_JMinv.m_jacobianM1.m_linear, linearM1); + a = a.MulAdd(row->m_JMinv.m_jacobianM1.m_angular, angularM1); + a = dgVector(rhs->m_coordenateAccel - rhs->m_force * rhs->m_diagDamp) - a.AddHorizontal(); + + dgVector f(rhs->m_force + rhs->m_invJinvMJt * a.GetScalar()); + dgAssert(rhs->m_normalForceIndex >= -1); + dgAssert(rhs->m_normalForceIndex <= rowsCount); + dgInt32 frictionIndex = rhs->m_normalForceIndex + 1; + + dgFloat32 frictionNormal = normalForce[frictionIndex]; + dgVector lowerFrictionForce(frictionNormal * rhs->m_lowerBoundFrictionCoefficent); + dgVector upperFrictionForce(frictionNormal * rhs->m_upperBoundFrictionCoefficent); + + a = a & (f < upperFrictionForce) & (f > lowerFrictionForce); + f = f.GetMax(lowerFrictionForce).GetMin(upperFrictionForce); + maxAccel = maxAccel.MulAdd (a, a); + + dgVector deltaForce(f - dgVector(rhs->m_force)); + + rhs->m_force = f.GetScalar(); + normalForce[j + 1] = f.GetScalar(); + + dgVector deltaforce0(preconditioner0 * deltaForce); + dgVector deltaforce1(preconditioner1 * deltaForce); + linearM0 = linearM0.MulAdd(row->m_Jt.m_jacobianM0.m_linear, deltaforce0); + angularM0 = angularM0.MulAdd(row->m_Jt.m_jacobianM0.m_angular, deltaforce0); + linearM1 = linearM1.MulAdd(row->m_Jt.m_jacobianM1.m_linear, deltaforce1); + angularM1 = angularM1.MulAdd(row->m_Jt.m_jacobianM1.m_angular, deltaforce1); + } + } + + for (dgInt32 i = 0; i < rowsCount; i++) { + dgRightHandSide* const rhs = &rightHandSide[rowStart + i]; + rhs->m_maxImpact = dgMax(dgAbs(rhs->m_force), rhs->m_maxImpact); + } + + internalForces[m0].m_linear = linearM0; + internalForces[m0].m_angular = angularM0; + internalForces[m1].m_linear = linearM1; + internalForces[m1].m_angular = angularM1; + } + return accNorm.GetScalar(); +} + + +dgJacobian dgWorldDynamicUpdate::IntegrateForceAndToque(dgDynamicBody* const body, const dgVector& force, const dgVector& torque, const dgVector& timestep) const +{ + dgJacobian velocStep; + if (body->m_gyroTorqueOn) { + dgVector dtHalf(timestep * dgVector::m_half); + dgMatrix matrix(body->m_gyroRotation, dgVector::m_wOne); + + dgVector localOmega(matrix.UnrotateVector(body->m_omega)); + dgVector localTorque(matrix.UnrotateVector(torque)); + + // derivative at half time step. (similar to midpoint Euler so that it does not loses too much energy) + dgVector dw(localOmega * dtHalf); + dgVector inertia(body->m_mass); + + dgMatrix jacobianMatrix( + dgVector(inertia[0], (inertia[2] - inertia[1]) * dw[2], (inertia[2] - inertia[1]) * dw[1], dgFloat32(0.0f)), + dgVector((inertia[0] - inertia[2]) * dw[2], inertia[1], (inertia[0] - inertia[2]) * dw[0], dgFloat32(1.0f)), + dgVector((inertia[1] - inertia[0]) * dw[1], (inertia[1] - inertia[0]) * dw[0], inertia[2], dgFloat32(1.0f)), + dgVector::m_wOne); + + // and solving for alpha we get the angular acceleration at t + dt + // calculate gradient at a full time step + //dgVector gradientStep(localTorque * timestep); + dgVector gradientStep (jacobianMatrix.SolveByGaussianElimination(localTorque * timestep)); + + dgVector omega(matrix.RotateVector(localOmega + gradientStep)); + dgAssert(omega.m_w == dgFloat32(0.0f)); + + // integrate rotation here + dgFloat32 omegaMag2 = omega.DotProduct(omega).GetScalar() + dgFloat32(1.0e-12f); + dgFloat32 invOmegaMag = dgRsqrt(omegaMag2); + dgVector omegaAxis(omega.Scale(invOmegaMag)); + dgFloat32 omegaAngle = invOmegaMag * omegaMag2 * timestep.GetScalar(); + dgQuaternion deltaRotation(omegaAxis, omegaAngle); + body->m_gyroRotation = body->m_gyroRotation * deltaRotation; + dgAssert((body->m_gyroRotation.DotProduct(body->m_gyroRotation) - dgFloat32(1.0f)) < dgFloat32(1.0e-5f)); + + matrix = dgMatrix(body->m_gyroRotation, dgVector::m_wOne); + localOmega = matrix.UnrotateVector(omega); + //dgVector angularMomentum(inertia * localOmega); + //body->m_gyroTorque = matrix.RotateVector(localOmega.CrossProduct(angularMomentum)); + //body->m_gyroAlpha = body->m_invWorldInertiaMatrix.RotateVector(body->m_gyroTorque); + dgVector localGyroTorque(localOmega.CrossProduct(inertia * localOmega)); + body->m_gyroTorque = matrix.RotateVector(localGyroTorque); + body->m_gyroAlpha = matrix.RotateVector(localGyroTorque * body->m_invMass); + + velocStep.m_angular = matrix.RotateVector(gradientStep); + } else { + velocStep.m_angular = body->m_invWorldInertiaMatrix.RotateVector(torque - body->m_gyroTorque) * timestep; +// velocStep.m_angular = velocStep.m_angular * dgVector::m_half; + } + + velocStep.m_linear = force.Scale(body->m_invMass.m_w) * timestep; + return velocStep; +} + + +void dgWorldDynamicUpdate::CalculateClusterReactionForces(const dgBodyCluster* const cluster, dgInt32 threadID, dgFloat32 timestep) const +{ + D_TRACKTIME(); + dgWorld* const world = (dgWorld*) this; + const dgInt32 bodyCount = cluster->m_bodyCount; + const dgInt32 jointCount = cluster->m_jointCount; + + dgJacobian* const internalForces = &m_solverMemory.m_internalForcesBuffer[cluster->m_bodyStart]; + dgBodyInfo* const bodyArray = &world->m_bodiesMemory[cluster->m_bodyStart]; + dgJointInfo* const constraintArray = &world->m_jointsMemory[cluster->m_jointStart]; + + dgRightHandSide* const rightHandSide = &m_solverMemory.m_righHandSizeBuffer[cluster->m_rowStart]; + const dgLeftHandSide* const leftHandSide = &m_solverMemory.m_leftHandSizeBuffer[cluster->m_rowStart]; + + const dgInt32 derivativesEvaluationsRK4 = 4; + dgFloat32 invTimestep = (timestep > dgFloat32(0.0f)) ? dgFloat32(1.0f) / timestep : dgFloat32(0.0f); + dgFloat32 invStepRK = (dgFloat32(1.0f) / dgFloat32(derivativesEvaluationsRK4)); + dgFloat32 timestepRK = timestep * invStepRK; + dgFloat32 invTimestepRK = invTimestep * dgFloat32(derivativesEvaluationsRK4); + dgAssert(bodyArray[0].m_body == world->m_sentinelBody); + + dgVector speedFreeze2(world->m_freezeSpeed2 * dgFloat32(0.1f)); + dgVector freezeOmega2(world->m_freezeOmega2 * dgFloat32(0.1f)); + + dgJointAccelerationDecriptor joindDesc; + joindDesc.m_timeStep = timestepRK; + joindDesc.m_invTimeStep = invTimestepRK; + joindDesc.m_firstPassCoefFlag = dgFloat32(0.0f); + + dgInt32 skeletonCount = 0; + dgSkeletonList& skeletonList = *world; + dgSkeletonContainer* skeletonArray[DG_MAX_SKELETON_JOINT_COUNT]; + dgInt32 lru = dgAtomicExchangeAndAdd(&skeletonList.m_lruMarker, 1); + for (dgInt32 i = 1; i < bodyCount; i++) { + dgDynamicBody* const body = (dgDynamicBody*)bodyArray[i].m_body; + dgSkeletonContainer* const container = body->GetSkeleton(); + if (container && (container->m_lru != lru)) { + container->m_lru = lru; + skeletonArray[skeletonCount] = container; + container->InitMassMatrix(constraintArray, leftHandSide, rightHandSide); + skeletonCount++; + dgAssert(skeletonCount < dgInt32(sizeof(skeletonArray) / sizeof(skeletonArray[0]))); + } + } + + const dgInt32 passes = world->m_solverIterations; + const dgFloat32 maxAccNorm = DG_SOLVER_MAX_ERROR * DG_SOLVER_MAX_ERROR; + for (dgInt32 step = 0; step < derivativesEvaluationsRK4; step++) { + + for (dgInt32 i = 0; i < jointCount; i++) { + dgJointInfo* const jointInfo = &constraintArray[i]; + dgConstraint* const constraint = jointInfo->m_joint; + const dgInt32 pairStart = jointInfo->m_pairStart; + + joindDesc.m_rowsCount = jointInfo->m_pairCount; + joindDesc.m_leftHandSide = &leftHandSide[pairStart]; + joindDesc.m_rightHandSide = &rightHandSide[pairStart]; + constraint->JointAccelerations(&joindDesc); + } + joindDesc.m_firstPassCoefFlag = dgFloat32(1.0f); + + dgFloat32 accNorm = maxAccNorm * dgFloat32(2.0f); + for (dgInt32 i = 0; (i < passes) && (accNorm > maxAccNorm); i++) { + accNorm = dgFloat32(0.0f); + for (dgInt32 j = 0; j < jointCount; j++) { + dgJointInfo* const jointInfo = &constraintArray[j]; + //dgFloat32 accel2 = CalculateJointForce_3_13(jointInfo, bodyArray, internalForces, leftHandSide); + dgFloat32 accel2 = CalculateJointForce(jointInfo, bodyArray, internalForces, leftHandSide, rightHandSide); + accNorm += accel2; + } + } + for (dgInt32 j = 0; j < skeletonCount; j++) { + skeletonArray[j]->CalculateJointForce(constraintArray, bodyArray, internalForces); + } + + if (timestepRK != dgFloat32(0.0f)) { + dgVector timestep4(timestepRK); + for (dgInt32 i = 1; i < bodyCount; i++) { + dgDynamicBody* const body = (dgDynamicBody*)bodyArray[i].m_body; + dgAssert(body->m_index == i); + if (body->IsRTTIType(dgBody::m_dynamicBodyRTTI)) { + const dgJacobian& forceAndTorque = internalForces[i]; + const dgVector force(body->m_externalForce + forceAndTorque.m_linear); + const dgVector torque(body->m_externalTorque + forceAndTorque.m_angular); + + dgJacobian velocStep(body->IntegrateForceAndToque(force, torque, timestep4)); + if (!body->m_resting) { + body->m_veloc += velocStep.m_linear; + body->m_omega += velocStep.m_angular; + } else { + const dgVector velocStep2(velocStep.m_linear.DotProduct(velocStep.m_linear)); + const dgVector omegaStep2(velocStep.m_angular.DotProduct(velocStep.m_angular)); + const dgVector test(((velocStep2 > speedFreeze2) | (omegaStep2 > speedFreeze2)) & dgVector::m_negOne); + const dgInt32 equilibrium = test.GetSignMask() ? 0 : 1; + body->m_resting &= equilibrium; + } + + dgAssert(body->m_veloc.m_w == dgFloat32(0.0f)); + dgAssert(body->m_omega.m_w == dgFloat32(0.0f)); + } + } + } else { + for (dgInt32 i = 1; i < bodyCount; i++) { + dgDynamicBody* const body = (dgDynamicBody*)bodyArray[i].m_body; + const dgVector& linearMomentum = internalForces[i].m_linear; + const dgVector& angularMomentum = internalForces[i].m_angular; + + body->m_veloc += linearMomentum.Scale(body->m_invMass.m_w); + body->m_omega += body->m_invWorldInertiaMatrix.RotateVector(angularMomentum); + } + } + } + + dgInt32 hasJointFeeback = 0; + if (timestepRK != dgFloat32(0.0f)) { + for (dgInt32 i = 0; i < jointCount; i++) { + dgJointInfo* const jointInfo = &constraintArray[i]; + dgConstraint* const constraint = jointInfo->m_joint; + + const dgInt32 first = jointInfo->m_pairStart; + const dgInt32 count = jointInfo->m_pairCount; + + for (dgInt32 j = 0; j < count; j++) { + dgRightHandSide* const rhs = &rightHandSide[j + first]; + dgAssert(dgCheckFloat(rhs->m_force)); + rhs->m_jointFeebackForce->Push(rhs->m_force); + rhs->m_jointFeebackForce->m_force = rhs->m_force; + rhs->m_jointFeebackForce->m_impact = rhs->m_maxImpact * timestepRK; + } + hasJointFeeback |= (constraint->m_updaFeedbackCallback ? 1 : 0); + } + + //const dgFloat32 zeroAceel = (jointCount >= 16) ? DG_SOLVER_MAX_ERROR : DG_SOLVER_MAX_ERROR * dgFloat32 (0.25f); + const dgFloat32 zeroAceel = (bodyCount >= DG_SMALL_ISLAND_COUNT) ? DG_SOLVER_MAX_ERROR : DG_SOLVER_MAX_ERROR * dgFloat32 (0.125f); + const dgVector invTime(invTimestep); + const dgVector maxAccNorm2(zeroAceel * zeroAceel); + + for (dgInt32 i = 1; i < bodyCount; i++) { + dgBody* const body = bodyArray[i].m_body; + CalculateNetAcceleration(body, invTime, maxAccNorm2); + } + + if (hasJointFeeback) { + for (dgInt32 i = 0; i < jointCount; i++) { + if (constraintArray[i].m_joint->m_updaFeedbackCallback) { + constraintArray[i].m_joint->m_updaFeedbackCallback(*constraintArray[i].m_joint, timestep, threadID); + } + } + } + } else { + for (dgInt32 i = 1; i < bodyCount; i++) { + dgBody* const body = bodyArray[i].m_body; + dgAssert(body->IsRTTIType(dgBody::m_dynamicBodyRTTI) || body->IsRTTIType(dgBody::m_kinematicBodyRTTI)); + body->m_accel = dgVector::m_zero; + body->m_alpha = dgVector::m_zero; + } + } +} + diff --git a/thirdparty/src/newton/dgPhysics/dgWorldPlugins.cpp b/thirdparty/src/newton/dgPhysics/dgWorldPlugins.cpp new file mode 100644 index 000000000..e6a4d65ad --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgWorldPlugins.cpp @@ -0,0 +1,207 @@ +/* Copyright (c) <2003-2019> +* +* 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 "dgPhysicsStdafx.h" +#include "dgWorld.h" +#include "dgWorldPlugins.h" + + +#if __linux__ +#include +#include +#endif + + +dgWorldPluginList::dgWorldPluginList(dgMemoryAllocator* const allocator) + :dgList(allocator) + ,m_currentPlugin(NULL) + ,m_preferedPlugin(NULL) +{ +} + +dgWorldPluginList::~dgWorldPluginList() +{ +} + + +void dgWorldPluginList::LoadVisualStudioPlugins(const char* const plugInPath) +{ +#if defined(DG_USE_PLUGINS) && defined(_MSC_VER) + char rootPathInPath[2048]; + sprintf(rootPathInPath, "%s/*.dll", plugInPath); + + dgInt32 score = 0; + dgWorld* const world = (dgWorld*) this; + + // scan for all plugins in this folder + _finddata_t data; + intptr_t handle = _findfirst(rootPathInPath, &data); + if (handle != -1) { + do { + sprintf(rootPathInPath, "%s/%s", plugInPath, data.name); + HMODULE module = LoadLibrary(rootPathInPath); + + if (module) { + // get the interface function pointer to the Plug in classes + InitPlugin initModule = (InitPlugin)GetProcAddress(module, "GetPlugin"); + if (initModule) { + dgWorldPlugin* const plugin = initModule(world, GetAllocator ()); + if (plugin) { + dgWorldPluginModulePair entry(plugin, module); + dgListNode* const node = Append(entry); + dgInt32 pluginValue = plugin->GetScore(); + bool wasMoved = false; + for (dgListNode* ptr = GetLast()->GetPrev(); ptr; ptr = ptr->GetPrev()) { + dgInt32 value = ptr->GetInfo().m_plugin->GetScore(); + if (value > pluginValue) { + InsertAfter (ptr, node); + wasMoved = true; + break; + } + } + if (!wasMoved) { + InsertBefore (GetFirst(), node); + } + + if (pluginValue > score) { + score = pluginValue; + m_preferedPlugin = node; + } + } else { + FreeLibrary(module); + } + } else { + FreeLibrary(module); + } + } + + } while (_findnext(handle, &data) == 0); + + _findclose(handle); + } +#endif +} + +void dgWorldPluginList::LoadLinuxPlugins(const char* const plugInPath) +{ +#if defined(DG_USE_PLUGINS) && defined(__linux__) + char rootPathInPath[2048]; + DIR* directory; + dirent* dirEntry; + directory = opendir(plugInPath); + + dgInt32 score = 0; + dgWorld* const world = (dgWorld*) this; + + if(directory != NULL) { + while((dirEntry = readdir(directory)) != NULL) { + const char* const ext = strrchr(dirEntry->d_name, '.'); + if(!strcmp(ext, ".so")) { + sprintf(rootPathInPath, "%s/%s", plugInPath, dirEntry->d_name); + void* module = dlopen(rootPathInPath, RTLD_LAZY); + auto err = dlerror(); + if(module) { + InitPlugin initModule = (InitPlugin)dlsym(module, "GetPlugin"); + if(initModule) { + dgWorldPlugin* const plugin = initModule(world, GetAllocator ()); + if (plugin) { + dgWorldPluginModulePair entry(plugin, module); + dgListNode* const node = Append(entry); + dgInt32 pluginValue = plugin->GetScore(); + if (pluginValue > score) { + score = pluginValue; + m_preferedPlugin = node; + } + } else { + dlclose(module); + } + } + } + } + } + closedir(directory); + } +#endif +} + +void dgWorldPluginList::LoadPlugins(const char* const path) +{ + UnloadPlugins(); + #ifdef _MSC_VER + LoadVisualStudioPlugins(path); + #elif __linux__ + LoadLinuxPlugins(path); + #endif +} + +void dgWorldPluginList::UnloadPlugins() +{ +#if defined(DG_USE_PLUGINS) + #ifdef _MSC_VER + dgWorldPluginList& pluginsList = *this; + for (dgWorldPluginList::dgListNode* node = pluginsList.GetFirst(); node; node = node->GetNext()) { + HMODULE module = (HMODULE)node->GetInfo().m_module; + FreeLibrary(module); + } + #elif __linux__ + dgWorldPluginList& pluginsList = *this; + for (dgWorldPluginList::dgListNode* node = pluginsList.GetFirst(); node; node = node->GetNext()) { + void* module = node->GetInfo().m_module; + dlclose(module); + } + #endif +#endif + m_currentPlugin = NULL; + m_preferedPlugin = NULL; +} + +dgWorldPluginList::dgListNode* dgWorldPluginList::GetCurrentPlugin() +{ + return m_currentPlugin; +} + +dgWorldPluginList::dgListNode* dgWorldPluginList::GetpreferedPlugin() +{ + return m_preferedPlugin; +} + +dgWorldPluginList::dgListNode* dgWorldPluginList::GetFirstPlugin() +{ + dgWorldPluginList& list = *this; + return list.GetFirst(); +} + +dgWorldPluginList::dgListNode* dgWorldPluginList::GetNextPlugin(dgListNode* const plugin) +{ + return plugin->GetNext(); +} + +const char* dgWorldPluginList::GetPluginId(dgListNode* const pluginNode) +{ + dgWorldPluginModulePair entry(pluginNode->GetInfo()); + dgWorldPlugin* const plugin = entry.m_plugin; + return plugin->GetId(); +} + +void dgWorldPluginList::SelectPlugin(dgListNode* const plugin) +{ + m_currentPlugin = plugin; +} diff --git a/thirdparty/src/newton/dgPhysics/dgWorldPlugins.h b/thirdparty/src/newton/dgPhysics/dgWorldPlugins.h new file mode 100644 index 000000000..3ea4f8711 --- /dev/null +++ b/thirdparty/src/newton/dgPhysics/dgWorldPlugins.h @@ -0,0 +1,111 @@ +/* Copyright (c) <2003-2019> +* +* 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_WORLD_PLUGINS_H_ +#define _DG_WORLD_PLUGINS_H_ + +class dgWorld; +class dgBodyInfo; +class dgJointInfo; +class dgBodyCluster; + + +class dgWorldPlugin +{ + public: + dgWorldPlugin(dgWorld* const world, dgMemoryAllocator* const allocator) + :m_world(world) + ,m_allocator(allocator) + { + } + + virtual ~dgWorldPlugin() + { + } + + virtual const char* GetId() const = 0; + virtual dgInt32 GetScore() const = 0; + virtual void FlushRegisters() const = 0; + virtual void CalculateJointForces(const dgBodyCluster& cluster, dgBodyInfo* const bodyArray, dgJointInfo* const jointArray, dgFloat32 timestep) = 0; + virtual void SolveDenseLcp(dgInt32 stride, dgInt32 size, const dgFloat32* const matrix, const dgFloat32* const x0, dgFloat32* const x, const dgFloat32* const b, const dgFloat32* const low, const dgFloat32* const high, const dgInt32* const normalIndex) const = 0; + + protected: + dgWorld* m_world; + dgMemoryAllocator* m_allocator; + friend class dgWorld; +}; + +#ifdef __cplusplus +extern "C" +{ + typedef dgWorldPlugin* (*InitPlugin)(dgWorld* const world, dgMemoryAllocator* const allocator); +} +#endif + + +class dgWorldPluginModulePair +{ + public: + dgWorldPluginModulePair (dgWorldPlugin* const plugin, void* module) + :m_plugin(plugin) + ,m_module(module) + { + } + dgWorldPlugin* m_plugin; + void* m_module; +}; + +class dgWorldPluginList: public dgList +{ + public: + dgWorldPluginList(dgMemoryAllocator* const allocator); + ~dgWorldPluginList(); + + void LoadPlugins(const char* const path); + void UnloadPlugins(); + + dgListNode* GetFirstPlugin(); + dgListNode* GetCurrentPlugin(); + dgListNode* GetpreferedPlugin(); + dgListNode* GetNextPlugin(dgListNode* const plugin); + const char* GetPluginId(dgListNode* const plugin); + void SelectPlugin(dgListNode* const plugin); + + void FlushRegisters() const + { + if (m_currentPlugin) { + dgWorldPlugin* const plugin = m_currentPlugin->GetInfo().m_plugin; + plugin->FlushRegisters(); + } + } + + private: + void LoadVisualStudioPlugins(const char* const path); + void LoadLinuxPlugins(const char* const path); + + dgListNode* m_currentPlugin; + dgListNode* m_preferedPlugin; +}; + + + + +#endif