diff --git a/.gitignore b/.gitignore index f5e9e54..f434d3b 100644 --- a/.gitignore +++ b/.gitignore @@ -11,3 +11,10 @@ builtins /.vscode .vscode *.der +*.sublime-project +*.sublime-workspace +compile_commands.json +compile_flags.txt + +.clang-format +.clangd \ No newline at end of file diff --git a/README.md b/README.md index 6a94c5b..48182e6 100644 --- a/README.md +++ b/README.md @@ -29,6 +29,10 @@ Open your game.project file and in the dependencies field under project add: ## Release Notes +2.1.3 + +- Small fix to prevent collision with Defold's build-in box2D lib. + 2.1.2 - Internal Update Frequency added. Default value is set from game.project file (display.frequency). diff --git a/daabbcc/include/DynamicTree.hpp b/daabbcc/include/DynamicTree.hpp index 3400a4a..fdf925b 100644 --- a/daabbcc/include/DynamicTree.hpp +++ b/daabbcc/include/DynamicTree.hpp @@ -1,5 +1,4 @@ -#ifndef DynamicTree_hpp -#define DynamicTree_hpp +#pragma once #include "DynamicTree/b2DynamicTree.h" #include @@ -9,6 +8,7 @@ #define JC_SORT_IMPLEMENTATION #include "jc/sort.h" +namespace daabbcc { struct orderResultValues { int32 proxyID; float32 distance; @@ -19,7 +19,8 @@ class DynamicTree { DynamicTree(); ~DynamicTree(); - int m_UpdateFrequency; // = dmConfigFile::GetInt(m_Config, "display.update_frequency", 0); + int m_UpdateFrequency; // = dmConfigFile::GetInt(m_Config, + // "display.update_frequency", 0); // Add new group int AddGroup(); @@ -44,19 +45,23 @@ class DynamicTree { bool QueryCallback(int32 proxyId, int groupId); // b2DynamicTree raycast callback - float32 RayCastCallback(const b2RayCastInputAABB &input, int32 proxyId, int groupId); + float32 RayCastCallback(const b2RayCastInputAABB &input, int32 proxyId, + int groupId); // RayCast to AABB - void RayCast(int groupId, float start_x, float start_y, float end_x, float end_y); + void RayCast(int groupId, float start_x, float start_y, float end_x, + float end_y); - void RayCastSort(int groupId, float start_x, float start_y, float end_x, float end_y); + void RayCastSort(int groupId, float start_x, float start_y, float end_x, + float end_y); // Query with AABB void QueryAABB(int groupId, float x, float y, int w, int h); // Query with ID - void QueryID(int groupId, - int proxyID); // std::vector QueryID(int groupId, int proxyID); + void + QueryID(int groupId, + int proxyID); // std::vector QueryID(int groupId, int proxyID); // Query with ID - Distance Ordered void QueryIDSort(int groupId, int proxyID); @@ -81,7 +86,8 @@ class DynamicTree { bool isSorted; - void AddGameObject(uint32_t groupID, int32 proxyId, dmGameObject::HInstance instance, int32 w, int32 h); + void AddGameObject(uint32_t groupID, int32 proxyId, + dmGameObject::HInstance instance, int32 w, int32 h); void GameobjectUpdate(); @@ -132,7 +138,8 @@ class DynamicTree { orderResultValues tmpOrder; dmArray tmpOrderResult; - static void IterateRemoveCallback(DynamicTree *context, const uint32_t *key, Groups *value); + static void IterateRemoveCallback(DynamicTree *context, const uint32_t *key, + Groups *value); void CalcTimeStep(float &step_dt, uint32_t &num_steps); void DestroyProxyID(int groupId, int proxyID); @@ -140,5 +147,4 @@ class DynamicTree { // Reset class void ResetTree(); }; - -#endif /* DynamicTree_hpp */ +} // namespace daabbcc diff --git a/daabbcc/include/DynamicTree/b2Collision.h b/daabbcc/include/DynamicTree/b2Collision.h index d6a8d3d..b4993e2 100755 --- a/daabbcc/include/DynamicTree/b2Collision.h +++ b/daabbcc/include/DynamicTree/b2Collision.h @@ -1,122 +1,108 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this 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 B2_COLLISION_H -#define B2_COLLISION_H + * Copyright (c) 2006-2009 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this 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. + */ + +#pragma once #include "b2Math.h" -/// Ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1). -struct b2RayCastInputAABB -{ - b2Vec2 p1, p2; - float32 maxFraction; +namespace daabbcc { + +/// Ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - +/// p1). +struct b2RayCastInputAABB { + b2Vec2 p1, p2; + float32 maxFraction; }; -/// Ray-cast output data. The ray hits at p1 + fraction * (p2 - p1), where p1 and p2 -/// come from b2RayCastInputAABB. -struct b2RayCastOutputAABB -{ - b2Vec2 normal; - float32 fraction; +/// Ray-cast output data. The ray hits at p1 + fraction * (p2 - p1), where p1 +/// and p2 come from b2RayCastInputAABB. +struct b2RayCastOutputAABB { + b2Vec2 normal; + float32 fraction; }; /// An axis aligned bounding box. -struct b2AABB -{ - /// Verify that the bounds are sorted. - bool IsValid() const; - - /// Get the center of the AABB. - b2Vec2 GetCenter() const - { - return 0.5f * (lowerBound + upperBound); - } - - /// Get the extents of the AABB (half-widths). - b2Vec2 GetExtents() const - { - return 0.5f * (upperBound - lowerBound); - } - - /// Get the perimeter length - float32 GetPerimeter() const - { - float32 wx = upperBound.x - lowerBound.x; - float32 wy = upperBound.y - lowerBound.y; - return 2.0f * (wx + wy); - } - - /// Combine an AABB into this one. - void Combine(const b2AABB& aabb) - { - lowerBound = b2Min(lowerBound, aabb.lowerBound); - upperBound = b2Max(upperBound, aabb.upperBound); - } - - /// Combine two AABBs into this one. - void Combine(const b2AABB& aabb1, const b2AABB& aabb2) - { - lowerBound = b2Min(aabb1.lowerBound, aabb2.lowerBound); - upperBound = b2Max(aabb1.upperBound, aabb2.upperBound); - } - - /// Does this aabb contain the provided AABB. - bool Contains(const b2AABB& aabb) const - { - bool result = true; - result = result && lowerBound.x <= aabb.lowerBound.x; - result = result && lowerBound.y <= aabb.lowerBound.y; - result = result && aabb.upperBound.x <= upperBound.x; - result = result && aabb.upperBound.y <= upperBound.y; - return result; - } - - bool RayCast(b2RayCastOutputAABB* output, const b2RayCastInputAABB& input) const; - - b2Vec2 lowerBound; ///< the lower vertex - b2Vec2 upperBound; ///< the upper vertex +struct b2AABB { + /// Verify that the bounds are sorted. + bool IsValid() const; + + /// Get the center of the AABB. + b2Vec2 GetCenter() const { return 0.5f * (lowerBound + upperBound); } + + /// Get the extents of the AABB (half-widths). + b2Vec2 GetExtents() const { return 0.5f * (upperBound - lowerBound); } + + /// Get the perimeter length + float32 GetPerimeter() const { + float32 wx = upperBound.x - lowerBound.x; + float32 wy = upperBound.y - lowerBound.y; + return 2.0f * (wx + wy); + } + + /// Combine an AABB into this one. + void Combine(const b2AABB &aabb) { + lowerBound = b2Min(lowerBound, aabb.lowerBound); + upperBound = b2Max(upperBound, aabb.upperBound); + } + + /// Combine two AABBs into this one. + void Combine(const b2AABB &aabb1, const b2AABB &aabb2) { + lowerBound = b2Min(aabb1.lowerBound, aabb2.lowerBound); + upperBound = b2Max(aabb1.upperBound, aabb2.upperBound); + } + + /// Does this aabb contain the provided AABB. + bool Contains(const b2AABB &aabb) const { + bool result = true; + result = result && lowerBound.x <= aabb.lowerBound.x; + result = result && lowerBound.y <= aabb.lowerBound.y; + result = result && aabb.upperBound.x <= upperBound.x; + result = result && aabb.upperBound.y <= upperBound.y; + return result; + } + + bool RayCast(b2RayCastOutputAABB *output, + const b2RayCastInputAABB &input) const; + + b2Vec2 lowerBound; ///< the lower vertex + b2Vec2 upperBound; ///< the upper vertex }; - // ---------------- Inline Functions ------------------------------------------ -inline bool b2AABB::IsValid() const -{ - b2Vec2 d = upperBound - lowerBound; - bool valid = d.x >= 0.0f && d.y >= 0.0f; - valid = valid && lowerBound.IsValid() && upperBound.IsValid(); - return valid; +inline bool b2AABB::IsValid() const { + b2Vec2 d = upperBound - lowerBound; + bool valid = d.x >= 0.0f && d.y >= 0.0f; + valid = valid && lowerBound.IsValid() && upperBound.IsValid(); + return valid; } -inline bool b2TestOverlap(const b2AABB& a, const b2AABB& b) -{ - b2Vec2 d1, d2; - d1 = b.lowerBound - a.upperBound; - d2 = a.lowerBound - b.upperBound; +inline bool b2TestOverlap(const b2AABB &a, const b2AABB &b) { + b2Vec2 d1, d2; + d1 = b.lowerBound - a.upperBound; + d2 = a.lowerBound - b.upperBound; - if (d1.x > 0.0f || d1.y > 0.0f) - return false; + if (d1.x > 0.0f || d1.y > 0.0f) + return false; - if (d2.x > 0.0f || d2.y > 0.0f) - return false; + if (d2.x > 0.0f || d2.y > 0.0f) + return false; - return true; + return true; } - -#endif +} // namespace daabbcc diff --git a/daabbcc/include/DynamicTree/b2DynamicTree.h b/daabbcc/include/DynamicTree/b2DynamicTree.h index 00a8c9f..6a90a29 100755 --- a/daabbcc/include/DynamicTree/b2DynamicTree.h +++ b/daabbcc/include/DynamicTree/b2DynamicTree.h @@ -16,274 +16,257 @@ * 3. This notice may not be removed or altered from any source distribution. */ -#ifndef B2_DYNAMIC_TREE_H -#define B2_DYNAMIC_TREE_H +#pragma once #include "b2Collision.h" #include "b2GrowableStack.h" +namespace daabbcc { + #define b2_nullNode (-1) /// A node in the dynamic tree. The client does not interact with this directly. -struct b2TreeNode -{ - bool IsLeaf() const - { - return child1 == b2_nullNode; - } +struct b2TreeNode { + bool IsLeaf() const { return child1 == b2_nullNode; } - /// Enlarged AABB - b2AABB aabb; + /// Enlarged AABB + b2AABB aabb; - void *userData; + void *userData; - union - { - int32 parent; - int32 next; - }; + union { + int32 parent; + int32 next; + }; - int32 child1; - int32 child2; + int32 child1; + int32 child2; - // leaf = 0, free node = -1 - int32 height; + // leaf = 0, free node = -1 + int32 height; }; /// A dynamic AABB tree broad-phase, inspired by Nathanael Presson's btDbvt. /// A dynamic tree arranges data in a binary tree to accelerate /// queries such as volume queries and ray casts. Leafs are proxies /// with an AABB. In the tree we expand the proxy AABB by b2_fatAABBFactor -/// so that the proxy AABB is bigger than the client object. This allows the client -/// object to move by small amounts without triggering a tree update. +/// so that the proxy AABB is bigger than the client object. This allows the +/// client object to move by small amounts without triggering a tree update. /// -/// Nodes are pooled and relocatable, so we use node indices rather than pointers. -class b2DynamicTree -{ +/// Nodes are pooled and relocatable, so we use node indices rather than +/// pointers. +class b2DynamicTree { public: - /// Constructing the tree initializes the node pool. - b2DynamicTree(); - - /// Destroy the tree, freeing the node pool. - ~b2DynamicTree(); - - /// Create a proxy. Provide a tight fitting AABB and a userData pointer. - int32 CreateProxy(const b2AABB &aabb, void *userData); - - /// Destroy a proxy. This asserts if the id is invalid. - void DestroyProxy(int32 proxyId); - - /// Move a proxy with a swepted AABB. If the proxy has moved outside of its fattened AABB, - /// then the proxy is removed from the tree and re-inserted. Otherwise - /// the function returns immediately. - /// @return true if the proxy was re-inserted. - bool MoveProxy(int32 proxyId, const b2AABB &aabb1, const b2Vec2 &displacement); - - /// Get proxy user data. - /// @return the proxy user data or 0 if the id is invalid. - void *GetUserData(int32 proxyId) const; - - /// Get the fat AABB for a proxy. - const b2AABB &GetFatAABB(int32 proxyId) const; - - /// Query an AABB for overlapping proxies. The callback class - /// is called for each proxy that overlaps the supplied AABB. - template - void Query(T *callback, const b2AABB &aabb, int groupId) const; - - /// Ray-cast against the proxies in the tree. This relies on the callback - /// to perform a exact ray-cast in the case were the proxy contains a shape. - /// The callback also performs the any collision filtering. This has performance - /// roughly equal to k * log(n), where k is the number of collisions and n is the - /// number of proxies in the tree. - /// @param input the ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1). - /// @param callback a callback class that is called for each proxy that is hit by the ray. - template - void RayCast(T *callback, const b2RayCastInputAABB &input, int groupId) const; - - /// Validate this tree. For testing. - void Validate() const; - - /// Compute the height of the binary tree in O(N) time. Should not be - /// called often. - int32 GetHeight() const; - - /// Get the maximum balance of an node in the tree. The balance is the difference - /// in height of the two children of a node. - int32 GetMaxBalance() const; - - /// Get the ratio of the sum of the node areas to the root area. - float32 GetAreaRatio() const; - - /// Build an optimal tree. Very expensive. For testing. - void RebuildBottomUp(); - - /// Shift the world origin. Useful for large worlds. - /// The shift formula is: position -= newOrigin - /// @param newOrigin the new origin with respect to the old origin - void ShiftOrigin(const b2Vec2 &newOrigin); + /// Constructing the tree initializes the node pool. + b2DynamicTree(); + + /// Destroy the tree, freeing the node pool. + ~b2DynamicTree(); + + /// Create a proxy. Provide a tight fitting AABB and a userData pointer. + int32 CreateProxy(const b2AABB &aabb, void *userData); + + /// Destroy a proxy. This asserts if the id is invalid. + void DestroyProxy(int32 proxyId); + + /// Move a proxy with a swepted AABB. If the proxy has moved outside of its + /// fattened AABB, then the proxy is removed from the tree and re-inserted. + /// Otherwise the function returns immediately. + /// @return true if the proxy was re-inserted. + bool MoveProxy(int32 proxyId, const b2AABB &aabb1, + const b2Vec2 &displacement); + + /// Get proxy user data. + /// @return the proxy user data or 0 if the id is invalid. + void *GetUserData(int32 proxyId) const; + + /// Get the fat AABB for a proxy. + const b2AABB &GetFatAABB(int32 proxyId) const; + + /// Query an AABB for overlapping proxies. The callback class + /// is called for each proxy that overlaps the supplied AABB. + template + void Query(T *callback, const b2AABB &aabb, int groupId) const; + + /// Ray-cast against the proxies in the tree. This relies on the callback + /// to perform a exact ray-cast in the case were the proxy contains a shape. + /// The callback also performs the any collision filtering. This has + /// performance roughly equal to k * log(n), where k is the number of + /// collisions and n is the number of proxies in the tree. + /// @param input the ray-cast input data. The ray extends from p1 to p1 + + /// maxFraction * (p2 - p1). + /// @param callback a callback class that is called for each proxy that is hit + /// by the ray. + template + void RayCast(T *callback, const b2RayCastInputAABB &input, int groupId) const; + + /// Validate this tree. For testing. + void Validate() const; + + /// Compute the height of the binary tree in O(N) time. Should not be + /// called often. + int32 GetHeight() const; + + /// Get the maximum balance of an node in the tree. The balance is the + /// difference in height of the two children of a node. + int32 GetMaxBalance() const; + + /// Get the ratio of the sum of the node areas to the root area. + float32 GetAreaRatio() const; + + /// Build an optimal tree. Very expensive. For testing. + void RebuildBottomUp(); + + /// Shift the world origin. Useful for large worlds. + /// The shift formula is: position -= newOrigin + /// @param newOrigin the new origin with respect to the old origin + void ShiftOrigin(const b2Vec2 &newOrigin); private: - int32 AllocateNode(); - void FreeNode(int32 node); + int32 AllocateNode(); + void FreeNode(int32 node); - int32 PickBest(b2AABB to_insert); - void InsertLeaf(int32 node); - void RemoveLeaf(int32 node); + int32 PickBest(b2AABB to_insert); + void InsertLeaf(int32 node); + void RemoveLeaf(int32 node); - int32 Balance(int32 index); + int32 Balance(int32 index); - int32 ComputeHeight() const; - int32 ComputeHeight(int32 nodeId) const; + int32 ComputeHeight() const; + int32 ComputeHeight(int32 nodeId) const; - void ValidateStructure(int32 index) const; - void ValidateMetrics(int32 index) const; + void ValidateStructure(int32 index) const; + void ValidateMetrics(int32 index) const; - int32 m_root; + int32 m_root; - b2TreeNode *m_nodes; - int32 m_nodeCount; - int32 m_nodeCapacity; + b2TreeNode *m_nodes; + int32 m_nodeCount; + int32 m_nodeCapacity; - int32 m_freeList; + int32 m_freeList; - int32 m_insertionCount; + int32 m_insertionCount; }; -inline void *b2DynamicTree::GetUserData(int32 proxyId) const -{ - b2Assert(0 <= proxyId && proxyId < m_nodeCapacity); - return m_nodes[proxyId].userData; +inline void *b2DynamicTree::GetUserData(int32 proxyId) const { + b2Assert(0 <= proxyId && proxyId < m_nodeCapacity); + return m_nodes[proxyId].userData; } -inline const b2AABB &b2DynamicTree::GetFatAABB(int32 proxyId) const -{ - b2Assert(0 <= proxyId && proxyId < m_nodeCapacity); - return m_nodes[proxyId].aabb; +inline const b2AABB &b2DynamicTree::GetFatAABB(int32 proxyId) const { + b2Assert(0 <= proxyId && proxyId < m_nodeCapacity); + return m_nodes[proxyId].aabb; } template -inline void b2DynamicTree::Query(T *callback, const b2AABB &aabb, int groupId) const -{ - b2GrowableStack stack; - stack.Push(m_root); - - while (stack.GetCount() > 0) - { - int32 nodeId = stack.Pop(); - if (nodeId == b2_nullNode) - { - continue; - } +inline void b2DynamicTree::Query(T *callback, const b2AABB &aabb, + int groupId) const { + b2GrowableStack stack; + stack.Push(m_root); + + while (stack.GetCount() > 0) { + int32 nodeId = stack.Pop(); + if (nodeId == b2_nullNode) { + continue; + } + + const b2TreeNode *node = m_nodes + nodeId; - const b2TreeNode *node = m_nodes + nodeId; - - if (b2TestOverlap(node->aabb, aabb)) - { - if (node->IsLeaf()) - { - bool proceed = callback->QueryCallback(nodeId, groupId); - if (proceed == false) - { - return; - } - } - else - { - stack.Push(node->child1); - stack.Push(node->child2); - } + if (b2TestOverlap(node->aabb, aabb)) { + if (node->IsLeaf()) { + bool proceed = callback->QueryCallback(nodeId, groupId); + if (proceed == false) { + return; } + } else { + stack.Push(node->child1); + stack.Push(node->child2); + } } + } } /****** * GROUP ID ADDED *******/ template -inline void b2DynamicTree::RayCast(T *callback, const b2RayCastInputAABB &input, int groupId) const // Group ID is not the part of the original code +inline void b2DynamicTree::RayCast( + T *callback, const b2RayCastInputAABB &input, + int groupId) const // Group ID is not the part of the original code { - b2Vec2 p1 = input.p1; - b2Vec2 p2 = input.p2; - b2Vec2 r = p2 - p1; - b2Assert(r.LengthSquared() > 0.0f); - r.Normalize(); + b2Vec2 p1 = input.p1; + b2Vec2 p2 = input.p2; + b2Vec2 r = p2 - p1; + b2Assert(r.LengthSquared() > 0.0f); + r.Normalize(); + + // v is perpendicular to the segment. + b2Vec2 v = b2Cross(1.0f, r); + b2Vec2 abs_v = b2Abs(v); + + // Separating axis for segment (Gino, p80). + // |dot(v, p1 - c)| > dot(|v|, h) + + float32 maxFraction = input.maxFraction; + + // Build a bounding box for the segment. + b2AABB segmentAABB; + { + b2Vec2 t = p1 + maxFraction * (p2 - p1); + segmentAABB.lowerBound = b2Min(p1, t); + segmentAABB.upperBound = b2Max(p1, t); + } + + b2GrowableStack stack; + stack.Push(m_root); + + while (stack.GetCount() > 0) { + int32 nodeId = stack.Pop(); + if (nodeId == b2_nullNode) { + continue; + } + + const b2TreeNode *node = m_nodes + nodeId; - // v is perpendicular to the segment. - b2Vec2 v = b2Cross(1.0f, r); - b2Vec2 abs_v = b2Abs(v); + if (b2TestOverlap(node->aabb, segmentAABB) == false) { + continue; + } // Separating axis for segment (Gino, p80). // |dot(v, p1 - c)| > dot(|v|, h) - - float32 maxFraction = input.maxFraction; - - // Build a bounding box for the segment. - b2AABB segmentAABB; - { - b2Vec2 t = p1 + maxFraction * (p2 - p1); - segmentAABB.lowerBound = b2Min(p1, t); - segmentAABB.upperBound = b2Max(p1, t); + b2Vec2 c = node->aabb.GetCenter(); + b2Vec2 h = node->aabb.GetExtents(); + float32 separation = b2Abs(b2Dot(v, p1 - c)) - b2Dot(abs_v, h); + if (separation > 0.0f) { + continue; } - b2GrowableStack stack; - stack.Push(m_root); + if (node->IsLeaf()) { + b2RayCastInputAABB subInput; + subInput.p1 = input.p1; + subInput.p2 = input.p2; + subInput.maxFraction = maxFraction; - while (stack.GetCount() > 0) - { - int32 nodeId = stack.Pop(); - if (nodeId == b2_nullNode) - { - continue; - } - - const b2TreeNode *node = m_nodes + nodeId; + float32 value = callback->RayCastCallback(subInput, nodeId, groupId); - if (b2TestOverlap(node->aabb, segmentAABB) == false) - { - continue; - } - - // Separating axis for segment (Gino, p80). - // |dot(v, p1 - c)| > dot(|v|, h) - b2Vec2 c = node->aabb.GetCenter(); - b2Vec2 h = node->aabb.GetExtents(); - float32 separation = b2Abs(b2Dot(v, p1 - c)) - b2Dot(abs_v, h); - if (separation > 0.0f) - { - continue; - } + if (value == 0.0f) { + // The client has terminated the ray cast. + return; + } - if (node->IsLeaf()) - { - b2RayCastInputAABB subInput; - subInput.p1 = input.p1; - subInput.p2 = input.p2; - subInput.maxFraction = maxFraction; - - float32 value = callback->RayCastCallback(subInput, nodeId, groupId); - - if (value == 0.0f) - { - // The client has terminated the ray cast. - return; - } - - if (value > 0.0f) - { - // Update segment bounding box. - maxFraction = value; - b2Vec2 t = p1 + maxFraction * (p2 - p1); - segmentAABB.lowerBound = b2Min(p1, t); - segmentAABB.upperBound = b2Max(p1, t); - } - } - else - { - stack.Push(node->child1); - stack.Push(node->child2); - } + if (value > 0.0f) { + // Update segment bounding box. + maxFraction = value; + b2Vec2 t = p1 + maxFraction * (p2 - p1); + segmentAABB.lowerBound = b2Min(p1, t); + segmentAABB.upperBound = b2Max(p1, t); + } + } else { + stack.Push(node->child1); + stack.Push(node->child2); } + } } -#endif +} // namespace daabbcc diff --git a/daabbcc/include/DynamicTree/b2GrowableStack.h b/daabbcc/include/DynamicTree/b2GrowableStack.h index 827c8dd..0daa11b 100755 --- a/daabbcc/include/DynamicTree/b2GrowableStack.h +++ b/daabbcc/include/DynamicTree/b2GrowableStack.h @@ -1,85 +1,73 @@ /* -* Copyright (c) 2010 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this 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. -*/ + * Copyright (c) 2010 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this 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 B2_GROWABLE_STACK_H -#define B2_GROWABLE_STACK_H +#pragma once #include "b2Settings.h" #include +namespace daabbcc { + /// This is a growable LIFO stack with an initial capacity of N. /// If the stack size exceeds the initial capacity, the heap is used /// to increase the size of the stack. -template -class b2GrowableStack -{ +template class b2GrowableStack { public: - b2GrowableStack() - { - m_stack = m_array; - m_count = 0; - m_capacity = N; - } + b2GrowableStack() { + m_stack = m_array; + m_count = 0; + m_capacity = N; + } - ~b2GrowableStack() - { - if (m_stack != m_array) - { - b2Free(m_stack); - m_stack = nullptr; - } - } + ~b2GrowableStack() { + if (m_stack != m_array) { + b2Free(m_stack); + m_stack = nullptr; + } + } - void Push(const T& element) - { - if (m_count == m_capacity) - { - T* old = m_stack; - m_capacity *= 2; - m_stack = (T*)b2Alloc(m_capacity * sizeof(T)); - memcpy(m_stack, old, m_count * sizeof(T)); - if (old != m_array) - { - b2Free(old); - } - } + void Push(const T &element) { + if (m_count == m_capacity) { + T *old = m_stack; + m_capacity *= 2; + m_stack = (T *)b2Alloc(m_capacity * sizeof(T)); + memcpy(m_stack, old, m_count * sizeof(T)); + if (old != m_array) { + b2Free(old); + } + } - m_stack[m_count] = element; - ++m_count; - } + m_stack[m_count] = element; + ++m_count; + } - T Pop() - { - b2Assert(m_count > 0); - --m_count; - return m_stack[m_count]; - } + T Pop() { + b2Assert(m_count > 0); + --m_count; + return m_stack[m_count]; + } - int32 GetCount() - { - return m_count; - } + int32 GetCount() { return m_count; } private: - T* m_stack; - T m_array[N]; - int32 m_count; - int32 m_capacity; + T *m_stack; + T m_array[N]; + int32 m_count; + int32 m_capacity; }; - -#endif +} // namespace daabbcc diff --git a/daabbcc/include/DynamicTree/b2Math.h b/daabbcc/include/DynamicTree/b2Math.h index 236c1a4..d8d7365 100755 --- a/daabbcc/include/DynamicTree/b2Math.h +++ b/daabbcc/include/DynamicTree/b2Math.h @@ -1,707 +1,632 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this 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 B2_MATH_H -#define B2_MATH_H + * Copyright (c) 2006-2009 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this 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. + */ + +#pragma once #include "b2Settings.h" #include -/// This function is used to ensure that a floating point number is not a NaN or infinity. -inline bool b2IsValid(float32 x) -{ - return isfinite(x); -} +namespace daabbcc { + +/// This function is used to ensure that a floating point number is not a NaN or +/// infinity. +inline bool b2IsValid(float32 x) { return isfinite(x); } -#define b2Sqrt(x) sqrtf(x) -#define b2Atan2(y, x) atan2f(y, x) +#define b2Sqrt(x) sqrtf(x) +#define b2Atan2(y, x) atan2f(y, x) /// A 2D column vector. -struct b2Vec2 -{ - /// Default constructor does nothing (for performance). - b2Vec2() {} - - /// Construct using coordinates. - b2Vec2(float32 xIn, float32 yIn) : x(xIn), y(yIn) {} - - /// Set this vector to all zeros. - void SetZero() { x = 0.0f; y = 0.0f; } - - /// Set this vector to some specified coordinates. - void Set(float32 x_, float32 y_) { x = x_; y = y_; } - - /// Negate this vector. - b2Vec2 operator -() const { b2Vec2 v; v.Set(-x, -y); return v; } - - /// Read from and indexed element. - float32 operator () (int32 i) const - { - return (&x)[i]; - } - - /// Write to an indexed element. - float32& operator () (int32 i) - { - return (&x)[i]; - } - - /// Add a vector to this vector. - void operator += (const b2Vec2& v) - { - x += v.x; y += v.y; - } - - /// Subtract a vector from this vector. - void operator -= (const b2Vec2& v) - { - x -= v.x; y -= v.y; - } - - /// Multiply this vector by a scalar. - void operator *= (float32 a) - { - x *= a; y *= a; - } - - /// Get the length of this vector (the norm). - float32 Length() const - { - return b2Sqrt(x * x + y * y); - } - - /// Get the length squared. For performance, use this instead of - /// b2Vec2::Length (if possible). - float32 LengthSquared() const - { - return x * x + y * y; - } - - /// Convert this vector into a unit vector. Returns the length. - float32 Normalize() - { - float32 length = Length(); - if (length < b2_epsilon) - { - return 0.0f; - } - float32 invLength = 1.0f / length; - x *= invLength; - y *= invLength; - - return length; - } - - /// Does this vector contain finite coordinates? - bool IsValid() const - { - return b2IsValid(x) && b2IsValid(y); - } - - /// Get the skew vector such that dot(skew_vec, other) == cross(vec, other) - b2Vec2 Skew() const - { - return b2Vec2(-y, x); - } - - float32 x, y; +struct b2Vec2 { + /// Default constructor does nothing (for performance). + b2Vec2() {} + + /// Construct using coordinates. + b2Vec2(float32 xIn, float32 yIn) : x(xIn), y(yIn) {} + + /// Set this vector to all zeros. + void SetZero() { + x = 0.0f; + y = 0.0f; + } + + /// Set this vector to some specified coordinates. + void Set(float32 x_, float32 y_) { + x = x_; + y = y_; + } + + /// Negate this vector. + b2Vec2 operator-() const { + b2Vec2 v; + v.Set(-x, -y); + return v; + } + + /// Read from and indexed element. + float32 operator()(int32 i) const { return (&x)[i]; } + + /// Write to an indexed element. + float32 &operator()(int32 i) { return (&x)[i]; } + + /// Add a vector to this vector. + void operator+=(const b2Vec2 &v) { + x += v.x; + y += v.y; + } + + /// Subtract a vector from this vector. + void operator-=(const b2Vec2 &v) { + x -= v.x; + y -= v.y; + } + + /// Multiply this vector by a scalar. + void operator*=(float32 a) { + x *= a; + y *= a; + } + + /// Get the length of this vector (the norm). + float32 Length() const { return b2Sqrt(x * x + y * y); } + + /// Get the length squared. For performance, use this instead of + /// b2Vec2::Length (if possible). + float32 LengthSquared() const { return x * x + y * y; } + + /// Convert this vector into a unit vector. Returns the length. + float32 Normalize() { + float32 length = Length(); + if (length < b2_epsilon) { + return 0.0f; + } + float32 invLength = 1.0f / length; + x *= invLength; + y *= invLength; + + return length; + } + + /// Does this vector contain finite coordinates? + bool IsValid() const { return b2IsValid(x) && b2IsValid(y); } + + /// Get the skew vector such that dot(skew_vec, other) == cross(vec, other) + b2Vec2 Skew() const { return b2Vec2(-y, x); } + + float32 x, y; }; /// A 2D column vector with 3 elements. -struct b2Vec3 -{ - /// Default constructor does nothing (for performance). - b2Vec3() {} - - /// Construct using coordinates. - b2Vec3(float32 xIn, float32 yIn, float32 zIn) : x(xIn), y(yIn), z(zIn) {} - - /// Set this vector to all zeros. - void SetZero() { x = 0.0f; y = 0.0f; z = 0.0f; } - - /// Set this vector to some specified coordinates. - void Set(float32 x_, float32 y_, float32 z_) { x = x_; y = y_; z = z_; } - - /// Negate this vector. - b2Vec3 operator -() const { b2Vec3 v; v.Set(-x, -y, -z); return v; } - - /// Add a vector to this vector. - void operator += (const b2Vec3& v) - { - x += v.x; y += v.y; z += v.z; - } - - /// Subtract a vector from this vector. - void operator -= (const b2Vec3& v) - { - x -= v.x; y -= v.y; z -= v.z; - } - - /// Multiply this vector by a scalar. - void operator *= (float32 s) - { - x *= s; y *= s; z *= s; - } - - float32 x, y, z; +struct b2Vec3 { + /// Default constructor does nothing (for performance). + b2Vec3() {} + + /// Construct using coordinates. + b2Vec3(float32 xIn, float32 yIn, float32 zIn) : x(xIn), y(yIn), z(zIn) {} + + /// Set this vector to all zeros. + void SetZero() { + x = 0.0f; + y = 0.0f; + z = 0.0f; + } + + /// Set this vector to some specified coordinates. + void Set(float32 x_, float32 y_, float32 z_) { + x = x_; + y = y_; + z = z_; + } + + /// Negate this vector. + b2Vec3 operator-() const { + b2Vec3 v; + v.Set(-x, -y, -z); + return v; + } + + /// Add a vector to this vector. + void operator+=(const b2Vec3 &v) { + x += v.x; + y += v.y; + z += v.z; + } + + /// Subtract a vector from this vector. + void operator-=(const b2Vec3 &v) { + x -= v.x; + y -= v.y; + z -= v.z; + } + + /// Multiply this vector by a scalar. + void operator*=(float32 s) { + x *= s; + y *= s; + z *= s; + } + + float32 x, y, z; }; /// A 2-by-2 matrix. Stored in column-major order. -struct b2Mat22 -{ - /// The default constructor does nothing (for performance). - b2Mat22() {} - - /// Construct this matrix using columns. - b2Mat22(const b2Vec2& c1, const b2Vec2& c2) - { - ex = c1; - ey = c2; - } - - /// Construct this matrix using scalars. - b2Mat22(float32 a11, float32 a12, float32 a21, float32 a22) - { - ex.x = a11; ex.y = a21; - ey.x = a12; ey.y = a22; - } - - /// Initialize this matrix using columns. - void Set(const b2Vec2& c1, const b2Vec2& c2) - { - ex = c1; - ey = c2; - } - - /// Set this to the identity matrix. - void SetIdentity() - { - ex.x = 1.0f; ey.x = 0.0f; - ex.y = 0.0f; ey.y = 1.0f; - } - - /// Set this matrix to all zeros. - void SetZero() - { - ex.x = 0.0f; ey.x = 0.0f; - ex.y = 0.0f; ey.y = 0.0f; - } - - b2Mat22 GetInverse() const - { - float32 a = ex.x, b = ey.x, c = ex.y, d = ey.y; - b2Mat22 B; - float32 det = a * d - b * c; - if (det != 0.0f) - { - det = 1.0f / det; - } - B.ex.x = det * d; B.ey.x = -det * b; - B.ex.y = -det * c; B.ey.y = det * a; - return B; - } - - /// Solve A * x = b, where b is a column vector. This is more efficient - /// than computing the inverse in one-shot cases. - b2Vec2 Solve(const b2Vec2& b) const - { - float32 a11 = ex.x, a12 = ey.x, a21 = ex.y, a22 = ey.y; - float32 det = a11 * a22 - a12 * a21; - if (det != 0.0f) - { - det = 1.0f / det; - } - b2Vec2 x; - x.x = det * (a22 * b.x - a12 * b.y); - x.y = det * (a11 * b.y - a21 * b.x); - return x; - } - - b2Vec2 ex, ey; +struct b2Mat22 { + /// The default constructor does nothing (for performance). + b2Mat22() {} + + /// Construct this matrix using columns. + b2Mat22(const b2Vec2 &c1, const b2Vec2 &c2) { + ex = c1; + ey = c2; + } + + /// Construct this matrix using scalars. + b2Mat22(float32 a11, float32 a12, float32 a21, float32 a22) { + ex.x = a11; + ex.y = a21; + ey.x = a12; + ey.y = a22; + } + + /// Initialize this matrix using columns. + void Set(const b2Vec2 &c1, const b2Vec2 &c2) { + ex = c1; + ey = c2; + } + + /// Set this to the identity matrix. + void SetIdentity() { + ex.x = 1.0f; + ey.x = 0.0f; + ex.y = 0.0f; + ey.y = 1.0f; + } + + /// Set this matrix to all zeros. + void SetZero() { + ex.x = 0.0f; + ey.x = 0.0f; + ex.y = 0.0f; + ey.y = 0.0f; + } + + b2Mat22 GetInverse() const { + float32 a = ex.x, b = ey.x, c = ex.y, d = ey.y; + b2Mat22 B; + float32 det = a * d - b * c; + if (det != 0.0f) { + det = 1.0f / det; + } + B.ex.x = det * d; + B.ey.x = -det * b; + B.ex.y = -det * c; + B.ey.y = det * a; + return B; + } + + /// Solve A * x = b, where b is a column vector. This is more efficient + /// than computing the inverse in one-shot cases. + b2Vec2 Solve(const b2Vec2 &b) const { + float32 a11 = ex.x, a12 = ey.x, a21 = ex.y, a22 = ey.y; + float32 det = a11 * a22 - a12 * a21; + if (det != 0.0f) { + det = 1.0f / det; + } + b2Vec2 x; + x.x = det * (a22 * b.x - a12 * b.y); + x.y = det * (a11 * b.y - a21 * b.x); + return x; + } + + b2Vec2 ex, ey; }; /// A 3-by-3 matrix. Stored in column-major order. -struct b2Mat33 -{ - /// The default constructor does nothing (for performance). - b2Mat33() {} - - /// Construct this matrix using columns. - b2Mat33(const b2Vec3& c1, const b2Vec3& c2, const b2Vec3& c3) - { - ex = c1; - ey = c2; - ez = c3; - } - - /// Set this matrix to all zeros. - void SetZero() - { - ex.SetZero(); - ey.SetZero(); - ez.SetZero(); - } - - /// Solve A * x = b, where b is a column vector. This is more efficient - /// than computing the inverse in one-shot cases. - b2Vec3 Solve33(const b2Vec3& b) const; - - /// Solve A * x = b, where b is a column vector. This is more efficient - /// than computing the inverse in one-shot cases. Solve only the upper - /// 2-by-2 matrix equation. - b2Vec2 Solve22(const b2Vec2& b) const; - - /// Get the inverse of this matrix as a 2-by-2. - /// Returns the zero matrix if singular. - void GetInverse22(b2Mat33* M) const; - - /// Get the symmetric inverse of this matrix as a 3-by-3. - /// Returns the zero matrix if singular. - void GetSymInverse33(b2Mat33* M) const; - - b2Vec3 ex, ey, ez; +struct b2Mat33 { + /// The default constructor does nothing (for performance). + b2Mat33() {} + + /// Construct this matrix using columns. + b2Mat33(const b2Vec3 &c1, const b2Vec3 &c2, const b2Vec3 &c3) { + ex = c1; + ey = c2; + ez = c3; + } + + /// Set this matrix to all zeros. + void SetZero() { + ex.SetZero(); + ey.SetZero(); + ez.SetZero(); + } + + /// Solve A * x = b, where b is a column vector. This is more efficient + /// than computing the inverse in one-shot cases. + b2Vec3 Solve33(const b2Vec3 &b) const; + + /// Solve A * x = b, where b is a column vector. This is more efficient + /// than computing the inverse in one-shot cases. Solve only the upper + /// 2-by-2 matrix equation. + b2Vec2 Solve22(const b2Vec2 &b) const; + + /// Get the inverse of this matrix as a 2-by-2. + /// Returns the zero matrix if singular. + void GetInverse22(b2Mat33 *M) const; + + /// Get the symmetric inverse of this matrix as a 3-by-3. + /// Returns the zero matrix if singular. + void GetSymInverse33(b2Mat33 *M) const; + + b2Vec3 ex, ey, ez; }; /// Rotation -struct b2Rot -{ - b2Rot() {} - - /// Initialize from an angle in radians - explicit b2Rot(float32 angle) - { - /// TODO_ERIN optimize - s = sinf(angle); - c = cosf(angle); - } - - /// Set using an angle in radians. - void Set(float32 angle) - { - /// TODO_ERIN optimize - s = sinf(angle); - c = cosf(angle); - } - - /// Set to the identity rotation - void SetIdentity() - { - s = 0.0f; - c = 1.0f; - } - - /// Get the angle in radians - float32 GetAngle() const - { - return b2Atan2(s, c); - } - - /// Get the x-axis - b2Vec2 GetXAxis() const - { - return b2Vec2(c, s); - } - - /// Get the u-axis - b2Vec2 GetYAxis() const - { - return b2Vec2(-s, c); - } - - /// Sine and cosine - float32 s, c; +struct b2Rot { + b2Rot() {} + + /// Initialize from an angle in radians + explicit b2Rot(float32 angle) { + /// TODO_ERIN optimize + s = sinf(angle); + c = cosf(angle); + } + + /// Set using an angle in radians. + void Set(float32 angle) { + /// TODO_ERIN optimize + s = sinf(angle); + c = cosf(angle); + } + + /// Set to the identity rotation + void SetIdentity() { + s = 0.0f; + c = 1.0f; + } + + /// Get the angle in radians + float32 GetAngle() const { return b2Atan2(s, c); } + + /// Get the x-axis + b2Vec2 GetXAxis() const { return b2Vec2(c, s); } + + /// Get the u-axis + b2Vec2 GetYAxis() const { return b2Vec2(-s, c); } + + /// Sine and cosine + float32 s, c; }; /// A transform contains translation and rotation. It is used to represent /// the position and orientation of rigid frames. -struct b2Transform -{ - /// The default constructor does nothing. - b2Transform() {} - - /// Initialize using a position vector and a rotation. - b2Transform(const b2Vec2& position, const b2Rot& rotation) : p(position), q(rotation) {} - - /// Set this to the identity transform. - void SetIdentity() - { - p.SetZero(); - q.SetIdentity(); - } - - /// Set this based on the position and angle. - void Set(const b2Vec2& position, float32 angle) - { - p = position; - q.Set(angle); - } - - b2Vec2 p; - b2Rot q; +struct b2Transform { + /// The default constructor does nothing. + b2Transform() {} + + /// Initialize using a position vector and a rotation. + b2Transform(const b2Vec2 &position, const b2Rot &rotation) + : p(position), q(rotation) {} + + /// Set this to the identity transform. + void SetIdentity() { + p.SetZero(); + q.SetIdentity(); + } + + /// Set this based on the position and angle. + void Set(const b2Vec2 &position, float32 angle) { + p = position; + q.Set(angle); + } + + b2Vec2 p; + b2Rot q; }; /// This describes the motion of a body/shape for TOI computation. /// Shapes are defined with respect to the body origin, which may /// no coincide with the center of mass. However, to support dynamics /// we must interpolate the center of mass position. -struct b2Sweep -{ - /// Get the interpolated transform at a specific time. - /// @param beta is a factor in [0,1], where 0 indicates alpha0. - void GetTransform(b2Transform* xfb, float32 beta) const; - - /// Advance the sweep forward, yielding a new initial state. - /// @param alpha the new initial time. - void Advance(float32 alpha); - - /// Normalize the angles. - void Normalize(); - - b2Vec2 localCenter; ///< local center of mass position - b2Vec2 c0, c; ///< center world positions - float32 a0, a; ///< world angles - - /// Fraction of the current time step in the range [0,1] - /// c0 and a0 are the positions at alpha0. - float32 alpha0; +struct b2Sweep { + /// Get the interpolated transform at a specific time. + /// @param beta is a factor in [0,1], where 0 indicates alpha0. + void GetTransform(b2Transform *xfb, float32 beta) const; + + /// Advance the sweep forward, yielding a new initial state. + /// @param alpha the new initial time. + void Advance(float32 alpha); + + /// Normalize the angles. + void Normalize(); + + b2Vec2 localCenter; ///< local center of mass position + b2Vec2 c0, c; ///< center world positions + float32 a0, a; ///< world angles + + /// Fraction of the current time step in the range [0,1] + /// c0 and a0 are the positions at alpha0. + float32 alpha0; }; /// Useful constant extern const b2Vec2 b2Vec2_zero; /// Perform the dot product on two vectors. -inline float32 b2Dot(const b2Vec2& a, const b2Vec2& b) -{ - return a.x * b.x + a.y * b.y; +inline float32 b2Dot(const b2Vec2 &a, const b2Vec2 &b) { + return a.x * b.x + a.y * b.y; } /// Perform the cross product on two vectors. In 2D this produces a scalar. -inline float32 b2Cross(const b2Vec2& a, const b2Vec2& b) -{ - return a.x * b.y - a.y * b.x; +inline float32 b2Cross(const b2Vec2 &a, const b2Vec2 &b) { + return a.x * b.y - a.y * b.x; } /// Perform the cross product on a vector and a scalar. In 2D this produces /// a vector. -inline b2Vec2 b2Cross(const b2Vec2& a, float32 s) -{ - return b2Vec2(s * a.y, -s * a.x); +inline b2Vec2 b2Cross(const b2Vec2 &a, float32 s) { + return b2Vec2(s * a.y, -s * a.x); } /// Perform the cross product on a scalar and a vector. In 2D this produces /// a vector. -inline b2Vec2 b2Cross(float32 s, const b2Vec2& a) -{ - return b2Vec2(-s * a.y, s * a.x); +inline b2Vec2 b2Cross(float32 s, const b2Vec2 &a) { + return b2Vec2(-s * a.y, s * a.x); } /// Multiply a matrix times a vector. If a rotation matrix is provided, /// then this transforms the vector from one frame to another. -inline b2Vec2 b2Mul(const b2Mat22& A, const b2Vec2& v) -{ - return b2Vec2(A.ex.x * v.x + A.ey.x * v.y, A.ex.y * v.x + A.ey.y * v.y); +inline b2Vec2 b2Mul(const b2Mat22 &A, const b2Vec2 &v) { + return b2Vec2(A.ex.x * v.x + A.ey.x * v.y, A.ex.y * v.x + A.ey.y * v.y); } -/// Multiply a matrix transpose times a vector. If a rotation matrix is provided, -/// then this transforms the vector from one frame to another (inverse transform). -inline b2Vec2 b2MulT(const b2Mat22& A, const b2Vec2& v) -{ - return b2Vec2(b2Dot(v, A.ex), b2Dot(v, A.ey)); +/// Multiply a matrix transpose times a vector. If a rotation matrix is +/// provided, then this transforms the vector from one frame to another (inverse +/// transform). +inline b2Vec2 b2MulT(const b2Mat22 &A, const b2Vec2 &v) { + return b2Vec2(b2Dot(v, A.ex), b2Dot(v, A.ey)); } /// Add two vectors component-wise. -inline b2Vec2 operator + (const b2Vec2& a, const b2Vec2& b) -{ - return b2Vec2(a.x + b.x, a.y + b.y); +inline b2Vec2 operator+(const b2Vec2 &a, const b2Vec2 &b) { + return b2Vec2(a.x + b.x, a.y + b.y); } /// Subtract two vectors component-wise. -inline b2Vec2 operator - (const b2Vec2& a, const b2Vec2& b) -{ - return b2Vec2(a.x - b.x, a.y - b.y); +inline b2Vec2 operator-(const b2Vec2 &a, const b2Vec2 &b) { + return b2Vec2(a.x - b.x, a.y - b.y); } -inline b2Vec2 operator * (float32 s, const b2Vec2& a) -{ - return b2Vec2(s * a.x, s * a.y); +inline b2Vec2 operator*(float32 s, const b2Vec2 &a) { + return b2Vec2(s * a.x, s * a.y); } -inline bool operator == (const b2Vec2& a, const b2Vec2& b) -{ - return a.x == b.x && a.y == b.y; +inline bool operator==(const b2Vec2 &a, const b2Vec2 &b) { + return a.x == b.x && a.y == b.y; } -inline bool operator != (const b2Vec2& a, const b2Vec2& b) -{ - return a.x != b.x || a.y != b.y; +inline bool operator!=(const b2Vec2 &a, const b2Vec2 &b) { + return a.x != b.x || a.y != b.y; } -inline float32 b2Distance(const b2Vec2& a, const b2Vec2& b) -{ - b2Vec2 c = a - b; - return c.Length(); +inline float32 b2Distance(const b2Vec2 &a, const b2Vec2 &b) { + b2Vec2 c = a - b; + return c.Length(); } -inline float32 b2DistanceSquared(const b2Vec2& a, const b2Vec2& b) -{ - b2Vec2 c = a - b; - return b2Dot(c, c); +inline float32 b2DistanceSquared(const b2Vec2 &a, const b2Vec2 &b) { + b2Vec2 c = a - b; + return b2Dot(c, c); } -inline b2Vec3 operator * (float32 s, const b2Vec3& a) -{ - return b2Vec3(s * a.x, s * a.y, s * a.z); +inline b2Vec3 operator*(float32 s, const b2Vec3 &a) { + return b2Vec3(s * a.x, s * a.y, s * a.z); } /// Add two vectors component-wise. -inline b2Vec3 operator + (const b2Vec3& a, const b2Vec3& b) -{ - return b2Vec3(a.x + b.x, a.y + b.y, a.z + b.z); +inline b2Vec3 operator+(const b2Vec3 &a, const b2Vec3 &b) { + return b2Vec3(a.x + b.x, a.y + b.y, a.z + b.z); } /// Subtract two vectors component-wise. -inline b2Vec3 operator - (const b2Vec3& a, const b2Vec3& b) -{ - return b2Vec3(a.x - b.x, a.y - b.y, a.z - b.z); +inline b2Vec3 operator-(const b2Vec3 &a, const b2Vec3 &b) { + return b2Vec3(a.x - b.x, a.y - b.y, a.z - b.z); } /// Perform the dot product on two vectors. -inline float32 b2Dot(const b2Vec3& a, const b2Vec3& b) -{ - return a.x * b.x + a.y * b.y + a.z * b.z; +inline float32 b2Dot(const b2Vec3 &a, const b2Vec3 &b) { + return a.x * b.x + a.y * b.y + a.z * b.z; } /// Perform the cross product on two vectors. -inline b2Vec3 b2Cross(const b2Vec3& a, const b2Vec3& b) -{ - return b2Vec3(a.y * b.z - a.z * b.y, a.z * b.x - a.x * b.z, a.x * b.y - a.y * b.x); +inline b2Vec3 b2Cross(const b2Vec3 &a, const b2Vec3 &b) { + return b2Vec3(a.y * b.z - a.z * b.y, a.z * b.x - a.x * b.z, + a.x * b.y - a.y * b.x); } -inline b2Mat22 operator + (const b2Mat22& A, const b2Mat22& B) -{ - return b2Mat22(A.ex + B.ex, A.ey + B.ey); +inline b2Mat22 operator+(const b2Mat22 &A, const b2Mat22 &B) { + return b2Mat22(A.ex + B.ex, A.ey + B.ey); } // A * B -inline b2Mat22 b2Mul(const b2Mat22& A, const b2Mat22& B) -{ - return b2Mat22(b2Mul(A, B.ex), b2Mul(A, B.ey)); +inline b2Mat22 b2Mul(const b2Mat22 &A, const b2Mat22 &B) { + return b2Mat22(b2Mul(A, B.ex), b2Mul(A, B.ey)); } // A^T * B -inline b2Mat22 b2MulT(const b2Mat22& A, const b2Mat22& B) -{ - b2Vec2 c1(b2Dot(A.ex, B.ex), b2Dot(A.ey, B.ex)); - b2Vec2 c2(b2Dot(A.ex, B.ey), b2Dot(A.ey, B.ey)); - return b2Mat22(c1, c2); +inline b2Mat22 b2MulT(const b2Mat22 &A, const b2Mat22 &B) { + b2Vec2 c1(b2Dot(A.ex, B.ex), b2Dot(A.ey, B.ex)); + b2Vec2 c2(b2Dot(A.ex, B.ey), b2Dot(A.ey, B.ey)); + return b2Mat22(c1, c2); } /// Multiply a matrix times a vector. -inline b2Vec3 b2Mul(const b2Mat33& A, const b2Vec3& v) -{ - return v.x * A.ex + v.y * A.ey + v.z * A.ez; +inline b2Vec3 b2Mul(const b2Mat33 &A, const b2Vec3 &v) { + return v.x * A.ex + v.y * A.ey + v.z * A.ez; } /// Multiply a matrix times a vector. -inline b2Vec2 b2Mul22(const b2Mat33& A, const b2Vec2& v) -{ - return b2Vec2(A.ex.x * v.x + A.ey.x * v.y, A.ex.y * v.x + A.ey.y * v.y); +inline b2Vec2 b2Mul22(const b2Mat33 &A, const b2Vec2 &v) { + return b2Vec2(A.ex.x * v.x + A.ey.x * v.y, A.ex.y * v.x + A.ey.y * v.y); } /// Multiply two rotations: q * r -inline b2Rot b2Mul(const b2Rot& q, const b2Rot& r) -{ - // [qc -qs] * [rc -rs] = [qc*rc-qs*rs -qc*rs-qs*rc] - // [qs qc] [rs rc] [qs*rc+qc*rs -qs*rs+qc*rc] - // s = qs * rc + qc * rs - // c = qc * rc - qs * rs - b2Rot qr; - qr.s = q.s * r.c + q.c * r.s; - qr.c = q.c * r.c - q.s * r.s; - return qr; +inline b2Rot b2Mul(const b2Rot &q, const b2Rot &r) { + // [qc -qs] * [rc -rs] = [qc*rc-qs*rs -qc*rs-qs*rc] + // [qs qc] [rs rc] [qs*rc+qc*rs -qs*rs+qc*rc] + // s = qs * rc + qc * rs + // c = qc * rc - qs * rs + b2Rot qr; + qr.s = q.s * r.c + q.c * r.s; + qr.c = q.c * r.c - q.s * r.s; + return qr; } /// Transpose multiply two rotations: qT * r -inline b2Rot b2MulT(const b2Rot& q, const b2Rot& r) -{ - // [ qc qs] * [rc -rs] = [qc*rc+qs*rs -qc*rs+qs*rc] - // [-qs qc] [rs rc] [-qs*rc+qc*rs qs*rs+qc*rc] - // s = qc * rs - qs * rc - // c = qc * rc + qs * rs - b2Rot qr; - qr.s = q.c * r.s - q.s * r.c; - qr.c = q.c * r.c + q.s * r.s; - return qr; +inline b2Rot b2MulT(const b2Rot &q, const b2Rot &r) { + // [ qc qs] * [rc -rs] = [qc*rc+qs*rs -qc*rs+qs*rc] + // [-qs qc] [rs rc] [-qs*rc+qc*rs qs*rs+qc*rc] + // s = qc * rs - qs * rc + // c = qc * rc + qs * rs + b2Rot qr; + qr.s = q.c * r.s - q.s * r.c; + qr.c = q.c * r.c + q.s * r.s; + return qr; } /// Rotate a vector -inline b2Vec2 b2Mul(const b2Rot& q, const b2Vec2& v) -{ - return b2Vec2(q.c * v.x - q.s * v.y, q.s * v.x + q.c * v.y); +inline b2Vec2 b2Mul(const b2Rot &q, const b2Vec2 &v) { + return b2Vec2(q.c * v.x - q.s * v.y, q.s * v.x + q.c * v.y); } /// Inverse rotate a vector -inline b2Vec2 b2MulT(const b2Rot& q, const b2Vec2& v) -{ - return b2Vec2(q.c * v.x + q.s * v.y, -q.s * v.x + q.c * v.y); +inline b2Vec2 b2MulT(const b2Rot &q, const b2Vec2 &v) { + return b2Vec2(q.c * v.x + q.s * v.y, -q.s * v.x + q.c * v.y); } -inline b2Vec2 b2Mul(const b2Transform& T, const b2Vec2& v) -{ - float32 x = (T.q.c * v.x - T.q.s * v.y) + T.p.x; - float32 y = (T.q.s * v.x + T.q.c * v.y) + T.p.y; +inline b2Vec2 b2Mul(const b2Transform &T, const b2Vec2 &v) { + float32 x = (T.q.c * v.x - T.q.s * v.y) + T.p.x; + float32 y = (T.q.s * v.x + T.q.c * v.y) + T.p.y; - return b2Vec2(x, y); + return b2Vec2(x, y); } -inline b2Vec2 b2MulT(const b2Transform& T, const b2Vec2& v) -{ - float32 px = v.x - T.p.x; - float32 py = v.y - T.p.y; - float32 x = (T.q.c * px + T.q.s * py); - float32 y = (-T.q.s * px + T.q.c * py); +inline b2Vec2 b2MulT(const b2Transform &T, const b2Vec2 &v) { + float32 px = v.x - T.p.x; + float32 py = v.y - T.p.y; + float32 x = (T.q.c * px + T.q.s * py); + float32 y = (-T.q.s * px + T.q.c * py); - return b2Vec2(x, y); + return b2Vec2(x, y); } // v2 = A.q.Rot(B.q.Rot(v1) + B.p) + A.p // = (A.q * B.q).Rot(v1) + A.q.Rot(B.p) + A.p -inline b2Transform b2Mul(const b2Transform& A, const b2Transform& B) -{ - b2Transform C; - C.q = b2Mul(A.q, B.q); - C.p = b2Mul(A.q, B.p) + A.p; - return C; +inline b2Transform b2Mul(const b2Transform &A, const b2Transform &B) { + b2Transform C; + C.q = b2Mul(A.q, B.q); + C.p = b2Mul(A.q, B.p) + A.p; + return C; } // v2 = A.q' * (B.q * v1 + B.p - A.p) // = A.q' * B.q * v1 + A.q' * (B.p - A.p) -inline b2Transform b2MulT(const b2Transform& A, const b2Transform& B) -{ - b2Transform C; - C.q = b2MulT(A.q, B.q); - C.p = b2MulT(A.q, B.p - A.p); - return C; +inline b2Transform b2MulT(const b2Transform &A, const b2Transform &B) { + b2Transform C; + C.q = b2MulT(A.q, B.q); + C.p = b2MulT(A.q, B.p - A.p); + return C; } -template -inline T b2Abs(T a) -{ - return a > T(0) ? a : -a; -} +template inline T b2Abs(T a) { return a > T(0) ? a : -a; } -inline b2Vec2 b2Abs(const b2Vec2& a) -{ - return b2Vec2(b2Abs(a.x), b2Abs(a.y)); -} +inline b2Vec2 b2Abs(const b2Vec2 &a) { return b2Vec2(b2Abs(a.x), b2Abs(a.y)); } -inline b2Mat22 b2Abs(const b2Mat22& A) -{ - return b2Mat22(b2Abs(A.ex), b2Abs(A.ey)); +inline b2Mat22 b2Abs(const b2Mat22 &A) { + return b2Mat22(b2Abs(A.ex), b2Abs(A.ey)); } -template -inline T b2Min(T a, T b) -{ - return a < b ? a : b; -} +template inline T b2Min(T a, T b) { return a < b ? a : b; } -inline b2Vec2 b2Min(const b2Vec2& a, const b2Vec2& b) -{ - return b2Vec2(b2Min(a.x, b.x), b2Min(a.y, b.y)); +inline b2Vec2 b2Min(const b2Vec2 &a, const b2Vec2 &b) { + return b2Vec2(b2Min(a.x, b.x), b2Min(a.y, b.y)); } -template -inline T b2Max(T a, T b) -{ - return a > b ? a : b; -} +template inline T b2Max(T a, T b) { return a > b ? a : b; } -inline b2Vec2 b2Max(const b2Vec2& a, const b2Vec2& b) -{ - return b2Vec2(b2Max(a.x, b.x), b2Max(a.y, b.y)); +inline b2Vec2 b2Max(const b2Vec2 &a, const b2Vec2 &b) { + return b2Vec2(b2Max(a.x, b.x), b2Max(a.y, b.y)); } -template -inline T b2Clamp(T a, T low, T high) -{ - return b2Max(low, b2Min(a, high)); +template inline T b2Clamp(T a, T low, T high) { + return b2Max(low, b2Min(a, high)); } -inline b2Vec2 b2Clamp(const b2Vec2& a, const b2Vec2& low, const b2Vec2& high) -{ - return b2Max(low, b2Min(a, high)); +inline b2Vec2 b2Clamp(const b2Vec2 &a, const b2Vec2 &low, const b2Vec2 &high) { + return b2Max(low, b2Min(a, high)); } -template inline void b2Swap(T& a, T& b) -{ - T tmp = a; - a = b; - b = tmp; +template inline void b2Swap(T &a, T &b) { + T tmp = a; + a = b; + b = tmp; } /// "Next Largest Power of 2 -/// Given a binary integer value x, the next largest power of 2 can be computed by a SWAR algorithm -/// that recursively "folds" the upper bits into the lower bits. This process yields a bit vector with -/// the same most significant 1 as x, but all 1's below it. Adding 1 to that value yields the next -/// largest power of 2. For a 32-bit value:" -inline uint32 b2NextPowerOfTwo(uint32 x) -{ - x |= (x >> 1); - x |= (x >> 2); - x |= (x >> 4); - x |= (x >> 8); - x |= (x >> 16); - return x + 1; +/// Given a binary integer value x, the next largest power of 2 can be computed +/// by a SWAR algorithm that recursively "folds" the upper bits into the lower +/// bits. This process yields a bit vector with the same most significant 1 as +/// x, but all 1's below it. Adding 1 to that value yields the next largest +/// power of 2. For a 32-bit value:" +inline uint32 b2NextPowerOfTwo(uint32 x) { + x |= (x >> 1); + x |= (x >> 2); + x |= (x >> 4); + x |= (x >> 8); + x |= (x >> 16); + return x + 1; } -inline bool b2IsPowerOfTwo(uint32 x) -{ - bool result = x > 0 && (x & (x - 1)) == 0; - return result; +inline bool b2IsPowerOfTwo(uint32 x) { + bool result = x > 0 && (x & (x - 1)) == 0; + return result; } -inline void b2Sweep::GetTransform(b2Transform* xf, float32 beta) const -{ - xf->p = (1.0f - beta) * c0 + beta * c; - float32 angle = (1.0f - beta) * a0 + beta * a; - xf->q.Set(angle); +inline void b2Sweep::GetTransform(b2Transform *xf, float32 beta) const { + xf->p = (1.0f - beta) * c0 + beta * c; + float32 angle = (1.0f - beta) * a0 + beta * a; + xf->q.Set(angle); - // Shift to origin - xf->p -= b2Mul(xf->q, localCenter); + // Shift to origin + xf->p -= b2Mul(xf->q, localCenter); } -inline void b2Sweep::Advance(float32 alpha) -{ - b2Assert(alpha0 < 1.0f); - float32 beta = (alpha - alpha0) / (1.0f - alpha0); - c0 += beta * (c - c0); - a0 += beta * (a - a0); - alpha0 = alpha; +inline void b2Sweep::Advance(float32 alpha) { + b2Assert(alpha0 < 1.0f); + float32 beta = (alpha - alpha0) / (1.0f - alpha0); + c0 += beta * (c - c0); + a0 += beta * (a - a0); + alpha0 = alpha; } /// Normalize an angle in radians to be between -pi and pi -inline void b2Sweep::Normalize() -{ - float32 twoPi = 2.0f * b2_pi; - float32 d = twoPi * floorf(a0 / twoPi); - a0 -= d; - a -= d; +inline void b2Sweep::Normalize() { + float32 twoPi = 2.0f * b2_pi; + float32 d = twoPi * floorf(a0 / twoPi); + a0 -= d; + a -= d; } - -#endif +} // namespace daabbcc diff --git a/daabbcc/include/DynamicTree/b2Settings.h b/daabbcc/include/DynamicTree/b2Settings.h index 1e2fc9e..b0d2420 100755 --- a/daabbcc/include/DynamicTree/b2Settings.h +++ b/daabbcc/include/DynamicTree/b2Settings.h @@ -1,36 +1,37 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this 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 B2_SETTINGS_H -#define B2_SETTINGS_H + * Copyright (c) 2006-2009 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this 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. + */ + +#pragma once -#include #include #include +#include + +namespace daabbcc { #if !defined(NDEBUG) - #define b2DEBUG +#define b2DEBUG #endif #define B2_NOT_USED(x) ((void)(x)) #define b2Assert(A) assert(A) -typedef signed char int8; +typedef signed char int8; typedef signed short int16; typedef signed int int32; typedef unsigned char uint8; @@ -39,49 +40,43 @@ typedef unsigned int uint32; typedef float float32; typedef double float64; -#define b2_maxFloat FLT_MAX -#define b2_epsilon FLT_EPSILON -#define b2_pi 3.14159265359f - - +#define b2_maxFloat FLT_MAX +#define b2_epsilon FLT_EPSILON +#define b2_pi 3.14159265359f /// The maximum number of vertices on a convex polygon. You cannot increase /// this too much because b2BlockAllocator has a maximum object size. -#define b2_maxPolygonVertices 8 +#define b2_maxPolygonVertices 8 /// This is used to fatten AABBs in the dynamic tree. This allows proxies /// to move by a small amount without triggering a tree adjustment. /// This is in meters. -#define b2_aabbExtension 0.1f +#define b2_aabbExtension 0.1f /// This is used to fatten AABBs in the dynamic tree. This is used to predict /// the future position based on the current displacement. /// This is a dimensionless multiplier. -#define b2_aabbMultiplier 2.0f - - +#define b2_aabbMultiplier 2.0f // Memory Allocation /// Implement this function to use your own memory allocator. -void* b2Alloc(int32 size); +void *b2Alloc(int32 size); /// If you implement b2Alloc, you should also implement this function. -void b2Free(void* mem); +void b2Free(void *mem); /// Logging function. -void b2Log(const char* string, ...); +void b2Log(const char *string, ...); /// Version numbering scheme. /// See http://en.wikipedia.org/wiki/Software_versioning -struct b2Version -{ - int32 major; ///< significant changes - int32 minor; ///< incremental changes - int32 revision; ///< bug fixes +struct b2Version { + int32 major; ///< significant changes + int32 minor; ///< incremental changes + int32 revision; ///< bug fixes }; /// Current version. extern b2Version b2_version; - -#endif +} // namespace daabbcc diff --git a/daabbcc/src/DynamicTree.cpp b/daabbcc/src/DynamicTree.cpp index f36af33..0a1cf3a 100644 --- a/daabbcc/src/DynamicTree.cpp +++ b/daabbcc/src/DynamicTree.cpp @@ -1,7 +1,11 @@ -#include "dmsdk/dlib/log.h" #include -DynamicTree::DynamicTree() : isSorted(false), groupCounter(0), nodeProxyID(0), state(true), updateCounter(0), m_AccumFrameTime(0), m_PreviousFrameTime(dmTime::GetTime()) { +using namespace daabbcc; + +DynamicTree::DynamicTree() + : isSorted(false), groupCounter(0), nodeProxyID(0), state(true), + updateCounter(0), m_AccumFrameTime(0), + m_PreviousFrameTime(dmTime::GetTime()) { result.SetCapacity(100); ray_result.SetCapacity(100); @@ -12,9 +16,14 @@ DynamicTree::DynamicTree() : isSorted(false), groupCounter(0), nodeProxyID(0), s DynamicTree::~DynamicTree() { ResetTree(); } // ITERATE CALLBACKS -void DynamicTree::IterateRemoveCallback(DynamicTree *context, const uint32_t *key, Groups *value) { delete value->m_tree; } +void DynamicTree::IterateRemoveCallback(DynamicTree *context, + const uint32_t *key, Groups *value) { + delete value->m_tree; +} -void DynamicTree::SetUpdateFrequency(int32_t updateFrequency) { m_UpdateFrequency = updateFrequency; } +void DynamicTree::SetUpdateFrequency(int32_t updateFrequency) { + m_UpdateFrequency = updateFrequency; +} // Reset void DynamicTree::ResetTree() { ht.Iterate(IterateRemoveCallback, this); @@ -49,7 +58,9 @@ int DynamicTree::AddGroup() { return c; } -void DynamicTree::AddGameObject(uint32_t groupID, int32 proxyId, dmGameObject::HInstance instance, int32 w, int32 h) { +void DynamicTree::AddGameObject(uint32_t groupID, int32 proxyId, + dmGameObject::HInstance instance, int32 w, + int32 h) { GameObjectContainer goContainer; goContainer.groupID = groupID; goContainer.proxyId = proxyId; @@ -115,11 +126,14 @@ void DynamicTree::GameobjectUpdate() { for (uint32_t i = 0; i < num_steps; ++i) { - for (updateCounter = 0; updateCounter < m_GameObjectContainer.Size(); ++updateCounter) { + for (updateCounter = 0; updateCounter < m_GameObjectContainer.Size(); + ++updateCounter) { updateContainer = &m_GameObjectContainer[updateCounter]; goPosition = dmGameObject::GetPosition(updateContainer->instance); - MoveProxy(updateContainer->groupID, updateContainer->proxyId, goPosition.getX(), goPosition.getY(), updateContainer->w, updateContainer->h); + MoveProxy(updateContainer->groupID, updateContainer->proxyId, + goPosition.getX(), goPosition.getY(), updateContainer->w, + updateContainer->h); } } } @@ -153,7 +167,8 @@ int32 DynamicTree::AddProxy(int groupId, float x, float y, int w, int h) { void DynamicTree::DestroyProxyID(int groupID, int proxyID) { for (uint32_t i = 0; i < m_GameObjectContainer.Size(); ++i) { - if (m_GameObjectContainer[i].groupID == groupID && m_GameObjectContainer[i].proxyId == proxyID) { + if (m_GameObjectContainer[i].groupID == groupID && + m_GameObjectContainer[i].proxyId == proxyID) { m_GameObjectContainer.EraseSwap(i); } } @@ -193,7 +208,8 @@ void DynamicTree::RemoveProxy(int groupID, int proxyID) { * RAY ******************************/ -float32 DynamicTree::RayCastCallback(const b2RayCastInputAABB &input, int32 proxyId, int groupId) { +float32 DynamicTree::RayCastCallback(const b2RayCastInputAABB &input, + int32 proxyId, int groupId) { b2AABB aabb = GetAABB(groupId, proxyId); b2RayCastOutputAABB output; bool hit = aabb.RayCast(&output, input); @@ -232,7 +248,8 @@ float32 DynamicTree::RayCastCallback(const b2RayCastInputAABB &input, int32 prox return input.maxFraction; } -void DynamicTree::RayCastSort(int groupId, float start_x, float start_y, float end_x, float end_y) { +void DynamicTree::RayCastSort(int groupId, float start_x, float start_y, + float end_x, float end_y) { isSorted = true; ray_result.SetSize(0); orderResult.SetSize(0); @@ -253,10 +270,12 @@ void DynamicTree::RayCastSort(int groupId, float start_x, float start_y, float e ht.Get(groupId)->m_tree->RayCast(this, m_rayCastInput, groupId); - jc::radix_sort(orderResult.Begin(), orderResult.End(), tmpOrderResult.Begin()); + jc::radix_sort(orderResult.Begin(), orderResult.End(), + tmpOrderResult.Begin()); } -void DynamicTree::RayCast(int groupId, float start_x, float start_y, float end_x, float end_y) { +void DynamicTree::RayCast(int groupId, float start_x, float start_y, + float end_x, float end_y) { ray_result.SetSize(0); b2RayCastInputAABB m_rayCastInput; @@ -319,7 +338,8 @@ void DynamicTree::QueryIDSort(int groupId, int proxyID) { b2AABB aabb = GetAABB(groupId, proxyID); Query(groupId, aabb); - jc::radix_sort(orderResult.Begin(), orderResult.End(), tmpOrderResult.Begin()); + jc::radix_sort(orderResult.Begin(), orderResult.End(), + tmpOrderResult.Begin()); } void DynamicTree::QueryAABBSort(int groupId, float x, float y, int w, int h) { @@ -331,7 +351,8 @@ void DynamicTree::QueryAABBSort(int groupId, float x, float y, int w, int h) { nodeProxyCenter = aabb.GetCenter(); Query(groupId, aabb); - jc::radix_sort(orderResult.Begin(), orderResult.End(), tmpOrderResult.Begin()); + jc::radix_sort(orderResult.Begin(), orderResult.End(), + tmpOrderResult.Begin()); } void DynamicTree::QueryID(int groupId, int proxyID) { @@ -359,14 +380,16 @@ void DynamicTree::Query(int groupId, b2AABB aabb) { void DynamicTree::updateGameobjectSize(int groupID, int proxyID, int w, int h) { for (uint32_t i = 0; i < m_GameObjectContainer.Size(); ++i) { - if (m_GameObjectContainer[i].groupID == groupID && m_GameObjectContainer[i].proxyId == proxyID) { + if (m_GameObjectContainer[i].groupID == groupID && + m_GameObjectContainer[i].proxyId == proxyID) { m_GameObjectContainer[i].w = w; m_GameObjectContainer[i].h = h; } } } -void DynamicTree::MoveProxy(int groupId, int proxyID, float x, float y, int w, int h) { +void DynamicTree::MoveProxy(int groupId, int proxyID, float x, float y, int w, + int h) { b2AABB current_aabb = GetAABB(groupId, proxyID); b2AABB next_aabb; diff --git a/daabbcc/src/DynamicTree/b2Collision.cpp b/daabbcc/src/DynamicTree/b2Collision.cpp index 7780e01..786387d 100755 --- a/daabbcc/src/DynamicTree/b2Collision.cpp +++ b/daabbcc/src/DynamicTree/b2Collision.cpp @@ -1,90 +1,80 @@ /* -* Copyright (c) 2007-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this 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. -*/ + * Copyright (c) 2007-2009 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this 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 -//#include "b2Distance.h" - +// #include "b2Distance.h" +using namespace daabbcc; // From Real-time Collision Detection, p179. -bool b2AABB::RayCast(b2RayCastOutputAABB* output, const b2RayCastInputAABB& input) const -{ - float32 tmin = -b2_maxFloat; - float32 tmax = b2_maxFloat; +bool b2AABB::RayCast(b2RayCastOutputAABB *output, + const b2RayCastInputAABB &input) const { + float32 tmin = -b2_maxFloat; + float32 tmax = b2_maxFloat; - b2Vec2 p = input.p1; - b2Vec2 d = input.p2 - input.p1; - b2Vec2 absD = b2Abs(d); + b2Vec2 p = input.p1; + b2Vec2 d = input.p2 - input.p1; + b2Vec2 absD = b2Abs(d); - b2Vec2 normal; + b2Vec2 normal; - for (int32 i = 0; i < 2; ++i) - { - if (absD(i) < b2_epsilon) - { - // Parallel. - if (p(i) < lowerBound(i) || upperBound(i) < p(i)) - { - return false; - } - } - else - { - float32 inv_d = 1.0f / d(i); - float32 t1 = (lowerBound(i) - p(i)) * inv_d; - float32 t2 = (upperBound(i) - p(i)) * inv_d; + for (int32 i = 0; i < 2; ++i) { + if (absD(i) < b2_epsilon) { + // Parallel. + if (p(i) < lowerBound(i) || upperBound(i) < p(i)) { + return false; + } + } else { + float32 inv_d = 1.0f / d(i); + float32 t1 = (lowerBound(i) - p(i)) * inv_d; + float32 t2 = (upperBound(i) - p(i)) * inv_d; - // Sign of the normal vector. - float32 s = -1.0f; + // Sign of the normal vector. + float32 s = -1.0f; - if (t1 > t2) - { - b2Swap(t1, t2); - s = 1.0f; - } + if (t1 > t2) { + b2Swap(t1, t2); + s = 1.0f; + } - // Push the min up - if (t1 > tmin) - { - normal.SetZero(); - normal(i) = s; - tmin = t1; - } + // Push the min up + if (t1 > tmin) { + normal.SetZero(); + normal(i) = s; + tmin = t1; + } - // Pull the max down - tmax = b2Min(tmax, t2); + // Pull the max down + tmax = b2Min(tmax, t2); - if (tmin > tmax) - { - return false; - } - } - } + if (tmin > tmax) { + return false; + } + } + } - // Does the ray start inside the box? - // Does the ray intersect beyond the max fraction? - if (tmin < 0.0f || input.maxFraction < tmin) - { - return false; - } + // Does the ray start inside the box? + // Does the ray intersect beyond the max fraction? + if (tmin < 0.0f || input.maxFraction < tmin) { + return false; + } - // Intersection. - output->fraction = tmin; - output->normal = normal; - return true; + // Intersection. + output->fraction = tmin; + output->normal = normal; + return true; } - diff --git a/daabbcc/src/DynamicTree/b2DynamicTree.cpp b/daabbcc/src/DynamicTree/b2DynamicTree.cpp index c6f2b75..87e68aa 100755 --- a/daabbcc/src/DynamicTree/b2DynamicTree.cpp +++ b/daabbcc/src/DynamicTree/b2DynamicTree.cpp @@ -15,844 +15,750 @@ * misrepresented as being the original software. * 3. This notice may not be removed or altered from any source distribution. */ - #include + #include -b2DynamicTree::b2DynamicTree() -{ - m_root = b2_nullNode; - - m_nodeCapacity = 16; - m_nodeCount = 0; - m_nodes = (b2TreeNode*)b2Alloc(m_nodeCapacity * sizeof(b2TreeNode)); - memset(m_nodes, 0, m_nodeCapacity * sizeof(b2TreeNode)); - - // Build a linked list for the free list. - for (int32 i = 0; i < m_nodeCapacity - 1; ++i) - { - m_nodes[i].next = i + 1; - m_nodes[i].height = -1; - } - m_nodes[m_nodeCapacity-1].next = b2_nullNode; - m_nodes[m_nodeCapacity-1].height = -1; - m_freeList = 0; - - m_insertionCount = 0; +using namespace daabbcc; + +b2DynamicTree::b2DynamicTree() { + m_root = b2_nullNode; + + m_nodeCapacity = 16; + m_nodeCount = 0; + m_nodes = (b2TreeNode *)b2Alloc(m_nodeCapacity * sizeof(b2TreeNode)); + memset(m_nodes, 0, m_nodeCapacity * sizeof(b2TreeNode)); + + // Build a linked list for the free list. + for (int32 i = 0; i < m_nodeCapacity - 1; ++i) { + m_nodes[i].next = i + 1; + m_nodes[i].height = -1; + } + m_nodes[m_nodeCapacity - 1].next = b2_nullNode; + m_nodes[m_nodeCapacity - 1].height = -1; + m_freeList = 0; + + m_insertionCount = 0; } -b2DynamicTree::~b2DynamicTree() -{ - - // This frees the entire tree in one shot. - b2Free(m_nodes); +b2DynamicTree::~b2DynamicTree() { + + // This frees the entire tree in one shot. + b2Free(m_nodes); } // Allocate a node from the pool. Grow the pool if necessary. -int32 b2DynamicTree::AllocateNode() -{ - // Expand the node pool as needed. - if (m_freeList == b2_nullNode) - { - b2Assert(m_nodeCount == m_nodeCapacity); - - // The free list is empty. Rebuild a bigger pool. - b2TreeNode* oldNodes = m_nodes; - m_nodeCapacity *= 2; - m_nodes = (b2TreeNode*)b2Alloc(m_nodeCapacity * sizeof(b2TreeNode)); - memcpy(m_nodes, oldNodes, m_nodeCount * sizeof(b2TreeNode)); - b2Free(oldNodes); - - // Build a linked list for the free list. The parent - // pointer becomes the "next" pointer. - for (int32 i = m_nodeCount; i < m_nodeCapacity - 1; ++i) - { - m_nodes[i].next = i + 1; - m_nodes[i].height = -1; - } - m_nodes[m_nodeCapacity-1].next = b2_nullNode; - m_nodes[m_nodeCapacity-1].height = -1; - m_freeList = m_nodeCount; - } - - // Peel a node off the free list. - int32 nodeId = m_freeList; - m_freeList = m_nodes[nodeId].next; - m_nodes[nodeId].parent = b2_nullNode; - m_nodes[nodeId].child1 = b2_nullNode; - m_nodes[nodeId].child2 = b2_nullNode; - m_nodes[nodeId].height = 0; - m_nodes[nodeId].userData = nullptr; - ++m_nodeCount; - return nodeId; +int32 b2DynamicTree::AllocateNode() { + // Expand the node pool as needed. + if (m_freeList == b2_nullNode) { + b2Assert(m_nodeCount == m_nodeCapacity); + + // The free list is empty. Rebuild a bigger pool. + b2TreeNode *oldNodes = m_nodes; + m_nodeCapacity *= 2; + m_nodes = (b2TreeNode *)b2Alloc(m_nodeCapacity * sizeof(b2TreeNode)); + memcpy(m_nodes, oldNodes, m_nodeCount * sizeof(b2TreeNode)); + b2Free(oldNodes); + + // Build a linked list for the free list. The parent + // pointer becomes the "next" pointer. + for (int32 i = m_nodeCount; i < m_nodeCapacity - 1; ++i) { + m_nodes[i].next = i + 1; + m_nodes[i].height = -1; + } + m_nodes[m_nodeCapacity - 1].next = b2_nullNode; + m_nodes[m_nodeCapacity - 1].height = -1; + m_freeList = m_nodeCount; + } + + // Peel a node off the free list. + int32 nodeId = m_freeList; + m_freeList = m_nodes[nodeId].next; + m_nodes[nodeId].parent = b2_nullNode; + m_nodes[nodeId].child1 = b2_nullNode; + m_nodes[nodeId].child2 = b2_nullNode; + m_nodes[nodeId].height = 0; + m_nodes[nodeId].userData = nullptr; + ++m_nodeCount; + return nodeId; } // Return a node to the pool. -void b2DynamicTree::FreeNode(int32 nodeId) -{ - b2Assert(0 <= nodeId && nodeId < m_nodeCapacity); - b2Assert(0 < m_nodeCount); - m_nodes[nodeId].next = m_freeList; - m_nodes[nodeId].height = -1; - m_freeList = nodeId; - --m_nodeCount; +void b2DynamicTree::FreeNode(int32 nodeId) { + b2Assert(0 <= nodeId && nodeId < m_nodeCapacity); + b2Assert(0 < m_nodeCount); + m_nodes[nodeId].next = m_freeList; + m_nodes[nodeId].height = -1; + m_freeList = nodeId; + --m_nodeCount; } // Create a proxy in the tree as a leaf node. We return the index // of the node instead of a pointer so that we can grow // the node pool. -int32 b2DynamicTree::CreateProxy(const b2AABB& aabb, void* userData) -{ - int32 proxyId = AllocateNode(); - - // Fatten the aabb. - b2Vec2 r(b2_aabbExtension, b2_aabbExtension); - m_nodes[proxyId].aabb.lowerBound = aabb.lowerBound - r; - m_nodes[proxyId].aabb.upperBound = aabb.upperBound + r; - m_nodes[proxyId].userData = userData; - m_nodes[proxyId].height = 0; - - InsertLeaf(proxyId); - - return proxyId; +int32 b2DynamicTree::CreateProxy(const b2AABB &aabb, void *userData) { + int32 proxyId = AllocateNode(); + + // Fatten the aabb. + b2Vec2 r(b2_aabbExtension, b2_aabbExtension); + m_nodes[proxyId].aabb.lowerBound = aabb.lowerBound - r; + m_nodes[proxyId].aabb.upperBound = aabb.upperBound + r; + m_nodes[proxyId].userData = userData; + m_nodes[proxyId].height = 0; + + InsertLeaf(proxyId); + + return proxyId; } -void b2DynamicTree::DestroyProxy(int32 proxyId) -{ - b2Assert(0 <= proxyId && proxyId < m_nodeCapacity); - b2Assert(m_nodes[proxyId].IsLeaf()); - - RemoveLeaf(proxyId); - FreeNode(proxyId); +void b2DynamicTree::DestroyProxy(int32 proxyId) { + b2Assert(0 <= proxyId && proxyId < m_nodeCapacity); + b2Assert(m_nodes[proxyId].IsLeaf()); + + RemoveLeaf(proxyId); + FreeNode(proxyId); } -bool b2DynamicTree::MoveProxy(int32 proxyId, const b2AABB& aabb, const b2Vec2& displacement) -{ - b2Assert(0 <= proxyId && proxyId < m_nodeCapacity); - - b2Assert(m_nodes[proxyId].IsLeaf()); - - if (m_nodes[proxyId].aabb.Contains(aabb)) - { - return false; - } - - RemoveLeaf(proxyId); - - // Extend AABB. - b2AABB b = aabb; - b2Vec2 r(b2_aabbExtension, b2_aabbExtension); - b.lowerBound = b.lowerBound - r; - b.upperBound = b.upperBound + r; - - // Predict AABB displacement. - b2Vec2 d = b2_aabbMultiplier * displacement; - - if (d.x < 0.0f) - { - b.lowerBound.x += d.x; - } - else - { - b.upperBound.x += d.x; - } - - if (d.y < 0.0f) - { - b.lowerBound.y += d.y; - } - else - { - b.upperBound.y += d.y; - } - - m_nodes[proxyId].aabb = b; - - InsertLeaf(proxyId); - return true; +bool b2DynamicTree::MoveProxy(int32 proxyId, const b2AABB &aabb, + const b2Vec2 &displacement) { + b2Assert(0 <= proxyId && proxyId < m_nodeCapacity); + + b2Assert(m_nodes[proxyId].IsLeaf()); + + if (m_nodes[proxyId].aabb.Contains(aabb)) { + return false; + } + + RemoveLeaf(proxyId); + + // Extend AABB. + b2AABB b = aabb; + b2Vec2 r(b2_aabbExtension, b2_aabbExtension); + b.lowerBound = b.lowerBound - r; + b.upperBound = b.upperBound + r; + + // Predict AABB displacement. + b2Vec2 d = b2_aabbMultiplier * displacement; + + if (d.x < 0.0f) { + b.lowerBound.x += d.x; + } else { + b.upperBound.x += d.x; + } + + if (d.y < 0.0f) { + b.lowerBound.y += d.y; + } else { + b.upperBound.y += d.y; + } + + m_nodes[proxyId].aabb = b; + + InsertLeaf(proxyId); + return true; } -struct b2PriorityQueue -{ - b2PriorityQueue() - { - b2Assert(m_capacity <= sizeof(m_indexArray) / sizeof(m_indexArray[0])); - b2Assert(m_capacity <= sizeof(m_costArray) / sizeof(m_costArray[0])); - } - - ~b2PriorityQueue() - { - if (m_costs != m_costArray) b2Free(m_costs); - if (m_indices != m_indexArray) b2Free(m_indices); +struct b2PriorityQueue { + b2PriorityQueue() { + b2Assert(m_capacity <= sizeof(m_indexArray) / sizeof(m_indexArray[0])); + b2Assert(m_capacity <= sizeof(m_costArray) / sizeof(m_costArray[0])); + } + + ~b2PriorityQueue() { + if (m_costs != m_costArray) + b2Free(m_costs); + if (m_indices != m_indexArray) + b2Free(m_indices); + } + + inline void Push(int32 index, float32 cost) { + if (m_count == m_capacity) { + m_capacity *= 2; + + float32 *oldCosts = m_costs; + m_costs = (float32 *)b2Alloc(m_capacity * sizeof(float32)); + memcpy(m_costs, oldCosts, m_count * sizeof(float32)); + if (oldCosts != m_costArray) + b2Free(oldCosts); + + int32 *oldIndices = m_indices; + m_indices = (int32 *)b2Alloc(m_capacity * sizeof(int32)); + memcpy(m_indices, oldIndices, m_count * sizeof(int32)); + if (oldIndices != m_indexArray) + b2Free(oldIndices); } - - inline void Push(int32 index, float32 cost) - { - if (m_count == m_capacity) - { - m_capacity *= 2; - - float32* oldCosts = m_costs; - m_costs = (float32*)b2Alloc(m_capacity * sizeof(float32)); - memcpy(m_costs, oldCosts, m_count * sizeof(float32)); - if (oldCosts != m_costArray) b2Free(oldCosts); - - int32* oldIndices = m_indices; - m_indices = (int32*)b2Alloc(m_capacity * sizeof(int32)); - memcpy(m_indices, oldIndices, m_count * sizeof(int32)); - if (oldIndices != m_indexArray) b2Free(oldIndices); - } - - m_indices[m_count] = index; - m_costs[m_count] = cost; - ++m_count; - - int32 i = m_count; - while (i > 1 && Predicate(i - 1, i / 2 - 1) > 0) - { - Swap(i - 1, i / 2 - 1); - i /= 2; - } + + m_indices[m_count] = index; + m_costs[m_count] = cost; + ++m_count; + + int32 i = m_count; + while (i > 1 && Predicate(i - 1, i / 2 - 1) > 0) { + Swap(i - 1, i / 2 - 1); + i /= 2; } - - inline bool Pop(int32* index, float32* cost) - { - if (!m_count) return false; - *index = m_indices[0]; - *cost = m_costs[0]; - - m_count--; - m_indices[0] = m_indices[m_count]; - m_costs[0] = m_costs[m_count]; - - int32 u = 0, v = 1; - while (u != v) - { - u = v; - if (2 * u + 1 <= m_count) { - if (Predicate(u - 1, 2 * u - 1) <= 0) v = 2 * u; - if (Predicate(v - 1, 2 * u + 1 - 1) <= 0) v = 2 * u + 1; - } else if (2 * u <= m_count) { - if (Predicate(u - 1, 2 * u - 1) <= 0) v = 2 * u; - } - - if (u != v) { - Swap(u - 1, v - 1); - } - } - - return true; + } + + inline bool Pop(int32 *index, float32 *cost) { + if (!m_count) + return false; + *index = m_indices[0]; + *cost = m_costs[0]; + + m_count--; + m_indices[0] = m_indices[m_count]; + m_costs[0] = m_costs[m_count]; + + int32 u = 0, v = 1; + while (u != v) { + u = v; + if (2 * u + 1 <= m_count) { + if (Predicate(u - 1, 2 * u - 1) <= 0) + v = 2 * u; + if (Predicate(v - 1, 2 * u + 1 - 1) <= 0) + v = 2 * u + 1; + } else if (2 * u <= m_count) { + if (Predicate(u - 1, 2 * u - 1) <= 0) + v = 2 * u; + } + + if (u != v) { + Swap(u - 1, v - 1); + } } - + + return true; + } + private: - inline int32 Predicate(int32 iA, int32 iB) - { - float32 costA = m_costs[iA]; - float32 costB = m_costs[iB]; - return costA < costB ? -1 : costA > costB ? 1 : 0; - } - - inline void Swap(int32 iA, int32 iB) - { - int32 ival = m_indices[iA]; - m_indices[iA] = m_indices[iB]; - m_indices[iB] = ival; - - float32 fval = m_costs[iA]; - m_costs[iA] = m_costs[iB]; - m_costs[iB] = fval; - } - - int32 m_count = 0; - int32 m_capacity = 256; - int32* m_indices = m_indexArray; - float32* m_costs = m_costArray; - int32 m_indexArray[256]; - float32 m_costArray[256]; + inline int32 Predicate(int32 iA, int32 iB) { + float32 costA = m_costs[iA]; + float32 costB = m_costs[iB]; + return costA < costB ? -1 : costA > costB ? 1 : 0; + } + + inline void Swap(int32 iA, int32 iB) { + int32 ival = m_indices[iA]; + m_indices[iA] = m_indices[iB]; + m_indices[iB] = ival; + + float32 fval = m_costs[iA]; + m_costs[iA] = m_costs[iB]; + m_costs[iB] = fval; + } + + int32 m_count = 0; + int32 m_capacity = 256; + int32 *m_indices = m_indexArray; + float32 *m_costs = m_costArray; + int32 m_indexArray[256]; + float32 m_costArray[256]; }; -static inline float Cost(b2AABB L, b2AABB searchAABB) -{ - L.Combine(searchAABB); - return L.GetPerimeter(); +static inline float Cost(b2AABB L, b2AABB searchAABB) { + L.Combine(searchAABB); + return L.GetPerimeter(); } -static inline float InheritedCost(b2AABB L, b2AABB candidate) -{ - L.Combine(candidate); - return L.GetPerimeter() - candidate.GetPerimeter(); +static inline float InheritedCost(b2AABB L, b2AABB candidate) { + L.Combine(candidate); + return L.GetPerimeter() - candidate.GetPerimeter(); } // Branch and bound method. // See http://box2d.org/files/GDC2019/ErinCatto_DynamicBVH_Full.pdf // And https://en.wikipedia.org/wiki/Branch_and_bound -int32 b2DynamicTree::PickBest(b2AABB L) -{ - b2PriorityQueue queue; - queue.Push(m_root, InheritedCost(L, m_nodes[m_root].aabb)); - - float toInsertSurfaceArea = L.GetPerimeter(); - float bestCost = FLT_MAX; - int bestIndex = b2_nullNode; - int searchIndex; - float searchInheritedCost; - while (queue.Pop(&searchIndex, &searchInheritedCost)) - { - b2AABB search_aabb = m_nodes[searchIndex].aabb; - float cost = Cost(L, search_aabb) + searchInheritedCost; - if (cost < bestCost) { - bestCost = cost; - bestIndex = searchIndex; - } - - float inheritedCost = InheritedCost(L, search_aabb) + searchInheritedCost; - float lowerBound = toInsertSurfaceArea + inheritedCost; - if (lowerBound < bestCost) { - int child1 = m_nodes[searchIndex].child1; - int child2 = m_nodes[searchIndex].child2; - if (child1 != b2_nullNode) { - assert(child2 != b2_nullNode); - queue.Push(child1, inheritedCost); - queue.Push(child2, inheritedCost); - } - } +int32 b2DynamicTree::PickBest(b2AABB L) { + b2PriorityQueue queue; + queue.Push(m_root, InheritedCost(L, m_nodes[m_root].aabb)); + + float toInsertSurfaceArea = L.GetPerimeter(); + float bestCost = FLT_MAX; + int bestIndex = b2_nullNode; + int searchIndex; + float searchInheritedCost; + while (queue.Pop(&searchIndex, &searchInheritedCost)) { + b2AABB search_aabb = m_nodes[searchIndex].aabb; + float cost = Cost(L, search_aabb) + searchInheritedCost; + if (cost < bestCost) { + bestCost = cost; + bestIndex = searchIndex; } - - return bestIndex; + + float inheritedCost = InheritedCost(L, search_aabb) + searchInheritedCost; + float lowerBound = toInsertSurfaceArea + inheritedCost; + if (lowerBound < bestCost) { + int child1 = m_nodes[searchIndex].child1; + int child2 = m_nodes[searchIndex].child2; + if (child1 != b2_nullNode) { + assert(child2 != b2_nullNode); + queue.Push(child1, inheritedCost); + queue.Push(child2, inheritedCost); + } + } + } + + return bestIndex; } -void b2DynamicTree::InsertLeaf(int32 leaf) -{ - ++m_insertionCount; - - if (m_root == b2_nullNode) - { - m_root = leaf; - m_nodes[m_root].parent = b2_nullNode; - return; - } - - // Find the best sibling for this node - b2AABB leafAABB = m_nodes[leaf].aabb; - int32 sibling = PickBest(leafAABB); - - // Create a new parent. - int32 oldParent = m_nodes[sibling].parent; - int32 newParent = AllocateNode(); - m_nodes[newParent].parent = oldParent; - m_nodes[newParent].userData = nullptr; - m_nodes[newParent].aabb.Combine(leafAABB, m_nodes[sibling].aabb); - m_nodes[newParent].height = m_nodes[sibling].height + 1; - - if (oldParent != b2_nullNode) - { - // The sibling was not the root. - if (m_nodes[oldParent].child1 == sibling) - { - m_nodes[oldParent].child1 = newParent; - } - else - { - m_nodes[oldParent].child2 = newParent; - } - - m_nodes[newParent].child1 = sibling; - m_nodes[newParent].child2 = leaf; - m_nodes[sibling].parent = newParent; - m_nodes[leaf].parent = newParent; - } - else - { - // The sibling was the root. - m_nodes[newParent].child1 = sibling; - m_nodes[newParent].child2 = leaf; - m_nodes[sibling].parent = newParent; - m_nodes[leaf].parent = newParent; - m_root = newParent; - } - - // Walk back up the tree fixing heights and AABBs - int32 index = m_nodes[leaf].parent; - while (index != b2_nullNode) - { - index = Balance(index); - - int32 child1 = m_nodes[index].child1; - int32 child2 = m_nodes[index].child2; - - b2Assert(child1 != b2_nullNode); - b2Assert(child2 != b2_nullNode); - - m_nodes[index].height = 1 + b2Max(m_nodes[child1].height, m_nodes[child2].height); - m_nodes[index].aabb.Combine(m_nodes[child1].aabb, m_nodes[child2].aabb); - - index = m_nodes[index].parent; +void b2DynamicTree::InsertLeaf(int32 leaf) { + ++m_insertionCount; + + if (m_root == b2_nullNode) { + m_root = leaf; + m_nodes[m_root].parent = b2_nullNode; + return; + } + + // Find the best sibling for this node + b2AABB leafAABB = m_nodes[leaf].aabb; + int32 sibling = PickBest(leafAABB); + + // Create a new parent. + int32 oldParent = m_nodes[sibling].parent; + int32 newParent = AllocateNode(); + m_nodes[newParent].parent = oldParent; + m_nodes[newParent].userData = nullptr; + m_nodes[newParent].aabb.Combine(leafAABB, m_nodes[sibling].aabb); + m_nodes[newParent].height = m_nodes[sibling].height + 1; + + if (oldParent != b2_nullNode) { + // The sibling was not the root. + if (m_nodes[oldParent].child1 == sibling) { + m_nodes[oldParent].child1 = newParent; + } else { + m_nodes[oldParent].child2 = newParent; } - - //Validate(); + + m_nodes[newParent].child1 = sibling; + m_nodes[newParent].child2 = leaf; + m_nodes[sibling].parent = newParent; + m_nodes[leaf].parent = newParent; + } else { + // The sibling was the root. + m_nodes[newParent].child1 = sibling; + m_nodes[newParent].child2 = leaf; + m_nodes[sibling].parent = newParent; + m_nodes[leaf].parent = newParent; + m_root = newParent; + } + + // Walk back up the tree fixing heights and AABBs + int32 index = m_nodes[leaf].parent; + while (index != b2_nullNode) { + index = Balance(index); + + int32 child1 = m_nodes[index].child1; + int32 child2 = m_nodes[index].child2; + + b2Assert(child1 != b2_nullNode); + b2Assert(child2 != b2_nullNode); + + m_nodes[index].height = + 1 + b2Max(m_nodes[child1].height, m_nodes[child2].height); + m_nodes[index].aabb.Combine(m_nodes[child1].aabb, m_nodes[child2].aabb); + + index = m_nodes[index].parent; + } + + // Validate(); } -void b2DynamicTree::RemoveLeaf(int32 leaf) -{ - if (leaf == m_root) - { - m_root = b2_nullNode; - return; - } - - int32 parent = m_nodes[leaf].parent; - int32 grandParent = m_nodes[parent].parent; - int32 sibling; - if (m_nodes[parent].child1 == leaf) - { - sibling = m_nodes[parent].child2; - } - else - { - sibling = m_nodes[parent].child1; - } - - if (grandParent != b2_nullNode) - { - // Destroy parent and connect sibling to grandParent. - if (m_nodes[grandParent].child1 == parent) - { - m_nodes[grandParent].child1 = sibling; - } - else - { - m_nodes[grandParent].child2 = sibling; - } - m_nodes[sibling].parent = grandParent; - FreeNode(parent); - - // Adjust ancestor bounds. - int32 index = grandParent; - while (index != b2_nullNode) - { - index = Balance(index); - - int32 child1 = m_nodes[index].child1; - int32 child2 = m_nodes[index].child2; - - m_nodes[index].aabb.Combine(m_nodes[child1].aabb, m_nodes[child2].aabb); - m_nodes[index].height = 1 + b2Max(m_nodes[child1].height, m_nodes[child2].height); - - index = m_nodes[index].parent; - } - } - else - { - m_root = sibling; - m_nodes[sibling].parent = b2_nullNode; - FreeNode(parent); +void b2DynamicTree::RemoveLeaf(int32 leaf) { + if (leaf == m_root) { + m_root = b2_nullNode; + return; + } + + int32 parent = m_nodes[leaf].parent; + int32 grandParent = m_nodes[parent].parent; + int32 sibling; + if (m_nodes[parent].child1 == leaf) { + sibling = m_nodes[parent].child2; + } else { + sibling = m_nodes[parent].child1; + } + + if (grandParent != b2_nullNode) { + // Destroy parent and connect sibling to grandParent. + if (m_nodes[grandParent].child1 == parent) { + m_nodes[grandParent].child1 = sibling; + } else { + m_nodes[grandParent].child2 = sibling; + } + m_nodes[sibling].parent = grandParent; + FreeNode(parent); + + // Adjust ancestor bounds. + int32 index = grandParent; + while (index != b2_nullNode) { + index = Balance(index); + + int32 child1 = m_nodes[index].child1; + int32 child2 = m_nodes[index].child2; + + m_nodes[index].aabb.Combine(m_nodes[child1].aabb, m_nodes[child2].aabb); + m_nodes[index].height = + 1 + b2Max(m_nodes[child1].height, m_nodes[child2].height); + + index = m_nodes[index].parent; } - - //Validate(); + } else { + m_root = sibling; + m_nodes[sibling].parent = b2_nullNode; + FreeNode(parent); + } + + // Validate(); } // Perform a left or right rotation if node A is imbalanced. // Returns the new root index. -int32 b2DynamicTree::Balance(int32 iA) -{ - b2Assert(iA != b2_nullNode); - - b2TreeNode* A = m_nodes + iA; - if (A->IsLeaf() || A->height < 2) - { - return iA; +int32 b2DynamicTree::Balance(int32 iA) { + b2Assert(iA != b2_nullNode); + + b2TreeNode *A = m_nodes + iA; + if (A->IsLeaf() || A->height < 2) { + return iA; + } + + int32 iB = A->child1; + int32 iC = A->child2; + b2Assert(0 <= iB && iB < m_nodeCapacity); + b2Assert(0 <= iC && iC < m_nodeCapacity); + + b2TreeNode *B = m_nodes + iB; + b2TreeNode *C = m_nodes + iC; + + int32 balance = C->height - B->height; + + // Rotate C up + if (balance > 1) { + int32 iF = C->child1; + int32 iG = C->child2; + b2TreeNode *F = m_nodes + iF; + b2TreeNode *G = m_nodes + iG; + b2Assert(0 <= iF && iF < m_nodeCapacity); + b2Assert(0 <= iG && iG < m_nodeCapacity); + + // Swap A and C + C->child1 = iA; + C->parent = A->parent; + A->parent = iC; + + // A's old parent should point to C + if (C->parent != b2_nullNode) { + if (m_nodes[C->parent].child1 == iA) { + m_nodes[C->parent].child1 = iC; + } else { + b2Assert(m_nodes[C->parent].child2 == iA); + m_nodes[C->parent].child2 = iC; + } + } else { + m_root = iC; } - - int32 iB = A->child1; - int32 iC = A->child2; - b2Assert(0 <= iB && iB < m_nodeCapacity); - b2Assert(0 <= iC && iC < m_nodeCapacity); - - b2TreeNode* B = m_nodes + iB; - b2TreeNode* C = m_nodes + iC; - - int32 balance = C->height - B->height; - - // Rotate C up - if (balance > 1) - { - int32 iF = C->child1; - int32 iG = C->child2; - b2TreeNode* F = m_nodes + iF; - b2TreeNode* G = m_nodes + iG; - b2Assert(0 <= iF && iF < m_nodeCapacity); - b2Assert(0 <= iG && iG < m_nodeCapacity); - - // Swap A and C - C->child1 = iA; - C->parent = A->parent; - A->parent = iC; - - // A's old parent should point to C - if (C->parent != b2_nullNode) - { - if (m_nodes[C->parent].child1 == iA) - { - m_nodes[C->parent].child1 = iC; - } - else - { - b2Assert(m_nodes[C->parent].child2 == iA); - m_nodes[C->parent].child2 = iC; - } - } - else - { - m_root = iC; - } - - // Rotate - if (F->height > G->height) - { - C->child2 = iF; - A->child2 = iG; - G->parent = iA; - A->aabb.Combine(B->aabb, G->aabb); - C->aabb.Combine(A->aabb, F->aabb); - - A->height = 1 + b2Max(B->height, G->height); - C->height = 1 + b2Max(A->height, F->height); - } - else - { - C->child2 = iG; - A->child2 = iF; - F->parent = iA; - A->aabb.Combine(B->aabb, F->aabb); - C->aabb.Combine(A->aabb, G->aabb); - - A->height = 1 + b2Max(B->height, F->height); - C->height = 1 + b2Max(A->height, G->height); - } - - return iC; + + // Rotate + if (F->height > G->height) { + C->child2 = iF; + A->child2 = iG; + G->parent = iA; + A->aabb.Combine(B->aabb, G->aabb); + C->aabb.Combine(A->aabb, F->aabb); + + A->height = 1 + b2Max(B->height, G->height); + C->height = 1 + b2Max(A->height, F->height); + } else { + C->child2 = iG; + A->child2 = iF; + F->parent = iA; + A->aabb.Combine(B->aabb, F->aabb); + C->aabb.Combine(A->aabb, G->aabb); + + A->height = 1 + b2Max(B->height, F->height); + C->height = 1 + b2Max(A->height, G->height); } - - // Rotate B up - if (balance < -1) - { - int32 iD = B->child1; - int32 iE = B->child2; - b2TreeNode* D = m_nodes + iD; - b2TreeNode* E = m_nodes + iE; - b2Assert(0 <= iD && iD < m_nodeCapacity); - b2Assert(0 <= iE && iE < m_nodeCapacity); - - // Swap A and B - B->child1 = iA; - B->parent = A->parent; - A->parent = iB; - - // A's old parent should point to B - if (B->parent != b2_nullNode) - { - if (m_nodes[B->parent].child1 == iA) - { - m_nodes[B->parent].child1 = iB; - } - else - { - b2Assert(m_nodes[B->parent].child2 == iA); - m_nodes[B->parent].child2 = iB; - } - } - else - { - m_root = iB; - } - - // Rotate - if (D->height > E->height) - { - B->child2 = iD; - A->child1 = iE; - E->parent = iA; - A->aabb.Combine(C->aabb, E->aabb); - B->aabb.Combine(A->aabb, D->aabb); - - A->height = 1 + b2Max(C->height, E->height); - B->height = 1 + b2Max(A->height, D->height); - } - else - { - B->child2 = iE; - A->child1 = iD; - D->parent = iA; - A->aabb.Combine(C->aabb, D->aabb); - B->aabb.Combine(A->aabb, E->aabb); - - A->height = 1 + b2Max(C->height, D->height); - B->height = 1 + b2Max(A->height, E->height); - } - - return iB; + + return iC; + } + + // Rotate B up + if (balance < -1) { + int32 iD = B->child1; + int32 iE = B->child2; + b2TreeNode *D = m_nodes + iD; + b2TreeNode *E = m_nodes + iE; + b2Assert(0 <= iD && iD < m_nodeCapacity); + b2Assert(0 <= iE && iE < m_nodeCapacity); + + // Swap A and B + B->child1 = iA; + B->parent = A->parent; + A->parent = iB; + + // A's old parent should point to B + if (B->parent != b2_nullNode) { + if (m_nodes[B->parent].child1 == iA) { + m_nodes[B->parent].child1 = iB; + } else { + b2Assert(m_nodes[B->parent].child2 == iA); + m_nodes[B->parent].child2 = iB; + } + } else { + m_root = iB; } - - return iA; -} -int32 b2DynamicTree::GetHeight() const -{ - if (m_root == b2_nullNode) - { - return 0; + // Rotate + if (D->height > E->height) { + B->child2 = iD; + A->child1 = iE; + E->parent = iA; + A->aabb.Combine(C->aabb, E->aabb); + B->aabb.Combine(A->aabb, D->aabb); + + A->height = 1 + b2Max(C->height, E->height); + B->height = 1 + b2Max(A->height, D->height); + } else { + B->child2 = iE; + A->child1 = iD; + D->parent = iA; + A->aabb.Combine(C->aabb, D->aabb); + B->aabb.Combine(A->aabb, E->aabb); + + A->height = 1 + b2Max(C->height, D->height); + B->height = 1 + b2Max(A->height, E->height); } - - return m_nodes[m_root].height; + + return iB; + } + + return iA; +} + +int32 b2DynamicTree::GetHeight() const { + if (m_root == b2_nullNode) { + return 0; + } + + return m_nodes[m_root].height; } // -float32 b2DynamicTree::GetAreaRatio() const -{ - if (m_root == b2_nullNode) - { - return 0.0f; - } - - const b2TreeNode* root = m_nodes + m_root; - float32 rootArea = root->aabb.GetPerimeter(); - - float32 totalArea = 0.0f; - for (int32 i = 0; i < m_nodeCapacity; ++i) - { - const b2TreeNode* node = m_nodes + i; - if (node->height < 0) - { - // Free node in pool - continue; - } - - totalArea += node->aabb.GetPerimeter(); +float32 b2DynamicTree::GetAreaRatio() const { + if (m_root == b2_nullNode) { + return 0.0f; + } + + const b2TreeNode *root = m_nodes + m_root; + float32 rootArea = root->aabb.GetPerimeter(); + + float32 totalArea = 0.0f; + for (int32 i = 0; i < m_nodeCapacity; ++i) { + const b2TreeNode *node = m_nodes + i; + if (node->height < 0) { + // Free node in pool + continue; } - - return totalArea / rootArea; + + totalArea += node->aabb.GetPerimeter(); + } + + return totalArea / rootArea; } // Compute the height of a sub-tree. -int32 b2DynamicTree::ComputeHeight(int32 nodeId) const -{ - b2Assert(0 <= nodeId && nodeId < m_nodeCapacity); - b2TreeNode* node = m_nodes + nodeId; - - if (node->IsLeaf()) - { - return 0; - } - - int32 height1 = ComputeHeight(node->child1); - int32 height2 = ComputeHeight(node->child2); - return 1 + b2Max(height1, height2); +int32 b2DynamicTree::ComputeHeight(int32 nodeId) const { + b2Assert(0 <= nodeId && nodeId < m_nodeCapacity); + b2TreeNode *node = m_nodes + nodeId; + + if (node->IsLeaf()) { + return 0; + } + + int32 height1 = ComputeHeight(node->child1); + int32 height2 = ComputeHeight(node->child2); + return 1 + b2Max(height1, height2); } -int32 b2DynamicTree::ComputeHeight() const -{ - int32 height = ComputeHeight(m_root); - return height; +int32 b2DynamicTree::ComputeHeight() const { + int32 height = ComputeHeight(m_root); + return height; } -void b2DynamicTree::ValidateStructure(int32 index) const -{ - if (index == b2_nullNode) - { - return; - } - - if (index == m_root) - { - b2Assert(m_nodes[index].parent == b2_nullNode); - } - - const b2TreeNode* node = m_nodes + index; - - int32 child1 = node->child1; - int32 child2 = node->child2; - - if (node->IsLeaf()) - { - b2Assert(child1 == b2_nullNode); - b2Assert(child2 == b2_nullNode); - b2Assert(node->height == 0); - return; - } - - b2Assert(0 <= child1 && child1 < m_nodeCapacity); - b2Assert(0 <= child2 && child2 < m_nodeCapacity); - - b2Assert(m_nodes[child1].parent == index); - b2Assert(m_nodes[child2].parent == index); - - ValidateStructure(child1); - ValidateStructure(child2); +void b2DynamicTree::ValidateStructure(int32 index) const { + if (index == b2_nullNode) { + return; + } + + if (index == m_root) { + b2Assert(m_nodes[index].parent == b2_nullNode); + } + + const b2TreeNode *node = m_nodes + index; + + int32 child1 = node->child1; + int32 child2 = node->child2; + + if (node->IsLeaf()) { + b2Assert(child1 == b2_nullNode); + b2Assert(child2 == b2_nullNode); + b2Assert(node->height == 0); + return; + } + + b2Assert(0 <= child1 && child1 < m_nodeCapacity); + b2Assert(0 <= child2 && child2 < m_nodeCapacity); + + b2Assert(m_nodes[child1].parent == index); + b2Assert(m_nodes[child2].parent == index); + + ValidateStructure(child1); + ValidateStructure(child2); } -void b2DynamicTree::ValidateMetrics(int32 index) const -{ - if (index == b2_nullNode) - { - return; - } - - const b2TreeNode* node = m_nodes + index; - - int32 child1 = node->child1; - int32 child2 = node->child2; - - if (node->IsLeaf()) - { - b2Assert(child1 == b2_nullNode); - b2Assert(child2 == b2_nullNode); - b2Assert(node->height == 0); - return; - } - - b2Assert(0 <= child1 && child1 < m_nodeCapacity); - b2Assert(0 <= child2 && child2 < m_nodeCapacity); - - int32 height1 = m_nodes[child1].height; - int32 height2 = m_nodes[child2].height; - int32 height; - height = 1 + b2Max(height1, height2); - b2Assert(node->height == height); - - b2AABB aabb; - aabb.Combine(m_nodes[child1].aabb, m_nodes[child2].aabb); - - b2Assert(aabb.lowerBound == node->aabb.lowerBound); - b2Assert(aabb.upperBound == node->aabb.upperBound); - - ValidateMetrics(child1); - ValidateMetrics(child2); +void b2DynamicTree::ValidateMetrics(int32 index) const { + if (index == b2_nullNode) { + return; + } + + const b2TreeNode *node = m_nodes + index; + + int32 child1 = node->child1; + int32 child2 = node->child2; + + if (node->IsLeaf()) { + b2Assert(child1 == b2_nullNode); + b2Assert(child2 == b2_nullNode); + b2Assert(node->height == 0); + return; + } + + b2Assert(0 <= child1 && child1 < m_nodeCapacity); + b2Assert(0 <= child2 && child2 < m_nodeCapacity); + + int32 height1 = m_nodes[child1].height; + int32 height2 = m_nodes[child2].height; + int32 height; + height = 1 + b2Max(height1, height2); + b2Assert(node->height == height); + + b2AABB aabb; + aabb.Combine(m_nodes[child1].aabb, m_nodes[child2].aabb); + + b2Assert(aabb.lowerBound == node->aabb.lowerBound); + b2Assert(aabb.upperBound == node->aabb.upperBound); + + ValidateMetrics(child1); + ValidateMetrics(child2); } -void b2DynamicTree::Validate() const -{ +void b2DynamicTree::Validate() const { #if defined(b2DEBUG) - ValidateStructure(m_root); - ValidateMetrics(m_root); - - int32 freeCount = 0; - int32 freeIndex = m_freeList; - while (freeIndex != b2_nullNode) - { - b2Assert(0 <= freeIndex && freeIndex < m_nodeCapacity); - freeIndex = m_nodes[freeIndex].next; - ++freeCount; - } - - b2Assert(GetHeight() == ComputeHeight()); - - b2Assert(m_nodeCount + freeCount == m_nodeCapacity); + ValidateStructure(m_root); + ValidateMetrics(m_root); + + int32 freeCount = 0; + int32 freeIndex = m_freeList; + while (freeIndex != b2_nullNode) { + b2Assert(0 <= freeIndex && freeIndex < m_nodeCapacity); + freeIndex = m_nodes[freeIndex].next; + ++freeCount; + } + + b2Assert(GetHeight() == ComputeHeight()); + + b2Assert(m_nodeCount + freeCount == m_nodeCapacity); #endif } -int32 b2DynamicTree::GetMaxBalance() const -{ - int32 maxBalance = 0; - for (int32 i = 0; i < m_nodeCapacity; ++i) - { - const b2TreeNode* node = m_nodes + i; - if (node->height <= 1) - { - continue; - } - - b2Assert(node->IsLeaf() == false); - - int32 child1 = node->child1; - int32 child2 = node->child2; - int32 balance = b2Abs(m_nodes[child2].height - m_nodes[child1].height); - maxBalance = b2Max(maxBalance, balance); +int32 b2DynamicTree::GetMaxBalance() const { + int32 maxBalance = 0; + for (int32 i = 0; i < m_nodeCapacity; ++i) { + const b2TreeNode *node = m_nodes + i; + if (node->height <= 1) { + continue; } - - return maxBalance; + + b2Assert(node->IsLeaf() == false); + + int32 child1 = node->child1; + int32 child2 = node->child2; + int32 balance = b2Abs(m_nodes[child2].height - m_nodes[child1].height); + maxBalance = b2Max(maxBalance, balance); + } + + return maxBalance; } -void b2DynamicTree::RebuildBottomUp() -{ - int32* nodes = (int32*)b2Alloc(m_nodeCount * sizeof(int32)); - int32 count = 0; - - // Build array of leaves. Free the rest. - for (int32 i = 0; i < m_nodeCapacity; ++i) - { - if (m_nodes[i].height < 0) - { - // free node in pool - continue; - } - - if (m_nodes[i].IsLeaf()) - { - m_nodes[i].parent = b2_nullNode; - nodes[count] = i; - ++count; - } - else - { - FreeNode(i); - } +void b2DynamicTree::RebuildBottomUp() { + int32 *nodes = (int32 *)b2Alloc(m_nodeCount * sizeof(int32)); + int32 count = 0; + + // Build array of leaves. Free the rest. + for (int32 i = 0; i < m_nodeCapacity; ++i) { + if (m_nodes[i].height < 0) { + // free node in pool + continue; } - - while (count > 1) - { - float32 minCost = b2_maxFloat; - int32 iMin = -1, jMin = -1; - for (int32 i = 0; i < count; ++i) - { - b2AABB aabbi = m_nodes[nodes[i]].aabb; - - for (int32 j = i + 1; j < count; ++j) - { - b2AABB aabbj = m_nodes[nodes[j]].aabb; - b2AABB b; - b.Combine(aabbi, aabbj); - float32 cost = b.GetPerimeter(); - if (cost < minCost) - { - iMin = i; - jMin = j; - minCost = cost; - } - } + + if (m_nodes[i].IsLeaf()) { + m_nodes[i].parent = b2_nullNode; + nodes[count] = i; + ++count; + } else { + FreeNode(i); + } + } + + while (count > 1) { + float32 minCost = b2_maxFloat; + int32 iMin = -1, jMin = -1; + for (int32 i = 0; i < count; ++i) { + b2AABB aabbi = m_nodes[nodes[i]].aabb; + + for (int32 j = i + 1; j < count; ++j) { + b2AABB aabbj = m_nodes[nodes[j]].aabb; + b2AABB b; + b.Combine(aabbi, aabbj); + float32 cost = b.GetPerimeter(); + if (cost < minCost) { + iMin = i; + jMin = j; + minCost = cost; } - - int32 index1 = nodes[iMin]; - int32 index2 = nodes[jMin]; - b2TreeNode* child1 = m_nodes + index1; - b2TreeNode* child2 = m_nodes + index2; - - int32 parentIndex = AllocateNode(); - b2TreeNode* parent = m_nodes + parentIndex; - parent->child1 = index1; - parent->child2 = index2; - parent->height = 1 + b2Max(child1->height, child2->height); - parent->aabb.Combine(child1->aabb, child2->aabb); - parent->parent = b2_nullNode; - - child1->parent = parentIndex; - child2->parent = parentIndex; - - nodes[jMin] = nodes[count-1]; - nodes[iMin] = parentIndex; - --count; + } } - - m_root = nodes[0]; - b2Free(nodes); - - Validate(); + + int32 index1 = nodes[iMin]; + int32 index2 = nodes[jMin]; + b2TreeNode *child1 = m_nodes + index1; + b2TreeNode *child2 = m_nodes + index2; + + int32 parentIndex = AllocateNode(); + b2TreeNode *parent = m_nodes + parentIndex; + parent->child1 = index1; + parent->child2 = index2; + parent->height = 1 + b2Max(child1->height, child2->height); + parent->aabb.Combine(child1->aabb, child2->aabb); + parent->parent = b2_nullNode; + + child1->parent = parentIndex; + child2->parent = parentIndex; + + nodes[jMin] = nodes[count - 1]; + nodes[iMin] = parentIndex; + --count; + } + + m_root = nodes[0]; + b2Free(nodes); + + Validate(); } -void b2DynamicTree::ShiftOrigin(const b2Vec2& newOrigin) -{ - for (int32 i = 0; i < m_nodeCapacity; ++i) - { - m_nodes[i].aabb.lowerBound -= newOrigin; - m_nodes[i].aabb.upperBound -= newOrigin; - } +void b2DynamicTree::ShiftOrigin(const b2Vec2 &newOrigin) { + for (int32 i = 0; i < m_nodeCapacity; ++i) { + m_nodes[i].aabb.lowerBound -= newOrigin; + m_nodes[i].aabb.upperBound -= newOrigin; + } } diff --git a/daabbcc/src/DynamicTree/b2Math.cpp b/daabbcc/src/DynamicTree/b2Math.cpp index 67b60ac..ebdff37 100755 --- a/daabbcc/src/DynamicTree/b2Math.cpp +++ b/daabbcc/src/DynamicTree/b2Math.cpp @@ -1,94 +1,94 @@ /* -* Copyright (c) 2007-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this 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. -*/ + * Copyright (c) 2007-2009 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this 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 +using namespace daabbcc; + const b2Vec2 b2Vec2_zero(0.0f, 0.0f); /// Solve A * x = b, where b is a column vector. This is more efficient /// than computing the inverse in one-shot cases. -b2Vec3 b2Mat33::Solve33(const b2Vec3& b) const -{ - float32 det = b2Dot(ex, b2Cross(ey, ez)); - if (det != 0.0f) - { - det = 1.0f / det; - } - b2Vec3 x; - x.x = det * b2Dot(b, b2Cross(ey, ez)); - x.y = det * b2Dot(ex, b2Cross(b, ez)); - x.z = det * b2Dot(ex, b2Cross(ey, b)); - return x; +b2Vec3 b2Mat33::Solve33(const b2Vec3 &b) const { + float32 det = b2Dot(ex, b2Cross(ey, ez)); + if (det != 0.0f) { + det = 1.0f / det; + } + b2Vec3 x; + x.x = det * b2Dot(b, b2Cross(ey, ez)); + x.y = det * b2Dot(ex, b2Cross(b, ez)); + x.z = det * b2Dot(ex, b2Cross(ey, b)); + return x; } /// Solve A * x = b, where b is a column vector. This is more efficient /// than computing the inverse in one-shot cases. -b2Vec2 b2Mat33::Solve22(const b2Vec2& b) const -{ - float32 a11 = ex.x, a12 = ey.x, a21 = ex.y, a22 = ey.y; - float32 det = a11 * a22 - a12 * a21; - if (det != 0.0f) - { - det = 1.0f / det; - } - b2Vec2 x; - x.x = det * (a22 * b.x - a12 * b.y); - x.y = det * (a11 * b.y - a21 * b.x); - return x; +b2Vec2 b2Mat33::Solve22(const b2Vec2 &b) const { + float32 a11 = ex.x, a12 = ey.x, a21 = ex.y, a22 = ey.y; + float32 det = a11 * a22 - a12 * a21; + if (det != 0.0f) { + det = 1.0f / det; + } + b2Vec2 x; + x.x = det * (a22 * b.x - a12 * b.y); + x.y = det * (a11 * b.y - a21 * b.x); + return x; } /// -void b2Mat33::GetInverse22(b2Mat33* M) const -{ - float32 a = ex.x, b = ey.x, c = ex.y, d = ey.y; - float32 det = a * d - b * c; - if (det != 0.0f) - { - det = 1.0f / det; - } +void b2Mat33::GetInverse22(b2Mat33 *M) const { + float32 a = ex.x, b = ey.x, c = ex.y, d = ey.y; + float32 det = a * d - b * c; + if (det != 0.0f) { + det = 1.0f / det; + } - M->ex.x = det * d; M->ey.x = -det * b; M->ex.z = 0.0f; - M->ex.y = -det * c; M->ey.y = det * a; M->ey.z = 0.0f; - M->ez.x = 0.0f; M->ez.y = 0.0f; M->ez.z = 0.0f; + M->ex.x = det * d; + M->ey.x = -det * b; + M->ex.z = 0.0f; + M->ex.y = -det * c; + M->ey.y = det * a; + M->ey.z = 0.0f; + M->ez.x = 0.0f; + M->ez.y = 0.0f; + M->ez.z = 0.0f; } /// Returns the zero matrix if singular. -void b2Mat33::GetSymInverse33(b2Mat33* M) const -{ - float32 det = b2Dot(ex, b2Cross(ey, ez)); - if (det != 0.0f) - { - det = 1.0f / det; - } +void b2Mat33::GetSymInverse33(b2Mat33 *M) const { + float32 det = b2Dot(ex, b2Cross(ey, ez)); + if (det != 0.0f) { + det = 1.0f / det; + } - float32 a11 = ex.x, a12 = ey.x, a13 = ez.x; - float32 a22 = ey.y, a23 = ez.y; - float32 a33 = ez.z; + float32 a11 = ex.x, a12 = ey.x, a13 = ez.x; + float32 a22 = ey.y, a23 = ez.y; + float32 a33 = ez.z; - M->ex.x = det * (a22 * a33 - a23 * a23); - M->ex.y = det * (a13 * a23 - a12 * a33); - M->ex.z = det * (a12 * a23 - a13 * a22); + M->ex.x = det * (a22 * a33 - a23 * a23); + M->ex.y = det * (a13 * a23 - a12 * a33); + M->ex.z = det * (a12 * a23 - a13 * a22); - M->ey.x = M->ex.y; - M->ey.y = det * (a11 * a33 - a13 * a13); - M->ey.z = det * (a13 * a12 - a11 * a23); + M->ey.x = M->ex.y; + M->ey.y = det * (a11 * a33 - a13 * a13); + M->ey.z = det * (a13 * a12 - a11 * a23); - M->ez.x = M->ex.z; - M->ez.y = M->ey.z; - M->ez.z = det * (a11 * a22 - a12 * a12); + M->ez.x = M->ex.z; + M->ez.y = M->ey.z; + M->ez.z = det * (a11 * a22 - a12 * a12); } diff --git a/daabbcc/src/DynamicTree/b2Settings.cpp b/daabbcc/src/DynamicTree/b2Settings.cpp index 3ff2224..a8ff9e9 100755 --- a/daabbcc/src/DynamicTree/b2Settings.cpp +++ b/daabbcc/src/DynamicTree/b2Settings.cpp @@ -1,44 +1,40 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this 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. -*/ + * Copyright (c) 2006-2009 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this 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 -#include #include +#include #include +namespace daabbcc { + b2Version b2_version = {2, 3, 2}; // Memory allocators. Modify these to use your own allocator. -void* b2Alloc(int32 size) -{ - return malloc(size); -} +void *b2Alloc(int32 size) { return malloc(size); } -void b2Free(void* mem) -{ - free(mem); -} +void b2Free(void *mem) { free(mem); } // You can modify this to use your logging facility. -void b2Log(const char* string, ...) -{ - va_list args; - va_start(args, string); - vprintf(string, args); - va_end(args); +void b2Log(const char *string, ...) { + va_list args; + va_start(args, string); + vprintf(string, args); + va_end(args); } +} // namespace daabbcc \ No newline at end of file diff --git a/daabbcc/src/daabbcc.cpp b/daabbcc/src/daabbcc.cpp index 04f360f..0e6fb11 100644 --- a/daabbcc/src/daabbcc.cpp +++ b/daabbcc/src/daabbcc.cpp @@ -8,7 +8,7 @@ #include #include -DynamicTree dynamicTree; +daabbcc::DynamicTree dynamicTree; static int AddGroup(lua_State *L) { int groupID = dynamicTree.AddGroup(); @@ -376,31 +376,32 @@ static int UpdateFrequency(lua_State *L) { } // Functions exposed to Lua -static const luaL_reg Module_methods[] = {{"new_group", AddGroup}, +static const luaL_reg Module_methods[] = { + {"new_group", AddGroup}, - {"raycast", RayCast}, - {"raycast_sort", RayCastSort}, + {"raycast", RayCast}, + {"raycast_sort", RayCastSort}, - {"query", QueryAABB}, - {"query_id", QueryID}, - {"query_sort", QueryAABBSort}, - {"query_id_sort", QueryIDSort}, + {"query", QueryAABB}, + {"query_id", QueryID}, + {"query_sort", QueryAABBSort}, + {"query_id_sort", QueryIDSort}, - {"insert", AddProxy}, - {"insert_gameobject", AddProxyGameobject}, + {"insert", AddProxy}, + {"insert_gameobject", AddProxyGameobject}, - {"remove", RemoveProxy}, - {"remove_group", RemoveGroup}, - {"remove_gameobject", RemoveProxyGameobject}, + {"remove", RemoveProxy}, + {"remove_group", RemoveGroup}, + {"remove_gameobject", RemoveProxyGameobject}, - {"update", MoveProxy}, - {"update_gameobject", updateGameobject}, + {"update", MoveProxy}, + {"update_gameobject", updateGameobject}, - {"clear", Clear}, - {"run", Run}, - {"update_frequency", UpdateFrequency}, + {"clear", Clear}, + {"run", Run}, + {"update_frequency", UpdateFrequency}, - {NULL, NULL}}; + {NULL, NULL}}; static void LuaInit(lua_State *L) { int top = lua_gettop(L); @@ -410,7 +411,8 @@ static void LuaInit(lua_State *L) { } dmExtension::Result AppInitializeDAABBCC(dmExtension::AppParams *params) { - dynamicTree.m_UpdateFrequency = dmConfigFile::GetInt(params->m_ConfigFile, "display.update_frequency", 0); + dynamicTree.m_UpdateFrequency = + dmConfigFile::GetInt(params->m_ConfigFile, "display.update_frequency", 0); return dmExtension::RESULT_OK; } @@ -427,8 +429,14 @@ static dmExtension::Result OnUpdateMyExtension(dmExtension::Params *params) { return dmExtension::RESULT_OK; } -dmExtension::Result AppFinalizeDAABBCC(dmExtension::AppParams *params) { return dmExtension::RESULT_OK; } +dmExtension::Result AppFinalizeDAABBCC(dmExtension::AppParams *params) { + return dmExtension::RESULT_OK; +} -dmExtension::Result FinalizeDAABBCC(dmExtension::Params *params) { return dmExtension::RESULT_OK; } +dmExtension::Result FinalizeDAABBCC(dmExtension::Params *params) { + return dmExtension::RESULT_OK; +} -DM_DECLARE_EXTENSION(DAABBCC, LIB_NAME, AppInitializeDAABBCC, AppFinalizeDAABBCC, InitializeDAABBCC, OnUpdateMyExtension, 0, FinalizeDAABBCC) +DM_DECLARE_EXTENSION(DAABBCC, LIB_NAME, AppInitializeDAABBCC, + AppFinalizeDAABBCC, InitializeDAABBCC, OnUpdateMyExtension, + 0, FinalizeDAABBCC) diff --git a/main/test/test.collection b/main/test/test.collection index 921daf8..233b171 100644 --- a/main/test/test.collection +++ b/main/test/test.collection @@ -5,37 +5,8 @@ embedded_instances { data: "components {\n" " id: \"test\"\n" " component: \"/main/test/test.script\"\n" - " position {\n" - " x: 0.0\n" - " y: 0.0\n" - " z: 0.0\n" - " }\n" - " rotation {\n" - " x: 0.0\n" - " y: 0.0\n" - " z: 0.0\n" - " w: 1.0\n" - " }\n" - " property_decls {\n" - " }\n" "}\n" "" - position { - x: 0.0 - y: 0.0 - z: 0.0 - } - rotation { - x: 0.0 - y: 0.0 - z: 0.0 - w: 1.0 - } - scale3 { - x: 1.0 - y: 1.0 - z: 1.0 - } } embedded_instances { id: "factory" @@ -43,36 +14,36 @@ embedded_instances { " id: \"box\"\n" " type: \"factory\"\n" " data: \"prototype: \\\"/main/assets/box.go\\\"\\n" - "load_dynamically: false\\n" - "dynamic_prototype: false\\n" "\"\n" - " position {\n" - " x: 0.0\n" - " y: 0.0\n" - " z: 0.0\n" - " }\n" - " rotation {\n" - " x: 0.0\n" - " y: 0.0\n" - " z: 0.0\n" - " w: 1.0\n" - " }\n" "}\n" "" - position { - x: 0.0 - y: 0.0 - z: 0.0 - } - rotation { - x: 0.0 - y: 0.0 - z: 0.0 - w: 1.0 - } - scale3 { - x: 1.0 - y: 1.0 - z: 1.0 - } +} +embedded_instances { + id: "dummy_collision" + data: "embedded_components {\n" + " id: \"collisionobject\"\n" + " type: \"collisionobject\"\n" + " data: \"type: COLLISION_OBJECT_TYPE_DYNAMIC\\n" + "mass: 1.0\\n" + "friction: 0.1\\n" + "restitution: 0.5\\n" + "group: \\\"default\\\"\\n" + "mask: \\\"default\\\"\\n" + "embedded_collision_shape {\\n" + " shapes {\\n" + " shape_type: TYPE_BOX\\n" + " position {\\n" + " }\\n" + " rotation {\\n" + " }\\n" + " index: 0\\n" + " count: 3\\n" + " }\\n" + " data: 10.0\\n" + " data: 10.0\\n" + " data: 10.0\\n" + "}\\n" + "\"\n" + "}\n" + "" } diff --git a/main/test/test.script b/main/test/test.script index 2ba5cb8..a795708 100644 --- a/main/test/test.script +++ b/main/test/test.script @@ -1,9 +1,12 @@ local boxes = {} -local box_group +local box_group = -1 local ray_start = vmath.vector3(0, 0, 0) local ray_end = vmath.vector3(365, 370, 0) +local query_result = {} +local result_count = 0 + local function add_box(box_pos) local pos = vmath.vector3(box_pos.x, box_pos.y, 0) local box = factory.create("/factory#box", pos) @@ -48,7 +51,7 @@ function init(self) aabb.update(box_group, boxes[2].proxy_id, boxes[2].pos.x, boxes[2].pos.y, boxes[2].size.x, boxes[2].size.y) -- Query the possible overlaps using AABB. Returns itself. - local query_result, result_count = aabb.query(box_group, boxes[2].pos.x, boxes[2].pos.y, boxes[2].size.x, + query_result, result_count = aabb.query(box_group, boxes[2].pos.x, boxes[2].pos.y, boxes[2].size.x, boxes[2].size.y) print(result_count) @@ -64,53 +67,52 @@ function update(self, dt) msg.post("@render:", "draw_line", { start_point = ray_start, end_point = ray_end, color = vmath.vector4(1, 1, 1, 1) }) -- raycast_sort - local ray_result, result_count = aabb.raycast_sort(box_group, ray_start.x, ray_start.y, ray_end.x, ray_end.y) - if ray_result then + query_result, result_count = aabb.raycast_sort(box_group, ray_start.x, ray_start.y, ray_end.x, ray_end.y) + if query_result then print("raycast_sort") print(result_count) - pprint(ray_result) + pprint(query_result) + end + + -- raycast + query_result, result_count = aabb.raycast(box_group, ray_start.x, ray_start.y, ray_end.x, ray_end.y) + if query_result then + print("raycast") + print(result_count) + pprint(query_result) end -- query_id_sort - local query_id_sort, result_count = aabb.query_id_sort(box_group, boxes[5].proxy_id) - if query_id_sort then + query_result, result_count = aabb.query_id_sort(box_group, boxes[5].proxy_id) + if query_result then print("query_id_sort") print(result_count) - pprint(query_id_sort) + pprint(query_result) end -- query_sort - local query_sort, result_count = aabb.query_sort(box_group, boxes[3].pos.x, boxes[3].pos.y, boxes[3].size.x, + query_result, result_count = aabb.query_sort(box_group, boxes[3].pos.x, boxes[3].pos.y, boxes[3].size.x, boxes[3].size.y) - if query_sort then + if query_result then print("query_sort") print(result_count) - pprint(query_sort) - end - - -- raycast - local ray_result, result_count = aabb.raycast(box_group, ray_start.x, ray_start.y, ray_end.x, ray_end.y) - if ray_result then - print("raycast_sort") - print(result_count) - pprint(ray_result) + pprint(query_result) end -- query_id - local query_id_sort, result_count = aabb.query_id(box_group, boxes[5].proxy_id) - if query_id_sort then - print("query_id_sort") + query_result, result_count = aabb.query_id(box_group, boxes[5].proxy_id) + if query_result then + print("query_id") print(result_count) - pprint(query_id_sort) + pprint(query_result) end -- query - local query_sort, result_count = aabb.query(box_group, boxes[3].pos.x, boxes[3].pos.y, boxes[3].size.x, - boxes[3].size.y) - if query_sort then - print("query_sort") + query_result, result_count = aabb.query(box_group, boxes[3].pos.x, boxes[3].pos.y, boxes[3].size.x, boxes[3].size.y) + if query_result then + print("query") print(result_count) - pprint(query_sort) + pprint(query_result) end end