diff options
Diffstat (limited to 'Box2d/Assets/Program')
94 files changed, 16606 insertions, 380 deletions
diff --git a/Box2d/Assets/Program/Box2d/Box2DXDebug.cs b/Box2d/Assets/Program/Box2d/Box2DXDebug.cs new file mode 100644 index 0000000..49013a6 --- /dev/null +++ b/Box2d/Assets/Program/Box2d/Box2DXDebug.cs @@ -0,0 +1,67 @@ +/* + Box2DX Copyright (c) 2008 Ihar Kalasouski http://code.google.com/p/box2dx + Box2D original C++ version Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this 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. +*/ + +using System; +using System.Text; +using System.Diagnostics; +using System.Collections.Generic; + +namespace Box2DX +{ + public static class Box2DXDebug + { + [Conditional("DEBUG")] + public static void Assert(bool condition) + { + if (!condition) + { + condition = condition; + } + Debug.Assert(condition); + } + + [Conditional("DEBUG")] + public static void Assert(bool condition, string message) + { + if (!condition) + { + condition = condition; + } + Debug.Assert(condition, message); + } + + [Conditional("DEBUG")] + public static void Assert(bool condition, string message, string detailMessage) + { + if (!condition) + { + condition = condition; + } + Debug.Assert(condition, message, detailMessage); + } + + public static void ThrowBox2DXException(String message) + { + string msg = String.Format("Error: {0}", message); + throw new Exception(msg); + } + } +} diff --git a/Box2d/Assets/Program/Box2d/Box2d.cs.meta b/Box2d/Assets/Program/Box2d/Box2DXDebug.cs.meta index 3e25ab6..eb18c32 100644 --- a/Box2d/Assets/Program/Box2d/Box2d.cs.meta +++ b/Box2d/Assets/Program/Box2d/Box2DXDebug.cs.meta @@ -1,5 +1,5 @@ fileFormatVersion: 2 -guid: 7d074173b374c384397568691e8bbb72 +guid: 5f18711c9f994954bbce75799319eb0f MonoImporter: externalObjects: {} serializedVersion: 2 diff --git a/Box2d/Assets/Program/Box2d/Box2d.cs b/Box2d/Assets/Program/Box2d/Box2d.cs deleted file mode 100644 index 48acd74..0000000 --- a/Box2d/Assets/Program/Box2d/Box2d.cs +++ /dev/null @@ -1,10 +0,0 @@ -using System.Collections; -using System.Collections.Generic; - -namespace Box2D -{ - /////////////////////////////////// - // Box2D C# port - // author: chai - /////////////////////////////////// -} diff --git a/Box2d/Assets/Program/Box2d/Collision/BroadPhase.cs b/Box2d/Assets/Program/Box2d/Collision/BroadPhase.cs new file mode 100644 index 0000000..8a9e4c9 --- /dev/null +++ b/Box2d/Assets/Program/Box2d/Collision/BroadPhase.cs @@ -0,0 +1,1183 @@ +/* + Box2DX Copyright (c) 2008 Ihar Kalasouski http://code.google.com/p/box2dx + Box2D original C++ version Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this 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. +*/ + +/* +This broad phase uses the Sweep and Prune algorithm as described in: +Collision Detection in Interactive 3D Environments by Gino van den Bergen +Also, some ideas, such as using integral values for fast compares comes from +Bullet (http:/www.bulletphysics.com). +*/ + +// Notes: +// - we use bound arrays instead of linked lists for cache coherence. +// - we use quantized integral values for fast compares. +// - we use short indices rather than pointers to save memory. +// - we use a stabbing count for fast overlap queries (less than order N). +// - we also use a time stamp on each proxy to speed up the registration of +// overlap query results. +// - where possible, we compare bound indices instead of values to reduce +// cache misses (TODO_ERIN). +// - no broadphase is perfect and neither is this one: it is not great for huge +// worlds (use a multi-SAP instead), it is not great for large objects. + +#define ALLOWUNSAFE +//#define TARGET_FLOAT32_IS_FIXED + +using System; +using System.Collections.Generic; +using System.Text; + +using Box2DX.Common; + +namespace Box2DX.Collision +{ + public delegate float SortKeyFunc(object shape); + +#warning "CAS" + public class BoundValues + { + public ushort[/*2*/] LowerValues = new ushort[2]; + public ushort[/*2*/] UpperValues = new ushort[2]; + } +#warning "CAS" + public class Bound + { + public bool IsLower { get { return (Value & (ushort)1) == (ushort)0; } } + public bool IsUpper { get { return (Value & (ushort)1) == (ushort)1; } } + + public ushort Value; + public ushort ProxyId; + public ushort StabbingCount; + + public Bound Clone() + { + Bound newBound = new Bound(); + newBound.Value = this.Value; + newBound.ProxyId = this.ProxyId; + newBound.StabbingCount = this.StabbingCount; + return newBound; + } + } +#warning "CAS" + public class Proxy + { + public ushort[/*2*/] LowerBounds = new ushort[2], UpperBounds = new ushort[2]; + public ushort OverlapCount; + public ushort TimeStamp; + public object UserData; + + public ushort Next + { + get { return LowerBounds[0]; } + set { LowerBounds[0] = value; } + } + + public bool IsValid { get { return OverlapCount != BroadPhase.Invalid; } } + } + + public class BroadPhase + { +#if TARGET_FLOAT32_IS_FIXED + public static readonly ushort BROADPHASE_MAX = (Common.Math.USHRT_MAX/2); +#else + public static readonly ushort BROADPHASE_MAX = Common.Math.USHRT_MAX; +#endif + + public static readonly ushort Invalid = BROADPHASE_MAX; + public static readonly ushort NullEdge = BROADPHASE_MAX; + + public PairManager _pairManager; + + public Proxy[] _proxyPool = new Proxy[Settings.MaxProxies]; + public ushort _freeProxy; + + public Bound[][] _bounds = new Bound[2][/*(2 * Settings.MaxProxies)*/]; + + public ushort[] _queryResults = new ushort[Settings.MaxProxies]; + public float[] _querySortKeys = new float[Settings.MaxProxies]; + public int _queryResultCount; + + public AABB _worldAABB; + public Vec2 _quantizationFactor; + public int _proxyCount; + public ushort _timeStamp; + + public static bool IsValidate = false; + + public BroadPhase(AABB worldAABB, PairCallback callback) + { + _pairManager = new PairManager(); + _pairManager.Initialize(this, callback); + + Box2DXDebug.Assert(worldAABB.IsValid); + _worldAABB = worldAABB; + _proxyCount = 0; + + Vec2 d = worldAABB.UpperBound - worldAABB.LowerBound; + _quantizationFactor.X = (float)BROADPHASE_MAX / d.X; + _quantizationFactor.Y = (float)BROADPHASE_MAX / d.Y; + + for (ushort i = 0; i < Settings.MaxProxies - 1; ++i) + { + _proxyPool[i] = new Proxy(); + _proxyPool[i].Next = (ushort)(i + 1); + _proxyPool[i].TimeStamp = 0; + _proxyPool[i].OverlapCount = BroadPhase.Invalid; + _proxyPool[i].UserData = null; + } + _proxyPool[Settings.MaxProxies - 1] = new Proxy(); + _proxyPool[Settings.MaxProxies - 1].Next = PairManager.NullProxy; + _proxyPool[Settings.MaxProxies - 1].TimeStamp = 0; + _proxyPool[Settings.MaxProxies - 1].OverlapCount = BroadPhase.Invalid; + _proxyPool[Settings.MaxProxies - 1].UserData = null; + _freeProxy = 0; + + _timeStamp = 1; + _queryResultCount = 0; + + for (int i = 0; i < 2; i++) + { + _bounds[i] = new Bound[(2 * Settings.MaxProxies)]; + } + + int bCount = 2 * Settings.MaxProxies; + for (int j = 0; j < 2; j++) + for (int k = 0; k < bCount; k++) + _bounds[j][k] = new Bound(); + } + + // Use this to see if your proxy is in range. If it is not in range, + // it should be destroyed. Otherwise you may get O(m^2) pairs, where m + // is the number of proxies that are out of range. + public bool InRange(AABB aabb) + { + Vec2 d = Common.Math.Max(aabb.LowerBound - _worldAABB.UpperBound, _worldAABB.LowerBound - aabb.UpperBound); + return Common.Math.Max(d.X, d.Y) < 0.0f; + } + + // Create and destroy proxies. These call Flush first. + public ushort CreateProxy(AABB aabb, object userData) + { + Box2DXDebug.Assert(_proxyCount < Settings.MaxProxies); + Box2DXDebug.Assert(_freeProxy != PairManager.NullProxy); + + ushort proxyId = _freeProxy; + Proxy proxy = _proxyPool[proxyId]; + _freeProxy = proxy.Next; + + proxy.OverlapCount = 0; + proxy.UserData = userData; + + int boundCount = 2 * _proxyCount; + + ushort[] lowerValues = new ushort[2], upperValues = new ushort[2]; + ComputeBounds(out lowerValues, out upperValues, aabb); + + for (int axis = 0; axis < 2; ++axis) + { + Bound[] bounds = _bounds[axis]; + int lowerIndex, upperIndex; + Query(out lowerIndex, out upperIndex, lowerValues[axis], upperValues[axis], bounds, boundCount, axis); + +#warning "Check this" + //memmove(bounds + upperIndex + 2, bounds + upperIndex, (boundCount - upperIndex) * sizeof(b2Bound)); + Bound[] tmp = new Bound[boundCount - upperIndex]; + for (int i = 0; i < (boundCount - upperIndex); i++) + { + tmp[i] = bounds[upperIndex + i].Clone(); + } + for (int i = 0; i < (boundCount - upperIndex); i++) + { + bounds[upperIndex + 2 + i] = tmp[i]; + } + + //memmove(bounds + lowerIndex + 1, bounds + lowerIndex, (upperIndex - lowerIndex) * sizeof(b2Bound)); + tmp = new Bound[upperIndex - lowerIndex]; + for (int i = 0; i < (upperIndex - lowerIndex); i++) + { + tmp[i] = bounds[lowerIndex + i].Clone(); + } + for (int i = 0; i < (upperIndex - lowerIndex); i++) + { + bounds[lowerIndex + 1 + i] = tmp[i]; + } + + // The upper index has increased because of the lower bound insertion. + ++upperIndex; + + // Copy in the new bounds. + bounds[lowerIndex].Value = lowerValues[axis]; + bounds[lowerIndex].ProxyId = proxyId; + bounds[upperIndex].Value = upperValues[axis]; + bounds[upperIndex].ProxyId = proxyId; + + bounds[lowerIndex].StabbingCount = lowerIndex == 0 ? (ushort)0 : bounds[lowerIndex - 1].StabbingCount; + bounds[upperIndex].StabbingCount = bounds[upperIndex - 1].StabbingCount; + + // Adjust the stabbing count between the new bounds. + for (int index = lowerIndex; index < upperIndex; ++index) + { + ++bounds[index].StabbingCount; + } + + // Adjust the all the affected bound indices. + for (int index = lowerIndex; index < boundCount + 2; ++index) + { + Proxy proxy_ = _proxyPool[bounds[index].ProxyId]; + if (bounds[index].IsLower) + { + proxy_.LowerBounds[axis] = (ushort)index; + } + else + { + proxy_.UpperBounds[axis] = (ushort)index; + } + } + } + + ++_proxyCount; + + Box2DXDebug.Assert(_queryResultCount < Settings.MaxProxies); + + // Create pairs if the AABB is in range. + for (int i = 0; i < _queryResultCount; ++i) + { + Box2DXDebug.Assert(_queryResults[i] < Settings.MaxProxies); + Box2DXDebug.Assert(_proxyPool[_queryResults[i]].IsValid); + + _pairManager.AddBufferedPair(proxyId, _queryResults[i]); + } + + _pairManager.Commit(); + + if (IsValidate) + { + Validate(); + } + + // Prepare for next query. + _queryResultCount = 0; + IncrementTimeStamp(); + + return proxyId; + } + + public void DestroyProxy(int proxyId) + { + Box2DXDebug.Assert(0 < _proxyCount && _proxyCount <= Settings.MaxProxies); + Proxy proxy = _proxyPool[proxyId]; + Box2DXDebug.Assert(proxy.IsValid); + + int boundCount = 2 * _proxyCount; + + for (int axis = 0; axis < 2; ++axis) + { + Bound[] bounds = _bounds[axis]; + + int lowerIndex = proxy.LowerBounds[axis]; + int upperIndex = proxy.UpperBounds[axis]; + ushort lowerValue = bounds[lowerIndex].Value; + ushort upperValue = bounds[upperIndex].Value; + +#warning "Check this" + //memmove(bounds + lowerIndex, bounds + lowerIndex + 1, (upperIndex - lowerIndex - 1) * sizeof(b2Bound)); + Bound[] tmp = new Bound[upperIndex - lowerIndex - 1]; + for (int i = 0; i < (upperIndex - lowerIndex - 1); i++) + { + tmp[i] = bounds[lowerIndex + 1 + i].Clone(); + } + for (int i = 0; i < (upperIndex - lowerIndex - 1); i++) + { + bounds[lowerIndex + i] = tmp[i]; + } + + //memmove(bounds + upperIndex - 1, bounds + upperIndex + 1, (boundCount - upperIndex - 1) * sizeof(b2Bound)); + tmp = new Bound[boundCount - upperIndex - 1]; + for (int i = 0; i < (boundCount - upperIndex - 1); i++) + { + tmp[i] = bounds[upperIndex + 1 + i].Clone(); + } + for (int i = 0; i < (boundCount - upperIndex - 1); i++) + { + bounds[upperIndex - 1 + i] = tmp[i]; + } + + // Fix bound indices. + for (int index = lowerIndex; index < boundCount - 2; ++index) + { + Proxy proxy_ = _proxyPool[bounds[index].ProxyId]; + if (bounds[index].IsLower) + { + proxy_.LowerBounds[axis] = (ushort)index; + } + else + { + proxy_.UpperBounds[axis] = (ushort)index; + } + } + + // Fix stabbing count. + for (int index = lowerIndex; index < upperIndex - 1; ++index) + { + --bounds[index].StabbingCount; + } + + // Query for pairs to be removed. lowerIndex and upperIndex are not needed. + Query(out lowerIndex, out upperIndex, lowerValue, upperValue, bounds, boundCount - 2, axis); + } + + Box2DXDebug.Assert(_queryResultCount < Settings.MaxProxies); + + for (int i = 0; i < _queryResultCount; ++i) + { + Box2DXDebug.Assert(_proxyPool[_queryResults[i]].IsValid); + _pairManager.RemoveBufferedPair(proxyId, _queryResults[i]); + } + + _pairManager.Commit(); + + // Prepare for next query. + _queryResultCount = 0; + IncrementTimeStamp(); + + // Return the proxy to the pool. + proxy.UserData = null; + proxy.OverlapCount = BroadPhase.Invalid; + proxy.LowerBounds[0] = BroadPhase.Invalid; + proxy.LowerBounds[1] = BroadPhase.Invalid; + proxy.UpperBounds[0] = BroadPhase.Invalid; + proxy.UpperBounds[1] = BroadPhase.Invalid; + + proxy.Next = _freeProxy; + _freeProxy = (ushort)proxyId; + --_proxyCount; + + if (IsValidate) + { + Validate(); + } + } + + // Call MoveProxy as many times as you like, then when you are done + // call Commit to finalized the proxy pairs (for your time step). + public void MoveProxy(int proxyId, AABB aabb) + { + if (proxyId == PairManager.NullProxy || Settings.MaxProxies <= proxyId) + { + Box2DXDebug.Assert(false); + return; + } + + if (aabb.IsValid == false) + { + Box2DXDebug.Assert(false); + return; + } + + int boundCount = 2 * _proxyCount; + + Proxy proxy = _proxyPool[proxyId]; + + // Get new bound values + BoundValues newValues = new BoundValues(); ; + ComputeBounds(out newValues.LowerValues, out newValues.UpperValues, aabb); + + // Get old bound values + BoundValues oldValues = new BoundValues(); + for (int axis = 0; axis < 2; ++axis) + { + oldValues.LowerValues[axis] = _bounds[axis][proxy.LowerBounds[axis]].Value; + oldValues.UpperValues[axis] = _bounds[axis][proxy.UpperBounds[axis]].Value; + } + + for (int axis = 0; axis < 2; ++axis) + { + Bound[] bounds = _bounds[axis]; + + int lowerIndex = proxy.LowerBounds[axis]; + int upperIndex = proxy.UpperBounds[axis]; + + ushort lowerValue = newValues.LowerValues[axis]; + ushort upperValue = newValues.UpperValues[axis]; + + int deltaLower = lowerValue - bounds[lowerIndex].Value; + int deltaUpper = upperValue - bounds[upperIndex].Value; + + bounds[lowerIndex].Value = lowerValue; + bounds[upperIndex].Value = upperValue; + + // + // Expanding adds overlaps + // + + // Should we move the lower bound down? + if (deltaLower < 0) + { + int index = lowerIndex; + while (index > 0 && lowerValue < bounds[index - 1].Value) + { + Bound bound = bounds[index]; + Bound prevBound = bounds[index - 1]; + + int prevProxyId = prevBound.ProxyId; + Proxy prevProxy = _proxyPool[prevBound.ProxyId]; + + ++prevBound.StabbingCount; + + if (prevBound.IsUpper == true) + { + if (TestOverlap(newValues, prevProxy)) + { + _pairManager.AddBufferedPair(proxyId, prevProxyId); + } + + ++prevProxy.UpperBounds[axis]; + ++bound.StabbingCount; + } + else + { + ++prevProxy.LowerBounds[axis]; + --bound.StabbingCount; + } + + --proxy.LowerBounds[axis]; + Common.Math.Swap<Bound>(ref bounds[index], ref bounds[index - 1]); + --index; + } + } + + // Should we move the upper bound up? + if (deltaUpper > 0) + { + int index = upperIndex; + while (index < boundCount - 1 && bounds[index + 1].Value <= upperValue) + { + Bound bound = bounds[index]; + Bound nextBound = bounds[index + 1]; + int nextProxyId = nextBound.ProxyId; + Proxy nextProxy = _proxyPool[nextProxyId]; + + ++nextBound.StabbingCount; + + if (nextBound.IsLower == true) + { + if (TestOverlap(newValues, nextProxy)) + { + _pairManager.AddBufferedPair(proxyId, nextProxyId); + } + + --nextProxy.LowerBounds[axis]; + ++bound.StabbingCount; + } + else + { + --nextProxy.UpperBounds[axis]; + --bound.StabbingCount; + } + + ++proxy.UpperBounds[axis]; + Common.Math.Swap<Bound>(ref bounds[index], ref bounds[index + 1]); + ++index; + } + } + + // + // Shrinking removes overlaps + // + + // Should we move the lower bound up? + if (deltaLower > 0) + { + int index = lowerIndex; + while (index < boundCount - 1 && bounds[index + 1].Value <= lowerValue) + { + Bound bound = bounds[index]; + Bound nextBound = bounds[index + 1]; + + int nextProxyId = nextBound.ProxyId; + Proxy nextProxy = _proxyPool[nextProxyId]; + + --nextBound.StabbingCount; + + if (nextBound.IsUpper) + { + if (TestOverlap(oldValues, nextProxy)) + { + _pairManager.RemoveBufferedPair(proxyId, nextProxyId); + } + + --nextProxy.UpperBounds[axis]; + --bound.StabbingCount; + } + else + { + --nextProxy.LowerBounds[axis]; + ++bound.StabbingCount; + } + + ++proxy.LowerBounds[axis]; + Common.Math.Swap<Bound>(ref bounds[index], ref bounds[index + 1]); + ++index; + } + } + + // Should we move the upper bound down? + if (deltaUpper < 0) + { + int index = upperIndex; + while (index > 0 && upperValue < bounds[index - 1].Value) + { + Bound bound = bounds[index]; + Bound prevBound = bounds[index - 1]; + + int prevProxyId = prevBound.ProxyId; + Proxy prevProxy = _proxyPool[prevProxyId]; + + --prevBound.StabbingCount; + + if (prevBound.IsLower == true) + { + if (TestOverlap(oldValues, prevProxy)) + { + _pairManager.RemoveBufferedPair(proxyId, prevProxyId); + } + + ++prevProxy.LowerBounds[axis]; + --bound.StabbingCount; + } + else + { + ++prevProxy.UpperBounds[axis]; + ++bound.StabbingCount; + } + + --proxy.UpperBounds[axis]; + Common.Math.Swap<Bound>(ref bounds[index], ref bounds[index - 1]); + --index; + } + } + } + + if (IsValidate) + { + Validate(); + } + } + + public void Commit() + { + _pairManager.Commit(); + } + + // Get a single proxy. Returns NULL if the id is invalid. + public Proxy GetProxy(int proxyId) + { + if (proxyId == PairManager.NullProxy || _proxyPool[proxyId].IsValid == false) + { + return null; + } + + return _proxyPool[proxyId]; + } + + // Query an AABB for overlapping proxies, returns the user data and + // the count, up to the supplied maximum count. + public int Query(AABB aabb, object[] userData, int maxCount) + { + ushort[] lowerValues; + ushort[] upperValues; + ComputeBounds(out lowerValues, out upperValues, aabb); + + int lowerIndex, upperIndex; + + Query(out lowerIndex, out upperIndex, lowerValues[0], upperValues[0], _bounds[0], 2 * _proxyCount, 0); + Query(out lowerIndex, out upperIndex, lowerValues[1], upperValues[1], _bounds[1], 2 * _proxyCount, 1); + + Box2DXDebug.Assert(_queryResultCount < Settings.MaxProxies); + + int count = 0; + for (int i = 0; i < _queryResultCount && count < maxCount; ++i, ++count) + { + Box2DXDebug.Assert(_queryResults[i] < Settings.MaxProxies); + Proxy proxy = _proxyPool[_queryResults[i]]; + Box2DXDebug.Assert(proxy.IsValid); + userData[i] = proxy.UserData; + } + + // Prepare for next query. + _queryResultCount = 0; + IncrementTimeStamp(); + + return count; + } + + /// <summary> + /// Query a segment for overlapping proxies, returns the user data and + /// the count, up to the supplied maximum count. + /// If sortKey is provided, then it is a function mapping from proxy userDatas to distances along the segment (between 0 & 1) + /// Then the returned proxies are sorted on that, before being truncated to maxCount + /// The sortKey of a proxy is assumed to be larger than the closest point inside the proxy along the segment, this allows for early exits + /// Proxies with a negative sortKey are discarded + /// </summary> + public +#if ALLOWUNSAFE + unsafe +#endif //#if ALLOWUNSAFE + int QuerySegment(Segment segment, object[] userData, int maxCount, SortKeyFunc sortKey) + { + float maxLambda = 1; + + float dx = (segment.P2.X - segment.P1.X) * _quantizationFactor.X; + float dy = (segment.P2.Y - segment.P1.Y) * _quantizationFactor.Y; + + int sx = dx < -Settings.FLT_EPSILON ? -1 : (dx > Settings.FLT_EPSILON ? 1 : 0); + int sy = dy < -Settings.FLT_EPSILON ? -1 : (dy > Settings.FLT_EPSILON ? 1 : 0); + + Box2DXDebug.Assert(sx != 0 || sy != 0); + + float p1x = (segment.P1.X - _worldAABB.LowerBound.X) * _quantizationFactor.X; + float p1y = (segment.P1.Y - _worldAABB.LowerBound.Y) * _quantizationFactor.Y; +#if ALLOWUNSAFE + ushort* startValues = stackalloc ushort[2]; + ushort* startValues2 = stackalloc ushort[2]; +#else + ushort[] startValues = new ushort[2]; + ushort[] startValues2 = new ushort[2]; +#endif + + int xIndex; + int yIndex; + + ushort proxyId; + Proxy proxy; + + // TODO_ERIN implement fast float to ushort conversion. + startValues[0] = (ushort)((ushort)(p1x) & (BROADPHASE_MAX - 1)); + startValues2[0] = (ushort)((ushort)(p1x) | 1); + + startValues[1] = (ushort)((ushort)(p1y) & (BROADPHASE_MAX - 1)); + startValues2[1] = (ushort)((ushort)(p1y) | 1); + + //First deal with all the proxies that contain segment.p1 + int lowerIndex; + int upperIndex; + Query(out lowerIndex, out upperIndex, startValues[0], startValues2[0], _bounds[0], 2 * _proxyCount, 0); + if (sx >= 0) xIndex = upperIndex - 1; + else xIndex = lowerIndex; + Query(out lowerIndex, out upperIndex, startValues[1], startValues2[1], _bounds[1], 2 * _proxyCount, 1); + if (sy >= 0) yIndex = upperIndex - 1; + else yIndex = lowerIndex; + + //If we are using sortKey, then sort what we have so far, filtering negative keys + if (sortKey != null) + { + //Fill keys + for (int j = 0; j < _queryResultCount; j++) + { + _querySortKeys[j] = sortKey(_proxyPool[_queryResults[j]].UserData); + } + //Bubble sort keys + //Sorting negative values to the top, so we can easily remove them + int i = 0; + while (i < _queryResultCount - 1) + { + float a = _querySortKeys[i]; + float b = _querySortKeys[i + 1]; + if ((a < 0) ? (b >= 0) : (a > b && b >= 0)) + { + _querySortKeys[i + 1] = a; + _querySortKeys[i] = b; + ushort tempValue = _queryResults[i + 1]; + _queryResults[i + 1] = _queryResults[i]; + _queryResults[i] = tempValue; + i--; + if (i == -1) i = 1; + } + else + { + i++; + } + } + //Skim off negative values + while (_queryResultCount > 0 && _querySortKeys[_queryResultCount - 1] < 0) + _queryResultCount--; + } + + //Now work through the rest of the segment + for (; ; ) + { + float xProgress = 0; + float yProgress = 0; + if (xIndex < 0 || xIndex >= _proxyCount * 2) + break; + if (yIndex < 0 || yIndex >= _proxyCount * 2) + break; + if (sx != 0) + { + //Move on to the next bound + if (sx > 0) + { + xIndex++; + if (xIndex == _proxyCount * 2) + break; + } + else + { + xIndex--; + if (xIndex < 0) + break; + } + xProgress = (_bounds[0][xIndex].Value - p1x) / dx; + } + if (sy != 0) + { + //Move on to the next bound + if (sy > 0) + { + yIndex++; + if (yIndex == _proxyCount * 2) + break; + } + else + { + yIndex--; + if (yIndex < 0) + break; + } + yProgress = (_bounds[1][yIndex].Value - p1y) / dy; + } + for (; ; ) + { + if (sy == 0 || (sx != 0 && xProgress < yProgress)) + { + if (xProgress > maxLambda) + break; + + //Check that we are entering a proxy, not leaving + if (sx > 0 ? _bounds[0][xIndex].IsLower : _bounds[0][xIndex].IsUpper) + { + //Check the other axis of the proxy + proxyId = _bounds[0][xIndex].ProxyId; + proxy = _proxyPool[proxyId]; + if (sy >= 0) + { + if (proxy.LowerBounds[1] <= yIndex - 1 && proxy.UpperBounds[1] >= yIndex) + { + //Add the proxy + if (sortKey != null) + { + AddProxyResult(proxyId, proxy, maxCount, sortKey); + } + else + { + _queryResults[_queryResultCount] = proxyId; + ++_queryResultCount; + } + } + } + else + { + if (proxy.LowerBounds[1] <= yIndex && proxy.UpperBounds[1] >= yIndex + 1) + { + //Add the proxy + if (sortKey != null) + { + AddProxyResult(proxyId, proxy, maxCount, sortKey); + } + else + { + _queryResults[_queryResultCount] = proxyId; + ++_queryResultCount; + } + } + } + } + + //Early out + if (sortKey != null && _queryResultCount == maxCount && _queryResultCount > 0 && xProgress > _querySortKeys[_queryResultCount - 1]) + break; + + //Move on to the next bound + if (sx > 0) + { + xIndex++; + if (xIndex == _proxyCount * 2) + break; + } + else + { + xIndex--; + if (xIndex < 0) + break; + } + xProgress = (_bounds[0][xIndex].Value - p1x) / dx; + } + else + { + if (yProgress > maxLambda) + break; + + //Check that we are entering a proxy, not leaving + if (sy > 0 ? _bounds[1][yIndex].IsLower : _bounds[1][yIndex].IsUpper) + { + //Check the other axis of the proxy + proxyId = _bounds[1][yIndex].ProxyId; + proxy = _proxyPool[proxyId]; + if (sx >= 0) + { + if (proxy.LowerBounds[0] <= xIndex - 1 && proxy.UpperBounds[0] >= xIndex) + { + //Add the proxy + if (sortKey != null) + { + AddProxyResult(proxyId, proxy, maxCount, sortKey); + } + else + { + _queryResults[_queryResultCount] = proxyId; + ++_queryResultCount; + } + } + } + else + { + if (proxy.LowerBounds[0] <= xIndex && proxy.UpperBounds[0] >= xIndex + 1) + { + //Add the proxy + if (sortKey != null) + { + AddProxyResult(proxyId, proxy, maxCount, sortKey); + } + else + { + _queryResults[_queryResultCount] = proxyId; + ++_queryResultCount; + } + } + } + } + + //Early out + if (sortKey != null && _queryResultCount == maxCount && _queryResultCount > 0 && yProgress > _querySortKeys[_queryResultCount - 1]) + break; + + //Move on to the next bound + if (sy > 0) + { + yIndex++; + if (yIndex == _proxyCount * 2) + break; + } + else + { + yIndex--; + if (yIndex < 0) + break; + } + yProgress = (_bounds[1][yIndex].Value - p1y) / dy; + } + } + + break; + } + + int count = 0; + for (int i = 0; i < _queryResultCount && count < maxCount; ++i, ++count) + { + Box2DXDebug.Assert(_queryResults[i] < Settings.MaxProxies); + Proxy proxy_ = _proxyPool[_queryResults[i]]; + Box2DXDebug.Assert(proxy_.IsValid); + userData[i] = proxy_.UserData; + } + + // Prepare for next query. + _queryResultCount = 0; + IncrementTimeStamp(); + + return count; + } + + public void Validate() + { + for (int axis = 0; axis < 2; ++axis) + { + Bound[] bounds = _bounds[axis]; + + int boundCount = 2 * _proxyCount; + ushort stabbingCount = 0; + + for (int i = 0; i < boundCount; ++i) + { + Bound bound = bounds[i]; + Box2DXDebug.Assert(i == 0 || bounds[i - 1].Value <= bound.Value); + Box2DXDebug.Assert(bound.ProxyId != PairManager.NullProxy); + Box2DXDebug.Assert(_proxyPool[bound.ProxyId].IsValid); + + if (bound.IsLower == true) + { + Box2DXDebug.Assert(_proxyPool[bound.ProxyId].LowerBounds[axis] == i); + ++stabbingCount; + } + else + { + Box2DXDebug.Assert(_proxyPool[bound.ProxyId].UpperBounds[axis] == i); + --stabbingCount; + } + + Box2DXDebug.Assert(bound.StabbingCount == stabbingCount); + } + } + } + + private void ComputeBounds(out ushort[] lowerValues, out ushort[] upperValues, AABB aabb) + { + lowerValues = new ushort[2]; + upperValues = new ushort[2]; + + Box2DXDebug.Assert(aabb.UpperBound.X >= aabb.LowerBound.X); + Box2DXDebug.Assert(aabb.UpperBound.Y >= aabb.LowerBound.Y); + + Vec2 minVertex = Common.Math.Clamp(aabb.LowerBound, _worldAABB.LowerBound, _worldAABB.UpperBound); + Vec2 maxVertex = Common.Math.Clamp(aabb.UpperBound, _worldAABB.LowerBound, _worldAABB.UpperBound); + + // Bump lower bounds downs and upper bounds up. This ensures correct sorting of + // lower/upper bounds that would have equal values. + // TODO_ERIN implement fast float to uint16 conversion. + lowerValues[0] = (ushort)((ushort)(_quantizationFactor.X * (minVertex.X - _worldAABB.LowerBound.X)) & (BROADPHASE_MAX - 1)); + upperValues[0] = (ushort)((ushort)(_quantizationFactor.X * (maxVertex.X - _worldAABB.LowerBound.X)) | 1); + + lowerValues[1] = (ushort)((ushort)(_quantizationFactor.Y * (minVertex.Y - _worldAABB.LowerBound.Y)) & (BROADPHASE_MAX - 1)); + upperValues[1] = (ushort)((ushort)(_quantizationFactor.Y * (maxVertex.Y - _worldAABB.LowerBound.Y)) | 1); + } + + // This one is only used for validation. + internal bool TestOverlap(Proxy p1, Proxy p2) + { + for (int axis = 0; axis < 2; ++axis) + { + Bound[] bounds = _bounds[axis]; + + Box2DXDebug.Assert(p1.LowerBounds[axis] < 2 * _proxyCount); + Box2DXDebug.Assert(p1.UpperBounds[axis] < 2 * _proxyCount); + Box2DXDebug.Assert(p2.LowerBounds[axis] < 2 * _proxyCount); + Box2DXDebug.Assert(p2.UpperBounds[axis] < 2 * _proxyCount); + + if (bounds[p1.LowerBounds[axis]].Value > bounds[p2.UpperBounds[axis]].Value) + return false; + + if (bounds[p1.UpperBounds[axis]].Value < bounds[p2.LowerBounds[axis]].Value) + return false; + } + + return true; + } + + internal bool TestOverlap(BoundValues b, Proxy p) + { + for (int axis = 0; axis < 2; ++axis) + { + Bound[] bounds = _bounds[axis]; + + Box2DXDebug.Assert(p.LowerBounds[axis] < 2 * _proxyCount); + Box2DXDebug.Assert(p.UpperBounds[axis] < 2 * _proxyCount); + + if (b.LowerValues[axis] > bounds[p.UpperBounds[axis]].Value) + return false; + + if (b.UpperValues[axis] < bounds[p.LowerBounds[axis]].Value) + return false; + } + + return true; + } + + private void Query(out int lowerQueryOut, out int upperQueryOut, + ushort lowerValue, ushort upperValue, + Bound[] bounds, int boundCount, int axis) + { + int lowerQuery = BinarySearch(bounds, boundCount, lowerValue); + int upperQuery = BinarySearch(bounds, boundCount, upperValue); + + // Easy case: lowerQuery <= lowerIndex(i) < upperQuery + // Solution: search query range for min bounds. + for (int i = lowerQuery; i < upperQuery; ++i) + { + if (bounds[i].IsLower) + { + IncrementOverlapCount(bounds[i].ProxyId); + } + } + + // Hard case: lowerIndex(i) < lowerQuery < upperIndex(i) + // Solution: use the stabbing count to search down the bound array. + if (lowerQuery > 0) + { + int i = lowerQuery - 1; + int s = bounds[i].StabbingCount; + + // Find the s overlaps. + while (s != 0) + { + Box2DXDebug.Assert(i >= 0); + + if (bounds[i].IsLower) + { + Proxy proxy = _proxyPool[bounds[i].ProxyId]; + if (lowerQuery <= proxy.UpperBounds[axis]) + { + IncrementOverlapCount(bounds[i].ProxyId); + --s; + } + } + --i; + } + } + + lowerQueryOut = lowerQuery; + upperQueryOut = upperQuery; + } + int qi1 = 0; + int qi2 = 0; + private void IncrementOverlapCount(int proxyId) + { + Proxy proxy = _proxyPool[proxyId]; + if (proxy.TimeStamp < _timeStamp) + { + proxy.TimeStamp = _timeStamp; + proxy.OverlapCount = 1; + qi1++; + } + else + { + proxy.OverlapCount = 2; + Box2DXDebug.Assert(_queryResultCount < Settings.MaxProxies); + _queryResults[_queryResultCount] = (ushort)proxyId; + ++_queryResultCount; + qi2++; + } + } + + private void IncrementTimeStamp() + { + if (_timeStamp == BROADPHASE_MAX) + { + for (ushort i = 0; i < Settings.MaxProxies; ++i) + { + _proxyPool[i].TimeStamp = 0; + } + _timeStamp = 1; + } + else + { + ++_timeStamp; + } + } + +#if ALLOWUNSAFE + public unsafe void AddProxyResult(ushort proxyId, Proxy proxy, int maxCount, SortKeyFunc sortKey) + { + float key = sortKey(proxy.UserData); + //Filter proxies on positive keys + if (key < 0) + return; + //Merge the new key into the sorted list. + //float32* p = std::lower_bound(m_querySortKeys,m_querySortKeys+m_queryResultCount,key); + fixed (float* querySortKeysPtr = _querySortKeys) + { + float* p = querySortKeysPtr; + while (*p < key && p < &querySortKeysPtr[_queryResultCount]) + p++; + int i = (int)(p - &querySortKeysPtr[0]); + if (maxCount == _queryResultCount && i == _queryResultCount) + return; + if (maxCount == _queryResultCount) + _queryResultCount--; + //std::copy_backward + for (int j = _queryResultCount + 1; j > i; --j) + { + _querySortKeys[j] = _querySortKeys[j - 1]; + _queryResults[j] = _queryResults[j - 1]; + } + _querySortKeys[i] = key; + _queryResults[i] = proxyId; + _queryResultCount++; + } + } +#else + public void AddProxyResult(ushort proxyId, Proxy proxy, int maxCount, SortKeyFunc sortKey) + { + float key = sortKey(proxy.UserData); + //Filter proxies on positive keys + if (key < 0) + return; + //Merge the new key into the sorted list. + //float32* p = std::lower_bound(m_querySortKeys,m_querySortKeys+m_queryResultCount,key); + float[] querySortKeysPtr = _querySortKeys; + + int ip = 0; + float p = querySortKeysPtr[ip]; + while (p < key && ip < _queryResultCount) + { + p = querySortKeysPtr[ip]; + ip++; + } + int i = ip; + if (maxCount == _queryResultCount && i == _queryResultCount) + return; + if (maxCount == _queryResultCount) + _queryResultCount--; + //std::copy_backward + for (int j = _queryResultCount + 1; j > i; --j) + { + _querySortKeys[j] = _querySortKeys[j - 1]; + _queryResults[j] = _queryResults[j - 1]; + } + _querySortKeys[i] = key; + _queryResults[i] = proxyId; + _queryResultCount++; + } +#endif + + private static int BinarySearch(Bound[] bounds, int count, ushort value) + { + int low = 0; + int high = count - 1; + while (low <= high) + { + int mid = (low + high) >> 1; + if (bounds[mid].Value > value) + { + high = mid - 1; + } + else if (bounds[mid].Value < value) + { + low = mid + 1; + } + else + { + return (ushort)mid; + } + } + + return low; + } + } +} diff --git a/Box2d/Assets/Program/Box2d/Rope/Rope.cs.meta b/Box2d/Assets/Program/Box2d/Collision/BroadPhase.cs.meta index fa9b631..efd4471 100644 --- a/Box2d/Assets/Program/Box2d/Rope/Rope.cs.meta +++ b/Box2d/Assets/Program/Box2d/Collision/BroadPhase.cs.meta @@ -1,5 +1,5 @@ fileFormatVersion: 2 -guid: d56353be4c8ccf942941c4bd085c71ba +guid: 507394c33d29e434193da81ba5fe878a MonoImporter: externalObjects: {} serializedVersion: 2 diff --git a/Box2d/Assets/Program/Box2d/Collision/Collision.CollideCircle.cs b/Box2d/Assets/Program/Box2d/Collision/Collision.CollideCircle.cs new file mode 100644 index 0000000..fa10a9e --- /dev/null +++ b/Box2d/Assets/Program/Box2d/Collision/Collision.CollideCircle.cs @@ -0,0 +1,155 @@ +/* + Box2DX Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx + Box2D original C++ version Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this 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. +*/ + +using Box2DX.Common; + +namespace Box2DX.Collision +{ + public partial class Collision + { + public static void CollideCircles(ref Manifold manifold, + CircleShape circle1, XForm xf1, CircleShape circle2, XForm xf2) + { + manifold.PointCount = 0; + + Vec2 p1 = Common.Math.Mul(xf1, circle1._position); + Vec2 p2 = Common.Math.Mul(xf2, circle2._position); + + Vec2 d = p2 - p1; + float distSqr = Vec2.Dot(d, d); + float radius = circle1._radius + circle2._radius; + if (distSqr > radius * radius) + { + return; + } + + manifold.Type = ManifoldType.Circles; + manifold.LocalPoint = circle1._position; + manifold.LocalPlaneNormal.SetZero(); + manifold.PointCount = 1; + + manifold.Points[0].LocalPoint = circle2._position; + manifold.Points[0].ID.Key = 0; + } + + public static void CollidePolygonAndCircle(ref Manifold manifold, + PolygonShape polygon, XForm xf1, CircleShape circle, XForm xf2) + { + manifold.PointCount = 0; + + // Compute circle position in the frame of the polygon. + Vec2 c = Common.Math.Mul(xf2, circle._position); + Vec2 cLocal = Common.Math.MulT(xf1, c); + + // Find the min separating edge. + int normalIndex = 0; + float separation = -Settings.FLT_MAX; + float radius = polygon._radius + circle._radius; + int vertexCount = polygon._vertexCount; + Vec2[] vertices = polygon._vertices; + Vec2[] normals = polygon._normals; + + for (int i = 0; i < vertexCount; ++i) + { + float s = Vec2.Dot(normals[i], cLocal - vertices[i]); + if (s > radius) + { + // Early out. + return; + } + + if (s > separation) + { + separation = s; + normalIndex = i; + } + } + + // Vertices that subtend the incident face. + int vertIndex1 = normalIndex; + int vertIndex2 = vertIndex1 + 1 < vertexCount ? vertIndex1 + 1 : 0; + Vec2 v1 = vertices[vertIndex1]; + Vec2 v2 = vertices[vertIndex2]; + + // If the center is inside the polygon ... + if (separation < Common.Settings.FLT_EPSILON) + { + manifold.PointCount = 1; + manifold.Type = ManifoldType.FaceA; + manifold.LocalPlaneNormal = normals[normalIndex]; + manifold.LocalPoint = 0.5f * (v1 + v2); + manifold.Points[0].LocalPoint = circle._position; + manifold.Points[0].ID.Key = 0; + return; + } + + // Compute barycentric coordinates + float u1 = Vec2.Dot(cLocal - v1, v2 - v1); + float u2 = Vec2.Dot(cLocal - v2, v1 - v2); + if (u1 <= 0.0f) + { + if (Vec2.DistanceSquared(cLocal, v1) > radius * radius) + { + return; + } + + manifold.PointCount = 1; + manifold.Type = ManifoldType.FaceA; + manifold.LocalPlaneNormal = cLocal - v1; + manifold.LocalPlaneNormal.Normalize(); + manifold.LocalPoint = v1; + manifold.Points[0].LocalPoint = circle._position; + manifold.Points[0].ID.Key = 0; + } + else if (u2 <= 0.0f) + { + if (Vec2.DistanceSquared(cLocal, v2) > radius * radius) + { + return; + } + + manifold.PointCount = 1; + manifold.Type = ManifoldType.FaceA; + manifold.LocalPlaneNormal = cLocal - v2; + manifold.LocalPlaneNormal.Normalize(); + manifold.LocalPoint = v2; + manifold.Points[0].LocalPoint = circle._position; + manifold.Points[0].ID.Key = 0; + } + else + { + Vec2 faceCenter = 0.5f * (v1 + v2); + float separation_ = Vec2.Dot(cLocal - faceCenter, normals[vertIndex1]); + if (separation_ > radius) + { + return; + } + + manifold.PointCount = 1; + manifold.Type = ManifoldType.FaceA; + manifold.LocalPlaneNormal = normals[vertIndex1]; + manifold.LocalPoint = faceCenter; + manifold.Points[0].LocalPoint = circle._position; + manifold.Points[0].ID.Key = 0; + } + } + } +}
\ No newline at end of file diff --git a/Box2d/Assets/Program/Box2d/Common/Transform.cs.meta b/Box2d/Assets/Program/Box2d/Collision/Collision.CollideCircle.cs.meta index d8b844b..4d53a38 100644 --- a/Box2d/Assets/Program/Box2d/Common/Transform.cs.meta +++ b/Box2d/Assets/Program/Box2d/Collision/Collision.CollideCircle.cs.meta @@ -1,5 +1,5 @@ fileFormatVersion: 2 -guid: 0ad1c5e9a9ce05e40995e4c863ba9adc +guid: 71b3a2ec035f35d47b5bcc3a724a5907 MonoImporter: externalObjects: {} serializedVersion: 2 diff --git a/Box2d/Assets/Program/Box2d/Collision/Collision.CollideEdge.cs b/Box2d/Assets/Program/Box2d/Collision/Collision.CollideEdge.cs new file mode 100644 index 0000000..8ea3821 --- /dev/null +++ b/Box2d/Assets/Program/Box2d/Collision/Collision.CollideEdge.cs @@ -0,0 +1,100 @@ +/* + Box2DX Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx + Box2D original C++ version Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this 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. +*/ + +using Box2DX.Common; + +namespace Box2DX.Collision +{ + public partial class Collision + { + // This implements 2-sided edge vs circle collision. + public static void CollideEdgeAndCircle(ref Manifold manifold, EdgeShape edge, XForm transformA, CircleShape circle, XForm transformB) + { + manifold.PointCount = 0; + Vec2 cLocal = Common.Math.MulT(transformA, Common.Math.Mul(transformB, circle._position)); + Vec2 normal = edge._normal; + Vec2 v1 = edge._v1; + Vec2 v2 = edge._v2; + float radius = edge._radius + circle._radius; + + // Barycentric coordinates + float u1 = Vec2.Dot(cLocal - v1, v2 - v1); + float u2 = Vec2.Dot(cLocal - v2, v1 - v2); + + if (u1 <= 0.0f) + { + // Behind v1 + if (Vec2.DistanceSquared(cLocal, v1) > radius * radius) + { + return; + } + + manifold.PointCount = 1; + manifold.Type = ManifoldType.FaceA; + manifold.LocalPlaneNormal = cLocal - v1; + manifold.LocalPlaneNormal.Normalize(); + manifold.LocalPoint = v1; + manifold.Points[0].LocalPoint = circle._position; + manifold.Points[0].ID.Key = 0; + } + else if (u2 <= 0.0f) + { + // Ahead of v2 + if (Vec2.DistanceSquared(cLocal, v2) > radius * radius) + { + return; + } + + manifold.PointCount = 1; + manifold.Type = ManifoldType.FaceA; + manifold.LocalPlaneNormal = cLocal - v2; + manifold.LocalPlaneNormal.Normalize(); + manifold.LocalPoint = v2; + manifold.Points[0].LocalPoint = circle._position; + manifold.Points[0].ID.Key = 0; + } + else + { + float separation = Vec2.Dot(cLocal - v1, normal); + if (separation < -radius || radius < separation) + { + return; + } + + manifold.PointCount = 1; + manifold.Type = ManifoldType.FaceA; + manifold.LocalPlaneNormal = separation < 0.0f ? -normal : normal; + manifold.LocalPoint = 0.5f * (v1 + v2); + manifold.Points[0].LocalPoint = circle._position; + manifold.Points[0].ID.Key = 0; + } + } + + // Polygon versus 2-sided edge. + public static void CollidePolyAndEdge(ref Manifold manifold, PolygonShape polygon, XForm transformA, EdgeShape edge, XForm transformB) + { + PolygonShape polygonB = new PolygonShape(); + polygonB.SetAsEdge(edge._v1, edge._v2); + + CollidePolygons(ref manifold, polygon, transformA, polygonB, transformB); + } + } +}
\ No newline at end of file diff --git a/Box2d/Assets/Program/Box2d/Collision/Collision.CollideEdge.cs.meta b/Box2d/Assets/Program/Box2d/Collision/Collision.CollideEdge.cs.meta new file mode 100644 index 0000000..b71db55 --- /dev/null +++ b/Box2d/Assets/Program/Box2d/Collision/Collision.CollideEdge.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: b683cd8505628a247bde0d512af54021 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Box2d/Assets/Program/Box2d/Collision/Collision.CollidePoly.cs b/Box2d/Assets/Program/Box2d/Collision/Collision.CollidePoly.cs new file mode 100644 index 0000000..65c48e4 --- /dev/null +++ b/Box2d/Assets/Program/Box2d/Collision/Collision.CollidePoly.cs @@ -0,0 +1,309 @@ +/* + Box2DX Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx + Box2D original C++ version Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this 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. +*/ + +using Box2DX.Common; + +namespace Box2DX.Collision +{ + public partial class Collision + { + /// <summary> + /// Find the separation between poly1 and poly2 for a give edge normal on poly1. + /// </summary> + public static float EdgeSeparation(PolygonShape poly1, XForm xf1, int edge1, PolygonShape poly2, XForm xf2) + { + int count1 = poly1._vertexCount; + Vec2[] vertices1 = poly1._vertices; + Vec2[] normals1 = poly1._normals; + + int count2 = poly2._vertexCount; + Vec2[] vertices2 = poly2._vertices; + + Box2DXDebug.Assert(0 <= edge1 && edge1 < count1); + + // Convert normal from poly1's frame into poly2's frame. + Vec2 normal1World = Common.Math.Mul(xf1.R, normals1[edge1]); + Vec2 normal1 = Common.Math.MulT(xf2.R, normal1World); + + // Find support vertex on poly2 for -normal. + int index = 0; + float minDot = Common.Settings.FLT_MAX; + for (int i = 0; i < count2; ++i) + { + float dot = Vec2.Dot(vertices2[i], normal1); + if (dot < minDot) + { + minDot = dot; + index = i; + } + } + + Vec2 v1 = Common.Math.Mul(xf1, vertices1[edge1]); + Vec2 v2 = Common.Math.Mul(xf2, vertices2[index]); + float separation = Vec2.Dot(v2 - v1, normal1World); + return separation; + } + + /// <summary> + /// Find the max separation between poly1 and poly2 using edge normals from poly1. + /// </summary> + public static float FindMaxSeparation(ref int edgeIndex, PolygonShape poly1, XForm xf1, PolygonShape poly2, XForm xf2) + { + int count1 = poly1._vertexCount; + Vec2[] normals1 = poly1._normals; + + // Vector pointing from the centroid of poly1 to the centroid of poly2. + Vec2 d = Common.Math.Mul(xf2, poly2._centroid) - Common.Math.Mul(xf1, poly1._centroid); + Vec2 dLocal1 = Common.Math.MulT(xf1.R, d); + + // Find edge normal on poly1 that has the largest projection onto d. + int edge = 0; + float maxDot = -Common.Settings.FLT_MAX; + for (int i = 0; i < count1; ++i) + { + float dot = Vec2.Dot(normals1[i], dLocal1); + if (dot > maxDot) + { + maxDot = dot; + edge = i; + } + } + + // Get the separation for the edge normal. + float s = Collision.EdgeSeparation(poly1, xf1, edge, poly2, xf2); + + // Check the separation for the previous edge normal. + int prevEdge = edge - 1 >= 0 ? edge - 1 : count1 - 1; + float sPrev = Collision.EdgeSeparation(poly1, xf1, prevEdge, poly2, xf2); + + // Check the separation for the next edge normal. + int nextEdge = edge + 1 < count1 ? edge + 1 : 0; + float sNext = Collision.EdgeSeparation(poly1, xf1, nextEdge, poly2, xf2); + + // Find the best edge and the search direction. + int bestEdge; + float bestSeparation; + int increment; + if (sPrev > s && sPrev > sNext) + { + increment = -1; + bestEdge = prevEdge; + bestSeparation = sPrev; + } + else if (sNext > s) + { + increment = 1; + bestEdge = nextEdge; + bestSeparation = sNext; + } + else + { + edgeIndex = edge; + return s; + } + + // Perform a local search for the best edge normal. + for (; ; ) + { + if (increment == -1) + edge = bestEdge - 1 >= 0 ? bestEdge - 1 : count1 - 1; + else + edge = bestEdge + 1 < count1 ? bestEdge + 1 : 0; + + s = Collision.EdgeSeparation(poly1, xf1, edge, poly2, xf2); + + if (s > bestSeparation) + { + bestEdge = edge; + bestSeparation = s; + } + else + { + break; + } + } + + edgeIndex = bestEdge; + return bestSeparation; + } + + public static void FindIncidentEdge(out ClipVertex[] c, + PolygonShape poly1, XForm xf1, int edge1, PolygonShape poly2, XForm xf2) + { + int count1 = poly1._vertexCount; + Vec2[] normals1 = poly1._normals; + + int count2 = poly2._vertexCount; + Vec2[] vertices2 = poly2._vertices; + Vec2[] normals2 = poly2._normals; + + Box2DXDebug.Assert(0 <= edge1 && edge1 < count1); + + // Get the normal of the reference edge in poly2's frame. + Vec2 normal1 = Common.Math.MulT(xf2.R, Common.Math.Mul(xf1.R, normals1[edge1])); + + // Find the incident edge on poly2. + int index = 0; + float minDot = Settings.FLT_MAX; + for (int i = 0; i < count2; ++i) + { + float dot = Vec2.Dot(normal1, normals2[i]); + if (dot < minDot) + { + minDot = dot; + index = i; + } + } + + // Build the clip vertices for the incident edge. + int i1 = index; + int i2 = i1 + 1 < count2 ? i1 + 1 : 0; + + c = new ClipVertex[2]; + + c[0].V = Common.Math.Mul(xf2, vertices2[i1]); + c[0].ID.Features.ReferenceEdge = (byte)edge1; + c[0].ID.Features.IncidentEdge = (byte)i1; + c[0].ID.Features.IncidentVertex = 0; + + c[1].V = Common.Math.Mul(xf2, vertices2[i2]); + c[1].ID.Features.ReferenceEdge = (byte)edge1; + c[1].ID.Features.IncidentEdge = (byte)i2; + c[1].ID.Features.IncidentVertex = 1; + } + + // Find edge normal of max separation on A - return if separating axis is found + // Find edge normal of max separation on B - return if separation axis is found + // Choose reference edge as min(minA, minB) + // Find incident edge + // Clip + // The normal points from 1 to 2 + public static void CollidePolygons(ref Manifold manifold, + PolygonShape polyA, XForm xfA, PolygonShape polyB, XForm xfB) + { + manifold.PointCount = 0; + float totalRadius = polyA._radius + polyB._radius; + + int edgeA = 0; + float separationA = Collision.FindMaxSeparation(ref edgeA, polyA, xfA, polyB, xfB); + if (separationA > totalRadius) + return; + + int edgeB = 0; + float separationB = Collision.FindMaxSeparation(ref edgeB, polyB, xfB, polyA, xfA); + if (separationB > totalRadius) + return; + + PolygonShape poly1; // reference poly + PolygonShape poly2; // incident poly + XForm xf1, xf2; + int edge1; // reference edge + byte flip; + const float k_relativeTol = 0.98f; + const float k_absoluteTol = 0.001f; + + if (separationB > k_relativeTol * separationA + k_absoluteTol) + { + poly1 = polyB; + poly2 = polyA; + xf1 = xfB; + xf2 = xfA; + edge1 = edgeB; + manifold.Type = ManifoldType.FaceB; + flip = 1; + } + else + { + poly1 = polyA; + poly2 = polyB; + xf1 = xfA; + xf2 = xfB; + edge1 = edgeA; + manifold.Type = ManifoldType.FaceA; + flip = 0; + } + + ClipVertex[] incidentEdge; + Collision.FindIncidentEdge(out incidentEdge, poly1, xf1, edge1, poly2, xf2); + + int count1 = poly1._vertexCount; + Vec2[] vertices1 = poly1._vertices; + + Vec2 v11 = vertices1[edge1]; + Vec2 v12 = edge1 + 1 < count1 ? vertices1[edge1 + 1] : vertices1[0]; + + Vec2 dv = v12 - v11; + + Vec2 localNormal = Vec2.Cross(dv, 1.0f); + localNormal.Normalize(); + Vec2 planePoint = 0.5f * (v11 + v12); + + Vec2 sideNormal = Common.Math.Mul(xf1.R, v12 - v11); + sideNormal.Normalize(); + Vec2 frontNormal = Vec2.Cross(sideNormal, 1.0f); + + v11 = Common.Math.Mul(xf1, v11); + v12 = Common.Math.Mul(xf1, v12); + + float frontOffset = Vec2.Dot(frontNormal, v11); + float sideOffset1 = -Vec2.Dot(sideNormal, v11); + float sideOffset2 = Vec2.Dot(sideNormal, v12); + + // Clip incident edge against extruded edge1 side edges. + ClipVertex[] clipPoints1; + ClipVertex[] clipPoints2; + int np; + + // Clip to box side 1 + np = Collision.ClipSegmentToLine(out clipPoints1, incidentEdge, -sideNormal, sideOffset1); + + if (np < 2) + return; + + // Clip to negative box side 1 + np = ClipSegmentToLine(out clipPoints2, clipPoints1, sideNormal, sideOffset2); + + if (np < 2) + return; + + // Now clipPoints2 contains the clipped points. + manifold.LocalPlaneNormal = localNormal; + manifold.LocalPoint = planePoint; + + int pointCount = 0; + for (int i = 0; i < Settings.MaxManifoldPoints; ++i) + { + float separation = Vec2.Dot(frontNormal, clipPoints2[i].V) - frontOffset; + + if (separation <= totalRadius) + { + ManifoldPoint cp = manifold.Points[pointCount]; + cp.LocalPoint = Common.Math.MulT(xf2, clipPoints2[i].V); + cp.ID = clipPoints2[i].ID; + cp.ID.Features.Flip = flip; + ++pointCount; + } + } + + manifold.PointCount = pointCount; + } + } +}
\ No newline at end of file diff --git a/Box2d/Assets/Program/Box2d/Collision/Collision.CollidePoly.cs.meta b/Box2d/Assets/Program/Box2d/Collision/Collision.CollidePoly.cs.meta new file mode 100644 index 0000000..f206015 --- /dev/null +++ b/Box2d/Assets/Program/Box2d/Collision/Collision.CollidePoly.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: ff780a3060f9b7c48a616f7935751bb0 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Box2d/Assets/Program/Box2d/Collision/Collision.Distance.cs b/Box2d/Assets/Program/Box2d/Collision/Collision.Distance.cs new file mode 100644 index 0000000..d6abeb6 --- /dev/null +++ b/Box2d/Assets/Program/Box2d/Collision/Collision.Distance.cs @@ -0,0 +1,630 @@ +/* + Box2DX Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx + Box2D original C++ version Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this 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. +*/ + +#define DEBUG + +using System; +using Box2DX.Common; + +namespace Box2DX.Collision +{ + /// <summary> + /// Used to warm start Distance. + /// Set count to zero on first call. + /// </summary> + public struct SimplexCache + { + /// <summary> + /// Length or area. + /// </summary> + public Single Metric; + public UInt16 Count; + /// <summary> + /// Vertices on shape A. + /// </summary> + //public Byte[/*3*/] IndexA; + public IndexArray IndexA; + /// <summary> + /// Vertices on shape B. + /// </summary> + //public Byte[/*3*/] IndexB; + public IndexArray IndexB; + + //public SimplexCache(byte init) + //{ + // Metric = 0; + // Count = 0; + // IndexA = new Byte[3]; + // IndexB = new Byte[3]; + //} + } + + public struct IndexArray + { + private Byte I0, I1, I2; + + public Byte this[int index] + { + get + { +#if DEBUG + Box2DXDebug.Assert(index >= 0 && index < 3); +#endif + if (index == 0) return I0; + else if (index == 1) return I1; + else return I2; + } + set + { +#if DEBUG + Box2DXDebug.Assert(index >= 0 && index < 3); +#endif + if (index == 0) I0 = value; + else if (index == 1) I1 = value; + else I2 = value; + } + } + } + + /// <summary> + /// Input for Distance. + /// You have to option to use the shape radii + /// in the computation. + /// </summary> + public struct DistanceInput + { + public XForm TransformA; + public XForm TransformB; + public bool UseRadii; + } + + /// <summary> + /// Output for Distance. + /// </summary> + public struct DistanceOutput + { + /// <summary> + /// Closest point on shapeA. + /// </summary> + public Vec2 PointA; + /// <summary> + /// Closest point on shapeB. + /// </summary> + public Vec2 PointB; + public float Distance; + /// <summary> + /// Number of GJK iterations used. + /// </summary> + public int Iterations; + } + + // GJK using Voronoi regions (Christer Ericson) and Barycentric coordinates. + + internal struct SimplexVertex + { + internal Vec2 wA; // support point in shapeA + internal Vec2 wB; // support point in shapeB + internal Vec2 w; // wB - wA + internal float a; // barycentric coordinate for closest point + internal int indexA; // wA index + internal int indexB; // wB index + } + + internal struct Simplex + { + internal SimplexVertex _v1, _v2, _v3; + internal int _count; + + internal unsafe void ReadCache(SimplexCache* cache, Shape shapeA, XForm transformA, Shape shapeB, XForm transformB) + { + Box2DXDebug.Assert(0 <= cache->Count && cache->Count <= 3); + + // Copy data from cache. + _count = cache->Count; + SimplexVertex** vertices = stackalloc SimplexVertex*[3]; + fixed (SimplexVertex* v1Ptr = &_v1, v2Ptr = &_v2, v3Ptr = &_v3) + { + vertices[0] = v1Ptr; + vertices[1] = v2Ptr; + vertices[2] = v3Ptr; + for (int i = 0; i < _count; ++i) + { + SimplexVertex* v = vertices[i]; + v->indexA = cache->IndexA[i]; + v->indexB = cache->IndexB[i]; + Vec2 wALocal = shapeA.GetVertex(v->indexA); + Vec2 wBLocal = shapeB.GetVertex(v->indexB); + v->wA = Common.Math.Mul(transformA, wALocal); + v->wB = Common.Math.Mul(transformB, wBLocal); + v->w = v->wB - v->wA; + v->a = 0.0f; + } + + // Compute the new simplex metric, if it is substantially different than + // old metric then flush the simplex. + if (_count > 1) + { + float metric1 = cache->Metric; + float metric2 = GetMetric(); + if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < Common.Settings.FLT_EPSILON) + { + // Reset the simplex. + _count = 0; + } + } + + // If the cache is empty or invalid ... + if (_count == 0) + { + SimplexVertex* v = vertices[0]; + v->indexA = 0; + v->indexB = 0; + Vec2 wALocal = shapeA.GetVertex(0); + Vec2 wBLocal = shapeB.GetVertex(0); + v->wA = Common.Math.Mul(transformA, wALocal); + v->wB = Common.Math.Mul(transformB, wBLocal); + v->w = v->wB - v->wA; + _count = 1; + } + } + } + + internal unsafe void WriteCache(SimplexCache* cache) + { + cache->Metric = GetMetric(); + cache->Count = (UInt16)_count; + SimplexVertex** vertices = stackalloc SimplexVertex*[3]; + fixed (SimplexVertex* v1Ptr = &_v1, v2Ptr = &_v2, v3Ptr = &_v3) + { + vertices[0] = v1Ptr; + vertices[1] = v2Ptr; + vertices[2] = v3Ptr; + for (int i = 0; i < _count; ++i) + { + cache->IndexA[i] = (Byte)(vertices[i]->indexA); + cache->IndexB[i] = (Byte)(vertices[i]->indexB); + } + } + } + + internal Vec2 GetClosestPoint() + { + switch (_count) + { + case 0: +#if DEBUG + Box2DXDebug.Assert(false); +#endif + return Vec2.Zero; + case 1: + return _v1.w; + case 2: + return _v1.a * _v1.w + _v2.a * _v2.w; + case 3: + return Vec2.Zero; + default: +#if DEBUG + Box2DXDebug.Assert(false); +#endif + return Vec2.Zero; + } + } + + internal unsafe void GetWitnessPoints(Vec2* pA, Vec2* pB) + { + switch (_count) + { + case 0: + Box2DXDebug.Assert(false); + break; + + case 1: + *pA = _v1.wA; + *pB = _v1.wB; + break; + + case 2: + *pA = _v1.a * _v1.wA + _v2.a * _v2.wA; + *pB = _v1.a * _v1.wB + _v2.a * _v2.wB; + break; + + case 3: + *pA = _v1.a * _v1.wA + _v2.a * _v2.wA + _v3.a * _v3.wA; + *pB = *pA; + break; + + default: + Box2DXDebug.Assert(false); + break; + } + } + + internal float GetMetric() + { + switch (_count) + { + case 0: +#if DEBUG + Box2DXDebug.Assert(false); +#endif + return 0.0f; + + case 1: + return 0.0f; + + case 2: + return Vec2.Distance(_v1.w, _v2.w); + + case 3: + return Vec2.Cross(_v2.w - _v1.w, _v3.w - _v1.w); + + default: +#if DEBUG + Box2DXDebug.Assert(false); +#endif + return 0.0f; + } + } + + // Solve a line segment using barycentric coordinates. + // + // p = a1 * w1 + a2 * w2 + // a1 + a2 = 1 + // + // The vector from the origin to the closest point on the line is + // perpendicular to the line. + // e12 = w2 - w1 + // dot(p, e) = 0 + // a1 * dot(w1, e) + a2 * dot(w2, e) = 0 + // + // 2-by-2 linear system + // [1 1 ][a1] = [1] + // [w1.e12 w2.e12][a2] = [0] + // + // Define + // d12_1 = dot(w2, e12) + // d12_2 = -dot(w1, e12) + // d12 = d12_1 + d12_2 + // + // Solution + // a1 = d12_1 / d12 + // a2 = d12_2 / d12 + internal void Solve2() + { + Vec2 w1 = _v1.w; + Vec2 w2 = _v2.w; + Vec2 e12 = w2 - w1; + + // w1 region + float d12_2 = -Vec2.Dot(w1, e12); + if (d12_2 <= 0.0f) + { + // a2 <= 0, so we clamp it to 0 + _v1.a = 1.0f; + _count = 1; + return; + } + + // w2 region + float d12_1 = Vec2.Dot(w2, e12); + if (d12_1 <= 0.0f) + { + // a1 <= 0, so we clamp it to 0 + _v2.a = 1.0f; + _count = 1; + _v1 = _v2; + return; + } + + // Must be in e12 region. + float inv_d12 = 1.0f / (d12_1 + d12_2); + _v1.a = d12_1 * inv_d12; + _v2.a = d12_2 * inv_d12; + _count = 2; + } + + // Possible regions: + // - points[2] + // - edge points[0]-points[2] + // - edge points[1]-points[2] + // - inside the triangle + internal void Solve3() + { + Vec2 w1 = _v1.w; + Vec2 w2 = _v2.w; + Vec2 w3 = _v3.w; + + // Edge12 + // [1 1 ][a1] = [1] + // [w1.e12 w2.e12][a2] = [0] + // a3 = 0 + Vec2 e12 = w2 - w1; + float w1e12 = Vec2.Dot(w1, e12); + float w2e12 = Vec2.Dot(w2, e12); + float d12_1 = w2e12; + float d12_2 = -w1e12; + + // Edge13 + // [1 1 ][a1] = [1] + // [w1.e13 w3.e13][a3] = [0] + // a2 = 0 + Vec2 e13 = w3 - w1; + float w1e13 = Vec2.Dot(w1, e13); + float w3e13 = Vec2.Dot(w3, e13); + float d13_1 = w3e13; + float d13_2 = -w1e13; + + // Edge23 + // [1 1 ][a2] = [1] + // [w2.e23 w3.e23][a3] = [0] + // a1 = 0 + Vec2 e23 = w3 - w2; + float w2e23 = Vec2.Dot(w2, e23); + float w3e23 = Vec2.Dot(w3, e23); + float d23_1 = w3e23; + float d23_2 = -w2e23; + + // Triangle123 + float n123 = Vec2.Cross(e12, e13); + + float d123_1 = n123 * Vec2.Cross(w2, w3); + float d123_2 = n123 * Vec2.Cross(w3, w1); + float d123_3 = n123 * Vec2.Cross(w1, w2); + + // w1 region + if (d12_2 <= 0.0f && d13_2 <= 0.0f) + { + _v1.a = 1.0f; + _count = 1; + return; + } + + // e12 + if (d12_1 > 0.0f && d12_2 > 0.0f && d123_3 <= 0.0f) + { + float inv_d12 = 1.0f / (d12_1 + d12_2); + _v1.a = d12_1 * inv_d12; + _v2.a = d12_1 * inv_d12; + _count = 2; + return; + } + + // e13 + if (d13_1 > 0.0f && d13_2 > 0.0f && d123_2 <= 0.0f) + { + float inv_d13 = 1.0f / (d13_1 + d13_2); + _v1.a = d13_1 * inv_d13; + _v3.a = d13_2 * inv_d13; + _count = 2; + _v2 = _v3; + return; + } + + // w2 region + if (d12_1 <= 0.0f && d23_2 <= 0.0f) + { + _v2.a = 1.0f; + _count = 1; + _v1 = _v2; + return; + } + + // w3 region + if (d13_1 <= 0.0f && d23_1 <= 0.0f) + { + _v3.a = 1.0f; + _count = 1; + _v1 = _v3; + return; + } + + // e23 + if (d23_1 > 0.0f && d23_2 > 0.0f && d123_1 <= 0.0f) + { + float inv_d23 = 1.0f / (d23_1 + d23_2); + _v2.a = d23_1 * inv_d23; + _v3.a = d23_2 * inv_d23; + _count = 2; + _v1 = _v3; + return; + } + + // Must be in triangle123 + float inv_d123 = 1.0f / (d123_1 + d123_2 + d123_3); + _v1.a = d123_1 * inv_d123; + _v2.a = d123_2 * inv_d123; + _v3.a = d123_3 * inv_d123; + _count = 3; + } + } + + public partial class Collision + { + /// <summary> + /// Compute the closest points between two shapes. Supports any combination of: + /// CircleShape, PolygonShape, EdgeShape. The simplex cache is input/output. + /// On the first call set SimplexCache.Count to zero. + /// </summary> + public unsafe static void Distance(out DistanceOutput output, ref SimplexCache cache, ref DistanceInput input, Shape shapeA, Shape shapeB) + { + output = new DistanceOutput(); + + XForm transformA = input.TransformA; + XForm transformB = input.TransformB; + + // Initialize the simplex. + Simplex simplex = new Simplex(); + fixed (SimplexCache* sPtr = &cache) + { + simplex.ReadCache(sPtr, shapeA, transformA, shapeB, transformB); + } + + // Get simplex vertices as an array. + SimplexVertex* vertices = &simplex._v1; + + // These store the vertices of the last simplex so that we + // can check for duplicates and prevent cycling. + int* lastA = stackalloc int[4], lastB = stackalloc int[4]; + int lastCount; + + // Main iteration loop. + int iter = 0; + const int k_maxIterationCount = 20; + while (iter < k_maxIterationCount) + { + // Copy simplex so we can identify duplicates. + lastCount = simplex._count; + int i; + for (i = 0; i < lastCount; ++i) + { + lastA[i] = vertices[i].indexA; + lastB[i] = vertices[i].indexB; + } + + switch (simplex._count) + { + case 1: + break; + + case 2: + simplex.Solve2(); + break; + + case 3: + simplex.Solve3(); + break; + + default: +#if DEBUG + Box2DXDebug.Assert(false); +#endif + break; + } + + // If we have 3 points, then the origin is in the corresponding triangle. + if (simplex._count == 3) + { + break; + } + + // Compute closest point. + Vec2 p = simplex.GetClosestPoint(); + float distanceSqr = p.LengthSquared(); + + // Ensure the search direction is numerically fit. + if (distanceSqr < Common.Settings.FLT_EPSILON_SQUARED) + { + // The origin is probably contained by a line segment + // or triangle. Thus the shapes are overlapped. + + // We can't return zero here even though there may be overlap. + // In case the simplex is a point, segment, or triangle it is difficult + // to determine if the origin is contained in the CSO or very close to it. + break; + } + + // Compute a tentative new simplex vertex using support points. + SimplexVertex* vertex = vertices + simplex._count; + vertex->indexA = shapeA.GetSupport(Common.Math.MulT(transformA.R, p)); + vertex->wA = Common.Math.Mul(transformA, shapeA.GetVertex(vertex->indexA)); + //Vec2 wBLocal; + vertex->indexB = shapeB.GetSupport(Common.Math.MulT(transformB.R, -p)); + vertex->wB = Common.Math.Mul(transformB, shapeB.GetVertex(vertex->indexB)); + vertex->w = vertex->wB - vertex->wA; + + // Iteration count is equated to the number of support point calls. + ++iter; + + // Check for convergence. + float lowerBound = Vec2.Dot(p, vertex->w); + float upperBound = distanceSqr; + const float k_relativeTolSqr = 0.01f * 0.01f; // 1:100 + if (upperBound - lowerBound <= k_relativeTolSqr * upperBound) + { + // Converged! + break; + } + + // Check for duplicate support points. + bool duplicate = false; + for (i = 0; i < lastCount; ++i) + { + if (vertex->indexA == lastA[i] && vertex->indexB == lastB[i]) + { + duplicate = true; + break; + } + } + + // If we found a duplicate support point we must exit to avoid cycling. + if (duplicate) + { + break; + } + + // New vertex is ok and needed. + ++simplex._count; + } + + + fixed (DistanceOutput* doPtr = &output) + { + // Prepare output. + simplex.GetWitnessPoints(&doPtr->PointA, &doPtr->PointB); + doPtr->Distance = Vec2.Distance(doPtr->PointA, doPtr->PointB); + doPtr->Iterations = iter; + } + + fixed (SimplexCache* sPtr = &cache) + { + // Cache the simplex. + simplex.WriteCache(sPtr); + } + + // Apply radii if requested. + if (input.UseRadii) + { + float rA = shapeA._radius; + float rB = shapeB._radius; + + if (output.Distance > rA + rB && output.Distance > Common.Settings.FLT_EPSILON) + { + // Shapes are still no overlapped. + // Move the witness points to the outer surface. + output.Distance -= rA + rB; + Vec2 normal = output.PointB - output.PointA; + normal.Normalize(); + output.PointA += rA * normal; + output.PointB -= rB * normal; + } + else + { + // Shapes are overlapped when radii are considered. + // Move the witness points to the middle. + Vec2 p = 0.5f * (output.PointA + output.PointB); + output.PointA = p; + output.PointB = p; + output.Distance = 0.0f; + } + } + } + } +}
\ No newline at end of file diff --git a/Box2d/Assets/Program/Box2d/Collision/Collision.Distance.cs.meta b/Box2d/Assets/Program/Box2d/Collision/Collision.Distance.cs.meta new file mode 100644 index 0000000..3d28a57 --- /dev/null +++ b/Box2d/Assets/Program/Box2d/Collision/Collision.Distance.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 5656ed372fda8334c9f0ff84dc0922fe +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Box2d/Assets/Program/Box2d/Collision/Collision.TimeOfImpact.cs b/Box2d/Assets/Program/Box2d/Collision/Collision.TimeOfImpact.cs new file mode 100644 index 0000000..b59f19b --- /dev/null +++ b/Box2d/Assets/Program/Box2d/Collision/Collision.TimeOfImpact.cs @@ -0,0 +1,378 @@ +/* + Box2DX Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx + Box2D original C++ version Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this 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. +*/ + +using Box2DX.Common; + +namespace Box2DX.Collision +{ + /// <summary> + /// Inpute parameters for TimeOfImpact + /// </summary> + public struct TOIInput + { + public Sweep SweepA; + public Sweep SweepB; + public float SweepRadiusA; + public float SweepRadiusB; + public float Tolerance; + } + + internal struct SeparationFunction + { + internal enum Type + { + Points, + FaceA, + FaceB + }; + + internal unsafe void Initialize(SimplexCache* cache, + Shape shapeA, XForm transformA, + Shape shapeB, XForm transformB) + { + ShapeA = shapeA; + ShapeB = shapeB; + int count = cache->Count; + Box2DXDebug.Assert(0 < count && count < 3); + + if (count == 1) + { + FaceType = Type.Points; + Vec2 localPointA = ShapeA.GetVertex(cache->IndexA[0]); + Vec2 localPointB = ShapeB.GetVertex(cache->IndexB[0]); + Vec2 pointA = Common.Math.Mul(transformA, localPointA); + Vec2 pointB = Common.Math.Mul(transformB, localPointB); + Axis = pointB - pointA; + Axis.Normalize(); + } + else if (cache->IndexB[0] == cache->IndexB[1]) + { + // Two points on A and one on B + FaceType = Type.FaceA; + Vec2 localPointA1 = ShapeA.GetVertex(cache->IndexA[0]); + Vec2 localPointA2 = ShapeA.GetVertex(cache->IndexA[1]); + Vec2 localPointB = ShapeB.GetVertex(cache->IndexB[0]); + LocalPoint = 0.5f * (localPointA1 + localPointA2); + Axis = Vec2.Cross(localPointA2 - localPointA1, 1.0f); + Axis.Normalize(); + + Vec2 normal = Common.Math.Mul(transformA.R, Axis); + Vec2 pointA = Common.Math.Mul(transformA, LocalPoint); + Vec2 pointB = Common.Math.Mul(transformB, localPointB); + + float s = Vec2.Dot(pointB - pointA, normal); + if (s < 0.0f) + { + Axis = -Axis; + } + } + else + { + // Two points on B and one or two points on A. + // We ignore the second point on A. + FaceType = Type.FaceB; + Vec2 localPointA = shapeA.GetVertex(cache->IndexA[0]); + Vec2 localPointB1 = shapeB.GetVertex(cache->IndexB[0]); + Vec2 localPointB2 = shapeB.GetVertex(cache->IndexB[1]); + LocalPoint = 0.5f * (localPointB1 + localPointB2); + Axis = Vec2.Cross(localPointB2 - localPointB1, 1.0f); + Axis.Normalize(); + + Vec2 normal = Common.Math.Mul(transformB.R, Axis); + Vec2 pointB = Common.Math.Mul(transformB, LocalPoint); + Vec2 pointA = Common.Math.Mul(transformA, localPointA); + + float s = Vec2.Dot(pointA - pointB, normal); + if (s < 0.0f) + { + Axis = -Axis; + } + } + } + + internal float Evaluate(XForm transformA, XForm transformB) + { + switch (FaceType) + { + case Type.Points: + { + Vec2 axisA = Common.Math.MulT(transformA.R, Axis); + Vec2 axisB = Common.Math.MulT(transformB.R, -Axis); + Vec2 localPointA = ShapeA.GetSupportVertex(axisA); + Vec2 localPointB = ShapeB.GetSupportVertex(axisB); + Vec2 pointA = Common.Math.Mul(transformA, localPointA); + Vec2 pointB = Common.Math.Mul(transformB, localPointB); + float separation = Vec2.Dot(pointB - pointA, Axis); + return separation; + } + + case Type.FaceA: + { + Vec2 normal = Common.Math.Mul(transformA.R, Axis); + Vec2 pointA = Common.Math.Mul(transformA, LocalPoint); + + Vec2 axisB = Common.Math.MulT(transformB.R, -normal); + + Vec2 localPointB = ShapeB.GetSupportVertex(axisB); + Vec2 pointB = Common.Math.Mul(transformB, localPointB); + + float separation = Vec2.Dot(pointB - pointA, normal); + return separation; + } + + case Type.FaceB: + { + Vec2 normal = Common.Math.Mul(transformB.R, Axis); + Vec2 pointB = Common.Math.Mul(transformB, LocalPoint); + + Vec2 axisA = Common.Math.MulT(transformA.R, -normal); + + Vec2 localPointA = ShapeA.GetSupportVertex(axisA); + Vec2 pointA = Common.Math.Mul(transformA, localPointA); + + float separation = Vec2.Dot(pointA - pointB, normal); + return separation; + } + + default: + Box2DXDebug.Assert(false); + return 0.0f; + } + } + + internal Shape ShapeA; + internal Shape ShapeB; + internal Type FaceType; + internal Vec2 LocalPoint; + internal Vec2 Axis; + } + + public partial class Collision + { + public static int MaxToiIters; + public static int MaxToiRootIters; + + // CCD via the secant method. + /// <summary> + /// Compute the time when two shapes begin to touch or touch at a closer distance. + /// TOI considers the shape radii. It attempts to have the radii overlap by the tolerance. + /// Iterations terminate with the overlap is within 0.5 * tolerance. The tolerance should be + /// smaller than sum of the shape radii. + /// Warning the sweeps must have the same time interval. + /// </summary> + /// <returns> + /// The fraction between [0,1] in which the shapes first touch. + /// fraction=0 means the shapes begin touching/overlapped, and fraction=1 means the shapes don't touch. + /// </returns> + public static float TimeOfImpact(TOIInput input, Shape shapeA, Shape shapeB) + { + Sweep sweepA = input.SweepA; + Sweep sweepB = input.SweepB; + + Box2DXDebug.Assert(sweepA.T0 == sweepB.T0); + Box2DXDebug.Assert(1.0f - sweepA.T0 > Common.Settings.FLT_EPSILON); + + float radius = shapeA._radius + shapeB._radius; + float tolerance = input.Tolerance; + + float alpha = 0.0f; + + const int k_maxIterations = 1000; // TODO_ERIN b2Settings + int iter = 0; + float target = 0.0f; + + // Prepare input for distance query. + SimplexCache cache = new SimplexCache(); + cache.Count = 0; + DistanceInput distanceInput; + distanceInput.UseRadii = false; + + for (; ; ) + { + XForm xfA, xfB; + sweepA.GetTransform(out xfA, alpha); + sweepB.GetTransform(out xfB, alpha); + + // Get the distance between shapes. + distanceInput.TransformA = xfA; + distanceInput.TransformB = xfB; + DistanceOutput distanceOutput; + Distance(out distanceOutput, ref cache, ref distanceInput, shapeA, shapeB); + + if (distanceOutput.Distance <= 0.0f) + { + alpha = 1.0f; + break; + } + + SeparationFunction fcn = new SeparationFunction(); + unsafe + { + fcn.Initialize(&cache, shapeA, xfA, shapeB, xfB); + } + + float separation = fcn.Evaluate(xfA, xfB); + if (separation <= 0.0f) + { + alpha = 1.0f; + break; + } + + if (iter == 0) + { + // Compute a reasonable target distance to give some breathing room + // for conservative advancement. We take advantage of the shape radii + // to create additional clearance. + if (separation > radius) + { + target = Common.Math.Max(radius - tolerance, 0.75f * radius); + } + else + { + target = Common.Math.Max(separation - tolerance, 0.02f * radius); + } + } + + if (separation - target < 0.5f * tolerance) + { + if (iter == 0) + { + alpha = 1.0f; + break; + } + + break; + } + +#if _FALSE + // Dump the curve seen by the root finder + { + const int32 N = 100; + float32 dx = 1.0f / N; + float32 xs[N+1]; + float32 fs[N+1]; + + float32 x = 0.0f; + + for (int32 i = 0; i <= N; ++i) + { + sweepA.GetTransform(&xfA, x); + sweepB.GetTransform(&xfB, x); + float32 f = fcn.Evaluate(xfA, xfB) - target; + + printf("%g %g\n", x, f); + + xs[i] = x; + fs[i] = f; + + x += dx; + } + } +#endif + + // Compute 1D root of: f(x) - target = 0 + float newAlpha = alpha; + { + float x1 = alpha, x2 = 1.0f; + + float f1 = separation; + + sweepA.GetTransform(out xfA, x2); + sweepB.GetTransform(out xfB, x2); + float f2 = fcn.Evaluate(xfA, xfB); + + // If intervals don't overlap at t2, then we are done. + if (f2 >= target) + { + alpha = 1.0f; + break; + } + + // Determine when intervals intersect. + int rootIterCount = 0; + for (; ; ) + { + // Use a mix of the secant rule and bisection. + float x; + if ((rootIterCount & 1) != 0) + { + // Secant rule to improve convergence. + x = x1 + (target - f1) * (x2 - x1) / (f2 - f1); + } + else + { + // Bisection to guarantee progress. + x = 0.5f * (x1 + x2); + } + + sweepA.GetTransform(out xfA, x); + sweepB.GetTransform(out xfB, x); + + float f = fcn.Evaluate(xfA, xfB); + + if (Common.Math.Abs(f - target) < 0.025f * tolerance) + { + newAlpha = x; + break; + } + + // Ensure we continue to bracket the root. + if (f > target) + { + x1 = x; + f1 = f; + } + else + { + x2 = x; + f2 = f; + } + + ++rootIterCount; + + Box2DXDebug.Assert(rootIterCount < 50); + } + + MaxToiRootIters = Common.Math.Max(MaxToiRootIters, rootIterCount); + } + + // Ensure significant advancement. + if (newAlpha < (1.0f + 100.0f * Common.Settings.FLT_EPSILON) * alpha) + { + break; + } + + alpha = newAlpha; + + ++iter; + + if (iter == k_maxIterations) + { + break; + } + } + + MaxToiIters = Common.Math.Max(MaxToiIters, iter); + + return alpha; + } + } +}
\ No newline at end of file diff --git a/Box2d/Assets/Program/Box2d/Collision/Collision.TimeOfImpact.cs.meta b/Box2d/Assets/Program/Box2d/Collision/Collision.TimeOfImpact.cs.meta new file mode 100644 index 0000000..146887c --- /dev/null +++ b/Box2d/Assets/Program/Box2d/Collision/Collision.TimeOfImpact.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: d06765dfbd7e9c940b64509b22111f19 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Box2d/Assets/Program/Box2d/Collision/Collision.cs b/Box2d/Assets/Program/Box2d/Collision/Collision.cs index 85ae672..e77e23b 100644 --- a/Box2d/Assets/Program/Box2d/Collision/Collision.cs +++ b/Box2d/Assets/Program/Box2d/Collision/Collision.cs @@ -1,7 +1,623 @@ -using System.Collections; -using System.Collections.Generic; +/* + Box2DX Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx + Box2D original C++ version Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com -namespace Box2D + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this 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. +*/ + +using System; +using Box2DX.Common; + +namespace Box2DX.Collision { + // Structures and functions used for computing contact points, distance + // queries, and TOI queries. + + public partial class Collision + { + public static readonly byte NullFeature = Common.Math.UCHAR_MAX; + + public static bool TestOverlap(AABB a, AABB b) + { + Vec2 d1, d2; + d1 = b.LowerBound - a.UpperBound; + d2 = a.LowerBound - b.UpperBound; + + if (d1.X > 0.0f || d1.Y > 0.0f) + return false; + + if (d2.X > 0.0f || d2.Y > 0.0f) + return false; + + return true; + } + + /// <summary> + /// Compute the point states given two manifolds. The states pertain to the transition from manifold1 + /// to manifold2. So state1 is either persist or remove while state2 is either add or persist. + /// </summary> + public static void GetPointStates(PointState[/*b2_maxManifoldPoints*/] state1, PointState[/*b2_maxManifoldPoints*/] state2, + Manifold manifold1, Manifold manifold2) + { + for (int i = 0; i < Common.Settings.MaxManifoldPoints; ++i) + { + state1[i] = PointState.NullState; + state2[i] = PointState.NullState; + } + + // Detect persists and removes. + for (int i = 0; i < manifold1.PointCount; ++i) + { + ContactID id = manifold1.Points[i].ID; + + state1[i] = PointState.RemoveState; + + for (int j = 0; j < manifold2.PointCount; ++j) + { + if (manifold2.Points[j].ID.Key == id.Key) + { + state1[i] = PointState.PersistState; + break; + } + } + } + + // Detect persists and adds. + for (int i = 0; i < manifold2.PointCount; ++i) + { + ContactID id = manifold2.Points[i].ID; + + state2[i] = PointState.AddState; + + for (int j = 0; j < manifold1.PointCount; ++j) + { + if (manifold1.Points[j].ID.Key == id.Key) + { + state2[i] = PointState.PersistState; + break; + } + } + } + } + + // Sutherland-Hodgman clipping. + public static int ClipSegmentToLine(out ClipVertex[/*2*/] vOut, ClipVertex[/*2*/] vIn, Vec2 normal, float offset) + { + vOut = new ClipVertex[2]; + + // Start with no output points + int numOut = 0; + + // Calculate the distance of end points to the line + float distance0 = Vec2.Dot(normal, vIn[0].V) - offset; + float distance1 = Vec2.Dot(normal, vIn[1].V) - offset; + + // If the points are behind the plane + if (distance0 <= 0.0f) vOut[numOut++] = vIn[0]; + if (distance1 <= 0.0f) vOut[numOut++] = vIn[1]; + + // If the points are on different sides of the plane + if (distance0 * distance1 < 0.0f) + { + // Find intersection point of edge and plane + float interp = distance0 / (distance0 - distance1); + vOut[numOut].V = vIn[0].V + interp * (vIn[1].V - vIn[0].V); + if (distance0 > 0.0f) + { + vOut[numOut].ID = vIn[0].ID; + } + else + { + vOut[numOut].ID = vIn[1].ID; + } + ++numOut; + } + + return numOut; + } + } + + /// <summary> + /// The features that intersect to form the contact point. + /// </summary> + public struct Features + { + /// <summary> + /// The edge that defines the outward contact normal. + /// </summary> + public Byte ReferenceEdge; + + /// <summary> + /// The edge most anti-parallel to the reference edge. + /// </summary> + public Byte IncidentEdge; + + /// <summary> + /// The vertex (0 or 1) on the incident edge that was clipped. + /// </summary> + public Byte IncidentVertex; + + /// <summary> + /// A value of 1 indicates that the reference edge is on shape2. + /// </summary> + public Byte Flip; + } + + /// <summary> + /// Contact ids to facilitate warm starting. + /// </summary> + [System.Runtime.InteropServices.StructLayout(System.Runtime.InteropServices.LayoutKind.Explicit)] + public struct ContactID + { + [System.Runtime.InteropServices.FieldOffset(0)] + public Features Features; + + /// <summary> + /// Used to quickly compare contact ids. + /// </summary> + [System.Runtime.InteropServices.FieldOffset(0)] + public UInt32 Key; + } + + /// <summary> + /// A manifold point is a contact point belonging to a contact + /// manifold. It holds details related to the geometry and dynamics + /// of the contact points. + /// The local point usage depends on the manifold type: + /// -Circles: the local center of circleB + /// -FaceA: the local center of cirlceB or the clip point of polygonB + /// -FaceB: the clip point of polygonA + /// This structure is stored across time steps, so we keep it small. + /// Note: the impulses are used for internal caching and may not + /// provide reliable contact forces, especially for high speed collisions. + /// </summary> + public class ManifoldPoint + { + /// <summary> + /// Usage depends on manifold type. + /// </summary> + public Vec2 LocalPoint; + + /// <summary> + /// The non-penetration impulse. + /// </summary> + public float NormalImpulse; + + /// <summary> + /// The friction impulse. + /// </summary> + public float TangentImpulse; + + /// <summary> + /// Uniquely identifies a contact point between two shapes. + /// </summary> + public ContactID ID; + + public ManifoldPoint Clone() + { + ManifoldPoint newPoint = new ManifoldPoint(); + newPoint.LocalPoint = this.LocalPoint; + newPoint.NormalImpulse = this.NormalImpulse; + newPoint.TangentImpulse = this.TangentImpulse; + newPoint.ID = this.ID; + return newPoint; + } + } + + public enum ManifoldType + { + Circles, + FaceA, + FaceB + } + + /// <summary> + /// A manifold for two touching convex shapes. + /// </summary> + public class Manifold + { + /// <summary> + /// The points of contact. + /// </summary> + public ManifoldPoint[/*Settings.MaxManifoldPoints*/] Points = new ManifoldPoint[Settings.MaxManifoldPoints]; + + public Vec2 LocalPlaneNormal; + + /// <summary> + /// Usage depends on manifold type. + /// </summary> + public Vec2 LocalPoint; + + public ManifoldType Type; + + /// <summary> + /// The number of manifold points. + /// </summary> + public int PointCount; + + public Manifold() + { + for (int i = 0; i < Settings.MaxManifoldPoints; i++) + Points[i] = new ManifoldPoint(); + } + + public Manifold Clone() + { + Manifold newManifold = new Manifold(); + newManifold.LocalPlaneNormal = this.LocalPlaneNormal; + newManifold.LocalPoint = this.LocalPoint; + newManifold.Type = this.Type; + newManifold.PointCount = this.PointCount; + int pointCount = this.Points.Length; + ManifoldPoint[] tmp = new ManifoldPoint[pointCount]; + for (int i = 0; i < pointCount; i++) + { + tmp[i] = this.Points[i].Clone(); + } + newManifold.Points = tmp; + return newManifold; + } + } + + /// <summary> + /// A line segment. + /// </summary> + public struct Segment + { + // Collision Detection in Interactive 3D Environments by Gino van den Bergen + // From Section 3.4.1 + // x = mu1 * p1 + mu2 * p2 + // mu1 + mu2 = 1 && mu1 >= 0 && mu2 >= 0 + // mu1 = 1 - mu2; + // x = (1 - mu2) * p1 + mu2 * p2 + // = p1 + mu2 * (p2 - p1) + // x = s + a * r (s := start, r := end - start) + // s + a * r = p1 + mu2 * d (d := p2 - p1) + // -a * r + mu2 * d = b (b := s - p1) + // [-r d] * [a; mu2] = b + // Cramer's rule: + // denom = det[-r d] + // a = det[b d] / denom + // mu2 = det[-r b] / denom + /// <summary> + /// Ray cast against this segment with another segment. + /// </summary> + public bool TestSegment(out float lambda, out Vec2 normal, Segment segment, float maxLambda) + { + lambda = 0f; + normal = new Vec2(); + + Vec2 s = segment.P1; + Vec2 r = segment.P2 - s; + Vec2 d = P2 - P1; + Vec2 n = Vec2.Cross(d, 1.0f); + + float k_slop = 100.0f * Common.Settings.FLT_EPSILON; + float denom = -Vec2.Dot(r, n); + + // Cull back facing collision and ignore parallel segments. + if (denom > k_slop) + { + // Does the segment intersect the infinite line associated with this segment? + Vec2 b = s - P1; + float a = Vec2.Dot(b, n); + + if (0.0f <= a && a <= maxLambda * denom) + { + float mu2 = -r.X * b.Y + r.Y * b.X; + + // Does the segment intersect this segment? + if (-k_slop * denom <= mu2 && mu2 <= denom * (1.0f + k_slop)) + { + a /= denom; + n.Normalize(); + lambda = a; + normal = n; + return true; + } + } + } + + return false; + } + + /// <summary> + /// The starting point. + /// </summary> + public Vec2 P1; + + /// <summary> + /// The ending point. + /// </summary> + public Vec2 P2; + } + + /// <summary> + /// An axis aligned bounding box. + /// </summary> + public struct AABB + { + /// <summary> + /// The lower vertex. + /// </summary> + public Vec2 LowerBound; + + /// <summary> + /// The upper vertex. + /// </summary> + public Vec2 UpperBound; + + /// Verify that the bounds are sorted. + public bool IsValid + { + get + { + Vec2 d = UpperBound - LowerBound; + bool valid = d.X >= 0.0f && d.Y >= 0.0f; + valid = valid && LowerBound.IsValid && UpperBound.IsValid; + return valid; + } + } + + /// Get the center of the AABB. + public Vec2 Center + { + get { return 0.5f * (LowerBound + UpperBound); } + } + + /// Get the extents of the AABB (half-widths). + public Vec2 Extents + { + get { return 0.5f * (UpperBound - LowerBound); } + } + + /// Combine two AABBs into this one. + public void Combine(AABB aabb1, AABB aabb2) + { + LowerBound = Common.Math.Min(aabb1.LowerBound, aabb2.LowerBound); + UpperBound = Common.Math.Max(aabb1.UpperBound, aabb2.UpperBound); + } + + /// Does this aabb contain the provided AABB. + public bool Contains(AABB aabb) + { + bool 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; + } + + /// <summary> + // From Real-time Collision Detection, p179. + /// </summary> + public void RayCast(out RayCastOutput output, RayCastInput input) + { + float tmin = -Common.Settings.FLT_MAX; + float tmax = Common.Settings.FLT_MAX; + + output = new RayCastOutput(); + + output.Hit = false; + + Vec2 p = input.P1; + Vec2 d = input.P2 - input.P1; + Vec2 absD = Common.Math.Abs(d); + + Vec2 normal = new Vec2(0); + + for (int i = 0; i < 2; ++i) + { + if (absD[i] < Common.Settings.FLT_EPSILON) + { + // Parallel. + if (p[i] < LowerBound[i] || UpperBound[i] < p[i]) + { + return; + } + } + else + { + float inv_d = 1.0f / d[i]; + float t1 = (LowerBound[i] - p[i]) * inv_d; + float t2 = (UpperBound[i] - p[i]) * inv_d; + + // Sign of the normal vector. + float s = -1.0f; + + if (t1 > t2) + { + Common.Math.Swap(ref t1, ref t2); + s = 1.0f; + } + + // Push the min up + if (t1 > tmin) + { + normal.SetZero(); + normal[i] = s; + tmin = t1; + } + + // Pull the max down + tmax = Common.Math.Min(tmax, t2); + + if (tmin > tmax) + { + return; + } + } + } + + // Does the ray start inside the box? + // Does the ray intersect beyond the max fraction? + if (tmin < 0.0f || input.MaxFraction < tmin) + { + return; + } + + // Intersection. + output.Fraction = tmin; + output.Normal = normal; + output.Hit = true; + } + } + + /// <summary> + /// This is used for determining the state of contact points. + /// </summary> + public enum PointState + { + /// <summary> + /// Point does not exist. + /// </summary> + NullState, + /// <summary> + /// Point was added in the update. + /// </summary> + AddState, + /// <summary> + /// Point persisted across the update. + /// </summary> + PersistState, + /// <summary> + ///Point was removed in the update. + /// </summary> + RemoveState + } + + /// <summary> + /// This is used to compute the current state of a contact manifold. + /// </summary> + public class WorldManifold + { + /// <summary> + /// World vector pointing from A to B. + /// </summary> + public Vec2 Normal; + /// <summary> + /// World contact point (point of intersection). + /// </summary> + public Vec2[] Points = new Vec2[Common.Settings.MaxManifoldPoints]; + + public WorldManifold Clone() + { + WorldManifold newManifold = new WorldManifold(); + newManifold.Normal = this.Normal; + this.Points.CopyTo(newManifold.Points, 0); + return newManifold; + } + + /// Evaluate the manifold with supplied transforms. This assumes + /// modest motion from the original state. This does not change the + /// point count, impulses, etc. The radii must come from the shapes + /// that generated the manifold. + public void Initialize(Manifold manifold, XForm xfA, float radiusA, XForm xfB, float radiusB) + { + if (manifold.PointCount == 0) + { + return; + } + + switch (manifold.Type) + { + case ManifoldType.Circles: + { + Vec2 pointA = Common.Math.Mul(xfA, manifold.LocalPoint); + Vec2 pointB = Common.Math.Mul(xfB, manifold.Points[0].LocalPoint); + Vec2 normal = new Vec2(1.0f, 0.0f); + if (Vec2.DistanceSquared(pointA, pointB) > Common.Settings.FLT_EPSILON_SQUARED) + { + normal = pointB - pointA; + normal.Normalize(); + } + + Normal = normal; + + Vec2 cA = pointA + radiusA * normal; + Vec2 cB = pointB - radiusB * normal; + Points[0] = 0.5f * (cA + cB); + } + break; + + case ManifoldType.FaceA: + { + Vec2 normal = Common.Math.Mul(xfA.R, manifold.LocalPlaneNormal); + Vec2 planePoint = Common.Math.Mul(xfA, manifold.LocalPoint); + + // Ensure normal points from A to B. + Normal = normal; + + for (int i = 0; i < manifold.PointCount; ++i) + { + Vec2 clipPoint = Common.Math.Mul(xfB, manifold.Points[i].LocalPoint); + Vec2 cA = clipPoint + (radiusA - Vec2.Dot(clipPoint - planePoint, normal)) * normal; + Vec2 cB = clipPoint - radiusB * normal; + Points[i] = 0.5f * (cA + cB); + } + } + break; + + case ManifoldType.FaceB: + { + Vec2 normal = Common.Math.Mul(xfB.R, manifold.LocalPlaneNormal); + Vec2 planePoint = Common.Math.Mul(xfB, manifold.LocalPoint); + + // Ensure normal points from A to B. + Normal = -normal; + + for (int i = 0; i < manifold.PointCount; ++i) + { + Vec2 clipPoint = Common.Math.Mul(xfA, manifold.Points[i].LocalPoint); + Vec2 cA = clipPoint - radiusA * normal; + Vec2 cB = clipPoint + (radiusB - Vec2.Dot(clipPoint - planePoint, normal)) * normal; + Points[i] = 0.5f * (cA + cB); + } + } + break; + } + } + } + + /// <summary> + /// Used for computing contact manifolds. + /// </summary> + public struct ClipVertex + { + public Vec2 V; + public ContactID ID; + } + + /// <summary> + /// Ray-cast input data. + /// </summary> + public struct RayCastInput + { + public Vec2 P1, P2; + public float MaxFraction; + } -} + /// <summary> + /// Ray-cast output data. + /// </summary> + public struct RayCastOutput + { + public Vec2 Normal; + public float Fraction; + public bool Hit; + } +}
\ No newline at end of file diff --git a/Box2d/Assets/Program/Box2d/Collision/PairManager.cs b/Box2d/Assets/Program/Box2d/Collision/PairManager.cs new file mode 100644 index 0000000..0fcb8ab --- /dev/null +++ b/Box2d/Assets/Program/Box2d/Collision/PairManager.cs @@ -0,0 +1,493 @@ +/* + Box2DX Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx + Box2D original C++ version Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this 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. +*/ + +// The pair manager is used by the broad-phase to quickly add/remove/find pairs +// of overlapping proxies. It is based closely on code provided by Pierre Terdiman. +// http://www.codercorner.com/IncrementalSAP.txt + +#define DEBUG + +using System; +using Box2DX.Common; + +namespace Box2DX.Collision +{ + public class Pair + { + [Flags] + public enum PairStatus + { + PairBuffered = 0x0001, + PairRemoved = 0x0002, + PairFinal = 0x0004 + } + + public void SetBuffered() { Status |= PairStatus.PairBuffered; } + public void ClearBuffered() { Status &= ~PairStatus.PairBuffered; } + public bool IsBuffered() { return (Status & PairStatus.PairBuffered) == PairStatus.PairBuffered; } + + public void SetRemoved() { Status |= PairStatus.PairRemoved; } + public void ClearRemoved() { Status &= ~PairStatus.PairRemoved; } + public bool IsRemoved() { return (Status & PairStatus.PairRemoved) == PairStatus.PairRemoved; } + + public void SetFinal() { Status |= PairStatus.PairFinal; } + public bool IsFinal() { return (Status & PairStatus.PairFinal) == PairStatus.PairFinal; } + + public object UserData; + public ushort ProxyId1; + public ushort ProxyId2; + public ushort Next; + public PairStatus Status; + } + + public struct BufferedPair + { + public ushort ProxyId1; + public ushort ProxyId2; + } + + public abstract class PairCallback + { + // This should return the new pair user data. It is ok if the + // user data is null. + public abstract object PairAdded(object proxyUserData1, object proxyUserData2); + + // This should free the pair's user data. In extreme circumstances, it is possible + // this will be called with null pairUserData because the pair never existed. + public abstract void PairRemoved(object proxyUserData1, object proxyUserData2, object pairUserData); + } + + public class PairManager + { + public static readonly ushort NullPair = Common.Math.USHRT_MAX; + public static readonly ushort NullProxy = Common.Math.USHRT_MAX; + public static readonly int TableCapacity = Settings.MaxPairs; // must be a power of two + public static readonly int TableMask = PairManager.TableCapacity - 1; + + public BroadPhase _broadPhase; + public PairCallback _callback; + public Pair[] _pairs = new Pair[Settings.MaxPairs]; + public ushort _freePair; + public int _pairCount; + + public BufferedPair[] _pairBuffer = new BufferedPair[Settings.MaxPairs]; + public int _pairBufferCount; + + public ushort[] _hashTable = new ushort[PairManager.TableCapacity]; + + public PairManager() + { + Box2DXDebug.Assert(Common.Math.IsPowerOfTwo((uint)PairManager.TableCapacity) == true); + Box2DXDebug.Assert(PairManager.TableCapacity >= Settings.MaxPairs); + for (int i = 0; i < PairManager.TableCapacity; ++i) + { + _hashTable[i] = PairManager.NullPair; + } + _freePair = 0; + for (int i = 0; i < Settings.MaxPairs; ++i) + { + _pairs[i] = new Pair();//todo: need some pool here + _pairs[i].ProxyId1 = PairManager.NullProxy; + _pairs[i].ProxyId2 = PairManager.NullProxy; + _pairs[i].UserData = null; + _pairs[i].Status = 0; + _pairs[i].Next = (ushort)(i + 1U); + } + _pairs[Settings.MaxPairs - 1].Next = PairManager.NullPair; + _pairCount = 0; + _pairBufferCount = 0; + } + + public void Initialize(BroadPhase broadPhase, PairCallback callback) + { + _broadPhase = broadPhase; + _callback = callback; + } + + /* + As proxies are created and moved, many pairs are created and destroyed. Even worse, the same + pair may be added and removed multiple times in a single time step of the physics engine. To reduce + traffic in the pair manager, we try to avoid destroying pairs in the pair manager until the + end of the physics step. This is done by buffering all the RemovePair requests. AddPair + requests are processed immediately because we need the hash table entry for quick lookup. + + All user user callbacks are delayed until the buffered pairs are confirmed in Commit. + This is very important because the user callbacks may be very expensive and client logic + may be harmed if pairs are added and removed within the same time step. + + Buffer a pair for addition. + We may add a pair that is not in the pair manager or pair buffer. + We may add a pair that is already in the pair manager and pair buffer. + If the added pair is not a new pair, then it must be in the pair buffer (because RemovePair was called). + */ + public void AddBufferedPair(int id1, int id2) + { + Box2DXDebug.Assert(id1 != PairManager.NullProxy && id2 != PairManager.NullProxy); + Box2DXDebug.Assert(_pairBufferCount < Settings.MaxPairs); + + Pair pair = AddPair(id1, id2); + + // If this pair is not in the pair buffer ... + if (pair.IsBuffered() == false) + { + // This must be a newly added pair. + Box2DXDebug.Assert(pair.IsFinal() == false); + + // Add it to the pair buffer. + pair.SetBuffered(); + _pairBuffer[_pairBufferCount].ProxyId1 = pair.ProxyId1; + _pairBuffer[_pairBufferCount].ProxyId2 = pair.ProxyId2; + ++_pairBufferCount; + + Box2DXDebug.Assert(_pairBufferCount <= _pairCount); + } + + // Confirm this pair for the subsequent call to Commit. + pair.ClearRemoved(); + + if (BroadPhase.IsValidate) + { + ValidateBuffer(); + } + } + + // Buffer a pair for removal. + public void RemoveBufferedPair(int id1, int id2) + { + Box2DXDebug.Assert(id1 != PairManager.NullProxy && id2 != PairManager.NullProxy); + Box2DXDebug.Assert(_pairBufferCount < Settings.MaxPairs); + + Pair pair = Find(id1, id2); + + if (pair == null) + { + // The pair never existed. This is legal (due to collision filtering). + return; + } + + // If this pair is not in the pair buffer ... + if (pair.IsBuffered() == false) + { + // This must be an old pair. + Box2DXDebug.Assert(pair.IsFinal() == true); + + pair.SetBuffered(); + _pairBuffer[_pairBufferCount].ProxyId1 = pair.ProxyId1; + _pairBuffer[_pairBufferCount].ProxyId2 = pair.ProxyId2; + ++_pairBufferCount; + + Box2DXDebug.Assert(_pairBufferCount <= _pairCount); + } + + pair.SetRemoved(); + + if (BroadPhase.IsValidate) + { + ValidateBuffer(); + } + } + + public void Commit() + { + int removeCount = 0; + + Proxy[] proxies = _broadPhase._proxyPool; + + for (int i = 0; i < _pairBufferCount; ++i) + { + Pair pair = Find(_pairBuffer[i].ProxyId1, _pairBuffer[i].ProxyId2); + Box2DXDebug.Assert(pair.IsBuffered()); + pair.ClearBuffered(); + + Box2DXDebug.Assert(pair.ProxyId1 < Settings.MaxProxies && pair.ProxyId2 < Settings.MaxProxies); + + Proxy proxy1 = proxies[pair.ProxyId1]; + Proxy proxy2 = proxies[pair.ProxyId2]; + + Box2DXDebug.Assert(proxy1.IsValid); + Box2DXDebug.Assert(proxy2.IsValid); + + if (pair.IsRemoved()) + { + // It is possible a pair was added then removed before a commit. Therefore, + // we should be careful not to tell the user the pair was removed when the + // the user didn't receive a matching add. + if (pair.IsFinal() == true) + { + _callback.PairRemoved(proxy1.UserData, proxy2.UserData, pair.UserData); + } + + // Store the ids so we can actually remove the pair below. + _pairBuffer[removeCount].ProxyId1 = pair.ProxyId1; + _pairBuffer[removeCount].ProxyId2 = pair.ProxyId2; + ++removeCount; + } + else + { + Box2DXDebug.Assert(_broadPhase.TestOverlap(proxy1, proxy2) == true); + + if (pair.IsFinal() == false) + { + pair.UserData = _callback.PairAdded(proxy1.UserData, proxy2.UserData); + pair.SetFinal(); + } + } + } + + for (int i = 0; i < removeCount; ++i) + { + RemovePair(_pairBuffer[i].ProxyId1, _pairBuffer[i].ProxyId2); + } + + _pairBufferCount = 0; + + if (BroadPhase.IsValidate) + { + ValidateTable(); + } + } + + private Pair Find(int proxyId1, int proxyId2) + { + if (proxyId1 > proxyId2) + Common.Math.Swap<int>(ref proxyId1, ref proxyId2); + + uint hash = (uint)(Hash((uint)proxyId1, (uint)proxyId2) & PairManager.TableMask); + + return Find(proxyId1, proxyId2, hash); + } + + private Pair Find(int proxyId1, int proxyId2, uint hash) + { + int index = _hashTable[hash]; + + while (index != PairManager.NullPair && Equals(_pairs[index], proxyId1, proxyId2) == false) + { + index = _pairs[index].Next; + } + + if (index == PairManager.NullPair) + { + return null; + } + + Box2DXDebug.Assert(index < Settings.MaxPairs); + + return _pairs[index]; + } + + // Returns existing pair or creates a new one. + private Pair AddPair(int proxyId1, int proxyId2) + { + if (proxyId1 > proxyId2) + Common.Math.Swap<int>(ref proxyId1, ref proxyId2); + + int hash = (int)(Hash((uint)proxyId1, (uint)proxyId2) & PairManager.TableMask); + + Pair pair = Find(proxyId1, proxyId2, (uint)hash); + if (pair != null) + { + return pair; + } + + Box2DXDebug.Assert(_pairCount < Settings.MaxPairs && _freePair != PairManager.NullPair); + + ushort pairIndex = _freePair; + pair = _pairs[pairIndex]; + _freePair = pair.Next; + + pair.ProxyId1 = (ushort)proxyId1; + pair.ProxyId2 = (ushort)proxyId2; + pair.Status = 0; + pair.UserData = null; + pair.Next = _hashTable[hash]; + + _hashTable[hash] = pairIndex; + + ++_pairCount; + + return pair; + } + + // Removes a pair. The pair must exist. + private object RemovePair(int proxyId1, int proxyId2) + { + Box2DXDebug.Assert(_pairCount > 0); + + if (proxyId1 > proxyId2) + Common.Math.Swap<int>(ref proxyId1, ref proxyId2); + + int hash = (int)(Hash((uint)proxyId1, (uint)proxyId2) & PairManager.TableMask); + + //uint16* node = &m_hashTable[hash]; + ushort node = _hashTable[hash]; + bool ion = false; + int ni = 0; + while (node != PairManager.NullPair) + { + if (Equals(_pairs[node], proxyId1, proxyId2)) + { + //uint16 index = *node; + //*node = m_pairs[*node].next; + + ushort index = node; + node = _pairs[node].Next; + if (ion) + _pairs[ni].Next = node; + else + { + _hashTable[hash] = node; + } + + Pair pair = _pairs[index]; + object userData = pair.UserData; + + // Scrub + pair.Next = _freePair; + pair.ProxyId1 = PairManager.NullProxy; + pair.ProxyId2 = PairManager.NullProxy; + pair.UserData = null; + pair.Status = 0; + + _freePair = index; + --_pairCount; + return userData; + } + else + { + //node = &m_pairs[*node].next; + ni = node; + node = _pairs[ni].Next; + ion = true; + } + } + + Box2DXDebug.Assert(false); + return null; + } + + private void ValidateBuffer() + { +#if DEBUG + Box2DXDebug.Assert(_pairBufferCount <= _pairCount); + + //std::sort(m_pairBuffer, m_pairBuffer + m_pairBufferCount); + BufferedPair[] tmp = new BufferedPair[_pairBufferCount]; + Array.Copy(_pairBuffer, 0, tmp, 0, _pairBufferCount); + Array.Sort<BufferedPair>(tmp, BufferedPairSortPredicate); + Array.Copy(tmp, 0, _pairBuffer, 0, _pairBufferCount); + + for (int i = 0; i < _pairBufferCount; ++i) + { + if (i > 0) + { + Box2DXDebug.Assert(Equals(_pairBuffer[i], _pairBuffer[i - 1]) == false); + } + + Pair pair = Find(_pairBuffer[i].ProxyId1, _pairBuffer[i].ProxyId2); + Box2DXDebug.Assert(pair.IsBuffered()); + + Box2DXDebug.Assert(pair.ProxyId1 != pair.ProxyId2); + Box2DXDebug.Assert(pair.ProxyId1 < Settings.MaxProxies); + Box2DXDebug.Assert(pair.ProxyId2 < Settings.MaxProxies); + + Proxy proxy1 = _broadPhase._proxyPool[pair.ProxyId1]; + Proxy proxy2 = _broadPhase._proxyPool[pair.ProxyId2]; + + Box2DXDebug.Assert(proxy1.IsValid == true); + Box2DXDebug.Assert(proxy2.IsValid == true); + } +#endif + } + + private void ValidateTable() + { +#if DEBUG + for (int i = 0; i < PairManager.TableCapacity; ++i) + { + ushort index = _hashTable[i]; + while (index != PairManager.NullPair) + { + Pair pair = _pairs[index]; + Box2DXDebug.Assert(pair.IsBuffered() == false); + Box2DXDebug.Assert(pair.IsFinal() == true); + Box2DXDebug.Assert(pair.IsRemoved() == false); + + Box2DXDebug.Assert(pair.ProxyId1 != pair.ProxyId2); + Box2DXDebug.Assert(pair.ProxyId1 < Settings.MaxProxies); + Box2DXDebug.Assert(pair.ProxyId2 < Settings.MaxProxies); + + Proxy proxy1 = _broadPhase._proxyPool[pair.ProxyId1]; + Proxy proxy2 = _broadPhase._proxyPool[pair.ProxyId2]; + + Box2DXDebug.Assert(proxy1.IsValid == true); + Box2DXDebug.Assert(proxy2.IsValid == true); + + Box2DXDebug.Assert(_broadPhase.TestOverlap(proxy1, proxy2) == true); + + index = pair.Next; + } + } +#endif + } + + // Thomas Wang's hash, see: http://www.concentric.net/~Ttwang/tech/inthash.htm + // This assumes proxyId1 and proxyId2 are 16-bit. + private uint Hash(uint proxyId1, uint proxyId2) + { + uint key = (proxyId2 << 16) | proxyId1; + key = ~key + (key << 15); + key = key ^ (key >> 12); + key = key + (key << 2); + key = key ^ (key >> 4); + key = key * 2057; + key = key ^ (key >> 16); + return key; + } + + private bool Equals(Pair pair, int proxyId1, int proxyId2) + { + return pair.ProxyId1 == proxyId1 && pair.ProxyId2 == proxyId2; + } + + private bool Equals(ref BufferedPair pair1, ref BufferedPair pair2) + { + return pair1.ProxyId1 == pair2.ProxyId1 && pair1.ProxyId2 == pair2.ProxyId2; + } + + public static int BufferedPairSortPredicate(BufferedPair pair1, BufferedPair pair2) + { + if (pair1.ProxyId1 < pair2.ProxyId1) + return 1; + else if (pair1.ProxyId1 > pair2.ProxyId1) + return -1; + else + { + if (pair1.ProxyId2 < pair2.ProxyId2) + return 1; + else if (pair1.ProxyId2 > pair2.ProxyId2) + return -1; + } + + return 0; + } + } +}
\ No newline at end of file diff --git a/Box2d/Assets/Program/Box2d/Collision/PairManager.cs.meta b/Box2d/Assets/Program/Box2d/Collision/PairManager.cs.meta new file mode 100644 index 0000000..4aedb0e --- /dev/null +++ b/Box2d/Assets/Program/Box2d/Collision/PairManager.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 2c78d8fd3c4c22944a9cade8b7fdb31b +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Box2d/Assets/Program/Box2d/Collision/Shapes/CircleShape.cs b/Box2d/Assets/Program/Box2d/Collision/Shapes/CircleShape.cs new file mode 100644 index 0000000..553accd --- /dev/null +++ b/Box2d/Assets/Program/Box2d/Collision/Shapes/CircleShape.cs @@ -0,0 +1,179 @@ +/* + Box2DX Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx + Box2D original C++ version Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this 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. +*/ + +using Box2DX.Common; + +namespace Box2DX.Collision +{ + /// <summary> + /// A circle shape. + /// </summary> + public class CircleShape : Shape + { + // Position + internal Vec2 _position; + + public CircleShape() + { + _type = ShapeType.CircleShape; + } + + public override bool TestPoint(XForm transform, Vec2 p) + { + Vec2 center = transform.Position + Common.Math.Mul(transform.R, _position); + Vec2 d = p - center; + return Vec2.Dot(d, d) <= _radius * _radius; + } + + // Collision Detection in Interactive 3D Environments by Gino van den Bergen + // From Section 3.1.2 + // x = s + a * r + // norm(x) = radius + public override SegmentCollide TestSegment(XForm transform, out float lambda, out Vec2 normal, Segment segment, float maxLambda) + { + lambda = 0f; + normal = Vec2.Zero; + + Vec2 position = transform.Position + Common.Math.Mul(transform.R, _position); + Vec2 s = segment.P1 - position; + float b = Vec2.Dot(s, s) - _radius * _radius; + + // Does the segment start inside the circle? + if (b < 0.0f) + { + lambda = 0f; + return SegmentCollide.StartInsideCollide; + } + + // Solve quadratic equation. + Vec2 r = segment.P2 - segment.P1; + float c = Vec2.Dot(s, r); + float rr = Vec2.Dot(r, r); + float sigma = c * c - rr * b; + + // Check for negative discriminant and short segment. + if (sigma < 0.0f || rr < Common.Settings.FLT_EPSILON) + { + return SegmentCollide.MissCollide; + } + + // Find the point of intersection of the line with the circle. + float a = -(c + Common.Math.Sqrt(sigma)); + + // Is the intersection point on the segment? + if (0.0f <= a && a <= maxLambda * rr) + { + a /= rr; + lambda = a; + normal = s + a * r; + normal.Normalize(); + return SegmentCollide.HitCollide; + } + + return SegmentCollide.MissCollide; + } + + public override void ComputeAABB(out AABB aabb, XForm transform) + { + aabb = new AABB(); + + Vec2 p = transform.Position + Common.Math.Mul(transform.R, _position); + aabb.LowerBound.Set(p.X - _radius, p.Y - _radius); + aabb.UpperBound.Set(p.X + _radius, p.Y + _radius); + } + + public override void ComputeMass(out MassData massData, float density) + { + massData = new MassData(); + + massData.Mass = density * Settings.Pi * _radius * _radius; + massData.Center = _position; + + // inertia about the local origin + massData.I = massData.Mass * (0.5f * _radius * _radius + Vec2.Dot(_position, _position)); + } + + public override float ComputeSubmergedArea(Vec2 normal, float offset, XForm xf, out Vec2 c) + { + Vec2 p = Box2DX.Common.Math.Mul(xf, _position); + float l = -(Vec2.Dot(normal, p) - offset); + if (l < -_radius + Box2DX.Common.Settings.FLT_EPSILON) + { + //Completely dry + c = new Vec2(); + return 0; + } + if (l > _radius) + { + //Completely wet + c = p; + return Box2DX.Common.Settings.Pi * _radius * _radius; + } + + //Magic + float r2 = _radius * _radius; + float l2 = l * l; + float area = r2 * ((float)System.Math.Asin(l / _radius) + Box2DX.Common.Settings.Pi / 2) + + l * Box2DX.Common.Math.Sqrt(r2 - l2); + float com = -2.0f / 3.0f * (float)System.Math.Pow(r2 - l2, 1.5f) / area; + + c.X = p.X + normal.X * com; + c.Y = p.Y + normal.Y * com; + + return area; + } + + /// <summary> + /// Get the supporting vertex index in the given direction. + /// </summary> + public override int GetSupport(Vec2 d) + { + return 0; + } + + /// <summary> + /// Get the supporting vertex in the given direction. + /// </summary> + public override Vec2 GetSupportVertex(Vec2 d) + { + return _position; + } + + /// <summary> + /// Get a vertex by index. Used by Distance. + /// </summary> + public override Vec2 GetVertex(int index) + { + Box2DXDebug.Assert(index == 0); + return _position; + } + + public override float ComputeSweepRadius(Vec2 pivot) + { + return Vec2.Distance(_position, pivot); + } + + /// <summary> + /// Get the vertex count. + /// </summary> + public int VertexCount { get { return 1; } } + } +}
\ No newline at end of file diff --git a/Box2d/Assets/Program/Box2d/Collision/Shapes/CircleShape.cs.meta b/Box2d/Assets/Program/Box2d/Collision/Shapes/CircleShape.cs.meta new file mode 100644 index 0000000..f9be13f --- /dev/null +++ b/Box2d/Assets/Program/Box2d/Collision/Shapes/CircleShape.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: bd3059081b10a7c49be772d675ea7525 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Box2d/Assets/Program/Box2d/Collision/Shapes/EdgeShape.cs b/Box2d/Assets/Program/Box2d/Collision/Shapes/EdgeShape.cs new file mode 100644 index 0000000..348a293 --- /dev/null +++ b/Box2d/Assets/Program/Box2d/Collision/Shapes/EdgeShape.cs @@ -0,0 +1,277 @@ +/* + Box2DX Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx + Box2D original C++ version Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this 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. +*/ + +using System; +using System.Collections.Generic; +using System.Text; + +using Box2DX.Common; + +namespace Box2DX.Collision +{ + public class EdgeShape : Shape + { + public Vec2 _v1; + public Vec2 _v2; + + public float _length; + + public Vec2 _normal; + + public Vec2 _direction; + + // Unit vector halfway between m_direction and m_prevEdge.m_direction: + public Vec2 _cornerDir1; + + // Unit vector halfway between m_direction and m_nextEdge.m_direction: + public Vec2 _cornerDir2; + + public bool _cornerConvex1; + public bool _cornerConvex2; + + public EdgeShape _nextEdge; + public EdgeShape _prevEdge; + + public EdgeShape() + { + _type = ShapeType.EdgeShape; + _radius = Settings.PolygonRadius; + } + + public override void Dispose() + { + if (_prevEdge != null) + { + _prevEdge._nextEdge = null; + } + + if (_nextEdge != null) + { + _nextEdge._prevEdge = null; + } + } + + public void Set(Vec2 v1, Vec2 v2) + { + _v1 = v1; + _v2 = v2; + + _direction = _v2 - _v1; + _length = _direction.Normalize(); + _normal = Vec2.Cross(_direction, 1.0f); + + _cornerDir1 = _normal; + _cornerDir2 = -1.0f * _normal; + } + + public override bool TestPoint(XForm transform, Vec2 p) + { + return false; + } + + public override SegmentCollide TestSegment(XForm transform, out float lambda, out Vec2 normal, Segment segment, float maxLambda) + { + Vec2 r = segment.P2 - segment.P1; + Vec2 v1 = Common.Math.Mul(transform, _v1); + Vec2 d = Common.Math.Mul(transform, _v2) - v1; + Vec2 n = Vec2.Cross(d, 1.0f); + + float k_slop = 100.0f * Common.Settings.FLT_EPSILON; + float denom = -Vec2.Dot(r, n); + + // Cull back facing collision and ignore parallel segments. + if (denom > k_slop) + { + // Does the segment intersect the infinite line associated with this segment? + Vec2 b = segment.P1 - v1; + float a = Vec2.Dot(b, n); + + if (0.0f <= a && a <= maxLambda * denom) + { + float mu2 = -r.X * b.Y + r.Y * b.X; + + // Does the segment intersect this segment? + if (-k_slop * denom <= mu2 && mu2 <= denom * (1.0f + k_slop)) + { + a /= denom; + n.Normalize(); + lambda = a; + normal = n; + return SegmentCollide.HitCollide; + } + } + } + + lambda = 0; + normal = new Vec2(); + return SegmentCollide.MissCollide; + } + + public override void ComputeAABB(out AABB aabb, XForm transform) + { + Vec2 v1 = Common.Math.Mul(transform, _v1); + Vec2 v2 = Common.Math.Mul(transform, _v2); + + Vec2 r = new Vec2(_radius, _radius); + aabb.LowerBound = Common.Math.Min(v1, v2) - r; + aabb.UpperBound = Common.Math.Max(v1, v2) + r; + } + + public override void ComputeMass(out MassData massData, float density) + { + massData.Mass = 0.0f; + massData.Center = _v1; + massData.I = 0.0f; + } + + public void SetPrevEdge(EdgeShape edge, Vec2 cornerDir, bool convex) + { + _prevEdge = edge; + _cornerDir1 = cornerDir; + _cornerConvex1 = convex; + } + + public void SetNextEdge(EdgeShape edge, Vec2 cornerDir, bool convex) + { + _nextEdge = edge; + _cornerDir2 = cornerDir; + _cornerConvex2 = convex; + } + + public override float ComputeSubmergedArea(Vec2 normal, float offset, XForm xf, out Vec2 c) + { + //Note that v0 is independent of any details of the specific edge + //We are relying on v0 being consistent between multiple edges of the same body + Vec2 v0 = offset * normal; + //b2Vec2 v0 = xf.position + (offset - b2Dot(normal, xf.position)) * normal; + + Vec2 v1 = Common.Math.Mul(xf, _v1); + Vec2 v2 = Common.Math.Mul(xf, _v2); + + float d1 = Vec2.Dot(normal, v1) - offset; + float d2 = Vec2.Dot(normal, v2) - offset; + + if (d1 > 0.0f) + { + if (d2 > 0.0f) + { + c = new Vec2(); + return 0.0f; + } + else + { + v1 = -d2 / (d1 - d2) * v1 + d1 / (d1 - d2) * v2; + } + } + else + { + if (d2 > 0.0f) + { + v2 = -d2 / (d1 - d2) * v1 + d1 / (d1 - d2) * v2; + } + else + { + //Nothing + } + } + + // v0,v1,v2 represents a fully submerged triangle + float k_inv3 = 1.0f / 3.0f; + + // Area weighted centroid + c = k_inv3 * (v0 + v1 + v2); + + Vec2 e1 = v1 - v0; + Vec2 e2 = v2 - v0; + + return 0.5f * Vec2.Cross(e1, e2); + } + + public float Length + { + get { return _length; } + } + + public Vec2 Vertex1 + { + get { return _v1; } + } + + public Vec2 Vertex2 + { + get { return _v2; } + } + + public Vec2 NormalVector + { + get { return _normal; } + } + + public Vec2 DirectionVector + { + get { return _direction; } + } + + public Vec2 Corner1Vector + { + get { return _cornerDir1; } + } + + public Vec2 Corner2Vector + { + get { return _cornerDir2; } + } + + public override int GetSupport(Vec2 d) + { + return Vec2.Dot(_v1, d) > Vec2.Dot(_v2, d) ? 0 : 1; + } + + public override Vec2 GetSupportVertex(Vec2 d) + { + return Vec2.Dot(_v1, d) > Vec2.Dot(_v2, d) ? _v1 : _v2; + } + + public override Vec2 GetVertex(int index) + { + Box2DXDebug.Assert(0 <= index && index < 2); + if (index == 0) return _v1; + else return _v2; + } + + public bool Corner1IsConvex + { + get { return _cornerConvex1; } + } + + public bool Corner2IsConvex + { + get { return _cornerConvex2; } + } + + public override float ComputeSweepRadius(Vec2 pivot) + { + float ds1 = Vec2.DistanceSquared(_v1, pivot); + float ds2 = Vec2.DistanceSquared(_v2, pivot); + return Common.Math.Sqrt(Common.Math.Max(ds1, ds2)); + } + } +} diff --git a/Box2d/Assets/Program/Box2d/Collision/Shapes/EdgeShape.cs.meta b/Box2d/Assets/Program/Box2d/Collision/Shapes/EdgeShape.cs.meta new file mode 100644 index 0000000..8051fd4 --- /dev/null +++ b/Box2d/Assets/Program/Box2d/Collision/Shapes/EdgeShape.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 1e86753e9f98b2d429a099076e61bf32 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Box2d/Assets/Program/Box2d/Collision/Shapes/PolygonShape.cs b/Box2d/Assets/Program/Box2d/Collision/Shapes/PolygonShape.cs index 85ae672..6a655f7 100644 --- a/Box2d/Assets/Program/Box2d/Collision/Shapes/PolygonShape.cs +++ b/Box2d/Assets/Program/Box2d/Collision/Shapes/PolygonShape.cs @@ -1,7 +1,724 @@ -using System.Collections; +/* + Box2DX Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx + Box2D original C++ version Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this 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. +*/ + +#define DEBUG + +using System; using System.Collections.Generic; +using System.Text; + +using Box2DX.Common; -namespace Box2D +namespace Box2DX.Collision { + /// <summary> + /// A convex polygon. It is assumed that the interior of the polygon is to the left of each edge. + /// </summary> + public class PolygonShape : Shape + { + internal Vec2 _centroid; + internal Vec2[] _vertices = new Vec2[Settings.MaxPolygonVertices]; + internal Vec2[] _normals = new Vec2[Settings.MaxPolygonVertices]; + + internal int _vertexCount; + + public int VertexCount + { + get { return _vertexCount; } + } + + public Vec2[] Vertices + { + get { return _vertices; } + } + + /// <summary> + /// Copy vertices. This assumes the vertices define a convex polygon. + /// It is assumed that the exterior is the the right of each edge. + /// </summary> + public void Set(Vec2[] vertices, int count) + { + Box2DXDebug.Assert(3 <= count && count <= Settings.MaxPolygonVertices); + _vertexCount = count; + + int i; + // Copy vertices. + for (i = 0; i < _vertexCount; ++i) + { + _vertices[i] = vertices[i]; + } + + // Compute normals. Ensure the edges have non-zero length. + for (i = 0; i < _vertexCount; ++i) + { + int i1 = i; + int i2 = i + 1 < count ? i + 1 : 0; + Vec2 edge = _vertices[i2] - _vertices[i1]; + Box2DXDebug.Assert(edge.LengthSquared() > Settings.FLT_EPSILON_SQUARED); + _normals[i] = Vec2.Cross(edge, 1.0f); + _normals[i].Normalize(); + } + +#if DEBUG + // Ensure the polygon is convex and the interior + // is to the left of each edge. + for (i = 0; i < _vertexCount; ++i) + { + int i1 = i; + int i2 = i + 1 < count ? i + 1 : 0; + Vec2 edge = _vertices[i2] - _vertices[i1]; + + for (int j = 0; j < _vertexCount; ++j) + { + // Don't check vertices on the current edge. + if (j == i1 || j == i2) + { + continue; + } + + Vec2 r = _vertices[j] - _vertices[i1]; + + // Your polygon is non-convex (it has an indentation) or + // has colinear edges. + float s = Vec2.Cross(edge, r); + Box2DXDebug.Assert(s > 0.0f); + } + } +#endif + + // Compute the polygon centroid. + _centroid = ComputeCentroid(_vertices, _vertexCount); + } + + /// <summary> + /// Build vertices to represent an axis-aligned box. + /// </summary> + /// <param name="hx">The half-width</param> + /// <param name="hy">The half-height.</param> + public void SetAsBox(float hx, float hy) + { + _vertexCount = 4; + _vertices[0].Set(-hx, -hy); + _vertices[1].Set(hx, -hy); + _vertices[2].Set(hx, hy); + _vertices[3].Set(-hx, hy); + _normals[0].Set(0.0f, -1.0f); + _normals[1].Set(1.0f, 0.0f); + _normals[2].Set(0.0f, 1.0f); + _normals[3].Set(-1.0f, 0.0f); + _centroid = new Vec2(0); + } + + + /// <summary> + /// Build vertices to represent an oriented box. + /// </summary> + /// <param name="hx">The half-width</param> + /// <param name="hy">The half-height.</param> + /// <param name="center">The center of the box in local coordinates.</param> + /// <param name="angle">The rotation of the box in local coordinates.</param> + public void SetAsBox(float hx, float hy, Vec2 center, float angle) + { + SetAsBox(hx, hy); + + XForm xf = new XForm(); + xf.Position = center; + xf.R.Set(angle); + + // Transform vertices and normals. + for (int i = 0; i < _vertexCount; ++i) + { + _vertices[i] = Common.Math.Mul(xf, _vertices[i]); + _normals[i] = Common.Math.Mul(xf.R, _normals[i]); + } + } + + public void SetAsEdge(Vec2 v1, Vec2 v2) + { + _vertexCount = 2; + _vertices[0] = v1; + _vertices[1] = v2; + _centroid = 0.5f * (v1 + v2); + _normals[0] = Vec2.Cross(v2 - v1, 1.0f); + _normals[0].Normalize(); + _normals[1] = -_normals[0]; + } + + public override bool TestPoint(XForm xf, Vec2 p) + { + Vec2 pLocal = Common.Math.MulT(xf.R, p - xf.Position); + + int vc = _vertexCount; + for (int i = 0; i < vc; ++i) + { + float dot = Vec2.Dot(_normals[i], pLocal - _vertices[i]); + if (dot > 0.0f) + { + return false; + } + } + + return true; + } + + public override SegmentCollide TestSegment(XForm xf, out float lambda, out Vec2 normal, Segment segment, float maxLambda) + { + lambda = 0f; + normal = Vec2.Zero; + + float lower = 0.0f, upper = maxLambda; + + Vec2 p1 = Common.Math.MulT(xf.R, segment.P1 - xf.Position); + Vec2 p2 = Common.Math.MulT(xf.R, segment.P2 - xf.Position); + Vec2 d = p2 - p1; + int index = -1; + + for (int i = 0; i < _vertexCount; ++i) + { + // p = p1 + a * d + // dot(normal, p - v) = 0 + // dot(normal, p1 - v) + a * dot(normal, d) = 0 + float numerator = Vec2.Dot(_normals[i], _vertices[i] - p1); + float denominator = Vec2.Dot(_normals[i], d); + + if (denominator == 0.0f) + { + if (numerator < 0.0f) + { + return SegmentCollide.MissCollide; + } + } + else + { + // Note: we want this predicate without division: + // lower < numerator / denominator, where denominator < 0 + // Since denominator < 0, we have to flip the inequality: + // lower < numerator / denominator <==> denominator * lower > numerator. + if (denominator < 0.0f && numerator < lower * denominator) + { + // Increase lower. + // The segment enters this half-space. + lower = numerator / denominator; + index = i; + } + else if (denominator > 0.0f && numerator < upper * denominator) + { + // Decrease upper. + // The segment exits this half-space. + upper = numerator / denominator; + } + } + + if (upper < lower) + { + return SegmentCollide.MissCollide; + } + } + + Box2DXDebug.Assert(0.0f <= lower && lower <= maxLambda); + + if (index >= 0) + { + lambda = lower; + normal = Common.Math.Mul(xf.R, _normals[index]); + return SegmentCollide.HitCollide; + } + + lambda = 0f; + return SegmentCollide.StartInsideCollide; + } + + public override void ComputeAABB(out AABB aabb, XForm xf) + { + Vec2 lower = Common.Math.Mul(xf, _vertices[0]); + Vec2 upper = lower; + + for (int i = 1; i < _vertexCount; ++i) + { + Vec2 v = Common.Math.Mul(xf, _vertices[i]); + lower = Common.Math.Min(lower, v); + upper = Common.Math.Max(upper, v); + } + + Vec2 r = new Vec2(_radius); + aabb.LowerBound = lower - r; + aabb.UpperBound = upper + r; + } + + public override void ComputeMass(out MassData massData, float denstity) + { + // Polygon mass, centroid, and inertia. + // Let rho be the polygon density in mass per unit area. + // Then: + // mass = rho * int(dA) + // centroid.x = (1/mass) * rho * int(x * dA) + // centroid.y = (1/mass) * rho * int(y * dA) + // I = rho * int((x*x + y*y) * dA) + // + // We can compute these integrals by summing all the integrals + // for each triangle of the polygon. To evaluate the integral + // for a single triangle, we make a change of variables to + // the (u,v) coordinates of the triangle: + // x = x0 + e1x * u + e2x * v + // y = y0 + e1y * u + e2y * v + // where 0 <= u && 0 <= v && u + v <= 1. + // + // We integrate u from [0,1-v] and then v from [0,1]. + // We also need to use the Jacobian of the transformation: + // D = cross(e1, e2) + // + // Simplification: triangle centroid = (1/3) * (p1 + p2 + p3) + // + // The rest of the derivation is handled by computer algebra. + + Box2DXDebug.Assert(_vertexCount >= 3); + + Vec2 center = new Vec2(0); + float area = 0.0f; + float I = 0.0f; + + // pRef is the reference point for forming triangles. + // It's location doesn't change the result (except for rounding error). + Vec2 pRef = new Vec2(0); + +#if O + // This code would put the reference point inside the polygon. + for (int i = 0; i < vCount; ++i) + { + pRef += _vertices[i]; + } + pRef *= 1.0f / count; +#endif + + const float k_inv3 = 1.0f / 3.0f; + + for (int i = 0; i < _vertexCount; ++i) + { + // Triangle vertices. + Vec2 p1 = pRef; + Vec2 p2 = _vertices[i]; + Vec2 p3 = i + 1 < _vertexCount ? _vertices[i + 1] : _vertices[0]; + + Vec2 e1 = p2 - p1; + Vec2 e2 = p3 - p1; + + float D = Vec2.Cross(e1, e2); + + float triangleArea = 0.5f * D; + area += triangleArea; + + // Area weighted centroid + center += triangleArea * k_inv3 * (p1 + p2 + p3); + + float px = p1.X, py = p1.Y; + float ex1 = e1.X, ey1 = e1.Y; + float ex2 = e2.X, ey2 = e2.Y; + + float intx2 = k_inv3 * (0.25f * (ex1 * ex1 + ex2 * ex1 + ex2 * ex2) + (px * ex1 + px * ex2)) + 0.5f * px * px; + float inty2 = k_inv3 * (0.25f * (ey1 * ey1 + ey2 * ey1 + ey2 * ey2) + (py * ey1 + py * ey2)) + 0.5f * py * py; + + I += D * (intx2 + inty2); + } + + // Total mass + massData.Mass = denstity * area; + + // Center of mass + Box2DXDebug.Assert(area > Common.Settings.FLT_EPSILON); + center *= 1.0f / area; + massData.Center = center; + + // Inertia tensor relative to the local origin. + massData.I = denstity * I; + } + + public override float ComputeSubmergedArea(Vec2 normal, float offset, XForm xf, out Vec2 c) + { + //Transform plane into shape co-ordinates + Vec2 normalL = Box2DX.Common.Math.MulT(xf.R, normal); + float offsetL = offset - Vec2.Dot(normal, xf.Position); + + float[] depths = new float[Common.Settings.MaxPolygonVertices]; + int diveCount = 0; + int intoIndex = -1; + int outoIndex = -1; + + bool lastSubmerged = false; + int i; + for (i = 0; i < _vertexCount; i++) + { + depths[i] = Vec2.Dot(normalL, _vertices[i]) - offsetL; + bool isSubmerged = depths[i] < -Common.Settings.FLT_EPSILON; + if (i > 0) + { + if (isSubmerged) + { + if (!lastSubmerged) + { + intoIndex = i - 1; + diveCount++; + } + } + else + { + if (lastSubmerged) + { + outoIndex = i - 1; + diveCount++; + } + } + } + lastSubmerged = isSubmerged; + } + switch (diveCount) + { + case 0: + if (lastSubmerged) + { + //Completely submerged + MassData md; + ComputeMass(out md, 1f); + c = Common.Math.Mul(xf, md.Center); + return md.Mass; + } + else + { + //Completely dry + c = new Vec2(); + return 0; + } + break; + case 1: + if (intoIndex == -1) + { + intoIndex = _vertexCount - 1; + } + else + { + outoIndex = _vertexCount - 1; + } + break; + } + int intoIndex2 = (intoIndex + 1) % _vertexCount; + int outoIndex2 = (outoIndex + 1) % _vertexCount; + + float intoLambda = (0 - depths[intoIndex]) / (depths[intoIndex2] - depths[intoIndex]); + float outoLambda = (0 - depths[outoIndex]) / (depths[outoIndex2] - depths[outoIndex]); + + Vec2 intoVec = new Vec2(_vertices[intoIndex].X * (1 - intoLambda) + _vertices[intoIndex2].X * intoLambda, + _vertices[intoIndex].Y * (1 - intoLambda) + _vertices[intoIndex2].Y * intoLambda); + Vec2 outoVec = new Vec2(_vertices[outoIndex].X * (1 - outoLambda) + _vertices[outoIndex2].X * outoLambda, + _vertices[outoIndex].Y * (1 - outoLambda) + _vertices[outoIndex2].Y * outoLambda); + + //Initialize accumulator + float area = 0; + Vec2 center = new Vec2(0); + Vec2 p2 = _vertices[intoIndex2]; + Vec2 p3; + + const float k_inv3 = 1.0f / 3.0f; + + //An awkward loop from intoIndex2+1 to outIndex2 + i = intoIndex2; + while (i != outoIndex2) + { + i = (i + 1) % _vertexCount; + if (i == outoIndex2) + p3 = outoVec; + else + p3 = _vertices[i]; + //Add the triangle formed by intoVec,p2,p3 + { + Vec2 e1 = p2 - intoVec; + Vec2 e2 = p3 - intoVec; + + float D = Vec2.Cross(e1, e2); + + float triangleArea = 0.5f * D; + + area += triangleArea; + + // Area weighted centroid + center += triangleArea * k_inv3 * (intoVec + p2 + p3); + + } + // + p2 = p3; + } + + //Normalize and transform centroid + center *= 1.0f / area; + + c = Common.Math.Mul(xf, center); + + return area; + } + + public override float ComputeSweepRadius(Vec2 pivot) + { + int vCount = _vertexCount; + Box2DXDebug.Assert(vCount > 0); + float sr = Vec2.DistanceSquared(_vertices[0], pivot); + for (int i = 1; i < vCount; ++i) + { + sr = Common.Math.Max(sr, Vec2.DistanceSquared(_vertices[i], pivot)); + } + + return Common.Math.Sqrt(sr); + } + + /// <summary> + /// Get the supporting vertex index in the given direction. + /// </summary> + public override int GetSupport(Vec2 d) + { + int bestIndex = 0; + float bestValue = Vec2.Dot(_vertices[0], d); + for (int i = 1; i < _vertexCount; ++i) + { + float value = Vec2.Dot(_vertices[i], d); + if (value > bestValue) + { + bestIndex = i; + bestValue = value; + } + } + + return bestIndex; + } + + public override Vec2 GetSupportVertex(Vec2 d) + { + int bestIndex = 0; + float bestValue = Vec2.Dot(_vertices[0], d); + for (int i = 1; i < _vertexCount; ++i) + { + float value = Vec2.Dot(_vertices[i], d); + if (value > bestValue) + { + bestIndex = i; + bestValue = value; + } + } + + return _vertices[bestIndex]; + } + + public override Vec2 GetVertex(int index) + { + Box2DXDebug.Assert(0 <= index && index < _vertexCount); + return _vertices[index]; + } + + public static Vec2 ComputeCentroid(Vec2[] vs, int count) + { + Box2DXDebug.Assert(count >= 3); + + Vec2 c = new Vec2(0f); + float area = 0f; + + // pRef is the reference point for forming triangles. + // It's location doesn't change the result (except for rounding error). + Vec2 pRef = new Vec2(0f); +#if O + // This code would put the reference point inside the polygon. + for (int i = 0; i < count; ++i) + { + pRef += vs[i]; + } + pRef *= 1.0f / count; +#endif + + const float inv3 = 1.0f / 3.0f; + + for (int i = 0; i < count; ++i) + { + // Triangle vertices. + Vec2 p1 = pRef; + Vec2 p2 = vs[i]; + Vec2 p3 = i + 1 < count ? vs[i + 1] : vs[0]; + + Vec2 e1 = p2 - p1; + Vec2 e2 = p3 - p1; + + float D = Vec2.Cross(e1, e2); + + float triangleArea = 0.5f * D; + area += triangleArea; + + // Area weighted centroid + c += triangleArea * inv3 * (p1 + p2 + p3); + } + + // Centroid + Box2DXDebug.Assert(area > Common.Settings.FLT_EPSILON); + c *= 1.0f / area; + return c; + } + + public PolygonShape() + { + _type = ShapeType.PolygonShape; + _radius = Settings.PolygonRadius; + + /*Box2DXDebug.Assert(def.Type == ShapeType.PolygonShape); + _type = ShapeType.PolygonShape; + PolygonDef poly = (PolygonDef)def; + + // Get the vertices transformed into the body frame. + _vertexCount = poly.VertexCount; + Box2DXDebug.Assert(3 <= _vertexCount && _vertexCount <= Settings.MaxPolygonVertices); + + // Copy vertices. + for (int i = 0; i < _vertexCount; ++i) + { + _vertices[i] = poly.Vertices[i]; + } + + // Compute normals. Ensure the edges have non-zero length. + for (int i = 0; i < _vertexCount; ++i) + { + int i1 = i; + int i2 = i + 1 < _vertexCount ? i + 1 : 0; + Vec2 edge = _vertices[i2] - _vertices[i1]; + Box2DXDebug.Assert(edge.LengthSquared() > Common.Settings.FLT_EPSILON * Common.Settings.FLT_EPSILON); + _normals[i] = Vec2.Cross(edge, 1.0f); + _normals[i].Normalize(); + } + +#if DEBUG + // Ensure the polygon is convex. + for (int i = 0; i < _vertexCount; ++i) + { + for (int j = 0; j < _vertexCount; ++j) + { + // Don't check vertices on the current edge. + if (j == i || j == (i + 1) % _vertexCount) + { + continue; + } + + // Your polygon is non-convex (it has an indentation). + // Or your polygon is too skinny. + float s = Vec2.Dot(_normals[i], _vertices[j] - _vertices[i]); + Box2DXDebug.Assert(s < -Settings.LinearSlop); + } + } + + // Ensure the polygon is counter-clockwise. + for (int i = 1; i < _vertexCount; ++i) + { + float cross = Vec2.Cross(_normals[i - 1], _normals[i]); + + // Keep asinf happy. + cross = Common.Math.Clamp(cross, -1.0f, 1.0f); + + // You have consecutive edges that are almost parallel on your polygon. + float angle = (float)System.Math.Asin(cross); + Box2DXDebug.Assert(angle > Settings.AngularSlop); + } +#endif + + // Compute the polygon centroid. + _centroid = ComputeCentroid(poly.Vertices, poly.VertexCount); + + // Compute the oriented bounding box. + ComputeOBB(out _obb, _vertices, _vertexCount); + + // Create core polygon shape by shifting edges inward. + // Also compute the min/max radius for CCD. + for (int i = 0; i < _vertexCount; ++i) + { + int i1 = i - 1 >= 0 ? i - 1 : _vertexCount - 1; + int i2 = i; + + Vec2 n1 = _normals[i1]; + Vec2 n2 = _normals[i2]; + Vec2 v = _vertices[i] - _centroid; ; + + Vec2 d = new Vec2(); + d.X = Vec2.Dot(n1, v) - Settings.ToiSlop; + d.Y = Vec2.Dot(n2, v) - Settings.ToiSlop; + + // Shifting the edge inward by b2_toiSlop should + // not cause the plane to pass the centroid. + + // Your shape has a radius/extent less than b2_toiSlop. + Box2DXDebug.Assert(d.X >= 0.0f); + Box2DXDebug.Assert(d.Y >= 0.0f); + Mat22 A = new Mat22(); + A.Col1.X = n1.X; A.Col2.X = n1.Y; + A.Col1.Y = n2.X; A.Col2.Y = n2.Y; + _coreVertices[i] = A.Solve(d) + _centroid; + }*/ + } + + /*// http://www.geometrictools.com/Documentation/MinimumAreaRectangle.pdf + public static void ComputeOBB(out OBB obb, Vec2[] vs, int count) + { + obb = new OBB(); + + Box2DXDebug.Assert(count <= Settings.MaxPolygonVertices); + Vec2[] p = new Vec2[Settings.MaxPolygonVertices + 1]; + for (int i = 0; i < count; ++i) + { + p[i] = vs[i]; + } + p[count] = p[0]; + + float minArea = Common.Settings.FLT_MAX; + + for (int i = 1; i <= count; ++i) + { + Vec2 root = p[i - 1]; + Vec2 ux = p[i] - root; + float length = ux.Normalize(); + Box2DXDebug.Assert(length > Common.Settings.FLT_EPSILON); + Vec2 uy = new Vec2(-ux.Y, ux.X); + Vec2 lower = new Vec2(Common.Settings.FLT_MAX, Common.Settings.FLT_MAX); + Vec2 upper = new Vec2(-Common.Settings.FLT_MAX, -Common.Settings.FLT_MAX); + + for (int j = 0; j < count; ++j) + { + Vec2 d = p[j] - root; + Vec2 r = new Vec2(); + r.X = Vec2.Dot(ux, d); + r.Y = Vec2.Dot(uy, d); + lower = Common.Math.Min(lower, r); + upper = Common.Math.Max(upper, r); + } + + float area = (upper.X - lower.X) * (upper.Y - lower.Y); + if (area < 0.95f * minArea) + { + minArea = area; + obb.R.Col1 = ux; + obb.R.Col2 = uy; + Vec2 center = 0.5f * (lower + upper); + obb.Center = root + Common.Math.Mul(obb.R, center); + obb.Extents = 0.5f * (upper - lower); + } + } -} + Box2DXDebug.Assert(minArea < Common.Settings.FLT_MAX); + }*/ + } +}
\ No newline at end of file diff --git a/Box2d/Assets/Program/Box2d/Collision/Shapes/Shape.cs b/Box2d/Assets/Program/Box2d/Collision/Shapes/Shape.cs new file mode 100644 index 0000000..1afd116 --- /dev/null +++ b/Box2d/Assets/Program/Box2d/Collision/Shapes/Shape.cs @@ -0,0 +1,144 @@ +/* + Box2DX Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx + Box2D original C++ version Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this 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. +*/ + +using System; +using Box2DX.Common; + +namespace Box2DX.Collision +{ + /// <summary> + /// This holds the mass data computed for a shape. + /// </summary> + public struct MassData + { + /// <summary> + /// The mass of the shape, usually in kilograms. + /// </summary> + public float Mass; + + /// <summary> + /// The position of the shape's centroid relative to the shape's origin. + /// </summary> + public Vec2 Center; + + /// <summary> + /// The rotational inertia of the shape. + /// </summary> + public float I; + } + + /// <summary> + /// The various collision shape types supported by Box2D. + /// </summary> + public enum ShapeType + { + UnknownShape = -1, + CircleShape, + PolygonShape, + EdgeShape, + ShapeTypeCount, + } + + /// <summary> + /// Returns code from TestSegment + /// </summary> + public enum SegmentCollide + { + StartInsideCollide = -1, + MissCollide = 0, + HitCollide = 1 + } + + /// <summary> + /// A shape is used for collision detection. You can create a shape however you like. + /// Shapes used for simulation in World are created automatically when a Fixture is created. + /// </summary> + public abstract class Shape : IDisposable + { + #region Fields + + protected ShapeType _type = ShapeType.UnknownShape; + internal float _radius; + + #endregion Fields + + protected Shape() { } + + /// <summary> + /// Test a point for containment in this shape. This only works for convex shapes. + /// </summary> + /// <param name="xf">The shape world transform.</param> + /// <param name="p">A point in world coordinates.</param> + /// <returns></returns> + public abstract bool TestPoint(XForm xf, Vec2 p); + + /// <summary> + /// Perform a ray cast against this shape. + /// </summary> + /// <param name="xf">The shape world transform.</param> + /// <param name="lambda">Returns the hit fraction. You can use this to compute the contact point + /// p = (1 - lambda) * segment.P1 + lambda * segment.P2.</param> + /// <param name="normal"> Returns the normal at the contact point. If there is no intersection, + /// the normal is not set.</param> + /// <param name="segment">Defines the begin and end point of the ray cast.</param> + /// <param name="maxLambda">A number typically in the range [0,1].</param> + public abstract SegmentCollide TestSegment(XForm xf, out float lambda, out Vec2 normal, Segment segment, float maxLambda); + + /// <summary> + /// Given a transform, compute the associated axis aligned bounding box for this shape. + /// </summary> + /// <param name="aabb">Returns the axis aligned box.</param> + /// <param name="xf">The world transform of the shape.</param> + public abstract void ComputeAABB(out AABB aabb, XForm xf); + + /// <summary> + /// Compute the mass properties of this shape using its dimensions and density. + /// The inertia tensor is computed about the local origin, not the centroid. + /// </summary> + /// <param name="massData">Returns the mass data for this shape</param> + public abstract void ComputeMass(out MassData massData, float density); + + /// <summary> + /// Compute the volume and centroid of this shape intersected with a half plane. + /// </summary> + /// <param name="normal">Normal the surface normal.</param> + /// <param name="offset">Offset the surface offset along normal.</param> + /// <param name="xf">The shape transform.</param> + /// <param name="c">Returns the centroid.</param> + /// <returns>The total volume less than offset along normal.</returns> + public abstract float ComputeSubmergedArea(Vec2 normal, float offset, XForm xf, out Vec2 c); + + /// <summary> + /// Compute the sweep radius. This is used for conservative advancement (continuous collision detection). + /// </summary> + /// <param name="pivot">Pivot is the pivot point for rotation.</param> + /// <returns>The distance of the furthest point from the pivot.</returns> + public abstract float ComputeSweepRadius(Vec2 pivot); + + public abstract Vec2 GetVertex(int index); + + public abstract int GetSupport(Vec2 d); + + public abstract Vec2 GetSupportVertex(Vec2 d); + + public virtual void Dispose(){} + } +}
\ No newline at end of file diff --git a/Box2d/Assets/Program/Box2d/Collision/Shapes/Shape.cs.meta b/Box2d/Assets/Program/Box2d/Collision/Shapes/Shape.cs.meta new file mode 100644 index 0000000..b67f3b4 --- /dev/null +++ b/Box2d/Assets/Program/Box2d/Collision/Shapes/Shape.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: cfde5d8f3b66ef849b75c54fff38f0b6 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Box2d/Assets/Program/Box2d/Collision/TODO.txt b/Box2d/Assets/Program/Box2d/Collision/TODO.txt new file mode 100644 index 0000000..312bdc2 --- /dev/null +++ b/Box2d/Assets/Program/Box2d/Collision/TODO.txt @@ -0,0 +1 @@ +check broadphase, port dynamictree
\ No newline at end of file diff --git a/Box2d/Assets/Program/Box2d/Collision/TODO.txt.meta b/Box2d/Assets/Program/Box2d/Collision/TODO.txt.meta new file mode 100644 index 0000000..3719ec6 --- /dev/null +++ b/Box2d/Assets/Program/Box2d/Collision/TODO.txt.meta @@ -0,0 +1,7 @@ +fileFormatVersion: 2 +guid: 18843330e42568c4bbff292ac904fd78 +TextScriptImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Box2d/Assets/Program/Box2d/Common/Mat22.cs b/Box2d/Assets/Program/Box2d/Common/Mat22.cs index 01ef830..ba70149 100644 --- a/Box2d/Assets/Program/Box2d/Common/Mat22.cs +++ b/Box2d/Assets/Program/Box2d/Common/Mat22.cs @@ -1,99 +1,150 @@ -using System; +/* + Box2DX Copyright (c) 2008 Ihar Kalasouski http://code.google.com/p/box2dx + Box2D original C++ version Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com -namespace Box2D + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this 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. +*/ + +using System; +using System.Collections.Generic; +using System.Text; + +namespace Box2DX.Common { - class Mat22 - { - public Mat22() - { - } - - public Mat22(Vec2 c1, Vec2 c2) - { - col1.Set(c1); - col2.Set(c2); - } - - public Mat22(float a11, float a12, float a21, float a22) - { - col1.x = a11; col1.y = a21; - col2.x = a12; col2.y = a22; - } - - public void Set(Vec2 c1, Vec2 c2) - { - col1.Set(c1); - col2.Set(c2); - } - - void SetIdentity() - { - col1.x = 1.0f; col2.x = 0.0f; - col1.y = 0.0f; col2.y = 1.0f; - } - - void SetZero() - { - col1.Set(0, 0); - col2.Set(0, 0); - } - - public float GetAngle() - { - return Convert.ToSingle(System.Math.Atan2(col1.y, col1.x)); - } - - /// <summary> - /// Compute the inverse of this matrix, such that inv(A) * A = identity. - /// </summary> - /// <returns></returns> - public Mat22 GetInverse() - { - float a = col1.x; - float b = col2.x; - float c = col1.y; - float d = col2.y; - float det = a * d - b * c; - if(det != 0.0) - { - det = 1 / det; - } - Mat22 m = new Mat22(); - m.col1.x = det * d; m.col2.x = -det * b; - m.col1.y = -det * c; m.col2.y = det * a; - return m; - } - - public Vec2 Solve(float bx, float by) - { - float a11 = col1.x; - float a12 = col2.x; - float a21 = col1.y; - float a22 = col2.y; - float det = a11 * a22 - a12 * a21; - if(det != 0.0) - { - det = 1 / det; - } - Vec2 v = new Vec2(); - v.x = det * (a22 * bx - a12 * by); - v.y = det * (a11 * by - a21 * bx); - return v; - } - - public void Abs() - { - col1.Abs(); - col2.Abs(); - } - - /* - * col1.x col2.x - * - * - * col1.y col2.y - */ - Vec2 col1 = new Vec2(); - Vec2 col2 = new Vec2(); - } + /// <summary> + /// A 2-by-2 matrix. Stored in column-major order. + /// </summary> + public struct Mat22 + { + public Vec2 Col1, Col2; + + /// <summary> + /// Construct this matrix using columns. + /// </summary> + public Mat22(Vec2 c1, Vec2 c2) + { + Col1 = c1; + Col2 = c2; + } + + /// <summary> + /// Construct this matrix using scalars. + /// </summary> + public Mat22(float a11, float a12, float a21, float a22) + { + Col1.X = a11; Col1.Y = a21; + Col2.X = a12; Col2.Y = a22; + } + + /// <summary> + /// Construct this matrix using an angle. + /// This matrix becomes an orthonormal rotation matrix. + /// </summary> + public Mat22(float angle) + { + float c = (float)System.Math.Cos(angle), s = (float)System.Math.Sin(angle); + Col1.X = c; Col2.X = -s; + Col1.Y = s; Col2.Y = c; + } + + /// <summary> + /// Initialize this matrix using columns. + /// </summary> + public void Set(Vec2 c1, Vec2 c2) + { + Col1 = c1; + Col2 = c2; + } + + /// <summary> + /// Initialize this matrix using an angle. + /// This matrix becomes an orthonormal rotation matrix. + /// </summary> + public void Set(float angle) + { + float c = (float)System.Math.Cos(angle), s = (float)System.Math.Sin(angle); + Col1.X = c; Col2.X = -s; + Col1.Y = s; Col2.Y = c; + } + + /// <summary> + /// Set this to the identity matrix. + /// </summary> + public void SetIdentity() + { + Col1.X = 1.0f; Col2.X = 0.0f; + Col1.Y = 0.0f; Col2.Y = 1.0f; + } + + /// <summary> + /// Set this matrix to all zeros. + /// </summary> + public void SetZero() + { + Col1.X = 0.0f; Col2.X = 0.0f; + Col1.Y = 0.0f; Col2.Y = 0.0f; + } + + /// <summary> + /// Extract the angle from this matrix (assumed to be a rotation matrix). + /// </summary> + public float GetAngle() + { + return (float)System.Math.Atan2(Col1.Y, Col1.X); + } + + /// <summary> + /// Compute the inverse of this matrix, such that inv(A) * A = identity. + /// </summary> + public Mat22 GetInverse() + { + float a = Col1.X, b = Col2.X, c = Col1.Y, d = Col2.Y; + Mat22 B = new Mat22(); + float det = a * d - b * c; + Box2DXDebug.Assert(det != 0.0f); + det = 1.0f / det; + B.Col1.X = det * d; B.Col2.X = -det * b; + B.Col1.Y = -det * c; B.Col2.Y = det * a; + return B; + } + + /// <summary> + /// Solve A * x = b, where b is a column vector. This is more efficient + /// than computing the inverse in one-shot cases. + /// </summary> + public Vec2 Solve(Vec2 b) + { + float a11 = Col1.X, a12 = Col2.X, a21 = Col1.Y, a22 = Col2.Y; + float det = a11 * a22 - a12 * a21; + Box2DXDebug.Assert(det != 0.0f); + det = 1.0f / det; + Vec2 x = new Vec2(); + x.X = det * (a22 * b.X - a12 * b.Y); + x.Y = det * (a11 * b.Y - a21 * b.X); + return x; + } + + public static Mat22 Identity { get { return new Mat22(1, 0, 0, 1); } } + + public static Mat22 operator +(Mat22 A, Mat22 B) + { + Mat22 C = new Mat22(); + C.Set(A.Col1 + B.Col1, A.Col2 + B.Col2); + return C; + } + } } diff --git a/Box2d/Assets/Program/Box2d/Common/Mat33.cs b/Box2d/Assets/Program/Box2d/Common/Mat33.cs index 13f5205..ffb6036 100644 --- a/Box2d/Assets/Program/Box2d/Common/Mat33.cs +++ b/Box2d/Assets/Program/Box2d/Common/Mat33.cs @@ -1,11 +1,90 @@ -using System; +/* + Box2DX Copyright (c) 2008 Ihar Kalasouski http://code.google.com/p/box2dx + Box2D original C++ version Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this 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. +*/ + +//r175 + +using System; using System.Collections.Generic; -using System.Linq; using System.Text; -namespace Assets.Program.Box2D.Common +namespace Box2DX.Common { - class Mat33 - { - } -} + /// <summary> + /// A 3-by-3 matrix. Stored in column-major order. + /// </summary> + public struct Mat33 + { + /// <summary> + /// Construct this matrix using columns. + /// </summary> + public Mat33(Vec3 c1, Vec3 c2, Vec3 c3) + { + Col1 = c1; + Col2 = c2; + Col3 = c3; + } + + /// <summary> + /// Set this matrix to all zeros. + /// </summary> + public void SetZero() + { + Col1.SetZero(); + Col2.SetZero(); + Col3.SetZero(); + } + + /// <summary> + /// Solve A * x = b, where b is a column vector. This is more efficient + /// than computing the inverse in one-shot cases. + /// </summary> + public Vec3 Solve33(Vec3 b) + { + float det = Vec3.Dot(Col1, Vec3.Cross(Col2, Col3)); + Box2DXDebug.Assert(det != 0.0f); + det = 1.0f / det; + Vec3 x = new Vec3(); + x.X = det * Vec3.Dot(b, Vec3.Cross(Col2, Col3)); + x.Y = det * Vec3.Dot(Col1, Vec3.Cross(b, Col3)); + x.Z = det * Vec3.Dot(Col1, Vec3.Cross(Col2, b)); + return x; + } + + /// <summary> + /// 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. + /// </summary> + public Vec2 Solve22(Vec2 b) + { + float a11 = Col1.X, a12 = Col2.X, a21 = Col1.Y, a22 = Col2.Y; + float det = a11 * a22 - a12 * a21; + Box2DXDebug.Assert(det != 0.0f); + det = 1.0f / det; + Vec2 x = new Vec2(); + x.X = det * (a22 * b.X - a12 * b.Y); + x.Y = det * (a11 * b.Y - a21 * b.X); + return x; + } + + public Vec3 Col1, Col2, Col3; + } +}
\ No newline at end of file diff --git a/Box2d/Assets/Program/Box2d/Common/Math.cs b/Box2d/Assets/Program/Box2d/Common/Math.cs index d3a5afc..8c35e3a 100644 --- a/Box2d/Assets/Program/Box2d/Common/Math.cs +++ b/Box2d/Assets/Program/Box2d/Common/Math.cs @@ -1,33 +1,265 @@ -using System; +/* + Box2DX Copyright (c) 2008 Ihar Kalasouski http://code.google.com/p/box2dx + Box2D original C++ version Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com -namespace Box2D + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this 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. +*/ + +using System; +using System.Collections.Generic; +using System.Text; + +namespace Box2DX.Common { + public class Math + { + public static readonly ushort USHRT_MAX = 0xffff; + public static readonly byte UCHAR_MAX = 0xff; + public static readonly int RAND_LIMIT = 32767; + + /// <summary> + /// This function is used to ensure that a floating point number is + /// not a NaN or infinity. + /// </summary> + public static bool IsValid(float x) + { + return !(float.IsNaN(x) || float.IsNegativeInfinity(x) || float.IsPositiveInfinity(x)); + } + + [System.Runtime.InteropServices.StructLayout(System.Runtime.InteropServices.LayoutKind.Explicit)] + public struct Convert + { + [System.Runtime.InteropServices.FieldOffset(0)] + public float x; + + [System.Runtime.InteropServices.FieldOffset(0)] + public int i; + } + + /// <summary> + /// This is a approximate yet fast inverse square-root. + /// </summary> + public static float InvSqrt(float x) + { + Convert convert = new Convert(); + convert.x = x; + float xhalf = 0.5f * x; + convert.i = 0x5f3759df - (convert.i >> 1); + x = convert.x; + x = x * (1.5f - xhalf * x * x); + return x; + } + + public static float Sqrt(float x) + { + return (float)System.Math.Sqrt(x); + } + + private static Random s_rnd = new Random(); + /// <summary> + /// Random number in range [-1,1] + /// </summary> + public static float Random() + { + float r = (float)(s_rnd.Next() & RAND_LIMIT); + r /= RAND_LIMIT; + r = 2.0f * r - 1.0f; + return r; + } + + /// <summary> + /// Random floating point number in range [lo, hi] + /// </summary> + public static float Random(float lo, float hi) + { + float r = (float)(s_rnd.Next() & RAND_LIMIT); + r /= RAND_LIMIT; + r = (hi - lo) * r + lo; + return r; + } + + /// <summary> + /// "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:" + /// </summary> + public static uint NextPowerOfTwo(uint x) + { + x |= (x >> 1); + x |= (x >> 2); + x |= (x >> 4); + x |= (x >> 8); + x |= (x >> 16); + return x + 1; + } + + public static bool IsPowerOfTwo(uint x) + { + bool result = x > 0 && (x & (x - 1)) == 0; + return result; + } + + public static float Abs(float a) + { + return a > 0.0f ? a : -a; + } + + public static Vec2 Abs(Vec2 a) + { + Vec2 b = new Vec2(); + b.Set(Math.Abs(a.X), Math.Abs(a.Y)); + return b; + } + + public static Mat22 Abs(Mat22 A) + { + Mat22 B = new Mat22(); + B.Set(Math.Abs(A.Col1), Math.Abs(A.Col2)); + return B; + } + + public static float Min(float a, float b) + { + return a < b ? a : b; + } + + public static int Min(int a, int b) + { + return a < b ? a : b; + } + + public static Vec2 Min(Vec2 a, Vec2 b) + { + Vec2 c = new Vec2(); + c.X = Math.Min(a.X, b.X); + c.Y = Math.Min(a.Y, b.Y); + return c; + } + + public static float Max(float a, float b) + { + return a > b ? a : b; + } + + public static int Max(int a, int b) + { + return a > b ? a : b; + } + + public static Vec2 Max(Vec2 a, Vec2 b) + { + Vec2 c = new Vec2(); + c.X = Math.Max(a.X, b.X); + c.Y = Math.Max(a.Y, b.Y); + return c; + } + + public static float Clamp(float a, float low, float high) + { + return Math.Max(low, Math.Min(a, high)); + } + + public static int Clamp(int a, int low, int high) + { + return Math.Max(low, Math.Min(a, high)); + } + + public static Vec2 Clamp(Vec2 a, Vec2 low, Vec2 high) + { + return Math.Max(low, Math.Min(a, high)); + } + + public static void Swap<T>(ref T a, ref T b) + { + T tmp = a; + a = b; + b = tmp; + } + + /// <summary> + /// Multiply a matrix times a vector. If a rotation matrix is provided, + /// then this transforms the vector from one frame to another. + /// </summary> + public static Vec2 Mul(Mat22 A, Vec2 v) + { + Vec2 u = new Vec2(); + u.Set(A.Col1.X * v.X + A.Col2.X * v.Y, A.Col1.Y * v.X + A.Col2.Y * v.Y); + return u; + } + + /// <summary> + /// 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). + /// </summary> + public static Vec2 MulT(Mat22 A, Vec2 v) + { + Vec2 u = new Vec2(); + u.Set(Vec2.Dot(v, A.Col1), Vec2.Dot(v, A.Col2)); + return u; + } + + /// <summary> + /// A * B + /// </summary> + public static Mat22 Mul(Mat22 A, Mat22 B) + { + Mat22 C = new Mat22(); + C.Set(Math.Mul(A, B.Col1), Math.Mul(A, B.Col2)); + return C; + } + + /// <summary> + /// A^T * B + /// </summary> + public static Mat22 MulT(Mat22 A, Mat22 B) + { + Vec2 c1 = new Vec2(); + c1.Set(Vec2.Dot(A.Col1, B.Col1), Vec2.Dot(A.Col2, B.Col1)); + Vec2 c2 = new Vec2(); + c2.Set(Vec2.Dot(A.Col1, B.Col2), Vec2.Dot(A.Col2, B.Col2)); + Mat22 C = new Mat22(); + C.Set(c1, c2); + return C; + } + + public static Vec2 Mul(XForm T, Vec2 v) + { + return T.Position + Math.Mul(T.R, v); + } + + public static Vec2 MulT(XForm T, Vec2 v) + { + return Math.MulT(T.R, v - T.Position); + } - /// <summary> - /// common functions - /// </summary> - class Math - { - /// This function is used to ensure that a floating point number is not a NaN or infinity. - public static bool IsValid(float x) - { - return x != float.PositiveInfinity - && x != float.NaN - && x != float.NegativeInfinity; - } - - /// This is a approximate yet fast inverse square-root. - public static float InvSqrt(float x) - { - //float xhalf = 0.5f * x; - //int i = BitConverter.ToInt32(BitConverter.GetBytes(x), 0); - //i = 0x5f3759df - (i >> 1); - //x = BitConverter.ToSingle(BitConverter.GetBytes(i), 0); - //x = x * (1.5f - xhalf * x * x); - //return x; - return Convert.ToSingle(1 / System.Math.Sqrt(x)); - } - - } + /// <summary> + /// Multiply a matrix times a vector. + /// </summary> + public static Vec3 Mul(Mat33 A, Vec3 v) + { + Vec3 u = v.X * A.Col1 + v.Y * A.Col2 + v.Z * A.Col3; + return u; + } + public static float Atan2(float y, float x) + { + return (float)System.Math.Atan2(y, x); + } + } } diff --git a/Box2d/Assets/Program/Box2d/Common/Settings.cs b/Box2d/Assets/Program/Box2d/Common/Settings.cs new file mode 100644 index 0000000..d9184ad --- /dev/null +++ b/Box2d/Assets/Program/Box2d/Common/Settings.cs @@ -0,0 +1,181 @@ +/* + Box2DX Copyright (c) 2008 Ihar Kalasouski http://code.google.com/p/box2dx + Box2D original C++ version Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this 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. +*/ + +using System; +using System.Collections.Generic; +using System.Text; + +namespace Box2DX.Common +{ + public class Settings + { +#if TARGET_FLOAT32_IS_FIXED + public static readonly float FLT_EPSILON = FIXED_EPSILON; + public static readonly float FLT_MAX = FIXED_MAX; + public static float FORCE_SCALE2(x){ return x<<7;} + public static float FORCE_INV_SCALE2(x) {return x>>7;} +#else + public static readonly float FLT_EPSILON = 1.192092896e-07F;//smallest such that 1.0f+FLT_EPSILON != 1.0f + public static readonly float FLT_EPSILON_SQUARED = FLT_EPSILON * FLT_EPSILON;//smallest such that 1.0f+FLT_EPSILON != 1.0f + public static readonly float FLT_MAX = 3.402823466e+38F; + public static float FORCE_SCALE(float x) { return x; } + public static float FORCE_INV_SCALE(float x) { return x; } +#endif + + public static readonly float Pi = 3.14159265359f; + + // Global tuning constants based on meters-kilograms-seconds (MKS) units. + + // Collision + public static readonly int MaxManifoldPoints = 2; + public static readonly int MaxPolygonVertices = 8; + public static readonly int MaxProxies = 512; // this must be a power of two + public static readonly int MaxPairs = 8 * MaxProxies; // this must be a power of two + + // Dynamics + + /// <summary> + /// A small length used as a collision and constraint tolerance. Usually it is + /// chosen to be numerically significant, but visually insignificant. + /// </summary> + public static readonly float LinearSlop = 0.005f; // 0.5 cm + + /// <summary> + /// A small angle used as a collision and constraint tolerance. Usually it is + /// chosen to be numerically significant, but visually insignificant. + /// </summary> + public static readonly float AngularSlop = 2.0f / 180.0f * Pi; // 2 degrees + + /// <summary> + /// The radius of the polygon/edge shape skin. This should not be modified. Making + /// this smaller means polygons will have and insufficient for continuous collision. + /// Making it larger may create artifacts for vertex collision. + /// </summary> + public static readonly float PolygonRadius = 2.0f * LinearSlop; + + /// <summary> + /// Continuous collision detection (CCD) works with core, shrunken shapes. This is amount + /// by which shapes are automatically shrunk to work with CCD. + /// This must be larger than LinearSlop. + /// </summary> + public static readonly float ToiSlop = 8.0f * LinearSlop; + + /// <summary> + /// Maximum number of contacts to be handled to solve a TOI island. + /// </summary> + public static readonly int MaxTOIContactsPerIsland = 32; + + /// <summary> + /// Maximum number of joints to be handled to solve a TOI island. + /// </summary> + public static readonly int MaxTOIJointsPerIsland = 32; + + /// <summary> + /// A velocity threshold for elastic collisions. Any collision with a relative linear + /// velocity below this threshold will be treated as inelastic. + /// </summary> + public static readonly float VelocityThreshold = 1.0f; // 1 m/s + + /// <summary> + /// The maximum linear position correction used when solving constraints. + /// This helps to prevent overshoot. + /// </summary> + public static readonly float MaxLinearCorrection = 0.2f; // 20 cm + + /// <summary> + /// The maximum angular position correction used when solving constraints. + /// This helps to prevent overshoot. + /// </summary> + public static readonly float MaxAngularCorrection = 8.0f / 180.0f * Pi; // 8 degrees + + /// <summary> + /// The maximum linear velocity of a body. This limit is very large and is used + /// to prevent numerical problems. You shouldn't need to adjust this. + /// </summary> +#if TARGET_FLOAT32_IS_FIXED + public static readonly float MaxLinearVelocity = 100.0f; +#else + public static readonly float MaxLinearVelocity = 200.0f; + public static readonly float MaxLinearVelocitySquared = MaxLinearVelocity * MaxLinearVelocity; +#endif + /// <summary> + /// The maximum angular velocity of a body. This limit is very large and is used + /// to prevent numerical problems. You shouldn't need to adjust this. + /// </summary> + public static readonly float MaxAngularVelocity = 250.0f; +#if !TARGET_FLOAT32_IS_FIXED + public static readonly float MaxAngularVelocitySquared = MaxAngularVelocity * MaxAngularVelocity; +#endif + + /// <summary> + /// The maximum linear velocity of a body. This limit is very large and is used + /// to prevent numerical problems. You shouldn't need to adjust this. + /// </summary> + public static readonly float MaxTranslation = 2.0f; + public static readonly float MaxTranslationSquared = (MaxTranslation * MaxTranslation); + + /// <summary> + /// The maximum angular velocity of a body. This limit is very large and is used + /// to prevent numerical problems. You shouldn't need to adjust this. + /// </summary> + public static readonly float MaxRotation = (0.5f * Pi); + public static readonly float MaxRotationSquared = (MaxRotation * MaxRotation); + + /// <summary> + /// This scale factor controls how fast overlap is resolved. Ideally this would be 1 so + /// that overlap is removed in one time step. However using values close to 1 often lead to overshoot. + /// </summary> + public static readonly float ContactBaumgarte = 0.2f; + + // Sleep + + /// <summary> + /// The time that a body must be still before it will go to sleep. + /// </summary> + public static readonly float TimeToSleep = 0.5f; // half a second + + /// <summary> + /// A body cannot sleep if its linear velocity is above this tolerance. + /// </summary> + public static readonly float LinearSleepTolerance = 0.01f; // 1 cm/s + + /// <summary> + /// A body cannot sleep if its angular velocity is above this tolerance. + /// </summary> + public static readonly float AngularSleepTolerance = 2.0f / 180.0f; // 2 degrees/s + + /// <summary> + /// Friction mixing law. Feel free to customize this. + /// </summary> + public static float MixFriction(float friction1, float friction2) + { + return (float)System.Math.Sqrt(friction1 * friction2); + } + + /// <summary> + /// Restitution mixing law. Feel free to customize this. + /// </summary> + public static float MixRestitution(float restitution1, float restitution2) + { + return restitution1 > restitution2 ? restitution1 : restitution2; + } + } +} diff --git a/Box2d/Assets/Program/Box2d/Common/Settings.cs.meta b/Box2d/Assets/Program/Box2d/Common/Settings.cs.meta new file mode 100644 index 0000000..68e59da --- /dev/null +++ b/Box2d/Assets/Program/Box2d/Common/Settings.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: ab0be1eda9abcd94ca9b9e1ca92f6f1b +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Box2d/Assets/Program/Box2d/Common/Sweep.cs b/Box2d/Assets/Program/Box2d/Common/Sweep.cs index ba1fd07..09e9560 100644 --- a/Box2d/Assets/Program/Box2d/Common/Sweep.cs +++ b/Box2d/Assets/Program/Box2d/Common/Sweep.cs @@ -1,11 +1,67 @@ -using System; +/* + Box2DX Copyright (c) 2008 Ihar Kalasouski http://code.google.com/p/box2dx + Box2D original C++ version Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this 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. +*/ + +//r175 + +using System; using System.Collections.Generic; -using System.Linq; using System.Text; -namespace Assets.Program.Box2D.Common +namespace Box2DX.Common { - class Sweep - { - } + public struct Sweep + { + public Vec2 LocalCenter; //local center of mass position + public Vec2 C0, C; //local center of mass position + public float A0, A; //world angles + public float T0; //time interval = [T0,1], where T0 is in [0,1] + + /// <summary> + /// Get the interpolated transform at a specific time. + /// </summary> + /// <param name="alpha">Alpha is a factor in [0,1], where 0 indicates t0.</param> + public void GetTransform(out XForm xf, float alpha) + { + xf = new XForm(); + xf.Position = (1.0f - alpha) * C0 + alpha * C; + float angle = (1.0f - alpha) * A0 + alpha * A; + xf.R.Set(angle); + + // Shift to origin + xf.Position -= Common.Math.Mul(xf.R, LocalCenter); + } + + /// <summary> + /// Advance the sweep forward, yielding a new initial state. + /// </summary> + /// <param name="t">The new initial time.</param> + public void Advance(float t) + { + if (T0 < t && 1.0f - T0 > Settings.FLT_EPSILON) + { + float alpha = (t - T0) / (1.0f - T0); + C0 = (1.0f - alpha) * C0 + alpha * C; + A0 = (1.0f - alpha) * A0 + alpha * A; + T0 = t; + } + } + } } diff --git a/Box2d/Assets/Program/Box2d/Common/Transform.cs b/Box2d/Assets/Program/Box2d/Common/Transform.cs deleted file mode 100644 index b244fb0..0000000 --- a/Box2d/Assets/Program/Box2d/Common/Transform.cs +++ /dev/null @@ -1,23 +0,0 @@ -using System; -using System.Collections.Generic; -using System.Linq; -using System.Text; - -namespace Box2D -{ - class Transform - { - public Transform() - { - - } - - public void Initialize() - { - - } - - public Vec2 position; - public Mat22 R; - } -} diff --git a/Box2d/Assets/Program/Box2d/Common/Vec2.cs b/Box2d/Assets/Program/Box2d/Common/Vec2.cs index ab803dd..d614845 100644 --- a/Box2d/Assets/Program/Box2d/Common/Vec2.cs +++ b/Box2d/Assets/Program/Box2d/Common/Vec2.cs @@ -1,113 +1,230 @@ -using System; - -namespace Box2D +/* + Box2DX Copyright (c) 2008 Ihar Kalasouski http://code.google.com/p/box2dx + Box2D original C++ version Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this 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. +*/ + +using System; +using System.Collections.Generic; +using System.Text; + +namespace Box2DX.Common { - class Vec2 - { - public Vec2(float x, float y) - { - this.x = x; - this.y = y; - } - - public Vec2() - { - this.x = 0; - this.y = 0; - } - - public void SetZero() - { - x = 0; - y = 0; - } - - public void Set(float _x, float _y) - { - x = _x; - y = _y; - } - - public void Set(Vec2 v) - { - x = v.x; - y = v.y; - } - - public Vec2 GetNegtive() - { - return new Vec2(-x, -y); - } - - public Vec2 Copy() - { - return new Vec2(x, y); - } - - public Vec2 Clone() - { - return new Vec2(x, y); - } - - public static Vec2 operator -(Vec2 l, Vec2 r) - { - Vec2 v = new Vec2(l.x - r.x, l.y - r.y); - return v; - } - - public static Vec2 operator +(Vec2 l, Vec2 r) - { - Vec2 v = new Vec2(l.x + r.x, l.y + r.y); - return v; - } - - public static Vec2 operator *(Vec2 l, float a) - { - return new Vec2(l.x * a, l.y * a); - } - - public float Length() - { - return Convert.ToSingle(System.Math.Sqrt(x * x + y * y)); - } - - public float LengthSquared() - { - return x * x + y * y; - } - - /// Convert this vector into a unit vector. Returns the length. - public float Normalize() - { - float length = Length(); - if(length < float.Epsilon) - { - return 0; - } - float invLength = 1 / length; - x *= invLength; - y *= invLength; - return length; - } - - /// Does this vector contain finite coordinates? - public bool IsValid() - { - return Math.IsValid(x) && Math.IsValid(y); - } - - /// Get the skew vector such that dot(skew_vec, other) == cross(vec, other) - public Vec2 Skew() - { - return new Vec2(-y, x); - } - - public void Abs() - { - if (x < 0) x = -x; - if (y < 0) y = -y; - } - - public float x, y; - } + /// <summary> + /// A 2D column vector. + /// </summary> + public struct Vec2 + { + public float X, Y; + + public float this[int i] + { + get + { + if (i == 0) return X; + else if (i == 1) return Y; + else + { + Box2DXDebug.Assert(false, "Incorrect Vec2 element!"); + return 0; + } + } + set + { + if (i == 0) X = value; + else if (i == 1) Y = value; + else + { + Box2DXDebug.Assert(false, "Incorrect Vec2 element!"); + } + } + } + + /// <summary> + /// Construct using coordinates. + /// </summary> + public Vec2(float x) + { + X = x; + Y = x; + } + + /// <summary> + /// Construct using coordinates. + /// </summary> + public Vec2(float x, float y) + { + X = x; + Y = y; + } + + /// <summary> + /// Set this vector to all zeros. + /// </summary> + public void SetZero() { X = 0.0f; Y = 0.0f; } + + /// <summary> + /// Set this vector to some specified coordinates. + /// </summary> + public void Set(float x, float y) { X = x; Y = y; } + + public void Set(float xy) { X = xy; Y = xy; } + + /// <summary> + /// Get the length of this vector (the norm). + /// </summary> + public float Length() + { + return (float)System.Math.Sqrt(X * X + Y * Y); + } + + /// <summary> + /// Get the length squared. For performance, use this instead of + /// Length (if possible). + /// </summary> + public float LengthSquared() + { + return X * X + Y * Y; + } + + /// <summary> + /// Convert this vector into a unit vector. Returns the length. + /// </summary> + public float Normalize() + { + float length = Length(); + if (length < Settings.FLT_EPSILON) + { + return 0.0f; + } + float invLength = 1.0f / length; + X *= invLength; + Y *= invLength; + + return length; + } + + /// <summary> + /// Does this vector contain finite coordinates? + /// </summary> + public bool IsValid + { + get { return Math.IsValid(X) && Math.IsValid(Y); } + } + + /// <summary> + /// Negate this vector. + /// </summary> + public static Vec2 operator -(Vec2 v1) + { + Vec2 v = new Vec2(); + v.Set(-v1.X, -v1.Y); + return v; + } + + public static Vec2 operator +(Vec2 v1, Vec2 v2) + { + Vec2 v = new Vec2(); + v.Set(v1.X + v2.X, v1.Y + v2.Y); + return v; + } + + public static Vec2 operator -(Vec2 v1, Vec2 v2) + { + Vec2 v = new Vec2(); + v.Set(v1.X - v2.X, v1.Y - v2.Y); + return v; + } + + public static Vec2 operator *(Vec2 v1, float a) + { + Vec2 v = new Vec2(); + v.Set(v1.X * a, v1.Y * a); + return v; + } + + public static Vec2 operator *(float a, Vec2 v1) + { + Vec2 v = new Vec2(); + v.Set(v1.X * a, v1.Y * a); + return v; + } + + public static bool operator ==(Vec2 a, Vec2 b) + { + return a.X == b.X && a.Y == b.Y; + } + + public static bool operator !=(Vec2 a, Vec2 b) + { + return a.X != b.X || a.Y != b.Y; + } + + public static Vec2 Zero { get { return new Vec2(0, 0); } } + + /// <summary> + /// Peform the dot product on two vectors. + /// </summary> + public static float Dot(Vec2 a, Vec2 b) + { + return a.X * b.X + a.Y * b.Y; + } + + /// <summary> + /// Perform the cross product on two vectors. In 2D this produces a scalar. + /// </summary> + public static float Cross(Vec2 a, Vec2 b) + { + return a.X * b.Y - a.Y * b.X; + } + + /// <summary> + /// Perform the cross product on a vector and a scalar. + /// In 2D this produces a vector. + /// </summary> + public static Vec2 Cross(Vec2 a, float s) + { + Vec2 v = new Vec2(); + v.Set(s * a.Y, -s * a.X); + return v; + } + + /// <summary> + /// Perform the cross product on a scalar and a vector. + /// In 2D this produces a vector. + /// </summary> + public static Vec2 Cross(float s, Vec2 a) + { + Vec2 v = new Vec2(); + v.Set(-s * a.Y, s * a.X); + return v; + } + + public static float Distance(Vec2 a, Vec2 b) + { + Vec2 c = a - b; + return c.Length(); + } + + public static float DistanceSquared(Vec2 a, Vec2 b) + { + Vec2 c = a - b; + return Vec2.Dot(c, c); + } + } } diff --git a/Box2d/Assets/Program/Box2d/Common/Vec3.cs b/Box2d/Assets/Program/Box2d/Common/Vec3.cs index d4bb361..f702021 100644 --- a/Box2d/Assets/Program/Box2d/Common/Vec3.cs +++ b/Box2d/Assets/Program/Box2d/Common/Vec3.cs @@ -1,77 +1,108 @@ -using System; +/* + Box2DX Copyright (c) 2008 Ihar Kalasouski http://code.google.com/p/box2dx + Box2D original C++ version Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this 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. +*/ + +//r175 + +using System; using System.Collections.Generic; -using System.Linq; using System.Text; -namespace Box2D +namespace Box2DX.Common { - class Vec3 - { - public Vec3() - { - x = 0; - y = 0; - z = 0; - } - - public Vec3(float _x, float _y, float _z) - { - x = _x; - y = _y; - z = _z; - } - - public void SetZero() - { - x = y = z = 0.0f; - } - - public void Set(float _x, float _y, float _z) - { - x = _x; - y = _y; - z = _z; - } - - public static Vec3 operator -(Vec3 l, Vec3 r) - { - Vec3 v = new Vec3(l.x - r.x, l.y - r.y, l.z - r.z); - return v; - } - - public static Vec3 operator +(Vec3 l, Vec3 r) - { - Vec3 v = new Vec3(l.x + r.x, l.y + r.y, l.z + r.z); - return v; - } - - public static Vec3 operator *(Vec3 l, float a) - { - Vec3 v = new Vec3(l.x * a, l.y * a, l.z * a); - return v; - } - - public void Add(Vec3 r) - { - x += r.x; - y += r.y; - z += r.z; - } - - public void Minus(Vec3 r) - { - x -= r.x; - y -= r.y; - z -= r.z; - } - - public void Multiply(float a) - { - x *= a; - y *= a; - z *= a; - } - - public float x, y, z; - } -} + /// <summary> + /// A 2D column vector with 3 elements. + /// </summary> + public struct Vec3 + { + /// <summary> + /// Construct using coordinates. + /// </summary> + public Vec3(float x, float y, float z) { X = x; Y = y; Z = z; } + + /// <summary> + /// Set this vector to all zeros. + /// </summary> + public void SetZero() { X = 0.0f; Y = 0.0f; Z = 0.0f; } + + /// <summary> + /// Set this vector to some specified coordinates. + /// </summary> + public void Set(float x, float y, float z) { X = x; Y = y; Z = z; } + + /// <summary> + /// Perform the dot product on two vectors. + /// </summary> + public static float Dot(Vec3 a, Vec3 b) + { + return a.X * b.X + a.Y * b.Y + a.Z * b.Z; + } + + /// <summary> + /// Perform the cross product on two vectors. + /// </summary> + public static Vec3 Cross(Vec3 a, Vec3 b) + { + return new Vec3(a.Y * b.Z - a.Z * b.Y, a.Z * b.X - a.X * b.Z, a.X * b.Y - a.Y * b.X); + } + + /// <summary> + /// Negate this vector. + /// </summary> + public static Vec3 operator -(Vec3 v) + { + return new Vec3(-v.X, -v.Y, -v.Z); + } + + /// <summary> + /// Add two vectors component-wise. + /// </summary> + public static Vec3 operator +(Vec3 v1, Vec3 v2) + { + return new Vec3(v1.X + v2.X, v1.Y + v2.Y, v1.Z + v2.Z); + } + + /// <summary> + /// Subtract two vectors component-wise. + /// </summary> + public static Vec3 operator -(Vec3 v1, Vec3 v2) + { + return new Vec3(v1.X - v2.X, v1.Y - v2.Y, v1.Z - v2.Z); + } + + /// <summary> + /// Multiply this vector by a scalar. + /// </summary> + public static Vec3 operator *(Vec3 v, float s) + { + return new Vec3(v.X * s, v.Y * s, v.Z * s); + } + + /// <summary> + /// Multiply this vector by a scalar. + /// </summary> + public static Vec3 operator *(float s, Vec3 v) + { + return new Vec3(v.X * s, v.Y * s, v.Z * s); + } + + public float X, Y, Z; + } +}
\ No newline at end of file diff --git a/Box2d/Assets/Program/Box2d/Common/Vec3.cs.meta b/Box2d/Assets/Program/Box2d/Common/Vec3.cs.meta new file mode 100644 index 0000000..5f28808 --- /dev/null +++ b/Box2d/Assets/Program/Box2d/Common/Vec3.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 35d36f97a10c8e4478fc2d9a1fc36613 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Box2d/Assets/Program/Box2d/Common/XForm.cs b/Box2d/Assets/Program/Box2d/Common/XForm.cs new file mode 100644 index 0000000..de507b5 --- /dev/null +++ b/Box2d/Assets/Program/Box2d/Common/XForm.cs @@ -0,0 +1,72 @@ +/* + Box2DX Copyright (c) 2008 Ihar Kalasouski http://code.google.com/p/box2dx + Box2D original C++ version Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this 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. +*/ + +using System; +using System.Collections.Generic; +using System.Text; + +namespace Box2DX.Common +{ + /// <summary> + /// A transform contains translation and rotation. + /// It is used to represent the position and orientation of rigid frames. + /// </summary> + public struct XForm + { + public Vec2 Position; + public Mat22 R; + + /// <summary> + /// Initialize using a position vector and a rotation matrix. + /// </summary> + /// <param name="position"></param> + /// <param name="R"></param> + public XForm(Vec2 position, Mat22 rotation) + { + Position = position; + R = rotation; + } + + /// <summary> + /// Set this to the identity transform. + /// </summary> + public void SetIdentity() + { + Position.SetZero(); + R.SetIdentity(); + } + + /// Set this based on the position and angle. + public void Set(Vec2 p, float angle) + { + Position = p; + R.Set(angle); + } + + /// Calculate the angle that the rotation matrix represents. + public float GetAngle() + { + return Math.Atan2(R.Col1.Y, R.Col1.X); + } + + public static XForm Identity { get { return new XForm(Vec2.Zero, Mat22.Identity); } } + } +} diff --git a/Box2d/Assets/Program/Box2d/Common/XForm.cs.meta b/Box2d/Assets/Program/Box2d/Common/XForm.cs.meta new file mode 100644 index 0000000..082e1aa --- /dev/null +++ b/Box2d/Assets/Program/Box2d/Common/XForm.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 1808091e7f7554e4a8925c67159c00e7 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Body.cs b/Box2d/Assets/Program/Box2d/Dynamics/Body.cs index 85ae672..1daa9a5 100644 --- a/Box2d/Assets/Program/Box2d/Dynamics/Body.cs +++ b/Box2d/Assets/Program/Box2d/Dynamics/Body.cs @@ -1,7 +1,1070 @@ -using System.Collections; -using System.Collections.Generic; +/* + Box2DX Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx + Box2D original C++ version Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com -namespace Box2D + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this 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. +*/ + +using System; + +using Box2DX.Common; +using Box2DX.Collision; + +namespace Box2DX.Dynamics { + /// <summary> + /// A body definition holds all the data needed to construct a rigid body. + /// You can safely re-use body definitions. + /// </summary> + public struct BodyDef + { + /// <summary> + /// This constructor sets the body definition default values. + /// </summary> + public BodyDef(byte init) + { + MassData = new MassData(); + MassData.Center.SetZero(); + MassData.Mass = 0.0f; + MassData.I = 0.0f; + UserData = null; + Position = new Vec2(); + Position.Set(0.0f, 0.0f); + Angle = 0.0f; + LinearVelocity = new Vec2(0f, 0f); + AngularVelocity = 0.0f; + LinearDamping = 0.0f; + AngularDamping = 0.0f; + AllowSleep = true; + IsSleeping = false; + FixedRotation = false; + IsBullet = false; + } + + /// <summary> + /// You can use this to initialized the mass properties of the body. + /// If you prefer, you can set the mass properties after the shapes + /// have been added using Body.SetMassFromShapes. + /// </summary> + public MassData MassData; + + /// <summary> + /// Use this to store application specific body data. + /// </summary> + public object UserData; + + /// <summary> + /// The world position of the body. Avoid creating bodies at the origin + /// since this can lead to many overlapping shapes. + /// </summary> + public Vec2 Position; + + /// <summary> + /// The world angle of the body in radians. + /// </summary> + public float Angle; + + /// The linear velocity of the body in world co-ordinates. + public Vec2 LinearVelocity; + + // The angular velocity of the body. + public float AngularVelocity; + + /// <summary> + /// Linear damping is use to reduce the linear velocity. The damping parameter + /// can be larger than 1.0f but the damping effect becomes sensitive to the + /// time step when the damping parameter is large. + /// </summary> + public float LinearDamping; + + /// <summary> + /// Angular damping is use to reduce the angular velocity. The damping parameter + /// can be larger than 1.0f but the damping effect becomes sensitive to the + /// time step when the damping parameter is large. + /// </summary> + public float AngularDamping; + + /// <summary> + /// Set this flag to false if this body should never fall asleep. Note that + /// this increases CPU usage. + /// </summary> + public bool AllowSleep; + + /// <summary> + /// Is this body initially sleeping? + /// </summary> + public bool IsSleeping; + + /// <summary> + /// Should this body be prevented from rotating? Useful for characters. + /// </summary> + public bool FixedRotation; + + /// <summary> + /// Is this a fast moving body that should be prevented from tunneling through + /// other moving bodies? Note that all bodies are prevented from tunneling through + /// static bodies. + /// @warning You should use this flag sparingly since it increases processing time. + /// </summary> + public bool IsBullet; + } + + /// <summary> + /// A rigid body. These are created via World.CreateBody. + /// </summary> + public class Body : IDisposable + { + [Flags] + public enum BodyFlags + { + Frozen = 0x0002, + Island = 0x0004, + Sleep = 0x0008, + AllowSleep = 0x0010, + Bullet = 0x0020, + FixedRotation = 0x0040 + } + + public enum BodyType + { + Static, + Dynamic, + MaxTypes + } + + internal BodyFlags _flags; + private BodyType _type; + + internal int _islandIndex; + + internal XForm _xf; // the body origin transform + + internal Sweep _sweep; // the swept motion for CCD + + internal Vec2 _linearVelocity; + internal float _angularVelocity; + + internal Vec2 _force; + internal float _torque; + + private World _world; + internal Body _prev; + internal Body _next; + + internal Fixture _fixtureList; + internal int _fixtureCount; + + internal JointEdge _jointList; + internal ContactEdge _contactList; + + internal Controllers.ControllerEdge _controllerList; + + internal float _mass; + internal float _invMass; + internal float _I; + internal float _invI; + + internal float _linearDamping; + internal float _angularDamping; + + internal float _sleepTime; + + private object _userData; + + internal Body(BodyDef bd, World world) + { + Box2DXDebug.Assert(world._lock == false); + + _flags = 0; + + if (bd.IsBullet) + { + _flags |= BodyFlags.Bullet; + } + if (bd.FixedRotation) + { + _flags |= BodyFlags.FixedRotation; + } + if (bd.AllowSleep) + { + _flags |= BodyFlags.AllowSleep; + } + if (bd.IsSleeping) + { + _flags |= BodyFlags.Sleep; + } + + _world = world; + + _xf.Position = bd.Position; + _xf.R.Set(bd.Angle); + + _sweep.LocalCenter = bd.MassData.Center; + _sweep.T0 = 1.0f; + _sweep.A0 = _sweep.A = bd.Angle; + _sweep.C0 = _sweep.C = Common.Math.Mul(_xf, _sweep.LocalCenter); + + //_jointList = null; + //_contactList = null; + //_controllerList = null; + //_prev = null; + //_next = null; + + _linearVelocity = bd.LinearVelocity; + _angularVelocity = bd.AngularVelocity; + + _linearDamping = bd.LinearDamping; + _angularDamping = bd.AngularDamping; + + //_force.Set(0.0f, 0.0f); + //_torque = 0.0f; + + //_linearVelocity.SetZero(); + //_angularVelocity = 0.0f; + + //_sleepTime = 0.0f; + + //_invMass = 0.0f; + //_I = 0.0f; + //_invI = 0.0f; + + _mass = bd.MassData.Mass; + + if (_mass > 0.0f) + { + _invMass = 1.0f / _mass; + } + + _I = bd.MassData.I; + + if (_I > 0.0f && (_flags & BodyFlags.FixedRotation) == 0) + { + _invI = 1.0f / _I; + } + + if (_invMass == 0.0f && _invI == 0.0f) + { + _type = BodyType.Static; + } + else + { + _type = BodyType.Dynamic; + } + + _userData = bd.UserData; + + //_fixtureList = null; + //_fixtureCount = 0; + } + + public void Dispose() + { + Box2DXDebug.Assert(_world._lock == false); + // shapes and joints are destroyed in World.Destroy + } + + internal bool SynchronizeFixtures() + { + XForm xf1 = new XForm(); + xf1.R.Set(_sweep.A0); + xf1.Position = _sweep.C0 - Common.Math.Mul(xf1.R, _sweep.LocalCenter); + + bool inRange = true; + for (Fixture f = _fixtureList; f != null; f = f.Next) + { + inRange = f.Synchronize(_world._broadPhase, xf1, _xf); + if (inRange == false) + { + break; + } + } + + if (inRange == false) + { + _flags |= BodyFlags.Frozen; + _linearVelocity.SetZero(); + _angularVelocity = 0.0f; + + // Failure + return false; + } + + // Success + return true; + } + + // This is used to prevent connected bodies from colliding. + // It may lie, depending on the collideConnected flag. + internal bool IsConnected(Body other) + { + for (JointEdge jn = _jointList; jn != null; jn = jn.Next) + { + if (jn.Other == other) + return jn.Joint._collideConnected == false; + } + + return false; + } + + /// <summary> + /// Creates a fixture and attach it to this body. + /// @warning This function is locked during callbacks. + /// </summary> + /// <param name="def">The fixture definition.</param> + public Fixture CreateFixture(FixtureDef def) + { + Box2DXDebug.Assert(_world._lock == false); + if (_world._lock == true) + { + return null; + } + + BroadPhase broadPhase = _world._broadPhase; + + Fixture fixture = new Fixture(); + fixture.Create(broadPhase, this, _xf, def); + + fixture._next = _fixtureList; + _fixtureList = fixture; + ++_fixtureCount; + + fixture._body = this; + + return fixture; + } + + /// <summary> + /// Destroy a fixture. This removes the fixture from the broad-phase and + /// therefore destroys any contacts associated with this fixture. All fixtures + /// attached to a body are implicitly destroyed when the body is destroyed. + /// @warning This function is locked during callbacks. + /// </summary> + /// <param name="fixture">The fixture to be removed.</param> + public void DestroyFixture(Fixture fixture) + { + Box2DXDebug.Assert(_world._lock == false); + if (_world._lock == true) + { + return; + } + + Box2DXDebug.Assert(fixture.Body == this); + + // Remove the fixture from this body's singly linked list. + Box2DXDebug.Assert(_fixtureCount > 0); + Fixture node = _fixtureList; + bool found = false; + while (node != null) + { + if (node == fixture) + { + //*node = fixture->m_next; + _fixtureList = fixture.Next; + found = true; + break; + } + + node = node.Next; + } + + // You tried to remove a shape that is not attached to this body. + Box2DXDebug.Assert(found); + + BroadPhase broadPhase = _world._broadPhase; + + fixture.Destroy(broadPhase); + fixture._body = null; + fixture._next = null; + + --_fixtureCount; + } + + // TODO_ERIN adjust linear velocity and torque to account for movement of center. + /// <summary> + /// Set the mass properties. Note that this changes the center of mass position. + /// If you are not sure how to compute mass properties, use SetMassFromShapes. + /// The inertia tensor is assumed to be relative to the center of mass. + /// </summary> + /// <param name="massData">The mass properties.</param> + public void SetMass(MassData massData) + { + Box2DXDebug.Assert(_world._lock == false); + if (_world._lock == true) + { + return; + } + + _invMass = 0.0f; + _I = 0.0f; + _invI = 0.0f; + + _mass = massData.Mass; + + if (_mass > 0.0f) + { + _invMass = 1.0f / _mass; + } + + _I = massData.I; + + if (_I > 0.0f && (_flags & BodyFlags.FixedRotation) == 0) + { + _invI = 1.0f / _I; + } + + // Move center of mass. + _sweep.LocalCenter = massData.Center; + _sweep.C0 = _sweep.C = Common.Math.Mul(_xf, _sweep.LocalCenter); + + BodyType oldType = _type; + if (_invMass == 0.0f && _invI == 0.0f) + { + _type = BodyType.Static; + } + else + { + _type = BodyType.Dynamic; + } + + // If the body type changed, we need to refilter the broad-phase proxies. + if (oldType != _type) + { + for (Fixture f = _fixtureList; f != null; f = f.Next) + { + f.RefilterProxy(_world._broadPhase, _xf); + } + } + } + + // TODO_ERIN adjust linear velocity and torque to account for movement of center. + /// <summary> + /// Compute the mass properties from the attached shapes. You typically call this + /// after adding all the shapes. If you add or remove shapes later, you may want + /// to call this again. Note that this changes the center of mass position. + /// </summary> + public void SetMassFromShapes() + { + Box2DXDebug.Assert(_world._lock == false); + if (_world._lock == true) + { + return; + } + + // Compute mass data from shapes. Each shape has its own density. + _mass = 0.0f; + _invMass = 0.0f; + _I = 0.0f; + _invI = 0.0f; + + Vec2 center = Vec2.Zero; + for (Fixture f = _fixtureList; f != null; f = f.Next) + { + MassData massData; + f.ComputeMass(out massData); + _mass += massData.Mass; + center += massData.Mass * massData.Center; + _I += massData.I; + } + + // Compute center of mass, and shift the origin to the COM. + if (_mass > 0.0f) + { + _invMass = 1.0f / _mass; + center *= _invMass; + } + + if (_I > 0.0f && (_flags & BodyFlags.FixedRotation) == 0) + { + // Center the inertia about the center of mass. + _I -= _mass * Vec2.Dot(center, center); + Box2DXDebug.Assert(_I > 0.0f); + _invI = 1.0f / _I; + } + else + { + _I = 0.0f; + _invI = 0.0f; + } + + // Move center of mass. + _sweep.LocalCenter = center; + _sweep.C0 = _sweep.C = Common.Math.Mul(_xf, _sweep.LocalCenter); + + BodyType oldType = _type; + if (_invMass == 0.0f && _invI == 0.0f) + { + _type = BodyType.Static; + } + else + { + _type = BodyType.Dynamic; + } + + // If the body type changed, we need to refilter the broad-phase proxies. + if (oldType != _type) + { + for (Fixture f = _fixtureList; f != null; f = f.Next) + { + f.RefilterProxy(_world._broadPhase, _xf); + } + } + } + + /// <summary> + /// Set the position of the body's origin and rotation (radians). + /// This breaks any contacts and wakes the other bodies. + /// </summary> + /// <param name="position">The new world position of the body's origin (not necessarily + /// the center of mass).</param> + /// <param name="angle">The new world rotation angle of the body in radians.</param> + /// <returns>Return false if the movement put a shape outside the world. In this case the + /// body is automatically frozen.</returns> + public bool SetXForm(Vec2 position, float angle) + { + Box2DXDebug.Assert(_world._lock == false); + if (_world._lock == true) + { + return true; + } + + if (IsFrozen()) + { + return false; + } + + _xf.R.Set(angle); + _xf.Position = position; + + _sweep.C0 = _sweep.C = Common.Math.Mul(_xf, _sweep.LocalCenter); + _sweep.A0 = _sweep.A = angle; + + bool freeze = false; + for (Fixture f = _fixtureList; f != null; f = f.Next) + { + bool inRange = f.Synchronize(_world._broadPhase, _xf, _xf); + + if (inRange == false) + { + freeze = true; + break; + } + } + + if (freeze == true) + { + _flags |= BodyFlags.Frozen; + _linearVelocity.SetZero(); + _angularVelocity = 0.0f; + + // Failure + return false; + } + + // Success + _world._broadPhase.Commit(); + return true; + } + + /// <summary> + /// Set the position of the body's origin and rotation (radians). + /// This breaks any contacts and wakes the other bodies. + /// Note this is less efficient than the other overload - you should use that + /// if the angle is available. + /// </summary> + /// <param name="xf">The transform of position and angle to set the body to.</param> + /// <returns>False if the movement put a shape outside the world. In this case the + /// body is automatically frozen.</returns> + public bool SetXForm(XForm xf) + { + return SetXForm(xf.Position, xf.GetAngle()); + } + + /// <summary> + /// Get the body transform for the body's origin. + /// </summary> + /// <returns>Return the world transform of the body's origin.</returns> + public XForm GetXForm() + { + return _xf; + } + + /// <summary> + /// Set the world body origin position. + /// </summary> + /// <param name="position">The new position of the body.</param> + public void SetPosition(Vec2 position) + { + SetXForm(position, GetAngle()); + } + + /// <summary> + /// Set the world body angle. + /// </summary> + /// <param name="angle">The new angle of the body.</param> + public void SetAngle(float angle) + { + SetXForm(GetPosition(), angle); + } + + /// <summary> + /// Get the world body origin position. + /// </summary> + /// <returns>Return the world position of the body's origin.</returns> + public Vec2 GetPosition() + { + return _xf.Position; + } + + /// <summary> + /// Get the angle in radians. + /// </summary> + /// <returns>Return the current world rotation angle in radians.</returns> + public float GetAngle() + { + return _sweep.A; + } + + /// <summary> + /// Get the world position of the center of mass. + /// </summary> + /// <returns></returns> + public Vec2 GetWorldCenter() + { + return _sweep.C; + } + + /// <summary> + /// Get the local position of the center of mass. + /// </summary> + /// <returns></returns> + public Vec2 GetLocalCenter() + { + return _sweep.LocalCenter; + } + + /// <summary> + /// Set the linear velocity of the center of mass. + /// </summary> + /// <param name="v">The new linear velocity of the center of mass.</param> + public void SetLinearVelocity(Vec2 v) + { + _linearVelocity = v; + } + + /// <summary> + /// Get the linear velocity of the center of mass. + /// </summary> + /// <returns>Return the linear velocity of the center of mass.</returns> + public Vec2 GetLinearVelocity() + { + return _linearVelocity; + } + + /// <summary> + /// Set the angular velocity. + /// </summary> + /// <param name="omega">The new angular velocity in radians/second.</param> + public void SetAngularVelocity(float w) + { + _angularVelocity = w; + } + + /// <summary> + /// Get the angular velocity. + /// </summary> + /// <returns>Return the angular velocity in radians/second.</returns> + public float GetAngularVelocity() + { + return _angularVelocity; + } + + /// <summary> + /// Apply a force at a world point. If the force is not + /// applied at the center of mass, it will generate a torque and + /// affect the angular velocity. This wakes up the body. + /// </summary> + /// <param name="force">The world force vector, usually in Newtons (N).</param> + /// <param name="point">The world position of the point of application.</param> + public void ApplyForce(Vec2 force, Vec2 point) + { + if (IsSleeping()) + { + WakeUp(); + } + _force += force; + _torque += Vec2.Cross(point - _sweep.C, force); + } + + /// <summary> + /// Apply a torque. This affects the angular velocity + /// without affecting the linear velocity of the center of mass. + /// This wakes up the body. + /// </summary> + /// <param name="torque">Torque about the z-axis (out of the screen), usually in N-m.</param> + public void ApplyTorque(float torque) + { + if (IsSleeping()) + { + WakeUp(); + } + _torque += torque; + } + + /// <summary> + /// Apply an impulse at a point. This immediately modifies the velocity. + /// It also modifies the angular velocity if the point of application + /// is not at the center of mass. This wakes up the body. + /// </summary> + /// <param name="impulse">The world impulse vector, usually in N-seconds or kg-m/s.</param> + /// <param name="point">The world position of the point of application.</param> + public void ApplyImpulse(Vec2 impulse, Vec2 point) + { + if (IsSleeping()) + { + WakeUp(); + } + _linearVelocity += _invMass * impulse; + _angularVelocity += _invI * Vec2.Cross(point - _sweep.C, impulse); + } + + /// <summary> + /// Get the total mass of the body. + /// </summary> + /// <returns>Return the mass, usually in kilograms (kg).</returns> + public float GetMass() + { + return _mass; + } + + /// <summary> + /// Get the central rotational inertia of the body. + /// </summary> + /// <returns>Return the rotational inertia, usually in kg-m^2.</returns> + public float GetInertia() + { + return _I; + } + + /// <summary> + /// Get the mass data of the body. + /// </summary> + /// <returns>A struct containing the mass, inertia and center of the body.</returns> + public MassData GetMassData() + { + MassData massData = new MassData(); + massData.Mass = _mass; + massData.I = _I; + massData.Center = GetWorldCenter(); + return massData; + } + + /// <summary> + /// Get the world coordinates of a point given the local coordinates. + /// </summary> + /// <param name="localPoint">A point on the body measured relative the the body's origin.</param> + /// <returns>Return the same point expressed in world coordinates.</returns> + public Vec2 GetWorldPoint(Vec2 localPoint) + { + return Common.Math.Mul(_xf, localPoint); + } + + /// <summary> + /// Get the world coordinates of a vector given the local coordinates. + /// </summary> + /// <param name="localVector">A vector fixed in the body.</param> + /// <returns>Return the same vector expressed in world coordinates.</returns> + public Vec2 GetWorldVector(Vec2 localVector) + { + return Common.Math.Mul(_xf.R, localVector); + } + + /// <summary> + /// Gets a local point relative to the body's origin given a world point. + /// </summary> + /// <param name="worldPoint">A point in world coordinates.</param> + /// <returns>Return the corresponding local point relative to the body's origin.</returns> + public Vec2 GetLocalPoint(Vec2 worldPoint) + { + return Common.Math.MulT(_xf, worldPoint); + } + + /// <summary> + /// Gets a local vector given a world vector. + /// </summary> + /// <param name="worldVector">A vector in world coordinates.</param> + /// <returns>Return the corresponding local vector.</returns> + public Vec2 GetLocalVector(Vec2 worldVector) + { + return Common.Math.MulT(_xf.R, worldVector); + } + + /// <summary> + /// Get the world linear velocity of a world point attached to this body. + /// </summary> + /// <param name="worldPoint">A point in world coordinates.</param> + /// <returns>The world velocity of a point.</returns> + public Vec2 GetLinearVelocityFromWorldPoint(Vec2 worldPoint) + { + return _linearVelocity + Vec2.Cross(_angularVelocity, worldPoint - _sweep.C); + } + + /// <summary> + /// Get the world velocity of a local point. + /// </summary> + /// <param name="localPoint">A point in local coordinates.</param> + /// <returns>The world velocity of a point.</returns> + public Vec2 GetLinearVelocityFromLocalPoint(Vec2 localPoint) + { + return GetLinearVelocityFromWorldPoint(GetWorldPoint(localPoint)); + } + + public float GetLinearDamping() + { + return _linearDamping; + } + + public void SetLinearDamping(float linearDamping) + { + _linearDamping = linearDamping; + } + + public float GetAngularDamping() + { + return _angularDamping; + } + + public void SetAngularDamping(float angularDamping) + { + _angularDamping = angularDamping; + } + + /// <summary> + /// Is this body treated like a bullet for continuous collision detection? + /// </summary> + /// <returns></returns> + public bool IsBullet() + { + return (_flags & BodyFlags.Bullet) == BodyFlags.Bullet; + } + + /// <summary> + /// Should this body be treated like a bullet for continuous collision detection? + /// </summary> + /// <param name="flag"></param> + public void SetBullet(bool flag) + { + if (flag) + { + _flags |= BodyFlags.Bullet; + } + else + { + _flags &= ~BodyFlags.Bullet; + } + } + + public bool IsFixedRotation() + { + return (_flags & BodyFlags.FixedRotation) == BodyFlags.FixedRotation; + } + + public void SetFixedRotation(bool fixedr) + { + if (fixedr) + { + _angularVelocity = 0.0f; + _invI = 0.0f; + _flags |= BodyFlags.FixedRotation; + } + else + { + if (_I > 0.0f) + { + // Recover _invI from _I. + _invI = 1.0f / _I; + _flags &= BodyFlags.FixedRotation; + } + // TODO: Else what? + } + } + + /// <summary> + /// Is this body static (immovable)? + /// </summary> + /// <returns></returns> + public bool IsStatic() + { + return _type == BodyType.Static; + } + + public void SetStatic() + { + if (_type == BodyType.Static) + return; + _mass = 0.0f; + _invMass = 0.0f; + _I = 0.0f; + _invI = 0.0f; + _type = BodyType.Static; + + for (Fixture f = _fixtureList; f != null; f = f.Next) + { + f.RefilterProxy(_world._broadPhase, _xf); + } + } + + /// <summary> + /// Is this body dynamic (movable)? + /// </summary> + /// <returns></returns> + public bool IsDynamic() + { + return _type == BodyType.Dynamic; + } + + /// <summary> + /// Is this body frozen? + /// </summary> + /// <returns></returns> + public bool IsFrozen() + { + return (_flags & BodyFlags.Frozen) == BodyFlags.Frozen; + } + + /// <summary> + /// Is this body sleeping (not simulating). + /// </summary> + /// <returns></returns> + public bool IsSleeping() + { + return (_flags & BodyFlags.Sleep) == BodyFlags.Sleep; + } + + public bool IsAllowSleeping() + { + return (_flags & BodyFlags.AllowSleep) == BodyFlags.AllowSleep; + } + + /// <summary> + /// You can disable sleeping on this body. + /// </summary> + /// <param name="flag"></param> + public void AllowSleeping(bool flag) + { + if (flag) + { + _flags |= BodyFlags.AllowSleep; + } + else + { + _flags &= ~BodyFlags.AllowSleep; + WakeUp(); + } + } + + /// <summary> + /// Wake up this body so it will begin simulating. + /// </summary> + public void WakeUp() + { + _flags &= ~BodyFlags.Sleep; + _sleepTime = 0.0f; + } + + /// <summary> + /// Put this body to sleep so it will stop simulating. + /// This also sets the velocity to zero. + /// </summary> + public void PutToSleep() + { + _flags |= BodyFlags.Sleep; + _sleepTime = 0.0f; + _linearVelocity.SetZero(); + _angularVelocity = 0.0f; + _force.SetZero(); + _torque = 0.0f; + } + + /// <summary> + /// Get the list of all fixtures attached to this body. + /// </summary> + /// <returns></returns> + public Fixture GetFixtureList() + { + return _fixtureList; + } + + /// <summary> + /// Get the list of all joints attached to this body. + /// </summary> + /// <returns></returns> + public JointEdge GetJointList() + { + return _jointList; + } + + public Controllers.ControllerEdge GetControllerList() + { + return _controllerList; + } + + /// <summary> + /// Get the next body in the world's body list. + /// </summary> + /// <returns></returns> + public Body GetNext() + { + return _next; + } + + /// <summary> + /// Get the user data pointer that was provided in the body definition. + /// </summary> + /// <returns></returns> + public object GetUserData() + { + return _userData; + } + + /// <summary> + /// Set the user data. Use this to store your application specific data. + /// </summary> + /// <param name="data"></param> + public void SetUserData(object data) { _userData = data; } + + /// <summary> + /// Get the parent world of this body. + /// </summary> + /// <returns></returns> + public World GetWorld() { return _world; } + + internal void SynchronizeTransform() + { + _xf.R.Set(_sweep.A); + _xf.Position = _sweep.C - Common.Math.Mul(_xf.R, _sweep.LocalCenter); + } + internal void Advance(float t) + { + // Advance to the new safe time. + _sweep.Advance(t); + _sweep.C = _sweep.C0; + _sweep.A = _sweep.A0; + SynchronizeTransform(); + } + } } diff --git a/Box2d/Assets/Program/Box2d/Dynamics/ContactManager.cs b/Box2d/Assets/Program/Box2d/Dynamics/ContactManager.cs new file mode 100644 index 0000000..667c848 --- /dev/null +++ b/Box2d/Assets/Program/Box2d/Dynamics/ContactManager.cs @@ -0,0 +1,232 @@ +/* + Box2DX Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx + Box2D original C++ version Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this 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. +*/ + +using Box2DX.Collision; + +namespace Box2DX.Dynamics +{ + /// <summary> + // Delegate of World. + /// </summary> + public class ContactManager : PairCallback + { + public World _world; + + // This lets us provide broadphase proxy pair user data for + // contacts that shouldn't exist. + public NullContact _nullContact; + + public bool _destroyImmediate; + + public ContactManager() { } + + // This is a callback from the broadphase when two AABB proxies begin + // to overlap. We create a Contact to manage the narrow phase. + public override object PairAdded(object proxyUserDataA, object proxyUserDataB) + { + Fixture fixtureA = proxyUserDataA as Fixture; + Fixture fixtureB = proxyUserDataB as Fixture; + + Body bodyA = fixtureA.Body; + Body bodyB = fixtureB.Body; + + if (bodyA.IsStatic() && bodyB.IsStatic()) + { + return _nullContact; + } + + if (fixtureA.Body == fixtureB.Body) + { + return _nullContact; + } + + if (bodyB.IsConnected(bodyA)) + { + return _nullContact; + } + + if (_world._contactFilter != null && _world._contactFilter.ShouldCollide(fixtureA, fixtureB) == false) + { + return _nullContact; + } + + // Call the factory. + Contact c = Contact.Create(fixtureA, fixtureB); + + if (c == null) + { + return _nullContact; + } + + // Contact creation may swap shapes. + fixtureA = c.FixtureA; + fixtureB = c.FixtureB; + bodyA = fixtureA.Body; + bodyB = fixtureB.Body; + + // Insert into the world. + c._prev = null; + c._next = _world._contactList; + if (_world._contactList != null) + { + _world._contactList._prev = c; + } + _world._contactList = c; + + // Connect to island graph. + + // Connect to body 1 + c._nodeA.Contact = c; + c._nodeA.Other = bodyB; + + c._nodeA.Prev = null; + c._nodeA.Next = bodyA._contactList; + if (bodyA._contactList != null) + { + bodyA._contactList.Prev = c._nodeA; + } + bodyA._contactList = c._nodeA; + + // Connect to body 2 + c._nodeB.Contact = c; + c._nodeB.Other = bodyA; + + c._nodeB.Prev = null; + c._nodeB.Next = bodyB._contactList; + if (bodyB._contactList != null) + { + bodyB._contactList.Prev = c._nodeB; + } + bodyB._contactList = c._nodeB; + + ++_world._contactCount; + return c; + } + + // This is a callback from the broadphase when two AABB proxies cease + // to overlap. We retire the Contact. + public override void PairRemoved(object proxyUserData1, object proxyUserData2, object pairUserData) + { + //B2_NOT_USED(proxyUserData1); + //B2_NOT_USED(proxyUserData2); + + if (pairUserData == null) + { + return; + } + + Contact c = pairUserData as Contact; + if (c == _nullContact) + { + return; + } + + // An attached body is being destroyed, we must destroy this contact + // immediately to avoid orphaned shape pointers. + Destroy(c); + } + + public void Destroy(Contact c) + { + Fixture fixtureA = c.FixtureA; + Fixture fixtureB = c.FixtureB; + Body bodyA = fixtureA.Body; + Body bodyB = fixtureB.Body; + + if (c.Manifold.PointCount > 0) + { + if(_world._contactListener!=null) + _world._contactListener.EndContact(c); + } + + // Remove from the world. + if (c._prev != null) + { + c._prev._next = c._next; + } + + if (c._next != null) + { + c._next._prev = c._prev; + } + + if (c == _world._contactList) + { + _world._contactList = c._next; + } + + // Remove from body 1 + if (c._nodeA.Prev != null) + { + c._nodeA.Prev.Next = c._nodeA.Next; + } + + if (c._nodeA.Next != null) + { + c._nodeA.Next.Prev = c._nodeA.Prev; + } + + if (c._nodeA == bodyA._contactList) + { + bodyA._contactList = c._nodeA.Next; + } + + // Remove from body 2 + if (c._nodeB.Prev != null) + { + c._nodeB.Prev.Next = c._nodeB.Next; + } + + if (c._nodeB.Next != null) + { + c._nodeB.Next.Prev = c._nodeB.Prev; + } + + if (c._nodeB == bodyB._contactList) + { + bodyB._contactList = c._nodeB.Next; + } + + // Call the factory. + Contact.Destroy(ref c); + --_world._contactCount; + } + + // This is the top level collision call for the time step. Here + // all the narrow phase collision is processed for the world + // contact list. + public void Collide() + { + // Update awake contacts. + for (Contact c = _world._contactList; c != null; c = c.GetNext()) + { + Body bodyA = c._fixtureA.Body; + Body bodyB = c._fixtureB.Body; + if (bodyA.IsSleeping() && bodyB.IsSleeping()) + { + continue; + } + + c.Update(_world._contactListener); + } + } + } +} diff --git a/Box2d/Assets/Program/Box2d/Dynamics/ContactManager.cs.meta b/Box2d/Assets/Program/Box2d/Dynamics/ContactManager.cs.meta new file mode 100644 index 0000000..83bfbf8 --- /dev/null +++ b/Box2d/Assets/Program/Box2d/Dynamics/ContactManager.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 67b73dad9d40aa0458f9fc9e0fb15b9d +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Contacts/CircleContact.cs b/Box2d/Assets/Program/Box2d/Dynamics/Contacts/CircleContact.cs new file mode 100644 index 0000000..98fe5fd --- /dev/null +++ b/Box2d/Assets/Program/Box2d/Dynamics/Contacts/CircleContact.cs @@ -0,0 +1,52 @@ +/* + Box2DX Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx + Box2D original C++ version Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this 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. +*/ + +using Box2DX.Collision; +using Box2DX.Common; + +namespace Box2DX.Dynamics +{ + public class CircleContact : Contact + { + public CircleContact(Fixture fixtureA, Fixture fixtureB) + : base(fixtureA, fixtureB) + { + Box2DXDebug.Assert(fixtureA.ShapeType == ShapeType.CircleShape); + Box2DXDebug.Assert(fixtureB.ShapeType == ShapeType.CircleShape); + CollideShapeFunction = CollideCircles; + } + + private static void CollideCircles(ref Manifold manifold, Shape shape1, XForm xf1, Shape shape2, XForm xf2) + { + Collision.Collision.CollideCircles(ref manifold, (CircleShape)shape1, xf1, (CircleShape)shape2, xf2); + } + + new public static Contact Create(Fixture fixtureA, Fixture fixtureB) + { + return new CircleContact(fixtureA, fixtureB); + } + + new public static void Destroy(ref Contact contact) + { + contact = null; + } + } +} diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Contacts/CircleContact.cs.meta b/Box2d/Assets/Program/Box2d/Dynamics/Contacts/CircleContact.cs.meta new file mode 100644 index 0000000..5f63bd5 --- /dev/null +++ b/Box2d/Assets/Program/Box2d/Dynamics/Contacts/CircleContact.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: bca3f5e232ed4e240a0e1cb79225517a +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Contacts/Contact.cs b/Box2d/Assets/Program/Box2d/Dynamics/Contacts/Contact.cs index 85ae672..7c23ca5 100644 --- a/Box2d/Assets/Program/Box2d/Dynamics/Contacts/Contact.cs +++ b/Box2d/Assets/Program/Box2d/Dynamics/Contacts/Contact.cs @@ -1,7 +1,376 @@ -using System.Collections; -using System.Collections.Generic; +/* + Box2DX Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx + Box2D original C++ version Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com -namespace Box2D + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this 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. +*/ + +using System; +using Box2DX.Collision; +using Box2DX.Common; + +namespace Box2DX.Dynamics { + public delegate Contact ContactCreateFcn(Fixture fixtureA, Fixture fixtureB); + public delegate void ContactDestroyFcn(ref Contact contact); + + public struct ContactRegister + { + public ContactCreateFcn CreateFcn; + public ContactDestroyFcn DestroyFcn; + public bool Primary; + } + + /// <summary> + /// A contact edge is used to connect bodies and contacts together + /// in a contact graph where each body is a node and each contact + /// is an edge. A contact edge belongs to a doubly linked list + /// maintained in each attached body. Each contact has two contact + /// nodes, one for each attached body. + /// </summary> + public class ContactEdge + { + /// <summary> + /// Provides quick access to the other body attached. + /// </summary> + public Body Other; + /// <summary> + /// The contact. + /// </summary> + public Contact Contact; + /// <summary> + /// The previous contact edge in the body's contact list. + /// </summary> + public ContactEdge Prev; + /// <summary> + /// The next contact edge in the body's contact list. + /// </summary> + public ContactEdge Next; + } + + /// <summary> + /// The class manages contact between two shapes. A contact exists for each overlapping + /// AABB in the broad-phase (except if filtered). Therefore a contact object may exist + /// that has no contact points. + /// </summary> + public abstract class Contact + { + [Flags] + public enum CollisionFlags + { + NonSolid = 0x0001, + Slow = 0x0002, + Island = 0x0004, + Toi = 0x0008, + Touch = 0x0010 + } + + public static ContactRegister[][] s_registers = + new ContactRegister[(int)ShapeType.ShapeTypeCount][/*(int)ShapeType.ShapeTypeCount*/]; + public static bool s_initialized; + + public CollisionFlags _flags; + + // World pool and list pointers. + public Contact _prev; + public Contact _next; + + // Nodes for connecting bodies. + public ContactEdge _nodeA; + public ContactEdge _nodeB; + + public Fixture _fixtureA; + public Fixture _fixtureB; + + public Manifold _manifold = new Manifold(); + + public float _toi; + + internal delegate void CollideShapeDelegate( + ref Manifold manifold, Shape circle1, XForm xf1, Shape circle2, XForm xf2); + internal CollideShapeDelegate CollideShapeFunction; + + public Contact(){} + + public Contact(Fixture fA, Fixture fB) + { + _flags = 0; + + if (fA.IsSensor || fB.IsSensor) + { + _flags |= CollisionFlags.NonSolid; + } + + _fixtureA = fA; + _fixtureB = fB; + + _manifold.PointCount = 0; + + _prev = null; + _next = null; + + _nodeA = new ContactEdge(); + _nodeB = new ContactEdge(); + } + + public static void AddType(ContactCreateFcn createFcn, ContactDestroyFcn destoryFcn, + ShapeType type1, ShapeType type2) + { + Box2DXDebug.Assert(ShapeType.UnknownShape < type1 && type1 < ShapeType.ShapeTypeCount); + Box2DXDebug.Assert(ShapeType.UnknownShape < type2 && type2 < ShapeType.ShapeTypeCount); + + if (s_registers[(int)type1] == null) + s_registers[(int)type1] = new ContactRegister[(int)ShapeType.ShapeTypeCount]; + + s_registers[(int)type1][(int)type2].CreateFcn = createFcn; + s_registers[(int)type1][(int)type2].DestroyFcn = destoryFcn; + s_registers[(int)type1][(int)type2].Primary = true; + + if (type1 != type2) + { + s_registers[(int)type2][(int)type1].CreateFcn = createFcn; + s_registers[(int)type2][(int)type1].DestroyFcn = destoryFcn; + s_registers[(int)type2][(int)type1].Primary = false; + } + } + + public static void InitializeRegisters() + { + AddType(CircleContact.Create, CircleContact.Destroy, ShapeType.CircleShape, ShapeType.CircleShape); + AddType(PolyAndCircleContact.Create, PolyAndCircleContact.Destroy, ShapeType.PolygonShape, ShapeType.CircleShape); + AddType(PolygonContact.Create, PolygonContact.Destroy, ShapeType.PolygonShape, ShapeType.PolygonShape); + + AddType(EdgeAndCircleContact.Create, EdgeAndCircleContact.Destroy, ShapeType.EdgeShape, ShapeType.CircleShape); + AddType(PolyAndEdgeContact.Create, PolyAndEdgeContact.Destroy, ShapeType.PolygonShape, ShapeType.EdgeShape); + } + + public static Contact Create(Fixture fixtureA, Fixture fixtureB) + { + if (s_initialized == false) + { + InitializeRegisters(); + s_initialized = true; + } + + ShapeType type1 = fixtureA.ShapeType; + ShapeType type2 = fixtureB.ShapeType; + + Box2DXDebug.Assert(ShapeType.UnknownShape < type1 && type1 < ShapeType.ShapeTypeCount); + Box2DXDebug.Assert(ShapeType.UnknownShape < type2 && type2 < ShapeType.ShapeTypeCount); + + ContactCreateFcn createFcn = s_registers[(int)type1][(int)type2].CreateFcn; + if (createFcn != null) + { + if (s_registers[(int)type1][(int)type2].Primary) + { + return createFcn(fixtureA, fixtureB); + } + else + { + return createFcn(fixtureB, fixtureA); + } + } + else + { + return null; + } + } + + public static void Destroy(ref Contact contact) + { + Box2DXDebug.Assert(s_initialized == true); + + if (contact._manifold.PointCount > 0) + { + contact.FixtureA.Body.WakeUp(); + contact.FixtureB.Body.WakeUp(); + } + + ShapeType typeA = contact.FixtureA.ShapeType; + ShapeType typeB = contact.FixtureB.ShapeType; + + Box2DXDebug.Assert(ShapeType.UnknownShape < typeA && typeA < ShapeType.ShapeTypeCount); + Box2DXDebug.Assert(ShapeType.UnknownShape < typeB && typeB < ShapeType.ShapeTypeCount); + + ContactDestroyFcn destroyFcn = s_registers[(int)typeA][(int)typeB].DestroyFcn; + destroyFcn(ref contact); + } + + public void Update(ContactListener listener) + { + Manifold oldManifold = _manifold.Clone(); + + Evaluate(); + + Body bodyA = _fixtureA.Body; + Body bodyB = _fixtureB.Body; + + int oldCount = oldManifold.PointCount; + int newCount = _manifold.PointCount; + + if (newCount == 0 && oldCount > 0) + { + bodyA.WakeUp(); + bodyB.WakeUp(); + } + + // Slow contacts don't generate TOI events. + if (bodyA.IsStatic() || bodyA.IsBullet() || bodyB.IsStatic() || bodyB.IsBullet()) + { + _flags &= ~CollisionFlags.Slow; + } + else + { + _flags |= CollisionFlags.Slow; + } + + // Match old contact ids to new contact ids and copy the + // stored impulses to warm start the solver. + for (int i = 0; i < _manifold.PointCount; ++i) + { + ManifoldPoint mp2 = _manifold.Points[i]; + mp2.NormalImpulse = 0.0f; + mp2.TangentImpulse = 0.0f; + ContactID id2 = mp2.ID; + + for (int j = 0; j < oldManifold.PointCount; ++j) + { + ManifoldPoint mp1 = oldManifold.Points[j]; + + if (mp1.ID.Key == id2.Key) + { + mp2.NormalImpulse = mp1.NormalImpulse; + mp2.TangentImpulse = mp1.TangentImpulse; + break; + } + } + } + + if (oldCount == 0 && newCount > 0) + { + _flags |= CollisionFlags.Touch; + if(listener!=null) + listener.BeginContact(this); + } + + if (oldCount > 0 && newCount == 0) + { + _flags &= ~CollisionFlags.Touch; + if (listener != null) + listener.EndContact(this); + } + + if ((_flags & CollisionFlags.NonSolid) == 0) + { + if (listener != null) + listener.PreSolve(this, oldManifold); + + // The user may have disabled contact. + if (_manifold.PointCount == 0) + { + _flags &= ~CollisionFlags.Touch; + } + } + } + + public void Evaluate() + { + Body bodyA = _fixtureA.Body; + Body bodyB = _fixtureB.Body; + + Box2DXDebug.Assert(CollideShapeFunction!=null); + + CollideShapeFunction(ref _manifold, _fixtureA.Shape, bodyA.GetXForm(), _fixtureB.Shape, bodyB.GetXForm()); + } + + public float ComputeTOI(Sweep sweepA, Sweep sweepB) + { + TOIInput input = new TOIInput(); + input.SweepA = sweepA; + input.SweepB = sweepB; + input.SweepRadiusA = _fixtureA.ComputeSweepRadius(sweepA.LocalCenter); + input.SweepRadiusB = _fixtureB.ComputeSweepRadius(sweepB.LocalCenter); + input.Tolerance = Common.Settings.LinearSlop; + + return Collision.Collision.TimeOfImpact(input, _fixtureA.Shape, _fixtureB.Shape); + } + + /// <summary> + /// Get the contact manifold. + /// </summary> + public Manifold Manifold + { + get { return _manifold; } + } + + /// <summary> + /// Get the world manifold. + /// </summary> + public void GetWorldManifold(out WorldManifold worldManifold) + { + worldManifold = new WorldManifold(); + + Body bodyA = _fixtureA.Body; + Body bodyB = _fixtureB.Body; + Shape shapeA = _fixtureA.Shape; + Shape shapeB = _fixtureB.Shape; + + worldManifold.Initialize(_manifold, bodyA.GetXForm(), shapeA._radius, bodyB.GetXForm(), shapeB._radius); + } + + /// <summary> + /// Is this contact solid? + /// </summary> + /// <returns>True if this contact should generate a response.</returns> + public bool IsSolid + { + get { return (_flags & CollisionFlags.NonSolid) == 0; } + } + + /// <summary> + /// Are fixtures touching? + /// </summary> + public bool AreTouching + { + get { return (_flags & CollisionFlags.Touch) == CollisionFlags.Touch; } + } + + /// <summary> + /// Get the next contact in the world's contact list. + /// </summary> + public Contact GetNext() + { + return _next; + } + + /// <summary> + /// Get the first fixture in this contact. + /// </summary> + public Fixture FixtureA + { + get { return _fixtureA; } + } + /// <summary> + /// Get the second fixture in this contact. + /// </summary> + public Fixture FixtureB + { + get { return _fixtureB; } + } + } } diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Contacts/ContactSolver.cs b/Box2d/Assets/Program/Box2d/Dynamics/Contacts/ContactSolver.cs new file mode 100644 index 0000000..a8affb6 --- /dev/null +++ b/Box2d/Assets/Program/Box2d/Dynamics/Contacts/ContactSolver.cs @@ -0,0 +1,712 @@ +/* + Box2DX Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx + Box2D original C++ version Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this 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. +*/ + +//#define B2_DEBUG_SOLVER + +using System; +using Box2DX.Collision; +using Box2DX.Common; + +namespace Box2DX.Dynamics +{ + public struct ContactConstraintPoint + { + public Vec2 LocalPoint; + public Vec2 RA; + public Vec2 RB; + public float NormalImpulse; + public float TangentImpulse; + public float NormalMass; + public float TangentMass; + public float EqualizedMass; + public float VelocityBias; + } + + public class ContactConstraint + { + public ContactConstraintPoint[] Points = new ContactConstraintPoint[Settings.MaxManifoldPoints]; + public Vec2 LocalPlaneNormal; + public Vec2 LocalPoint; + public Vec2 Normal; + public Mat22 NormalMass; + public Mat22 K; + public Body BodyA; + public Body BodyB; + public ManifoldType Type; + public float Radius; + public float Friction; + public float Restitution; + public int PointCount; + public Manifold Manifold; + + //public ContactConstraint() + //{ + // for (int i = 0; i < Settings.MaxManifoldPoints; i++) + // Points[i] = new ContactConstraintPoint(); + //} + } + + public class ContactSolver : IDisposable + { + public TimeStep _step; + public ContactConstraint[] _constraints; + public int _constraintCount; + + public ContactSolver(TimeStep step, Contact[] contacts, int contactCount) + { + _step = step; + _constraintCount = contactCount; + + _constraints = new ContactConstraint[_constraintCount]; + for (int i = 0; i < _constraintCount; i++) + _constraints[i] = new ContactConstraint(); + + int count = 0; + for (int i = 0; i < _constraintCount; ++i) + { + Contact contact = contacts[i]; + + Fixture fixtureA = contact._fixtureA; + Fixture fixtureB = contact._fixtureB; + Shape shapeA = fixtureA.Shape; + Shape shapeB = fixtureB.Shape; + float radiusA = shapeA._radius; + float radiusB = shapeB._radius; + Body bodyA = fixtureA.Body; + Body bodyB = fixtureB.Body; + Manifold manifold = contact.Manifold; + + float friction = Settings.MixFriction(fixtureA.Friction, fixtureB.Friction); + float restitution = Settings.MixRestitution(fixtureA.Restitution, fixtureB.Restitution); + + Vec2 vA = bodyA._linearVelocity; + Vec2 vB = bodyB._linearVelocity; + float wA = bodyA._angularVelocity; + float wB = bodyB._angularVelocity; + + Box2DXDebug.Assert(manifold.PointCount > 0); + + WorldManifold worldManifold = new WorldManifold(); + worldManifold.Initialize(manifold, bodyA._xf, radiusA, bodyB._xf, radiusB); + + ContactConstraint cc = _constraints[i]; + cc.BodyA = bodyA; + cc.BodyB = bodyB; + cc.Manifold = manifold; + cc.Normal = worldManifold.Normal; + cc.PointCount = manifold.PointCount; + cc.Friction = friction; + cc.Restitution = restitution; + + cc.LocalPlaneNormal = manifold.LocalPlaneNormal; + cc.LocalPoint = manifold.LocalPoint; + cc.Radius = radiusA + radiusB; + cc.Type = manifold.Type; + + unsafe + { + fixed (ContactConstraintPoint* ccPointsPtr = cc.Points) + { + for (int j = 0; j < cc.PointCount; ++j) + { + ManifoldPoint cp = manifold.Points[j]; + ContactConstraintPoint* ccp = &ccPointsPtr[j]; + + ccp->NormalImpulse = cp.NormalImpulse; + ccp->TangentImpulse = cp.TangentImpulse; + + ccp->LocalPoint = cp.LocalPoint; + + ccp->RA = worldManifold.Points[j] - bodyA._sweep.C; + ccp->RB = worldManifold.Points[j] - bodyB._sweep.C; + + float rnA = Vec2.Cross(ccp->RA, cc.Normal); + float rnB = Vec2.Cross(ccp->RB, cc.Normal); + rnA *= rnA; + rnB *= rnB; + + float kNormal = bodyA._invMass + bodyB._invMass + bodyA._invI * rnA + bodyB._invI * rnB; + + Box2DXDebug.Assert(kNormal > Common.Settings.FLT_EPSILON); + ccp->NormalMass = 1.0f / kNormal; + + float kEqualized = bodyA._mass * bodyA._invMass + bodyB._mass * bodyB._invMass; + kEqualized += bodyA._mass * bodyA._invI * rnA + bodyB._mass * bodyB._invI * rnB; + + Box2DXDebug.Assert(kEqualized > Common.Settings.FLT_EPSILON); + ccp->EqualizedMass = 1.0f / kEqualized; + + Vec2 tangent = Vec2.Cross(cc.Normal, 1.0f); + + float rtA = Vec2.Cross(ccp->RA, tangent); + float rtB = Vec2.Cross(ccp->RB, tangent); + rtA *= rtA; + rtB *= rtB; + + float kTangent = bodyA._invMass + bodyB._invMass + bodyA._invI * rtA + bodyB._invI * rtB; + + Box2DXDebug.Assert(kTangent > Common.Settings.FLT_EPSILON); + ccp->TangentMass = 1.0f / kTangent; + + // Setup a velocity bias for restitution. + ccp->VelocityBias = 0.0f; + float vRel = Vec2.Dot(cc.Normal, vB + Vec2.Cross(wB, ccp->RB) - vA - Vec2.Cross(wA, ccp->RA)); + if (vRel < -Common.Settings.VelocityThreshold) + { + ccp->VelocityBias = -cc.Restitution * vRel; + } + } + + // If we have two points, then prepare the block solver. + if (cc.PointCount == 2) + { + ContactConstraintPoint* ccp1 = &ccPointsPtr[0]; + ContactConstraintPoint* ccp2 = &ccPointsPtr[1]; + + float invMassA = bodyA._invMass; + float invIA = bodyA._invI; + float invMassB = bodyB._invMass; + float invIB = bodyB._invI; + + float rn1A = Vec2.Cross(ccp1->RA, cc.Normal); + float rn1B = Vec2.Cross(ccp1->RB, cc.Normal); + float rn2A = Vec2.Cross(ccp2->RA, cc.Normal); + float rn2B = Vec2.Cross(ccp2->RB, cc.Normal); + + float k11 = invMassA + invMassB + invIA * rn1A * rn1A + invIB * rn1B * rn1B; + float k22 = invMassA + invMassB + invIA * rn2A * rn2A + invIB * rn2B * rn2B; + float k12 = invMassA + invMassB + invIA * rn1A * rn2A + invIB * rn1B * rn2B; + + // Ensure a reasonable condition number. + const float k_maxConditionNumber = 100.0f; + if (k11 * k11 < k_maxConditionNumber * (k11 * k22 - k12 * k12)) + { + // K is safe to invert. + cc.K.Col1.Set(k11, k12); + cc.K.Col2.Set(k12, k22); + cc.NormalMass = cc.K.GetInverse(); + } + else + { + // The constraints are redundant, just use one. + // TODO_ERIN use deepest? + cc.PointCount = 1; + } + } + } + } + } + } + + public void Dispose() + { + _constraints = null; + } + + public void InitVelocityConstraints(TimeStep step) + { + unsafe + { + // Warm start. + for (int i = 0; i < _constraintCount; ++i) + { + ContactConstraint c = _constraints[i]; + + Body bodyA = c.BodyA; + Body bodyB = c.BodyB; + float invMassA = bodyA._invMass; + float invIA = bodyA._invI; + float invMassB = bodyB._invMass; + float invIB = bodyB._invI; + Vec2 normal = c.Normal; + Vec2 tangent = Vec2.Cross(normal, 1.0f); + + fixed (ContactConstraintPoint* pointsPtr = c.Points) + { + if (step.WarmStarting) + { + for (int j = 0; j < c.PointCount; ++j) + { + ContactConstraintPoint* ccp = &pointsPtr[j]; + ccp->NormalImpulse *= step.DtRatio; + ccp->TangentImpulse *= step.DtRatio; + Vec2 P = ccp->NormalImpulse * normal + ccp->TangentImpulse * tangent; + bodyA._angularVelocity -= invIA * Vec2.Cross(ccp->RA, P); + bodyA._linearVelocity -= invMassA * P; + bodyB._angularVelocity += invIB * Vec2.Cross(ccp->RB, P); + bodyB._linearVelocity += invMassB * P; + } + } + else + { + for (int j = 0; j < c.PointCount; ++j) + { + ContactConstraintPoint* ccp = &pointsPtr[j]; + ccp->NormalImpulse = 0.0f; + ccp->TangentImpulse = 0.0f; + } + } + } + } + } + } + + public void SolveVelocityConstraints() + { + for (int i = 0; i < _constraintCount; ++i) + { + ContactConstraint c = _constraints[i]; + Body bodyA = c.BodyA; + Body bodyB = c.BodyB; + float wA = bodyA._angularVelocity; + float wB = bodyB._angularVelocity; + Vec2 vA = bodyA._linearVelocity; + Vec2 vB = bodyB._linearVelocity; + float invMassA = bodyA._invMass; + float invIA = bodyA._invI; + float invMassB = bodyB._invMass; + float invIB = bodyB._invI; + Vec2 normal = c.Normal; + Vec2 tangent = Vec2.Cross(normal, 1.0f); + float friction = c.Friction; + + Box2DXDebug.Assert(c.PointCount == 1 || c.PointCount == 2); + + unsafe + { + fixed (ContactConstraintPoint* pointsPtr = c.Points) + { + // Solve tangent constraints + for (int j = 0; j < c.PointCount; ++j) + { + ContactConstraintPoint* ccp = &pointsPtr[j]; + + // Relative velocity at contact + Vec2 dv = vB + Vec2.Cross(wB, ccp->RB) - vA - Vec2.Cross(wA, ccp->RA); + + // Compute tangent force + float vt = Vec2.Dot(dv, tangent); + float lambda = ccp->TangentMass * (-vt); + + // b2Clamp the accumulated force + float maxFriction = friction * ccp->NormalImpulse; + float newImpulse = Common.Math.Clamp(ccp->TangentImpulse + lambda, -maxFriction, maxFriction); + lambda = newImpulse - ccp->TangentImpulse; + + // Apply contact impulse + Vec2 P = lambda * tangent; + + vA -= invMassA * P; + wA -= invIA * Vec2.Cross(ccp->RA, P); + + vB += invMassB * P; + wB += invIB * Vec2.Cross(ccp->RB, P); + + ccp->TangentImpulse = newImpulse; + } + + // Solve normal constraints + if (c.PointCount == 1) + { + ContactConstraintPoint ccp = c.Points[0]; + + // Relative velocity at contact + Vec2 dv = vB + Vec2.Cross(wB, ccp.RB) - vA - Vec2.Cross(wA, ccp.RA); + + // Compute normal impulse + float vn = Vec2.Dot(dv, normal); + float lambda = -ccp.NormalMass * (vn - ccp.VelocityBias); + + // Clamp the accumulated impulse + float newImpulse = Common.Math.Max(ccp.NormalImpulse + lambda, 0.0f); + lambda = newImpulse - ccp.NormalImpulse; + + // Apply contact impulse + Vec2 P = lambda * normal; + vA -= invMassA * P; + wA -= invIA * Vec2.Cross(ccp.RA, P); + + vB += invMassB * P; + wB += invIB * Vec2.Cross(ccp.RB, P); + ccp.NormalImpulse = newImpulse; + } + else + { + // Block solver developed in collaboration with Dirk Gregorius (back in 01/07 on Box2D_Lite). + // Build the mini LCP for this contact patch + // + // vn = A * x + b, vn >= 0, , vn >= 0, x >= 0 and vn_i * x_i = 0 with i = 1..2 + // + // A = J * W * JT and J = ( -n, -r1 x n, n, r2 x n ) + // b = vn_0 - velocityBias + // + // The system is solved using the "Total enumeration method" (s. Murty). The complementary constraint vn_i * x_i + // implies that we must have in any solution either vn_i = 0 or x_i = 0. So for the 2D contact problem the cases + // vn1 = 0 and vn2 = 0, x1 = 0 and x2 = 0, x1 = 0 and vn2 = 0, x2 = 0 and vn1 = 0 need to be tested. The first valid + // solution that satisfies the problem is chosen. + // + // In order to account of the accumulated impulse 'a' (because of the iterative nature of the solver which only requires + // that the accumulated impulse is clamped and not the incremental impulse) we change the impulse variable (x_i). + // + // Substitute: + // + // x = x' - a + // + // Plug into above equation: + // + // vn = A * x + b + // = A * (x' - a) + b + // = A * x' + b - A * a + // = A * x' + b' + // b' = b - A * a; + + ContactConstraintPoint* cp1 = &pointsPtr[0]; + ContactConstraintPoint* cp2 = &pointsPtr[1]; + + Vec2 a = new Vec2(cp1->NormalImpulse, cp2->NormalImpulse); + Box2DXDebug.Assert(a.X >= 0.0f && a.Y >= 0.0f); + + // Relative velocity at contact + Vec2 dv1 = vB + Vec2.Cross(wB, cp1->RB) - vA - Vec2.Cross(wA, cp1->RA); + Vec2 dv2 = vB + Vec2.Cross(wB, cp2->RB) - vA - Vec2.Cross(wA, cp2->RA); + + // Compute normal velocity + float vn1 = Vec2.Dot(dv1, normal); + float vn2 = Vec2.Dot(dv2, normal); + + Vec2 b; + b.X = vn1 - cp1->VelocityBias; + b.Y = vn2 - cp2->VelocityBias; + b -= Common.Math.Mul(c.K, a); + + const float k_errorTol = 1e-3f; + //B2_NOT_USED(k_errorTol); + + for (; ; ) + { + // + // Case 1: vn = 0 + // + // 0 = A * x' + b' + // + // Solve for x': + // + // x' = - inv(A) * b' + // + Vec2 x = -Common.Math.Mul(c.NormalMass, b); + + if (x.X >= 0.0f && x.Y >= 0.0f) + { + // Resubstitute for the incremental impulse + Vec2 d = x - a; + + // Apply incremental impulse + Vec2 P1 = d.X * normal; + Vec2 P2 = d.Y * normal; + vA -= invMassA * (P1 + P2); + wA -= invIA * (Vec2.Cross(cp1->RA, P1) + Vec2.Cross(cp2->RA, P2)); + + vB += invMassB * (P1 + P2); + wB += invIB * (Vec2.Cross(cp1->RB, P1) + Vec2.Cross(cp2->RB, P2)); + + // Accumulate + cp1->NormalImpulse = x.X; + cp2->NormalImpulse = x.Y; + +#if DEBUG_SOLVER + // Postconditions + dv1 = vB + Vec2.Cross(wB, cp1->RB) - vA - Vec2.Cross(wA, cp1->RA); + dv2 = vB + Vec2.Cross(wB, cp2->RB) - vA - Vec2.Cross(wA, cp2->RA); + + // Compute normal velocity + vn1 = Vec2.Dot(dv1, normal); + vn2 = Vec2.Dot(dv2, normal); + + Box2DXDebug.Assert(Common.Math.Abs(vn1 - cp1.VelocityBias) < k_errorTol); + Box2DXDebug.Assert(Common.Math.Abs(vn2 - cp2.VelocityBias) < k_errorTol); +#endif + break; + } + + // + // Case 2: vn1 = 0 and x2 = 0 + // + // 0 = a11 * x1' + a12 * 0 + b1' + // vn2 = a21 * x1' + a22 * 0 + b2' + // + x.X = -cp1->NormalMass * b.X; + x.Y = 0.0f; + vn1 = 0.0f; + vn2 = c.K.Col1.Y * x.X + b.Y; + + if (x.X >= 0.0f && vn2 >= 0.0f) + { + // Resubstitute for the incremental impulse + Vec2 d = x - a; + + // Apply incremental impulse + Vec2 P1 = d.X * normal; + Vec2 P2 = d.Y * normal; + vA -= invMassA * (P1 + P2); + wA -= invIA * (Vec2.Cross(cp1->RA, P1) + Vec2.Cross(cp2->RA, P2)); + + vB += invMassB * (P1 + P2); + wB += invIB * (Vec2.Cross(cp1->RB, P1) + Vec2.Cross(cp2->RB, P2)); + + // Accumulate + cp1->NormalImpulse = x.X; + cp2->NormalImpulse = x.Y; + +#if DEBUG_SOLVER + // Postconditions + dv1 = vB + Vec2.Cross(wB, cp1->RB) - vA - Vec2.Cross(wA, cp1->RA); + + // Compute normal velocity + vn1 = Vec2.Dot(dv1, normal); + + Box2DXDebug.Assert(Common.Math.Abs(vn1 - cp1.VelocityBias) < k_errorTol); +#endif + break; + } + + + // + // Case 3: w2 = 0 and x1 = 0 + // + // vn1 = a11 * 0 + a12 * x2' + b1' + // 0 = a21 * 0 + a22 * x2' + b2' + // + x.X = 0.0f; + x.Y = -cp2->NormalMass * b.Y; + vn1 = c.K.Col2.X * x.Y + b.X; + vn2 = 0.0f; + + if (x.Y >= 0.0f && vn1 >= 0.0f) + { + // Resubstitute for the incremental impulse + Vec2 d = x - a; + + // Apply incremental impulse + Vec2 P1 = d.X * normal; + Vec2 P2 = d.Y * normal; + vA -= invMassA * (P1 + P2); + wA -= invIA * (Vec2.Cross(cp1->RA, P1) + Vec2.Cross(cp2->RA, P2)); + + vB += invMassB * (P1 + P2); + wB += invIB * (Vec2.Cross(cp1->RB, P1) + Vec2.Cross(cp2->RB, P2)); + + // Accumulate + cp1->NormalImpulse = x.X; + cp2->NormalImpulse = x.Y; + +#if DEBUG_SOLVER + // Postconditions + dv2 = vB + Vec2.Cross(wB, cp2->RB) - vA - Vec2.Cross(wA, cp2->RA); + + // Compute normal velocity + vn2 = Vec2.Dot(dv2, normal); + + Box2DXDebug.Assert(Common.Math.Abs(vn2 - cp2.VelocityBias) < k_errorTol); +#endif + break; + } + + // + // Case 4: x1 = 0 and x2 = 0 + // + // vn1 = b1 + // vn2 = b2; + x.X = 0.0f; + x.Y = 0.0f; + vn1 = b.X; + vn2 = b.Y; + + if (vn1 >= 0.0f && vn2 >= 0.0f) + { + // Resubstitute for the incremental impulse + Vec2 d = x - a; + + // Apply incremental impulse + Vec2 P1 = d.X * normal; + Vec2 P2 = d.Y * normal; + vA -= invMassA * (P1 + P2); + wA -= invIA * (Vec2.Cross(cp1->RA, P1) + Vec2.Cross(cp2->RA, P2)); + + vB += invMassB * (P1 + P2); + wB += invIB * (Vec2.Cross(cp1->RB, P1) + Vec2.Cross(cp2->RB, P2)); + + // Accumulate + cp1->NormalImpulse = x.X; + cp2->NormalImpulse = x.Y; + + break; + } + + // No solution, give up. This is hit sometimes, but it doesn't seem to matter. + break; + } + } + + bodyA._linearVelocity = vA; + bodyA._angularVelocity = wA; + bodyB._linearVelocity = vB; + bodyB._angularVelocity = wB; + } + } + } + } + + public void FinalizeVelocityConstraints() + { + for (int i = 0; i < _constraintCount; ++i) + { + ContactConstraint c = _constraints[i]; + Manifold m = c.Manifold; + + for (int j = 0; j < c.PointCount; ++j) + { + m.Points[j].NormalImpulse = c.Points[j].NormalImpulse; + m.Points[j].TangentImpulse = c.Points[j].TangentImpulse; + } + } + } + + internal class PositionSolverManifold + { + internal Vec2 Normal; + internal Vec2[] Points = new Vec2[Settings.MaxManifoldPoints]; + internal float[] Separations = new float[Settings.MaxManifoldPoints]; + + internal void Initialize(ContactConstraint cc) + { + Box2DXDebug.Assert(cc.PointCount > 0); + + switch (cc.Type) + { + case ManifoldType.Circles: + { + Vec2 pointA = cc.BodyA.GetWorldPoint(cc.LocalPoint); + Vec2 pointB = cc.BodyB.GetWorldPoint(cc.Points[0].LocalPoint); + if (Vec2.DistanceSquared(pointA, pointB) > Settings.FLT_EPSILON_SQUARED) + { + Normal = pointB - pointA; + Normal.Normalize(); + } + else + { + Normal.Set(1.0f, 0.0f); + } + + Points[0] = 0.5f * (pointA + pointB); + Separations[0] = Vec2.Dot(pointB - pointA, Normal) - cc.Radius; + } + break; + + case ManifoldType.FaceA: + { + Normal = cc.BodyA.GetWorldVector(cc.LocalPlaneNormal); + Vec2 planePoint = cc.BodyA.GetWorldPoint(cc.LocalPoint); + + for (int i = 0; i < cc.PointCount; ++i) + { + Vec2 clipPoint = cc.BodyB.GetWorldPoint(cc.Points[i].LocalPoint); + Separations[i] = Vec2.Dot(clipPoint - planePoint, Normal) - cc.Radius; + Points[i] = clipPoint; + } + } + break; + + case ManifoldType.FaceB: + { + Normal = cc.BodyB.GetWorldVector(cc.LocalPlaneNormal); + Vec2 planePoint = cc.BodyB.GetWorldPoint(cc.LocalPoint); + + for (int i = 0; i < cc.PointCount; ++i) + { + Vec2 clipPoint = cc.BodyA.GetWorldPoint(cc.Points[i].LocalPoint); + Separations[i] = Vec2.Dot(clipPoint - planePoint, Normal) - cc.Radius; + Points[i] = clipPoint; + } + + // Ensure normal points from A to B + Normal = -Normal; + } + break; + } + } + } + + private static PositionSolverManifold s_PositionSolverManifold = new PositionSolverManifold(); + + public bool SolvePositionConstraints(float baumgarte) + { + float minSeparation = 0.0f; + + for (int i = 0; i < _constraintCount; ++i) + { + ContactConstraint c = _constraints[i]; + Body bodyA = c.BodyA; + Body bodyB = c.BodyB; + + float invMassA = bodyA._mass * bodyA._invMass; + float invIA = bodyA._mass * bodyA._invI; + float invMassB = bodyB._mass * bodyB._invMass; + float invIB = bodyB._mass * bodyB._invI; + + s_PositionSolverManifold.Initialize(c); + Vec2 normal = s_PositionSolverManifold.Normal; + + // Solver normal constraints + for (int j = 0; j < c.PointCount; ++j) + { + Vec2 point = s_PositionSolverManifold.Points[j]; + float separation = s_PositionSolverManifold.Separations[j]; + + Vec2 rA = point - bodyA._sweep.C; + Vec2 rB = point - bodyB._sweep.C; + + // Track max constraint error. + minSeparation = Common.Math.Min(minSeparation, separation); + + // Prevent large corrections and allow slop. + float C = baumgarte * Common.Math.Clamp(separation + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f); + + // Compute normal impulse + float impulse = -c.Points[j].EqualizedMass * C; + + Vec2 P = impulse * normal; + + bodyA._sweep.C -= invMassA * P; + bodyA._sweep.A -= invIA * Vec2.Cross(rA, P); + bodyA.SynchronizeTransform(); + + bodyB._sweep.C += invMassB * P; + bodyB._sweep.A += invIB * Vec2.Cross(rB, P); + bodyB.SynchronizeTransform(); + } + } + + // We can't expect minSpeparation >= -Settings.LinearSlop because we don't + // push the separation above -Settings.LinearSlop. + return minSeparation >= -1.5f * Settings.LinearSlop; + } + } +} diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Contacts/ContactSolver.cs.meta b/Box2d/Assets/Program/Box2d/Dynamics/Contacts/ContactSolver.cs.meta new file mode 100644 index 0000000..de24a40 --- /dev/null +++ b/Box2d/Assets/Program/Box2d/Dynamics/Contacts/ContactSolver.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 9948c0dabccb51240a006dacbe528d2e +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Contacts/EdgeAndCircleContact.cs b/Box2d/Assets/Program/Box2d/Dynamics/Contacts/EdgeAndCircleContact.cs new file mode 100644 index 0000000..999154c --- /dev/null +++ b/Box2d/Assets/Program/Box2d/Dynamics/Contacts/EdgeAndCircleContact.cs @@ -0,0 +1,55 @@ +/* + Box2DX Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx + Box2D original C++ version Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this 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. +*/ + +using Box2DX.Collision; +using Box2DX.Common; + +namespace Box2DX.Dynamics +{ + public class EdgeAndCircleContact : Contact + { + public EdgeAndCircleContact(Fixture fixtureA, Fixture fixtureB) + : base(fixtureA, fixtureB) + { + Box2DXDebug.Assert(fixtureA.ShapeType == ShapeType.EdgeShape); + Box2DXDebug.Assert(fixtureB.ShapeType == ShapeType.CircleShape); + _manifold.PointCount = 0; + _manifold.Points[0].NormalImpulse = 0.0f; + _manifold.Points[0].TangentImpulse = 0.0f; + CollideShapeFunction = CollideEdgeAndCircle; + } + + private static void CollideEdgeAndCircle(ref Manifold manifold, Shape shape1, XForm xf1, Shape shape2, XForm xf2) + { + Collision.Collision.CollideEdgeAndCircle(ref manifold, (EdgeShape)shape1, xf1, (CircleShape)shape2, xf2); + } + + new public static Contact Create(Fixture fixtureA, Fixture fixtureB) + { + return new EdgeAndCircleContact(fixtureA, fixtureB); + } + + new public static void Destroy(ref Contact contact) + { + contact = null; + } + } +}
\ No newline at end of file diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Contacts/EdgeAndCircleContact.cs.meta b/Box2d/Assets/Program/Box2d/Dynamics/Contacts/EdgeAndCircleContact.cs.meta new file mode 100644 index 0000000..442c45d --- /dev/null +++ b/Box2d/Assets/Program/Box2d/Dynamics/Contacts/EdgeAndCircleContact.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 576caff6a1c4ee846aa596cfb70c8a29 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Contacts/NullContact.cs b/Box2d/Assets/Program/Box2d/Dynamics/Contacts/NullContact.cs new file mode 100644 index 0000000..31c7bb1 --- /dev/null +++ b/Box2d/Assets/Program/Box2d/Dynamics/Contacts/NullContact.cs @@ -0,0 +1,35 @@ +/* + Box2DX Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx + Box2D original C++ version Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this 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. +*/ + +using Box2DX.Collision; +using Box2DX.Common; + +namespace Box2DX.Dynamics +{ + public class NullContact : Contact + { + public NullContact() + { + CollideShapeFunction = Collide; + } + private static void Collide(ref Manifold manifold, Shape shape1, XForm xf1, Shape shape2, XForm xf2) { } + } +}
\ No newline at end of file diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Contacts/NullContact.cs.meta b/Box2d/Assets/Program/Box2d/Dynamics/Contacts/NullContact.cs.meta new file mode 100644 index 0000000..fca685f --- /dev/null +++ b/Box2d/Assets/Program/Box2d/Dynamics/Contacts/NullContact.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 75a72f933e8900b4a97d40ee2cb66e8d +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Contacts/PolyAndCircleContact.cs b/Box2d/Assets/Program/Box2d/Dynamics/Contacts/PolyAndCircleContact.cs new file mode 100644 index 0000000..4a63bf8 --- /dev/null +++ b/Box2d/Assets/Program/Box2d/Dynamics/Contacts/PolyAndCircleContact.cs @@ -0,0 +1,52 @@ +/* + Box2DX Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx + Box2D original C++ version Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this 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. +*/ + +using Box2DX.Collision; +using Box2DX.Common; + +namespace Box2DX.Dynamics +{ + public class PolyAndCircleContact : Contact + { + public PolyAndCircleContact(Fixture fixtureA, Fixture fixtureB) + : base(fixtureA, fixtureB) + { + Box2DXDebug.Assert(fixtureA.ShapeType == ShapeType.PolygonShape); + Box2DXDebug.Assert(fixtureB.ShapeType == ShapeType.CircleShape); + CollideShapeFunction = CollidePolygonCircle; + } + + private static void CollidePolygonCircle(ref Manifold manifold, Shape shape1, XForm xf1, Shape shape2, XForm xf2) + { + Collision.Collision.CollidePolygonAndCircle(ref manifold, (PolygonShape)shape1, xf1, (CircleShape)shape2, xf2); + } + + new public static Contact Create(Fixture fixtureA, Fixture fixtureB) + { + return new PolyAndCircleContact(fixtureA, fixtureB); + } + + new public static void Destroy(ref Contact contact) + { + contact = null; + } + } +} diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Contacts/PolyAndCircleContact.cs.meta b/Box2d/Assets/Program/Box2d/Dynamics/Contacts/PolyAndCircleContact.cs.meta new file mode 100644 index 0000000..23fa9d5 --- /dev/null +++ b/Box2d/Assets/Program/Box2d/Dynamics/Contacts/PolyAndCircleContact.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 17b4ab04cd464e741a5c1d390f898c69 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Contacts/PolyAndEdgeContact.cs b/Box2d/Assets/Program/Box2d/Dynamics/Contacts/PolyAndEdgeContact.cs new file mode 100644 index 0000000..a50553f --- /dev/null +++ b/Box2d/Assets/Program/Box2d/Dynamics/Contacts/PolyAndEdgeContact.cs @@ -0,0 +1,52 @@ +/* + Box2DX Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx + Box2D original C++ version Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this 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. +*/ + +using Box2DX.Collision; +using Box2DX.Common; + +namespace Box2DX.Dynamics +{ + public class PolyAndEdgeContact : Contact + { + public PolyAndEdgeContact(Fixture fixtureA, Fixture fixtureB) + : base(fixtureA, fixtureB) + { + Box2DXDebug.Assert(fixtureA.ShapeType == ShapeType.PolygonShape); + Box2DXDebug.Assert(fixtureB.ShapeType == ShapeType.EdgeShape); + CollideShapeFunction = CollidePolyAndEdgeContact; + } + + private static void CollidePolyAndEdgeContact(ref Manifold manifold, Shape shape1, XForm xf1, Shape shape2, XForm xf2) + { + Collision.Collision.CollidePolyAndEdge(ref manifold, (PolygonShape)shape1, xf1, (EdgeShape)shape2, xf2); + } + + new public static Contact Create(Fixture fixtureA, Fixture fixtureB) + { + return new PolyAndEdgeContact(fixtureA, fixtureB); + } + + new public static void Destroy(ref Contact contact) + { + contact = null; + } + } +}
\ No newline at end of file diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Contacts/PolyAndEdgeContact.cs.meta b/Box2d/Assets/Program/Box2d/Dynamics/Contacts/PolyAndEdgeContact.cs.meta new file mode 100644 index 0000000..5f83e8a --- /dev/null +++ b/Box2d/Assets/Program/Box2d/Dynamics/Contacts/PolyAndEdgeContact.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: c42cbf67f18e2414192739161ff94e89 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Contacts/PolyContact.cs b/Box2d/Assets/Program/Box2d/Dynamics/Contacts/PolyContact.cs new file mode 100644 index 0000000..5b101fb --- /dev/null +++ b/Box2d/Assets/Program/Box2d/Dynamics/Contacts/PolyContact.cs @@ -0,0 +1,52 @@ +/* + Box2DX Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx + Box2D original C++ version Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this 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. +*/ + +using Box2DX.Collision; +using Box2DX.Common; + +namespace Box2DX.Dynamics +{ + public class PolygonContact : Contact + { + public PolygonContact(Fixture fixtureA, Fixture fixtureB) + : base(fixtureA, fixtureB) + { + Box2DXDebug.Assert(fixtureA.ShapeType == ShapeType.PolygonShape); + Box2DXDebug.Assert(fixtureB.ShapeType == ShapeType.PolygonShape); + CollideShapeFunction = CollidePolygons; + } + + private static void CollidePolygons(ref Manifold manifold, Shape shape1, XForm xf1, Shape shape2, XForm xf2) + { + Collision.Collision.CollidePolygons(ref manifold, (PolygonShape)shape1, xf1, (PolygonShape)shape2, xf2); + } + + new public static Contact Create(Fixture fixtureA, Fixture fixtureB) + { + return new PolygonContact(fixtureA, fixtureB); + } + + new public static void Destroy(ref Contact contact) + { + contact = null; + } + } +}
\ No newline at end of file diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Contacts/PolyContact.cs.meta b/Box2d/Assets/Program/Box2d/Dynamics/Contacts/PolyContact.cs.meta new file mode 100644 index 0000000..75be79e --- /dev/null +++ b/Box2d/Assets/Program/Box2d/Dynamics/Contacts/PolyContact.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: e0b5ca3d1b8329748865e1dae48d3aee +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Box2d/Assets/Program/Box2d/Rope.meta b/Box2d/Assets/Program/Box2d/Dynamics/Controllers.meta index 5581851..446ed50 100644 --- a/Box2d/Assets/Program/Box2d/Rope.meta +++ b/Box2d/Assets/Program/Box2d/Dynamics/Controllers.meta @@ -1,5 +1,5 @@ fileFormatVersion: 2 -guid: dbd6d3a358e95ab42ade19d1fcbeed91 +guid: 960c2a03774a86d4986f403eb182daaa folderAsset: yes DefaultImporter: externalObjects: {} diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Controllers/BuoyancyController.cs b/Box2d/Assets/Program/Box2d/Dynamics/Controllers/BuoyancyController.cs new file mode 100644 index 0000000..dcdb244 --- /dev/null +++ b/Box2d/Assets/Program/Box2d/Dynamics/Controllers/BuoyancyController.cs @@ -0,0 +1,156 @@ +using Box2DX.Common; + +namespace Box2DX.Dynamics.Controllers +{ + /// <summary> + /// This class is used to build buoyancy controllers + /// </summary> + public class BuoyancyControllerDef + { + /// The outer surface normal + public Vec2 Normal; + /// The height of the fluid surface along the normal + public float Offset; + /// The fluid density + public float Density; + /// Fluid velocity, for drag calculations + public Vec2 Velocity; + /// Linear drag co-efficient + public float LinearDrag; + /// Linear drag co-efficient + public float AngularDrag; + /// If false, bodies are assumed to be uniformly dense, otherwise use the shapes densities + public bool UseDensity; //False by default to prevent a gotcha + /// If true, gravity is taken from the world instead of the gravity parameter. + public bool UseWorldGravity; + /// Gravity vector, if the world's gravity is not used + public Vec2 Gravity; + + public BuoyancyControllerDef() + { + Normal = new Vec2(0, 1); + Offset = 0; + Density = 0; + Velocity = new Vec2(0, 0); + LinearDrag = 0; + AngularDrag = 0; + UseDensity = false; + UseWorldGravity = true; + Gravity = new Vec2(0, 0); + } + } + + /// <summary> + /// Calculates buoyancy forces for fluids in the form of a half plane. + /// </summary> + public class BuoyancyController : Controller + { + /// The outer surface normal + public Vec2 Normal; + /// The height of the fluid surface along the normal + public float Offset; + /// The fluid density + public float Density; + /// Fluid velocity, for drag calculations + public Vec2 Velocity; + /// Linear drag co-efficient + public float LinearDrag; + /// Linear drag co-efficient + public float AngularDrag; + /// If false, bodies are assumed to be uniformly dense, otherwise use the shapes densities + public bool UseDensity; //False by default to prevent a gotcha + /// If true, gravity is taken from the world instead of the gravity parameter. + public bool UseWorldGravity; + /// Gravity vector, if the world's gravity is not used + public Vec2 Gravity; + + public BuoyancyController(BuoyancyControllerDef buoyancyControllerDef) + { + Normal = buoyancyControllerDef.Normal; + Offset = buoyancyControllerDef.Offset; + Density = buoyancyControllerDef.Density; + Velocity = buoyancyControllerDef.Velocity; + LinearDrag = buoyancyControllerDef.LinearDrag; + AngularDrag = buoyancyControllerDef.AngularDrag; + UseDensity = buoyancyControllerDef.UseDensity; + UseWorldGravity = buoyancyControllerDef.UseWorldGravity; + Gravity = buoyancyControllerDef.Gravity; + } + + public override void Step(TimeStep step) + { + //B2_NOT_USED(step); + if (_bodyList == null) + return; + + if (UseWorldGravity) + { + Gravity = _world.Gravity; + } + for (ControllerEdge i = _bodyList; i != null; i = i.nextBody) + { + Body body = i.body; + if (body.IsSleeping()) + { + //Buoyancy force is just a function of position, + //so unlike most forces, it is safe to ignore sleeping bodes + continue; + } + Vec2 areac = new Vec2(0, 0); + Vec2 massc = new Vec2(0, 0); + float area = 0; + float mass = 0; + for (Fixture shape = body.GetFixtureList(); shape != null; shape = shape.Next) + { + Vec2 sc; + float sarea = shape.ComputeSubmergedArea(Normal, Offset, out sc); + area += sarea; + areac.X += sarea * sc.X; + areac.Y += sarea * sc.Y; + float shapeDensity = 0; + if (UseDensity) + { + //TODO: Expose density publicly + shapeDensity = shape.Density; + } + else + { + shapeDensity = 1; + } + mass += sarea * shapeDensity; + massc.X += sarea * sc.X * shapeDensity; + massc.Y += sarea * sc.Y * shapeDensity; + } + areac.X /= area; + areac.Y /= area; + //Vec2 localCentroid = Math.MulT(body.GetXForm(), areac); + massc.X /= mass; + massc.Y /= mass; + if (area < Settings.FLT_EPSILON) + continue; + //Buoyancy + Vec2 buoyancyForce = -Density * area * Gravity; + body.ApplyForce(buoyancyForce, massc); + //Linear drag + Vec2 dragForce = body.GetLinearVelocityFromWorldPoint(areac) - Velocity; + dragForce *= -LinearDrag * area; + body.ApplyForce(dragForce, areac); + //Angular drag + //TODO: Something that makes more physical sense? + body.ApplyTorque(-body.GetInertia() / body.GetMass() * area * body.GetAngularVelocity() * AngularDrag); + + } + } + + public override void Draw(DebugDraw debugDraw) + { + float r = 1000; + Vec2 p1 = Offset * Normal + Vec2.Cross(Normal, r); + Vec2 p2 = Offset * Normal - Vec2.Cross(Normal, r); + + Color color = new Color(0, 0, 0.8f); + + debugDraw.DrawSegment(p1, p2, color); + } + } +}
\ No newline at end of file diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Controllers/BuoyancyController.cs.meta b/Box2d/Assets/Program/Box2d/Dynamics/Controllers/BuoyancyController.cs.meta new file mode 100644 index 0000000..7785241 --- /dev/null +++ b/Box2d/Assets/Program/Box2d/Dynamics/Controllers/BuoyancyController.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 41543fe534c548f4b817e2ae3fab50e5 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Controllers/ConstantAccelController.cs b/Box2d/Assets/Program/Box2d/Dynamics/Controllers/ConstantAccelController.cs new file mode 100644 index 0000000..421ca04 --- /dev/null +++ b/Box2d/Assets/Program/Box2d/Dynamics/Controllers/ConstantAccelController.cs @@ -0,0 +1,56 @@ +/* +* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this 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. +*/ + +using Box2DX.Common; + +namespace Box2DX.Dynamics.Controllers +{ + + /// This class is used to build constant acceleration controllers + public class ConstantAccelControllerDef + { + /// <summary> + /// The force to apply + /// </summary> + public Vec2 A; + } + + public class ConstantAccelController : Controller + { + /// <summary> + /// The force to apply + /// </summary> + public Vec2 A; + + public ConstantAccelController(ConstantAccelControllerDef def) + { + A = def.A; + } + + public override void Step(TimeStep step) + { + for (ControllerEdge i = _bodyList; i != null; i = i.nextBody) + { + Body body = i.body; + if (body.IsSleeping()) + continue; + body.SetLinearVelocity(body.GetLinearVelocity() + step.Dt * A); + } + } + } +} diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Controllers/ConstantAccelController.cs.meta b/Box2d/Assets/Program/Box2d/Dynamics/Controllers/ConstantAccelController.cs.meta new file mode 100644 index 0000000..80e33fc --- /dev/null +++ b/Box2d/Assets/Program/Box2d/Dynamics/Controllers/ConstantAccelController.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 9c5b0816bc173a74f8f8534af62524d4 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Controllers/ConstantForceController.cs b/Box2d/Assets/Program/Box2d/Dynamics/Controllers/ConstantForceController.cs new file mode 100644 index 0000000..8ef9a57 --- /dev/null +++ b/Box2d/Assets/Program/Box2d/Dynamics/Controllers/ConstantForceController.cs @@ -0,0 +1,57 @@ +/* +* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this 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. +*/ + +using Box2DX.Common; + +namespace Box2DX.Dynamics.Controllers +{ + + /// <summary> + /// This class is used to build constant force controllers + /// </summary> + public class ConstantForceControllerDef + { + /// The force to apply + public Vec2 F; + } + + public class ConstantForceController : Controller + { + /// <summary> + /// The force to apply + /// </summary> + Vec2 F; + + public ConstantForceController(ConstantForceControllerDef def) + { + F = def.F; + } + + public override void Step(TimeStep step) + { + //B2_NOT_USED(step); + for (ControllerEdge i = _bodyList; i != null; i = i.nextBody) + { + Body body = i.body; + if (body.IsSleeping()) + continue; + body.ApplyForce(F, body.GetWorldCenter()); + } + } + } +} diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Controllers/ConstantForceController.cs.meta b/Box2d/Assets/Program/Box2d/Dynamics/Controllers/ConstantForceController.cs.meta new file mode 100644 index 0000000..fb47c5a --- /dev/null +++ b/Box2d/Assets/Program/Box2d/Dynamics/Controllers/ConstantForceController.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: aa5d8d9cf6eba0b46bd1484725fb9423 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Controllers/Controller.cs b/Box2d/Assets/Program/Box2d/Dynamics/Controllers/Controller.cs new file mode 100644 index 0000000..26bfbdf --- /dev/null +++ b/Box2d/Assets/Program/Box2d/Dynamics/Controllers/Controller.cs @@ -0,0 +1,174 @@ +using System; + +namespace Box2DX.Dynamics.Controllers +{ + /// <summary> + /// A controller edge is used to connect bodies and controllers together + /// in a bipartite graph. + /// </summary> + public class ControllerEdge + { + public Controller controller; // provides quick access to other end of this edge. + public Body body; // the body + public ControllerEdge prevBody; // the previous controller edge in the controllers's joint list + public ControllerEdge nextBody; // the next controller edge in the controllers's joint list + public ControllerEdge prevController; // the previous controller edge in the body's joint list + public ControllerEdge nextController; // the next controller edge in the body's joint list + } + + /// <summary> + /// Base class for controllers. Controllers are a convience for encapsulating common + /// per-step functionality. + /// </summary> + public abstract class Controller : IDisposable + { + internal Controller _prev; + internal Controller _next; + + internal World _world; + protected ControllerEdge _bodyList; + protected int _bodyCount; + + public Controller() + { + _bodyList = null; + _bodyCount = 0; + } + + public Controller(World world) + { + _bodyList = null; + _bodyCount = 0; + + _world = world; + } + + public void Dispose() + { + //Remove attached bodies + + //Previus implementation: + //while (_bodyCount > 0) + // RemoveBody(_bodyList.body); + + Clear(); + } + + /// <summary> + /// Controllers override this to implement per-step functionality. + /// </summary> + public abstract void Step(TimeStep step); + + /// <summary> + /// Controllers override this to provide debug drawing. + /// </summary> + public virtual void Draw(DebugDraw debugDraw) { } + + /// <summary> + /// Adds a body to the controller list. + /// </summary> + public void AddBody(Body body) + { + ControllerEdge edge = new ControllerEdge(); + + edge.body = body; + edge.controller = this; + + //Add edge to controller list + edge.nextBody = _bodyList; + edge.prevBody = null; + if (_bodyList != null) + _bodyList.prevBody = edge; + _bodyList = edge; + ++_bodyCount; + + //Add edge to body list + edge.nextController = body._controllerList; + edge.prevController = null; + if (body._controllerList != null) + body._controllerList.prevController = edge; + body._controllerList = edge; + } + + /// <summary> + /// Removes a body from the controller list. + /// </summary> + public void RemoveBody(Body body) + { + //Assert that the controller is not empty + Box2DXDebug.Assert(_bodyCount > 0); + + //Find the corresponding edge + ControllerEdge edge = _bodyList; + while (edge != null && edge.body != body) + edge = edge.nextBody; + + //Assert that we are removing a body that is currently attached to the controller + Box2DXDebug.Assert(edge != null); + + //Remove edge from controller list + if (edge.prevBody != null) + edge.prevBody.nextBody = edge.nextBody; + if (edge.nextBody != null) + edge.nextBody.prevBody = edge.prevBody; + if (edge == _bodyList) + _bodyList = edge.nextBody; + --_bodyCount; + + //Remove edge from body list + if (edge.prevController != null) + edge.prevController.nextController = edge.nextController; + if (edge.nextController != null) + edge.nextController.prevController = edge.prevController; + if (edge == body._controllerList) + body._controllerList = edge.nextController; + + //Free the edge + edge = null; + } + + /// <summary> + /// Removes all bodies from the controller list. + /// </summary> + public void Clear() + { +#warning "Check this" + ControllerEdge current = _bodyList; + while (current != null) + { + ControllerEdge edge = current; + + //Remove edge from controller list + _bodyList = edge.nextBody; + + //Remove edge from body list + if (edge.prevController != null) + edge.prevController.nextController = edge.nextController; + if (edge.nextController != null) + edge.nextController.prevController = edge.prevController; + if (edge == edge.body._controllerList) + edge.body._controllerList = edge.nextController; + + //Free the edge + //m_world->m_blockAllocator.Free(edge, sizeof(b2ControllerEdge)); + } + + _bodyCount = 0; + } + + /// <summary> + /// Get the next body in the world's body list. + /// </summary> + internal Controller GetNext() { return _next; } + + /// <summary> + /// Get the parent world of this body. + /// </summary> + internal World GetWorld() { return _world; } + + /// <summary> + /// Get the attached body list + /// </summary> + internal ControllerEdge GetBodyList() { return _bodyList; } + } +}
\ No newline at end of file diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Controllers/Controller.cs.meta b/Box2d/Assets/Program/Box2d/Dynamics/Controllers/Controller.cs.meta new file mode 100644 index 0000000..197cb7f --- /dev/null +++ b/Box2d/Assets/Program/Box2d/Dynamics/Controllers/Controller.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 5cca52c5b9850aa469cd4033061b8049 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Controllers/GravityController.cs b/Box2d/Assets/Program/Box2d/Dynamics/Controllers/GravityController.cs new file mode 100644 index 0000000..333e51c --- /dev/null +++ b/Box2d/Assets/Program/Box2d/Dynamics/Controllers/GravityController.cs @@ -0,0 +1,96 @@ +/* +* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this 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. +*/ + +using Box2DX.Common; +using Math = Box2DX.Common.Math; + +namespace Box2DX.Dynamics.Controllers +{ + /// This class is used to build gravity controllers + public class GravityControllerDef + { + /// <summary> + /// Specifies the strength of the gravitiation force + /// </summary> + public float G; + + /// <summary> + /// If true, gravity is proportional to r^-2, otherwise r^-1 + /// </summary> + public bool InvSqr; + } + + public class GravityController : Controller + { + /// <summary> + /// Specifies the strength of the gravitiation force + /// </summary> + public float G; + + /// If true, gravity is proportional to r^-2, otherwise r^-1 + public bool InvSqr; + + public GravityController(GravityControllerDef def) + { + G = def.G; + InvSqr = def.InvSqr; + } + + public override void Step(TimeStep step) + { + //B2_NOT_USED(step); + if (InvSqr) + { + for (ControllerEdge i = _bodyList; i != null; i = i.nextBody) + { + Body body1 = i.body; + for (ControllerEdge j = _bodyList; j != i; j = j.nextBody) + { + Body body2 = j.body; + Vec2 d = body2.GetWorldCenter() - body1.GetWorldCenter(); + float r2 = d.LengthSquared(); + if (r2 < Settings.FLT_EPSILON) + continue; + + Vec2 f = G / r2 / Math.Sqrt(r2) * body1.GetMass() * body2.GetMass() * d; + body1.ApplyForce(f, body1.GetWorldCenter()); + body2.ApplyForce(-1.0f * f, body2.GetWorldCenter()); + } + } + } + else + { + for (ControllerEdge i = _bodyList; i != null; i = i.nextBody) + { + Body body1 = i.body; + for (ControllerEdge j = _bodyList; j != i; j = j.nextBody) + { + Body body2 = j.body; + Vec2 d = body2.GetWorldCenter() - body1.GetWorldCenter(); + float r2 = d.LengthSquared(); + if (r2 < Settings.FLT_EPSILON) + continue; + Vec2 f = G / r2 * body1.GetMass() * body2.GetMass() * d; + body1.ApplyForce(f, body1.GetWorldCenter()); + body2.ApplyForce(-1.0f * f, body2.GetWorldCenter()); + } + } + } + } + } +} diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Controllers/GravityController.cs.meta b/Box2d/Assets/Program/Box2d/Dynamics/Controllers/GravityController.cs.meta new file mode 100644 index 0000000..b010f74 --- /dev/null +++ b/Box2d/Assets/Program/Box2d/Dynamics/Controllers/GravityController.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: d18e2e93089bd984d9a454597208b26e +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Controllers/TensorDampingController.cs b/Box2d/Assets/Program/Box2d/Dynamics/Controllers/TensorDampingController.cs new file mode 100644 index 0000000..8fb5af3 --- /dev/null +++ b/Box2d/Assets/Program/Box2d/Dynamics/Controllers/TensorDampingController.cs @@ -0,0 +1,89 @@ +/* +* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this 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. +*/ + +using Box2DX.Common; + +namespace Box2DX.Dynamics.Controllers +{ + + /// <summary> + /// This class is used to build tensor damping controllers + /// </summary> + public class b2TensorDampingControllerDef + { + /// Tensor to use in damping model + Mat22 T; + /// Set this to a positive number to clamp the maximum amount of damping done. + float maxTimestep; + }; + + public class TensorDampingController : Controller + { + + /// <summary> + /// Tensor to use in damping model + /// Some examples (matrixes in format (row1; row2) ) + ///(-a 0;0 -a) Standard isotropic damping with strength a + ///(0 a;-a 0) Electron in fixed field - a force at right angles to velocity with proportional magnitude + ///(-a 0;0 -b) Differing x and y damping. Useful e.g. for top-down wheels. + ///By the way, tensor in this case just means matrix, don't let the terminology get you down. + /// </summary> + Mat22 T; + + /// <summary> + /// Set this to a positive number to clamp the maximum amount of damping done. + /// Typically one wants maxTimestep to be 1/(max eigenvalue of T), so that damping will never cause something to reverse direction + /// </summary> + float MaxTimestep; + + /// Sets damping independantly along the x and y axes + public void SetAxisAligned(float xDamping, float yDamping) + { + T.Col1.X = -xDamping; + T.Col1.Y = 0; + T.Col2.X = 0; + T.Col2.Y = -yDamping; + if (xDamping > 0 || yDamping > 0) + { + MaxTimestep = 1 / Math.Max(xDamping, yDamping); + } + else + { + MaxTimestep = 0; + } + } + + public override void Step(TimeStep step) + { + float timestep = step.Dt; + if (timestep <= Settings.FLT_EPSILON) + return; + if (timestep > MaxTimestep && MaxTimestep > 0) + timestep = MaxTimestep; + for (ControllerEdge i = _bodyList; i != null; i = i.nextBody) + { + Body body = i.body; + if (body.IsSleeping()) + continue; + + Vec2 damping = body.GetWorldVector(Math.Mul(T, body.GetLocalVector(body.GetLinearVelocity()))); + body.SetLinearVelocity(body.GetLinearVelocity() + timestep*damping); + } + } + } +} diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Controllers/TensorDampingController.cs.meta b/Box2d/Assets/Program/Box2d/Dynamics/Controllers/TensorDampingController.cs.meta new file mode 100644 index 0000000..a975c8f --- /dev/null +++ b/Box2d/Assets/Program/Box2d/Dynamics/Controllers/TensorDampingController.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: c119a4aba6eaccf408daeb709363782c +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Fixture.cs b/Box2d/Assets/Program/Box2d/Dynamics/Fixture.cs new file mode 100644 index 0000000..eecfd1f --- /dev/null +++ b/Box2d/Assets/Program/Box2d/Dynamics/Fixture.cs @@ -0,0 +1,478 @@ +/* + Box2DX Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx + Box2D original C++ version Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this 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. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using Box2DX.Collision; +using Box2DX.Common; + +namespace Box2DX.Dynamics +{ + /// <summary> + /// This holds contact filtering data. + /// </summary> + public struct FilterData + { + /// <summary> + /// The collision category bits. Normally you would just set one bit. + /// </summary> + public ushort CategoryBits; + + /// <summary> + /// The collision mask bits. This states the categories that this + /// shape would accept for collision. + /// </summary> + public ushort MaskBits; + + /// <summary> + /// Collision groups allow a certain group of objects to never collide (negative) + /// or always collide (positive). Zero means no collision group. Non-zero group + /// filtering always wins against the mask bits. + /// </summary> + public short GroupIndex; + } + + /// <summary> + /// A fixture definition is used to create a fixture. This class defines an + /// abstract fixture definition. You can reuse fixture definitions safely. + /// </summary> + public class FixtureDef + { + /// <summary> + /// The constructor sets the default fixture definition values. + /// </summary> + public FixtureDef() + { + Type = ShapeType.UnknownShape; + UserData = null; + Friction = 0.2f; + Restitution = 0.0f; + Density = 0.0f; + Filter.CategoryBits = 0x0001; + Filter.MaskBits = 0xFFFF; + Filter.GroupIndex = 0; + IsSensor = false; + } + + /// <summary> + /// Holds the shape type for down-casting. + /// </summary> + public ShapeType Type; + + /// <summary> + /// Use this to store application specific fixture data. + /// </summary> + public object UserData; + + /// <summary> + /// The friction coefficient, usually in the range [0,1]. + /// </summary> + public float Friction; + + /// <summary> + /// The restitution (elasticity) usually in the range [0,1]. + /// </summary> + public float Restitution; + + /// <summary> + /// The density, usually in kg/m^2. + /// </summary> + public float Density; + + /// <summary> + /// A sensor shape collects contact information but never generates a collision response. + /// </summary> + public bool IsSensor; + + /// <summary> + /// Contact filtering data. + /// </summary> + public FilterData Filter; + } + + /// <summary> + /// This structure is used to build a fixture with a circle shape. + /// </summary> + public class CircleDef : FixtureDef + { + public Vec2 LocalPosition; + public float Radius; + + public CircleDef() + { + Type = ShapeType.CircleShape; + LocalPosition = Vec2.Zero; + Radius = 1.0f; + } + } + + /// <summary> + /// Convex polygon. The vertices must be ordered so that the outside of + /// the polygon is on the right side of the edges (looking along the edge + /// from start to end). + /// </summary> + public class PolygonDef : FixtureDef + { + /// <summary> + /// The number of polygon vertices. + /// </summary> + public int VertexCount; + + /// <summary> + /// The polygon vertices in local coordinates. + /// </summary> + public Vec2[] Vertices = new Vec2[Settings.MaxPolygonVertices]; + + public PolygonDef() + { + Type = ShapeType.PolygonShape; + VertexCount = 0; + } + + /// <summary> + /// Build vertices to represent an axis-aligned box. + /// </summary> + /// <param name="hx">The half-width</param> + /// <param name="hy">The half-height.</param> + public void SetAsBox(float hx, float hy) + { + VertexCount = 4; + Vertices[0].Set(-hx, -hy); + Vertices[1].Set(hx, -hy); + Vertices[2].Set(hx, hy); + Vertices[3].Set(-hx, hy); + } + + + /// <summary> + /// Build vertices to represent an oriented box. + /// </summary> + /// <param name="hx">The half-width</param> + /// <param name="hy">The half-height.</param> + /// <param name="center">The center of the box in local coordinates.</param> + /// <param name="angle">The rotation of the box in local coordinates.</param> + public void SetAsBox(float hx, float hy, Vec2 center, float angle) + { + SetAsBox(hx, hy); + + XForm xf = new XForm(); + xf.Position = center; + xf.R.Set(angle); + + for (int i = 0; i < VertexCount; ++i) + { + Vertices[i] = Common.Math.Mul(xf, Vertices[i]); + } + } + } + + /// <summary> + /// This structure is used to build a chain of edges. + /// </summary> + public class EdgeDef : FixtureDef + { + public EdgeDef() + { + Type = ShapeType.EdgeShape; + } + + /// <summary> + /// The start vertex. + /// </summary> + public Vec2 Vertex1; + + /// <summary> + /// The end vertex. + /// </summary> + public Vec2 Vertex2; + } + + /// <summary> + /// A fixture is used to attach a shape to a body for collision detection. A fixture + /// inherits its transform from its parent. Fixtures hold additional non-geometric data + /// such as friction, collision filters, etc. + /// Fixtures are created via Body.CreateFixture. + /// @warning you cannot reuse fixtures. + /// </summary> + public class Fixture + { + protected ShapeType _type; + protected bool _isSensor; + protected UInt16 _proxyId; + + internal Body _body; + protected Shape _shape; + internal Fixture _next; + + /// <summary> + /// Contact filtering data. You must call b2World::Refilter to correct + /// existing contacts/non-contacts. + /// </summary> + public FilterData Filter; + + /// <summary> + /// Is this fixture a sensor (non-solid)? + /// </summary> + public bool IsSensor { get { return _isSensor; } } + + /// <summary> + /// Get the child shape. You can modify the child shape, however you should not change the + /// number of vertices because this will crash some collision caching mechanisms. + /// </summary> + public Shape Shape { get { return _shape; } } + + /// <summary> + /// Get the type of this shape. You can use this to down cast to the concrete shape. + /// </summary> + public ShapeType ShapeType { get { return _type; } } + + /// <summary> + /// Get the next fixture in the parent body's fixture list. + /// </summary> + public Fixture Next { get { return _next; } } + + /// <summary> + /// Get the parent body of this fixture. This is NULL if the fixture is not attached. + /// </summary> + public Body Body { get { return _body; } } + + /// <summary> + /// User data that was assigned in the fixture definition. Use this to + /// store your application specific data. + /// </summary> + public object UserData; + + /// <summary> + /// Friction coefficient, usually in the range [0,1]. + /// </summary> + public float Friction; + + /// <summary> + /// Restitution (elasticity) usually in the range [0,1]. + /// </summary> + public float Restitution; + + /// <summary> + /// Density, usually in kg/m^2. + /// </summary> + public float Density; + + public Fixture() + { + _proxyId = PairManager.NullProxy; + } + + public void Create(BroadPhase broadPhase, Body body, XForm xf, FixtureDef def) + { + UserData = def.UserData; + Friction = def.Friction; + Restitution = def.Restitution; + Density = def.Density; + + _body = body; + _next = null; + + Filter = def.Filter; + + _isSensor = def.IsSensor; + + _type = def.Type; + + // Allocate and initialize the child shape. + switch (_type) + { + case ShapeType.CircleShape: + { + CircleShape circle = new CircleShape(); + CircleDef circleDef = (CircleDef)def; + circle._position = circleDef.LocalPosition; + circle._radius = circleDef.Radius; + _shape = circle; + } + break; + + case ShapeType.PolygonShape: + { + PolygonShape polygon = new PolygonShape(); + PolygonDef polygonDef = (PolygonDef)def; + polygon.Set(polygonDef.Vertices, polygonDef.VertexCount); + _shape = polygon; + } + break; + + case ShapeType.EdgeShape: + { + EdgeShape edge = new EdgeShape(); + EdgeDef edgeDef = (EdgeDef)def; + edge.Set(edgeDef.Vertex1, edgeDef.Vertex2); + _shape = edge; + } + break; + + default: + Box2DXDebug.Assert(false); + break; + } + + // Create proxy in the broad-phase. + AABB aabb; + _shape.ComputeAABB(out aabb, xf); + + bool inRange = broadPhase.InRange(aabb); + + // You are creating a shape outside the world box. + Box2DXDebug.Assert(inRange); + + if (inRange) + { + _proxyId = broadPhase.CreateProxy(aabb, this); + } + else + { + _proxyId = PairManager.NullProxy; + } + } + + public void Destroy(BroadPhase broadPhase) + { + // Remove proxy from the broad-phase. + if (_proxyId != PairManager.NullProxy) + { + broadPhase.DestroyProxy(_proxyId); + _proxyId = PairManager.NullProxy; + } + + // Free the child shape. + _shape.Dispose(); + _shape = null; + } + + internal bool Synchronize(BroadPhase broadPhase, XForm transform1, XForm transform2) + { + if (_proxyId == PairManager.NullProxy) + { + return false; + } + + // Compute an AABB that covers the swept shape (may miss some rotation effect). + AABB aabb1, aabb2; + _shape.ComputeAABB(out aabb1, transform1); + _shape.ComputeAABB(out aabb2, transform2); + + AABB aabb = new AABB(); + aabb.Combine(aabb1, aabb2); + + if (broadPhase.InRange(aabb)) + { + broadPhase.MoveProxy(_proxyId, aabb); + return true; + } + else + { + return false; + } + } + + internal void RefilterProxy(BroadPhase broadPhase, XForm transform) + { + if (_proxyId == PairManager.NullProxy) + { + return; + } + + broadPhase.DestroyProxy(_proxyId); + + AABB aabb; + _shape.ComputeAABB(out aabb, transform); + + bool inRange = broadPhase.InRange(aabb); + + if (inRange) + { + _proxyId = broadPhase.CreateProxy(aabb, this); + } + else + { + _proxyId = PairManager.NullProxy; + } + } + + public virtual void Dispose() + { + Box2DXDebug.Assert(_proxyId == PairManager.NullProxy); + Box2DXDebug.Assert(_shape == null); + } + + /// <summary> + /// Compute the mass properties of this shape using its dimensions and density. + /// The inertia tensor is computed about the local origin, not the centroid. + /// </summary> + /// <param name="massData">Returns the mass data for this shape.</param> + public void ComputeMass(out MassData massData) + { + _shape.ComputeMass(out massData, Density); + } + + /// <summary> + /// Compute the volume and centroid of this fixture intersected with a half plane. + /// </summary> + /// <param name="normal">Normal the surface normal.</param> + /// <param name="offset">Offset the surface offset along normal.</param> + /// <param name="c">Returns the centroid.</param> + /// <returns>The total volume less than offset along normal.</returns> + public float ComputeSubmergedArea(Vec2 normal, float offset, out Vec2 c) + { + return _shape.ComputeSubmergedArea(normal, offset, _body.GetXForm(), out c); + } + + /// <summary> + /// Test a point for containment in this fixture. This only works for convex shapes. + /// </summary> + /// <param name="p">A point in world coordinates.</param> + public bool TestPoint(Vec2 p) + { + return _shape.TestPoint(_body.GetXForm(), p); + } + + /// <summary> + /// Perform a ray cast against this shape. + /// </summary> + /// <param name="lambda">Returns the hit fraction. You can use this to compute the contact point + /// p = (1 - lambda) * segment.p1 + lambda * segment.p2.</param> + /// <param name="normal">Returns the normal at the contact point. If there is no intersection, the normal + /// is not set.</param> + /// <param name="segment">Defines the begin and end point of the ray cast.</param> + /// <param name="maxLambda">A number typically in the range [0,1].</param> + public SegmentCollide TestSegment(out float lambda, out Vec2 normal, Segment segment, float maxLambda) + { + return _shape.TestSegment(_body.GetXForm(), out lambda, out normal, segment, maxLambda); + } + + /// <summary> + /// Get the maximum radius about the parent body's center of mass. + /// </summary> + public float ComputeSweepRadius(Vec2 pivot) + { + return _shape.ComputeSweepRadius(pivot); + } + } +} diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Fixture.cs.meta b/Box2d/Assets/Program/Box2d/Dynamics/Fixture.cs.meta new file mode 100644 index 0000000..c5f3148 --- /dev/null +++ b/Box2d/Assets/Program/Box2d/Dynamics/Fixture.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 121b855d9b0acd24c8362d3416dc5907 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Island.cs b/Box2d/Assets/Program/Box2d/Dynamics/Island.cs new file mode 100644 index 0000000..0f24b8e --- /dev/null +++ b/Box2d/Assets/Program/Box2d/Dynamics/Island.cs @@ -0,0 +1,522 @@ +/* + Box2DX Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx + Box2D original C++ version Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this 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. +*/ + +/* +Position Correction Notes +========================= +I tried the several algorithms for position correction of the 2D revolute joint. +I looked at these systems: +- simple pendulum (1m diameter sphere on massless 5m stick) with initial angular velocity of 100 rad/s. +- suspension bridge with 30 1m long planks of length 1m. +- multi-link chain with 30 1m long links. + +Here are the algorithms: + +Baumgarte - A fraction of the position error is added to the velocity error. There is no +separate position solver. + +Pseudo Velocities - After the velocity solver and position integration, +the position error, Jacobian, and effective mass are recomputed. Then +the velocity constraints are solved with pseudo velocities and a fraction +of the position error is added to the pseudo velocity error. The pseudo +velocities are initialized to zero and there is no warm-starting. After +the position solver, the pseudo velocities are added to the positions. +This is also called the First Order World method or the Position LCP method. + +Modified Nonlinear Gauss-Seidel (NGS) - Like Pseudo Velocities except the +position error is re-computed for each constraint and the positions are updated +after the constraint is solved. The radius vectors (aka Jacobians) are +re-computed too (otherwise the algorithm has horrible instability). The pseudo +velocity states are not needed because they are effectively zero at the beginning +of each iteration. Since we have the current position error, we allow the +iterations to terminate early if the error becomes smaller than b2_linearSlop. + +Full NGS or just NGS - Like Modified NGS except the effective mass are re-computed +each time a constraint is solved. + +Here are the results: +Baumgarte - this is the cheapest algorithm but it has some stability problems, +especially with the bridge. The chain links separate easily close to the root +and they jitter as they struggle to pull together. This is one of the most common +methods in the field. The big drawback is that the position correction artificially +affects the momentum, thus leading to instabilities and false bounce. I used a +bias factor of 0.2. A larger bias factor makes the bridge less stable, a smaller +factor makes joints and contacts more spongy. + +Pseudo Velocities - the is more stable than the Baumgarte method. The bridge is +stable. However, joints still separate with large angular velocities. Drag the +simple pendulum in a circle quickly and the joint will separate. The chain separates +easily and does not recover. I used a bias factor of 0.2. A larger value lead to +the bridge collapsing when a heavy cube drops on it. + +Modified NGS - this algorithm is better in some ways than Baumgarte and Pseudo +Velocities, but in other ways it is worse. The bridge and chain are much more +stable, but the simple pendulum goes unstable at high angular velocities. + +Full NGS - stable in all tests. The joints display good stiffness. The bridge +still sags, but this is better than infinite forces. + +Recommendations +Pseudo Velocities are not really worthwhile because the bridge and chain cannot +recover from joint separation. In other cases the benefit over Baumgarte is small. + +Modified NGS is not a robust method for the revolute joint due to the violent +instability seen in the simple pendulum. Perhaps it is viable with other constraint +types, especially scalar constraints where the effective mass is a scalar. + +This leaves Baumgarte and Full NGS. Baumgarte has small, but manageable instabilities +and is very fast. I don't think we can escape Baumgarte, especially in highly +demanding cases where high constraint fidelity is not needed. + +Full NGS is robust and easy on the eyes. I recommend this as an option for +higher fidelity simulation and certainly for suspension bridges and long chains. +Full NGS might be a good choice for ragdolls, especially motorized ragdolls where +joint separation can be problematic. The number of NGS iterations can be reduced +for better performance without harming robustness much. + +Each joint in a can be handled differently in the position solver. So I recommend +a system where the user can select the algorithm on a per joint basis. I would +probably default to the slower Full NGS and let the user select the faster +Baumgarte method in performance critical scenarios. +*/ + +/* +Cache Performance + +The Box2D solvers are dominated by cache misses. Data structures are designed +to increase the number of cache hits. Much of misses are due to random access +to body data. The constraint structures are iterated over linearly, which leads +to few cache misses. + +The _bodies are not accessed during iteration. Instead read only data, such as +the mass values are stored with the constraints. The mutable data are the constraint +impulses and the _bodies velocities/positions. The impulses are held inside the +constraint structures. The body velocities/positions are held in compact, temporary +arrays to increase the number of cache hits. Linear and angular velocity are +stored in a single array since multiple arrays lead to multiple misses. +*/ + +/* +2D Rotation + +R = [cos(theta) -sin(theta)] + [sin(theta) cos(theta) ] + +thetaDot = omega + +Let q1 = cos(theta), q2 = sin(theta). +R = [q1 -q2] + [q2 q1] + +q1Dot = -thetaDot * q2 +q2Dot = thetaDot * q1 + +q1_new = q1_old - dt * w * q2 +q2_new = q2_old + dt * w * q1 +then normalize. + +This might be faster than computing sin+cos. +However, we can compute sin+cos of the same angle fast. +*/ + +using System; +using System.Collections.Generic; +using System.Text; + +using Box2DX.Common; +using Box2DX.Collision; + +namespace Box2DX.Dynamics +{ + public struct Position + { + public Vec2 x; + public float a; + } + + public struct Velocity + { + public Vec2 v; + public float w; + } + + public class Island : IDisposable + { + public ContactListener _listener; + + public Body[] _bodies; + public Contact[] _contacts; + public Joint[] _joints; + + public Position[] _positions; + public Velocity[] _velocities; + + public int _bodyCount; + public int _jointCount; + public int _contactCount; + + public int _bodyCapacity; + public int _contactCapacity; + public int _jointCapacity; + + public int _positionIterationCount; + + public Island(int bodyCapacity, int contactCapacity, int jointCapacity, ContactListener listener) + { + _bodyCapacity = bodyCapacity; + _contactCapacity = contactCapacity; + _jointCapacity = jointCapacity; + //__bodyCount = 0; + //_contactCount = 0; + //_jointCount = 0; + + _listener = listener; + + _bodies = new Body[bodyCapacity]; + _contacts = new Contact[contactCapacity]; + _joints = new Joint[jointCapacity]; + + _velocities = new Velocity[_bodyCapacity]; + _positions = new Position[_bodyCapacity]; + } + + public void Dispose() + { + // Warning: the order should reverse the constructor order. + _positions = null; + _velocities = null; + _joints = null; + _contacts = null; + _bodies = null; + } + + public void Clear() + { + _bodyCount = 0; + _contactCount = 0; + _jointCount = 0; + } + + public void Solve(TimeStep step, Vec2 gravity, bool allowSleep) + { + // Integrate velocities and apply damping. + for (int i = 0; i < _bodyCount; ++i) + { + Body b = _bodies[i]; + + if (b.IsStatic()) + continue; + + // Integrate velocities. + b._linearVelocity += step.Dt * (gravity + b._invMass * b._force); + b._angularVelocity += step.Dt * b._invI * b._torque; + + // Reset forces. + b._force.Set(0.0f, 0.0f); + b._torque = 0.0f; + + // Apply damping. + // ODE: dv/dt + c * v = 0 + // Solution: v(t) = v0 * exp(-c * t) + // Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt) + // v2 = exp(-c * dt) * v1 + // Taylor expansion: + // v2 = (1.0f - c * dt) * v1 + b._linearVelocity *= Common.Math.Clamp(1.0f - step.Dt * b._linearDamping, 0.0f, 1.0f); + b._angularVelocity *= Common.Math.Clamp(1.0f - step.Dt * b._angularDamping, 0.0f, 1.0f); + } + + ContactSolver contactSolver = new ContactSolver(step, _contacts, _contactCount); + + // Initialize velocity constraints. + contactSolver.InitVelocityConstraints(step); + + for (int i = 0; i < _jointCount; ++i) + { + _joints[i].InitVelocityConstraints(step); + } + + // Solve velocity constraints. + for (int i = 0; i < step.VelocityIterations; ++i) + { + for (int j = 0; j < _jointCount; ++j) + { + _joints[j].SolveVelocityConstraints(step); + } + contactSolver.SolveVelocityConstraints(); + } + + // Post-solve (store impulses for warm starting). + contactSolver.FinalizeVelocityConstraints(); + + // Integrate positions. + for (int i = 0; i < _bodyCount; ++i) + { + Body b = _bodies[i]; + + if (b.IsStatic()) + continue; + + // Check for large velocities. + Vec2 translation = step.Dt * b._linearVelocity; + if (Common.Vec2.Dot(translation, translation) > Settings.MaxTranslationSquared) + { + translation.Normalize(); + b._linearVelocity = (Settings.MaxTranslation * step.Inv_Dt) * translation; + } + + float rotation = step.Dt * b._angularVelocity; + if (rotation * rotation > Settings.MaxRotationSquared) + { + if (rotation < 0.0) + { + b._angularVelocity = -step.Inv_Dt * Settings.MaxRotation; + } + else + { + b._angularVelocity = step.Inv_Dt * Settings.MaxRotation; + } + } + + // Store positions for continuous collision. + b._sweep.C0 = b._sweep.C; + b._sweep.A0 = b._sweep.A; + + // Integrate + b._sweep.C += step.Dt * b._linearVelocity; + b._sweep.A += step.Dt * b._angularVelocity; + + // Compute new transform + b.SynchronizeTransform(); + + // Note: shapes are synchronized later. + } + + // Iterate over constraints. + for (int i = 0; i < step.PositionIterations; ++i) + { + bool contactsOkay = contactSolver.SolvePositionConstraints(Settings.ContactBaumgarte); + + bool jointsOkay = true; + for (int j = 0; j < _jointCount; ++j) + { + bool jointOkay = _joints[j].SolvePositionConstraints(Settings.ContactBaumgarte); + jointsOkay = jointsOkay && jointOkay; + } + + if (contactsOkay && jointsOkay) + { + // Exit early if the position errors are small. + break; + } + } + + Report(contactSolver._constraints); + + if (allowSleep) + { + float minSleepTime = Settings.FLT_MAX; + +#if !TARGET_FLOAT32_IS_FIXED + float linTolSqr = Settings.LinearSleepTolerance * Settings.LinearSleepTolerance; + float angTolSqr = Settings.AngularSleepTolerance * Settings.AngularSleepTolerance; +#endif + + for (int i = 0; i < _bodyCount; ++i) + { + Body b = _bodies[i]; + if (b._invMass == 0.0f) + { + continue; + } + + if ((b._flags & Body.BodyFlags.AllowSleep) == 0) + { + b._sleepTime = 0.0f; + minSleepTime = 0.0f; + } + + if ((b._flags & Body.BodyFlags.AllowSleep) == 0 || +#if TARGET_FLOAT32_IS_FIXED + Common.Math.Abs(b._angularVelocity) > Settings.AngularSleepTolerance || + Common.Math.Abs(b._linearVelocity.X) > Settings.LinearSleepTolerance || + Common.Math.Abs(b._linearVelocity.Y) > Settings.LinearSleepTolerance) +#else + b._angularVelocity * b._angularVelocity > angTolSqr || + Vec2.Dot(b._linearVelocity, b._linearVelocity) > linTolSqr) +#endif + { + b._sleepTime = 0.0f; + minSleepTime = 0.0f; + } + else + { + b._sleepTime += step.Dt; + minSleepTime = Common.Math.Min(minSleepTime, b._sleepTime); + } + } + + if (minSleepTime >= Settings.TimeToSleep) + { + for (int i = 0; i < _bodyCount; ++i) + { + Body b = _bodies[i]; + b._flags |= Body.BodyFlags.Sleep; + b._linearVelocity = Vec2.Zero; + b._angularVelocity = 0.0f; + } + } + } + } + + public void SolveTOI(ref TimeStep subStep) + { + ContactSolver contactSolver = new ContactSolver(subStep, _contacts, _contactCount); + + // No warm starting is needed for TOI events because warm + // starting impulses were applied in the discrete solver. + + // Warm starting for joints is off for now, but we need to + // call this function to compute Jacobians. + for (int i = 0; i < _jointCount; ++i) + { + _joints[i].InitVelocityConstraints(subStep); + } + + // Solve velocity constraints. + for (int i = 0; i < subStep.VelocityIterations; ++i) + { + contactSolver.SolveVelocityConstraints(); + for (int j = 0; j < _jointCount; ++j) + { + _joints[j].SolveVelocityConstraints(subStep); + } + } + + // Don't store the TOI contact forces for warm starting + // because they can be quite large. + + // Integrate positions. + for (int i = 0; i < _bodyCount; ++i) + { + Body b = _bodies[i]; + + if (b.IsStatic()) + continue; + + // Check for large velocities. + Vec2 translation = subStep.Dt * b._linearVelocity; + if (Vec2.Dot(translation, translation) > Settings.MaxTranslationSquared) + { + translation.Normalize(); + b._linearVelocity = (Settings.MaxTranslation * subStep.Inv_Dt) * translation; + } + + float rotation = subStep.Dt * b._angularVelocity; + if (rotation * rotation > Settings.MaxRotationSquared) + { + if (rotation < 0.0) + { + b._angularVelocity = -subStep.Inv_Dt * Settings.MaxRotation; + } + else + { + b._angularVelocity = subStep.Inv_Dt * Settings.MaxRotation; + } + } + + // Store positions for continuous collision. + b._sweep.C0 = b._sweep.C; + b._sweep.A0 = b._sweep.A; + + // Integrate + b._sweep.C += subStep.Dt * b._linearVelocity; + b._sweep.A += subStep.Dt * b._angularVelocity; + + // Compute new transform + b.SynchronizeTransform(); + + // Note: shapes are synchronized later. + } + + // Solve position constraints. + const float k_toiBaumgarte = 0.75f; + for (int i = 0; i < subStep.PositionIterations; ++i) + { + bool contactsOkay = contactSolver.SolvePositionConstraints(k_toiBaumgarte); + bool jointsOkay = true; + for (int j = 0; j < _jointCount; ++j) + { + bool jointOkay = _joints[j].SolvePositionConstraints(k_toiBaumgarte); + jointsOkay = jointsOkay && jointOkay; + } + + if (contactsOkay && jointsOkay) + { + break; + } + } + + Report(contactSolver._constraints); + } + + public void Add(Body body) + { + Box2DXDebug.Assert(_bodyCount < _bodyCapacity); + body._islandIndex = _bodyCount; + _bodies[_bodyCount++] = body; + } + + public void Add(Contact contact) + { + Box2DXDebug.Assert(_contactCount < _contactCapacity); + _contacts[_contactCount++] = contact; + } + + public void Add(Joint joint) + { + Box2DXDebug.Assert(_jointCount < _jointCapacity); + _joints[_jointCount++] = joint; + } + + public void Report(ContactConstraint[] constraints) + { + if (_listener == null) + { + return; + } + + for (int i = 0; i < _contactCount; ++i) + { + Contact c = _contacts[i]; + ContactConstraint cc = constraints[i]; + ContactImpulse impulse = new ContactImpulse(); + for (int j = 0; j < cc.PointCount; ++j) + { + impulse.normalImpulses[j] = cc.Points[j].NormalImpulse; + impulse.tangentImpulses[j] = cc.Points[j].TangentImpulse; + } + + _listener.PostSolve(c, impulse); + } + } + } +}
\ No newline at end of file diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Island.cs.meta b/Box2d/Assets/Program/Box2d/Dynamics/Island.cs.meta new file mode 100644 index 0000000..b73ec93 --- /dev/null +++ b/Box2d/Assets/Program/Box2d/Dynamics/Island.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 94208ebd7c2c8bc4187ea12969896349 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Joints/DistanceJoint.cs b/Box2d/Assets/Program/Box2d/Dynamics/Joints/DistanceJoint.cs new file mode 100644 index 0000000..f00b664 --- /dev/null +++ b/Box2d/Assets/Program/Box2d/Dynamics/Joints/DistanceJoint.cs @@ -0,0 +1,277 @@ +/* + Box2DX Copyright (c) 2008 Ihar Kalasouski http://code.google.com/p/box2dx + Box2D original C++ version Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this 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. +*/ + +// 1-D constrained system +// m (v2 - v1) = lambda +// v2 + (beta/h) * x1 + gamma * lambda = 0, gamma has units of inverse mass. +// x2 = x1 + h * v2 + +// 1-D mass-damper-spring system +// m (v2 - v1) + h * d * v2 + h * k * + +// C = norm(p2 - p1) - L +// u = (p2 - p1) / norm(p2 - p1) +// Cdot = dot(u, v2 + cross(w2, r2) - v1 - cross(w1, r1)) +// J = [-u -cross(r1, u) u cross(r2, u)] +// K = J * invM * JT +// = invMass1 + invI1 * cross(r1, u)^2 + invMass2 + invI2 * cross(r2, u)^2 + +using System; +using System.Collections.Generic; +using System.Text; + +using Box2DX.Common; + +namespace Box2DX.Dynamics +{ + /// <summary> + /// Distance joint definition. This requires defining an + /// anchor point on both bodies and the non-zero length of the + /// distance joint. The definition uses local anchor points + /// so that the initial configuration can violate the constraint + /// slightly. This helps when saving and loading a game. + /// @warning Do not use a zero or short length. + /// </summary> + public class DistanceJointDef : JointDef + { + public DistanceJointDef() + { + Type = JointType.DistanceJoint; + LocalAnchor1.Set(0.0f, 0.0f); + LocalAnchor2.Set(0.0f, 0.0f); + Length = 1.0f; + FrequencyHz = 0.0f; + DampingRatio = 0.0f; + } + + /// <summary> + /// Initialize the bodies, anchors, and length using the world anchors. + /// </summary> + public void Initialize(Body body1, Body body2, Vec2 anchor1, Vec2 anchor2) + { + Body1 = body1; + Body2 = body2; + LocalAnchor1 = body1.GetLocalPoint(anchor1); + LocalAnchor2 = body2.GetLocalPoint(anchor2); + Vec2 d = anchor2 - anchor1; + Length = d.Length(); + } + + /// <summary> + /// The local anchor point relative to body1's origin. + /// </summary> + public Vec2 LocalAnchor1; + + /// <summary> + /// The local anchor point relative to body2's origin. + /// </summary> + public Vec2 LocalAnchor2; + + /// <summary> + /// The equilibrium length between the anchor points. + /// </summary> + public float Length; + + /// <summary> + /// The response speed. + /// </summary> + public float FrequencyHz; + + /// <summary> + /// The damping ratio. 0 = no damping, 1 = critical damping. + /// </summary> + public float DampingRatio; + } + + /// <summary> + /// A distance joint constrains two points on two bodies + /// to remain at a fixed distance from each other. You can view + /// this as a massless, rigid rod. + /// </summary> + public class DistanceJoint : Joint + { + public Vec2 _localAnchor1; + public Vec2 _localAnchor2; + public Vec2 _u; + public float _frequencyHz; + public float _dampingRatio; + public float _gamma; + public float _bias; + public float _impulse; + public float _mass; // effective mass for the constraint. + public float _length; + + public override Vec2 Anchor1 + { + get { return _body1.GetWorldPoint(_localAnchor1);} + } + + public override Vec2 Anchor2 + { + get { return _body2.GetWorldPoint(_localAnchor2);} + } + + public override Vec2 GetReactionForce(float inv_dt) + { + return (inv_dt * _impulse) * _u; + } + + public override float GetReactionTorque(float inv_dt) + { + return 0.0f; + } + + public DistanceJoint(DistanceJointDef def) + : base(def) + { + _localAnchor1 = def.LocalAnchor1; + _localAnchor2 = def.LocalAnchor2; + _length = def.Length; + _frequencyHz = def.FrequencyHz; + _dampingRatio = def.DampingRatio; + _impulse = 0.0f; + _gamma = 0.0f; + _bias = 0.0f; + } + + internal override void InitVelocityConstraints(TimeStep step) + { + Body b1 = _body1; + Body b2 = _body2; + + // Compute the effective mass matrix. + Vec2 r1 = Common.Math.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter()); + Vec2 r2 = Common.Math.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter()); + _u = b2._sweep.C + r2 - b1._sweep.C - r1; + + // Handle singularity. + float length = _u.Length(); + if (length > Settings.LinearSlop) + { + _u *= 1.0f / length; + } + else + { + _u.Set(0.0f, 0.0f); + } + + float cr1u = Vec2.Cross(r1, _u); + float cr2u = Vec2.Cross(r2, _u); + float invMass = b1._invMass + b1._invI * cr1u * cr1u + b2._invMass + b2._invI * cr2u * cr2u; + Box2DXDebug.Assert(invMass > Settings.FLT_EPSILON); + _mass = 1.0f / invMass; + + if (_frequencyHz > 0.0f) + { + float C = length - _length; + + // Frequency + float omega = 2.0f * Settings.Pi * _frequencyHz; + + // Damping coefficient + float d = 2.0f * _mass * _dampingRatio * omega; + + // Spring stiffness + float k = _mass * omega * omega; + + // magic formulas + _gamma = 1.0f / (step.Dt * (d + step.Dt * k)); + _bias = C * step.Dt * k * _gamma; + + _mass = 1.0f / (invMass + _gamma); + } + + if (step.WarmStarting) + { + //Scale the inpulse to support a variable timestep. + _impulse *= step.DtRatio; + Vec2 P = _impulse * _u; + b1._linearVelocity -= b1._invMass * P; + b1._angularVelocity -= b1._invI * Vec2.Cross(r1, P); + b2._linearVelocity += b2._invMass * P; + b2._angularVelocity += b2._invI * Vec2.Cross(r2, P); + } + else + { + _impulse = 0.0f; + } + } + + internal override bool SolvePositionConstraints(float baumgarte) + { + if (_frequencyHz > 0.0f) + { + //There is no possition correction for soft distace constraint. + return true; + } + + Body b1 = _body1; + Body b2 = _body2; + + Vec2 r1 = Common.Math.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter()); + Vec2 r2 = Common.Math.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter()); + + Vec2 d = b2._sweep.C + r2 - b1._sweep.C - r1; + + float length = d.Normalize(); + float C = length - _length; + C = Common.Math.Clamp(C, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection); + + float impulse = -_mass * C; + _u = d; + Vec2 P = impulse * _u; + + b1._sweep.C -= b1._invMass * P; + b1._sweep.A -= b1._invI * Vec2.Cross(r1, P); + b2._sweep.C += b2._invMass * P; + b2._sweep.A += b2._invI * Vec2.Cross(r2, P); + + b1.SynchronizeTransform(); + b2.SynchronizeTransform(); + + return System.Math.Abs(C) < Settings.LinearSlop; + } + + internal override void SolveVelocityConstraints(TimeStep step) + { + //B2_NOT_USED(step); + + Body b1 = _body1; + Body b2 = _body2; + + Vec2 r1 = Common.Math.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter()); + Vec2 r2 = Common.Math.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter()); + + // Cdot = dot(u, v + cross(w, r)) + Vec2 v1 = b1._linearVelocity + Vec2.Cross(b1._angularVelocity, r1); + Vec2 v2 = b2._linearVelocity + Vec2.Cross(b2._angularVelocity, r2); + float Cdot = Vec2.Dot(_u, v2 - v1); + float impulse = -_mass * (Cdot + _bias + _gamma * _impulse); + _impulse += impulse; + + Vec2 P = impulse * _u; + b1._linearVelocity -= b1._invMass * P; + b1._angularVelocity -= b1._invI * Vec2.Cross(r1, P); + b2._linearVelocity += b2._invMass * P; + b2._angularVelocity += b2._invI * Vec2.Cross(r2, P); + } + } +} diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Joints/DistanceJoint.cs.meta b/Box2d/Assets/Program/Box2d/Dynamics/Joints/DistanceJoint.cs.meta new file mode 100644 index 0000000..ee77e15 --- /dev/null +++ b/Box2d/Assets/Program/Box2d/Dynamics/Joints/DistanceJoint.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 788b8ce508e4a6e47a8b05a48110d372 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Joints/GearJoint.cs b/Box2d/Assets/Program/Box2d/Dynamics/Joints/GearJoint.cs new file mode 100644 index 0000000..7dbdf6a --- /dev/null +++ b/Box2d/Assets/Program/Box2d/Dynamics/Joints/GearJoint.cs @@ -0,0 +1,322 @@ +/* + Box2DX Copyright (c) 2008 Ihar Kalasouski http://code.google.com/p/box2dx + Box2D original C++ version Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this 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. +*/ + +// Gear Joint: +// C0 = (coordinate1 + ratio * coordinate2)_initial +// C = C0 - (cordinate1 + ratio * coordinate2) = 0 +// Cdot = -(Cdot1 + ratio * Cdot2) +// J = -[J1 ratio * J2] +// K = J * invM * JT +// = J1 * invM1 * J1T + ratio * ratio * J2 * invM2 * J2T +// +// Revolute: +// coordinate = rotation +// Cdot = angularVelocity +// J = [0 0 1] +// K = J * invM * JT = invI +// +// Prismatic: +// coordinate = dot(p - pg, ug) +// Cdot = dot(v + cross(w, r), ug) +// J = [ug cross(r, ug)] +// K = J * invM * JT = invMass + invI * cross(r, ug)^2 + +using System; +using System.Collections.Generic; +using System.Text; + +using Box2DX.Common; + +namespace Box2DX.Dynamics +{ + /// <summary> + /// Gear joint definition. This definition requires two existing + /// revolute or prismatic joints (any combination will work). + /// The provided joints must attach a dynamic body to a static body. + /// </summary> + public class GearJointDef : JointDef + { + public GearJointDef() + { + Type = JointType.GearJoint; + Joint1 = null; + Joint2 = null; + Ratio = 1.0f; + } + + /// <summary> + /// The first revolute/prismatic joint attached to the gear joint. + /// </summary> + public Joint Joint1; + + /// <summary> + /// The second revolute/prismatic joint attached to the gear joint. + /// </summary> + public Joint Joint2; + + /// <summary> + /// The gear ratio. + /// @see GearJoint for explanation. + /// </summary> + public float Ratio; + } + + /// <summary> + /// A gear joint is used to connect two joints together. Either joint + /// can be a revolute or prismatic joint. You specify a gear ratio + /// to bind the motions together: + /// coordinate1 + ratio * coordinate2 = constant + /// The ratio can be negative or positive. If one joint is a revolute joint + /// and the other joint is a prismatic joint, then the ratio will have units + /// of length or units of 1/length. + /// @warning The revolute and prismatic joints must be attached to + /// fixed bodies (which must be body1 on those joints). + /// </summary> + public class GearJoint : Joint + { + public Body _ground1; + public Body _ground2; + + // One of these is NULL. + public RevoluteJoint _revolute1; + public PrismaticJoint _prismatic1; + + // One of these is NULL. + public RevoluteJoint _revolute2; + public PrismaticJoint _prismatic2; + + public Vec2 _groundAnchor1; + public Vec2 _groundAnchor2; + + public Vec2 _localAnchor1; + public Vec2 _localAnchor2; + + public Jacobian _J; + + public float _constant; + public float _ratio; + + // Effective mass + public float _mass; + + // Impulse for accumulation/warm starting. + public float _impulse; + + public override Vec2 Anchor1 { get { return _body1.GetWorldPoint(_localAnchor1); } } + public override Vec2 Anchor2 { get { return _body2.GetWorldPoint(_localAnchor2); } } + + public override Vec2 GetReactionForce(float inv_dt) + { + // TODO_ERIN not tested + Vec2 P = _impulse * _J.Linear2; + return inv_dt * P; + } + + public override float GetReactionTorque(float inv_dt) + { + // TODO_ERIN not tested + Vec2 r = Common.Math.Mul(_body2.GetXForm().R, _localAnchor2 - _body2.GetLocalCenter()); + Vec2 P = _impulse * _J.Linear2; + float L = _impulse * _J.Angular2 - Vec2.Cross(r, P); + return inv_dt * L; + } + + /// <summary> + /// Get the gear ratio. + /// </summary> + public float Ratio { get { return _ratio; } } + + public GearJoint(GearJointDef def) + : base(def) + { + JointType type1 = def.Joint1.GetType(); + JointType type2 = def.Joint2.GetType(); + + Box2DXDebug.Assert(type1 == JointType.RevoluteJoint || type1 == JointType.PrismaticJoint); + Box2DXDebug.Assert(type2 == JointType.RevoluteJoint || type2 == JointType.PrismaticJoint); + Box2DXDebug.Assert(def.Joint1.GetBody1().IsStatic()); + Box2DXDebug.Assert(def.Joint2.GetBody1().IsStatic()); + + _revolute1 = null; + _prismatic1 = null; + _revolute2 = null; + _prismatic2 = null; + + float coordinate1, coordinate2; + + _ground1 = def.Joint1.GetBody1(); + _body1 = def.Joint1.GetBody2(); + if (type1 == JointType.RevoluteJoint) + { + _revolute1 = (RevoluteJoint)def.Joint1; + _groundAnchor1 = _revolute1._localAnchor1; + _localAnchor1 = _revolute1._localAnchor2; + coordinate1 = _revolute1.JointAngle; + } + else + { + _prismatic1 = (PrismaticJoint)def.Joint1; + _groundAnchor1 = _prismatic1._localAnchor1; + _localAnchor1 = _prismatic1._localAnchor2; + coordinate1 = _prismatic1.JointTranslation; + } + + _ground2 = def.Joint2.GetBody1(); + _body2 = def.Joint2.GetBody2(); + if (type2 == JointType.RevoluteJoint) + { + _revolute2 = (RevoluteJoint)def.Joint2; + _groundAnchor2 = _revolute2._localAnchor1; + _localAnchor2 = _revolute2._localAnchor2; + coordinate2 = _revolute2.JointAngle; + } + else + { + _prismatic2 = (PrismaticJoint)def.Joint2; + _groundAnchor2 = _prismatic2._localAnchor1; + _localAnchor2 = _prismatic2._localAnchor2; + coordinate2 = _prismatic2.JointTranslation; + } + + _ratio = def.Ratio; + + _constant = coordinate1 + _ratio * coordinate2; + + _impulse = 0.0f; + } + + internal override void InitVelocityConstraints(TimeStep step) + { + Body g1 = _ground1; + Body g2 = _ground2; + Body b1 = _body1; + Body b2 = _body2; + + float K = 0.0f; + _J.SetZero(); + + if (_revolute1!=null) + { + _J.Angular1 = -1.0f; + K += b1._invI; + } + else + { + Vec2 ug = Common.Math.Mul(g1.GetXForm().R, _prismatic1._localXAxis1); + Vec2 r = Common.Math.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter()); + float crug = Vec2.Cross(r, ug); + _J.Linear1 = -ug; + _J.Angular1 = -crug; + K += b1._invMass + b1._invI * crug * crug; + } + + if (_revolute2!=null) + { + _J.Angular2 = -_ratio; + K += _ratio * _ratio * b2._invI; + } + else + { + Vec2 ug = Common.Math.Mul(g2.GetXForm().R, _prismatic2._localXAxis1); + Vec2 r = Common.Math.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter()); + float crug = Vec2.Cross(r, ug); + _J.Linear2 = -_ratio * ug; + _J.Angular2 = -_ratio * crug; + K += _ratio * _ratio * (b2._invMass + b2._invI * crug * crug); + } + + // Compute effective mass. + Box2DXDebug.Assert(K > 0.0f); + _mass = 1.0f / K; + + if (step.WarmStarting) + { + // Warm starting. + b1._linearVelocity += b1._invMass * _impulse * _J.Linear1; + b1._angularVelocity += b1._invI * _impulse * _J.Angular1; + b2._linearVelocity += b2._invMass * _impulse * _J.Linear2; + b2._angularVelocity += b2._invI * _impulse * _J.Angular2; + } + else + { + _impulse = 0.0f; + } + } + + internal override void SolveVelocityConstraints(TimeStep step) + { + Body b1 = _body1; + Body b2 = _body2; + + float Cdot = _J.Compute(b1._linearVelocity, b1._angularVelocity, b2._linearVelocity, b2._angularVelocity); + + float impulse = _mass * (-Cdot); + _impulse += impulse; + + b1._linearVelocity += b1._invMass * impulse * _J.Linear1; + b1._angularVelocity += b1._invI * impulse * _J.Angular1; + b2._linearVelocity += b2._invMass * impulse * _J.Linear2; + b2._angularVelocity += b2._invI * impulse * _J.Angular2; + } + + internal override bool SolvePositionConstraints(float baumgarte) + { + float linearError = 0.0f; + + Body b1 = _body1; + Body b2 = _body2; + + float coordinate1, coordinate2; + if (_revolute1 != null) + { + coordinate1 = _revolute1.JointAngle; + } + else + { + coordinate1 = _prismatic1.JointTranslation; + } + + if (_revolute2 != null) + { + coordinate2 = _revolute2.JointAngle; + } + else + { + coordinate2 = _prismatic2.JointTranslation; + } + + float C = _constant - (coordinate1 + _ratio * coordinate2); + + float impulse = _mass * (-C); + + b1._sweep.C += b1._invMass * impulse * _J.Linear1; + b1._sweep.A += b1._invI * impulse * _J.Angular1; + b2._sweep.C += b2._invMass * impulse * _J.Linear2; + b2._sweep.A += b2._invI * impulse * _J.Angular2; + + b1.SynchronizeTransform(); + b2.SynchronizeTransform(); + + //TODO_ERIN not implemented + return linearError < Settings.LinearSlop; + } + } +} diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Joints/GearJoint.cs.meta b/Box2d/Assets/Program/Box2d/Dynamics/Joints/GearJoint.cs.meta new file mode 100644 index 0000000..b761f87 --- /dev/null +++ b/Box2d/Assets/Program/Box2d/Dynamics/Joints/GearJoint.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 20f2b2bb9c28bfa4c8ff62cada5632b1 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Joints/Joint.cs b/Box2d/Assets/Program/Box2d/Dynamics/Joints/Joint.cs index 85ae672..232f98b 100644 --- a/Box2d/Assets/Program/Box2d/Dynamics/Joints/Joint.cs +++ b/Box2d/Assets/Program/Box2d/Dynamics/Joints/Joint.cs @@ -1,7 +1,316 @@ -using System.Collections; +/* + Box2DX Copyright (c) 2008 Ihar Kalasouski http://code.google.com/p/box2dx + Box2D original C++ version Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this 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. +*/ + +using System; using System.Collections.Generic; +using System.Text; + +using Box2DX.Common; -namespace Box2D +namespace Box2DX.Dynamics { + public enum JointType + { + UnknownJoint, + RevoluteJoint, + PrismaticJoint, + DistanceJoint, + PulleyJoint, + MouseJoint, + GearJoint, + LineJoint + } + + public enum LimitState + { + InactiveLimit, + AtLowerLimit, + AtUpperLimit, + EqualLimits + } + + public struct Jacobian + { + public Vec2 Linear1; + public float Angular1; + public Vec2 Linear2; + public float Angular2; + + public void SetZero() + { + Linear1.SetZero(); Angular1 = 0.0f; + Linear2.SetZero(); Angular2 = 0.0f; + } + + public void Set(Vec2 x1, float a1, Vec2 x2, float a2) + { + Linear1 = x1; Angular1 = a1; + Linear2 = x2; Angular2 = a2; + } + + public float Compute(Vec2 x1, float a1, Vec2 x2, float a2) + { + return Vec2.Dot(Linear1, x1) + Angular1 * a1 + Vec2.Dot(Linear2, x2) + Angular2 * a2; + } + } + +#warning "CAS" + /// <summary> + /// A joint edge is used to connect bodies and joints together + /// in a joint graph where each body is a node and each joint + /// is an edge. A joint edge belongs to a doubly linked list + /// maintained in each attached body. Each joint has two joint + /// nodes, one for each attached body. + /// </summary> + public class JointEdge + { + /// <summary> + /// Provides quick access to the other body attached. + /// </summary> + public Body Other; + + /// <summary> + /// The joint. + /// </summary> + public Joint Joint; + + /// <summary> + /// The previous joint edge in the body's joint list. + /// </summary> + public JointEdge Prev; + + /// <summary> + /// The next joint edge in the body's joint list. + /// </summary> + public JointEdge Next; + } + +#warning "CAS" + /// <summary> + /// Joint definitions are used to construct joints. + /// </summary> + public class JointDef + { + public JointDef() + { + Type = JointType.UnknownJoint; + UserData = null; + Body1 = null; + Body2 = null; + CollideConnected = false; + } + + /// <summary> + /// The joint type is set automatically for concrete joint types. + /// </summary> + public JointType Type; + + /// <summary> + /// Use this to attach application specific data to your joints. + /// </summary> + public object UserData; + + /// <summary> + /// The first attached body. + /// </summary> + public Body Body1; + + /// <summary> + /// The second attached body. + /// </summary> + public Body Body2; + + /// <summary> + /// Set this flag to true if the attached bodies should collide. + /// </summary> + public bool CollideConnected; + } + + /// <summary> + /// The base joint class. Joints are used to constraint two bodies together in + /// various fashions. Some joints also feature limits and motors. + /// </summary> + public abstract class Joint + { + protected JointType _type; + internal Joint _prev; + internal Joint _next; + internal JointEdge _node1 = new JointEdge(); + internal JointEdge _node2 = new JointEdge(); + internal Body _body1; + internal Body _body2; + + internal bool _islandFlag; + internal bool _collideConnected; + + protected object _userData; + + // Cache here per time step to reduce cache misses. + protected Vec2 _localCenter1, _localCenter2; + protected float _invMass1, _invI1; + protected float _invMass2, _invI2; + + /// <summary> + /// Get the type of the concrete joint. + /// </summary> + public new JointType GetType() + { + return _type; + } + + /// <summary> + /// Get the first body attached to this joint. + /// </summary> + /// <returns></returns> + public Body GetBody1() + { + return _body1; + } + + /// <summary> + /// Get the second body attached to this joint. + /// </summary> + /// <returns></returns> + public Body GetBody2() + { + return _body2; + } + + /// <summary> + /// Get the anchor point on body1 in world coordinates. + /// </summary> + /// <returns></returns> + public abstract Vec2 Anchor1 { get; } + + /// <summary> + /// Get the anchor point on body2 in world coordinates. + /// </summary> + /// <returns></returns> + public abstract Vec2 Anchor2 { get; } + + /// <summary> + /// Get the reaction force on body2 at the joint anchor. + /// </summary> + public abstract Vec2 GetReactionForce(float inv_dt); + + /// <summary> + /// Get the reaction torque on body2. + /// </summary> + public abstract float GetReactionTorque(float inv_dt); + + /// <summary> + /// Get the next joint the world joint list. + /// </summary> + /// <returns></returns> + public Joint GetNext() + { + return _next; + } + + /// <summary> + /// Get/Set the user data pointer. + /// </summary> + /// <returns></returns> + public object UserData + { + get { return _userData; } + set { _userData = value; } + } + + protected Joint(JointDef def) + { + _type = def.Type; + _prev = null; + _next = null; + _body1 = def.Body1; + _body2 = def.Body2; + _collideConnected = def.CollideConnected; + _islandFlag = false; + _userData = def.UserData; + } + + internal static Joint Create(JointDef def) + { + Joint joint = null; + + switch (def.Type) + { + case JointType.DistanceJoint: + { + joint = new DistanceJoint((DistanceJointDef)def); + } + break; + case JointType.MouseJoint: + { + joint = new MouseJoint((MouseJointDef)def); + } + break; + case JointType.PrismaticJoint: + { + joint = new PrismaticJoint((PrismaticJointDef)def); + } + break; + case JointType.RevoluteJoint: + { + joint = new RevoluteJoint((RevoluteJointDef)def); + } + break; + case JointType.PulleyJoint: + { + joint = new PulleyJoint((PulleyJointDef)def); + } + break; + case JointType.GearJoint: + { + joint = new GearJoint((GearJointDef)def); + } + break; + case JointType.LineJoint: + { + joint = new LineJoint((LineJointDef)def); + } + break; + default: + Box2DXDebug.Assert(false); + break; + } + + return joint; + } + + internal static void Destroy(Joint joint) + { + joint = null; + } + + internal abstract void InitVelocityConstraints(TimeStep step); + internal abstract void SolveVelocityConstraints(TimeStep step); + + // This returns true if the position errors are within tolerance. + internal abstract bool SolvePositionConstraints(float baumgarte); + internal void ComputeXForm(ref XForm xf, Vec2 center, Vec2 localCenter, float angle) + { + xf.R.Set(angle); + xf.Position = center - Box2DX.Common.Math.Mul(xf.R, localCenter); + } + } } diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Joints/LineJoint.cs b/Box2d/Assets/Program/Box2d/Dynamics/Joints/LineJoint.cs new file mode 100644 index 0000000..04b2103 --- /dev/null +++ b/Box2d/Assets/Program/Box2d/Dynamics/Joints/LineJoint.cs @@ -0,0 +1,710 @@ +/* + Box2DX Copyright (c) 2008 Ihar Kalasouski http://code.google.com/p/box2dx + Box2D original C++ version Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this 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. +*/ + +// Linear constraint (point-to-line) +// d = p2 - p1 = x2 + r2 - x1 - r1 +// C = dot(perp, d) +// Cdot = dot(d, cross(w1, perp)) + dot(perp, v2 + cross(w2, r2) - v1 - cross(w1, r1)) +// = -dot(perp, v1) - dot(cross(d + r1, perp), w1) + dot(perp, v2) + dot(cross(r2, perp), v2) +// J = [-perp, -cross(d + r1, perp), perp, cross(r2,perp)] +// +// K = J * invM * JT +// +// J = [-a -s1 a s2] +// a = perp +// s1 = cross(d + r1, a) = cross(p2 - x1, a) +// s2 = cross(r2, a) = cross(p2 - x2, a) + + +// Motor/Limit linear constraint +// C = dot(ax1, d) +// Cdot = = -dot(ax1, v1) - dot(cross(d + r1, ax1), w1) + dot(ax1, v2) + dot(cross(r2, ax1), v2) +// J = [-ax1 -cross(d+r1,ax1) ax1 cross(r2,ax1)] + +// Block Solver +// We develop a block solver that includes the joint limit. This makes the limit stiff (inelastic) even +// when the mass has poor distribution (leading to large torques about the joint anchor points). +// +// The Jacobian has 3 rows: +// J = [-uT -s1 uT s2] // linear +// [-vT -a1 vT a2] // limit +// +// u = perp +// v = axis +// s1 = cross(d + r1, u), s2 = cross(r2, u) +// a1 = cross(d + r1, v), a2 = cross(r2, v) + +// M * (v2 - v1) = JT * df +// J * v2 = bias +// +// v2 = v1 + invM * JT * df +// J * (v1 + invM * JT * df) = bias +// K * df = bias - J * v1 = -Cdot +// K = J * invM * JT +// Cdot = J * v1 - bias +// +// Now solve for f2. +// df = f2 - f1 +// K * (f2 - f1) = -Cdot +// f2 = invK * (-Cdot) + f1 +// +// Clamp accumulated limit impulse. +// lower: f2(2) = max(f2(2), 0) +// upper: f2(2) = min(f2(2), 0) +// +// Solve for correct f2(1) +// K(1,1) * f2(1) = -Cdot(1) - K(1,2) * f2(2) + K(1,1:2) * f1 +// = -Cdot(1) - K(1,2) * f2(2) + K(1,1) * f1(1) + K(1,2) * f1(2) +// K(1,1) * f2(1) = -Cdot(1) - K(1,2) * (f2(2) - f1(2)) + K(1,1) * f1(1) +// f2(1) = invK(1,1) * (-Cdot(1) - K(1,2) * (f2(2) - f1(2))) + f1(1) +// +// Now compute impulse to be applied: +// df = f2 - f1 + +using System; +using System.Collections.Generic; +using System.Text; + +using Box2DX.Common; + +namespace Box2DX.Dynamics +{ + /// <summary> + /// Line joint definition. This requires defining a line of + /// motion using an axis and an anchor point. The definition uses local + /// anchor points and a local axis so that the initial configuration + /// can violate the constraint slightly. The joint translation is zero + /// when the local anchor points coincide in world space. Using local + /// anchors and a local axis helps when saving and loading a game. + /// </summary> + public class LineJointDef : JointDef + { + public LineJointDef() + { + Type = JointType.LineJoint; + localAnchor1.SetZero(); + localAnchor2.SetZero(); + localAxis1.Set(1.0f, 0.0f); + enableLimit = false; + lowerTranslation = 0.0f; + upperTranslation = 0.0f; + enableMotor = false; + maxMotorForce = 0.0f; + motorSpeed = 0.0f; + } + + /// <summary> + /// Initialize the bodies, anchors, axis, and reference angle using the world + /// anchor and world axis. + /// </summary> + public void Initialize(Body body1, Body body2, Vec2 anchor, Vec2 axis) + { + Body1 = body1; + Body2 = body2; + localAnchor1 = body1.GetLocalPoint(anchor); + localAnchor2 = body2.GetLocalPoint(anchor); + localAxis1 = body1.GetLocalVector(axis); + } + + /// <summary> + /// The local anchor point relative to body1's origin. + /// </summary> + public Vec2 localAnchor1; + + /// <summary> + /// The local anchor point relative to body2's origin. + /// </summary> + public Vec2 localAnchor2; + + /// <summary> + /// The local translation axis in body1. + /// </summary> + public Vec2 localAxis1; + + /// <summary> + /// Enable/disable the joint limit. + /// </summary> + public bool enableLimit; + + /// <summary> + /// The lower translation limit, usually in meters. + /// </summary> + public float lowerTranslation; + + /// <summary> + /// The upper translation limit, usually in meters. + /// </summary> + public float upperTranslation; + + /// <summary> + /// Enable/disable the joint motor. + /// </summary> + public bool enableMotor; + + /// <summary> + /// The maximum motor torque, usually in N-m. + /// </summary> + public float maxMotorForce; + + /// <summary> + /// The desired motor speed in radians per second. + /// </summary> + public float motorSpeed; + } + + /// <summary> + /// A line joint. This joint provides one degree of freedom: translation + /// along an axis fixed in body1. You can use a joint limit to restrict + /// the range of motion and a joint motor to drive the motion or to + /// model joint friction. + /// </summary> + public class LineJoint : Joint + { + public Vec2 _localAnchor1; + public Vec2 _localAnchor2; + public Vec2 _localXAxis1; + public Vec2 _localYAxis1; + + public Vec2 _axis, _perp; + public float _s1, _s2; + public float _a1, _a2; + + public Mat22 _K; + public Vec2 _impulse; + + public float _motorMass; // effective mass for motor/limit translational constraint. + public float _motorImpulse; + + public float _lowerTranslation; + public float _upperTranslation; + public float _maxMotorForce; + public float _motorSpeed; + + public bool _enableLimit; + public bool _enableMotor; + public LimitState _limitState; + + public LineJoint(LineJointDef def) + : base(def) + { + _localAnchor1 = def.localAnchor1; + _localAnchor2 = def.localAnchor2; + _localXAxis1 = def.localAxis1; + _localYAxis1 = Vec2.Cross(1.0f, _localXAxis1); + + _impulse.SetZero(); + _motorMass = 0.0f; + _motorImpulse = 0.0f; + + _lowerTranslation = def.lowerTranslation; + _upperTranslation = def.upperTranslation; + _maxMotorForce = Settings.FORCE_INV_SCALE(def.maxMotorForce); + _motorSpeed = def.motorSpeed; + _enableLimit = def.enableLimit; + _enableMotor = def.enableMotor; + _limitState = LimitState.InactiveLimit; + + _axis.SetZero(); + _perp.SetZero(); + } + + public override Vec2 Anchor1 + { + get { return _body1.GetWorldPoint(_localAnchor1); } + } + + public override Vec2 Anchor2 + { + get { return _body2.GetWorldPoint(_localAnchor2); } + } + + public override Vec2 GetReactionForce(float inv_dt) + { + return inv_dt * (_impulse.X * _perp + (_motorImpulse + _impulse.Y) * _axis); + } + + public override float GetReactionTorque(float inv_dt) + { + return 0.0f; + } + + /// <summary> + /// Get the current joint translation, usually in meters. + /// </summary> + public float GetJointTranslation() + { + Body b1 = _body1; + Body b2 = _body2; + + Vec2 p1 = b1.GetWorldPoint(_localAnchor1); + Vec2 p2 = b2.GetWorldPoint(_localAnchor2); + Vec2 d = p2 - p1; + Vec2 axis = b1.GetWorldVector(_localXAxis1); + + float translation = Vec2.Dot(d, axis); + return translation; + } + + /// <summary> + /// Get the current joint translation speed, usually in meters per second. + /// </summary> + public float GetJointSpeed() + { + Body b1 = _body1; + Body b2 = _body2; + + Vec2 r1 = Box2DX.Common.Math.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter()); + Vec2 r2 = Box2DX.Common.Math.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter()); + Vec2 p1 = b1._sweep.C + r1; + Vec2 p2 = b2._sweep.C + r2; + Vec2 d = p2 - p1; + Vec2 axis = b1.GetWorldVector(_localXAxis1); + + Vec2 v1 = b1._linearVelocity; + Vec2 v2 = b2._linearVelocity; + float w1 = b1._angularVelocity; + float w2 = b2._angularVelocity; + + float speed = Vec2.Dot(d, Vec2.Cross(w1, axis)) + Vec2.Dot(axis, v2 + Vec2.Cross(w2, r2) - v1 - Vec2.Cross(w1, r1)); + return speed; + } + + /// <summary> + /// Is the joint limit enabled? + /// </summary> + public bool IsLimitEnabled() + { + return _enableLimit; + } + + /// <summary> + /// Enable/disable the joint limit. + /// </summary> + public void EnableLimit(bool flag) + { + _body1.WakeUp(); + _body2.WakeUp(); + _enableLimit = flag; + } + + /// <summary> + /// Get the lower joint limit, usually in meters. + /// </summary> + public float GetLowerLimit() + { + return _lowerTranslation; + } + + /// <summary> + /// Get the upper joint limit, usually in meters. + /// </summary> + public float GetUpperLimit() + { + return _upperTranslation; + } + + /// <summary> + /// Set the joint limits, usually in meters. + /// </summary> + public void SetLimits(float lower, float upper) + { + Box2DXDebug.Assert(lower <= upper); + _body1.WakeUp(); + _body2.WakeUp(); + _lowerTranslation = lower; + _upperTranslation = upper; + } + + /// <summary> + /// Is the joint motor enabled? + /// </summary> + public bool IsMotorEnabled() + { + return _enableMotor; + } + + /// <summary> + /// Enable/disable the joint motor. + /// </summary> + public void EnableMotor(bool flag) + { + _body1.WakeUp(); + _body2.WakeUp(); + _enableMotor = flag; + } + + /// <summary> + /// Set the motor speed, usually in meters per second. + /// </summary> + public void SetMotorSpeed(float speed) + { + _body1.WakeUp(); + _body2.WakeUp(); + _motorSpeed = speed; + } + + /// <summary> + /// Set the maximum motor force, usually in N. + /// </summary> + public void SetMaxMotorForce(float force) + { + _body1.WakeUp(); + _body2.WakeUp(); + _maxMotorForce = Settings.FORCE_SCALE(1.0f) * force; + } + + /// <summary> + /// Get the current motor force, usually in N. + /// </summary> + public float GetMotorForce() + { + return _motorImpulse; + } + + /// <summary> + /// Get the motor speed, usually in meters per second. + /// </summary> + public float GetMotorSpeed() + { + return _motorSpeed; + } + + internal override void InitVelocityConstraints(TimeStep step) + { + Body b1 = _body1; + Body b2 = _body2; + + _localCenter1 = b1.GetLocalCenter(); + _localCenter2 = b2.GetLocalCenter(); + + XForm xf1 = b1.GetXForm(); + XForm xf2 = b2.GetXForm(); + + // Compute the effective masses. + Vec2 r1 = Box2DX.Common.Math.Mul(xf1.R, _localAnchor1 - _localCenter1); + Vec2 r2 = Box2DX.Common.Math.Mul(xf2.R, _localAnchor2 - _localCenter2); + Vec2 d = b2._sweep.C + r2 - b1._sweep.C - r1; + + _invMass1 = b1._invMass; + _invI1 = b1._invI; + _invMass2 = b2._invMass; + _invI2 = b2._invI; + + // Compute motor Jacobian and effective mass. + { + _axis = Box2DX.Common.Math.Mul(xf1.R, _localXAxis1); + _a1 = Vec2.Cross(d + r1, _axis); + _a2 = Vec2.Cross(r2, _axis); + + _motorMass = _invMass1 + _invMass2 + _invI1 * _a1 * _a1 + _invI2 * _a2 * _a2; + Box2DXDebug.Assert(_motorMass > Settings.FLT_EPSILON); + _motorMass = 1.0f / _motorMass; + } + + // Prismatic constraint. + { + _perp = Box2DX.Common.Math.Mul(xf1.R, _localYAxis1); + + _s1 = Vec2.Cross(d + r1, _perp); + _s2 = Vec2.Cross(r2, _perp); + + float m1 = _invMass1, m2 = _invMass2; + float i1 = _invI1, i2 = _invI2; + + float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2; + float k12 = i1 * _s1 * _a1 + i2 * _s2 * _a2; + float k22 = m1 + m2 + i1 * _a1 * _a1 + i2 * _a2 * _a2; + + _K.Col1.Set(k11, k12); + _K.Col2.Set(k12, k22); + } + + // Compute motor and limit terms. + if (_enableLimit) + { + float jointTranslation = Vec2.Dot(_axis, d); + if (Box2DX.Common.Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop) + { + _limitState = LimitState.EqualLimits; + } + else if (jointTranslation <= _lowerTranslation) + { + if (_limitState != LimitState.AtLowerLimit) + { + _limitState = LimitState.AtLowerLimit; + _impulse.Y = 0.0f; + } + } + else if (jointTranslation >= _upperTranslation) + { + if (_limitState != LimitState.AtUpperLimit) + { + _limitState = LimitState.AtUpperLimit; + _impulse.Y = 0.0f; + } + } + else + { + _limitState = LimitState.InactiveLimit; + _impulse.Y = 0.0f; + } + } + else + { + _limitState = LimitState.InactiveLimit; + } + + if (_enableMotor == false) + { + _motorImpulse = 0.0f; + } + + if (step.WarmStarting) + { + // Account for variable time step. + _impulse *= step.DtRatio; + _motorImpulse *= step.DtRatio; + + Vec2 P = _impulse.X * _perp + (_motorImpulse + _impulse.Y) * _axis; + float L1 = _impulse.X * _s1 + (_motorImpulse + _impulse.Y) * _a1; + float L2 = _impulse.X * _s2 + (_motorImpulse + _impulse.Y) * _a2; + + b1._linearVelocity -= _invMass1 * P; + b1._angularVelocity -= _invI1 * L1; + + b2._linearVelocity += _invMass2 * P; + b2._angularVelocity += _invI2 * L2; + } + else + { + _impulse.SetZero(); + _motorImpulse = 0.0f; + } + } + + internal override void SolveVelocityConstraints(TimeStep step) + { + Body b1 = _body1; + Body b2 = _body2; + + Vec2 v1 = b1._linearVelocity; + float w1 = b1._angularVelocity; + Vec2 v2 = b2._linearVelocity; + float w2 = b2._angularVelocity; + + // Solve linear motor constraint. + if (_enableMotor && _limitState != LimitState.EqualLimits) + { + float Cdot = Vec2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1; + float impulse = _motorMass * (_motorSpeed - Cdot); + float oldImpulse = _motorImpulse; + float maxImpulse = step.Dt * _maxMotorForce; + _motorImpulse = Box2DX.Common.Math.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse); + impulse = _motorImpulse - oldImpulse; + + Vec2 P = impulse * _axis; + float L1 = impulse * _a1; + float L2 = impulse * _a2; + + v1 -= _invMass1 * P; + w1 -= _invI1 * L1; + + v2 += _invMass2 * P; + w2 += _invI2 * L2; + } + + float Cdot1 = Vec2.Dot(_perp, v2 - v1) + _s2 * w2 - _s1 * w1; + + if (_enableLimit && _limitState != LimitState.InactiveLimit) + { + // Solve prismatic and limit constraint in block form. + float Cdot2 = Vec2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1; + Vec2 Cdot = new Vec2(Cdot1, Cdot2); + + Vec2 f1 = _impulse; + Vec2 df = _K.Solve(-Cdot); + _impulse += df; + + if (_limitState == LimitState.AtLowerLimit) + { + _impulse.Y = Box2DX.Common.Math.Max(_impulse.Y, 0.0f); + } + else if (_limitState == LimitState.AtUpperLimit) + { + _impulse.Y = Box2DX.Common.Math.Min(_impulse.Y, 0.0f); + } + + // f2(1) = invK(1,1) * (-Cdot(1) - K(1,2) * (f2(2) - f1(2))) + f1(1) + float b = -Cdot1 - (_impulse.Y - f1.Y) * _K.Col2.X; + float f2r = b / _K.Col1.X + f1.X; + _impulse.X = f2r; + + df = _impulse - f1; + + Vec2 P = df.X * _perp + df.Y * _axis; + float L1 = df.X * _s1 + df.Y * _a1; + float L2 = df.X * _s2 + df.Y * _a2; + + v1 -= _invMass1 * P; + w1 -= _invI1 * L1; + + v2 += _invMass2 * P; + w2 += _invI2 * L2; + } + else + { + // Limit is inactive, just solve the prismatic constraint in block form. + float df = (-Cdot1) / _K.Col1.X; + _impulse.X += df; + + Vec2 P = df * _perp; + float L1 = df * _s1; + float L2 = df * _s2; + + v1 -= _invMass1 * P; + w1 -= _invI1 * L1; + + v2 += _invMass2 * P; + w2 += _invI2 * L2; + } + + b1._linearVelocity = v1; + b1._angularVelocity = w1; + b2._linearVelocity = v2; + b2._angularVelocity = w2; + } + + internal override bool SolvePositionConstraints(float baumgarte) + { + Body b1 = _body1; + Body b2 = _body2; + + Vec2 c1 = b1._sweep.C; + float a1 = b1._sweep.A; + + Vec2 c2 = b2._sweep.C; + float a2 = b2._sweep.A; + + // Solve linear limit constraint. + float linearError = 0.0f, angularError = 0.0f; + bool active = false; + float C2 = 0.0f; + + Mat22 R1 = new Mat22(a1), R2 = new Mat22(a2); + + Vec2 r1 = Box2DX.Common.Math.Mul(R1, _localAnchor1 - _localCenter1); + Vec2 r2 = Box2DX.Common.Math.Mul(R2, _localAnchor2 - _localCenter2); + Vec2 d = c2 + r2 - c1 - r1; + + if (_enableLimit) + { + _axis = Box2DX.Common.Math.Mul(R1, _localXAxis1); + + _a1 = Vec2.Cross(d + r1, _axis); + _a2 = Vec2.Cross(r2, _axis); + + float translation = Vec2.Dot(_axis, d); + if (Box2DX.Common.Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop) + { + // Prevent large angular corrections + C2 = Box2DX.Common.Math.Clamp(translation, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection); + linearError = Box2DX.Common.Math.Abs(translation); + active = true; + } + else if (translation <= _lowerTranslation) + { + // Prevent large linear corrections and allow some slop. + C2 = Box2DX.Common.Math.Clamp(translation - _lowerTranslation + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f); + linearError = _lowerTranslation - translation; + active = true; + } + else if (translation >= _upperTranslation) + { + // Prevent large linear corrections and allow some slop. + C2 = Box2DX.Common.Math.Clamp(translation - _upperTranslation - Settings.LinearSlop, 0.0f, Settings.MaxLinearCorrection); + linearError = translation - _upperTranslation; + active = true; + } + } + + _perp = Box2DX.Common.Math.Mul(R1, _localYAxis1); + + _s1 = Vec2.Cross(d + r1, _perp); + _s2 = Vec2.Cross(r2, _perp); + + Vec2 impulse; + float C1; + C1 = Vec2.Dot(_perp, d); + + linearError = Box2DX.Common.Math.Max(linearError, Box2DX.Common.Math.Abs(C1)); + angularError = 0.0f; + + if (active) + { + float m1 = _invMass1, m2 = _invMass2; + float i1 = _invI1, i2 = _invI2; + + float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2; + float k12 = i1 * _s1 * _a1 + i2 * _s2 * _a2; + float k22 = m1 + m2 + i1 * _a1 * _a1 + i2 * _a2 * _a2; + + _K.Col1.Set(k11, k12); + _K.Col2.Set(k12, k22); + + Vec2 C = new Vec2(); + C.X = C1; + C.Y = C2; + + impulse = _K.Solve(-C); + } + else + { + float m1 = _invMass1, m2 = _invMass2; + float i1 = _invI1, i2 = _invI2; + + float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2; + + float impulse1 = (-C1) / k11; + impulse.X = impulse1; + impulse.Y = 0.0f; + } + + Vec2 P = impulse.X * _perp + impulse.Y * _axis; + float L1 = impulse.X * _s1 + impulse.Y * _a1; + float L2 = impulse.X * _s2 + impulse.Y * _a2; + + c1 -= _invMass1 * P; + a1 -= _invI1 * L1; + c2 += _invMass2 * P; + a2 += _invI2 * L2; + + // TODO_ERIN remove need for this. + b1._sweep.C = c1; + b1._sweep.A = a1; + b2._sweep.C = c2; + b2._sweep.A = a2; + b1.SynchronizeTransform(); + b2.SynchronizeTransform(); + + return linearError <= Settings.LinearSlop && angularError <= Settings.AngularSlop; + } + } +}
\ No newline at end of file diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Joints/LineJoint.cs.meta b/Box2d/Assets/Program/Box2d/Dynamics/Joints/LineJoint.cs.meta new file mode 100644 index 0000000..f649ab4 --- /dev/null +++ b/Box2d/Assets/Program/Box2d/Dynamics/Joints/LineJoint.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 9817f1297db25b14d95b7c32530a3af6 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Joints/MouseJoint.cs b/Box2d/Assets/Program/Box2d/Dynamics/Joints/MouseJoint.cs new file mode 100644 index 0000000..19eb83c --- /dev/null +++ b/Box2d/Assets/Program/Box2d/Dynamics/Joints/MouseJoint.cs @@ -0,0 +1,229 @@ +/* + Box2DX Copyright (c) 2008 Ihar Kalasouski http://code.google.com/p/box2dx + Box2D original C++ version Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this 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. +*/ + +// p = attached point, m = mouse point +// C = p - m +// Cdot = v +// = v + cross(w, r) +// J = [I r_skew] +// Identity used: +// w k % (rx i + ry j) = w * (-ry i + rx j) + +using System; +using System.Collections.Generic; +using System.Text; + +using Box2DX.Common; + +namespace Box2DX.Dynamics +{ + /// <summary> + /// Mouse joint definition. This requires a world target point, + /// tuning parameters, and the time step. + /// </summary> + public class MouseJointDef : JointDef + { + public MouseJointDef() + { + Type = JointType.MouseJoint; + Target.Set(0.0f, 0.0f); + MaxForce = 0.0f; + FrequencyHz = 5.0f; + DampingRatio = 0.7f; + } + + /// <summary> + /// The initial world target point. This is assumed + /// to coincide with the body anchor initially. + /// </summary> + public Vec2 Target; + + /// <summary> + /// The maximum constraint force that can be exerted + /// to move the candidate body. Usually you will express + /// as some multiple of the weight (multiplier * mass * gravity). + /// </summary> + public float MaxForce; + + /// <summary> + /// The response speed. + /// </summary> + public float FrequencyHz; + + /// <summary> + /// The damping ratio. 0 = no damping, 1 = critical damping. + /// </summary> + public float DampingRatio; + } + + /// <summary> + /// A mouse joint is used to make a point on a body track a + /// specified world point. This a soft constraint with a maximum + /// force. This allows the constraint to stretch and without + /// applying huge forces. + /// </summary> + public class MouseJoint : Joint + { + public Vec2 _localAnchor; + public Vec2 _target; + public Vec2 _impulse; + + public Mat22 _mass; // effective mass for point-to-point constraint. + public Vec2 _C; // position error + public float _maxForce; + public float _frequencyHz; + public float _dampingRatio; + public float _beta; + public float _gamma; + + public override Vec2 Anchor1 + { + get { return _target; } + } + + public override Vec2 Anchor2 + { + get { return _body2.GetWorldPoint(_localAnchor); } + } + + public override Vec2 GetReactionForce(float inv_dt) + { + return inv_dt * _impulse; + } + + public override float GetReactionTorque(float inv_dt) + { + return inv_dt * 0.0f; + } + + /// <summary> + /// Use this to update the target point. + /// </summary> + public void SetTarget(Vec2 target) + { + if (_body2.IsSleeping()) + { + _body2.WakeUp(); + } + _target = target; + } + + public MouseJoint(MouseJointDef def) + : base(def) + { + _target = def.Target; + _localAnchor = Common.Math.MulT(_body2.GetXForm(), _target); + + _maxForce = def.MaxForce; + _impulse.SetZero(); + + _frequencyHz = def.FrequencyHz; + _dampingRatio = def.DampingRatio; + + _beta = 0.0f; + _gamma = 0.0f; + } + + internal override void InitVelocityConstraints(TimeStep step) + { + Body b = _body2; + + float mass = b.GetMass(); + + // Frequency + float omega = 2.0f * Settings.Pi * _frequencyHz; + + // Damping coefficient + float d = 2.0f * mass * _dampingRatio * omega; + + // Spring stiffness + float k = mass * (omega * omega); + + // magic formulas + // gamma has units of inverse mass. + // beta has units of inverse time. + Box2DXDebug.Assert(d + step.Dt * k > Settings.FLT_EPSILON); + _gamma = 1.0f / (step.Dt * (d + step.Dt * k)); + _beta = step.Dt * k * _gamma; + + // Compute the effective mass matrix. + Vec2 r = Common.Math.Mul(b.GetXForm().R, _localAnchor - b.GetLocalCenter()); + + // K = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)] + // = [1/m1+1/m2 0 ] + invI1 * [r1.y*r1.y -r1.x*r1.y] + invI2 * [r1.y*r1.y -r1.x*r1.y] + // [ 0 1/m1+1/m2] [-r1.x*r1.y r1.x*r1.x] [-r1.x*r1.y r1.x*r1.x] + float invMass = b._invMass; + float invI = b._invI; + + Mat22 K1 = new Mat22(); + K1.Col1.X = invMass; K1.Col2.X = 0.0f; + K1.Col1.Y = 0.0f; K1.Col2.Y = invMass; + + Mat22 K2 = new Mat22(); + K2.Col1.X = invI * r.Y * r.Y; K2.Col2.X = -invI * r.X * r.Y; + K2.Col1.Y = -invI * r.X * r.Y; K2.Col2.Y = invI * r.X * r.X; + + Mat22 K = K1 + K2; + K.Col1.X += _gamma; + K.Col2.Y += _gamma; + + _mass = K.GetInverse(); + + _C = b._sweep.C + r - _target; + + // Cheat with some damping + b._angularVelocity *= 0.98f; + + // Warm starting. + _impulse *= step.DtRatio; + b._linearVelocity += invMass * _impulse; + b._angularVelocity += invI * Vec2.Cross(r, _impulse); + } + + internal override void SolveVelocityConstraints(TimeStep step) + { + Body b = _body2; + + Vec2 r = Common.Math.Mul(b.GetXForm().R, _localAnchor - b.GetLocalCenter()); + + // Cdot = v + cross(w, r) + Vec2 Cdot = b._linearVelocity + Vec2.Cross(b._angularVelocity, r); + Vec2 impulse = Box2DX.Common.Math.Mul(_mass, -(Cdot + _beta * _C + _gamma * _impulse)); + + Vec2 oldImpulse = _impulse; + _impulse += impulse; + float maxImpulse = step.Dt * _maxForce; + if (_impulse.LengthSquared() > maxImpulse * maxImpulse) + { + _impulse *= maxImpulse / _impulse.Length(); + } + impulse = _impulse - oldImpulse; + + b._linearVelocity += b._invMass * impulse; + b._angularVelocity += b._invI * Vec2.Cross(r, impulse); + } + + internal override bool SolvePositionConstraints(float baumgarte) + { + return true; + } + } +} diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Joints/MouseJoint.cs.meta b/Box2d/Assets/Program/Box2d/Dynamics/Joints/MouseJoint.cs.meta new file mode 100644 index 0000000..6b7075b --- /dev/null +++ b/Box2d/Assets/Program/Box2d/Dynamics/Joints/MouseJoint.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 64a51a39bb7cd3f48b12a72161bd3199 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Joints/PrismaticJoint.cs b/Box2d/Assets/Program/Box2d/Dynamics/Joints/PrismaticJoint.cs new file mode 100644 index 0000000..c6ee2cf --- /dev/null +++ b/Box2d/Assets/Program/Box2d/Dynamics/Joints/PrismaticJoint.cs @@ -0,0 +1,756 @@ +/* + Box2DX Copyright (c) 2008 Ihar Kalasouski http://code.google.com/p/box2dx + Box2D original C++ version Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this 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. +*/ + +// Linear constraint (point-to-line) +// d = p2 - p1 = x2 + r2 - x1 - r1 +// C = dot(perp, d) +// Cdot = dot(d, cross(w1, perp)) + dot(perp, v2 + cross(w2, r2) - v1 - cross(w1, r1)) +// = -dot(perp, v1) - dot(cross(d + r1, perp), w1) + dot(perp, v2) + dot(cross(r2, perp), v2) +// J = [-perp, -cross(d + r1, perp), perp, cross(r2,perp)] +// +// Angular constraint +// C = a2 - a1 + a_initial +// Cdot = w2 - w1 +// J = [0 0 -1 0 0 1] +// +// K = J * invM * JT +// +// J = [-a -s1 a s2] +// [0 -1 0 1] +// a = perp +// s1 = cross(d + r1, a) = cross(p2 - x1, a) +// s2 = cross(r2, a) = cross(p2 - x2, a) + + +// Motor/Limit linear constraint +// C = dot(ax1, d) +// Cdot = = -dot(ax1, v1) - dot(cross(d + r1, ax1), w1) + dot(ax1, v2) + dot(cross(r2, ax1), v2) +// J = [-ax1 -cross(d+r1,ax1) ax1 cross(r2,ax1)] + +// Block Solver +// We develop a block solver that includes the joint limit. This makes the limit stiff (inelastic) even +// when the mass has poor distribution (leading to large torques about the joint anchor points). +// +// The Jacobian has 3 rows: +// J = [-uT -s1 uT s2] // linear +// [0 -1 0 1] // angular +// [-vT -a1 vT a2] // limit +// +// u = perp +// v = axis +// s1 = cross(d + r1, u), s2 = cross(r2, u) +// a1 = cross(d + r1, v), a2 = cross(r2, v) + +// M * (v2 - v1) = JT * df +// J * v2 = bias +// +// v2 = v1 + invM * JT * df +// J * (v1 + invM * JT * df) = bias +// K * df = bias - J * v1 = -Cdot +// K = J * invM * JT +// Cdot = J * v1 - bias +// +// Now solve for f2. +// df = f2 - f1 +// K * (f2 - f1) = -Cdot +// f2 = invK * (-Cdot) + f1 +// +// Clamp accumulated limit impulse. +// lower: f2(3) = max(f2(3), 0) +// upper: f2(3) = min(f2(3), 0) +// +// Solve for correct f2(1:2) +// K(1:2, 1:2) * f2(1:2) = -Cdot(1:2) - K(1:2,3) * f2(3) + K(1:2,1:3) * f1 +// = -Cdot(1:2) - K(1:2,3) * f2(3) + K(1:2,1:2) * f1(1:2) + K(1:2,3) * f1(3) +// K(1:2, 1:2) * f2(1:2) = -Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3)) + K(1:2,1:2) * f1(1:2) +// f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + f1(1:2) +// +// Now compute impulse to be applied: +// df = f2 - f1 + +using System; +using System.Collections.Generic; +using System.Text; + +using Box2DX.Common; + +namespace Box2DX.Dynamics +{ + using Box2DXMath = Box2DX.Common.Math; + using SystemMath = System.Math; + + /// <summary> + /// Prismatic joint definition. This requires defining a line of + /// motion using an axis and an anchor point. The definition uses local + /// anchor points and a local axis so that the initial configuration + /// can violate the constraint slightly. The joint translation is zero + /// when the local anchor points coincide in world space. Using local + /// anchors and a local axis helps when saving and loading a game. + /// </summary> + public class PrismaticJointDef : JointDef + { + public PrismaticJointDef() + { + Type = JointType.PrismaticJoint; + LocalAnchor1.SetZero(); + LocalAnchor2.SetZero(); + LocalAxis1.Set(1.0f, 0.0f); + ReferenceAngle = 0.0f; + EnableLimit = false; + LowerTranslation = 0.0f; + UpperTranslation = 0.0f; + EnableMotor = false; + MaxMotorForce = 0.0f; + MotorSpeed = 0.0f; + } + + /// <summary> + /// Initialize the bodies, anchors, axis, and reference angle using the world + /// anchor and world axis. + /// </summary> + public void Initialize(Body body1, Body body2, Vec2 anchor, Vec2 axis) + { + Body1 = body1; + Body2 = body2; + LocalAnchor1 = body1.GetLocalPoint(anchor); + LocalAnchor2 = body2.GetLocalPoint(anchor); + LocalAxis1 = body1.GetLocalVector(axis); + ReferenceAngle = body2.GetAngle() - body1.GetAngle(); + } + + /// <summary> + /// The local anchor point relative to body1's origin. + /// </summary> + public Vec2 LocalAnchor1; + + /// <summary> + /// The local anchor point relative to body2's origin. + /// </summary> + public Vec2 LocalAnchor2; + + /// <summary> + /// The local translation axis in body1. + /// </summary> + public Vec2 LocalAxis1; + + /// <summary> + /// The constrained angle between the bodies: body2_angle - body1_angle. + /// </summary> + public float ReferenceAngle; + + /// <summary> + /// Enable/disable the joint limit. + /// </summary> + public bool EnableLimit; + + /// <summary> + /// The lower translation limit, usually in meters. + /// </summary> + public float LowerTranslation; + + /// <summary> + /// The upper translation limit, usually in meters. + /// </summary> + public float UpperTranslation; + + /// <summary> + /// Enable/disable the joint motor. + /// </summary> + public bool EnableMotor; + + /// <summary> + /// The maximum motor torque, usually in N-m. + /// </summary> + public float MaxMotorForce; + + /// <summary> + /// The desired motor speed in radians per second. + /// </summary> + public float MotorSpeed; + } + + /// <summary> + /// A prismatic joint. This joint provides one degree of freedom: translation + /// along an axis fixed in body1. Relative rotation is prevented. You can + /// use a joint limit to restrict the range of motion and a joint motor to + /// drive the motion or to model joint friction. + /// </summary> + public class PrismaticJoint : Joint + { + public Vec2 _localAnchor1; + public Vec2 _localAnchor2; + public Vec2 _localXAxis1; + public Vec2 _localYAxis1; + public float _refAngle; + + public Vec2 _axis, _perp; + public float _s1, _s2; + public float _a1, _a2; + + public Mat33 _K; + public Vec3 _impulse; + + public float _motorMass; // effective mass for motor/limit translational constraint. + public float _motorImpulse; + + public float _lowerTranslation; + public float _upperTranslation; + public float _maxMotorForce; + public float _motorSpeed; + + public bool _enableLimit; + public bool _enableMotor; + public LimitState _limitState; + + public override Vec2 Anchor1 + { + get { return _body1.GetWorldPoint(_localAnchor1); } + } + + public override Vec2 Anchor2 + { + get { return _body2.GetWorldPoint(_localAnchor2); } + } + + public override Vec2 GetReactionForce(float inv_dt) + { + return inv_dt * (_impulse.X * _perp + (_motorImpulse + _impulse.Z) * _axis); + } + + public override float GetReactionTorque(float inv_dt) + { + return inv_dt * _impulse.Y; + } + + /// <summary> + /// Get the current joint translation, usually in meters. + /// </summary> + public float JointTranslation + { + get + { + Body b1 = _body1; + Body b2 = _body2; + + Vec2 p1 = b1.GetWorldPoint(_localAnchor1); + Vec2 p2 = b2.GetWorldPoint(_localAnchor2); + Vec2 d = p2 - p1; + Vec2 axis = b1.GetWorldVector(_localXAxis1); + + float translation = Vec2.Dot(d, axis); + return translation; + } + } + + /// <summary> + /// Get the current joint translation speed, usually in meters per second. + /// </summary> + public float JointSpeed + { + get + { + Body b1 = _body1; + Body b2 = _body2; + + Vec2 r1 = Common.Math.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter()); + Vec2 r2 = Common.Math.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter()); + Vec2 p1 = b1._sweep.C + r1; + Vec2 p2 = b2._sweep.C + r2; + Vec2 d = p2 - p1; + Vec2 axis = b1.GetWorldVector(_localXAxis1); + + Vec2 v1 = b1._linearVelocity; + Vec2 v2 = b2._linearVelocity; + float w1 = b1._angularVelocity; + float w2 = b2._angularVelocity; + + float speed = Vec2.Dot(d, Vec2.Cross(w1, axis)) + Vec2.Dot(axis, v2 + Vec2.Cross(w2, r2) - v1 - Vec2.Cross(w1, r1)); + return speed; + } + } + + /// <summary> + /// Is the joint limit enabled? + /// </summary> + public bool IsLimitEnabled + { + get { return _enableLimit; } + } + + /// <summary> + /// Enable/disable the joint limit. + /// </summary> + public void EnableLimit(bool flag) + { + _body1.WakeUp(); + _body2.WakeUp(); + _enableLimit = flag; + } + + /// <summary> + /// Get the lower joint limit, usually in meters. + /// </summary> + public float LowerLimit + { + get { return _lowerTranslation; } + } + + /// <summary> + /// Get the upper joint limit, usually in meters. + /// </summary> + public float UpperLimit + { + get { return _upperTranslation; } + } + + /// <summary> + /// Set the joint limits, usually in meters. + /// </summary> + public void SetLimits(float lower, float upper) + { + Box2DXDebug.Assert(lower <= upper); + _body1.WakeUp(); + _body2.WakeUp(); + _lowerTranslation = lower; + _upperTranslation = upper; + } + + /// <summary> + /// Is the joint motor enabled? + /// </summary> + public bool IsMotorEnabled + { + get { return _enableMotor; } + } + + /// <summary> + /// Enable/disable the joint motor. + /// </summary> + public void EnableMotor(bool flag) + { + _body1.WakeUp(); + _body2.WakeUp(); + _enableMotor = flag; + } + + /// <summary> + /// Get\Set the motor speed, usually in meters per second. + /// </summary> + public float MotorSpeed + { + get { return _motorSpeed; } + set + { + _body1.WakeUp(); + _body2.WakeUp(); + _motorSpeed = value; + } + } + + /// <summary> + /// Set the maximum motor force, usually in N. + /// </summary> + public void SetMaxMotorForce(float force) + { + _body1.WakeUp(); + _body2.WakeUp(); + _maxMotorForce = Settings.FORCE_SCALE(1.0f) * force; + } + + /// <summary> + /// Get the current motor force, usually in N. + /// </summary> + public float MotorForce + { + get { return _motorImpulse; } + } + + public PrismaticJoint(PrismaticJointDef def) + : base(def) + { + _localAnchor1 = def.LocalAnchor1; + _localAnchor2 = def.LocalAnchor2; + _localXAxis1 = def.LocalAxis1; + _localYAxis1 = Vec2.Cross(1.0f, _localXAxis1); + _refAngle = def.ReferenceAngle; + + _impulse.SetZero(); + _motorMass = 0.0f; + _motorImpulse = 0.0f; + + _lowerTranslation = def.LowerTranslation; + _upperTranslation = def.UpperTranslation; + _maxMotorForce = Settings.FORCE_INV_SCALE(def.MaxMotorForce); + _motorSpeed = def.MotorSpeed; + _enableLimit = def.EnableLimit; + _enableMotor = def.EnableMotor; + _limitState = LimitState.InactiveLimit; + + _axis.SetZero(); + _perp.SetZero(); + } + + internal override void InitVelocityConstraints(TimeStep step) + { + Body b1 = _body1; + Body b2 = _body2; + + // You cannot create a prismatic joint between bodies that + // both have fixed rotation. + Box2DXDebug.Assert(b1._invI > 0.0f || b2._invI > 0.0f); + + _localCenter1 = b1.GetLocalCenter(); + _localCenter2 = b2.GetLocalCenter(); + + XForm xf1 = b1.GetXForm(); + XForm xf2 = b2.GetXForm(); + + // Compute the effective masses. + Vec2 r1 = Box2DX.Common.Math.Mul(xf1.R, _localAnchor1 - _localCenter1); + Vec2 r2 = Box2DX.Common.Math.Mul(xf2.R, _localAnchor2 - _localCenter2); + Vec2 d = b2._sweep.C + r2 - b1._sweep.C - r1; + + _invMass1 = b1._invMass; + _invI1 = b1._invI; + _invMass2 = b2._invMass; + _invI2 = b2._invI; + + // Compute motor Jacobian and effective mass. + { + _axis = Box2DX.Common.Math.Mul(xf1.R, _localXAxis1); + _a1 = Vec2.Cross(d + r1, _axis); + _a2 = Vec2.Cross(r2, _axis); + + _motorMass = _invMass1 + _invMass2 + _invI1 * _a1 * _a1 + _invI2 * _a2 * _a2; + Box2DXDebug.Assert(_motorMass > Settings.FLT_EPSILON); + _motorMass = 1.0f / _motorMass; + } + + // Prismatic constraint. + { + _perp = Box2DX.Common.Math.Mul(xf1.R, _localYAxis1); + + _s1 = Vec2.Cross(d + r1, _perp); + _s2 = Vec2.Cross(r2, _perp); + + float m1 = _invMass1, m2 = _invMass2; + float i1 = _invI1, i2 = _invI2; + + float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2; + float k12 = i1 * _s1 + i2 * _s2; + float k13 = i1 * _s1 * _a1 + i2 * _s2 * _a2; + float k22 = i1 + i2; + float k23 = i1 * _a1 + i2 * _a2; + float k33 = m1 + m2 + i1 * _a1 * _a1 + i2 * _a2 * _a2; + + _K.Col1.Set(k11, k12, k13); + _K.Col2.Set(k12, k22, k23); + _K.Col3.Set(k13, k23, k33); + } + + // Compute motor and limit terms. + if (_enableLimit) + { + float jointTranslation = Vec2.Dot(_axis, d); + if (Box2DX.Common.Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop) + { + _limitState = LimitState.EqualLimits; + } + else if (jointTranslation <= _lowerTranslation) + { + if (_limitState != LimitState.AtLowerLimit) + { + _limitState = LimitState.AtLowerLimit; + _impulse.Z = 0.0f; + } + } + else if (jointTranslation >= _upperTranslation) + { + if (_limitState != LimitState.AtUpperLimit) + { + _limitState = LimitState.AtUpperLimit; + _impulse.Z = 0.0f; + } + } + else + { + _limitState = LimitState.InactiveLimit; + _impulse.Z = 0.0f; + } + } + else + { + _limitState = LimitState.InactiveLimit; + } + + if (_enableMotor == false) + { + _motorImpulse = 0.0f; + } + + if (step.WarmStarting) + { + // Account for variable time step. + _impulse *= step.DtRatio; + _motorImpulse *= step.DtRatio; + + Vec2 P = _impulse.X * _perp + (_motorImpulse + _impulse.Z) * _axis; + float L1 = _impulse.X * _s1 + _impulse.Y + (_motorImpulse + _impulse.Z) * _a1; + float L2 = _impulse.X * _s2 + _impulse.Y + (_motorImpulse + _impulse.Z) * _a2; + + b1._linearVelocity -= _invMass1 * P; + b1._angularVelocity -= _invI1 * L1; + + b2._linearVelocity += _invMass2 * P; + b2._angularVelocity += _invI2 * L2; + } + else + { + _impulse.SetZero(); + _motorImpulse = 0.0f; + } + } + + internal override void SolveVelocityConstraints(TimeStep step) + { + Body b1 = _body1; + Body b2 = _body2; + + Vec2 v1 = b1._linearVelocity; + float w1 = b1._angularVelocity; + Vec2 v2 = b2._linearVelocity; + float w2 = b2._angularVelocity; + + // Solve linear motor constraint. + if (_enableMotor && _limitState != LimitState.EqualLimits) + { + float Cdot = Vec2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1; + float impulse = _motorMass * (_motorSpeed - Cdot); + float oldImpulse = _motorImpulse; + float maxImpulse = step.Dt * _maxMotorForce; + _motorImpulse = Box2DX.Common.Math.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse); + impulse = _motorImpulse - oldImpulse; + + Vec2 P = impulse * _axis; + float L1 = impulse * _a1; + float L2 = impulse * _a2; + + v1 -= _invMass1 * P; + w1 -= _invI1 * L1; + + v2 += _invMass2 * P; + w2 += _invI2 * L2; + } + + Vec2 Cdot1; + Cdot1.X = Vec2.Dot(_perp, v2 - v1) + _s2 * w2 - _s1 * w1; + Cdot1.Y = w2 - w1; + + if (_enableLimit && _limitState != LimitState.InactiveLimit) + { + // Solve prismatic and limit constraint in block form. + float Cdot2; + Cdot2 = Vec2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1; + Vec3 Cdot = new Vec3(Cdot1.X, Cdot1.Y, Cdot2); + + Vec3 f1 = _impulse; + Vec3 df = _K.Solve33(-Cdot); + _impulse += df; + + if (_limitState ==LimitState.AtLowerLimit) + { + _impulse.Z = Box2DX.Common.Math.Max(_impulse.Z, 0.0f); + } + else if (_limitState == LimitState.AtUpperLimit) + { + _impulse.Z = Box2DX.Common.Math.Min(_impulse.Z, 0.0f); + } + + // f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + f1(1:2) + Vec2 b = -Cdot1 - (_impulse.Z - f1.Z) * new Vec2(_K.Col3.X, _K.Col3.Y); + Vec2 f2r = _K.Solve22(b) + new Vec2(f1.X, f1.Y); + _impulse.X = f2r.X; + _impulse.Y = f2r.Y; + + df = _impulse - f1; + + Vec2 P = df.X * _perp + df.Z * _axis; + float L1 = df.X * _s1 + df.Y + df.Z * _a1; + float L2 = df.X * _s2 + df.Y + df.Z * _a2; + + v1 -= _invMass1 * P; + w1 -= _invI1 * L1; + + v2 += _invMass2 * P; + w2 += _invI2 * L2; + } + else + { + // Limit is inactive, just solve the prismatic constraint in block form. + Vec2 df = _K.Solve22(-Cdot1); + _impulse.X += df.X; + _impulse.Y += df.Y; + + Vec2 P = df.X * _perp; + float L1 = df.X * _s1 + df.Y; + float L2 = df.X * _s2 + df.Y; + + v1 -= _invMass1 * P; + w1 -= _invI1 * L1; + + v2 += _invMass2 * P; + w2 += _invI2 * L2; + } + + b1._linearVelocity = v1; + b1._angularVelocity = w1; + b2._linearVelocity = v2; + b2._angularVelocity = w2; + } + + internal override bool SolvePositionConstraints(float baumgarte) + { + Body b1 = _body1; + Body b2 = _body2; + + Vec2 c1 = b1._sweep.C; + float a1 = b1._sweep.A; + + Vec2 c2 = b2._sweep.C; + float a2 = b2._sweep.A; + + // Solve linear limit constraint. + float linearError = 0.0f, angularError = 0.0f; + bool active = false; + float C2 = 0.0f; + + Mat22 R1 = new Mat22(a1), R2 = new Mat22(a2); + + Vec2 r1 = Box2DX.Common.Math.Mul(R1, _localAnchor1 - _localCenter1); + Vec2 r2 = Box2DX.Common.Math.Mul(R2, _localAnchor2 - _localCenter2); + Vec2 d = c2 + r2 - c1 - r1; + + if (_enableLimit) + { + _axis = Box2DX.Common.Math.Mul(R1, _localXAxis1); + + _a1 = Vec2.Cross(d + r1, _axis); + _a2 = Vec2.Cross(r2, _axis); + + float translation = Vec2.Dot(_axis, d); + if (Box2DX.Common.Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop) + { + // Prevent large angular corrections + C2 = Box2DX.Common.Math.Clamp(translation, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection); + linearError = Box2DX.Common.Math.Abs(translation); + active = true; + } + else if (translation <= _lowerTranslation) + { + // Prevent large linear corrections and allow some slop. + C2 = Box2DX.Common.Math.Clamp(translation - _lowerTranslation + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f); + linearError = _lowerTranslation - translation; + active = true; + } + else if (translation >= _upperTranslation) + { + // Prevent large linear corrections and allow some slop. + C2 = Box2DX.Common.Math.Clamp(translation - _upperTranslation - Settings.LinearSlop, 0.0f, Settings.MaxLinearCorrection); + linearError = translation - _upperTranslation; + active = true; + } + } + + _perp = Box2DX.Common.Math.Mul(R1, _localYAxis1); + + _s1 = Vec2.Cross(d + r1, _perp); + _s2 = Vec2.Cross(r2, _perp); + + Vec3 impulse; + Vec2 C1 = new Vec2(); + C1.X = Vec2.Dot(_perp, d); + C1.Y = a2 - a1 - _refAngle; + + linearError = Box2DX.Common.Math.Max(linearError, Box2DX.Common.Math.Abs(C1.X)); + angularError = Box2DX.Common.Math.Abs(C1.Y); + + if (active) + { + float m1 = _invMass1, m2 = _invMass2; + float i1 = _invI1, i2 = _invI2; + + float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2; + float k12 = i1 * _s1 + i2 * _s2; + float k13 = i1 * _s1 * _a1 + i2 * _s2 * _a2; + float k22 = i1 + i2; + float k23 = i1 * _a1 + i2 * _a2; + float k33 = m1 + m2 + i1 * _a1 * _a1 + i2 * _a2 * _a2; + + _K.Col1.Set(k11, k12, k13); + _K.Col2.Set(k12, k22, k23); + _K.Col3.Set(k13, k23, k33); + + Vec3 C = new Vec3(); + C.X = C1.X; + C.Y = C1.Y; + C.Z = C2; + + impulse = _K.Solve33(-C); + } + else + { + float m1 = _invMass1, m2 = _invMass2; + float i1 = _invI1, i2 = _invI2; + + float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2; + float k12 = i1 * _s1 + i2 * _s2; + float k22 = i1 + i2; + + _K.Col1.Set(k11, k12, 0.0f); + _K.Col2.Set(k12, k22, 0.0f); + + Vec2 impulse1 = _K.Solve22(-C1); + impulse.X = impulse1.X; + impulse.Y = impulse1.Y; + impulse.Z = 0.0f; + } + + Vec2 P = impulse.X * _perp + impulse.Z * _axis; + float L1 = impulse.X * _s1 + impulse.Y + impulse.Z * _a1; + float L2 = impulse.X * _s2 + impulse.Y + impulse.Z * _a2; + + c1 -= _invMass1 * P; + a1 -= _invI1 * L1; + c2 += _invMass2 * P; + a2 += _invI2 * L2; + + // TODO_ERIN remove need for this. + b1._sweep.C = c1; + b1._sweep.A = a1; + b2._sweep.C = c2; + b2._sweep.A = a2; + b1.SynchronizeTransform(); + b2.SynchronizeTransform(); + + return linearError <= Settings.LinearSlop && angularError <= Settings.AngularSlop; + } + } +} diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Joints/PrismaticJoint.cs.meta b/Box2d/Assets/Program/Box2d/Dynamics/Joints/PrismaticJoint.cs.meta new file mode 100644 index 0000000..31fdd0e --- /dev/null +++ b/Box2d/Assets/Program/Box2d/Dynamics/Joints/PrismaticJoint.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 0d3c4a5269cc639479ba35c87378787f +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Joints/PulleyJoint.cs b/Box2d/Assets/Program/Box2d/Dynamics/Joints/PulleyJoint.cs new file mode 100644 index 0000000..ab92536 --- /dev/null +++ b/Box2d/Assets/Program/Box2d/Dynamics/Joints/PulleyJoint.cs @@ -0,0 +1,566 @@ +/* + Box2DX Copyright (c) 2008 Ihar Kalasouski http://code.google.com/p/box2dx + Box2D original C++ version Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this 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. +*/ + +// Pulley: +// length1 = norm(p1 - s1) +// length2 = norm(p2 - s2) +// C0 = (length1 + ratio * length2)_initial +// C = C0 - (length1 + ratio * length2) >= 0 +// u1 = (p1 - s1) / norm(p1 - s1) +// u2 = (p2 - s2) / norm(p2 - s2) +// Cdot = -dot(u1, v1 + cross(w1, r1)) - ratio * dot(u2, v2 + cross(w2, r2)) +// J = -[u1 cross(r1, u1) ratio * u2 ratio * cross(r2, u2)] +// K = J * invM * JT +// = invMass1 + invI1 * cross(r1, u1)^2 + ratio^2 * (invMass2 + invI2 * cross(r2, u2)^2) +// +// Limit: +// C = maxLength - length +// u = (p - s) / norm(p - s) +// Cdot = -dot(u, v + cross(w, r)) +// K = invMass + invI * cross(r, u)^2 +// 0 <= impulse + +using System; +using System.Collections.Generic; +using System.Text; + +using Box2DX.Common; + +namespace Box2DX.Dynamics +{ + using Box2DXMath = Box2DX.Common.Math; + using SystemMath = System.Math; + + /// <summary> + /// Pulley joint definition. This requires two ground anchors, + /// two dynamic body anchor points, max lengths for each side, + /// and a pulley ratio. + /// </summary> + public class PulleyJointDef : JointDef + { + public PulleyJointDef() + { + Type = JointType.PulleyJoint; + GroundAnchor1.Set(-1.0f, 1.0f); + GroundAnchor2.Set(1.0f, 1.0f); + LocalAnchor1.Set(-1.0f, 0.0f); + LocalAnchor2.Set(1.0f, 0.0f); + Length1 = 0.0f; + MaxLength1 = 0.0f; + Length2 = 0.0f; + MaxLength2 = 0.0f; + Ratio = 1.0f; + CollideConnected = true; + } + + /// Initialize the bodies, anchors, lengths, max lengths, and ratio using the world anchors. + public void Initialize(Body body1, Body body2, + Vec2 groundAnchor1, Vec2 groundAnchor2, + Vec2 anchor1, Vec2 anchor2, + float ratio) + { + Body1 = body1; + Body2 = body2; + GroundAnchor1 = groundAnchor1; + GroundAnchor2 = groundAnchor2; + LocalAnchor1 = body1.GetLocalPoint(anchor1); + LocalAnchor2 = body2.GetLocalPoint(anchor2); + Vec2 d1 = anchor1 - groundAnchor1; + Length1 = d1.Length(); + Vec2 d2 = anchor2 - groundAnchor2; + Length2 = d2.Length(); + Ratio = ratio; + Box2DXDebug.Assert(ratio > Settings.FLT_EPSILON); + float C = Length1 + ratio * Length2; + MaxLength1 = C - ratio * PulleyJoint.MinPulleyLength; + MaxLength2 = (C - PulleyJoint.MinPulleyLength) / ratio; + } + + /// <summary> + /// The first ground anchor in world coordinates. This point never moves. + /// </summary> + public Vec2 GroundAnchor1; + + /// <summary> + /// The second ground anchor in world coordinates. This point never moves. + /// </summary> + public Vec2 GroundAnchor2; + + /// <summary> + /// The local anchor point relative to body1's origin. + /// </summary> + public Vec2 LocalAnchor1; + + /// <summary> + /// The local anchor point relative to body2's origin. + /// </summary> + public Vec2 LocalAnchor2; + + /// <summary> + /// The a reference length for the segment attached to body1. + /// </summary> + public float Length1; + + /// <summary> + /// The maximum length of the segment attached to body1. + /// </summary> + public float MaxLength1; + + /// <summary> + /// The a reference length for the segment attached to body2. + /// </summary> + public float Length2; + + /// <summary> + /// The maximum length of the segment attached to body2. + /// </summary> + public float MaxLength2; + + /// <summary> + /// The pulley ratio, used to simulate a block-and-tackle. + /// </summary> + public float Ratio; + } + + /// <summary> + /// The pulley joint is connected to two bodies and two fixed ground points. + /// The pulley supports a ratio such that: + /// length1 + ratio * length2 <= constant + /// Yes, the force transmitted is scaled by the ratio. + /// The pulley also enforces a maximum length limit on both sides. This is + /// useful to prevent one side of the pulley hitting the top. + /// </summary> + public class PulleyJoint : Joint + { + public static readonly float MinPulleyLength = 2.0f; + + public Body _ground; + public Vec2 _groundAnchor1; + public Vec2 _groundAnchor2; + public Vec2 _localAnchor1; + public Vec2 _localAnchor2; + + public Vec2 _u1; + public Vec2 _u2; + + public float _constant; + public float _ratio; + + public float _maxLength1; + public float _maxLength2; + + // Effective masses + public float _pulleyMass; + public float _limitMass1; + public float _limitMass2; + + // Impulses for accumulation/warm starting. + public float _impulse; + public float _limitImpulse1; + public float _limitImpulse2; + + public LimitState _state; + public LimitState _limitState1; + public LimitState _limitState2; + + public override Vec2 Anchor1 + { + get { return _body1.GetWorldPoint(_localAnchor1); } + } + + public override Vec2 Anchor2 + { + get { return _body2.GetWorldPoint(_localAnchor2); } + } + + public override Vec2 GetReactionForce(float inv_dt) + { + Vec2 P = _impulse * _u2; + return inv_dt * P; + } + + public override float GetReactionTorque(float inv_dt) + { + return 0.0f; + } + + /// <summary> + /// Get the first ground anchor. + /// </summary> + public Vec2 GroundAnchor1 + { + get { return _ground.GetXForm().Position + _groundAnchor1; } + } + + /// <summary> + /// Get the second ground anchor. + /// </summary> + public Vec2 GroundAnchor2 + { + get { return _ground.GetXForm().Position + _groundAnchor2; } + } + + /// <summary> + /// Get the current length of the segment attached to body1. + /// </summary> + public float Length1 + { + get + { + Vec2 p = _body1.GetWorldPoint(_localAnchor1); + Vec2 s = _ground.GetXForm().Position + _groundAnchor1; + Vec2 d = p - s; + return d.Length(); + } + } + + /// <summary> + /// Get the current length of the segment attached to body2. + /// </summary> + public float Length2 + { + get + { + Vec2 p = _body2.GetWorldPoint(_localAnchor2); + Vec2 s = _ground.GetXForm().Position + _groundAnchor2; + Vec2 d = p - s; + return d.Length(); + } + } + + /// <summary> + /// Get the pulley ratio. + /// </summary> + public float Ratio + { + get { return _ratio; } + } + + public PulleyJoint(PulleyJointDef def) + : base(def) + { + _ground = _body1.GetWorld().GetGroundBody(); + _groundAnchor1 = def.GroundAnchor1 - _ground.GetXForm().Position; + _groundAnchor2 = def.GroundAnchor2 - _ground.GetXForm().Position; + _localAnchor1 = def.LocalAnchor1; + _localAnchor2 = def.LocalAnchor2; + + Box2DXDebug.Assert(def.Ratio != 0.0f); + _ratio = def.Ratio; + + _constant = def.Length1 + _ratio * def.Length2; + + _maxLength1 = Common.Math.Min(def.MaxLength1, _constant - _ratio * PulleyJoint.MinPulleyLength); + _maxLength2 = Common.Math.Min(def.MaxLength2, (_constant - PulleyJoint.MinPulleyLength) / _ratio); + + _impulse = 0.0f; + _limitImpulse1 = 0.0f; + _limitImpulse2 = 0.0f; + } + + internal override void InitVelocityConstraints(TimeStep step) + { + Body b1 = _body1; + Body b2 = _body2; + + Vec2 r1 = Box2DXMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter()); + Vec2 r2 = Box2DXMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter()); + + Vec2 p1 = b1._sweep.C + r1; + Vec2 p2 = b2._sweep.C + r2; + + Vec2 s1 = _ground.GetXForm().Position + _groundAnchor1; + Vec2 s2 = _ground.GetXForm().Position + _groundAnchor2; + + // Get the pulley axes. + _u1 = p1 - s1; + _u2 = p2 - s2; + + float length1 = _u1.Length(); + float length2 = _u2.Length(); + + if (length1 > Settings.LinearSlop) + { + _u1 *= 1.0f / length1; + } + else + { + _u1.SetZero(); + } + + if (length2 > Settings.LinearSlop) + { + _u2 *= 1.0f / length2; + } + else + { + _u2.SetZero(); + } + + float C = _constant - length1 - _ratio * length2; + if (C > 0.0f) + { + _state = LimitState.InactiveLimit; + _impulse = 0.0f; + } + else + { + _state = LimitState.AtUpperLimit; + } + + if (length1 < _maxLength1) + { + _limitState1 = LimitState.InactiveLimit; + _limitImpulse1 = 0.0f; + } + else + { + _limitState1 = LimitState.AtUpperLimit; + } + + if (length2 < _maxLength2) + { + _limitState2 = LimitState.InactiveLimit; + _limitImpulse2 = 0.0f; + } + else + { + _limitState2 = LimitState.AtUpperLimit; + } + + // Compute effective mass. + float cr1u1 = Vec2.Cross(r1, _u1); + float cr2u2 = Vec2.Cross(r2, _u2); + + _limitMass1 = b1._invMass + b1._invI * cr1u1 * cr1u1; + _limitMass2 = b2._invMass + b2._invI * cr2u2 * cr2u2; + _pulleyMass = _limitMass1 + _ratio * _ratio * _limitMass2; + Box2DXDebug.Assert(_limitMass1 > Settings.FLT_EPSILON); + Box2DXDebug.Assert(_limitMass2 > Settings.FLT_EPSILON); + Box2DXDebug.Assert(_pulleyMass > Settings.FLT_EPSILON); + _limitMass1 = 1.0f / _limitMass1; + _limitMass2 = 1.0f / _limitMass2; + _pulleyMass = 1.0f / _pulleyMass; + + if (step.WarmStarting) + { + // Scale impulses to support variable time steps. + _impulse *= step.DtRatio; + _limitImpulse1 *= step.DtRatio; + _limitImpulse2 *= step.DtRatio; + + // Warm starting. + Vec2 P1 = -(_impulse + _limitImpulse1) * _u1; + Vec2 P2 = (-_ratio * _impulse - _limitImpulse2) * _u2; + b1._linearVelocity += b1._invMass * P1; + b1._angularVelocity += b1._invI * Vec2.Cross(r1, P1); + b2._linearVelocity += b2._invMass * P2; + b2._angularVelocity += b2._invI * Vec2.Cross(r2, P2); + } + else + { + _impulse = 0.0f; + _limitImpulse1 = 0.0f; + _limitImpulse2 = 0.0f; + } + } + + internal override void SolveVelocityConstraints(TimeStep step) + { + Body b1 = _body1; + Body b2 = _body2; + + Vec2 r1 = Box2DXMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter()); + Vec2 r2 = Box2DXMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter()); + + if (_state == LimitState.AtUpperLimit) + { + Vec2 v1 = b1._linearVelocity + Vec2.Cross(b1._angularVelocity, r1); + Vec2 v2 = b2._linearVelocity + Vec2.Cross(b2._angularVelocity, r2); + + float Cdot = -Vec2.Dot(_u1, v1) - _ratio * Vec2.Dot(_u2, v2); + float impulse = _pulleyMass * (-Cdot); + float oldImpulse = _impulse; + _impulse = Box2DX.Common.Math.Max(0.0f, _impulse + impulse); + impulse = _impulse - oldImpulse; + + Vec2 P1 = -impulse * _u1; + Vec2 P2 = -_ratio * impulse * _u2; + b1._linearVelocity += b1._invMass * P1; + b1._angularVelocity += b1._invI * Vec2.Cross(r1, P1); + b2._linearVelocity += b2._invMass * P2; + b2._angularVelocity += b2._invI * Vec2.Cross(r2, P2); + } + + if (_limitState1 == LimitState.AtUpperLimit) + { + Vec2 v1 = b1._linearVelocity + Vec2.Cross(b1._angularVelocity, r1); + + float Cdot = -Vec2.Dot(_u1, v1); + float impulse = -_limitMass1 * Cdot; + float oldImpulse = _limitImpulse1; + _limitImpulse1 = Box2DX.Common.Math.Max(0.0f, _limitImpulse1 + impulse); + impulse = _limitImpulse1 - oldImpulse; + + Vec2 P1 = -impulse * _u1; + b1._linearVelocity += b1._invMass * P1; + b1._angularVelocity += b1._invI * Vec2.Cross(r1, P1); + } + + if (_limitState2 == LimitState.AtUpperLimit) + { + Vec2 v2 = b2._linearVelocity + Vec2.Cross(b2._angularVelocity, r2); + + float Cdot = -Vec2.Dot(_u2, v2); + float impulse = -_limitMass2 * Cdot; + float oldImpulse = _limitImpulse2; + _limitImpulse2 = Box2DX.Common.Math.Max(0.0f, _limitImpulse2 + impulse); + impulse = _limitImpulse2 - oldImpulse; + + Vec2 P2 = -impulse * _u2; + b2._linearVelocity += b2._invMass * P2; + b2._angularVelocity += b2._invI * Vec2.Cross(r2, P2); + } + } + + internal override bool SolvePositionConstraints(float baumgarte) + { + Body b1 = _body1; + Body b2 = _body2; + + Vec2 s1 = _ground.GetXForm().Position + _groundAnchor1; + Vec2 s2 = _ground.GetXForm().Position + _groundAnchor2; + + float linearError = 0.0f; + + if (_state == LimitState.AtUpperLimit) + { + Vec2 r1 = Box2DXMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter()); + Vec2 r2 = Box2DXMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter()); + + Vec2 p1 = b1._sweep.C + r1; + Vec2 p2 = b2._sweep.C + r2; + + // Get the pulley axes. + _u1 = p1 - s1; + _u2 = p2 - s2; + + float length1 = _u1.Length(); + float length2 = _u2.Length(); + + if (length1 > Settings.LinearSlop) + { + _u1 *= 1.0f / length1; + } + else + { + _u1.SetZero(); + } + + if (length2 > Settings.LinearSlop) + { + _u2 *= 1.0f / length2; + } + else + { + _u2.SetZero(); + } + + float C = _constant - length1 - _ratio * length2; + linearError = Box2DXMath.Max(linearError, -C); + + C = Box2DXMath.Clamp(C + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f); + float impulse = -_pulleyMass * C; + + Vec2 P1 = -impulse * _u1; + Vec2 P2 = -_ratio * impulse * _u2; + + b1._sweep.C += b1._invMass * P1; + b1._sweep.A += b1._invI * Vec2.Cross(r1, P1); + b2._sweep.C += b2._invMass * P2; + b2._sweep.A += b2._invI * Vec2.Cross(r2, P2); + + b1.SynchronizeTransform(); + b2.SynchronizeTransform(); + } + + if (_limitState1 == LimitState.AtUpperLimit) + { + Vec2 r1 = Box2DXMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter()); + Vec2 p1 = b1._sweep.C + r1; + + _u1 = p1 - s1; + float length1 = _u1.Length(); + + if (length1 > Settings.LinearSlop) + { + _u1 *= 1.0f / length1; + } + else + { + _u1.SetZero(); + } + + float C = _maxLength1 - length1; + linearError = Box2DXMath.Max(linearError, -C); + C = Box2DXMath.Clamp(C + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f); + float impulse = -_limitMass1 * C; + + Vec2 P1 = -impulse * _u1; + b1._sweep.C += b1._invMass * P1; + b1._sweep.A += b1._invI * Vec2.Cross(r1, P1); + + b1.SynchronizeTransform(); + } + + if (_limitState2 == LimitState.AtUpperLimit) + { + Vec2 r2 = Box2DXMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter()); + Vec2 p2 = b2._sweep.C + r2; + + _u2 = p2 - s2; + float length2 = _u2.Length(); + + if (length2 > Settings.LinearSlop) + { + _u2 *= 1.0f / length2; + } + else + { + _u2.SetZero(); + } + + float C = _maxLength2 - length2; + linearError = Box2DXMath.Max(linearError, -C); + C = Box2DXMath.Clamp(C + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f); + float impulse = -_limitMass2 * C; + + Vec2 P2 = -impulse * _u2; + b2._sweep.C += b2._invMass * P2; + b2._sweep.A += b2._invI * Vec2.Cross(r2, P2); + + b2.SynchronizeTransform(); + } + + return linearError < Settings.LinearSlop; + } + } +} diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Joints/PulleyJoint.cs.meta b/Box2d/Assets/Program/Box2d/Dynamics/Joints/PulleyJoint.cs.meta new file mode 100644 index 0000000..e028585 --- /dev/null +++ b/Box2d/Assets/Program/Box2d/Dynamics/Joints/PulleyJoint.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: b11c8edcbb41b7c4dba7334804b55d14 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Joints/RevoluteJoint.cs b/Box2d/Assets/Program/Box2d/Dynamics/Joints/RevoluteJoint.cs new file mode 100644 index 0000000..e201e01 --- /dev/null +++ b/Box2d/Assets/Program/Box2d/Dynamics/Joints/RevoluteJoint.cs @@ -0,0 +1,636 @@ +/* + Box2DX Copyright (c) 2008 Ihar Kalasouski http://code.google.com/p/box2dx + Box2D original C++ version Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this 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. +*/ + +// Point-to-point constraint +// C = p2 - p1 +// Cdot = v2 - v1 +// = v2 + cross(w2, r2) - v1 - cross(w1, r1) +// J = [-I -r1_skew I r2_skew ] +// Identity used: +// w k % (rx i + ry j) = w * (-ry i + rx j) + +// Motor constraint +// Cdot = w2 - w1 +// J = [0 0 -1 0 0 1] +// K = invI1 + invI2 + +using System; +using System.Collections.Generic; +using System.Text; + +using Box2DX.Common; + +namespace Box2DX.Dynamics +{ + using Box2DXMath = Box2DX.Common.Math; + using SystemMath = System.Math; + + /// <summary> + /// Revolute joint definition. This requires defining an + /// anchor point where the bodies are joined. The definition + /// uses local anchor points so that the initial configuration + /// can violate the constraint slightly. You also need to + /// specify the initial relative angle for joint limits. This + /// helps when saving and loading a game. + /// The local anchor points are measured from the body's origin + /// rather than the center of mass because: + /// 1. you might not know where the center of mass will be. + /// 2. if you add/remove shapes from a body and recompute the mass, + /// the joints will be broken. + /// </summary> + public class RevoluteJointDef : JointDef + { + public RevoluteJointDef() + { + Type = JointType.RevoluteJoint; + LocalAnchor1.Set(0.0f, 0.0f); + LocalAnchor2.Set(0.0f, 0.0f); + ReferenceAngle = 0.0f; + LowerAngle = 0.0f; + UpperAngle = 0.0f; + MaxMotorTorque = 0.0f; + MotorSpeed = 0.0f; + EnableLimit = false; + EnableMotor = false; + } + + /// <summary> + /// Initialize the bodies, anchors, and reference angle using the world + /// anchor. + /// </summary> + public void Initialize(Body body1, Body body2, Vec2 anchor) + { + Body1 = body1; + Body2 = body2; + LocalAnchor1 = body1.GetLocalPoint(anchor); + LocalAnchor2 = body2.GetLocalPoint(anchor); + ReferenceAngle = body2.GetAngle() - body1.GetAngle(); + } + + /// <summary> + /// The local anchor point relative to body1's origin. + /// </summary> + public Vec2 LocalAnchor1; + + /// <summary> + /// The local anchor point relative to body2's origin. + /// </summary> + public Vec2 LocalAnchor2; + + /// <summary> + /// The body2 angle minus body1 angle in the reference state (radians). + /// </summary> + public float ReferenceAngle; + + /// <summary> + /// A flag to enable joint limits. + /// </summary> + public bool EnableLimit; + + /// <summary> + /// The lower angle for the joint limit (radians). + /// </summary> + public float LowerAngle; + + /// <summary> + /// The upper angle for the joint limit (radians). + /// </summary> + public float UpperAngle; + + /// <summary> + /// A flag to enable the joint motor. + /// </summary> + public bool EnableMotor; + + /// <summary> + /// The desired motor speed. Usually in radians per second. + /// </summary> + public float MotorSpeed; + + /// <summary> + /// The maximum motor torque used to achieve the desired motor speed. + /// Usually in N-m. + /// </summary> + public float MaxMotorTorque; + } + + /// <summary> + /// A revolute joint constrains to bodies to share a common point while they + /// are free to rotate about the point. The relative rotation about the shared + /// point is the joint angle. You can limit the relative rotation with + /// a joint limit that specifies a lower and upper angle. You can use a motor + /// to drive the relative rotation about the shared point. A maximum motor torque + /// is provided so that infinite forces are not generated. + /// </summary> + public class RevoluteJoint : Joint + { + public Vec2 _localAnchor1; // relative + public Vec2 _localAnchor2; + public Vec3 _impulse; + public float _motorImpulse; + public Mat33 _mass; //effective mass for p2p constraint. + public float _motorMass; // effective mass for motor/limit angular constraint. + + public bool _enableMotor; + public float _maxMotorTorque; + public float _motorSpeed; + + public bool _enableLimit; + public float _referenceAngle; + public float _lowerAngle; + public float _upperAngle; + public LimitState _limitState; + + public override Vec2 Anchor1 + { + get { return _body1.GetWorldPoint(_localAnchor1); } + } + + public override Vec2 Anchor2 + { + get { return _body2.GetWorldPoint(_localAnchor2); } + } + + public override Vec2 GetReactionForce(float inv_dt) + { + Vec2 P = new Vec2(_impulse.X, _impulse.Y); + return inv_dt * P; + } + + public override float GetReactionTorque(float inv_dt) + { + return inv_dt * _impulse.Z; + } + + /// <summary> + /// Get the current joint angle in radians. + /// </summary> + public float JointAngle + { + get + { + Body b1 = _body1; + Body b2 = _body2; + return b2._sweep.A - b1._sweep.A - _referenceAngle; + } + } + + + /// <summary> + /// Get the current joint angle speed in radians per second. + /// </summary> + public float JointSpeed + { + get + { + Body b1 = _body1; + Body b2 = _body2; + return b2._angularVelocity - b1._angularVelocity; + } + } + + /// <summary> + /// Is the joint limit enabled? + /// </summary> + public bool IsLimitEnabled + { + get { return _enableLimit; } + } + + /// <summary> + /// Enable/disable the joint limit. + /// </summary> + public void EnableLimit(bool flag) + { + _body1.WakeUp(); + _body2.WakeUp(); + _enableLimit = flag; + } + + /// <summary> + /// Get the lower joint limit in radians. + /// </summary> + public float LowerLimit + { + get { return _lowerAngle; } + } + + /// <summary> + /// Get the upper joint limit in radians. + /// </summary> + public float UpperLimit + { + get { return _upperAngle; } + } + + /// <summary> + /// Set the joint limits in radians. + /// </summary> + public void SetLimits(float lower, float upper) + { + Box2DXDebug.Assert(lower <= upper); + _body1.WakeUp(); + _body2.WakeUp(); + _lowerAngle = lower; + _upperAngle = upper; + } + + /// <summary> + /// Is the joint motor enabled? + /// </summary> + public bool IsMotorEnabled + { + get { return _enableMotor; } + } + + /// <summary> + /// Enable/disable the joint motor. + /// </summary> + public void EnableMotor(bool flag) + { + _body1.WakeUp(); + _body2.WakeUp(); + _enableMotor = flag; + } + + /// <summary> + /// Get\Set the motor speed in radians per second. + /// </summary> + public float MotorSpeed + { + get { return _motorSpeed; } + set + { + _body1.WakeUp(); + _body2.WakeUp(); + _motorSpeed = value; + } + } + + /// <summary> + /// Set the maximum motor torque, usually in N-m. + /// </summary> + public void SetMaxMotorTorque(float torque) + { + _body1.WakeUp(); + _body2.WakeUp(); + _maxMotorTorque = torque; + } + + /// <summary> + /// Get the current motor torque, usually in N-m. + /// </summary> + public float MotorTorque + { + get { return _motorImpulse; } + } + + public RevoluteJoint(RevoluteJointDef def) + : base(def) + { + _localAnchor1 = def.LocalAnchor1; + _localAnchor2 = def.LocalAnchor2; + _referenceAngle = def.ReferenceAngle; + + _impulse = new Vec3(); + _motorImpulse = 0.0f; + + _lowerAngle = def.LowerAngle; + _upperAngle = def.UpperAngle; + _maxMotorTorque = def.MaxMotorTorque; + _motorSpeed = def.MotorSpeed; + _enableLimit = def.EnableLimit; + _enableMotor = def.EnableMotor; + _limitState = LimitState.InactiveLimit; + } + + internal override void InitVelocityConstraints(TimeStep step) + { + Body b1 = _body1; + Body b2 = _body2; + + if (_enableMotor || _enableLimit) + { + // You cannot create a rotation limit between bodies that + // both have fixed rotation. + Box2DXDebug.Assert(b1._invI > 0.0f || b2._invI > 0.0f); + } + + // Compute the effective mass matrix. + Vec2 r1 = Box2DXMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter()); + Vec2 r2 = Box2DXMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter()); + + // J = [-I -r1_skew I r2_skew] + // [ 0 -1 0 1] + // r_skew = [-ry; rx] + + // Matlab + // K = [ m1+r1y^2*i1+m2+r2y^2*i2, -r1y*i1*r1x-r2y*i2*r2x, -r1y*i1-r2y*i2] + // [ -r1y*i1*r1x-r2y*i2*r2x, m1+r1x^2*i1+m2+r2x^2*i2, r1x*i1+r2x*i2] + // [ -r1y*i1-r2y*i2, r1x*i1+r2x*i2, i1+i2] + + float m1 = b1._invMass, m2 = b2._invMass; + float i1 = b1._invI, i2 = b2._invI; + + _mass.Col1.X = m1 + m2 + r1.Y * r1.Y * i1 + r2.Y * r2.Y * i2; + _mass.Col2.X = -r1.Y * r1.X * i1 - r2.Y * r2.X * i2; + _mass.Col3.X = -r1.Y * i1 - r2.Y * i2; + _mass.Col1.Y = _mass.Col2.X; + _mass.Col2.Y = m1 + m2 + r1.X * r1.X * i1 + r2.X * r2.X * i2; + _mass.Col3.Y = r1.X * i1 + r2.X * i2; + _mass.Col1.Z = _mass.Col3.X; + _mass.Col2.Z = _mass.Col3.Y; + _mass.Col3.Z = i1 + i2; + + _motorMass = 1.0f / (i1 + i2); + + if (_enableMotor == false) + { + _motorImpulse = 0.0f; + } + + if (_enableLimit) + { + float jointAngle = b2._sweep.A - b1._sweep.A - _referenceAngle; + if (Box2DXMath.Abs(_upperAngle - _lowerAngle) < 2.0f * Settings.AngularSlop) + { + _limitState = LimitState.EqualLimits; + } + else if (jointAngle <= _lowerAngle) + { + if (_limitState != LimitState.AtLowerLimit) + { + _impulse.Z = 0.0f; + } + _limitState = LimitState.AtLowerLimit; + } + else if (jointAngle >= _upperAngle) + { + if (_limitState != LimitState.AtUpperLimit) + { + _impulse.Z = 0.0f; + } + _limitState = LimitState.AtUpperLimit; + } + else + { + _limitState = LimitState.InactiveLimit; + _impulse.Z = 0.0f; + } + } + else + { + _limitState = LimitState.InactiveLimit; + } + + if (step.WarmStarting) + { + // Scale impulses to support a variable time step. + _impulse *= step.DtRatio; + _motorImpulse *= step.DtRatio; + + Vec2 P = new Vec2(_impulse.X, _impulse.Y); + + b1._linearVelocity -= m1 * P; + b1._angularVelocity -= i1 * (Vec2.Cross(r1, P) + _motorImpulse + _impulse.Z); + + b2._linearVelocity += m2 * P; + b2._angularVelocity += i2 * (Vec2.Cross(r2, P) + _motorImpulse + _impulse.Z); + } + else + { + _impulse.SetZero(); + _motorImpulse = 0.0f; + } + } + + internal override void SolveVelocityConstraints(TimeStep step) + { + Body b1 = _body1; + Body b2 = _body2; + + Vec2 v1 = b1._linearVelocity; + float w1 = b1._angularVelocity; + Vec2 v2 = b2._linearVelocity; + float w2 = b2._angularVelocity; + + float m1 = b1._invMass, m2 = b2._invMass; + float i1 = b1._invI, i2 = b2._invI; + + //Solve motor constraint. + if (_enableMotor && _limitState != LimitState.EqualLimits) + { + float Cdot = w2 - w1 - _motorSpeed; + float impulse = _motorMass * (-Cdot); + float oldImpulse = _motorImpulse; + float maxImpulse = step.Dt * _maxMotorTorque; + _motorImpulse = Box2DXMath.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse); + impulse = _motorImpulse - oldImpulse; + + w1 -= i1 * impulse; + w2 += i2 * impulse; + } + + //Solve limit constraint. + if (_enableLimit && _limitState != LimitState.InactiveLimit) + { + Vec2 r1 = Box2DXMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter()); + Vec2 r2 = Box2DXMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter()); + + // Solve point-to-point constraint + Vec2 Cdot1 = v2 + Vec2.Cross(w2, r2) - v1 - Vec2.Cross(w1, r1); + float Cdot2 = w2 - w1; + Vec3 Cdot = new Vec3(Cdot1.X, Cdot1.Y, Cdot2); + + Vec3 impulse = _mass.Solve33(-Cdot); + + if (_limitState == LimitState.EqualLimits) + { + _impulse += impulse; + } + else if (_limitState == LimitState.AtLowerLimit) + { + float newImpulse = _impulse.Z + impulse.Z; + if (newImpulse < 0.0f) + { + Vec2 reduced = _mass.Solve22(-Cdot1); + impulse.X = reduced.X; + impulse.Y = reduced.Y; + impulse.Z = -_impulse.Z; + _impulse.X += reduced.X; + _impulse.Y += reduced.Y; + _impulse.Z = 0.0f; + } + } + else if (_limitState == LimitState.AtUpperLimit) + { + float newImpulse = _impulse.Z + impulse.Z; + if (newImpulse > 0.0f) + { + Vec2 reduced = _mass.Solve22(-Cdot1); + impulse.X = reduced.X; + impulse.Y = reduced.Y; + impulse.Z = -_impulse.Z; + _impulse.X += reduced.X; + _impulse.Y += reduced.Y; + _impulse.Z = 0.0f; + } + } + + Vec2 P = new Vec2(impulse.X, impulse.Y); + + v1 -= m1 * P; + w1 -= i1 * (Vec2.Cross(r1, P) + impulse.Z); + + v2 += m2 * P; + w2 += i2 * (Vec2.Cross(r2, P) + impulse.Z); + } + else + { + Vec2 r1 = Box2DXMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter()); + Vec2 r2 = Box2DXMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter()); + + // Solve point-to-point constraint + Vec2 Cdot = v2 + Vec2.Cross(w2, r2) - v1 - Vec2.Cross(w1, r1); + Vec2 impulse = _mass.Solve22(-Cdot); + + _impulse.X += impulse.X; + _impulse.Y += impulse.Y; + + v1 -= m1 * impulse; + w1 -= i1 * Vec2.Cross(r1, impulse); + + v2 += m2 * impulse; + w2 += i2 * Vec2.Cross(r2, impulse); + } + + b1._linearVelocity = v1; + b1._angularVelocity = w1; + b2._linearVelocity = v2; + b2._angularVelocity = w2; + } + + internal override bool SolvePositionConstraints(float baumgarte) + { + // TODO_ERIN block solve with limit. + + Body b1 = _body1; + Body b2 = _body2; + + float angularError = 0.0f; + float positionError = 0.0f; + + // Solve angular limit constraint. + if (_enableLimit && _limitState != LimitState.InactiveLimit) + { + float angle = b2._sweep.A - b1._sweep.A - _referenceAngle; + float limitImpulse = 0.0f; + + if (_limitState == LimitState.EqualLimits) + { + // Prevent large angular corrections + float C = Box2DXMath.Clamp(angle, -Settings.MaxAngularCorrection, Settings.MaxAngularCorrection); + limitImpulse = -_motorMass * C; + angularError = Box2DXMath.Abs(C); + } + else if (_limitState == LimitState.AtLowerLimit) + { + float C = angle - _lowerAngle; + angularError = -C; + + // Prevent large angular corrections and allow some slop. + C = Box2DXMath.Clamp(C + Settings.AngularSlop, -Settings.MaxAngularCorrection, 0.0f); + limitImpulse = -_motorMass * C; + } + else if (_limitState == LimitState.AtUpperLimit) + { + float C = angle - _upperAngle; + angularError = C; + + // Prevent large angular corrections and allow some slop. + C = Box2DXMath.Clamp(C - Settings.AngularSlop, 0.0f, Settings.MaxAngularCorrection); + limitImpulse = -_motorMass * C; + } + + b1._sweep.A -= b1._invI * limitImpulse; + b2._sweep.A += b2._invI * limitImpulse; + + b1.SynchronizeTransform(); + b2.SynchronizeTransform(); + } + + // Solve point-to-point constraint. + { + Vec2 r1 = Box2DXMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter()); + Vec2 r2 = Box2DXMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter()); + + Vec2 C = b2._sweep.C + r2 - b1._sweep.C - r1; + positionError = C.Length(); + + float invMass1 = b1._invMass, invMass2 = b2._invMass; + float invI1 = b1._invI, invI2 = b2._invI; + + // Handle large detachment. + float k_allowedStretch = 10.0f * Settings.LinearSlop; + if (C.LengthSquared() > k_allowedStretch * k_allowedStretch) + { + // Use a particle solution (no rotation). + Vec2 u = C; u.Normalize(); + float k = invMass1 + invMass2; + Box2DXDebug.Assert(k > Settings.FLT_EPSILON); + float m = 1.0f / k; + Vec2 impulse = m * (-C); + float k_beta = 0.5f; + b1._sweep.C -= k_beta * invMass1 * impulse; + b2._sweep.C += k_beta * invMass2 * impulse; + + C = b2._sweep.C + r2 - b1._sweep.C - r1; + } + + Mat22 K1 = new Mat22(); + K1.Col1.X = invMass1 + invMass2; K1.Col2.X = 0.0f; + K1.Col1.Y = 0.0f; K1.Col2.Y = invMass1 + invMass2; + + Mat22 K2 = new Mat22(); + K2.Col1.X = invI1 * r1.Y * r1.Y; K2.Col2.X = -invI1 * r1.X * r1.Y; + K2.Col1.Y = -invI1 * r1.X * r1.Y; K2.Col2.Y = invI1 * r1.X * r1.X; + + Mat22 K3 = new Mat22(); + K3.Col1.X = invI2 * r2.Y * r2.Y; K3.Col2.X = -invI2 * r2.X * r2.Y; + K3.Col1.Y = -invI2 * r2.X * r2.Y; K3.Col2.Y = invI2 * r2.X * r2.X; + + Mat22 K = K1 + K2 + K3; + Vec2 impulse_ = K.Solve(-C); + + b1._sweep.C -= b1._invMass * impulse_; + b1._sweep.A -= b1._invI * Vec2.Cross(r1, impulse_); + + b2._sweep.C += b2._invMass * impulse_; + b2._sweep.A += b2._invI * Vec2.Cross(r2, impulse_); + + b1.SynchronizeTransform(); + b2.SynchronizeTransform(); + } + + return positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop; + } + } +} diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Joints/RevoluteJoint.cs.meta b/Box2d/Assets/Program/Box2d/Dynamics/Joints/RevoluteJoint.cs.meta new file mode 100644 index 0000000..bbc1350 --- /dev/null +++ b/Box2d/Assets/Program/Box2d/Dynamics/Joints/RevoluteJoint.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: cb779b26e7eeeeb4f94a11a53b4f6eb2 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Box2d/Assets/Program/Box2d/Dynamics/World.cs b/Box2d/Assets/Program/Box2d/Dynamics/World.cs new file mode 100644 index 0000000..7ebc5eb --- /dev/null +++ b/Box2d/Assets/Program/Box2d/Dynamics/World.cs @@ -0,0 +1,1448 @@ +/* + Box2DX Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx + Box2D original C++ version Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this 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. +*/ + +using System; + +using Box2DX.Common; +using Box2DX.Collision; + +namespace Box2DX.Dynamics +{ + public struct TimeStep + { + public float Dt; // time step + public float Inv_Dt; // inverse time step (0 if dt == 0). + public float DtRatio; // dt * inv_dt0 + public int VelocityIterations; + public int PositionIterations; + public bool WarmStarting; + } + + /// <summary> + /// The world class manages all physics entities, dynamic simulation, + /// and asynchronous queries. + /// </summary> + public class World : IDisposable + { + internal bool _lock; + + internal BroadPhase _broadPhase; + private ContactManager _contactManager; + + private Body _bodyList; + private Joint _jointList; + private Controllers.Controller _controllerList; + + private Vec2 _raycastNormal; + private object _raycastUserData; + private Segment _raycastSegment; + private bool _raycastSolidShape; + + // Do not access + internal Contact _contactList; + + private int _bodyCount; + internal int _contactCount; + private int _jointCount; + private int _controllerCount; + + private Vec2 _gravity; + /// <summary> + /// Get\Set global gravity vector. + /// </summary> + public Vec2 Gravity { get { return _gravity; } set { _gravity = value; } } + + private bool _allowSleep; + + private Body _groundBody; + + private DestructionListener _destructionListener; + private BoundaryListener _boundaryListener; + internal ContactFilter _contactFilter; + internal ContactListener _contactListener; + private DebugDraw _debugDraw; + + // This is used to compute the time step ratio to + // support a variable time step. + private float _inv_dt0; + + // This is for debugging the solver. + private bool _warmStarting; + + // This is for debugging the solver. + private bool _continuousPhysics; + + /// <summary> + /// Construct a world object. + /// </summary> + /// <param name="worldAABB">A bounding box that completely encompasses all your shapes.</param> + /// <param name="gravity">The world gravity vector.</param> + /// <param name="doSleep">Improve performance by not simulating inactive bodies.</param> + public World(AABB worldAABB, Vec2 gravity, bool doSleep) + { + _destructionListener = null; + _boundaryListener = null; + _contactFilter = null; + _contactListener = null; + _debugDraw = null; + + _bodyList = null; + _contactList = null; + _jointList = null; + + _bodyCount = 0; + _contactCount = 0; + _jointCount = 0; + + _warmStarting = true; + _continuousPhysics = true; + + _allowSleep = doSleep; + _gravity = gravity; + + _lock = false; + + _inv_dt0 = 0.0f; + + _contactManager = new ContactManager(); + _contactManager._world = this; + _broadPhase = new BroadPhase(worldAABB, _contactManager); + + BodyDef bd = new BodyDef(); + _groundBody = CreateBody(bd); + } + + /// <summary> + /// Destruct the world. All physics entities are destroyed. + /// </summary> + public void Dispose() + { + DestroyBody(_groundBody); + if (_broadPhase is IDisposable) + (_broadPhase as IDisposable).Dispose(); + _broadPhase = null; + } + + /// <summary> + /// Register a destruction listener. + /// </summary> + /// <param name="listener"></param> + public void SetDestructionListener(DestructionListener listener) + { + _destructionListener = listener; + } + + /// <summary> + /// Register a broad-phase boundary listener. + /// </summary> + /// <param name="listener"></param> + public void SetBoundaryListener(BoundaryListener listener) + { + _boundaryListener = listener; + } + + /// <summary> + /// Register a contact filter to provide specific control over collision. + /// Otherwise the default filter is used (b2_defaultFilter). + /// </summary> + /// <param name="filter"></param> + public void SetContactFilter(ContactFilter filter) + { + _contactFilter = filter; + } + + /// <summary> + /// Register a contact event listener + /// </summary> + /// <param name="listener"></param> + public void SetContactListener(ContactListener listener) + { + _contactListener = listener; + } + + /// <summary> + /// Register a routine for debug drawing. The debug draw functions are called + /// inside the World.Step method, so make sure your renderer is ready to + /// consume draw commands when you call Step(). + /// </summary> + /// <param name="debugDraw"></param> + public void SetDebugDraw(DebugDraw debugDraw) + { + _debugDraw = debugDraw; + } + + /// <summary> + /// Create a rigid body given a definition. No reference to the definition + /// is retained. + /// @warning This function is locked during callbacks. + /// </summary> + /// <param name="def"></param> + /// <returns></returns> + public Body CreateBody(BodyDef def) + { + Box2DXDebug.Assert(_lock == false); + if (_lock == true) + { + return null; + } + + Body b = new Body(def, this); + + // Add to world doubly linked list. + b._prev = null; + b._next = _bodyList; + if (_bodyList != null) + { + _bodyList._prev = b; + } + _bodyList = b; + ++_bodyCount; + + return b; + } + + /// <summary> + /// Destroy a rigid body given a definition. No reference to the definition + /// is retained. This function is locked during callbacks. + /// @warning This automatically deletes all associated shapes and joints. + /// @warning This function is locked during callbacks. + /// </summary> + /// <param name="b"></param> + public void DestroyBody(Body b) + { + Box2DXDebug.Assert(_bodyCount > 0); + Box2DXDebug.Assert(_lock == false); + if (_lock == true) + { + return; + } + + // Delete the attached joints. + JointEdge jn = null; + if (b._jointList != null) + jn = b._jointList; + while (jn != null) + { + JointEdge jn0 = jn; + jn = jn.Next; + + if (_destructionListener != null) + { + _destructionListener.SayGoodbye(jn0.Joint); + } + + DestroyJoint(jn0.Joint); + } + + //Detach controllers attached to this body + Controllers.ControllerEdge ce = b._controllerList; + while (ce != null) + { + Controllers.ControllerEdge ce0 = ce; + ce = ce.nextController; + + ce0.controller.RemoveBody(b); + } + + // Delete the attached fixtures. This destroys broad-phase + // proxies and pairs, leading to the destruction of contacts. + Fixture f = b._fixtureList; + while (f != null) + { + Fixture f0 = f; + f = f.Next; + + if (_destructionListener != null) + { + _destructionListener.SayGoodbye(f0); + } + + f0.Destroy(_broadPhase); + } + + // Remove world body list. + if (b._prev != null) + { + b._prev._next = b._next; + } + + if (b._next != null) + { + b._next._prev = b._prev; + } + + if (b == _bodyList) + { + _bodyList = b._next; + } + + --_bodyCount; + if (b is IDisposable) + (b as IDisposable).Dispose(); + b = null; + } + + /// <summary> + /// Create a joint to constrain bodies together. No reference to the definition + /// is retained. This may cause the connected bodies to cease colliding. + /// @warning This function is locked during callbacks. + /// </summary> + /// <param name="def"></param> + /// <returns></returns> + public Joint CreateJoint(JointDef def) + { + Box2DXDebug.Assert(_lock == false); + + Joint j = Joint.Create(def); + + // Connect to the world list. + j._prev = null; + j._next = _jointList; + if (_jointList != null) + { + _jointList._prev = j; + } + _jointList = j; + ++_jointCount; + + // Connect to the bodies' doubly linked lists. + j._node1.Joint = j; + j._node1.Other = j._body2; + j._node1.Prev = null; + j._node1.Next = j._body1._jointList; + if (j._body1._jointList != null) + j._body1._jointList.Prev = j._node1; + j._body1._jointList = j._node1; + + j._node2.Joint = j; + j._node2.Other = j._body1; + j._node2.Prev = null; + j._node2.Next = j._body2._jointList; + if (j._body2._jointList != null) + j._body2._jointList.Prev = j._node2; + j._body2._jointList = j._node2; + + // If the joint prevents collisions, then reset collision filtering. + if (def.CollideConnected == false) + { + // Reset the proxies on the body with the minimum number of shapes. + Body b = def.Body1._fixtureCount < def.Body2._fixtureCount ? def.Body1 : def.Body2; + for (Fixture f = b._fixtureList; f != null; f = f.Next) + { + f.RefilterProxy(_broadPhase, b.GetXForm()); + } + } + + return j; + } + + /// <summary> + /// Destroy a joint. This may cause the connected bodies to begin colliding. + /// @warning This function is locked during callbacks. + /// </summary> + /// <param name="j"></param> + public void DestroyJoint(Joint j) + { + Box2DXDebug.Assert(_lock == false); + + bool collideConnected = j._collideConnected; + + // Remove from the doubly linked list. + if (j._prev != null) + { + j._prev._next = j._next; + } + + if (j._next != null) + { + j._next._prev = j._prev; + } + + if (j == _jointList) + { + _jointList = j._next; + } + + // Disconnect from island graph. + Body body1 = j._body1; + Body body2 = j._body2; + + // Wake up connected bodies. + body1.WakeUp(); + body2.WakeUp(); + + // Remove from body 1. + if (j._node1.Prev != null) + { + j._node1.Prev.Next = j._node1.Next; + } + + if (j._node1.Next != null) + { + j._node1.Next.Prev = j._node1.Prev; + } + + if (j._node1 == body1._jointList) + { + body1._jointList = j._node1.Next; + } + + j._node1.Prev = null; + j._node1.Next = null; + + // Remove from body 2 + if (j._node2.Prev != null) + { + j._node2.Prev.Next = j._node2.Next; + } + + if (j._node2.Next != null) + { + j._node2.Next.Prev = j._node2.Prev; + } + + if (j._node2 == body2._jointList) + { + body2._jointList = j._node2.Next; + } + + j._node2.Prev = null; + j._node2.Next = null; + + Joint.Destroy(j); + + Box2DXDebug.Assert(_jointCount > 0); + --_jointCount; + + // If the joint prevents collisions, then reset collision filtering. + if (collideConnected == false) + { + // Reset the proxies on the body with the minimum number of shapes. + Body b = body1._fixtureCount < body2._fixtureCount ? body1 : body2; + for (Fixture f = b._fixtureList; f != null; f = f.Next) + { + f.RefilterProxy(_broadPhase, b.GetXForm()); + } + } + } + + public Controllers.Controller AddController(Controllers.Controller def) + { + def._next = _controllerList; + def._prev = null; + if (_controllerList != null) + _controllerList._prev = def; + _controllerList = def; + ++_controllerCount; + + def._world = this; + + return def; + } + + public void RemoveController(Controllers.Controller controller) + { + Box2DXDebug.Assert(_controllerCount > 0); + if (controller._next != null) + controller._next._prev = controller._prev; + if (controller._prev != null) + controller._prev._next = controller._next; + if (controller == _controllerList) + _controllerList = controller._next; + --_controllerCount; + } + + /// <summary> + /// The world provides a single static ground body with no collision shapes. + /// You can use this to simplify the creation of joints and static shapes. + /// </summary> + /// <returns></returns> + public Body GetGroundBody() + { + return _groundBody; + } + + /// <summary> + /// Get the world body list. With the returned body, use Body.GetNext to get + /// the next body in the world list. A null body indicates the end of the list. + /// </summary> + /// <returns>The head of the world body list.</returns> + public Body GetBodyList() + { + return _bodyList; + } + + /// <summary> + /// Get the world joint list. With the returned joint, use Joint.GetNext to get + /// the next joint in the world list. A null joint indicates the end of the list. + /// </summary> + /// <returns>The head of the world joint list.</returns> + public Joint GetJointList() + { + return _jointList; + } + + public Controllers.Controller GetControllerList() + { + return _controllerList; + } + + public int GetControllerCount() + { + return _controllerCount; + } + + /// <summary> + /// Re-filter a fixture. This re-runs contact filtering on a fixture. + /// </summary> + public void Refilter(Fixture fixture) + { + Box2DXDebug.Assert(_lock == false); + fixture.RefilterProxy(_broadPhase, fixture.Body.GetXForm()); + } + + /// <summary> + /// Enable/disable warm starting. For testing. + /// </summary> + public void SetWarmStarting(bool flag) { _warmStarting = flag; } + + /// <summary> + /// Enable/disable continuous physics. For testing. + /// </summary> + public void SetContinuousPhysics(bool flag) { _continuousPhysics = flag; } + + /// <summary> + /// Perform validation of internal data structures. + /// </summary> + public void Validate() { _broadPhase.Validate(); } + + /// <summary> + /// Get the number of broad-phase proxies. + /// </summary> + public int GetProxyCount() { return _broadPhase._proxyCount; } + + /// <summary> + /// Get the number of broad-phase pairs. + /// </summary> + /// <returns></returns> + public int GetPairCount() { return _broadPhase._pairManager._pairCount; } + + /// <summary> + /// Get the number of bodies. + /// </summary> + /// <returns></returns> + public int GetBodyCount() { return _bodyCount; } + + /// <summary> + /// Get the number joints. + /// </summary> + /// <returns></returns> + public int GetJointCount() { return _jointCount; } + + /// <summary> + /// Get the number of contacts (each may have 0 or more contact points). + /// </summary> + /// <returns></returns> + public int GetContactCount() { return _contactCount; } + + /// <summary> + /// Take a time step. This performs collision detection, integration, + /// and constraint solution. + /// </summary> + /// <param name="dt">The amount of time to simulate, this should not vary.</param> + /// <param name="iterations">For the velocity constraint solver.</param> + /// <param name="iterations">For the positionconstraint solver.</param> + public void Step(float dt, int velocityIterations, int positionIteration) + { + _lock = true; + + TimeStep step = new TimeStep(); + step.Dt = dt; + step.VelocityIterations = velocityIterations; + step.PositionIterations = positionIteration; + if (dt > 0.0f) + { + step.Inv_Dt = 1.0f / dt; + } + else + { + step.Inv_Dt = 0.0f; + } + + step.DtRatio = _inv_dt0 * dt; + + step.WarmStarting = _warmStarting; + + // Update contacts. + _contactManager.Collide(); + + // Integrate velocities, solve velocity constraints, and integrate positions. + if (step.Dt > 0.0f) + { + Solve(step); + } + + // Handle TOI events. + if (_continuousPhysics && step.Dt > 0.0f) + { + SolveTOI(step); + } + + // Draw debug information. + DrawDebugData(); + + _inv_dt0 = step.Inv_Dt; + _lock = false; + } + + /// Query the world for all shapes that potentially overlap the + /// provided AABB. You provide a shape pointer buffer of specified + /// size. The number of shapes found is returned. + /// @param aabb the query box. + /// @param shapes a user allocated shape pointer array of size maxCount (or greater). + /// @param maxCount the capacity of the shapes array. + /// @return the number of shapes found in aabb. + public int Query(AABB aabb, Fixture[] fixtures, int maxCount) + { + //using (object[] results = new object[maxCount]) + { + object[] results = new object[maxCount]; + + int count = _broadPhase.Query(aabb, results, maxCount); + + for (int i = 0; i < count; ++i) + { + fixtures[i] = (Fixture)results[i]; + } + + results = null; + return count; + } + } + + /// <summary> + /// Query the world for all shapes that intersect a given segment. You provide a shap + /// pointer buffer of specified size. The number of shapes found is returned, and the buffer + /// is filled in order of intersection. + /// </summary> + /// <param name="segment">Defines the begin and end point of the ray cast, from p1 to p2. + /// Use Segment.Extend to create (semi-)infinite rays.</param> + /// <param name="shapes">A user allocated shape pointer array of size maxCount (or greater).</param> + /// <param name="maxCount">The capacity of the shapes array.</param> + /// <param name="solidShapes">Determines if shapes that the ray starts in are counted as hits.</param> + /// <param name="userData">Passed through the worlds contact filter, with method RayCollide. This can be used to filter valid shapes.</param> + /// <returns>The number of shapes found</returns> + public int Raycast(Segment segment, out Fixture[] fixtures, int maxCount, bool solidShapes, object userData) + { +#warning "PTR" + _raycastSegment = segment; + _raycastUserData = userData; + _raycastSolidShape = solidShapes; + + object[] results = new object[maxCount]; + fixtures = new Fixture[maxCount]; + int count = _broadPhase.QuerySegment(segment, results, maxCount, RaycastSortKey); + + for (int i = 0; i < count; ++i) + { + fixtures[i] = (Fixture)results[i]; + } + + return count; + } + + /// <summary> + /// Performs a raycast as with Raycast, finding the first intersecting shape. + /// </summary> + /// <param name="segment">Defines the begin and end point of the ray cast, from p1 to p2. + /// Use Segment.Extend to create (semi-)infinite rays.</param> + /// <param name="lambda">Returns the hit fraction. You can use this to compute the contact point + /// p = (1 - lambda) * segment.p1 + lambda * segment.p2.</param> + /// <param name="normal">Returns the normal at the contact point. If there is no intersection, the normal is not set.</param> + /// <param name="solidShapes">Determines if shapes that the ray starts in are counted as hits.</param> + /// <param name="userData"></param> + /// <returns>Returns the colliding shape shape, or null if not found.</returns> + public Fixture RaycastOne(Segment segment, out float lambda, out Vec2 normal, bool solidShapes, object userData) + { + int maxCount = 1; + Fixture[] fixture; + lambda = 0.0f; + normal = new Vec2(); + + int count = Raycast(segment, out fixture, maxCount, solidShapes, userData); + + if (count == 0) + return null; + + Box2DXDebug.Assert(count == 1); + + //Redundantly do TestSegment a second time, as the previous one's results are inaccessible + + fixture[0].TestSegment(out lambda, out normal, segment, 1); + //We already know it returns true + return fixture[0]; + } + + // Find islands, integrate and solve constraints, solve position constraints + private void Solve(TimeStep step) + { + // Step all controlls + for (Controllers.Controller controller = _controllerList; controller != null; controller = controller._next) + { + controller.Step(step); + } + + // Size the island for the worst case. + Island island = new Island(_bodyCount, _contactCount, _jointCount, _contactListener); + + // Clear all the island flags. + for (Body b = _bodyList; b != null; b = b._next) + { + b._flags &= ~Body.BodyFlags.Island; + } + for (Contact c = _contactList; c != null; c = c._next) + { + c._flags &= ~Contact.CollisionFlags.Island; + } + for (Joint j = _jointList; j != null; j = j._next) + { + j._islandFlag = false; + } + + // Build and simulate all awake islands. + int stackSize = _bodyCount; + { + Body[] stack = new Body[stackSize]; + + for (Body seed = _bodyList; seed != null; seed = seed._next) + { + if ((seed._flags & (Body.BodyFlags.Island | Body.BodyFlags.Sleep | Body.BodyFlags.Frozen)) != 0) + { + continue; + } + + if (seed.IsStatic()) + { + continue; + } + + // Reset island and stack. + island.Clear(); + int stackCount = 0; + stack[stackCount++] = seed; + seed._flags |= Body.BodyFlags.Island; + + // Perform a depth first search (DFS) on the constraint graph. + while (stackCount > 0) + { + // Grab the next body off the stack and add it to the island. + Body b = stack[--stackCount]; + island.Add(b); + + // Make sure the body is awake. + b._flags &= ~Body.BodyFlags.Sleep; + + // To keep islands as small as possible, we don't + // propagate islands across static bodies. + if (b.IsStatic()) + { + continue; + } + + // Search all contacts connected to this body. + for (ContactEdge cn = b._contactList; cn != null; cn = cn.Next) + { + // Has this contact already been added to an island? + if ((cn.Contact._flags & (Contact.CollisionFlags.Island | Contact.CollisionFlags.NonSolid)) != 0) + { + continue; + } + + // Is this contact touching? + if ((cn.Contact._flags & Contact.CollisionFlags.Touch) == (Contact.CollisionFlags)0) + { + continue; + } + + island.Add(cn.Contact); + cn.Contact._flags |= Contact.CollisionFlags.Island; + + Body other = cn.Other; + + // Was the other body already added to this island? + if ((other._flags & Body.BodyFlags.Island) != 0) + { + continue; + } + + Box2DXDebug.Assert(stackCount < stackSize); + stack[stackCount++] = other; + other._flags |= Body.BodyFlags.Island; + } + + // Search all joints connect to this body. + for (JointEdge jn = b._jointList; jn != null; jn = jn.Next) + { + if (jn.Joint._islandFlag == true) + { + continue; + } + + island.Add(jn.Joint); + jn.Joint._islandFlag = true; + + Body other = jn.Other; + if ((other._flags & Body.BodyFlags.Island) != 0) + { + continue; + } + + Box2DXDebug.Assert(stackCount < stackSize); + stack[stackCount++] = other; + other._flags |= Body.BodyFlags.Island; + } + } + + island.Solve(step, _gravity, _allowSleep); + + // Post solve cleanup. + for (int i = 0; i < island._bodyCount; ++i) + { + // Allow static bodies to participate in other islands. + Body b = island._bodies[i]; + if (b.IsStatic()) + { + b._flags &= ~Body.BodyFlags.Island; + } + } + } + + stack = null; + } + + // Synchronize shapes, check for out of range bodies. + for (Body b = _bodyList; b != null; b = b.GetNext()) + { + if ((b._flags & (Body.BodyFlags.Sleep | Body.BodyFlags.Frozen)) != 0) + { + continue; + } + + if (b.IsStatic()) + { + continue; + } + + // Update shapes (for broad-phase). If the shapes go out of + // the world AABB then shapes and contacts may be destroyed, + // including contacts that are + bool inRange = b.SynchronizeFixtures(); + + // Did the body's shapes leave the world? + if (inRange == false && _boundaryListener != null) + { + _boundaryListener.Violation(b); + } + } + + // Commit shape proxy movements to the broad-phase so that new contacts are created. + // Also, some contacts can be destroyed. + _broadPhase.Commit(); + } + + // Find TOI contacts and solve them. + private void SolveTOI(TimeStep step) + { + // Reserve an island and a queue for TOI island solution. + Island island = new Island(_bodyCount, Settings.MaxTOIContactsPerIsland, Settings.MaxTOIJointsPerIsland, _contactListener); + + //Simple one pass queue + //Relies on the fact that we're only making one pass + //through and each body can only be pushed/popped once. + //To push: + // queue[queueStart+queueSize++] = newElement; + //To pop: + // poppedElement = queue[queueStart++]; + // --queueSize; + int queueCapacity = _bodyCount; + Body[] queue = new Body[queueCapacity]; + + for (Body b = _bodyList; b != null; b = b._next) + { + b._flags &= ~Body.BodyFlags.Island; + b._sweep.T0 = 0.0f; + } + + for (Contact c = _contactList; c != null; c = c._next) + { + // Invalidate TOI + c._flags &= ~(Contact.CollisionFlags.Toi | Contact.CollisionFlags.Island); + } + + for (Joint j = _jointList; j != null; j = j._next) + { + j._islandFlag = false; + } + + // Find TOI events and solve them. + for (; ; ) + { + // Find the first TOI. + Contact minContact = null; + float minTOI = 1.0f; + + for (Contact c = _contactList; c != null; c = c._next) + { + if ((int)(c._flags & (Contact.CollisionFlags.Slow | Contact.CollisionFlags.NonSolid)) == 1) + { + continue; + } + + // TODO_ERIN keep a counter on the contact, only respond to M TOIs per contact. + + float toi = 1.0f; + if ((int)(c._flags & Contact.CollisionFlags.Toi) == 1) + { + // This contact has a valid cached TOI. + toi = c._toi; + } + else + { + // Compute the TOI for this contact. + Fixture s1 = c.FixtureA; + Fixture s2 = c.FixtureB; + Body b1 = s1.Body; + Body b2 = s2.Body; + + if ((b1.IsStatic() || b1.IsSleeping()) && (b2.IsStatic() || b2.IsSleeping())) + { + continue; + } + + // Put the sweeps onto the same time interval. + float t0 = b1._sweep.T0; + + if (b1._sweep.T0 < b2._sweep.T0) + { + t0 = b2._sweep.T0; + b1._sweep.Advance(t0); + } + else if (b2._sweep.T0 < b1._sweep.T0) + { + t0 = b1._sweep.T0; + b2._sweep.Advance(t0); + } + + Box2DXDebug.Assert(t0 < 1.0f); + + // Compute the time of impact. + toi = c.ComputeTOI(b1._sweep, b2._sweep); + //b2TimeOfImpact(c->m_fixtureA->GetShape(), b1->m_sweep, c->m_fixtureB->GetShape(), b2->m_sweep); + + Box2DXDebug.Assert(0.0f <= toi && toi <= 1.0f); + + // If the TOI is in range ... + if (0.0f < toi && toi < 1.0f) + { + // Interpolate on the actual range. + toi = Common.Math.Min((1.0f - toi) * t0 + toi, 1.0f); + } + + + c._toi = toi; + c._flags |= Contact.CollisionFlags.Toi; + } + + if (Settings.FLT_EPSILON < toi && toi < minTOI) + { + // This is the minimum TOI found so far. + minContact = c; + minTOI = toi; + } + } + + if (minContact == null || 1.0f - 100.0f * Settings.FLT_EPSILON < minTOI) + { + // No more TOI events. Done! + break; + } + + // Advance the bodies to the TOI. + Fixture f1 = minContact.FixtureA; + Fixture f2 = minContact.FixtureB; + Body b3 = f1.Body; + Body b4 = f2.Body; + b3.Advance(minTOI); + b4.Advance(minTOI); + + // The TOI contact likely has some new contact points. + minContact.Update(_contactListener); + minContact._flags &= ~Contact.CollisionFlags.Toi; + + if ((minContact._flags & Contact.CollisionFlags.Touch) == 0) + { + // This shouldn't happen. Numerical error? + //b2Assert(false); + continue; + } + + // Build the TOI island. We need a dynamic seed. + Body seed = b3; + if (seed.IsStatic()) + { + seed = b4; + } + + // Reset island and queue. + island.Clear(); + + int queueStart = 0; // starting index for queue + int queueSize = 0; // elements in queue + queue[queueStart + queueSize++] = seed; + seed._flags |= Body.BodyFlags.Island; + + // Perform a breadth first search (BFS) on the contact/joint graph. + while (queueSize > 0) + { + // Grab the next body off the stack and add it to the island. + Body b = queue[queueStart++]; + --queueSize; + + island.Add(b); + + // Make sure the body is awake. + b._flags &= ~Body.BodyFlags.Sleep; + + // To keep islands as small as possible, we don't + // propagate islands across static bodies. + if (b.IsStatic()) + { + continue; + } + + // Search all contacts connected to this body. + for (ContactEdge cEdge = b._contactList; cEdge != null; cEdge = cEdge.Next) + { + // Does the TOI island still have space for contacts? + if (island._contactCount == island._contactCapacity) + { + continue; + } + + // Has this contact already been added to an island? Skip slow or non-solid contacts. + if ((int)(cEdge.Contact._flags & (Contact.CollisionFlags.Island | Contact.CollisionFlags.Slow | Contact.CollisionFlags.NonSolid)) != 0) + { + continue; + } + + // Is this contact touching? For performance we are not updating this contact. + if ((cEdge.Contact._flags & Contact.CollisionFlags.Touch) == 0) + { + continue; + } + + island.Add(cEdge.Contact); + cEdge.Contact._flags |= Contact.CollisionFlags.Island; + + // Update other body. + Body other = cEdge.Other; + + // Was the other body already added to this island? + if ((int)(other._flags & Body.BodyFlags.Island) == 1) + { + continue; + } + + // March forward, this can do no harm since this is the min TOI. + if (other.IsStatic() == false) + { + other.Advance(minTOI); + other.WakeUp(); + } + + //Box2DXDebug.Assert(queueStart + queueSize < queueCapacity); + queue[queueStart + queueSize] = other; + ++queueSize; + other._flags |= Body.BodyFlags.Island; + } + + for (JointEdge jEdge = b._jointList; jEdge != null; jEdge = jEdge.Next) + { + if (island._jointCount == island._jointCapacity) + { + continue; + } + + if (jEdge.Joint._islandFlag == true) + { + continue; + } + + island.Add(jEdge.Joint); + + jEdge.Joint._islandFlag = true; + + Body other = jEdge.Other; + + if ((int)(other._flags & Body.BodyFlags.Island) == 1) + { + continue; + } + + if (!other.IsStatic()) + { + other.Advance(minTOI); + other.WakeUp(); + } + + //Box2DXDebug.Assert(queueStart + queueSize < queueCapacity); + queue[queueStart + queueSize] = other; + ++queueSize; + other._flags |= Body.BodyFlags.Island; + } + } + + TimeStep subStep; + subStep.WarmStarting = false; + subStep.Dt = (1.0f - minTOI) * step.Dt; + subStep.Inv_Dt = 1.0f / subStep.Dt; + subStep.DtRatio = 0.0f; + subStep.VelocityIterations = step.VelocityIterations; + subStep.PositionIterations = step.PositionIterations; + + island.SolveTOI(ref subStep); + + // Post solve cleanup. + for (int i = 0; i < island._bodyCount; ++i) + { + // Allow bodies to participate in future TOI islands. + Body b = island._bodies[i]; + b._flags &= ~Body.BodyFlags.Island; + + if ((int)(b._flags & (Body.BodyFlags.Sleep | Body.BodyFlags.Frozen)) == 1) + { + continue; + } + + if (b.IsStatic()) + { + continue; + } + + // Update fixtures (for broad-phase). If the fixtures go out of + // the world AABB then fixtures and contacts may be destroyed, + // including contacts that are + bool inRange = b.SynchronizeFixtures(); + + // Did the body's fixtures leave the world? + if (inRange == false && _boundaryListener != null) + { + _boundaryListener.Violation(b); + } + + // Invalidate all contact TOIs associated with this body. Some of these + // may not be in the island because they were not touching. + for (ContactEdge cn = b._contactList; cn != null; cn = cn.Next) + { + cn.Contact._flags &= ~Contact.CollisionFlags.Toi; + } + } + + for (int i = 0; i < island._contactCount; ++i) + { + // Allow contacts to participate in future TOI islands. + Contact c = island._contacts[i]; + c._flags &= ~(Contact.CollisionFlags.Toi | Contact.CollisionFlags.Island); + } + + for (int i = 0; i < island._jointCount; ++i) + { + // Allow joints to participate in future TOI islands. + Joint j = island._joints[i]; + j._islandFlag = false; + } + + // Commit fixture proxy movements to the broad-phase so that new contacts are created. + // Also, some contacts can be destroyed. + _broadPhase.Commit(); + } + + queue = null; + } + + private void DrawJoint(Joint joint) + { + Body b1 = joint.GetBody1(); + Body b2 = joint.GetBody2(); + XForm xf1 = b1.GetXForm(); + XForm xf2 = b2.GetXForm(); + Vec2 x1 = xf1.Position; + Vec2 x2 = xf2.Position; + Vec2 p1 = joint.Anchor1; + Vec2 p2 = joint.Anchor2; + + Color color = new Color(0.5f, 0.8f, 0.8f); + + switch (joint.GetType()) + { + case JointType.DistanceJoint: + _debugDraw.DrawSegment(p1, p2, color); + break; + + case JointType.PulleyJoint: + { + PulleyJoint pulley = (PulleyJoint)joint; + Vec2 s1 = pulley.GroundAnchor1; + Vec2 s2 = pulley.GroundAnchor2; + _debugDraw.DrawSegment(s1, p1, color); + _debugDraw.DrawSegment(s2, p2, color); + _debugDraw.DrawSegment(s1, s2, color); + } + break; + + case JointType.MouseJoint: + // don't draw this + break; + + default: + _debugDraw.DrawSegment(x1, p1, color); + _debugDraw.DrawSegment(p1, p2, color); + _debugDraw.DrawSegment(x2, p2, color); + break; + } + } + + private void DrawFixture(Fixture fixture, XForm xf, Color color, bool core) + { +#warning "the core argument is not used, the coreColor variable is also not used" + Color coreColor = new Color(0.9f, 0.6f, 0.6f); + + switch (fixture.ShapeType) + { + case ShapeType.CircleShape: + { + CircleShape circle = (CircleShape)fixture.Shape; + + Vec2 center = Common.Math.Mul(xf, circle._position); + float radius = circle._radius; + Vec2 axis = xf.R.Col1; + + _debugDraw.DrawSolidCircle(center, radius, axis, color); + } + break; + + case ShapeType.PolygonShape: + { + PolygonShape poly = (PolygonShape)fixture.Shape; + int vertexCount = poly._vertexCount; + Vec2[] localVertices = poly._vertices; + + Box2DXDebug.Assert(vertexCount <= Settings.MaxPolygonVertices); + Vec2[] vertices = new Vec2[Settings.MaxPolygonVertices]; + + for (int i = 0; i < vertexCount; ++i) + { + vertices[i] = Common.Math.Mul(xf, localVertices[i]); + } + + _debugDraw.DrawSolidPolygon(vertices, vertexCount, color); + } + break; + + case ShapeType.EdgeShape: + { + EdgeShape edge = (EdgeShape)fixture.Shape; + + _debugDraw.DrawSegment(Common.Math.Mul(xf, edge.Vertex1), Common.Math.Mul(xf, edge.Vertex2), color); + } + break; + } + } + + private void DrawDebugData() + { + if (_debugDraw == null) + { + return; + } + + DebugDraw.DrawFlags flags = _debugDraw.Flags; + + if ((flags & DebugDraw.DrawFlags.Shape) != 0) + { + bool core = (flags & DebugDraw.DrawFlags.CoreShape) == DebugDraw.DrawFlags.CoreShape; + + for (Body b = _bodyList; b != null; b = b.GetNext()) + { + XForm xf = b.GetXForm(); + for (Fixture f = b.GetFixtureList(); f != null; f = f.Next) + { + if (b.IsStatic()) + { + DrawFixture(f, xf, new Color(0.5f, 0.9f, 0.5f), core); + } + else if (b.IsSleeping()) + { + DrawFixture(f, xf, new Color(0.5f, 0.5f, 0.9f), core); + } + else + { + DrawFixture(f, xf, new Color(0.9f, 0.9f, 0.9f), core); + } + } + } + } + + if ((flags & DebugDraw.DrawFlags.Joint) != 0) + { + for (Joint j = _jointList; j != null; j = j.GetNext()) + { + if (j.GetType() != JointType.MouseJoint) + { + DrawJoint(j); + } + } + } + + if ((flags & DebugDraw.DrawFlags.Controller) != 0) + { + for (Controllers.Controller c = _controllerList; c != null; c = c.GetNext()) + { + c.Draw(_debugDraw); + } + } + + if ((flags & DebugDraw.DrawFlags.Pair) != 0) + { + BroadPhase bp = _broadPhase; + Vec2 invQ = new Vec2(); + invQ.Set(1.0f / bp._quantizationFactor.X, 1.0f / bp._quantizationFactor.Y); + Color color = new Color(0.9f, 0.9f, 0.3f); + + for (int i = 0; i < PairManager.TableCapacity; ++i) + { + ushort index = bp._pairManager._hashTable[i]; + while (index != PairManager.NullPair) + { + Pair pair = bp._pairManager._pairs[index]; + Proxy p1 = bp._proxyPool[pair.ProxyId1]; + Proxy p2 = bp._proxyPool[pair.ProxyId2]; + + AABB b1 = new AABB(), b2 = new AABB(); + b1.LowerBound.X = bp._worldAABB.LowerBound.X + invQ.X * bp._bounds[0][p1.LowerBounds[0]].Value; + b1.LowerBound.Y = bp._worldAABB.LowerBound.Y + invQ.Y * bp._bounds[1][p1.LowerBounds[1]].Value; + b1.UpperBound.X = bp._worldAABB.LowerBound.X + invQ.X * bp._bounds[0][p1.UpperBounds[0]].Value; + b1.UpperBound.Y = bp._worldAABB.LowerBound.Y + invQ.Y * bp._bounds[1][p1.UpperBounds[1]].Value; + b2.LowerBound.X = bp._worldAABB.LowerBound.X + invQ.X * bp._bounds[0][p2.LowerBounds[0]].Value; + b2.LowerBound.Y = bp._worldAABB.LowerBound.Y + invQ.Y * bp._bounds[1][p2.LowerBounds[1]].Value; + b2.UpperBound.X = bp._worldAABB.LowerBound.X + invQ.X * bp._bounds[0][p2.UpperBounds[0]].Value; + b2.UpperBound.Y = bp._worldAABB.LowerBound.Y + invQ.Y * bp._bounds[1][p2.UpperBounds[1]].Value; + + Vec2 x1 = 0.5f * (b1.LowerBound + b1.UpperBound); + Vec2 x2 = 0.5f * (b2.LowerBound + b2.UpperBound); + + _debugDraw.DrawSegment(x1, x2, color); + + index = pair.Next; + } + } + } + + if ((flags & DebugDraw.DrawFlags.Aabb) != 0) + { + BroadPhase bp = _broadPhase; + Vec2 worldLower = bp._worldAABB.LowerBound; + Vec2 worldUpper = bp._worldAABB.UpperBound; + + Vec2 invQ = new Vec2(); + invQ.Set(1.0f / bp._quantizationFactor.X, 1.0f / bp._quantizationFactor.Y); + Color color = new Color(0.9f, 0.3f, 0.9f); + for (int i = 0; i < Settings.MaxProxies; ++i) + { + Proxy p = bp._proxyPool[i]; + if (p.IsValid == false) + { + continue; + } + + AABB b = new AABB(); + b.LowerBound.X = worldLower.X + invQ.X * bp._bounds[0][p.LowerBounds[0]].Value; + b.LowerBound.Y = worldLower.Y + invQ.Y * bp._bounds[1][p.LowerBounds[1]].Value; + b.UpperBound.X = worldLower.X + invQ.X * bp._bounds[0][p.UpperBounds[0]].Value; + b.UpperBound.Y = worldLower.Y + invQ.Y * bp._bounds[1][p.UpperBounds[1]].Value; + + Vec2[] vs1 = new Vec2[4]; + vs1[0].Set(b.LowerBound.X, b.LowerBound.Y); + vs1[1].Set(b.UpperBound.X, b.LowerBound.Y); + vs1[2].Set(b.UpperBound.X, b.UpperBound.Y); + vs1[3].Set(b.LowerBound.X, b.UpperBound.Y); + + _debugDraw.DrawPolygon(vs1, 4, color); + } + + Vec2[] vs = new Vec2[4]; + vs[0].Set(worldLower.X, worldLower.Y); + vs[1].Set(worldUpper.X, worldLower.Y); + vs[2].Set(worldUpper.X, worldUpper.Y); + vs[3].Set(worldLower.X, worldUpper.Y); + _debugDraw.DrawPolygon(vs, 4, new Color(0.3f, 0.9f, 0.9f)); + } + + if ((flags & DebugDraw.DrawFlags.CenterOfMass) != 0) + { + for (Body b = _bodyList; b != null; b = b.GetNext()) + { + XForm xf = b.GetXForm(); + xf.Position = b.GetWorldCenter(); + _debugDraw.DrawXForm(xf); + } + } + } + + //Is it safe to pass private static function pointers? + private static float RaycastSortKey(object data) + { + Fixture fixture = data as Fixture; + Box2DXDebug.Assert(fixture != null); + Body body = fixture.Body; + World world = body.GetWorld(); + + if (world._contactFilter != null && !world._contactFilter.RayCollide(world._raycastUserData, fixture)) + return -1; + + float lambda; + + SegmentCollide collide = fixture.TestSegment(out lambda, out world._raycastNormal, world._raycastSegment, 1); + + if (world._raycastSolidShape && collide == SegmentCollide.MissCollide) + return -1; + if (!world._raycastSolidShape && collide != SegmentCollide.HitCollide) + return -1; + + return lambda; + } + + public bool InRange(AABB aabb) + { + return _broadPhase.InRange(aabb); + } + } +} diff --git a/Box2d/Assets/Program/Box2d/Dynamics/World.cs.meta b/Box2d/Assets/Program/Box2d/Dynamics/World.cs.meta new file mode 100644 index 0000000..886be35 --- /dev/null +++ b/Box2d/Assets/Program/Box2d/Dynamics/World.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 23b24b91d3d5f4142af072a2f30d996d +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Box2d/Assets/Program/Box2d/Dynamics/WorldCallbacks.cs b/Box2d/Assets/Program/Box2d/Dynamics/WorldCallbacks.cs new file mode 100644 index 0000000..4f7909b --- /dev/null +++ b/Box2d/Assets/Program/Box2d/Dynamics/WorldCallbacks.cs @@ -0,0 +1,240 @@ +/* + Box2DX Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx + Box2D original C++ version Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this 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. +*/ + +using System; + +using Box2DX.Common; +using Box2DX.Collision; + +namespace Box2DX.Dynamics +{ + /// <summary> + /// Joints and shapes are destroyed when their associated + /// body is destroyed. Implement this listener so that you + /// may nullify references to these joints and shapes. + /// </summary> + public abstract class DestructionListener + { + /// <summary> + /// Called when any joint is about to be destroyed due + /// to the destruction of one of its attached bodies. + /// </summary> + public abstract void SayGoodbye(Joint joint); + + /// <summary> + /// Called when any shape is about to be destroyed due + /// to the destruction of its parent body. + /// </summary> + public abstract void SayGoodbye(Fixture fixture); + } + + /// <summary> + /// This is called when a body's shape passes outside of the world boundary. + /// </summary> + public abstract class BoundaryListener + { + /// <summary> + /// This is called for each body that leaves the world boundary. + /// @warning you can't modify the world inside this callback. + /// </summary> + public abstract void Violation(Body body); + } + + /// <summary> + /// Implement this class to provide collision filtering. In other words, you can implement + /// this class if you want finer control over contact creation. + /// </summary> + public class ContactFilter + { + /// <summary> + /// Return true if contact calculations should be performed between these two shapes. + /// If you implement your own collision filter you may want to build from this implementation. + /// @warning for performance reasons this is only called when the AABBs begin to overlap. + /// </summary> + public virtual bool ShouldCollide(Fixture fixtureA, Fixture fixtureB) + { + FilterData filterA = fixtureA.Filter; + FilterData filterB = fixtureB.Filter; + + if (filterA.GroupIndex == filterB.GroupIndex && filterA.GroupIndex != 0) + { + return filterA.GroupIndex > 0; + } + + bool collide = (filterA.MaskBits & filterB.CategoryBits) != 0 && (filterA.CategoryBits & filterB.MaskBits) != 0; + return collide; + } + + /// <summary> + /// Return true if the given shape should be considered for ray intersection. + /// </summary> + public bool RayCollide(object userData, Fixture fixture) + { + //By default, cast userData as a shape, and then collide if the shapes would collide + if (userData == null) + { + return true; + } + + return ShouldCollide((Fixture)userData, fixture); + } + } + + /// Contact impulses for reporting. Impulses are used instead of forces because + /// sub-step forces may approach infinity for rigid body collisions. These + /// match up one-to-one with the contact points in b2Manifold. + public class ContactImpulse + { + public float[] normalImpulses = new float[Settings.MaxManifoldPoints]; + public float[] tangentImpulses = new float[Settings.MaxManifoldPoints]; + } + + /// Implement this class to get contact information. You can use these results for + /// things like sounds and game logic. You can also get contact results by + /// traversing the contact lists after the time step. However, you might miss + /// some contacts because continuous physics leads to sub-stepping. + /// Additionally you may receive multiple callbacks for the same contact in a + /// single time step. + /// You should strive to make your callbacks efficient because there may be + /// many callbacks per time step. + /// @warning You cannot create/destroy Box2DX entities inside these callbacks. + public interface ContactListener + { + /// Called when two fixtures begin to touch. + void BeginContact(Contact contact); + + /// Called when two fixtures cease to touch. + void EndContact(Contact contact); + + /// This is called after a contact is updated. This allows you to inspect a + /// contact before it goes to the solver. If you are careful, you can modify the + /// contact manifold (e.g. disable contact). + /// A copy of the old manifold is provided so that you can detect changes. + /// Note: this is called only for awake bodies. + /// Note: this is called even when the number of contact points is zero. + /// Note: this is not called for sensors. + /// Note: if you set the number of contact points to zero, you will not + /// get an EndContact callback. However, you may get a BeginContact callback + /// the next step. + void PreSolve(Contact contact, Manifold oldManifold); + + /// This lets you inspect a contact after the solver is finished. This is useful + /// for inspecting impulses. + /// Note: the contact manifold does not include time of impact impulses, which can be + /// arbitrarily large if the sub-step is small. Hence the impulse is provided explicitly + /// in a separate data structure. + /// Note: this is only called for contacts that are touching, solid, and awake. + void PostSolve(Contact contact, ContactImpulse impulse); + } + + /// <summary> + /// Color for debug drawing. Each value has the range [0,1]. + /// </summary> + public struct Color + { + public float R, G, B; + + public Color(float r, float g, float b) + { + R = r; G = g; B = b; + } + public void Set(float r, float g, float b) + { + R = r; G = g; B = b; + } + } + + /// <summary> + /// Implement and register this class with a b2World to provide debug drawing of physics + /// entities in your game. + /// </summary> + public abstract class DebugDraw + { + [Flags] + public enum DrawFlags + { + Shape = 0x0001, // draw shapes + Joint = 0x0002, // draw joint connections + CoreShape = 0x0004, // draw core (TOI) shapes // should be removed in this revision? + Aabb = 0x0008, // draw axis aligned bounding boxes + Obb = 0x0010, // draw oriented bounding boxes // should be removed in this revision? + Pair = 0x0020, // draw broad-phase pairs + CenterOfMass = 0x0040, // draw center of mass frame + Controller = 0x0080 // draw center of mass frame + }; + + protected DrawFlags _drawFlags; + + public DebugDraw() + { + _drawFlags = 0; + } + + public DrawFlags Flags { get { return _drawFlags; } set { _drawFlags = value; } } + + /// <summary> + /// Append flags to the current flags. + /// </summary> + public void AppendFlags(DrawFlags flags) + { + _drawFlags |= flags; + } + + /// <summary> + /// Clear flags from the current flags. + /// </summary> + public void ClearFlags(DrawFlags flags) + { + _drawFlags &= ~flags; + } + + /// <summary> + /// Draw a closed polygon provided in CCW order. + /// </summary> + public abstract void DrawPolygon(Vec2[] vertices, int vertexCount, Color color); + + /// <summary> + /// Draw a solid closed polygon provided in CCW order. + /// </summary> + public abstract void DrawSolidPolygon(Vec2[] vertices, int vertexCount, Color color); + + /// <summary> + /// Draw a circle. + /// </summary> + public abstract void DrawCircle(Vec2 center, float radius, Color color); + + /// <summary> + /// Draw a solid circle. + /// </summary> + public abstract void DrawSolidCircle(Vec2 center, float radius, Vec2 axis, Color color); + + /// <summary> + /// Draw a line segment. + /// </summary> + public abstract void DrawSegment(Vec2 p1, Vec2 p2, Color color); + + /// <summary> + /// Draw a transform. Choose your own length scale. + /// </summary> + /// <param name="xf">A transform.</param> + public abstract void DrawXForm(XForm xf); + } +} diff --git a/Box2d/Assets/Program/Box2d/Dynamics/WorldCallbacks.cs.meta b/Box2d/Assets/Program/Box2d/Dynamics/WorldCallbacks.cs.meta new file mode 100644 index 0000000..ff176d9 --- /dev/null +++ b/Box2d/Assets/Program/Box2d/Dynamics/WorldCallbacks.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 4e6c1ea8b531e954493f5e6f0509a0ed +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Box2d/Assets/Program/Box2d/Rope/Rope.cs b/Box2d/Assets/Program/Box2d/Rope/Rope.cs deleted file mode 100644 index 85ae672..0000000 --- a/Box2d/Assets/Program/Box2d/Rope/Rope.cs +++ /dev/null @@ -1,7 +0,0 @@ -using System.Collections; -using System.Collections.Generic; - -namespace Box2D -{ - -} |