├── .gitignore ├── License.txt ├── Package.swift ├── Readme.md ├── Sources └── Box2D │ ├── Box2D.h │ ├── Collision │ ├── Shapes │ │ ├── b2ChainShape.swift │ │ ├── b2CircleShape.swift │ │ ├── b2EdgeShape.swift │ │ ├── b2PolygonShape.swift │ │ └── b2Shape.swift │ ├── b2BroadPhase.swift │ ├── b2CollideCircles.swift │ ├── b2CollideEdge.swift │ ├── b2CollidePolygon.swift │ ├── b2Collision.swift │ ├── b2Distance.swift │ ├── b2DynamicTree.swift │ └── b2TimeOfImpact.swift │ ├── Common │ ├── b2Common.swift │ ├── b2Draw.swift │ ├── b2GrowableStack.swift │ ├── b2Math.swift │ ├── b2Settings.swift │ ├── b2Timer.swift │ └── b2Wrappers.swift │ ├── Dynamics │ ├── Contacts │ │ ├── b2ChainAndCircleContact.swift │ │ ├── b2ChainAndPolygonContact.swift │ │ ├── b2CircleContact.swift │ │ ├── b2Contact.swift │ │ ├── b2ContactSolver.swift │ │ ├── b2EdgeAndCircleContact.swift │ │ ├── b2EdgeAndPolygonContact.swift │ │ ├── b2PolygonAndCircleContact.swift │ │ └── b2PolygonContact.swift │ ├── Joints │ │ ├── b2DistanceJoint.swift │ │ ├── b2FrictionJoint.swift │ │ ├── b2GearJoint.swift │ │ ├── b2Joint.swift │ │ ├── b2MotorJoint.swift │ │ ├── b2MouseJoint.swift │ │ ├── b2PrismaticJoint.swift │ │ ├── b2PulleyJoint.swift │ │ ├── b2RevoluteJoint.swift │ │ ├── b2RopeJoint.swift │ │ ├── b2WeldJoint.swift │ │ └── b2WheelJoint.swift │ ├── b2Body.swift │ ├── b2ContactManager.swift │ ├── b2Fixture.swift │ ├── b2Island.swift │ ├── b2TimeStep.swift │ ├── b2World.swift │ └── b2WorldCallbacks.swift │ ├── Info.plist │ └── Rope │ └── b2Rope.swift └── Tests └── Box2DTests └── Box2DTests.swift /.gitignore: -------------------------------------------------------------------------------- 1 | UserInterfaceState.xcuserstate 2 | *.xcuserstate 3 | # Exclude the build directory 4 | build/* 5 | 6 | # Exclude temp nibs and swap files 7 | *~.nib 8 | *.swp 9 | 10 | # Exclude OS X folder attributes 11 | .DS_Store 12 | 13 | # Exclude user-specific XCode 3 and 4 files 14 | *.mode1 15 | *.mode1v3 16 | *.mode2v3 17 | *.perspective 18 | *.perspectivev3 19 | *.pbxuser 20 | *.xcworkspace 21 | xcuserdata 22 | -------------------------------------------------------------------------------- /License.txt: -------------------------------------------------------------------------------- 1 | Copyright (c) 2006-2014 Erin Catto http://www.box2d.org 2 | Copyright (c) 2015 - Yohei Yoshihara 3 | 4 | This software is provided 'as-is', without any express or implied 5 | warranty. In no event will the authors be held liable for any damages 6 | arising from the use of this software. 7 | 8 | Permission is granted to anyone to use this software for any purpose, 9 | including commercial applications, and to alter it and redistribute it 10 | freely, subject to the following restrictions: 11 | 12 | 1. The origin of this software must not be misrepresented; you must not 13 | claim that you wrote the original software. If you use this software 14 | in a product, an acknowledgment in the product documentation would be 15 | appreciated but is not required. 16 | 17 | 2. Altered source versions must be plainly marked as such, and must not be 18 | misrepresented as being the original software. 19 | 20 | 3. This notice may not be removed or altered from any source distribution. 21 | 22 | This version of box2d was developed by Yohei Yoshihara. It is based upon 23 | the original C++ code written by Erin Catto. 24 | -------------------------------------------------------------------------------- /Package.swift: -------------------------------------------------------------------------------- 1 | // swift-tools-version: 5.9 2 | // The swift-tools-version declares the minimum version of Swift required to build this package. 3 | 4 | import PackageDescription 5 | 6 | let package = Package( 7 | name: "Box2D", 8 | products: [ 9 | // Products define the executables and libraries a package produces, making them visible to other packages. 10 | .library( 11 | name: "Box2D", 12 | targets: ["Box2D"]), 13 | ], 14 | targets: [ 15 | // Targets are the basic building blocks of a package, defining a module or a test suite. 16 | // Targets can depend on other targets in this package and products from dependencies. 17 | .target( 18 | name: "Box2D"), 19 | .testTarget( 20 | name: "Box2DTests", 21 | dependencies: ["Box2D"]), 22 | ] 23 | ) 24 | -------------------------------------------------------------------------------- /Readme.md: -------------------------------------------------------------------------------- 1 | # Box2DSwift 2 | 3 | Box2DSwift is a Swift port of Box2D Physics Engine (Version 2.3.1). 4 | 5 | ## Testbed 6 | 7 | - SwiftUI: [Box2D testbed with SwiftUI](https://github.com/yohei-yoshihara/Box2DSwift-Testbed-swiftui) 8 | - iOS: [Box2D testbed for iOS](https://github.com/yohei-yoshihara/Box2DSwift-Testbed-iOS) 9 | - macOS: [Box2D testbed for mac](https://github.com/yohei-yoshihara/Box2DSwift-Testbed-mac) 10 | 11 | ## Requirements: 12 | - Xcode 15.1 13 | - Swift 5.9 14 | 15 | ## Status 16 | - Beta version 17 | -------------------------------------------------------------------------------- /Sources/Box2D/Box2D.h: -------------------------------------------------------------------------------- 1 | /** 2 | Copyright (c) 2006-2014 Erin Catto http://www.box2d.org 3 | Copyright (c) 2015 - Yohei Yoshihara 4 | 5 | This software is provided 'as-is', without any express or implied 6 | warranty. In no event will the authors be held liable for any damages 7 | arising from the use of this software. 8 | 9 | Permission is granted to anyone to use this software for any purpose, 10 | including commercial applications, and to alter it and redistribute it 11 | freely, subject to the following restrictions: 12 | 13 | 1. The origin of this software must not be misrepresented; you must not 14 | claim that you wrote the original software. If you use this software 15 | in a product, an acknowledgment in the product documentation would be 16 | appreciated but is not required. 17 | 18 | 2. Altered source versions must be plainly marked as such, and must not be 19 | misrepresented as being the original software. 20 | 21 | 3. This notice may not be removed or altered from any source distribution. 22 | 23 | This version of box2d was developed by Yohei Yoshihara. It is based upon 24 | the original C++ code written by Erin Catto. 25 | */ 26 | 27 | #import 28 | 29 | //! Project version number for Box2D. 30 | FOUNDATION_EXPORT double Box2DVersionNumber; 31 | 32 | //! Project version string for Box2D. 33 | FOUNDATION_EXPORT const unsigned char Box2DVersionString[]; 34 | 35 | // In this header, you should import all the public headers of your framework using statements like #import 36 | 37 | 38 | -------------------------------------------------------------------------------- /Sources/Box2D/Collision/Shapes/b2ChainShape.swift: -------------------------------------------------------------------------------- 1 | /** 2 | Copyright (c) 2006-2014 Erin Catto http://www.box2d.org 3 | Copyright (c) 2015 - Yohei Yoshihara 4 | 5 | This software is provided 'as-is', without any express or implied 6 | warranty. In no event will the authors be held liable for any damages 7 | arising from the use of this software. 8 | 9 | Permission is granted to anyone to use this software for any purpose, 10 | including commercial applications, and to alter it and redistribute it 11 | freely, subject to the following restrictions: 12 | 13 | 1. The origin of this software must not be misrepresented; you must not 14 | claim that you wrote the original software. If you use this software 15 | in a product, an acknowledgment in the product documentation would be 16 | appreciated but is not required. 17 | 18 | 2. Altered source versions must be plainly marked as such, and must not be 19 | misrepresented as being the original software. 20 | 21 | 3. This notice may not be removed or altered from any source distribution. 22 | 23 | This version of box2d was developed by Yohei Yoshihara. It is based upon 24 | the original C++ code written by Erin Catto. 25 | */ 26 | 27 | import Foundation 28 | 29 | /// A chain shape is a free form sequence of line segments. 30 | /// The chain has two-sided collision, so you can use inside and outside collision. 31 | /// Therefore, you may use any winding order. 32 | /// Since there may be many vertices, they are allocated using b2Alloc. 33 | /// Connectivity information is used to create smooth collisions. 34 | /// WARNING: The chain will not collide properly if there are self-intersections. 35 | open class b2ChainShape : b2Shape { 36 | public override init() { 37 | m_vertices = b2Array() 38 | m_hasPrevVertex = false 39 | m_hasNextVertex = false 40 | super.init() 41 | m_type = b2ShapeType.chain 42 | m_radius = b2_polygonRadius 43 | } 44 | 45 | /** 46 | Create a loop. This automatically adjusts connectivity. 47 | 48 | - parameter vertices: an array of vertices, these are copied 49 | */ 50 | open func createLoop(vertices: [b2Vec2]) { 51 | assert(m_vertices.count == 0) 52 | assert(vertices.count >= 3) 53 | for i in 1 ..< vertices.count { 54 | let v1 = vertices[i-1] 55 | let v2 = vertices[i] 56 | // If the code crashes here, it means your vertices are too close together. 57 | assert(b2DistanceSquared(v1, v2) > b2_linearSlop * b2_linearSlop) 58 | } 59 | 60 | for v in vertices { 61 | m_vertices.append(v) 62 | } 63 | m_vertices.append(m_vertices[0]) 64 | assert(m_vertices.count == vertices.count + 1) 65 | m_prevVertex = m_vertices[m_count - 2] 66 | m_nextVertex = m_vertices[1] 67 | m_hasPrevVertex = true 68 | m_hasNextVertex = true 69 | } 70 | 71 | /** 72 | Create a chain with isolated end vertices. 73 | 74 | - parameter vertices: an array of vertices, these are copied 75 | */ 76 | open func createChain(vertices: [b2Vec2]) { 77 | assert(m_vertices.count == 0) 78 | assert(vertices.count >= 2) 79 | for i in 1 ..< vertices.count { 80 | let v1 = vertices[i-1] 81 | let v2 = vertices[i] 82 | // If the code crashes here, it means your vertices are too close together. 83 | assert(b2DistanceSquared(v1, v2) > b2_linearSlop * b2_linearSlop) 84 | } 85 | 86 | for v in vertices { 87 | m_vertices.append(v) 88 | } 89 | 90 | m_hasPrevVertex = false 91 | m_hasNextVertex = false 92 | 93 | m_prevVertex.setZero() 94 | m_nextVertex.setZero() 95 | } 96 | 97 | /// Establish connectivity to a vertex that precedes the first vertex. 98 | /// Don't call this for loops. 99 | open func setPrevVertex(_ prevVertex: b2Vec2) { 100 | m_prevVertex = prevVertex 101 | m_hasPrevVertex = true 102 | } 103 | 104 | /// Establish connectivity to a vertex that follows the last vertex. 105 | /// Don't call this for loops. 106 | open func setNextVertex(_ nextVertex: b2Vec2) { 107 | m_nextVertex = nextVertex 108 | m_hasNextVertex = true 109 | } 110 | 111 | /// Implement b2Shape. Vertices are cloned using b2Alloc. 112 | open override func clone() -> b2Shape { 113 | let clone = b2ChainShape() 114 | clone.createChain(vertices: m_vertices.array) 115 | clone.m_prevVertex = m_prevVertex 116 | clone.m_nextVertex = m_nextVertex 117 | clone.m_hasPrevVertex = m_hasPrevVertex 118 | clone.m_hasNextVertex = m_hasNextVertex 119 | return clone 120 | } 121 | 122 | /// @see b2Shape::GetChildCount 123 | open override var childCount: Int { 124 | return m_count - 1 125 | } 126 | 127 | /// Get a child edge. 128 | open func getChildEdge(_ index : Int) -> b2EdgeShape { 129 | assert(0 <= index && index < m_vertices.count - 1) 130 | let edge = b2EdgeShape() 131 | edge.m_type = b2ShapeType.edge 132 | edge.m_radius = m_radius 133 | 134 | edge.m_vertex1 = m_vertices[index + 0] 135 | edge.m_vertex2 = m_vertices[index + 1] 136 | 137 | if index > 0 { 138 | edge.m_vertex0 = m_vertices[index - 1] 139 | edge.m_hasVertex0 = true 140 | } 141 | else { 142 | edge.m_vertex0 = m_prevVertex 143 | edge.m_hasVertex0 = m_hasPrevVertex 144 | } 145 | 146 | if index < m_count - 2 { 147 | edge.m_vertex3 = m_vertices[index + 2] 148 | edge.m_hasVertex3 = true 149 | } 150 | else { 151 | edge.m_vertex3 = m_nextVertex 152 | edge.m_hasVertex3 = m_hasNextVertex 153 | } 154 | return edge 155 | } 156 | 157 | /// This always return false. 158 | /// @see b2Shape::TestPoint 159 | open override func testPoint(transform: b2Transform, point: b2Vec2) -> Bool { 160 | return false 161 | } 162 | 163 | /// Implement b2Soverride hape. 164 | open override func rayCast(_ output: inout b2RayCastOutput, input : b2RayCastInput, transform xf: b2Transform, childIndex : Int) -> Bool { 165 | assert(childIndex < m_vertices.count) 166 | 167 | let edgeShape = b2EdgeShape() 168 | 169 | let i1 = childIndex 170 | var i2 = childIndex + 1 171 | if i2 == m_vertices.count { 172 | i2 = 0 173 | } 174 | 175 | edgeShape.m_vertex1 = m_vertices[i1] 176 | edgeShape.m_vertex2 = m_vertices[i2] 177 | 178 | return edgeShape.rayCast(&output, input: input, transform: xf, childIndex: 0) 179 | } 180 | 181 | /// @see b2Shape::ComputeAABB 182 | open override func computeAABB(_ aabb: inout b2AABB, transform: b2Transform, childIndex: Int) { 183 | assert(childIndex < m_vertices.count) 184 | 185 | let i1 = childIndex 186 | var i2 = childIndex + 1 187 | if i2 == m_count { 188 | i2 = 0 189 | } 190 | 191 | let v1 = b2Mul(transform, m_vertices[i1]) 192 | let v2 = b2Mul(transform, m_vertices[i2]) 193 | 194 | aabb.lowerBound = b2Min(v1, v2) 195 | aabb.upperBound = b2Max(v1, v2) 196 | } 197 | 198 | /// Chains have zero mass. 199 | /// @see b2Shape::ComputeMass 200 | open override func computeMass(density: b2Float) -> b2MassData { 201 | var massData = b2MassData() 202 | massData.mass = 0.0 203 | massData.center.setZero() 204 | massData.I = 0.0 205 | return massData 206 | } 207 | 208 | // MARK: private variables 209 | 210 | /// The vertices. Owned by this class. 211 | var m_vertices = b2Array() 212 | 213 | /// The vertex count. 214 | var m_count : Int { 215 | return m_vertices.count 216 | } 217 | 218 | var m_prevVertex = b2Vec2(), m_nextVertex = b2Vec2() 219 | var m_hasPrevVertex : Bool, m_hasNextVertex : Bool 220 | } 221 | -------------------------------------------------------------------------------- /Sources/Box2D/Collision/Shapes/b2CircleShape.swift: -------------------------------------------------------------------------------- 1 | /** 2 | Copyright (c) 2006-2014 Erin Catto http://www.box2d.org 3 | Copyright (c) 2015 - Yohei Yoshihara 4 | 5 | This software is provided 'as-is', without any express or implied 6 | warranty. In no event will the authors be held liable for any damages 7 | arising from the use of this software. 8 | 9 | Permission is granted to anyone to use this software for any purpose, 10 | including commercial applications, and to alter it and redistribute it 11 | freely, subject to the following restrictions: 12 | 13 | 1. The origin of this software must not be misrepresented; you must not 14 | claim that you wrote the original software. If you use this software 15 | in a product, an acknowledgment in the product documentation would be 16 | appreciated but is not required. 17 | 18 | 2. Altered source versions must be plainly marked as such, and must not be 19 | misrepresented as being the original software. 20 | 21 | 3. This notice may not be removed or altered from any source distribution. 22 | 23 | This version of box2d was developed by Yohei Yoshihara. It is based upon 24 | the original C++ code written by Erin Catto. 25 | */ 26 | 27 | import Foundation 28 | 29 | /// A circle shape. 30 | open class b2CircleShape : b2Shape { 31 | public override init() { 32 | super.init() 33 | m_type = b2ShapeType.circle 34 | m_radius = 0.0 35 | m_p = b2Vec2(0.0, 0.0) 36 | } 37 | 38 | /// Implement b2Shape. 39 | open override func clone() -> b2Shape { 40 | let clone = b2CircleShape() 41 | clone.m_radius = m_radius 42 | clone.m_p = m_p 43 | return clone 44 | } 45 | 46 | /// @see b2Shape::GetChildCount 47 | open override var childCount: Int { 48 | return 1 49 | } 50 | 51 | /// Implement b2Shape. 52 | open override func testPoint(transform: b2Transform, point: b2Vec2) -> Bool { 53 | let center = transform.p + b2Mul(transform.q, m_p) 54 | let d = p - center 55 | return b2Dot(d, d) <= m_radius * m_radius 56 | } 57 | 58 | // Collision Detection in Interactive 3D Environments by Gino van den Bergen 59 | // From Section 3.1.2 60 | // x = s + a * r 61 | // norm(x) = radius 62 | open override func rayCast(_ output: inout b2RayCastOutput, input: b2RayCastInput, transform: b2Transform, childIndex: Int) -> Bool { 63 | let position = transform.p + b2Mul(transform.q, m_p) 64 | let s = input.p1 - position 65 | let b = b2Dot(s, s) - m_radius * m_radius 66 | 67 | // Solve quadratic equation. 68 | let r = input.p2 - input.p1 69 | let c = b2Dot(s, r) 70 | let rr = b2Dot(r, r) 71 | let sigma = c * c - rr * b 72 | 73 | // Check for negative discriminant and short segment. 74 | if sigma < 0.0 || rr < b2_epsilon { 75 | return false 76 | } 77 | 78 | // Find the point of intersection of the line with the circle. 79 | var a = -(c + b2Sqrt(sigma)) 80 | 81 | // Is the intersection point on the segment? 82 | if 0.0 <= a && a <= input.maxFraction * rr { 83 | a /= rr 84 | output.fraction = a 85 | output.normal = s + a * r 86 | output.normal.normalize() 87 | return true 88 | } 89 | 90 | return false 91 | } 92 | 93 | /// @see b2Shape::ComputeAABB 94 | open override func computeAABB(_ aabb: inout b2AABB, transform: b2Transform, childIndex: Int) { 95 | let p = transform.p + b2Mul(transform.q, m_p) 96 | aabb.lowerBound.set(p.x - m_radius, p.y - m_radius) 97 | aabb.upperBound.set(p.x + m_radius, p.y + m_radius) 98 | } 99 | 100 | /// @see b2Shape::ComputeMass 101 | open override func computeMass(density: b2Float) -> b2MassData { 102 | var massData = b2MassData() 103 | massData.mass = density * b2_pi * m_radius * m_radius 104 | massData.center = m_p 105 | 106 | // inertia about the local origin 107 | massData.I = massData.mass * (0.5 * m_radius * m_radius + b2Dot(m_p, m_p)) 108 | return massData 109 | } 110 | 111 | /// Get the supporting vertex index in the given direction. 112 | open func getSupport(direction: b2Vec2) -> Int { 113 | return 0 114 | } 115 | 116 | /// Get the supporting vertex in the given direction. 117 | open func getSupportVertex(direction: b2Vec2) -> b2Vec2 { 118 | return m_p 119 | } 120 | 121 | /// Get the vertex count. 122 | open var vertexCount: Int { return 1 } 123 | 124 | /// Get a vertex by index. Used by b2Distance. 125 | open func vertex(index: Int) -> b2Vec2 { 126 | assert(index == 0) 127 | return m_p 128 | } 129 | 130 | open var p: b2Vec2 { 131 | get { 132 | return m_p_[0] 133 | } 134 | set { 135 | m_p_[0] = newValue 136 | } 137 | } 138 | 139 | // MARK: private variables 140 | 141 | /// Position 142 | var m_p_ = b2Array(count: 1, repeatedValue: b2Vec2()) 143 | var m_p: b2Vec2 { 144 | get { 145 | return m_p_[0] 146 | } 147 | set { 148 | m_p_[0] = newValue 149 | } 150 | } 151 | } 152 | -------------------------------------------------------------------------------- /Sources/Box2D/Collision/Shapes/b2EdgeShape.swift: -------------------------------------------------------------------------------- 1 | /** 2 | Copyright (c) 2006-2014 Erin Catto http://www.box2d.org 3 | Copyright (c) 2015 - Yohei Yoshihara 4 | 5 | This software is provided 'as-is', without any express or implied 6 | warranty. In no event will the authors be held liable for any damages 7 | arising from the use of this software. 8 | 9 | Permission is granted to anyone to use this software for any purpose, 10 | including commercial applications, and to alter it and redistribute it 11 | freely, subject to the following restrictions: 12 | 13 | 1. The origin of this software must not be misrepresented; you must not 14 | claim that you wrote the original software. If you use this software 15 | in a product, an acknowledgment in the product documentation would be 16 | appreciated but is not required. 17 | 18 | 2. Altered source versions must be plainly marked as such, and must not be 19 | misrepresented as being the original software. 20 | 21 | 3. This notice may not be removed or altered from any source distribution. 22 | 23 | This version of box2d was developed by Yohei Yoshihara. It is based upon 24 | the original C++ code written by Erin Catto. 25 | */ 26 | 27 | import Foundation 28 | 29 | /// A line segment (edge) shape. These can be connected in chains or loops 30 | /// to other edge shapes. The connectivity information is used to ensure 31 | /// correct contact normals. 32 | open class b2EdgeShape : b2Shape { 33 | public override init() { 34 | m_vertex0 = b2Vec2(0.0, 0.0) 35 | m_vertex3 = b2Vec2(0.0, 0.0) 36 | m_hasVertex0 = false 37 | m_hasVertex3 = false 38 | super.init() 39 | m_type = b2ShapeType.edge 40 | m_radius = b2_polygonRadius 41 | } 42 | 43 | /// Set this as an isolated edge. 44 | open func set(vertex1 v1: b2Vec2, vertex2 v2: b2Vec2) { 45 | m_vertex1 = v1 46 | m_vertex2 = v2 47 | m_hasVertex0 = false 48 | m_hasVertex3 = false 49 | } 50 | 51 | /// Implement b2Shape. 52 | open override func clone() -> b2Shape { 53 | let clone = b2EdgeShape() 54 | clone.m_radius = m_radius 55 | clone.m_vertices = m_vertices.clone() 56 | clone.m_vertex0 = m_vertex0 57 | clone.m_vertex3 = m_vertex3 58 | clone.m_hasVertex0 = m_hasVertex0 59 | clone.m_hasVertex3 = m_hasVertex3 60 | return clone 61 | } 62 | 63 | /// @see b2Shape::GetChildCount 64 | open override var childCount: Int { 65 | return 1 66 | } 67 | 68 | /// @see b2Shape::TestPoint 69 | open override func testPoint(transform: b2Transform, point: b2Vec2) -> Bool { 70 | return false 71 | } 72 | 73 | // p = p1 + t * d 74 | // v = v1 + s * e 75 | // p1 + t * d = v1 + s * e 76 | // s * e - t * d = p1 - v1 77 | open override func rayCast(_ output: inout b2RayCastOutput, input: b2RayCastInput, transform xf: b2Transform, childIndex: Int) -> Bool { 78 | // Put the ray into the edge's frame of reference. 79 | let p1 = b2MulT(xf.q, input.p1 - xf.p) 80 | let p2 = b2MulT(xf.q, input.p2 - xf.p) 81 | let d = p2 - p1 82 | 83 | let v1 = m_vertex1 84 | let v2 = m_vertex2 85 | let e = v2 - v1 86 | var normal = b2Vec2(e.y, -e.x) 87 | normal.normalize() 88 | 89 | // q = p1 + t * d 90 | // dot(normal, q - v1) = 0 91 | // dot(normal, p1 - v1) + t * dot(normal, d) = 0 92 | let numerator = b2Dot(normal, v1 - p1) 93 | let denominator = b2Dot(normal, d) 94 | 95 | if denominator == 0.0 { 96 | return false 97 | } 98 | 99 | let t = numerator / denominator 100 | if t < 0.0 || input.maxFraction < t { 101 | return false 102 | } 103 | 104 | let q = p1 + t * d 105 | 106 | // q = v1 + s * r 107 | // s = dot(q - v1, r) / dot(r, r) 108 | let r = v2 - v1 109 | let rr = b2Dot(r, r) 110 | if rr == 0.0 { 111 | return false 112 | } 113 | 114 | let s = b2Dot(q - v1, r) / rr 115 | if s < 0.0 || 1.0 < s { 116 | return false 117 | } 118 | 119 | output.fraction = t 120 | if numerator > 0.0 { 121 | output.normal = -b2Mul(xf.q, normal) 122 | } 123 | else { 124 | output.normal = b2Mul(xf.q, normal) 125 | } 126 | return true 127 | } 128 | 129 | /// @see b2Shape::ComputeAABB 130 | open override func computeAABB(_ aabb: inout b2AABB, transform: b2Transform, childIndex: Int) { 131 | let v1 = b2Mul(transform, m_vertex1) 132 | let v2 = b2Mul(transform, m_vertex2) 133 | 134 | let lower = b2Min(v1, v2) 135 | let upper = b2Max(v1, v2) 136 | 137 | let r = b2Vec2(m_radius, m_radius) 138 | aabb.lowerBound = lower - r 139 | aabb.upperBound = upper + r 140 | } 141 | 142 | /// @see b2Shape::ComputeMass 143 | open override func computeMass(density: b2Float) -> b2MassData { 144 | var massData = b2MassData() 145 | massData.mass = 0.0 146 | massData.center = 0.5 * (m_vertex1 + m_vertex2) 147 | massData.I = 0.0 148 | return massData 149 | } 150 | 151 | open var vertex1 : b2Vec2 { 152 | get { return m_vertices[0] } 153 | set { m_vertices[0] = newValue } 154 | } 155 | 156 | open var vertex2 : b2Vec2 { 157 | get { return m_vertices[1] } 158 | set { m_vertices[1] = newValue } 159 | } 160 | 161 | open var vertex0: b2Vec2 { 162 | get { return m_vertex0 } 163 | set { m_vertex0 = newValue } 164 | } 165 | 166 | open var vertex3: b2Vec2 { 167 | get { return m_vertex3 } 168 | set { m_vertex3 = newValue } 169 | } 170 | 171 | open var hasVertex0: Bool { 172 | get { return m_hasVertex0 } 173 | set { m_hasVertex0 = newValue } 174 | } 175 | 176 | open var hasVertex3: Bool { 177 | get { return m_hasVertex3 } 178 | set { m_hasVertex3 = newValue } 179 | } 180 | 181 | // MARK: private variables 182 | 183 | /// These are the edge vertices 184 | var m_vertices = b2Array(count: 2, repeatedValue: b2Vec2()) 185 | var m_vertex1 : b2Vec2 { 186 | get { return m_vertices[0] } 187 | set { m_vertices[0] = newValue } 188 | } 189 | var m_vertex2 : b2Vec2 { 190 | get { return m_vertices[1] } 191 | set { m_vertices[1] = newValue } 192 | } 193 | 194 | /// Optional adjacent vertices. These are used for smooth collision. 195 | var m_vertex0 : b2Vec2 196 | var m_vertex3 : b2Vec2 197 | var m_hasVertex0 : Bool 198 | var m_hasVertex3 : Bool 199 | } 200 | 201 | -------------------------------------------------------------------------------- /Sources/Box2D/Collision/Shapes/b2Shape.swift: -------------------------------------------------------------------------------- 1 | /** 2 | Copyright (c) 2006-2014 Erin Catto http://www.box2d.org 3 | Copyright (c) 2015 - Yohei Yoshihara 4 | 5 | This software is provided 'as-is', without any express or implied 6 | warranty. In no event will the authors be held liable for any damages 7 | arising from the use of this software. 8 | 9 | Permission is granted to anyone to use this software for any purpose, 10 | including commercial applications, and to alter it and redistribute it 11 | freely, subject to the following restrictions: 12 | 13 | 1. The origin of this software must not be misrepresented; you must not 14 | claim that you wrote the original software. If you use this software 15 | in a product, an acknowledgment in the product documentation would be 16 | appreciated but is not required. 17 | 18 | 2. Altered source versions must be plainly marked as such, and must not be 19 | misrepresented as being the original software. 20 | 21 | 3. This notice may not be removed or altered from any source distribution. 22 | 23 | This version of box2d was developed by Yohei Yoshihara. It is based upon 24 | the original C++ code written by Erin Catto. 25 | */ 26 | 27 | import Foundation 28 | 29 | /** 30 | This holds the mass data computed for a shape. 31 | */ 32 | public struct b2MassData : CustomStringConvertible { 33 | /** 34 | The mass of the shape, usually in kilograms. 35 | */ 36 | public var mass: b2Float = 0 37 | /** 38 | The position of the shape's centroid relative to the shape's origin. 39 | */ 40 | public var center = b2Vec2() 41 | /** 42 | The rotational inertia of the shape about the local origin. 43 | */ 44 | public var I: b2Float = 0 45 | 46 | public var description: String { 47 | return "b2MassData[mass=\(mass), center=\(center), I=\(I)]" 48 | } 49 | } 50 | 51 | public enum b2ShapeType: Int, CustomStringConvertible { 52 | case circle = 0 53 | case edge = 1 54 | case polygon = 2 55 | case chain = 3 56 | case typeCount = 4 57 | public var description: String { 58 | switch self { 59 | case .circle: return "circle" 60 | case .edge: return "edge" 61 | case .polygon: return "polygon" 62 | case .chain: return "chain" 63 | case .typeCount: return "typeCount" 64 | } 65 | } 66 | } 67 | 68 | // MARK: - 69 | /** 70 | A shape is used for collision detection. You can create a shape however you like. 71 | Shapes used for simulation in b2World are created automatically when a b2Fixture 72 | is created. Shapes may encapsulate a one or more child shapes. 73 | */ 74 | open class b2Shape : CustomStringConvertible { 75 | 76 | public init() { 77 | m_type = b2ShapeType.circle 78 | m_radius = 0 79 | } 80 | /** 81 | Clone the concrete shape using the provided allocator. 82 | */ 83 | open func clone() -> b2Shape { 84 | fatalError("must override") 85 | } 86 | /** 87 | Get the type of this shape. You can use this to down cast to the concrete shape. 88 | 89 | - returns: the shape type. 90 | */ 91 | open var type: b2ShapeType { 92 | return m_type 93 | } 94 | /** 95 | Get the number of child primitives. 96 | */ 97 | open var childCount: Int { 98 | fatalError("must override") 99 | } 100 | /** 101 | Test a point for containment in this shape. This only works for convex shapes. 102 | 103 | - parameter transform: the shape world transform. 104 | - parameter point: a point in world coordinates. 105 | */ 106 | open func testPoint(transform: b2Transform, point: b2Vec2) -> Bool { 107 | fatalError("must override") 108 | } 109 | /** 110 | Cast a ray against a child shape. 111 | 112 | - parameter output: the ray-cast results. 113 | - parameter input: the ray-cast input parameters. 114 | - parameter transform: the transform to be applied to the shape. 115 | - parameter childIndex: the child shape index 116 | */ 117 | open func rayCast(_ output: inout b2RayCastOutput, input: b2RayCastInput, transform: b2Transform, childIndex: Int) -> Bool { 118 | fatalError("must override") 119 | } 120 | /** 121 | Given a transform, compute the associated axis aligned bounding box for a child shape. 122 | 123 | - parameter aabb: returns the axis aligned box. 124 | - parameter xf: the world transform of the shape. 125 | - parameter childIndex: the child shape 126 | */ 127 | open func computeAABB(_ aabb: inout b2AABB, transform: b2Transform, childIndex: Int) { 128 | fatalError("must override") 129 | } 130 | /** 131 | Compute the mass properties of this shape using its dimensions and density. 132 | The inertia tensor is computed about the local origin. 133 | 134 | - parameter massData: returns the mass data for this shape. 135 | - parameter density: the density in kilograms per meter squared. 136 | */ 137 | open func computeMass(density: b2Float) -> b2MassData { 138 | fatalError("must override") 139 | } 140 | 141 | open var description: String { 142 | return "b2Shape[type=\(m_type), radius=\(m_radius)]" 143 | } 144 | 145 | // MARK: private variables 146 | 147 | var m_type: b2ShapeType 148 | var m_radius: b2Float 149 | open var radius: b2Float { 150 | get { return m_radius } 151 | set { m_radius = newValue } 152 | } 153 | } 154 | -------------------------------------------------------------------------------- /Sources/Box2D/Collision/b2BroadPhase.swift: -------------------------------------------------------------------------------- 1 | /** 2 | Copyright (c) 2006-2014 Erin Catto http://www.box2d.org 3 | Copyright (c) 2015 - Yohei Yoshihara 4 | 5 | This software is provided 'as-is', without any express or implied 6 | warranty. In no event will the authors be held liable for any damages 7 | arising from the use of this software. 8 | 9 | Permission is granted to anyone to use this software for any purpose, 10 | including commercial applications, and to alter it and redistribute it 11 | freely, subject to the following restrictions: 12 | 13 | 1. The origin of this software must not be misrepresented; you must not 14 | claim that you wrote the original software. If you use this software 15 | in a product, an acknowledgment in the product documentation would be 16 | appreciated but is not required. 17 | 18 | 2. Altered source versions must be plainly marked as such, and must not be 19 | misrepresented as being the original software. 20 | 21 | 3. This notice may not be removed or altered from any source distribution. 22 | 23 | This version of box2d was developed by Yohei Yoshihara. It is based upon 24 | the original C++ code written by Erin Catto. 25 | */ 26 | 27 | import Foundation 28 | 29 | public struct b2Pair : CustomStringConvertible { 30 | public var proxyIdA: Int = -1 31 | public var proxyIdB: Int = -1 32 | 33 | public var description: String { 34 | return "{\(proxyIdA):\(proxyIdB)}" 35 | } 36 | } 37 | 38 | // MARK: - 39 | /// The broad-phase is used for computing pairs and performing volume queries and ray casts. 40 | /// This broad-phase does not persist pairs. Instead, this reports potentially new pairs. 41 | /// It is up to the client to consume the new pairs and to track subsequent overlap. 42 | open class b2BroadPhase : b2QueryWrapper { 43 | public struct Const { 44 | public static let nullProxy = -1 45 | } 46 | 47 | // MARK: public methods 48 | public init() { 49 | m_proxyCount = 0 50 | m_pairBuffer = [b2Pair]() 51 | m_pairBuffer.reserveCapacity(16) 52 | m_moveBuffer = [Int]() 53 | m_moveBuffer.reserveCapacity(16) 54 | } 55 | 56 | /// Create a proxy with an initial AABB. Pairs are not reported until 57 | /// UpdatePairs is called. 58 | open func createProxy(aabb: b2AABB, userData: b2FixtureProxy) -> Int { 59 | let proxyId = m_tree.createProxy(aabb: aabb, userData: userData) 60 | m_proxyCount += 1 61 | bufferMove(proxyId) 62 | return proxyId 63 | } 64 | 65 | /// Destroy a proxy. It is up to the client to remove any pairs. 66 | open func destroyProxy(_ proxyId: Int) { 67 | unBufferMove(proxyId) 68 | m_proxyCount -= 1 69 | m_tree.destroyProxy(proxyId) 70 | } 71 | 72 | /// Call MoveProxy as many times as you like, then when you are done 73 | /// call UpdatePairs to finalized the proxy pairs (for your time step). 74 | open func moveProxy(_ proxyId: Int, aabb: b2AABB, displacement: b2Vec2) { 75 | let buffer = m_tree.moveProxy(proxyId, aabb: aabb, displacement: displacement) 76 | if buffer { 77 | bufferMove(proxyId) 78 | } 79 | } 80 | 81 | /// Call to trigger a re-processing of it's pairs on the next call to UpdatePairs. 82 | open func touchProxy(_ proxyId: Int) { 83 | bufferMove(proxyId) 84 | } 85 | 86 | /// Get the fat AABB for a proxy. 87 | open func getFatAABB(proxyId: Int) -> b2AABB { 88 | return m_tree.getFatAABB(proxyId) 89 | } 90 | 91 | /// Get user data from a proxy. Returns NULL if the id is invalid. 92 | open func getUserData(proxyId : Int) -> b2FixtureProxy? { 93 | return m_tree.getUserData(proxyId) 94 | } 95 | 96 | /// Test overlap of fat AABBs. 97 | open func testOverlap(proxyIdA : Int, proxyIdB : Int) -> Bool { 98 | let aabbA = m_tree.getFatAABB(proxyIdA) 99 | let aabbB = m_tree.getFatAABB(proxyIdB) 100 | return b2TestOverlap(aabbA, aabbB) 101 | } 102 | 103 | /// Get the number of proxies. 104 | open func getProxyCount() -> Int { 105 | return m_proxyCount 106 | } 107 | 108 | /// Update the pairs. This results in pair callbacks. This can only add pairs. 109 | open func updatePairs(callback: T) { 110 | // Reset pair buffer 111 | m_pairBuffer.removeAll(keepingCapacity: true) 112 | 113 | // Perform tree queries for all moving proxies. 114 | for i in 0 ..< m_moveBuffer.count { 115 | m_queryProxyId = m_moveBuffer[i] 116 | if m_queryProxyId == Const.nullProxy { 117 | continue 118 | } 119 | 120 | // We have to query the tree with the fat AABB so that 121 | // we don't fail to create a pair that may touch later. 122 | let fatAABB = m_tree.getFatAABB(m_queryProxyId) 123 | // Query tree, create pairs and add them pair buffer. 124 | m_tree.query(callback: self, aabb: fatAABB) 125 | } 126 | 127 | // Reset move buffer 128 | m_moveBuffer.removeAll(keepingCapacity: true) 129 | 130 | // Sort the pair buffer to expose duplicates. 131 | m_pairBuffer.sort { 132 | if $0.proxyIdA < $1.proxyIdA { 133 | return true 134 | } 135 | if $0.proxyIdA == $1.proxyIdA { 136 | return $0.proxyIdB < $1.proxyIdB 137 | } 138 | return false 139 | } 140 | 141 | // Send the pairs back to the client. 142 | var i = 0 143 | while i < m_pairBuffer.count { 144 | let primaryPair = m_pairBuffer[i] 145 | var userDataA = m_tree.getUserData(primaryPair.proxyIdA)! 146 | var userDataB = m_tree.getUserData(primaryPair.proxyIdB)! 147 | 148 | callback.addPair(&userDataA, &userDataB) 149 | i += 1 150 | 151 | // Skip any duplicate pairs. 152 | while i < m_pairBuffer.count { 153 | let pair = m_pairBuffer[i] 154 | if pair.proxyIdA != primaryPair.proxyIdA || pair.proxyIdB != primaryPair.proxyIdB { 155 | break 156 | } 157 | i += 1 158 | } 159 | } 160 | 161 | // Try to keep the tree balanced. 162 | //m_tree.Rebalance(4) 163 | } 164 | 165 | /// Query an AABB for overlapping proxies. The callback class 166 | /// is called for each proxy that overlaps the supplied AABB. 167 | open func query(callback : T, aabb: b2AABB) { 168 | m_tree.query(callback: callback, aabb: aabb) 169 | } 170 | 171 | /** 172 | Ray-cast against the proxies in the tree. This relies on the callback 173 | to perform a exact ray-cast in the case were the proxy contains a shape. 174 | The callback also performs the any collision filtering. This has performance 175 | roughly equal to k * log(n), where k is the number of collisions and n is the 176 | number of proxies in the tree. 177 | 178 | - parameter input: the ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1). 179 | - parameter callback: a callback class that is called for each proxy that is hit by the ray. 180 | */ 181 | open func rayCast(callback: T, input: b2RayCastInput) { 182 | m_tree.rayCast(callback: callback, input: input) 183 | } 184 | 185 | /// Get the height of the embedded tree. 186 | open func getTreeHeight() -> Int { 187 | return m_tree.getHeight() 188 | } 189 | 190 | /// Get the balance of the embedded tree. 191 | open func getTreeBalance() -> Int { 192 | return m_tree.getMaxBalance() 193 | } 194 | 195 | /// Get the quality metric of the embedded tree. 196 | open func getTreeQuality() -> b2Float { 197 | return m_tree.getAreaRatio() 198 | } 199 | 200 | /** 201 | Shift the world origin. Useful for large worlds. 202 | The shift formula is: position -= newOrigin 203 | 204 | - parameter newOrigin: the new origin with respect to the old origin 205 | */ 206 | open func shiftOrigin(_ newOrigin: b2Vec2) { 207 | m_tree.shiftOrigin(newOrigin) 208 | } 209 | 210 | // MARK: private methods 211 | func bufferMove(_ proxyId: Int) { 212 | m_moveBuffer.append(proxyId) 213 | } 214 | func unBufferMove(_ proxyId: Int) { 215 | for i in 0 ..< m_moveBuffer.count { 216 | if m_moveBuffer[i] == proxyId { 217 | m_moveBuffer[i] = Const.nullProxy 218 | } 219 | } 220 | } 221 | // This is called from b2DynamicTree::Query when we are gathering pairs. 222 | open func queryCallback(_ proxyId: Int) -> Bool { 223 | // A proxy cannot form a pair with itself. 224 | if proxyId == m_queryProxyId { 225 | return true 226 | } 227 | 228 | let pair = b2Pair(proxyIdA: min(proxyId, m_queryProxyId), 229 | proxyIdB: max(proxyId, m_queryProxyId)) 230 | m_pairBuffer.append(pair) 231 | return true 232 | } 233 | 234 | // MARK: private variables 235 | var m_tree = b2DynamicTree() 236 | var m_proxyCount: Int = 0 237 | var m_moveBuffer = [Int]() 238 | var m_pairBuffer = [b2Pair]() 239 | var m_queryProxyId: Int = 0 240 | 241 | } 242 | -------------------------------------------------------------------------------- /Sources/Box2D/Collision/b2CollideCircles.swift: -------------------------------------------------------------------------------- 1 | /** 2 | Copyright (c) 2006-2014 Erin Catto http://www.box2d.org 3 | Copyright (c) 2015 - Yohei Yoshihara 4 | 5 | This software is provided 'as-is', without any express or implied 6 | warranty. In no event will the authors be held liable for any damages 7 | arising from the use of this software. 8 | 9 | Permission is granted to anyone to use this software for any purpose, 10 | including commercial applications, and to alter it and redistribute it 11 | freely, subject to the following restrictions: 12 | 13 | 1. The origin of this software must not be misrepresented; you must not 14 | claim that you wrote the original software. If you use this software 15 | in a product, an acknowledgment in the product documentation would be 16 | appreciated but is not required. 17 | 18 | 2. Altered source versions must be plainly marked as such, and must not be 19 | misrepresented as being the original software. 20 | 21 | 3. This notice may not be removed or altered from any source distribution. 22 | 23 | This version of box2d was developed by Yohei Yoshihara. It is based upon 24 | the original C++ code written by Erin Catto. 25 | */ 26 | 27 | import Foundation 28 | 29 | /// Compute the collision manifold between two circles. 30 | public func b2CollideCircles( 31 | manifold: inout b2Manifold, 32 | circleA: b2CircleShape, transformA xfA: b2Transform, 33 | circleB: b2CircleShape, transformB xfB: b2Transform) 34 | { 35 | manifold.points.removeAll(keepingCapacity: true) 36 | 37 | let pA = b2Mul(xfA, circleA.m_p) 38 | let pB = b2Mul(xfB, circleB.m_p) 39 | 40 | let d = pB - pA 41 | let distSqr = b2Dot(d, d) 42 | let rA = circleA.m_radius 43 | let rB = circleB.m_radius 44 | let radius = rA + rB 45 | if distSqr > radius * radius { 46 | return 47 | } 48 | 49 | manifold.type = b2ManifoldType.circles 50 | manifold.localPoint = circleA.m_p 51 | manifold.localNormal.setZero() 52 | let cp = b2ManifoldPoint() 53 | cp.localPoint = circleB.m_p 54 | cp.id.setZero() 55 | manifold.points.append(cp) 56 | } 57 | 58 | /// Compute the collision manifold between a polygon and a circle. 59 | public func b2CollidePolygonAndCircle( 60 | manifold: inout b2Manifold, 61 | polygonA: b2PolygonShape, transformA xfA: b2Transform, 62 | circleB: b2CircleShape, transformB xfB: b2Transform) 63 | { 64 | manifold.points.removeAll(keepingCapacity: true) 65 | 66 | // Compute circle position in the frame of the polygon. 67 | let c = b2Mul(xfB, circleB.m_p) 68 | let cLocal = b2MulT(xfA, c) 69 | 70 | // Find the min separating edge. 71 | var normalIndex = 0 72 | var separation = -b2_maxFloat 73 | let radius = polygonA.m_radius + circleB.m_radius 74 | let vertexCount = polygonA.m_count 75 | let vertices = polygonA.m_vertices 76 | let normals = polygonA.m_normals 77 | 78 | for i in 0 ..< vertexCount { 79 | let s = b2Dot(normals[i], cLocal - vertices[i]) 80 | 81 | if s > radius { 82 | // Early out. 83 | return 84 | } 85 | 86 | if s > separation { 87 | separation = s 88 | normalIndex = i 89 | } 90 | } 91 | 92 | // Vertices that subtend the incident face. 93 | let vertIndex1 = normalIndex 94 | let vertIndex2 = vertIndex1 + 1 < vertexCount ? vertIndex1 + 1 : 0 95 | let v1 = vertices[vertIndex1] 96 | let v2 = vertices[vertIndex2] 97 | 98 | // If the center is inside the polygon ... 99 | if separation < b2_epsilon { 100 | manifold.type = b2ManifoldType.faceA 101 | manifold.localNormal = normals[normalIndex] 102 | manifold.localPoint = 0.5 * (v1 + v2) 103 | let cp = b2ManifoldPoint() 104 | cp.localPoint = circleB.m_p 105 | cp.id.setZero() 106 | manifold.points.append(cp) 107 | return 108 | } 109 | 110 | // Compute barycentric coordinates 111 | let u1 = b2Dot(cLocal - v1, v2 - v1) 112 | let u2 = b2Dot(cLocal - v2, v1 - v2) 113 | if u1 <= 0.0 { 114 | if b2DistanceSquared(cLocal, v1) > radius * radius { 115 | return 116 | } 117 | 118 | manifold.type = b2ManifoldType.faceA 119 | manifold.localNormal = cLocal - v1 120 | manifold.localNormal.normalize() 121 | manifold.localPoint = v1 122 | let cp = b2ManifoldPoint() 123 | cp.localPoint = circleB.m_p 124 | cp.id.setZero() 125 | manifold.points.append(cp) 126 | } 127 | else if u2 <= 0.0 { 128 | if b2DistanceSquared(cLocal, v2) > radius * radius { 129 | return 130 | } 131 | 132 | manifold.type = b2ManifoldType.faceA 133 | manifold.localNormal = cLocal - v2 134 | manifold.localNormal.normalize() 135 | manifold.localPoint = v2 136 | let cp = b2ManifoldPoint() 137 | cp.localPoint = circleB.m_p 138 | cp.id.setZero() 139 | manifold.points.append(cp) 140 | } 141 | else { 142 | let faceCenter = 0.5 * (v1 + v2) 143 | let separation = b2Dot(cLocal - faceCenter, normals[vertIndex1]) 144 | if separation > radius { 145 | return 146 | } 147 | 148 | manifold.type = b2ManifoldType.faceA 149 | manifold.localNormal = normals[vertIndex1] 150 | manifold.localPoint = faceCenter 151 | let cp = b2ManifoldPoint() 152 | cp.localPoint = circleB.m_p 153 | cp.id.setZero() 154 | manifold.points.append(cp) 155 | } 156 | return 157 | } 158 | 159 | -------------------------------------------------------------------------------- /Sources/Box2D/Collision/b2CollidePolygon.swift: -------------------------------------------------------------------------------- 1 | /** 2 | Copyright (c) 2006-2014 Erin Catto http://www.box2d.org 3 | Copyright (c) 2015 - Yohei Yoshihara 4 | 5 | This software is provided 'as-is', without any express or implied 6 | warranty. In no event will the authors be held liable for any damages 7 | arising from the use of this software. 8 | 9 | Permission is granted to anyone to use this software for any purpose, 10 | including commercial applications, and to alter it and redistribute it 11 | freely, subject to the following restrictions: 12 | 13 | 1. The origin of this software must not be misrepresented; you must not 14 | claim that you wrote the original software. If you use this software 15 | in a product, an acknowledgment in the product documentation would be 16 | appreciated but is not required. 17 | 18 | 2. Altered source versions must be plainly marked as such, and must not be 19 | misrepresented as being the original software. 20 | 21 | 3. This notice may not be removed or altered from any source distribution. 22 | 23 | This version of box2d was developed by Yohei Yoshihara. It is based upon 24 | the original C++ code written by Erin Catto. 25 | */ 26 | 27 | import Foundation 28 | 29 | // Find the max separation between poly1 and poly2 using edge normals from poly1. 30 | func b2FindMaxSeparation(_ poly1: b2PolygonShape, xf1: b2Transform, 31 | poly2: b2PolygonShape, xf2: b2Transform) -> (edgeIndex: Int, maxSeparation: b2Float) 32 | { 33 | let count1 = poly1.m_count 34 | let count2 = poly2.m_count 35 | let n1s = poly1.m_normals 36 | let v1s = poly1.m_vertices 37 | let v2s = poly2.m_vertices 38 | let xf = b2MulT(xf2, xf1) 39 | 40 | var bestIndex = 0 41 | var maxSeparation = -b2_maxFloat 42 | for i in 0 ..< count1 { 43 | // Get poly1 normal in frame2. 44 | let n = b2Mul(xf.q, n1s[i]) 45 | let v1 = b2Mul(xf, v1s[i]) 46 | 47 | // Find deepest point for normal i. 48 | var si = b2_maxFloat 49 | for j in 0 ..< count2 { 50 | let sij = b2Dot(n, v2s[j] - v1) 51 | if sij < si { 52 | si = sij 53 | } 54 | } 55 | 56 | if si > maxSeparation { 57 | maxSeparation = si 58 | bestIndex = i 59 | } 60 | } 61 | 62 | return (bestIndex, maxSeparation) 63 | } 64 | 65 | func b2FindIncidentEdge(_ poly1: b2PolygonShape, xf1: b2Transform, edge1: Int, 66 | poly2: b2PolygonShape, xf2: b2Transform) -> [b2ClipVertex] 67 | { 68 | let normals1 = poly1.m_normals 69 | 70 | let count2 = poly2.m_count 71 | let vertices2 = poly2.m_vertices 72 | let normals2 = poly2.m_normals 73 | 74 | assert(0 <= edge1 && edge1 < poly1.m_count) 75 | 76 | // Get the normal of the reference edge in poly2's frame. 77 | let normal1 = b2MulT(xf2.q, b2Mul(xf1.q, normals1[edge1])) 78 | 79 | // Find the incident edge on poly2. 80 | var index = 0 81 | var minDot = b2_maxFloat 82 | for i in 0 ..< count2 { 83 | let dot = b2Dot(normal1, normals2[i]) 84 | if dot < minDot { 85 | minDot = dot 86 | index = i 87 | } 88 | } 89 | 90 | // Build the clip vertices for the incident edge. 91 | let i1 = index 92 | let i2 = i1 + 1 < count2 ? i1 + 1 : 0 93 | 94 | var c = [b2ClipVertex](repeating: b2ClipVertex(), count: 2) 95 | c[0].v = b2Mul(xf2, vertices2[i1]) 96 | c[0].id.indexA = UInt8(edge1) 97 | c[0].id.indexB = UInt8(i1) 98 | c[0].id.typeA = b2ContactFeatureType.face 99 | c[0].id.typeB = b2ContactFeatureType.vertex 100 | 101 | c[1].v = b2Mul(xf2, vertices2[i2]) 102 | c[1].id.indexA = UInt8(edge1) 103 | c[1].id.indexB = UInt8(i2) 104 | c[1].id.typeA = b2ContactFeatureType.face 105 | c[1].id.typeB = b2ContactFeatureType.vertex 106 | return c 107 | } 108 | 109 | /// Compute the collision manifold between two polygons. 110 | // Find edge normal of max separation on A - return if separating axis is found 111 | // Find edge normal of max separation on B - return if separation axis is found 112 | // Choose reference edge as min(minA, minB) 113 | // Find incident edge 114 | // Clip 115 | // The normal points from 1 to 2 116 | public func b2CollidePolygons( 117 | manifold: inout b2Manifold, 118 | polygonA polyA: b2PolygonShape, transformA xfA: b2Transform, 119 | polygonB polyB: b2PolygonShape, transformB xfB: b2Transform) 120 | { 121 | manifold.points.removeAll(keepingCapacity: true) 122 | let totalRadius = polyA.m_radius + polyB.m_radius 123 | 124 | let (edgeA, separationA) = b2FindMaxSeparation(polyA, xf1: xfA, poly2: polyB, xf2: xfB) 125 | if separationA > totalRadius { 126 | return 127 | } 128 | 129 | let (edgeB, separationB) = b2FindMaxSeparation(polyB, xf1: xfB, poly2: polyA, xf2: xfA) 130 | if separationB > totalRadius { 131 | return 132 | } 133 | 134 | var poly1: b2PolygonShape // reference polygon 135 | var poly2: b2PolygonShape // incident polygon 136 | var xf1: b2Transform, xf2: b2Transform 137 | var edge1: Int // reference edge 138 | var flip: Bool 139 | let k_tol: b2Float = 0.1 * b2_linearSlop 140 | 141 | if separationB > separationA + k_tol { 142 | poly1 = polyB 143 | poly2 = polyA 144 | xf1 = xfB 145 | xf2 = xfA 146 | edge1 = edgeB 147 | manifold.type = b2ManifoldType.faceB 148 | flip = true 149 | } 150 | else { 151 | poly1 = polyA 152 | poly2 = polyB 153 | xf1 = xfA 154 | xf2 = xfB 155 | edge1 = edgeA 156 | manifold.type = b2ManifoldType.faceA 157 | flip = false 158 | } 159 | 160 | let incidentEdge = b2FindIncidentEdge(poly1, xf1: xf1, edge1: edge1, poly2: poly2, xf2: xf2) 161 | 162 | let count1 = poly1.m_count 163 | let vertices1 = poly1.m_vertices 164 | 165 | let iv1 = edge1 166 | let iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0 167 | 168 | var v11 = vertices1[iv1] 169 | var v12 = vertices1[iv2] 170 | 171 | var localTangent = v12 - v11 172 | localTangent.normalize() 173 | 174 | let localNormal = b2Cross(localTangent, 1.0) 175 | let planePoint = 0.5 * (v11 + v12) 176 | 177 | let tangent = b2Mul(xf1.q, localTangent) 178 | let normal = b2Cross(tangent, 1.0) 179 | 180 | v11 = b2Mul(xf1, v11) 181 | v12 = b2Mul(xf1, v12) 182 | 183 | // Face offset. 184 | let frontOffset = b2Dot(normal, v11) 185 | 186 | // Side offsets, extended by polytope skin thickness. 187 | let sideOffset1 = -b2Dot(tangent, v11) + totalRadius 188 | let sideOffset2 = b2Dot(tangent, v12) + totalRadius 189 | 190 | // Clip incident edge against extruded edge1 side edges. 191 | // Clip to box side 1 192 | let clipPoints1 = b2ClipSegmentToLine(inputVertices: incidentEdge, normal: -tangent, offset: sideOffset1, vertexIndexA: iv1) 193 | 194 | if clipPoints1.count < 2 { 195 | return 196 | } 197 | 198 | // Clip to negative box side 1 199 | let clipPoints2 = b2ClipSegmentToLine(inputVertices: clipPoints1, normal: tangent, offset: sideOffset2, vertexIndexA: iv2) 200 | 201 | if clipPoints2.count < 2 { 202 | return 203 | } 204 | 205 | // Now clipPoints2 contains the clipped points. 206 | manifold.localNormal = localNormal 207 | manifold.localPoint = planePoint 208 | 209 | var pointCount = 0 210 | for i in 0 ..< b2_maxManifoldPoints { 211 | let separation = b2Dot(normal, clipPoints2[i].v) - frontOffset 212 | 213 | if separation <= totalRadius { 214 | let cp = b2ManifoldPoint() // manifold.points[pointCount] 215 | cp.localPoint = b2MulT(xf2, clipPoints2[i].v) 216 | cp.id = clipPoints2[i].id 217 | if flip { 218 | // Swap features 219 | let cf = cp.id 220 | cp.id.indexA = cf.indexB 221 | cp.id.indexB = cf.indexA 222 | cp.id.typeA = cf.typeB 223 | cp.id.typeB = cf.typeA 224 | } 225 | manifold.points.append(cp) 226 | pointCount += 1 227 | } 228 | } 229 | return 230 | } 231 | -------------------------------------------------------------------------------- /Sources/Box2D/Common/b2Common.swift: -------------------------------------------------------------------------------- 1 | /** 2 | Copyright (c) 2006-2014 Erin Catto http://www.box2d.org 3 | Copyright (c) 2015 - Yohei Yoshihara 4 | 5 | This software is provided 'as-is', without any express or implied 6 | warranty. In no event will the authors be held liable for any damages 7 | arising from the use of this software. 8 | 9 | Permission is granted to anyone to use this software for any purpose, 10 | including commercial applications, and to alter it and redistribute it 11 | freely, subject to the following restrictions: 12 | 13 | 1. The origin of this software must not be misrepresented; you must not 14 | claim that you wrote the original software. If you use this software 15 | in a product, an acknowledgment in the product documentation would be 16 | appreciated but is not required. 17 | 18 | 2. Altered source versions must be plainly marked as such, and must not be 19 | misrepresented as being the original software. 20 | 21 | 3. This notice may not be removed or altered from any source distribution. 22 | 23 | This version of box2d was developed by Yohei Yoshihara. It is based upon 24 | the original C++ code written by Erin Catto. 25 | */ 26 | 27 | import Foundation 28 | 29 | // local scope 30 | // http://stackoverflow.com/questions/24011271/how-to-create-local-scopes-in-swift 31 | public func b2Locally(_ work: () -> ()) { 32 | work() 33 | } 34 | 35 | -------------------------------------------------------------------------------- /Sources/Box2D/Common/b2Draw.swift: -------------------------------------------------------------------------------- 1 | /** 2 | Copyright (c) 2006-2014 Erin Catto http://www.box2d.org 3 | Copyright (c) 2015 - Yohei Yoshihara 4 | 5 | This software is provided 'as-is', without any express or implied 6 | warranty. In no event will the authors be held liable for any damages 7 | arising from the use of this software. 8 | 9 | Permission is granted to anyone to use this software for any purpose, 10 | including commercial applications, and to alter it and redistribute it 11 | freely, subject to the following restrictions: 12 | 13 | 1. The origin of this software must not be misrepresented; you must not 14 | claim that you wrote the original software. If you use this software 15 | in a product, an acknowledgment in the product documentation would be 16 | appreciated but is not required. 17 | 18 | 2. Altered source versions must be plainly marked as such, and must not be 19 | misrepresented as being the original software. 20 | 21 | 3. This notice may not be removed or altered from any source distribution. 22 | 23 | This version of box2d was developed by Yohei Yoshihara. It is based upon 24 | the original C++ code written by Erin Catto. 25 | */ 26 | 27 | import Foundation 28 | 29 | /// Color for debug drawing. Each value has the range [0,1]. 30 | // type checked 31 | public struct b2Color : CustomStringConvertible { 32 | public var r: Float, g: Float, b: Float 33 | public init() { 34 | r = 0.0 35 | g = 0.0 36 | b = 0.0 37 | } 38 | public init(_ r: Float, _ g: Float, _ b: Float) { 39 | self.r = r 40 | self.g = g 41 | self.b = b 42 | } 43 | public mutating func set(_ r: Float, _ g: Float, _ b: Float) { 44 | self.r = r 45 | self.g = g 46 | self.b = b 47 | } 48 | public var description: String { 49 | return "b2Color[\(r),\(g),\(b)]" 50 | } 51 | } 52 | 53 | public struct b2DrawFlags { 54 | /// draw shapes 55 | public static let shapeBit: UInt32 = 0x0001 56 | /// draw joint connections 57 | public static let jointBit: UInt32 = 0x0002 58 | /// draw axis aligned bounding boxes 59 | public static let aabbBit: UInt32 = 0x0004 60 | /// draw broad-phase pairs 61 | public static let pairBit: UInt32 = 0x0008 62 | /// draw center of mass frame 63 | public static let centerOfMassBit: UInt32 = 0x0010 64 | } 65 | 66 | /// Implement and register this class with a b2World to provide debug drawing of physics 67 | /// entities in your game. 68 | // type checked 69 | public protocol b2Draw { 70 | /// Get the drawing flags. 71 | var flags: UInt32 { get } 72 | 73 | /// Draw a closed polygon provided in CCW order. 74 | func drawPolygon(_ vertices: [b2Vec2], _ color: b2Color) 75 | 76 | /// Draw a solid closed polygon provided in CCW order. 77 | func drawSolidPolygon(_ vertices: [b2Vec2], _ color: b2Color) 78 | 79 | /// Draw a circle. 80 | func drawCircle(_ center: b2Vec2, _ radius: b2Float, _ color: b2Color) 81 | 82 | /// Draw a solid circle. 83 | func drawSolidCircle(_ center: b2Vec2, _ radius: b2Float, _ axis: b2Vec2, _ color: b2Color) 84 | 85 | /// Draw a line segment. 86 | func drawSegment(_ p1: b2Vec2, _ p2: b2Vec2, _ color: b2Color) 87 | 88 | /** 89 | Draw a transform. Choose your own length scale. 90 | 91 | - parameter xf: a transform. 92 | */ 93 | func drawTransform(_ xf: b2Transform) 94 | } 95 | 96 | -------------------------------------------------------------------------------- /Sources/Box2D/Common/b2GrowableStack.swift: -------------------------------------------------------------------------------- 1 | /** 2 | Copyright (c) 2006-2014 Erin Catto http://www.box2d.org 3 | Copyright (c) 2015 - Yohei Yoshihara 4 | 5 | This software is provided 'as-is', without any express or implied 6 | warranty. In no event will the authors be held liable for any damages 7 | arising from the use of this software. 8 | 9 | Permission is granted to anyone to use this software for any purpose, 10 | including commercial applications, and to alter it and redistribute it 11 | freely, subject to the following restrictions: 12 | 13 | 1. The origin of this software must not be misrepresented; you must not 14 | claim that you wrote the original software. If you use this software 15 | in a product, an acknowledgment in the product documentation would be 16 | appreciated but is not required. 17 | 18 | 2. Altered source versions must be plainly marked as such, and must not be 19 | misrepresented as being the original software. 20 | 21 | 3. This notice may not be removed or altered from any source distribution. 22 | 23 | This version of box2d was developed by Yohei Yoshihara. It is based upon 24 | the original C++ code written by Erin Catto. 25 | */ 26 | 27 | import Foundation 28 | 29 | public struct b2GrowableStack { 30 | init() { 31 | m_array = [T]() 32 | } 33 | init(capacity: Int) { 34 | m_array = [T]() 35 | m_array.reserveCapacity(capacity) 36 | } 37 | 38 | mutating func push(_ element: T) { 39 | m_array.append(element) 40 | } 41 | 42 | mutating func pop() -> T { 43 | return m_array.removeLast() 44 | } 45 | 46 | var count: Int { 47 | return m_array.count 48 | } 49 | 50 | fileprivate var m_array = [T]() 51 | } 52 | -------------------------------------------------------------------------------- /Sources/Box2D/Common/b2Settings.swift: -------------------------------------------------------------------------------- 1 | /** 2 | Copyright (c) 2006-2014 Erin Catto http://www.box2d.org 3 | Copyright (c) 2015 - Yohei Yoshihara 4 | 5 | This software is provided 'as-is', without any express or implied 6 | warranty. In no event will the authors be held liable for any damages 7 | arising from the use of this software. 8 | 9 | Permission is granted to anyone to use this software for any purpose, 10 | including commercial applications, and to alter it and redistribute it 11 | freely, subject to the following restrictions: 12 | 13 | 1. The origin of this software must not be misrepresented; you must not 14 | claim that you wrote the original software. If you use this software 15 | in a product, an acknowledgment in the product documentation would be 16 | appreciated but is not required. 17 | 18 | 2. Altered source versions must be plainly marked as such, and must not be 19 | misrepresented as being the original software. 20 | 21 | 3. This notice may not be removed or altered from any source distribution. 22 | 23 | This version of box2d was developed by Yohei Yoshihara. It is based upon 24 | the original C++ code written by Erin Catto. 25 | */ 26 | 27 | import Foundation 28 | 29 | public let b2_minFloat = Float.leastNormalMagnitude 30 | public let b2_maxFloat = Float.greatestFiniteMagnitude 31 | public let b2_epsilon = Float.ulpOfOne 32 | public let b2_pi: b2Float = Float.pi 33 | 34 | /// @file 35 | /// Global tuning constants based on meters-kilograms-seconds (MKS) units. 36 | /// 37 | 38 | // Collision 39 | 40 | /// The maximum number of contact points between two convex shapes. Do 41 | /// not change this value. 42 | public let b2_maxManifoldPoints = 2 43 | 44 | /// The maximum number of vertices on a convex polygon. You cannot increase 45 | /// this too much because b2BlockAllocator has a maximum object size. 46 | public let b2_maxPolygonVertices = 8 47 | 48 | /// This is used to fatten AABBs in the dynamic tree. This allows proxies 49 | /// to move by a small amount without triggering a tree adjustment. 50 | /// This is in meters. 51 | public let b2_aabbExtension: b2Float = 0.1 52 | 53 | /// This is used to fatten AABBs in the dynamic tree. This is used to predict 54 | /// the future position based on the current displacement. 55 | /// This is a dimensionless multiplier. 56 | public let b2_aabbMultiplier: b2Float = 2.0 57 | 58 | /// A small length used as a collision and constraint tolerance. Usually it is 59 | /// chosen to be numerically significant, but visually insignificant. 60 | public let b2_linearSlop: b2Float = 0.005 61 | 62 | /// A small angle used as a collision and constraint tolerance. Usually it is 63 | /// chosen to be numerically significant, but visually insignificant. 64 | public let b2_angularSlop: b2Float = (2.0 / 180.0 * b2_pi) 65 | 66 | /// The radius of the polygon/edge shape skin. This should not be modified. Making 67 | /// this smaller means polygons will have an insufficient buffer for continuous collision. 68 | /// Making it larger may create artifacts for vertex collision. 69 | public let b2_polygonRadius: b2Float = (2.0 * b2_linearSlop) 70 | 71 | /// Maximum number of sub-steps per contact in continuous physics simulation. 72 | public let b2_maxSubSteps = 8 73 | 74 | 75 | // Dynamics 76 | 77 | /// Maximum number of contacts to be handled to solve a TOI impact. 78 | public let b2_maxTOIContacts = 32 79 | 80 | /// A velocity threshold for elastic collisions. Any collision with a relative linear 81 | /// velocity below this threshold will be treated as inelastic. 82 | public let b2_velocityThreshold: b2Float = 1.0 83 | 84 | /// The maximum linear position correction used when solving constraints. This helps to 85 | /// prevent overshoot. 86 | public let b2_maxLinearCorrection: b2Float = 0.2 87 | 88 | /// The maximum angular position correction used when solving constraints. This helps to 89 | /// prevent overshoot. 90 | public let b2_maxAngularCorrection: b2Float = (8.0 / 180.0 * b2_pi) 91 | 92 | /// The maximum linear velocity of a body. This limit is very large and is used 93 | /// to prevent numerical problems. You shouldn't need to adjust this. 94 | public let b2_maxTranslation: b2Float = 2.0 95 | public let b2_maxTranslationSquared: b2Float = (b2_maxTranslation * b2_maxTranslation) 96 | 97 | /// The maximum angular velocity of a body. This limit is very large and is used 98 | /// to prevent numerical problems. You shouldn't need to adjust this. 99 | public let b2_maxRotation: b2Float = (0.5 * b2_pi) 100 | public let b2_maxRotationSquared: b2Float = (b2_maxRotation * b2_maxRotation) 101 | 102 | /// This scale factor controls how fast overlap is resolved. Ideally this would be 1 so 103 | /// that overlap is removed in one time step. However using values close to 1 often lead 104 | /// to overshoot. 105 | public let b2_baumgarte: b2Float = 0.2 106 | public let b2_toiBaugarte: b2Float = 0.75 107 | 108 | 109 | // Sleep 110 | 111 | /// The time that a body must be still before it will go to sleep. 112 | public let b2_timeToSleep: b2Float = 0.5 113 | 114 | /// A body cannot sleep if its linear velocity is above this tolerance. 115 | public let b2_linearSleepTolerance: b2Float = 0.01 116 | 117 | /// A body cannot sleep if its angular velocity is above this tolerance. 118 | public let b2_angularSleepTolerance: b2Float = (2.0 / 180.0 * b2_pi) 119 | 120 | /// Version numbering scheme. 121 | /// See http://en.wikipedia.org/wiki/Software_versioning 122 | public struct b2Version 123 | { 124 | var major : Int ///< significant changes 125 | var minor : Int ///< incremental changes 126 | var revision : Int ///< bug fixes 127 | } 128 | 129 | /// Current version. 130 | public let b2_version = b2Version(major: 2, minor: 3, revision: 0) 131 | 132 | 133 | 134 | -------------------------------------------------------------------------------- /Sources/Box2D/Common/b2Timer.swift: -------------------------------------------------------------------------------- 1 | /** 2 | Copyright (c) 2006-2014 Erin Catto http://www.box2d.org 3 | Copyright (c) 2015 - Yohei Yoshihara 4 | 5 | This software is provided 'as-is', without any express or implied 6 | warranty. In no event will the authors be held liable for any damages 7 | arising from the use of this software. 8 | 9 | Permission is granted to anyone to use this software for any purpose, 10 | including commercial applications, and to alter it and redistribute it 11 | freely, subject to the following restrictions: 12 | 13 | 1. The origin of this software must not be misrepresented; you must not 14 | claim that you wrote the original software. If you use this software 15 | in a product, an acknowledgment in the product documentation would be 16 | appreciated but is not required. 17 | 18 | 2. Altered source versions must be plainly marked as such, and must not be 19 | misrepresented as being the original software. 20 | 21 | 3. This notice may not be removed or altered from any source distribution. 22 | 23 | This version of box2d was developed by Yohei Yoshihara. It is based upon 24 | the original C++ code written by Erin Catto. 25 | */ 26 | 27 | import Foundation 28 | import QuartzCore 29 | 30 | /// Timer for profiling. This has platform specific code and may 31 | /// not work on every platform. 32 | open class b2Timer { 33 | /// Constructor 34 | public init() { 35 | m_start = CACurrentMediaTime() 36 | } 37 | 38 | /// Reset the timer. 39 | open func reset() { 40 | m_start = CACurrentMediaTime() 41 | } 42 | 43 | /// Get the time since construction or the last reset. 44 | open var milliseconds: b2Float { 45 | return b2Float(CACurrentMediaTime() - m_start) * b2Float(1000.0) 46 | } 47 | 48 | var m_start: CFTimeInterval 49 | } 50 | -------------------------------------------------------------------------------- /Sources/Box2D/Common/b2Wrappers.swift: -------------------------------------------------------------------------------- 1 | /** 2 | Copyright (c) 2006-2014 Erin Catto http://www.box2d.org 3 | Copyright (c) 2015 - Yohei Yoshihara 4 | 5 | This software is provided 'as-is', without any express or implied 6 | warranty. In no event will the authors be held liable for any damages 7 | arising from the use of this software. 8 | 9 | Permission is granted to anyone to use this software for any purpose, 10 | including commercial applications, and to alter it and redistribute it 11 | freely, subject to the following restrictions: 12 | 13 | 1. The origin of this software must not be misrepresented; you must not 14 | claim that you wrote the original software. If you use this software 15 | in a product, an acknowledgment in the product documentation would be 16 | appreciated but is not required. 17 | 18 | 2. Altered source versions must be plainly marked as such, and must not be 19 | misrepresented as being the original software. 20 | 21 | 3. This notice may not be removed or altered from any source distribution. 22 | 23 | This version of box2d was developed by Yohei Yoshihara. It is based upon 24 | the original C++ code written by Erin Catto. 25 | */ 26 | 27 | import Foundation 28 | 29 | public protocol b2QueryWrapper { 30 | func queryCallback(_ proxyId: Int) -> Bool 31 | } 32 | 33 | public protocol b2RayCastWrapper { 34 | func rayCastCallback(_ input: b2RayCastInput, _ proxyId: Int) -> b2Float 35 | } 36 | 37 | public protocol b2BroadPhaseWrapper { 38 | func addPair(_ proxyUserDataA: inout b2FixtureProxy, _ proxyUserDataB: inout b2FixtureProxy) 39 | } 40 | 41 | -------------------------------------------------------------------------------- /Sources/Box2D/Dynamics/Contacts/b2ChainAndCircleContact.swift: -------------------------------------------------------------------------------- 1 | /** 2 | Copyright (c) 2006-2014 Erin Catto http://www.box2d.org 3 | Copyright (c) 2015 - Yohei Yoshihara 4 | 5 | This software is provided 'as-is', without any express or implied 6 | warranty. In no event will the authors be held liable for any damages 7 | arising from the use of this software. 8 | 9 | Permission is granted to anyone to use this software for any purpose, 10 | including commercial applications, and to alter it and redistribute it 11 | freely, subject to the following restrictions: 12 | 13 | 1. The origin of this software must not be misrepresented; you must not 14 | claim that you wrote the original software. If you use this software 15 | in a product, an acknowledgment in the product documentation would be 16 | appreciated but is not required. 17 | 18 | 2. Altered source versions must be plainly marked as such, and must not be 19 | misrepresented as being the original software. 20 | 21 | 3. This notice may not be removed or altered from any source distribution. 22 | 23 | This version of box2d was developed by Yohei Yoshihara. It is based upon 24 | the original C++ code written by Erin Catto. 25 | */ 26 | 27 | import Foundation 28 | 29 | open class b2ChainAndCircleContact : b2Contact { 30 | override class func create(_ fixtureA: b2Fixture, _ indexA: Int, _ fixtureB: b2Fixture, _ indexB: Int) -> b2Contact { 31 | return b2ChainAndCircleContact(fixtureA, indexA, fixtureB, indexB) 32 | } 33 | override class func destroy(_ contact: b2Contact) { 34 | } 35 | 36 | override init(_ fixtureA: b2Fixture, _ indexA: Int, _ fixtureB: b2Fixture, _ indexB: Int) { 37 | super.init(fixtureA, indexA, fixtureB, indexB) 38 | assert(m_fixtureA.type == b2ShapeType.chain) 39 | assert(m_fixtureB.type == b2ShapeType.circle) 40 | } 41 | 42 | override open func evaluate(_ manifold: inout b2Manifold, _ xfA: b2Transform, _ xfB: b2Transform) { 43 | let chain = m_fixtureA.shape as! b2ChainShape 44 | let edge = chain.getChildEdge(m_indexA) 45 | b2CollideEdgeAndCircle(manifold: &manifold, 46 | edgeA: edge, transformA: xfA, 47 | circleB: m_fixtureB.shape as! b2CircleShape, transformB: xfB) 48 | } 49 | } 50 | -------------------------------------------------------------------------------- /Sources/Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.swift: -------------------------------------------------------------------------------- 1 | /** 2 | Copyright (c) 2006-2014 Erin Catto http://www.box2d.org 3 | Copyright (c) 2015 - Yohei Yoshihara 4 | 5 | This software is provided 'as-is', without any express or implied 6 | warranty. In no event will the authors be held liable for any damages 7 | arising from the use of this software. 8 | 9 | Permission is granted to anyone to use this software for any purpose, 10 | including commercial applications, and to alter it and redistribute it 11 | freely, subject to the following restrictions: 12 | 13 | 1. The origin of this software must not be misrepresented; you must not 14 | claim that you wrote the original software. If you use this software 15 | in a product, an acknowledgment in the product documentation would be 16 | appreciated but is not required. 17 | 18 | 2. Altered source versions must be plainly marked as such, and must not be 19 | misrepresented as being the original software. 20 | 21 | 3. This notice may not be removed or altered from any source distribution. 22 | 23 | This version of box2d was developed by Yohei Yoshihara. It is based upon 24 | the original C++ code written by Erin Catto. 25 | */ 26 | 27 | import Foundation 28 | 29 | open class b2ChainAndPolygonContact : b2Contact { 30 | override class func create(_ fixtureA: b2Fixture, _ indexA: Int, _ fixtureB: b2Fixture, _ indexB: Int) -> b2Contact { 31 | return b2ChainAndPolygonContact(fixtureA, indexA, fixtureB, indexB) 32 | } 33 | 34 | override class func destroy(_ contact: b2Contact) { 35 | } 36 | 37 | override init(_ fixtureA: b2Fixture, _ indexA: Int, _ fixtureB: b2Fixture, _ indexB: Int) { 38 | super.init(fixtureA, indexA, fixtureB, indexB) 39 | assert(m_fixtureA.type == b2ShapeType.chain) 40 | assert(m_fixtureB.type == b2ShapeType.polygon) 41 | } 42 | 43 | override open func evaluate(_ manifold: inout b2Manifold, _ xfA: b2Transform, _ xfB: b2Transform) { 44 | let chain = m_fixtureA.shape as! b2ChainShape 45 | let edge = chain.getChildEdge(m_indexA) 46 | b2CollideEdgeAndPolygon( 47 | manifold: &manifold, 48 | edgeA: edge, transformA: xfA, 49 | polygonB: m_fixtureB.shape as! b2PolygonShape, transformB: xfB) 50 | } 51 | } 52 | -------------------------------------------------------------------------------- /Sources/Box2D/Dynamics/Contacts/b2CircleContact.swift: -------------------------------------------------------------------------------- 1 | /** 2 | Copyright (c) 2006-2014 Erin Catto http://www.box2d.org 3 | Copyright (c) 2015 - Yohei Yoshihara 4 | 5 | This software is provided 'as-is', without any express or implied 6 | warranty. In no event will the authors be held liable for any damages 7 | arising from the use of this software. 8 | 9 | Permission is granted to anyone to use this software for any purpose, 10 | including commercial applications, and to alter it and redistribute it 11 | freely, subject to the following restrictions: 12 | 13 | 1. The origin of this software must not be misrepresented; you must not 14 | claim that you wrote the original software. If you use this software 15 | in a product, an acknowledgment in the product documentation would be 16 | appreciated but is not required. 17 | 18 | 2. Altered source versions must be plainly marked as such, and must not be 19 | misrepresented as being the original software. 20 | 21 | 3. This notice may not be removed or altered from any source distribution. 22 | 23 | This version of box2d was developed by Yohei Yoshihara. It is based upon 24 | the original C++ code written by Erin Catto. 25 | */ 26 | 27 | import Foundation 28 | 29 | open class b2CircleContact : b2Contact { 30 | override class func create(_ fixtureA: b2Fixture, _ indexA: Int, _ fixtureB: b2Fixture, _ indexB: Int) -> b2Contact { 31 | return b2CircleContact(fixtureA, fixtureB) 32 | } 33 | 34 | override class func destroy(_ contact: b2Contact) { 35 | } 36 | 37 | init(_ fixtureA: b2Fixture, _ fixtureB: b2Fixture) { 38 | super.init(fixtureA, 0, fixtureB, 0) 39 | assert(m_fixtureA.type == b2ShapeType.circle) 40 | assert(m_fixtureB.type == b2ShapeType.circle) 41 | } 42 | 43 | override open func evaluate(_ manifold: inout b2Manifold, _ xfA: b2Transform, _ xfB: b2Transform) { 44 | b2CollideCircles(manifold: &manifold, 45 | circleA: m_fixtureA.shape as! b2CircleShape, transformA: xfA, 46 | circleB: m_fixtureB.shape as! b2CircleShape, transformB: xfB) 47 | } 48 | } 49 | -------------------------------------------------------------------------------- /Sources/Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.swift: -------------------------------------------------------------------------------- 1 | /** 2 | Copyright (c) 2006-2014 Erin Catto http://www.box2d.org 3 | Copyright (c) 2015 - Yohei Yoshihara 4 | 5 | This software is provided 'as-is', without any express or implied 6 | warranty. In no event will the authors be held liable for any damages 7 | arising from the use of this software. 8 | 9 | Permission is granted to anyone to use this software for any purpose, 10 | including commercial applications, and to alter it and redistribute it 11 | freely, subject to the following restrictions: 12 | 13 | 1. The origin of this software must not be misrepresented; you must not 14 | claim that you wrote the original software. If you use this software 15 | in a product, an acknowledgment in the product documentation would be 16 | appreciated but is not required. 17 | 18 | 2. Altered source versions must be plainly marked as such, and must not be 19 | misrepresented as being the original software. 20 | 21 | 3. This notice may not be removed or altered from any source distribution. 22 | 23 | This version of box2d was developed by Yohei Yoshihara. It is based upon 24 | the original C++ code written by Erin Catto. 25 | */ 26 | 27 | import Foundation 28 | 29 | open class b2EdgeAndCircleContact : b2Contact { 30 | override class func create(_ fixtureA: b2Fixture, _ indexA: Int, _ fixtureB: b2Fixture, _ indexB: Int) -> b2Contact { 31 | return b2EdgeAndCircleContact(fixtureA, fixtureB) 32 | } 33 | 34 | override class func destroy(_ contact: b2Contact) { 35 | } 36 | 37 | init(_ fixtureA: b2Fixture, _ fixtureB: b2Fixture) { 38 | super.init(fixtureA, 0, fixtureB, 0) 39 | assert(m_fixtureA.type == b2ShapeType.edge) 40 | assert(m_fixtureB.type == b2ShapeType.circle) 41 | } 42 | 43 | override open func evaluate(_ manifold: inout b2Manifold, _ xfA: b2Transform, _ xfB: b2Transform) { 44 | b2CollideEdgeAndCircle(manifold: &manifold, 45 | edgeA: m_fixtureA.shape as! b2EdgeShape, transformA: xfA, 46 | circleB: m_fixtureB.shape as! b2CircleShape, transformB: xfB) 47 | } 48 | } 49 | -------------------------------------------------------------------------------- /Sources/Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.swift: -------------------------------------------------------------------------------- 1 | /** 2 | Copyright (c) 2006-2014 Erin Catto http://www.box2d.org 3 | Copyright (c) 2015 - Yohei Yoshihara 4 | 5 | This software is provided 'as-is', without any express or implied 6 | warranty. In no event will the authors be held liable for any damages 7 | arising from the use of this software. 8 | 9 | Permission is granted to anyone to use this software for any purpose, 10 | including commercial applications, and to alter it and redistribute it 11 | freely, subject to the following restrictions: 12 | 13 | 1. The origin of this software must not be misrepresented; you must not 14 | claim that you wrote the original software. If you use this software 15 | in a product, an acknowledgment in the product documentation would be 16 | appreciated but is not required. 17 | 18 | 2. Altered source versions must be plainly marked as such, and must not be 19 | misrepresented as being the original software. 20 | 21 | 3. This notice may not be removed or altered from any source distribution. 22 | 23 | This version of box2d was developed by Yohei Yoshihara. It is based upon 24 | the original C++ code written by Erin Catto. 25 | */ 26 | 27 | import Foundation 28 | 29 | open class b2EdgeAndPolygonContact : b2Contact { 30 | override class func create(_ fixtureA: b2Fixture, _ indexA: Int, _ fixtureB: b2Fixture, _ indexB: Int) -> b2Contact { 31 | return b2EdgeAndPolygonContact(fixtureA, fixtureB) 32 | } 33 | 34 | override class func destroy(_ contact: b2Contact) { 35 | } 36 | 37 | init(_ fixtureA: b2Fixture, _ fixtureB: b2Fixture) { 38 | super.init(fixtureA, 0, fixtureB, 0) 39 | assert(m_fixtureA.type == b2ShapeType.edge) 40 | assert(m_fixtureB.type == b2ShapeType.polygon) 41 | } 42 | 43 | override open func evaluate(_ manifold: inout b2Manifold, _ xfA: b2Transform, _ xfB: b2Transform) { 44 | b2CollideEdgeAndPolygon( 45 | manifold: &manifold, 46 | edgeA: m_fixtureA.shape as! b2EdgeShape, transformA: xfA, 47 | polygonB: m_fixtureB.shape as! b2PolygonShape, transformB: xfB) 48 | } 49 | } 50 | -------------------------------------------------------------------------------- /Sources/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.swift: -------------------------------------------------------------------------------- 1 | /** 2 | Copyright (c) 2006-2014 Erin Catto http://www.box2d.org 3 | Copyright (c) 2015 - Yohei Yoshihara 4 | 5 | This software is provided 'as-is', without any express or implied 6 | warranty. In no event will the authors be held liable for any damages 7 | arising from the use of this software. 8 | 9 | Permission is granted to anyone to use this software for any purpose, 10 | including commercial applications, and to alter it and redistribute it 11 | freely, subject to the following restrictions: 12 | 13 | 1. The origin of this software must not be misrepresented; you must not 14 | claim that you wrote the original software. If you use this software 15 | in a product, an acknowledgment in the product documentation would be 16 | appreciated but is not required. 17 | 18 | 2. Altered source versions must be plainly marked as such, and must not be 19 | misrepresented as being the original software. 20 | 21 | 3. This notice may not be removed or altered from any source distribution. 22 | 23 | This version of box2d was developed by Yohei Yoshihara. It is based upon 24 | the original C++ code written by Erin Catto. 25 | */ 26 | 27 | import Foundation 28 | 29 | open class b2PolygonAndCircleContact : b2Contact { 30 | override class func create(_ fixtureA: b2Fixture, _ indexA: Int, _ fixtureB: b2Fixture, _ indexB: Int) -> b2Contact { 31 | return b2PolygonAndCircleContact(fixtureA, fixtureB) 32 | } 33 | override class func destroy(_ contact: b2Contact) { 34 | } 35 | 36 | init(_ fixtureA : b2Fixture, _ fixtureB : b2Fixture) { 37 | super.init(fixtureA, 0, fixtureB, 0) 38 | assert(m_fixtureA.type == b2ShapeType.polygon) 39 | assert(m_fixtureB.type == b2ShapeType.circle) 40 | } 41 | 42 | override open func evaluate(_ manifold: inout b2Manifold, _ xfA: b2Transform, _ xfB: b2Transform) { 43 | b2CollidePolygonAndCircle(manifold: &manifold, 44 | polygonA: m_fixtureA.shape as! b2PolygonShape, transformA: xfA, 45 | circleB: m_fixtureB.shape as! b2CircleShape, transformB: xfB) 46 | } 47 | } 48 | -------------------------------------------------------------------------------- /Sources/Box2D/Dynamics/Contacts/b2PolygonContact.swift: -------------------------------------------------------------------------------- 1 | /** 2 | Copyright (c) 2006-2014 Erin Catto http://www.box2d.org 3 | Copyright (c) 2015 - Yohei Yoshihara 4 | 5 | This software is provided 'as-is', without any express or implied 6 | warranty. In no event will the authors be held liable for any damages 7 | arising from the use of this software. 8 | 9 | Permission is granted to anyone to use this software for any purpose, 10 | including commercial applications, and to alter it and redistribute it 11 | freely, subject to the following restrictions: 12 | 13 | 1. The origin of this software must not be misrepresented; you must not 14 | claim that you wrote the original software. If you use this software 15 | in a product, an acknowledgment in the product documentation would be 16 | appreciated but is not required. 17 | 18 | 2. Altered source versions must be plainly marked as such, and must not be 19 | misrepresented as being the original software. 20 | 21 | 3. This notice may not be removed or altered from any source distribution. 22 | 23 | This version of box2d was developed by Yohei Yoshihara. It is based upon 24 | the original C++ code written by Erin Catto. 25 | */ 26 | 27 | import Foundation 28 | 29 | open class b2PolygonContact : b2Contact { 30 | override class func create(_ fixtureA: b2Fixture, _ indexA: Int, _ fixtureB: b2Fixture, _ indexB: Int) -> b2Contact { 31 | return b2PolygonContact(fixtureA, fixtureB) 32 | } 33 | override class func destroy(_ contact: b2Contact) { 34 | } 35 | 36 | init(_ fixtureA: b2Fixture, _ fixtureB: b2Fixture) { 37 | super.init(fixtureA, 0, fixtureB, 0) 38 | assert(m_fixtureA.type == b2ShapeType.polygon) 39 | assert(m_fixtureB.type == b2ShapeType.polygon) 40 | } 41 | 42 | override open func evaluate(_ manifold: inout b2Manifold, _ xfA: b2Transform, _ xfB: b2Transform) { 43 | b2CollidePolygons(manifold: &manifold, 44 | polygonA: m_fixtureA.shape as! b2PolygonShape, transformA: xfA, 45 | polygonB: m_fixtureB.shape as! b2PolygonShape, transformB: xfB) 46 | } 47 | } 48 | -------------------------------------------------------------------------------- /Sources/Box2D/Dynamics/Joints/b2DistanceJoint.swift: -------------------------------------------------------------------------------- 1 | /** 2 | Copyright (c) 2006-2014 Erin Catto http://www.box2d.org 3 | Copyright (c) 2015 - Yohei Yoshihara 4 | 5 | This software is provided 'as-is', without any express or implied 6 | warranty. In no event will the authors be held liable for any damages 7 | arising from the use of this software. 8 | 9 | Permission is granted to anyone to use this software for any purpose, 10 | including commercial applications, and to alter it and redistribute it 11 | freely, subject to the following restrictions: 12 | 13 | 1. The origin of this software must not be misrepresented; you must not 14 | claim that you wrote the original software. If you use this software 15 | in a product, an acknowledgment in the product documentation would be 16 | appreciated but is not required. 17 | 18 | 2. Altered source versions must be plainly marked as such, and must not be 19 | misrepresented as being the original software. 20 | 21 | 3. This notice may not be removed or altered from any source distribution. 22 | 23 | This version of box2d was developed by Yohei Yoshihara. It is based upon 24 | the original C++ code written by Erin Catto. 25 | */ 26 | 27 | import Foundation 28 | 29 | /// Distance joint definition. This requires defining an 30 | /// anchor point on both bodies and the non-zero length of the 31 | /// distance joint. The definition uses local anchor points 32 | /// so that the initial configuration can violate the constraint 33 | /// slightly. This helps when saving and loading a game. 34 | /// @warning Do not use a zero or short length. 35 | open class b2DistanceJointDef : b2JointDef { 36 | public override init() { 37 | localAnchorA = b2Vec2(0.0, 0.0) 38 | localAnchorB = b2Vec2(0.0, 0.0) 39 | length = 1.0 40 | frequencyHz = 0.0 41 | dampingRatio = 0.0 42 | super.init() 43 | type = b2JointType.distanceJoint 44 | } 45 | 46 | /// Initialize the bodies, anchors, and length using the world 47 | /// anchors. 48 | public convenience init(bodyA: b2Body, bodyB: b2Body, anchorA: b2Vec2, anchorB: b2Vec2) { 49 | self.init() 50 | initialize(bodyA: bodyA, bodyB: bodyB, anchorA: anchorA, anchorB: anchorB) 51 | } 52 | 53 | /// Initialize the bodies, anchors, and length using the world 54 | /// anchors. 55 | open func initialize(bodyA bA: b2Body, bodyB bB: b2Body, anchorA: b2Vec2, anchorB: b2Vec2) { 56 | bodyA = bA 57 | bodyB = bB 58 | localAnchorA = bodyA.getLocalPoint(anchorA) 59 | localAnchorB = bodyB.getLocalPoint(anchorB) 60 | let d = anchorB - anchorA 61 | length = d.length() 62 | } 63 | 64 | /// The local anchor point relative to bodyA's origin. 65 | open var localAnchorA = b2Vec2() 66 | 67 | /// The local anchor point relative to bodyB's origin. 68 | open var localAnchorB = b2Vec2() 69 | 70 | /// The natural length between the anchor points. 71 | open var length: b2Float = 1.0 72 | 73 | /// The mass-spring-damper frequency in Hertz. A value of 0 74 | /// disables softness. 75 | open var frequencyHz: b2Float = 0.0 76 | 77 | /// The damping ratio. 0 = no damping, 1 = critical damping. 78 | open var dampingRatio: b2Float = 0.0 79 | } 80 | 81 | // MARK: - 82 | /// A distance joint constrains two points on two bodies 83 | /// to remain at a fixed distance from each other. You can view 84 | /// this as a massless, rigid rod. 85 | open class b2DistanceJoint : b2Joint { 86 | open override var anchorA: b2Vec2 { 87 | return m_bodyA.getWorldPoint(m_localAnchorA) 88 | } 89 | open override var anchorB: b2Vec2 { 90 | return m_bodyB.getWorldPoint(m_localAnchorB) 91 | } 92 | 93 | /// Get the reaction force given the inverse time step. 94 | /// Unitoverride is N. 95 | open override func getReactionForce(inverseTimeStep inv_dt: b2Float) -> b2Vec2 { 96 | let F = (inv_dt * m_impulse) * m_u 97 | return F 98 | } 99 | 100 | /// Get the reaction torque given the inverse time step. 101 | /// Unit is N*m. This is always zero for a distance joint. 102 | open override func getReactionTorque(inverseTimeStep inv_dt: b2Float) -> b2Float { 103 | return 0.0 104 | } 105 | 106 | /// The local anchor point relative to bodyA's origin. 107 | open var localAnchorA: b2Vec2 { return m_localAnchorA } 108 | 109 | /// The local anchor point relative to bodyB's origin. 110 | open var localAnchorB: b2Vec2 { return m_localAnchorB } 111 | 112 | /// Set/get the natural length. 113 | /// Manipulating the length can lead to non-physical behavior when the frequency is zero. 114 | open func setLength(_ length: b2Float) { 115 | m_length = length 116 | } 117 | open var length: b2Float { 118 | return m_length 119 | } 120 | 121 | /// Set/get frequency in Hz. 122 | open func setFrequency(_ hz: b2Float) { 123 | m_frequencyHz = hz 124 | } 125 | open var frequency: b2Float { 126 | return m_frequencyHz 127 | } 128 | 129 | /// Set/get damping ratio. 130 | open func setDampingRatio(_ ratio: b2Float) { 131 | m_dampingRatio = ratio 132 | } 133 | open var dampingRatio: b2Float { 134 | return m_dampingRatio 135 | } 136 | 137 | /// Dump joint to dmLog 138 | open override func dump() { 139 | let indexA = m_bodyA.m_islandIndex 140 | let indexB = m_bodyB.m_islandIndex 141 | 142 | print(" b2DistanceJointDef jd;") 143 | print(" jd.bodyA = bodies[\(indexA)];") 144 | print(" jd.bodyB = bodies[\(indexB)];") 145 | print(" jd.collideConnected = bool(\(m_collideConnected));") 146 | print(" jd.localAnchorA.set(\(m_localAnchorA.x), \(m_localAnchorA.y);") 147 | print(" jd.localAnchorB.set(\(m_localAnchorB.x), \(m_localAnchorB.y);") 148 | print(" jd.length = \(m_length);") 149 | print(" jd.frequencyHz = \(m_frequencyHz);") 150 | print(" jd.dampingRatio = \(m_dampingRatio);") 151 | print(" joints[\(m_index)] = m_world->createJoint(&jd);") 152 | } 153 | 154 | // MARK: private methods 155 | init(_ def: b2DistanceJointDef) { 156 | m_localAnchorA = def.localAnchorA 157 | m_localAnchorB = def.localAnchorB 158 | m_length = def.length 159 | m_frequencyHz = def.frequencyHz 160 | m_dampingRatio = def.dampingRatio 161 | m_impulse = 0.0 162 | m_gamma = 0.0 163 | m_bias = 0.0 164 | super.init(def) 165 | } 166 | 167 | override func initVelocityConstraints(_ data: inout b2SolverData) { 168 | m_indexA = m_bodyA.m_islandIndex 169 | m_indexB = m_bodyB.m_islandIndex 170 | m_localCenterA = m_bodyA.m_sweep.localCenter 171 | m_localCenterB = m_bodyB.m_sweep.localCenter 172 | m_invMassA = m_bodyA.m_invMass 173 | m_invMassB = m_bodyB.m_invMass 174 | m_invIA = m_bodyA.m_invI 175 | m_invIB = m_bodyB.m_invI 176 | 177 | let cA = data.positions[m_indexA].c 178 | let aA = data.positions[m_indexA].a 179 | var vA = data.velocities[m_indexA].v 180 | var wA = data.velocities[m_indexA].w 181 | 182 | let cB = data.positions[m_indexB].c 183 | let aB = data.positions[m_indexB].a 184 | var vB = data.velocities[m_indexB].v 185 | var wB = data.velocities[m_indexB].w 186 | 187 | let qA = b2Rot(aA), qB = b2Rot(aB) 188 | 189 | m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA) 190 | m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB) 191 | m_u = cB + m_rB - cA - m_rA 192 | 193 | // Handle singularity. 194 | let length = m_u.length() 195 | if length > b2_linearSlop { 196 | m_u *= 1.0 / length 197 | } 198 | else { 199 | m_u.set(0.0, 0.0) 200 | } 201 | 202 | let crAu = b2Cross(m_rA, m_u) 203 | let crBu = b2Cross(m_rB, m_u) 204 | var invMass = m_invMassA + m_invIA * crAu * crAu + m_invMassB + m_invIB * crBu * crBu 205 | 206 | // Compute the effective mass matrix. 207 | m_mass = invMass != 0.0 ? 1.0 / invMass : 0.0 208 | 209 | if m_frequencyHz > 0.0 { 210 | let C = length - m_length 211 | 212 | // Frequency 213 | let omega = 2.0 * b2_pi * m_frequencyHz 214 | 215 | // Damping coefficient 216 | let d = 2.0 * m_mass * m_dampingRatio * omega 217 | 218 | // Spring stiffness 219 | let k = m_mass * omega * omega 220 | 221 | // magic formulas 222 | let h = data.step.dt 223 | m_gamma = h * (d + h * k) 224 | m_gamma = m_gamma != 0.0 ? 1.0 / m_gamma : 0.0 225 | m_bias = C * h * k * m_gamma 226 | 227 | invMass += m_gamma 228 | m_mass = invMass != 0.0 ? 1.0 / invMass : 0.0 229 | } 230 | else { 231 | m_gamma = 0.0 232 | m_bias = 0.0 233 | } 234 | 235 | if data.step.warmStarting { 236 | // Scale the impulse to support a variable time step. 237 | m_impulse *= data.step.dtRatio 238 | 239 | let P = m_impulse * m_u 240 | vA -= m_invMassA * P 241 | wA -= m_invIA * b2Cross(m_rA, P) 242 | vB += m_invMassB * P 243 | wB += m_invIB * b2Cross(m_rB, P) 244 | } 245 | else { 246 | m_impulse = 0.0 247 | } 248 | 249 | data.velocities[m_indexA].v = vA 250 | data.velocities[m_indexA].w = wA 251 | data.velocities[m_indexB].v = vB 252 | data.velocities[m_indexB].w = wB 253 | } 254 | 255 | override func solveVelocityConstraints(_ data: inout b2SolverData) { 256 | var vA = data.velocities[m_indexA].v 257 | var wA = data.velocities[m_indexA].w 258 | var vB = data.velocities[m_indexB].v 259 | var wB = data.velocities[m_indexB].w 260 | 261 | // Cdot = dot(u, v + cross(w, r)) 262 | let vpA = vA + b2Cross(wA, m_rA) 263 | let vpB = vB + b2Cross(wB, m_rB) 264 | let Cdot = b2Dot(m_u, vpB - vpA) 265 | 266 | let impulse = -m_mass * (Cdot + m_bias + m_gamma * m_impulse) 267 | m_impulse += impulse 268 | 269 | let P = impulse * m_u 270 | vA -= m_invMassA * P 271 | wA -= m_invIA * b2Cross(m_rA, P) 272 | vB += m_invMassB * P 273 | wB += m_invIB * b2Cross(m_rB, P) 274 | 275 | data.velocities[m_indexA].v = vA 276 | data.velocities[m_indexA].w = wA 277 | data.velocities[m_indexB].v = vB 278 | data.velocities[m_indexB].w = wB 279 | } 280 | 281 | // This returns true if the position errors are within tolerance. 282 | override func solvePositionConstraints(_ data: inout b2SolverData) -> Bool { 283 | if m_frequencyHz > 0.0 { 284 | // There is no position correction for soft distance constraints. 285 | return true 286 | } 287 | 288 | var cA = data.positions[m_indexA].c 289 | var aA = data.positions[m_indexA].a 290 | var cB = data.positions[m_indexB].c 291 | var aB = data.positions[m_indexB].a 292 | 293 | let qA = b2Rot(aA), qB = b2Rot(aB) 294 | 295 | let rA = b2Mul(qA, m_localAnchorA - m_localCenterA) 296 | let rB = b2Mul(qB, m_localAnchorB - m_localCenterB) 297 | var u = cB + rB - cA - rA 298 | 299 | let length = u.normalize() 300 | var C = length - m_length 301 | C = b2Clamp(C, -b2_maxLinearCorrection, b2_maxLinearCorrection) 302 | 303 | let impulse = -m_mass * C 304 | let P = impulse * u 305 | 306 | cA -= m_invMassA * P 307 | aA -= m_invIA * b2Cross(rA, P) 308 | cB += m_invMassB * P 309 | aB += m_invIB * b2Cross(rB, P) 310 | 311 | data.positions[m_indexA].c = cA 312 | data.positions[m_indexA].a = aA 313 | data.positions[m_indexB].c = cB 314 | data.positions[m_indexB].a = aB 315 | 316 | return abs(C) < b2_linearSlop 317 | } 318 | 319 | // MARK: private variables 320 | var m_frequencyHz: b2Float 321 | var m_dampingRatio: b2Float 322 | var m_bias: b2Float 323 | 324 | // Solver shared 325 | var m_localAnchorA: b2Vec2 326 | var m_localAnchorB: b2Vec2 327 | var m_gamma: b2Float 328 | var m_impulse: b2Float 329 | var m_length: b2Float 330 | 331 | // Solver temp 332 | var m_indexA: Int = 0 333 | var m_indexB: Int = 0 334 | var m_u = b2Vec2() 335 | var m_rA = b2Vec2() 336 | var m_rB = b2Vec2() 337 | var m_localCenterA = b2Vec2() 338 | var m_localCenterB = b2Vec2() 339 | var m_invMassA : b2Float = 0.0 340 | var m_invMassB : b2Float = 0.0 341 | var m_invIA : b2Float = 0.0 342 | var m_invIB: b2Float = 0.0 343 | var m_mass: b2Float = 0.0 344 | } 345 | -------------------------------------------------------------------------------- /Sources/Box2D/Dynamics/Joints/b2FrictionJoint.swift: -------------------------------------------------------------------------------- 1 | /** 2 | Copyright (c) 2006-2014 Erin Catto http://www.box2d.org 3 | Copyright (c) 2015 - Yohei Yoshihara 4 | 5 | This software is provided 'as-is', without any express or implied 6 | warranty. In no event will the authors be held liable for any damages 7 | arising from the use of this software. 8 | 9 | Permission is granted to anyone to use this software for any purpose, 10 | including commercial applications, and to alter it and redistribute it 11 | freely, subject to the following restrictions: 12 | 13 | 1. The origin of this software must not be misrepresented; you must not 14 | claim that you wrote the original software. If you use this software 15 | in a product, an acknowledgment in the product documentation would be 16 | appreciated but is not required. 17 | 18 | 2. Altered source versions must be plainly marked as such, and must not be 19 | misrepresented as being the original software. 20 | 21 | 3. This notice may not be removed or altered from any source distribution. 22 | 23 | This version of box2d was developed by Yohei Yoshihara. It is based upon 24 | the original C++ code written by Erin Catto. 25 | */ 26 | 27 | import Foundation 28 | 29 | /// Friction joint definition. 30 | open class b2FrictionJointDef : b2JointDef { 31 | public override init() { 32 | localAnchorA = b2Vec2() 33 | localAnchorB = b2Vec2() 34 | maxForce = 0.0 35 | maxTorque = 0.0 36 | super.init() 37 | type = b2JointType.frictionJoint 38 | } 39 | 40 | /// Initialize the bodies, anchors, axis, and reference angle using the world 41 | /// anchor and world axis. 42 | public convenience init(bodyA: b2Body, bodyB: b2Body, anchor: b2Vec2) { 43 | self.init() 44 | initialize(bodyA: bodyA, bodyB: bodyB, anchor: anchor) 45 | } 46 | 47 | /// Initialize the bodies, anchors, axis, and reference angle using the world 48 | /// anchor and world axis. 49 | open func initialize(bodyA bA: b2Body, bodyB bB: b2Body, anchor: b2Vec2) { 50 | bodyA = bA 51 | bodyB = bB 52 | localAnchorA = bodyA.getLocalPoint(anchor) 53 | localAnchorB = bodyB.getLocalPoint(anchor) 54 | } 55 | 56 | /// The local anchor point relative to bodyA's origin. 57 | open var localAnchorA: b2Vec2 58 | 59 | /// The local anchor point relative to bodyB's origin. 60 | open var localAnchorB: b2Vec2 61 | 62 | /// The maximum friction force in N. 63 | open var maxForce: b2Float 64 | 65 | /// The maximum friction torque in N-m. 66 | open var maxTorque: b2Float 67 | } 68 | 69 | // MARK: - 70 | /// Friction joint. This is used for top-down friction. 71 | /// It provides 2D translational friction and angular friction. 72 | open class b2FrictionJoint : b2Joint { 73 | open override var anchorA: b2Vec2 { 74 | return m_bodyA.getWorldPoint(m_localAnchorA) 75 | } 76 | open override var anchorB: b2Vec2 { 77 | return m_bodyB.getWorldPoint(m_localAnchorB) 78 | } 79 | 80 | /// Get the reaction force given the inverse time step. 81 | /// Unitoverride is N. 82 | open override func getReactionForce(inverseTimeStep inv_dt: b2Float) -> b2Vec2 { 83 | return inv_dt * m_linearImpulse 84 | } 85 | 86 | /// Get the reaction torque given the inverse time step. 87 | /// Unit is N*m. This is always zero for a distance joint. 88 | open override func getReactionTorque(inverseTimeStep inv_dt: b2Float) -> b2Float { 89 | return inv_dt * m_angularImpulse 90 | } 91 | 92 | /// The local anchor point relative to bodyA's origin. 93 | open var localAnchorA: b2Vec2 { return m_localAnchorA } 94 | 95 | /// The local anchor point relative to bodyB's origin. 96 | open var localAnchorB: b2Vec2 { return m_localAnchorB } 97 | 98 | /// Set the maximum friction force in N. 99 | open func setMaxForce(_ force: b2Float) { 100 | assert(b2IsValid(force) && force >= 0.0) 101 | m_maxForce = force 102 | } 103 | 104 | /// Get the maximum friction force in N. 105 | open var maxForce: b2Float { 106 | return m_maxForce 107 | } 108 | 109 | /// Set the maximum friction torque in N*m. 110 | open func setMaxTorque(_ torque: b2Float) { 111 | assert(b2IsValid(torque) && torque >= 0.0) 112 | m_maxTorque = torque 113 | } 114 | 115 | /// Get the maximum friction torque in N*m. 116 | open var maxTorque: b2Float { 117 | return m_maxTorque 118 | } 119 | 120 | /// Dump joint to dmLog 121 | open override func dump() { 122 | let indexA = m_bodyA.m_islandIndex 123 | let indexB = m_bodyB.m_islandIndex 124 | 125 | print(" b2FrictionJointDef jd;\n") 126 | print(" jd.bodyA = bodies[\(indexA)];") 127 | print(" jd.bodyB = bodies[\(indexB)];") 128 | print(" jd.collideConnected = bool(\(m_collideConnected));") 129 | print(" jd.localAnchorA.set(\(m_localAnchorA.x), \(m_localAnchorA.y));") 130 | print(" jd.localAnchorB.set(\(m_localAnchorB.x), \(m_localAnchorB.y));") 131 | print(" jd.maxForce = \(m_maxForce);") 132 | print(" jd.maxTorque = \(m_maxTorque);") 133 | print(" joints[\(m_index)] = m_world->createJoint(&jd);") 134 | } 135 | 136 | // MARK: private methods 137 | init(_ def: b2FrictionJointDef) { 138 | m_localAnchorA = def.localAnchorA 139 | m_localAnchorB = def.localAnchorB 140 | 141 | m_linearImpulse = b2Vec2(0.0, 0.0) 142 | m_angularImpulse = 0.0 143 | 144 | m_maxForce = def.maxForce 145 | m_maxTorque = def.maxTorque 146 | super.init(def) 147 | } 148 | 149 | override func initVelocityConstraints(_ data: inout b2SolverData) { 150 | m_indexA = m_bodyA.m_islandIndex 151 | m_indexB = m_bodyB.m_islandIndex 152 | m_localCenterA = m_bodyA.m_sweep.localCenter 153 | m_localCenterB = m_bodyB.m_sweep.localCenter 154 | m_invMassA = m_bodyA.m_invMass 155 | m_invMassB = m_bodyB.m_invMass 156 | m_invIA = m_bodyA.m_invI 157 | m_invIB = m_bodyB.m_invI 158 | 159 | let aA = data.positions[m_indexA].a 160 | var vA = data.velocities[m_indexA].v 161 | var wA = data.velocities[m_indexA].w 162 | 163 | let aB = data.positions[m_indexB].a 164 | var vB = data.velocities[m_indexB].v 165 | var wB = data.velocities[m_indexB].w 166 | 167 | let qA = b2Rot(aA), qB = b2Rot(aB) 168 | 169 | // Compute the effective mass matrix. 170 | m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA) 171 | m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB) 172 | 173 | // J = [-I -r1_skew I r2_skew] 174 | // [ 0 -1 0 1] 175 | // r_skew = [-ry; rx] 176 | 177 | // Matlab 178 | // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB] 179 | // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB] 180 | // [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB] 181 | 182 | let mA = m_invMassA, mB = m_invMassB 183 | let iA = m_invIA, iB = m_invIB 184 | 185 | var K = b2Mat22() 186 | K.ex.x = mA + mB + iA * m_rA.y * m_rA.y + iB * m_rB.y * m_rB.y 187 | K.ex.y = -iA * m_rA.x * m_rA.y - iB * m_rB.x * m_rB.y 188 | K.ey.x = K.ex.y 189 | K.ey.y = mA + mB + iA * m_rA.x * m_rA.x + iB * m_rB.x * m_rB.x 190 | 191 | m_linearMass = K.getInverse() 192 | 193 | m_angularMass = iA + iB 194 | if m_angularMass > 0.0 { 195 | m_angularMass = 1.0 / m_angularMass 196 | } 197 | 198 | if data.step.warmStarting { 199 | // Scale impulses to support a variable time step. 200 | m_linearImpulse *= data.step.dtRatio 201 | m_angularImpulse *= data.step.dtRatio 202 | 203 | let P = b2Vec2(m_linearImpulse.x, m_linearImpulse.y) 204 | vA -= mA * P 205 | wA -= iA * (b2Cross(m_rA, P) + m_angularImpulse) 206 | vB += mB * P 207 | wB += iB * (b2Cross(m_rB, P) + m_angularImpulse) 208 | } 209 | else { 210 | m_linearImpulse.setZero() 211 | m_angularImpulse = 0.0 212 | } 213 | 214 | data.velocities[m_indexA].v = vA 215 | data.velocities[m_indexA].w = wA 216 | data.velocities[m_indexB].v = vB 217 | data.velocities[m_indexB].w = wB 218 | } 219 | override func solveVelocityConstraints(_ data: inout b2SolverData) { 220 | var vA = data.velocities[m_indexA].v 221 | var wA = data.velocities[m_indexA].w 222 | var vB = data.velocities[m_indexB].v 223 | var wB = data.velocities[m_indexB].w 224 | 225 | let mA = m_invMassA, mB = m_invMassB 226 | let iA = m_invIA, iB = m_invIB 227 | 228 | let h = data.step.dt 229 | 230 | // Solve angular friction 231 | b2Locally { 232 | let Cdot = wB - wA 233 | var impulse = -self.m_angularMass * Cdot 234 | 235 | let oldImpulse = self.m_angularImpulse 236 | let maxImpulse = h * self.m_maxTorque 237 | self.m_angularImpulse = b2Clamp(self.m_angularImpulse + impulse, -maxImpulse, maxImpulse) 238 | impulse = self.m_angularImpulse - oldImpulse 239 | 240 | wA -= iA * impulse 241 | wB += iB * impulse 242 | } 243 | 244 | // Solve linear friction 245 | b2Locally { 246 | let Cdot = vB + b2Cross(wB, self.m_rB) - vA - b2Cross(wA, self.m_rA) 247 | 248 | var impulse = -b2Mul(self.m_linearMass, Cdot) 249 | let oldImpulse = self.m_linearImpulse 250 | self.m_linearImpulse += impulse 251 | 252 | let maxImpulse = h * self.m_maxForce 253 | 254 | if self.m_linearImpulse.lengthSquared() > maxImpulse * maxImpulse { 255 | self.m_linearImpulse.normalize() 256 | self.m_linearImpulse *= maxImpulse 257 | } 258 | 259 | impulse = self.m_linearImpulse - oldImpulse 260 | 261 | vA -= mA * impulse 262 | wA -= iA * b2Cross(self.m_rA, impulse) 263 | 264 | vB += mB * impulse 265 | wB += iB * b2Cross(self.m_rB, impulse) 266 | } 267 | 268 | data.velocities[m_indexA].v = vA 269 | data.velocities[m_indexA].w = wA 270 | data.velocities[m_indexB].v = vB 271 | data.velocities[m_indexB].w = wB 272 | } 273 | 274 | // This returns true if the position errors are within tolerance. 275 | override func solvePositionConstraints(_ data: inout b2SolverData) -> Bool { 276 | return true 277 | } 278 | 279 | // MARK: private variables 280 | var m_localAnchorA: b2Vec2 281 | var m_localAnchorB: b2Vec2 282 | 283 | // Solver shared 284 | var m_linearImpulse: b2Vec2 285 | var m_angularImpulse: b2Float 286 | var m_maxForce: b2Float 287 | var m_maxTorque: b2Float 288 | 289 | // Solver temp 290 | var m_indexA: Int = 0 291 | var m_indexB: Int = 0 292 | var m_rA = b2Vec2() 293 | var m_rB = b2Vec2() 294 | var m_localCenterA = b2Vec2() 295 | var m_localCenterB = b2Vec2() 296 | var m_invMassA: b2Float = 0.0 297 | var m_invMassB: b2Float = 0.0 298 | var m_invIA: b2Float = 0.0 299 | var m_invIB: b2Float = 0.0 300 | var m_linearMass = b2Mat22() 301 | var m_angularMass: b2Float = 0.0 302 | } 303 | -------------------------------------------------------------------------------- /Sources/Box2D/Dynamics/Joints/b2Joint.swift: -------------------------------------------------------------------------------- 1 | /** 2 | Copyright (c) 2006-2014 Erin Catto http://www.box2d.org 3 | Copyright (c) 2015 - Yohei Yoshihara 4 | 5 | This software is provided 'as-is', without any express or implied 6 | warranty. In no event will the authors be held liable for any damages 7 | arising from the use of this software. 8 | 9 | Permission is granted to anyone to use this software for any purpose, 10 | including commercial applications, and to alter it and redistribute it 11 | freely, subject to the following restrictions: 12 | 13 | 1. The origin of this software must not be misrepresented; you must not 14 | claim that you wrote the original software. If you use this software 15 | in a product, an acknowledgment in the product documentation would be 16 | appreciated but is not required. 17 | 18 | 2. Altered source versions must be plainly marked as such, and must not be 19 | misrepresented as being the original software. 20 | 21 | 3. This notice may not be removed or altered from any source distribution. 22 | 23 | This version of box2d was developed by Yohei Yoshihara. It is based upon 24 | the original C++ code written by Erin Catto. 25 | */ 26 | 27 | import Foundation 28 | 29 | public enum b2JointType : CustomStringConvertible { 30 | case unknownJoint 31 | case revoluteJoint 32 | case prismaticJoint 33 | case distanceJoint 34 | case pulleyJoint 35 | case mouseJoint 36 | case gearJoint 37 | case wheelJoint 38 | case weldJoint 39 | case frictionJoint 40 | case ropeJoint 41 | case motorJoint 42 | public var description: String { 43 | switch self { 44 | case .unknownJoint: return "unknownJoint" 45 | case .revoluteJoint: return "revoluteJoint" 46 | case .prismaticJoint: return "prismaticJoint" 47 | case .distanceJoint: return "distanceJoint" 48 | case .pulleyJoint: return "pulleyJoint" 49 | case .mouseJoint: return "mouseJoint" 50 | case .gearJoint: return "gearJoint" 51 | case .wheelJoint: return "wheelJoint" 52 | case .weldJoint: return "weldJoint" 53 | case .frictionJoint: return "frictionJoint" 54 | case .ropeJoint: return "ropeJoint" 55 | case .motorJoint: return "motorJoint" 56 | } 57 | } 58 | } 59 | 60 | public enum b2LimitState : CustomStringConvertible { 61 | case inactiveLimit 62 | case atLowerLimit 63 | case atUpperLimit 64 | case equalLimits 65 | public var description: String { 66 | switch self { 67 | case .inactiveLimit: return "inactiveLimit" 68 | case .atLowerLimit: return "atLowerLimit" 69 | case .atUpperLimit: return "atUpperLimit" 70 | case .equalLimits: return "equalLimits" 71 | } 72 | } 73 | } 74 | 75 | public struct b2Jacobian { 76 | var linear: b2Vec2 77 | var angularA: b2Float 78 | var angularB: b2Float 79 | } 80 | 81 | // MARK: - 82 | /// A joint edge is used to connect bodies and joints together 83 | /// in a joint graph where each body is a node and each joint 84 | /// is an edge. A joint edge belongs to a doubly linked list 85 | /// maintained in each attached body. Each joint has two joint 86 | /// nodes, one for each attached body. 87 | open class b2JointEdge { 88 | init(joint: b2Joint) { 89 | self.joint = joint 90 | } 91 | var other: b2Body! = nil ///< provides quick access to the other body attached. 92 | unowned var joint: b2Joint ///< the joint ** parent ** 93 | var prev: b2JointEdge? = nil ///< the previous joint edge in the body's joint list 94 | var next: b2JointEdge? = nil ///< the next joint edge in the body's joint list 95 | } 96 | 97 | // MARK: - 98 | /// Joint definitions are used to construct joints. 99 | open class b2JointDef { 100 | public init() { 101 | type = b2JointType.unknownJoint 102 | userData = nil 103 | bodyA = nil 104 | bodyB = nil 105 | collideConnected = false 106 | } 107 | 108 | /// The joint type is set automatically for concrete joint types. 109 | open var type: b2JointType 110 | 111 | /// Use this to attach application specific data to your joints. 112 | open var userData: AnyObject? 113 | 114 | /// The first attached body. 115 | open var bodyA: b2Body! 116 | 117 | /// The second attached body. 118 | open var bodyB: b2Body! 119 | 120 | /// Set this flag to true if the attached bodies should collide. 121 | open var collideConnected: Bool 122 | } 123 | 124 | // MARK: - 125 | /// various fashions. Some joints also feature limits and motors. 126 | open class b2Joint { 127 | /// Get the type of the concrete joint. 128 | open var type: b2JointType { 129 | return m_type 130 | } 131 | 132 | /// Get the first body attached to this joint. 133 | open var bodyA: b2Body { 134 | return m_bodyA 135 | } 136 | 137 | /// Get the second body attached to this joint. 138 | open var bodyB: b2Body { 139 | return m_bodyB 140 | } 141 | 142 | /// Get the anchor point on bodyA in world coordinates. 143 | open var anchorA: b2Vec2 { 144 | fatalError("must override") 145 | } 146 | 147 | /// Get the anchor point on bodyB in world coordinates. 148 | open var anchorB: b2Vec2 { 149 | fatalError("must override") 150 | } 151 | 152 | /// Get the reaction force on bodyB at the joint anchor in Newtons. 153 | open func getReactionForce(inverseTimeStep inv_dt: b2Float) -> b2Vec2 { 154 | fatalError("must override") 155 | } 156 | 157 | /// Get the reaction torque on bodyB in N*m. 158 | open func getReactionTorque(inverseTimeStep inv_dt: b2Float) -> b2Float { 159 | fatalError("must override") 160 | } 161 | 162 | /// Get the next joint the world joint list. 163 | open func getNext() -> b2Joint? { 164 | return m_next 165 | } 166 | 167 | /// Get the user data pointer. 168 | open var userData: AnyObject? { 169 | get { 170 | return m_userData 171 | } 172 | set { 173 | setUserData(newValue) 174 | } 175 | } 176 | 177 | /// Set the user data pointer. 178 | open func setUserData(_ data: AnyObject?) { 179 | m_userData = data 180 | } 181 | 182 | /// Short-cut function to determine if either body is inactive. 183 | open var isActive: Bool { 184 | return m_bodyA.isActive && m_bodyB.isActive 185 | } 186 | 187 | /// Get collide connected. 188 | /// Note: modifying the collide connect flag won't work correctly because 189 | /// the flag is only checked when fixture AABBs begin to overlap. 190 | open var collideConnected: Bool { 191 | return m_collideConnected 192 | } 193 | 194 | /// Dump this joint to the log file. 195 | open func dump() { NSLog("// Dump is not supported for this joint type."); } 196 | 197 | /// Shift the origin for any points stored in world coordinates. 198 | open func shiftOrigin(_ newOrigin: b2Vec2) { } 199 | 200 | // MARK: private methods 201 | 202 | class func create(_ def: b2JointDef) -> b2Joint { 203 | var joint: b2Joint 204 | 205 | switch def.type { 206 | case .distanceJoint: 207 | joint = b2DistanceJoint(def as! b2DistanceJointDef) 208 | 209 | case .mouseJoint: 210 | joint = b2MouseJoint(def as! b2MouseJointDef) 211 | 212 | case .prismaticJoint: 213 | joint = b2PrismaticJoint(def as! b2PrismaticJointDef) 214 | 215 | case .revoluteJoint: 216 | joint = b2RevoluteJoint(def as! b2RevoluteJointDef) 217 | 218 | case .pulleyJoint: 219 | joint = b2PulleyJoint(def as! b2PulleyJointDef) 220 | 221 | case .gearJoint: 222 | joint = b2GearJoint(def as! b2GearJointDef) 223 | 224 | case .wheelJoint: 225 | joint = b2WheelJoint(def as! b2WheelJointDef) 226 | 227 | case .weldJoint: 228 | joint = b2WeldJoint(def as! b2WeldJointDef) 229 | 230 | case .frictionJoint: 231 | joint = b2FrictionJoint(def as! b2FrictionJointDef) 232 | 233 | case .ropeJoint: 234 | joint = b2RopeJoint(def as! b2RopeJointDef) 235 | 236 | case .motorJoint: 237 | joint = b2MotorJoint(def as! b2MotorJointDef) 238 | 239 | default: 240 | fatalError("unknown joint type") 241 | } 242 | 243 | return joint 244 | } 245 | class func destroy(_ joint: b2Joint) { 246 | } 247 | 248 | init(_ def: b2JointDef) { 249 | assert(def.bodyA !== def.bodyB) 250 | 251 | m_type = def.type 252 | m_prev = nil 253 | m_next = nil 254 | m_bodyA = def.bodyA 255 | m_bodyB = def.bodyB 256 | m_index = 0 257 | m_collideConnected = def.collideConnected 258 | m_islandFlag = false 259 | m_userData = def.userData 260 | 261 | m_edgeA = b2JointEdge(joint: self) 262 | m_edgeA.other = nil 263 | m_edgeA.prev = nil 264 | m_edgeA.next = nil 265 | 266 | m_edgeB = b2JointEdge(joint: self) 267 | m_edgeB.other = nil 268 | m_edgeB.prev = nil 269 | m_edgeB.next = nil 270 | } 271 | 272 | func initVelocityConstraints(_ data: inout b2SolverData) { 273 | fatalError("must override") 274 | } 275 | func solveVelocityConstraints(_ data: inout b2SolverData) { 276 | fatalError("must override") 277 | } 278 | 279 | // This returns true if the position errors are within tolerance. 280 | func solvePositionConstraints(_ data: inout b2SolverData) -> Bool { 281 | fatalError("must override") 282 | } 283 | 284 | // MARK: private variables 285 | var m_type: b2JointType = b2JointType.unknownJoint 286 | var m_prev: b2Joint? = nil // ** linked list ** 287 | var m_next: b2Joint? = nil // ** linked list ** 288 | var m_edgeA : b2JointEdge! // ** owner ** 289 | var m_edgeB : b2JointEdge! // ** owner ** 290 | var m_bodyA: b2Body 291 | var m_bodyB: b2Body 292 | 293 | var m_index: Int = 0 294 | 295 | var m_islandFlag: Bool = false 296 | var m_collideConnected: Bool = false 297 | 298 | var m_userData: AnyObject? = nil 299 | } 300 | 301 | -------------------------------------------------------------------------------- /Sources/Box2D/Dynamics/Joints/b2MotorJoint.swift: -------------------------------------------------------------------------------- 1 | /** 2 | Copyright (c) 2006-2014 Erin Catto http://www.box2d.org 3 | Copyright (c) 2015 - Yohei Yoshihara 4 | 5 | This software is provided 'as-is', without any express or implied 6 | warranty. In no event will the authors be held liable for any damages 7 | arising from the use of this software. 8 | 9 | Permission is granted to anyone to use this software for any purpose, 10 | including commercial applications, and to alter it and redistribute it 11 | freely, subject to the following restrictions: 12 | 13 | 1. The origin of this software must not be misrepresented; you must not 14 | claim that you wrote the original software. If you use this software 15 | in a product, an acknowledgment in the product documentation would be 16 | appreciated but is not required. 17 | 18 | 2. Altered source versions must be plainly marked as such, and must not be 19 | misrepresented as being the original software. 20 | 21 | 3. This notice may not be removed or altered from any source distribution. 22 | 23 | This version of box2d was developed by Yohei Yoshihara. It is based upon 24 | the original C++ code written by Erin Catto. 25 | */ 26 | 27 | import Foundation 28 | 29 | /// Motor joint definition. 30 | open class b2MotorJointDef : b2JointDef { 31 | public override init() { 32 | linearOffset = b2Vec2() 33 | angularOffset = 0.0 34 | maxForce = 1.0 35 | maxTorque = 1.0 36 | correctionFactor = 0.3 37 | super.init() 38 | type = b2JointType.motorJoint 39 | } 40 | 41 | /// Initialize the bodies and offsets using the current transforms. 42 | public convenience init(bodyA: b2Body, bodyB: b2Body) { 43 | self.init() 44 | initialize(bodyA: bodyA, bodyB: bodyB) 45 | } 46 | 47 | /// Initialize the bodies and offsets using the current transforms. 48 | open func initialize(bodyA: b2Body, bodyB: b2Body) { 49 | self.bodyA = bodyA 50 | self.bodyB = bodyB 51 | let xB = bodyB.position 52 | linearOffset = bodyA.getLocalPoint(xB) 53 | 54 | let angleA = bodyA.angle 55 | let angleB = bodyB.angle 56 | angularOffset = angleB - angleA 57 | } 58 | 59 | /// Position of bodyB minus the position of bodyA, in bodyA's frame, in meters. 60 | open var linearOffset: b2Vec2 61 | 62 | /// The bodyB angle minus bodyA angle in radians. 63 | open var angularOffset: b2Float 64 | 65 | /// The maximum motor force in N. 66 | open var maxForce: b2Float 67 | 68 | /// The maximum motor torque in N-m. 69 | open var maxTorque: b2Float 70 | 71 | /// Position correction factor in the range [0,1]. 72 | open var correctionFactor: b2Float 73 | } 74 | 75 | // MARK: - 76 | /// A motor joint is used to control the relative motion 77 | /// between two bodies. A typical usage is to control the movement 78 | /// of a dynamic body with respect to the ground. 79 | open class b2MotorJoint : b2Joint { 80 | open override var anchorA: b2Vec2 { 81 | return m_bodyA.position 82 | } 83 | open override var anchorB: b2Vec2 { 84 | return m_bodyB.position 85 | } 86 | 87 | /// Get the reaction force given the inverse time step. 88 | /// Unitoverride is N. 89 | open override func getReactionForce(inverseTimeStep inv_dt: b2Float) -> b2Vec2 { 90 | return inv_dt * m_linearImpulse 91 | } 92 | 93 | /// Get the reaction torque given the inverse time step. 94 | /// Unit is N*m. This is always zero for a distance joint. 95 | open override func getReactionTorque(inverseTimeStep inv_dt: b2Float) -> b2Float { 96 | return inv_dt * m_angularImpulse 97 | } 98 | 99 | /// Set/get the target linear offset, in frame A, in meters. 100 | open func setLinearOffset(_ linearOffset: b2Vec2) { 101 | if linearOffset.x != m_linearOffset.x || linearOffset.y != m_linearOffset.y { 102 | m_bodyA.setAwake(true) 103 | m_bodyB.setAwake(true) 104 | m_linearOffset = linearOffset 105 | } 106 | } 107 | open var linearOffset: b2Vec2 { 108 | get { 109 | return m_linearOffset 110 | } 111 | set { 112 | setLinearOffset(newValue) 113 | } 114 | } 115 | 116 | /// Set/get the target angular offset, in radians. 117 | open func setAngularOffset(_ angularOffset: b2Float) { 118 | if angularOffset != m_angularOffset { 119 | m_bodyA.setAwake(true) 120 | m_bodyB.setAwake(true) 121 | m_angularOffset = angularOffset 122 | } 123 | } 124 | open var angularOffset: b2Float { 125 | get { 126 | return m_angularOffset 127 | } 128 | set { 129 | setAngularOffset(newValue) 130 | } 131 | } 132 | 133 | /// Set the maximum friction force in N. 134 | open func setMaxForce(_ force: b2Float) { 135 | assert(b2IsValid(force) && force >= 0.0) 136 | m_maxForce = force 137 | } 138 | 139 | /// Get the maximum friction force in N. 140 | open var maxForce: b2Float { 141 | get { 142 | return m_maxForce 143 | } 144 | set { 145 | setMaxForce(newValue) 146 | } 147 | } 148 | 149 | /// Set the maximum friction torque in N*m. 150 | open func setMaxTorque(_ torque: b2Float) { 151 | assert(b2IsValid(torque) && torque >= 0.0) 152 | m_maxTorque = torque 153 | } 154 | 155 | /// Get the maximum friction torque in N*m. 156 | open var maxTorque: b2Float { 157 | get { 158 | return m_maxTorque 159 | } 160 | set { 161 | setMaxTorque(newValue) 162 | } 163 | } 164 | 165 | /// Set the position correction factor in the range [0,1]. 166 | open func setCorrectionFactor(_ factor: b2Float) { 167 | assert(b2IsValid(factor) && 0.0 <= factor && factor <= 1.0) 168 | m_correctionFactor = factor 169 | } 170 | 171 | /// Get the position correction factor in the range [0,1]. 172 | open var correctionFactor: b2Float { 173 | get { 174 | return m_correctionFactor 175 | } 176 | set { 177 | setCorrectionFactor(newValue) 178 | } 179 | } 180 | 181 | /// Dump to b2Log 182 | open override func dump() { 183 | let indexA = m_bodyA.m_islandIndex 184 | let indexB = m_bodyB.m_islandIndex 185 | 186 | print(" b2MotorJointDef jd;") 187 | print(" jd.bodyA = bodies[\(indexA)];") 188 | print(" jd.bodyB = bodies[\(indexB)];") 189 | print(" jd.collideConnected = bool(\(m_collideConnected));") 190 | print(" jd.linearOffset.set(\(m_linearOffset.x), \(m_linearOffset.y));") 191 | print(" jd.angularOffset = \(m_angularOffset);") 192 | print(" jd.maxForce = \(m_maxForce);") 193 | print(" jd.maxTorque = \(m_maxTorque);") 194 | print(" jd.correctionFactor = \(m_correctionFactor);") 195 | print(" joints[\(m_index)] = m_world->createJoint(&jd);\n") 196 | } 197 | 198 | // MARK: private methods 199 | init(_ def: b2MotorJointDef) { 200 | m_linearOffset = def.linearOffset 201 | m_angularOffset = def.angularOffset 202 | 203 | m_linearImpulse = b2Vec2() 204 | m_angularImpulse = 0.0 205 | 206 | m_maxForce = def.maxForce 207 | m_maxTorque = def.maxTorque 208 | m_correctionFactor = def.correctionFactor 209 | super.init(def) 210 | } 211 | 212 | override func initVelocityConstraints(_ data: inout b2SolverData) { 213 | m_indexA = m_bodyA.m_islandIndex 214 | m_indexB = m_bodyB.m_islandIndex 215 | m_localCenterA = m_bodyA.m_sweep.localCenter 216 | m_localCenterB = m_bodyB.m_sweep.localCenter 217 | m_invMassA = m_bodyA.m_invMass 218 | m_invMassB = m_bodyB.m_invMass 219 | m_invIA = m_bodyA.m_invI 220 | m_invIB = m_bodyB.m_invI 221 | 222 | let cA = data.positions[m_indexA].c 223 | let aA = data.positions[m_indexA].a 224 | var vA = data.velocities[m_indexA].v 225 | var wA = data.velocities[m_indexA].w 226 | 227 | let cB = data.positions[m_indexB].c 228 | let aB = data.positions[m_indexB].a 229 | var vB = data.velocities[m_indexB].v 230 | var wB = data.velocities[m_indexB].w 231 | 232 | let qA = b2Rot(aA), qB = b2Rot(aB) 233 | 234 | // Compute the effective mass matrix. 235 | m_rA = b2Mul(qA, -m_localCenterA) 236 | m_rB = b2Mul(qB, -m_localCenterB) 237 | 238 | // J = [-I -r1_skew I r2_skew] 239 | // [ 0 -1 0 1] 240 | // r_skew = [-ry; rx] 241 | 242 | // Matlab 243 | // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB] 244 | // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB] 245 | // [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB] 246 | 247 | let mA = m_invMassA, mB = m_invMassB 248 | let iA = m_invIA, iB = m_invIB 249 | 250 | var K = b2Mat22() 251 | K.ex.x = mA + mB + iA * m_rA.y * m_rA.y + iB * m_rB.y * m_rB.y 252 | K.ex.y = -iA * m_rA.x * m_rA.y - iB * m_rB.x * m_rB.y 253 | K.ey.x = K.ex.y 254 | K.ey.y = mA + mB + iA * m_rA.x * m_rA.x + iB * m_rB.x * m_rB.x 255 | 256 | m_linearMass = K.getInverse() 257 | 258 | m_angularMass = iA + iB 259 | if m_angularMass > 0.0 { 260 | m_angularMass = 1.0 / m_angularMass 261 | } 262 | 263 | m_linearError = cB + m_rB - cA - m_rA - b2Mul(qA, m_linearOffset) 264 | m_angularError = aB - aA - m_angularOffset 265 | 266 | if data.step.warmStarting { 267 | // Scale impulses to support a variable time step. 268 | m_linearImpulse *= data.step.dtRatio 269 | m_angularImpulse *= data.step.dtRatio 270 | 271 | let P = b2Vec2(m_linearImpulse.x, m_linearImpulse.y) 272 | vA -= mA * P 273 | wA -= iA * (b2Cross(m_rA, P) + m_angularImpulse) 274 | vB += mB * P 275 | wB += iB * (b2Cross(m_rB, P) + m_angularImpulse) 276 | } 277 | else { 278 | m_linearImpulse.setZero() 279 | m_angularImpulse = 0.0 280 | } 281 | 282 | data.velocities[m_indexA].v = vA 283 | data.velocities[m_indexA].w = wA 284 | data.velocities[m_indexB].v = vB 285 | data.velocities[m_indexB].w = wB 286 | } 287 | override func solveVelocityConstraints(_ data: inout b2SolverData) { 288 | var vA = data.velocities[m_indexA].v 289 | var wA = data.velocities[m_indexA].w 290 | var vB = data.velocities[m_indexB].v 291 | var wB = data.velocities[m_indexB].w 292 | 293 | let mA = m_invMassA, mB = m_invMassB 294 | let iA = m_invIA, iB = m_invIB 295 | 296 | let h = data.step.dt 297 | let inv_h = data.step.inv_dt 298 | 299 | // Solve angular friction 300 | //{ 301 | b2Locally { 302 | let Cdot = wB - wA + inv_h * self.m_correctionFactor * self.m_angularError 303 | var impulse = -self.m_angularMass * Cdot 304 | 305 | let oldImpulse = self.m_angularImpulse 306 | let maxImpulse = h * self.m_maxTorque 307 | self.m_angularImpulse = b2Clamp(self.m_angularImpulse + impulse, -maxImpulse, maxImpulse) 308 | impulse = self.m_angularImpulse - oldImpulse 309 | 310 | wA -= iA * impulse 311 | wB += iB * impulse 312 | } 313 | 314 | // Solve linear friction 315 | b2Locally { 316 | let Cdot = vB + b2Cross(wB, self.m_rB) - vA - b2Cross(wA, self.m_rA) + inv_h * self.m_correctionFactor * self.m_linearError 317 | 318 | var impulse = -b2Mul(self.m_linearMass, Cdot) 319 | let oldImpulse = self.m_linearImpulse 320 | self.m_linearImpulse += impulse 321 | 322 | let maxImpulse = h * self.m_maxForce 323 | 324 | if self.m_linearImpulse.lengthSquared() > maxImpulse * maxImpulse { 325 | self.m_linearImpulse.normalize() 326 | self.m_linearImpulse *= maxImpulse 327 | } 328 | 329 | impulse = self.m_linearImpulse - oldImpulse 330 | 331 | vA -= mA * impulse 332 | wA -= iA * b2Cross(self.m_rA, impulse) 333 | 334 | vB += mB * impulse 335 | wB += iB * b2Cross(self.m_rB, impulse) 336 | } 337 | 338 | data.velocities[m_indexA].v = vA 339 | data.velocities[m_indexA].w = wA 340 | data.velocities[m_indexB].v = vB 341 | data.velocities[m_indexB].w = wB 342 | } 343 | 344 | // This returns true if the position errors are within tolerance. 345 | override func solvePositionConstraints(_ data: inout b2SolverData) -> Bool { 346 | return true 347 | } 348 | 349 | // MARK: private variables 350 | // Solver shared 351 | var m_linearOffset: b2Vec2 352 | var m_angularOffset: b2Float 353 | var m_linearImpulse: b2Vec2 354 | var m_angularImpulse: b2Float 355 | var m_maxForce: b2Float 356 | var m_maxTorque: b2Float 357 | var m_correctionFactor: b2Float 358 | 359 | // Solver temp 360 | var m_indexA: Int = 0 361 | var m_indexB: Int = 0 362 | var m_rA = b2Vec2() 363 | var m_rB = b2Vec2() 364 | var m_localCenterA = b2Vec2() 365 | var m_localCenterB = b2Vec2() 366 | var m_linearError = b2Vec2() 367 | var m_angularError: b2Float = 0 368 | var m_invMassA: b2Float = 0 369 | var m_invMassB: b2Float = 0 370 | var m_invIA: b2Float = 0 371 | var m_invIB: b2Float = 0 372 | var m_linearMass = b2Mat22() 373 | var m_angularMass: b2Float = 0 374 | } 375 | -------------------------------------------------------------------------------- /Sources/Box2D/Dynamics/Joints/b2MouseJoint.swift: -------------------------------------------------------------------------------- 1 | /** 2 | Copyright (c) 2006-2014 Erin Catto http://www.box2d.org 3 | Copyright (c) 2015 - Yohei Yoshihara 4 | 5 | This software is provided 'as-is', without any express or implied 6 | warranty. In no event will the authors be held liable for any damages 7 | arising from the use of this software. 8 | 9 | Permission is granted to anyone to use this software for any purpose, 10 | including commercial applications, and to alter it and redistribute it 11 | freely, subject to the following restrictions: 12 | 13 | 1. The origin of this software must not be misrepresented; you must not 14 | claim that you wrote the original software. If you use this software 15 | in a product, an acknowledgment in the product documentation would be 16 | appreciated but is not required. 17 | 18 | 2. Altered source versions must be plainly marked as such, and must not be 19 | misrepresented as being the original software. 20 | 21 | 3. This notice may not be removed or altered from any source distribution. 22 | 23 | This version of box2d was developed by Yohei Yoshihara. It is based upon 24 | the original C++ code written by Erin Catto. 25 | */ 26 | 27 | import Foundation 28 | 29 | /// Mouse joint definition. This requires a world target point, 30 | /// tuning parameters, and the time step. 31 | open class b2MouseJointDef : b2JointDef { 32 | public override init() { 33 | target = b2Vec2() 34 | maxForce = 0.0 35 | frequencyHz = 5.0 36 | dampingRatio = 0.7 37 | super.init() 38 | type = b2JointType.mouseJoint 39 | } 40 | 41 | /// The initial world target point. This is assumed 42 | /// to coincide with the body anchor initially. 43 | open var target: b2Vec2 44 | 45 | /// The maximum constraint force that can be exerted 46 | /// to move the candidate body. Usually you will express 47 | /// as some multiple of the weight (multiplier * mass * gravity). 48 | open var maxForce: b2Float 49 | 50 | /// The response speed. 51 | open var frequencyHz: b2Float 52 | 53 | /// The damping ratio. 0 = no damping, 1 = critical damping. 54 | open var dampingRatio: b2Float 55 | } 56 | 57 | // MARK: - 58 | /// A mouse joint is used to make a point on a body track a 59 | /// specified world point. This a soft constraint with a maximum 60 | /// force. This allows the constraint to stretch and without 61 | /// applying huge forces. 62 | /// NOTE: this joint is not documented in the manual because it was 63 | /// developed to be used in the testbed. If you want to learn how to 64 | /// use the mouse joint, look at the testbed. 65 | open class b2MouseJoint : b2Joint { 66 | /// Implements b2Joint. 67 | open override var anchorA: b2Vec2 { 68 | return m_targetA 69 | } 70 | 71 | /// Implements b2Joint. 72 | open override var anchorB: b2Vec2 { 73 | return m_bodyB.getWorldPoint(m_localAnchorB) 74 | } 75 | 76 | /// Implements b2Joint. 77 | open override func getReactionForce(inverseTimeStep inv_dt: b2Float) -> b2Vec2 { 78 | return inv_dt * m_impulse 79 | } 80 | 81 | /// Implements b2Joint. 82 | open override func getReactionTorque(inverseTimeStep inv_dt: b2Float) -> b2Float { 83 | return inv_dt * 0.0 84 | } 85 | 86 | /// Use this to update the target point. 87 | open func setTarget(_ target: b2Vec2) { 88 | if m_bodyB.isAwake == false { 89 | m_bodyB.setAwake(true) 90 | } 91 | m_targetA = target 92 | } 93 | open var target: b2Vec2 { 94 | get { 95 | return m_targetA 96 | } 97 | set { 98 | setTarget(newValue) 99 | } 100 | } 101 | 102 | /// Set/get the maximum force in Newtons. 103 | open func setMaxForce(_ force: b2Float) { 104 | m_maxForce = force 105 | } 106 | open var maxForce: b2Float { 107 | get { 108 | return m_maxForce 109 | } 110 | set { 111 | setMaxForce(newValue) 112 | } 113 | } 114 | 115 | /// Set/get the frequency in Hertz. 116 | open func setFrequency(_ hz: b2Float) { 117 | m_frequencyHz = hz 118 | } 119 | open var frequency: b2Float { 120 | get { 121 | return m_frequencyHz 122 | } 123 | set { 124 | setFrequency(newValue) 125 | } 126 | } 127 | 128 | /// Set/get the damping ratio (dimensionless). 129 | open func setDampingRatio(_ ratio: b2Float) { 130 | m_dampingRatio = ratio 131 | } 132 | open var dampingRatio: b2Float { 133 | get { 134 | return m_dampingRatio 135 | } 136 | set { 137 | setDampingRatio(newValue) 138 | } 139 | } 140 | 141 | /// The mouse joint does not support dumping. 142 | open override func dump() { print("Mouse joint dumping is not supported.") } 143 | 144 | /// Implement b2Joint::ShiftOrigin 145 | open override func shiftOrigin(_ newOrigin: b2Vec2) { 146 | m_targetA -= newOrigin 147 | } 148 | 149 | // MARK: private methods 150 | init(_ def: b2MouseJointDef) { 151 | assert(def.target.isValid()) 152 | assert(b2IsValid(def.maxForce) && def.maxForce >= 0.0) 153 | assert(b2IsValid(def.frequencyHz) && def.frequencyHz >= 0.0) 154 | assert(b2IsValid(def.dampingRatio) && def.dampingRatio >= 0.0) 155 | 156 | m_targetA = def.target 157 | 158 | m_maxForce = def.maxForce 159 | m_impulse = b2Vec2(0.0, 0.0) 160 | 161 | m_frequencyHz = def.frequencyHz 162 | m_dampingRatio = def.dampingRatio 163 | 164 | m_beta = 0.0 165 | m_gamma = 0.0 166 | super.init(def) 167 | m_localAnchorB = b2MulT(m_bodyB.transform, m_targetA) 168 | } 169 | 170 | override func initVelocityConstraints(_ data: inout b2SolverData) { 171 | m_indexB = m_bodyB.m_islandIndex 172 | m_localCenterB = m_bodyB.m_sweep.localCenter 173 | m_invMassB = m_bodyB.m_invMass 174 | m_invIB = m_bodyB.m_invI 175 | 176 | let cB = data.positions[m_indexB].c 177 | let aB = data.positions[m_indexB].a 178 | var vB = data.velocities[m_indexB].v 179 | var wB = data.velocities[m_indexB].w 180 | 181 | let qB = b2Rot(aB) 182 | 183 | let mass = m_bodyB.mass 184 | 185 | // Frequency 186 | let omega = 2.0 * b2_pi * m_frequencyHz 187 | 188 | // Damping coefficient 189 | let d = 2.0 * mass * m_dampingRatio * omega 190 | 191 | // Spring stiffness 192 | let k = mass * (omega * omega) 193 | 194 | // magic formulas 195 | // gamma has units of inverse mass. 196 | // beta has units of inverse time. 197 | let h = data.step.dt 198 | assert(d + h * k > b2_epsilon) 199 | m_gamma = h * (d + h * k) 200 | if m_gamma != 0.0 { 201 | m_gamma = 1.0 / m_gamma 202 | } 203 | m_beta = h * k * m_gamma 204 | 205 | // Compute the effective mass matrix. 206 | m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB) 207 | 208 | // K = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)] 209 | // = [1/m1+1/m2 0 ] + invI1 * [r1.y*r1.y -r1.x*r1.y] + invI2 * [r1.y*r1.y -r1.x*r1.y] 210 | // [ 0 1/m1+1/m2] [-r1.x*r1.y r1.x*r1.x] [-r1.x*r1.y r1.x*r1.x] 211 | var K = b2Mat22() 212 | K.ex.x = m_invMassB + m_invIB * m_rB.y * m_rB.y + m_gamma 213 | K.ex.y = -m_invIB * m_rB.x * m_rB.y 214 | K.ey.x = K.ex.y 215 | K.ey.y = m_invMassB + m_invIB * m_rB.x * m_rB.x + m_gamma 216 | 217 | m_mass = K.getInverse() 218 | 219 | m_C = cB + m_rB - m_targetA 220 | m_C *= m_beta 221 | 222 | // Cheat with some damping 223 | wB *= 0.98 224 | 225 | if data.step.warmStarting { 226 | m_impulse *= data.step.dtRatio 227 | vB += m_invMassB * m_impulse 228 | wB += m_invIB * b2Cross(m_rB, m_impulse) 229 | } 230 | else { 231 | m_impulse.setZero() 232 | } 233 | 234 | data.velocities[m_indexB].v = vB 235 | data.velocities[m_indexB].w = wB 236 | } 237 | override func solveVelocityConstraints(_ data: inout b2SolverData) { 238 | var vB = data.velocities[m_indexB].v 239 | var wB = data.velocities[m_indexB].w 240 | 241 | // Cdot = v + cross(w, r) 242 | let Cdot = vB + b2Cross(wB, m_rB) 243 | var impulse = b2Mul(m_mass, -(Cdot + m_C + m_gamma * m_impulse)) 244 | 245 | let oldImpulse = m_impulse 246 | m_impulse += impulse 247 | let maxImpulse = data.step.dt * m_maxForce 248 | if m_impulse.lengthSquared() > maxImpulse * maxImpulse { 249 | m_impulse *= maxImpulse / m_impulse.length() 250 | } 251 | impulse = m_impulse - oldImpulse 252 | 253 | vB += m_invMassB * impulse 254 | wB += m_invIB * b2Cross(m_rB, impulse) 255 | 256 | data.velocities[m_indexB].v = vB 257 | data.velocities[m_indexB].w = wB 258 | } 259 | override func solvePositionConstraints(_ data: inout b2SolverData) -> Bool { 260 | return true 261 | } 262 | 263 | // MARK: private variables 264 | var m_localAnchorB: b2Vec2! 265 | var m_targetA: b2Vec2 266 | var m_frequencyHz: b2Float 267 | var m_dampingRatio: b2Float 268 | var m_beta: b2Float 269 | 270 | // Solver shared 271 | var m_impulse: b2Vec2 272 | var m_maxForce: b2Float 273 | var m_gamma: b2Float 274 | 275 | // Solver temp 276 | var m_indexA: Int = 0 277 | var m_indexB: Int = 0 278 | var m_rB = b2Vec2() 279 | var m_localCenterB = b2Vec2() 280 | var m_invMassB: b2Float = 0.0 281 | var m_invIB: b2Float = 0.0 282 | var m_mass = b2Mat22() 283 | var m_C = b2Vec2() 284 | } 285 | -------------------------------------------------------------------------------- /Sources/Box2D/Dynamics/Joints/b2PulleyJoint.swift: -------------------------------------------------------------------------------- 1 | /** 2 | Copyright (c) 2006-2014 Erin Catto http://www.box2d.org 3 | Copyright (c) 2015 - Yohei Yoshihara 4 | 5 | This software is provided 'as-is', without any express or implied 6 | warranty. In no event will the authors be held liable for any damages 7 | arising from the use of this software. 8 | 9 | Permission is granted to anyone to use this software for any purpose, 10 | including commercial applications, and to alter it and redistribute it 11 | freely, subject to the following restrictions: 12 | 13 | 1. The origin of this software must not be misrepresented; you must not 14 | claim that you wrote the original software. If you use this software 15 | in a product, an acknowledgment in the product documentation would be 16 | appreciated but is not required. 17 | 18 | 2. Altered source versions must be plainly marked as such, and must not be 19 | misrepresented as being the original software. 20 | 21 | 3. This notice may not be removed or altered from any source distribution. 22 | 23 | This version of box2d was developed by Yohei Yoshihara. It is based upon 24 | the original C++ code written by Erin Catto. 25 | */ 26 | 27 | import Foundation 28 | 29 | let b2_minPulleyLength: b2Float = 2.0 30 | 31 | /// Pulley joint definition. This requires two ground anchors, 32 | /// two dynamic body anchor points, and a pulley ratio. 33 | open class b2PulleyJointDef : b2JointDef { 34 | public override init() { 35 | groundAnchorA = b2Vec2(-1.0, 1.0) 36 | groundAnchorB = b2Vec2(1.0, 1.0) 37 | localAnchorA = b2Vec2(-1.0, 0.0) 38 | localAnchorB = b2Vec2(1.0, 0.0) 39 | lengthA = 0.0 40 | lengthB = 0.0 41 | ratio = 1.0 42 | super.init() 43 | type = b2JointType.pulleyJoint 44 | collideConnected = true 45 | } 46 | 47 | /// Initialize the bodies, anchors, lengths, max lengths, and ratio using the world anchors. 48 | public convenience init(bodyA: b2Body, bodyB: b2Body, groundAnchorA: b2Vec2, groundAnchorB: b2Vec2, anchorA: b2Vec2, anchorB: b2Vec2, ratio: b2Float) { 49 | self.init() 50 | initialize(bodyA: bodyA, bodyB: bodyB, groundAnchorA: groundAnchorA, groundAnchorB: groundAnchorB, anchorA: anchorA, anchorB: anchorB, ratio: ratio) 51 | } 52 | 53 | /// Initialize the bodies, anchors, lengths, max lengths, and ratio using the world anchors. 54 | open func initialize(bodyA: b2Body, bodyB: b2Body, groundAnchorA: b2Vec2, groundAnchorB: b2Vec2, anchorA: b2Vec2, anchorB: b2Vec2, ratio: b2Float) { 55 | self.bodyA = bodyA 56 | self.bodyB = bodyB 57 | self.groundAnchorA = groundAnchorA 58 | self.groundAnchorB = groundAnchorB 59 | self.localAnchorA = self.bodyA.getLocalPoint(anchorA) 60 | self.localAnchorB = self.bodyB.getLocalPoint(anchorB) 61 | let dA = anchorA - groundAnchorA 62 | self.lengthA = dA.length() 63 | let dB = anchorB - groundAnchorB 64 | self.lengthB = dB.length() 65 | self.ratio = ratio 66 | assert(ratio > b2_epsilon) 67 | } 68 | 69 | /// The first ground anchor in world coordinates. This point never moves. 70 | open var groundAnchorA: b2Vec2 71 | 72 | /// The second ground anchor in world coordinates. This point never moves. 73 | open var groundAnchorB: b2Vec2 74 | 75 | /// The local anchor point relative to bodyA's origin. 76 | open var localAnchorA: b2Vec2 77 | 78 | /// The local anchor point relative to bodyB's origin. 79 | open var localAnchorB: b2Vec2 80 | 81 | /// The a reference length for the segment attached to bodyA. 82 | open var lengthA: b2Float 83 | 84 | /// The a reference length for the segment attached to bodyB. 85 | open var lengthB: b2Float 86 | 87 | /// The pulley ratio, used to simulate a block-and-tackle. 88 | open var ratio: b2Float 89 | } 90 | 91 | // MARK: - 92 | /// The pulley joint is connected to two bodies and two fixed ground points. 93 | /// The pulley supports a ratio such that: 94 | /// length1 + ratio * length2 <= constant 95 | /// Yes, the force transmitted is scaled by the ratio. 96 | /// Warning: the pulley joint can get a bit squirrelly by itself. They often 97 | /// work better when combined with prismatic joints. You should also cover the 98 | /// the anchor points with static shapes to prevent one side from going to 99 | /// zero length. 100 | open class b2PulleyJoint : b2Joint { 101 | open override var anchorA: b2Vec2 { 102 | return m_bodyA.getWorldPoint(m_localAnchorA) 103 | } 104 | open override var anchorB: b2Vec2 { 105 | return m_bodyB.getWorldPoint(m_localAnchorB) 106 | } 107 | 108 | open override func getReactionForce(inverseTimeStep inv_dt: b2Float) -> b2Vec2 { 109 | let P = m_impulse * m_uB 110 | return inv_dt * P 111 | } 112 | open override func getReactionTorque(inverseTimeStep inv_dt: b2Float) -> b2Float { 113 | return 0.0 114 | } 115 | 116 | /// Get the first ground anchor. 117 | open var groundAnchorA: b2Vec2 { 118 | return m_groundAnchorA 119 | } 120 | 121 | /// Get the second ground anchor. 122 | open var groundAnchorB: b2Vec2 { 123 | return m_groundAnchorB 124 | } 125 | 126 | /// Get the current length of the segment attached to bodyA. 127 | open var lengthA: b2Float { 128 | return m_lengthA 129 | } 130 | 131 | /// Get the current length of the segment attached to bodyB. 132 | open var lengthB: b2Float { 133 | return m_lengthB 134 | } 135 | 136 | /// Get the pulley ratio. 137 | open var ratio: b2Float { 138 | return m_ratio 139 | } 140 | 141 | /// Get the current length of the segment attached to bodyA. 142 | open var currentLengthA: b2Float { 143 | let p = m_bodyA.getWorldPoint(m_localAnchorA) 144 | let s = m_groundAnchorA 145 | let d = p - s 146 | return d.length() 147 | } 148 | 149 | /// Get the current length of the segment attached to bodyB. 150 | open var currentLengthB: b2Float { 151 | let p = m_bodyB.getWorldPoint(m_localAnchorB) 152 | let s = m_groundAnchorB 153 | let d = p - s 154 | return d.length() 155 | } 156 | 157 | /// Dump joint to dmLog 158 | open override func dump() { 159 | let indexA = m_bodyA.m_islandIndex 160 | let indexB = m_bodyB.m_islandIndex 161 | 162 | print(" b2PulleyJointDef jd;") 163 | print(" jd.bodyA = bodies[\(indexA)];") 164 | print(" jd.bodyB = bodies[\(indexB)];") 165 | print(" jd.collideConnected = bool(\(m_collideConnected));") 166 | print(" jd.groundAnchorA.set(\(m_groundAnchorA.x), \(m_groundAnchorA.y));") 167 | print(" jd.groundAnchorB.set(\(m_groundAnchorB.x), \(m_groundAnchorB.y));") 168 | print(" jd.localAnchorA.set(\(m_localAnchorA.x), \(m_localAnchorA.y));") 169 | print(" jd.localAnchorB.set(\(m_localAnchorB.x), \(m_localAnchorB.y));") 170 | print(" jd.lengthA = \(m_lengthA);") 171 | print(" jd.lengthB = \(m_lengthB);") 172 | print(" jd.ratio = \(m_ratio);") 173 | print(" joints[\(m_index)] = m_world->createJoint(&jd);") 174 | } 175 | 176 | /// Implement b2Joint::ShiftOrigin 177 | open override func shiftOrigin(_ newOrigin: b2Vec2) { 178 | m_groundAnchorA -= newOrigin 179 | m_groundAnchorB -= newOrigin 180 | } 181 | 182 | // MARK: private methods 183 | 184 | init(_ def: b2PulleyJointDef) { 185 | m_groundAnchorA = def.groundAnchorA 186 | m_groundAnchorB = def.groundAnchorB 187 | m_localAnchorA = def.localAnchorA 188 | m_localAnchorB = def.localAnchorB 189 | 190 | m_lengthA = def.lengthA 191 | m_lengthB = def.lengthB 192 | 193 | assert(def.ratio != 0.0) 194 | m_ratio = def.ratio 195 | 196 | m_constant = def.lengthA + m_ratio * def.lengthB 197 | 198 | m_impulse = 0.0 199 | super.init(def) 200 | } 201 | 202 | override func initVelocityConstraints(_ data: inout b2SolverData) { 203 | m_indexA = m_bodyA.m_islandIndex 204 | m_indexB = m_bodyB.m_islandIndex 205 | m_localCenterA = m_bodyA.m_sweep.localCenter 206 | m_localCenterB = m_bodyB.m_sweep.localCenter 207 | m_invMassA = m_bodyA.m_invMass 208 | m_invMassB = m_bodyB.m_invMass 209 | m_invIA = m_bodyA.m_invI 210 | m_invIB = m_bodyB.m_invI 211 | 212 | let cA = data.positions[m_indexA].c 213 | let aA = data.positions[m_indexA].a 214 | var vA = data.velocities[m_indexA].v 215 | var wA = data.velocities[m_indexA].w 216 | 217 | let cB = data.positions[m_indexB].c 218 | let aB = data.positions[m_indexB].a 219 | var vB = data.velocities[m_indexB].v 220 | var wB = data.velocities[m_indexB].w 221 | 222 | let qA = b2Rot(aA), qB = b2Rot(aB) 223 | 224 | m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA) 225 | m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB) 226 | 227 | // Get the pulley axes. 228 | m_uA = cA + m_rA - m_groundAnchorA 229 | m_uB = cB + m_rB - m_groundAnchorB 230 | 231 | let lengthA = m_uA.length() 232 | let lengthB = m_uB.length() 233 | 234 | if lengthA > 10.0 * b2_linearSlop { 235 | m_uA *= 1.0 / lengthA 236 | } 237 | else { 238 | m_uA.setZero() 239 | } 240 | 241 | if lengthB > 10.0 * b2_linearSlop { 242 | m_uB *= 1.0 / lengthB 243 | } 244 | else { 245 | m_uB.setZero() 246 | } 247 | 248 | // Compute effective mass. 249 | let ruA = b2Cross(m_rA, m_uA) 250 | let ruB = b2Cross(m_rB, m_uB) 251 | 252 | let mA = m_invMassA + m_invIA * ruA * ruA 253 | let mB = m_invMassB + m_invIB * ruB * ruB 254 | 255 | m_mass = mA + m_ratio * m_ratio * mB 256 | 257 | if m_mass > 0.0 { 258 | m_mass = 1.0 / m_mass 259 | } 260 | 261 | if data.step.warmStarting { 262 | // Scale impulses to support variable time steps. 263 | m_impulse *= data.step.dtRatio 264 | 265 | // Warm starting. 266 | let PA = -(m_impulse) * m_uA 267 | let PB = (-m_ratio * m_impulse) * m_uB 268 | 269 | vA += m_invMassA * PA 270 | wA += m_invIA * b2Cross(m_rA, PA) 271 | vB += m_invMassB * PB 272 | wB += m_invIB * b2Cross(m_rB, PB) 273 | } 274 | else { 275 | m_impulse = 0.0 276 | } 277 | 278 | data.velocities[m_indexA].v = vA 279 | data.velocities[m_indexA].w = wA 280 | data.velocities[m_indexB].v = vB 281 | data.velocities[m_indexB].w = wB 282 | } 283 | override func solveVelocityConstraints(_ data: inout b2SolverData) { 284 | var vA = data.velocities[m_indexA].v 285 | var wA = data.velocities[m_indexA].w 286 | var vB = data.velocities[m_indexB].v 287 | var wB = data.velocities[m_indexB].w 288 | 289 | let vpA = vA + b2Cross(wA, m_rA) 290 | let vpB = vB + b2Cross(wB, m_rB) 291 | 292 | let Cdot = -b2Dot(m_uA, vpA) - m_ratio * b2Dot(m_uB, vpB) 293 | let impulse = -m_mass * Cdot 294 | m_impulse += impulse 295 | 296 | let PA = -impulse * m_uA 297 | let PB = -m_ratio * impulse * m_uB 298 | vA += m_invMassA * PA 299 | wA += m_invIA * b2Cross(m_rA, PA) 300 | vB += m_invMassB * PB 301 | wB += m_invIB * b2Cross(m_rB, PB) 302 | 303 | data.velocities[m_indexA].v = vA 304 | data.velocities[m_indexA].w = wA 305 | data.velocities[m_indexB].v = vB 306 | data.velocities[m_indexB].w = wB 307 | } 308 | override func solvePositionConstraints(_ data: inout b2SolverData) -> Bool { 309 | var cA = data.positions[m_indexA].c 310 | var aA = data.positions[m_indexA].a 311 | var cB = data.positions[m_indexB].c 312 | var aB = data.positions[m_indexB].a 313 | 314 | let qA = b2Rot(aA), qB = b2Rot(aB) 315 | 316 | let rA = b2Mul(qA, m_localAnchorA - m_localCenterA) 317 | let rB = b2Mul(qB, m_localAnchorB - m_localCenterB) 318 | 319 | // Get the pulley axes. 320 | var uA = cA + rA - m_groundAnchorA 321 | var uB = cB + rB - m_groundAnchorB 322 | 323 | let lengthA = uA.length() 324 | let lengthB = uB.length() 325 | 326 | if lengthA > 10.0 * b2_linearSlop { 327 | uA *= 1.0 / lengthA 328 | } 329 | else { 330 | uA.setZero() 331 | } 332 | 333 | if lengthB > 10.0 * b2_linearSlop { 334 | uB *= 1.0 / lengthB 335 | } 336 | else { 337 | uB.setZero() 338 | } 339 | 340 | // Compute effective mass. 341 | let ruA = b2Cross(rA, uA) 342 | let ruB = b2Cross(rB, uB) 343 | 344 | let mA = m_invMassA + m_invIA * ruA * ruA 345 | let mB = m_invMassB + m_invIB * ruB * ruB 346 | 347 | var mass = mA + m_ratio * m_ratio * mB 348 | 349 | if mass > 0.0 { 350 | mass = 1.0 / mass 351 | } 352 | 353 | let C = m_constant - lengthA - m_ratio * lengthB 354 | let linearError = abs(C) 355 | 356 | let impulse = -mass * C 357 | 358 | let PA = -impulse * uA 359 | let PB = -m_ratio * impulse * uB 360 | 361 | cA += m_invMassA * PA 362 | aA += m_invIA * b2Cross(rA, PA) 363 | cB += m_invMassB * PB 364 | aB += m_invIB * b2Cross(rB, PB) 365 | 366 | data.positions[m_indexA].c = cA 367 | data.positions[m_indexA].a = aA 368 | data.positions[m_indexB].c = cB 369 | data.positions[m_indexB].a = aB 370 | 371 | return linearError < b2_linearSlop 372 | } 373 | 374 | // MARK: private variables 375 | 376 | var m_groundAnchorA: b2Vec2 377 | var m_groundAnchorB: b2Vec2 378 | var m_lengthA: b2Float 379 | var m_lengthB: b2Float 380 | 381 | // Solver shared 382 | var m_localAnchorA: b2Vec2 383 | var m_localAnchorB: b2Vec2 384 | var m_constant: b2Float 385 | var m_ratio: b2Float 386 | var m_impulse: b2Float 387 | 388 | // Solver temp 389 | var m_indexA: Int = 0 390 | var m_indexB: Int = 0 391 | var m_uA = b2Vec2() 392 | var m_uB = b2Vec2() 393 | var m_rA = b2Vec2() 394 | var m_rB = b2Vec2() 395 | var m_localCenterA = b2Vec2() 396 | var m_localCenterB = b2Vec2() 397 | var m_invMassA: b2Float = 0.0 398 | var m_invMassB: b2Float = 0.0 399 | var m_invIA: b2Float = 0.0 400 | var m_invIB: b2Float = 0.0 401 | var m_mass: b2Float = 0.0 402 | } 403 | -------------------------------------------------------------------------------- /Sources/Box2D/Dynamics/Joints/b2RopeJoint.swift: -------------------------------------------------------------------------------- 1 | /** 2 | Copyright (c) 2006-2014 Erin Catto http://www.box2d.org 3 | Copyright (c) 2015 - Yohei Yoshihara 4 | 5 | This software is provided 'as-is', without any express or implied 6 | warranty. In no event will the authors be held liable for any damages 7 | arising from the use of this software. 8 | 9 | Permission is granted to anyone to use this software for any purpose, 10 | including commercial applications, and to alter it and redistribute it 11 | freely, subject to the following restrictions: 12 | 13 | 1. The origin of this software must not be misrepresented; you must not 14 | claim that you wrote the original software. If you use this software 15 | in a product, an acknowledgment in the product documentation would be 16 | appreciated but is not required. 17 | 18 | 2. Altered source versions must be plainly marked as such, and must not be 19 | misrepresented as being the original software. 20 | 21 | 3. This notice may not be removed or altered from any source distribution. 22 | 23 | This version of box2d was developed by Yohei Yoshihara. It is based upon 24 | the original C++ code written by Erin Catto. 25 | */ 26 | 27 | import Foundation 28 | 29 | /// Rope joint definition. This requires two body anchor points and 30 | /// a maximum lengths. 31 | /// Note: by default the connected objects will not collide. 32 | /// see collideConnected in b2JointDef. 33 | open class b2RopeJointDef : b2JointDef { 34 | public override init() { 35 | localAnchorA = b2Vec2(-1.0, 0.0) 36 | localAnchorB = b2Vec2(1.0, 0.0) 37 | maxLength = 0.0 38 | super.init() 39 | type = b2JointType.ropeJoint 40 | } 41 | 42 | /// The local anchor point relative to bodyA's origin. 43 | open var localAnchorA: b2Vec2 44 | 45 | /// The local anchor point relative to bodyB's origin. 46 | open var localAnchorB: b2Vec2 47 | 48 | /// The maximum length of the rope. 49 | /// Warning: this must be larger than b2_linearSlop or 50 | /// the joint will have no effect. 51 | open var maxLength: b2Float 52 | } 53 | 54 | // MARK: - 55 | /// A rope joint enforces a maximum distance between two points 56 | /// on two bodies. It has no other effect. 57 | /// Warning: if you attempt to change the maximum length during 58 | /// the simulation you will get some non-physical behavior. 59 | /// A model that would allow you to dynamically modify the length 60 | /// would have some sponginess, so I chose not to implement it 61 | /// that way. See b2DistanceJoint if you want to dynamically 62 | /// control length. 63 | open class b2RopeJoint : b2Joint { 64 | open override var anchorA: b2Vec2 { 65 | return m_bodyA.getWorldPoint(m_localAnchorA) 66 | } 67 | open override var anchorB: b2Vec2 { 68 | return m_bodyB.getWorldPoint(m_localAnchorB) 69 | } 70 | 71 | open override func getReactionForce(inverseTimeStep inv_dt: b2Float) -> b2Vec2 { 72 | let F = (inv_dt * m_impulse) * m_u 73 | return F 74 | } 75 | open override func getReactionTorque(inverseTimeStep inv_dt: b2Float) -> b2Float { 76 | return 0.0 77 | } 78 | 79 | /// The local anchor point relative to bodyA's origin. 80 | open var localAnchorA: b2Vec2 { return m_localAnchorA } 81 | 82 | /// The local anchor point relative to bodyB's origin. 83 | open var localAnchorB: b2Vec2 { return m_localAnchorB } 84 | 85 | /// Set/Get the maximum length of the rope. 86 | open func setMaxLength(_ length: b2Float) { 87 | m_maxLength = length 88 | } 89 | open var maxLength: b2Float { 90 | get { 91 | return m_maxLength 92 | } 93 | set { 94 | setMaxLength(newValue) 95 | } 96 | } 97 | 98 | open var limitState: b2LimitState { 99 | return m_state 100 | } 101 | 102 | /// Dump joint to dmLog 103 | open override func dump() { 104 | let indexA = m_bodyA.m_islandIndex 105 | let indexB = m_bodyB.m_islandIndex 106 | 107 | print(" b2RopeJointDef jd;") 108 | print(" jd.bodyA = bodies[\(indexA)];") 109 | print(" jd.bodyB = bodies[\(indexB)];") 110 | print(" jd.collideConnected = bool(\(m_collideConnected));") 111 | print(" jd.localAnchorA.set(\(m_localAnchorA.x), \(m_localAnchorA.y));") 112 | print(" jd.localAnchorB.set(\(m_localAnchorB.x), \(m_localAnchorB.y));") 113 | print(" jd.maxLength = \(m_maxLength);") 114 | print(" joints[\(m_index)] = m_world->createJoint(&jd);") 115 | } 116 | 117 | // MARK: private methods 118 | 119 | init(_ def: b2RopeJointDef) { 120 | m_localAnchorA = def.localAnchorA 121 | m_localAnchorB = def.localAnchorB 122 | 123 | m_maxLength = def.maxLength 124 | 125 | m_mass = 0.0 126 | m_impulse = 0.0 127 | m_state = b2LimitState.inactiveLimit 128 | m_length = 0.0 129 | super.init(def) 130 | } 131 | 132 | override func initVelocityConstraints(_ data: inout b2SolverData) { 133 | m_indexA = m_bodyA.m_islandIndex 134 | m_indexB = m_bodyB.m_islandIndex 135 | m_localCenterA = m_bodyA.m_sweep.localCenter 136 | m_localCenterB = m_bodyB.m_sweep.localCenter 137 | m_invMassA = m_bodyA.m_invMass 138 | m_invMassB = m_bodyB.m_invMass 139 | m_invIA = m_bodyA.m_invI 140 | m_invIB = m_bodyB.m_invI 141 | 142 | let cA = data.positions[m_indexA].c 143 | let aA = data.positions[m_indexA].a 144 | var vA = data.velocities[m_indexA].v 145 | var wA = data.velocities[m_indexA].w 146 | 147 | let cB = data.positions[m_indexB].c 148 | let aB = data.positions[m_indexB].a 149 | var vB = data.velocities[m_indexB].v 150 | var wB = data.velocities[m_indexB].w 151 | 152 | let qA = b2Rot(aA), qB = b2Rot(aB) 153 | 154 | m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA) 155 | m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB) 156 | m_u = cB + m_rB - cA - m_rA 157 | 158 | m_length = m_u.length() 159 | 160 | let C = m_length - m_maxLength 161 | if C > 0.0 { 162 | m_state = b2LimitState.atUpperLimit 163 | } 164 | else { 165 | m_state = b2LimitState.inactiveLimit 166 | } 167 | 168 | if m_length > b2_linearSlop { 169 | m_u *= 1.0 / m_length 170 | } 171 | else { 172 | m_u.setZero() 173 | m_mass = 0.0 174 | m_impulse = 0.0 175 | return 176 | } 177 | 178 | // Compute effective mass. 179 | let crA = b2Cross(m_rA, m_u) 180 | let crB = b2Cross(m_rB, m_u) 181 | let invMass = m_invMassA + m_invIA * crA * crA + m_invMassB + m_invIB * crB * crB 182 | 183 | m_mass = invMass != 0.0 ? 1.0 / invMass : 0.0 184 | 185 | if data.step.warmStarting { 186 | // Scale the impulse to support a variable time step. 187 | m_impulse *= data.step.dtRatio 188 | 189 | let P = m_impulse * m_u 190 | vA -= m_invMassA * P 191 | wA -= m_invIA * b2Cross(m_rA, P) 192 | vB += m_invMassB * P 193 | wB += m_invIB * b2Cross(m_rB, P) 194 | } 195 | else { 196 | m_impulse = 0.0 197 | } 198 | 199 | data.velocities[m_indexA].v = vA 200 | data.velocities[m_indexA].w = wA 201 | data.velocities[m_indexB].v = vB 202 | data.velocities[m_indexB].w = wB 203 | } 204 | override func solveVelocityConstraints(_ data: inout b2SolverData) { 205 | var vA = data.velocities[m_indexA].v 206 | var wA = data.velocities[m_indexA].w 207 | var vB = data.velocities[m_indexB].v 208 | var wB = data.velocities[m_indexB].w 209 | 210 | // Cdot = dot(u, v + cross(w, r)) 211 | let vpA = vA + b2Cross(wA, m_rA) 212 | let vpB = vB + b2Cross(wB, m_rB) 213 | let C = m_length - m_maxLength 214 | var Cdot = b2Dot(m_u, vpB - vpA) 215 | 216 | // Predictive constraint. 217 | if C < 0.0 { 218 | Cdot += data.step.inv_dt * C 219 | } 220 | 221 | var impulse = -m_mass * Cdot 222 | let oldImpulse = m_impulse 223 | m_impulse = min(0.0, m_impulse + impulse) 224 | impulse = m_impulse - oldImpulse 225 | 226 | let P = impulse * m_u 227 | vA -= m_invMassA * P 228 | wA -= m_invIA * b2Cross(m_rA, P) 229 | vB += m_invMassB * P 230 | wB += m_invIB * b2Cross(m_rB, P) 231 | 232 | data.velocities[m_indexA].v = vA 233 | data.velocities[m_indexA].w = wA 234 | data.velocities[m_indexB].v = vB 235 | data.velocities[m_indexB].w = wB 236 | } 237 | override func solvePositionConstraints(_ data: inout b2SolverData) -> Bool { 238 | var cA = data.positions[m_indexA].c 239 | var aA = data.positions[m_indexA].a 240 | var cB = data.positions[m_indexB].c 241 | var aB = data.positions[m_indexB].a 242 | 243 | let qA = b2Rot(aA), qB = b2Rot(aB) 244 | 245 | let rA = b2Mul(qA, m_localAnchorA - m_localCenterA) 246 | let rB = b2Mul(qB, m_localAnchorB - m_localCenterB) 247 | var u = cB + rB - cA - rA 248 | 249 | let length = u.normalize() 250 | var C = length - m_maxLength 251 | 252 | C = b2Clamp(C, 0.0, b2_maxLinearCorrection) 253 | 254 | let impulse = -m_mass * C 255 | let P = impulse * u 256 | 257 | cA -= m_invMassA * P 258 | aA -= m_invIA * b2Cross(rA, P) 259 | cB += m_invMassB * P 260 | aB += m_invIB * b2Cross(rB, P) 261 | 262 | data.positions[m_indexA].c = cA 263 | data.positions[m_indexA].a = aA 264 | data.positions[m_indexB].c = cB 265 | data.positions[m_indexB].a = aB 266 | 267 | return length - m_maxLength < b2_linearSlop 268 | } 269 | 270 | // MARK: private variables 271 | 272 | // Solver shared 273 | var m_localAnchorA: b2Vec2 274 | var m_localAnchorB: b2Vec2 275 | var m_maxLength: b2Float 276 | var m_length: b2Float 277 | var m_impulse: b2Float 278 | 279 | // Solver temp 280 | var m_indexA: Int = 0 281 | var m_indexB: Int = 0 282 | var m_u = b2Vec2() 283 | var m_rA = b2Vec2() 284 | var m_rB = b2Vec2() 285 | var m_localCenterA = b2Vec2() 286 | var m_localCenterB = b2Vec2() 287 | var m_invMassA: b2Float = 0.0 288 | var m_invMassB: b2Float = 0.0 289 | var m_invIA: b2Float = 0.0 290 | var m_invIB: b2Float = 0.0 291 | var m_mass: b2Float = 0.0 292 | var m_state = b2LimitState.inactiveLimit 293 | } 294 | 295 | -------------------------------------------------------------------------------- /Sources/Box2D/Dynamics/Joints/b2WeldJoint.swift: -------------------------------------------------------------------------------- 1 | /** 2 | Copyright (c) 2006-2014 Erin Catto http://www.box2d.org 3 | Copyright (c) 2015 - Yohei Yoshihara 4 | 5 | This software is provided 'as-is', without any express or implied 6 | warranty. In no event will the authors be held liable for any damages 7 | arising from the use of this software. 8 | 9 | Permission is granted to anyone to use this software for any purpose, 10 | including commercial applications, and to alter it and redistribute it 11 | freely, subject to the following restrictions: 12 | 13 | 1. The origin of this software must not be misrepresented; you must not 14 | claim that you wrote the original software. If you use this software 15 | in a product, an acknowledgment in the product documentation would be 16 | appreciated but is not required. 17 | 18 | 2. Altered source versions must be plainly marked as such, and must not be 19 | misrepresented as being the original software. 20 | 21 | 3. This notice may not be removed or altered from any source distribution. 22 | 23 | This version of box2d was developed by Yohei Yoshihara. It is based upon 24 | the original C++ code written by Erin Catto. 25 | */ 26 | 27 | import Foundation 28 | 29 | /// Weld joint definition. You need to specify local anchor points 30 | /// where they are attached and the relative body angle. The position 31 | /// of the anchor points is important for computing the reaction torque. 32 | open class b2WeldJointDef : b2JointDef { 33 | public override init() { 34 | localAnchorA = b2Vec2(0.0, 0.0) 35 | localAnchorB = b2Vec2(0.0, 0.0) 36 | referenceAngle = 0.0 37 | frequencyHz = 0.0 38 | dampingRatio = 0.0 39 | super.init() 40 | type = b2JointType.weldJoint 41 | } 42 | 43 | /// Initialize the bodies, anchors, and reference angle using a world 44 | /// anchor point. 45 | public convenience init(bodyA: b2Body, bodyB: b2Body, anchor: b2Vec2) { 46 | self.init() 47 | initialize(bodyA: bodyA, bodyB: bodyB, anchor: anchor) 48 | } 49 | 50 | /// Initialize the bodies, anchors, and reference angle using a world 51 | /// anchor point. 52 | open func initialize(bodyA: b2Body, bodyB: b2Body, anchor: b2Vec2) { 53 | self.bodyA = bodyA 54 | self.bodyB = bodyB 55 | self.localAnchorA = bodyA.getLocalPoint(anchor) 56 | self.localAnchorB = bodyB.getLocalPoint(anchor) 57 | self.referenceAngle = bodyB.angle - bodyA.angle 58 | } 59 | 60 | /// The local anchor point relative to bodyA's origin. 61 | open var localAnchorA: b2Vec2 62 | 63 | /// The local anchor point relative to bodyB's origin. 64 | open var localAnchorB: b2Vec2 65 | 66 | /// The bodyB angle minus bodyA angle in the reference state (radians). 67 | open var referenceAngle: b2Float 68 | 69 | /// The mass-spring-damper frequency in Hertz. Rotation only. 70 | /// Disable softness with a value of 0. 71 | open var frequencyHz: b2Float 72 | 73 | /// The damping ratio. 0 = no damping, 1 = critical damping. 74 | open var dampingRatio: b2Float 75 | } 76 | 77 | // MARK: - 78 | /// A weld joint essentially glues two bodies together. A weld joint may 79 | /// distort somewhat because the island constraint solver is approximate. 80 | open class b2WeldJoint : b2Joint { 81 | open override var anchorA: b2Vec2 { 82 | return m_bodyA.getWorldPoint(m_localAnchorA) 83 | } 84 | open override var anchorB: b2Vec2 { 85 | return m_bodyB.getWorldPoint(m_localAnchorB) 86 | } 87 | 88 | open override func getReactionForce(inverseTimeStep inv_dt: b2Float) -> b2Vec2 { 89 | let P = b2Vec2(m_impulse.x, m_impulse.y) 90 | return inv_dt * P 91 | } 92 | open override func getReactionTorque(inverseTimeStep inv_dt: b2Float) -> b2Float { 93 | return inv_dt * m_impulse.z 94 | } 95 | 96 | /// The local anchor point relative to bodyA's origin. 97 | open var localAnchorA: b2Vec2 { return m_localAnchorA } 98 | 99 | /// The local anchor point relative to bodyB's origin. 100 | open var localAnchorB: b2Vec2 { return m_localAnchorB } 101 | 102 | /// Get the reference angle. 103 | open var referenceAngle: b2Float { return m_referenceAngle } 104 | 105 | /// Set/get frequency in Hz. 106 | open func setFrequency(_ hz: b2Float) { m_frequencyHz = hz } 107 | open var frequency: b2Float { return m_frequencyHz } 108 | 109 | /// Set/get damping ratio. 110 | open func setDampingRatio(_ ratio: b2Float) { m_dampingRatio = ratio } 111 | open var dampingRatio: b2Float { return m_dampingRatio } 112 | 113 | /// Dump to println 114 | open override func dump() { 115 | let indexA = m_bodyA.m_islandIndex 116 | let indexB = m_bodyB.m_islandIndex 117 | 118 | print(" b2WeldJointDef jd;") 119 | print(" jd.bodyA = bodies[\(indexA)];") 120 | print(" jd.bodyB = bodies[\(indexB)];") 121 | print(" jd.collideConnected = bool(\(m_collideConnected));") 122 | print(" jd.localAnchorA.set(\(m_localAnchorA.x), \(m_localAnchorA.y));") 123 | print(" jd.localAnchorB.set(\(m_localAnchorB.x), \(m_localAnchorB.y));") 124 | print(" jd.referenceAngle = \(m_referenceAngle);") 125 | print(" jd.frequencyHz = \(m_frequencyHz);") 126 | print(" jd.dampingRatio = \(m_dampingRatio);") 127 | print(" joints[\(m_index)] = m_world->createJoint(&jd);") 128 | } 129 | 130 | // MARK: private methods 131 | 132 | init(_ def: b2WeldJointDef) { 133 | m_localAnchorA = def.localAnchorA 134 | m_localAnchorB = def.localAnchorB 135 | m_referenceAngle = def.referenceAngle 136 | m_frequencyHz = def.frequencyHz 137 | m_dampingRatio = def.dampingRatio 138 | 139 | m_impulse = b2Vec3(0.0, 0.0, 0.0) 140 | super.init(def) 141 | } 142 | 143 | override func initVelocityConstraints(_ data: inout b2SolverData) { 144 | m_indexA = m_bodyA.m_islandIndex 145 | m_indexB = m_bodyB.m_islandIndex 146 | m_localCenterA = m_bodyA.m_sweep.localCenter 147 | m_localCenterB = m_bodyB.m_sweep.localCenter 148 | m_invMassA = m_bodyA.m_invMass 149 | m_invMassB = m_bodyB.m_invMass 150 | m_invIA = m_bodyA.m_invI 151 | m_invIB = m_bodyB.m_invI 152 | 153 | let aA = data.positions[m_indexA].a 154 | var vA = data.velocities[m_indexA].v 155 | var wA = data.velocities[m_indexA].w 156 | 157 | let aB = data.positions[m_indexB].a 158 | var vB = data.velocities[m_indexB].v 159 | var wB = data.velocities[m_indexB].w 160 | 161 | let qA = b2Rot(aA), qB = b2Rot(aB) 162 | 163 | m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA) 164 | m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB) 165 | 166 | // J = [-I -r1_skew I r2_skew] 167 | // [ 0 -1 0 1] 168 | // r_skew = [-ry; rx] 169 | 170 | // Matlab 171 | // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB] 172 | // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB] 173 | // [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB] 174 | 175 | let mA = m_invMassA, mB = m_invMassB 176 | let iA = m_invIA, iB = m_invIB 177 | 178 | var K = b2Mat33() 179 | K.ex.x = mA + mB + m_rA.y * m_rA.y * iA + m_rB.y * m_rB.y * iB 180 | K.ey.x = -m_rA.y * m_rA.x * iA - m_rB.y * m_rB.x * iB 181 | K.ez.x = -m_rA.y * iA - m_rB.y * iB 182 | K.ex.y = K.ey.x 183 | K.ey.y = mA + mB + m_rA.x * m_rA.x * iA + m_rB.x * m_rB.x * iB 184 | K.ez.y = m_rA.x * iA + m_rB.x * iB 185 | K.ex.z = K.ez.x 186 | K.ey.z = K.ez.y 187 | K.ez.z = iA + iB 188 | 189 | if m_frequencyHz > 0.0 { 190 | m_mass = K.getInverse22() 191 | 192 | var invM = iA + iB 193 | let m = invM > 0.0 ? 1.0 / invM : 0.0 194 | 195 | let C = aB - aA - m_referenceAngle 196 | 197 | // Frequency 198 | let omega = 2.0 * b2_pi * m_frequencyHz 199 | 200 | // Damping coefficient 201 | let d = 2.0 * m * m_dampingRatio * omega 202 | 203 | // Spring stiffness 204 | let k = m * omega * omega 205 | 206 | // magic formulas 207 | let h = data.step.dt 208 | m_gamma = h * (d + h * k) 209 | m_gamma = m_gamma != 0.0 ? 1.0 / m_gamma : 0.0 210 | m_bias = C * h * k * m_gamma 211 | 212 | invM += m_gamma 213 | m_mass.ez.z = invM != 0.0 ? 1.0 / invM : 0.0 214 | } 215 | else { 216 | m_mass = K.getSymInverse33() 217 | m_gamma = 0.0 218 | m_bias = 0.0 219 | } 220 | 221 | if data.step.warmStarting { 222 | // Scale impulses to support a variable time step. 223 | m_impulse *= data.step.dtRatio 224 | 225 | let P = b2Vec2(m_impulse.x, m_impulse.y) 226 | 227 | vA -= mA * P 228 | wA -= iA * (b2Cross(m_rA, P) + m_impulse.z) 229 | 230 | vB += mB * P 231 | wB += iB * (b2Cross(m_rB, P) + m_impulse.z) 232 | } 233 | else { 234 | m_impulse.setZero() 235 | } 236 | 237 | data.velocities[m_indexA].v = vA 238 | data.velocities[m_indexA].w = wA 239 | data.velocities[m_indexB].v = vB 240 | data.velocities[m_indexB].w = wB 241 | } 242 | override func solveVelocityConstraints(_ data: inout b2SolverData) { 243 | var vA = data.velocities[m_indexA].v 244 | var wA = data.velocities[m_indexA].w 245 | var vB = data.velocities[m_indexB].v 246 | var wB = data.velocities[m_indexB].w 247 | 248 | let mA = m_invMassA, mB = m_invMassB 249 | let iA = m_invIA, iB = m_invIB 250 | 251 | if m_frequencyHz > 0.0 { 252 | let Cdot2 = wB - wA 253 | 254 | let impulse2 = -m_mass.ez.z * (Cdot2 + m_bias + m_gamma * m_impulse.z) 255 | m_impulse.z += impulse2 256 | 257 | wA -= iA * impulse2 258 | wB += iB * impulse2 259 | 260 | let Cdot1 = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA) 261 | 262 | let impulse1 = -b2Mul22(m_mass, Cdot1) 263 | m_impulse.x += impulse1.x 264 | m_impulse.y += impulse1.y 265 | 266 | let P = impulse1 267 | 268 | vA -= mA * P 269 | wA -= iA * b2Cross(m_rA, P) 270 | 271 | vB += mB * P 272 | wB += iB * b2Cross(m_rB, P) 273 | } 274 | else { 275 | let Cdot1 = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA) 276 | let Cdot2 = wB - wA 277 | let Cdot = b2Vec3(Cdot1.x, Cdot1.y, Cdot2) 278 | 279 | let impulse = -b2Mul(m_mass, Cdot) 280 | m_impulse += impulse 281 | 282 | let P = b2Vec2(impulse.x, impulse.y) 283 | 284 | vA -= mA * P 285 | wA -= iA * (b2Cross(m_rA, P) + impulse.z) 286 | 287 | vB += mB * P 288 | wB += iB * (b2Cross(m_rB, P) + impulse.z) 289 | } 290 | 291 | data.velocities[m_indexA].v = vA 292 | data.velocities[m_indexA].w = wA 293 | data.velocities[m_indexB].v = vB 294 | data.velocities[m_indexB].w = wB 295 | } 296 | override func solvePositionConstraints(_ data: inout b2SolverData) -> Bool { 297 | var cA = data.positions[m_indexA].c 298 | var aA = data.positions[m_indexA].a 299 | var cB = data.positions[m_indexB].c 300 | var aB = data.positions[m_indexB].a 301 | 302 | let qA = b2Rot(aA), qB = b2Rot(aB) 303 | 304 | let mA = m_invMassA, mB = m_invMassB 305 | let iA = m_invIA, iB = m_invIB 306 | 307 | let rA = b2Mul(qA, m_localAnchorA - m_localCenterA) 308 | let rB = b2Mul(qB, m_localAnchorB - m_localCenterB) 309 | 310 | var positionError: b2Float, angularError: b2Float 311 | 312 | var K = b2Mat33() 313 | K.ex.x = mA + mB + rA.y * rA.y * iA + rB.y * rB.y * iB 314 | K.ey.x = -rA.y * rA.x * iA - rB.y * rB.x * iB 315 | K.ez.x = -rA.y * iA - rB.y * iB 316 | K.ex.y = K.ey.x 317 | K.ey.y = mA + mB + rA.x * rA.x * iA + rB.x * rB.x * iB 318 | K.ez.y = rA.x * iA + rB.x * iB 319 | K.ex.z = K.ez.x 320 | K.ey.z = K.ez.y 321 | K.ez.z = iA + iB 322 | 323 | if m_frequencyHz > 0.0 { 324 | let C1 = cB + rB - cA - rA 325 | 326 | positionError = C1.length() 327 | angularError = 0.0 328 | 329 | let P = -K.solve22(C1) 330 | 331 | cA -= mA * P 332 | aA -= iA * b2Cross(rA, P) 333 | 334 | cB += mB * P 335 | aB += iB * b2Cross(rB, P) 336 | } 337 | else { 338 | let C1 = cB + rB - cA - rA 339 | let C2 = aB - aA - m_referenceAngle 340 | 341 | positionError = C1.length() 342 | angularError = abs(C2) 343 | 344 | let C = b2Vec3(C1.x, C1.y, C2) 345 | 346 | let impulse = -K.solve33(C) 347 | let P = b2Vec2(impulse.x, impulse.y) 348 | 349 | cA -= mA * P 350 | aA -= iA * (b2Cross(rA, P) + impulse.z) 351 | 352 | cB += mB * P 353 | aB += iB * (b2Cross(rB, P) + impulse.z) 354 | } 355 | 356 | data.positions[m_indexA].c = cA 357 | data.positions[m_indexA].a = aA 358 | data.positions[m_indexB].c = cB 359 | data.positions[m_indexB].a = aB 360 | 361 | return positionError <= b2_linearSlop && angularError <= b2_angularSlop 362 | } 363 | 364 | // MARK: private variables 365 | 366 | var m_frequencyHz: b2Float = 0.0 367 | var m_dampingRatio: b2Float = 0.0 368 | var m_bias: b2Float = 0.0 369 | 370 | // Solver shared 371 | var m_localAnchorA = b2Vec2() 372 | var m_localAnchorB = b2Vec2() 373 | var m_referenceAngle: b2Float = 0.0 374 | var m_gamma: b2Float = 0.0 375 | var m_impulse = b2Vec3() 376 | 377 | // Solver temp 378 | var m_indexA: Int = 0 379 | var m_indexB: Int = 0 380 | var m_rA = b2Vec2() 381 | var m_rB = b2Vec2() 382 | var m_localCenterA = b2Vec2() 383 | var m_localCenterB = b2Vec2() 384 | var m_invMassA: b2Float = 0.0 385 | var m_invMassB: b2Float = 0.0 386 | var m_invIA: b2Float = 0.0 387 | var m_invIB: b2Float = 0.0 388 | var m_mass = b2Mat33() 389 | } 390 | -------------------------------------------------------------------------------- /Sources/Box2D/Dynamics/b2ContactManager.swift: -------------------------------------------------------------------------------- 1 | /** 2 | Copyright (c) 2006-2014 Erin Catto http://www.box2d.org 3 | Copyright (c) 2015 - Yohei Yoshihara 4 | 5 | This software is provided 'as-is', without any express or implied 6 | warranty. In no event will the authors be held liable for any damages 7 | arising from the use of this software. 8 | 9 | Permission is granted to anyone to use this software for any purpose, 10 | including commercial applications, and to alter it and redistribute it 11 | freely, subject to the following restrictions: 12 | 13 | 1. The origin of this software must not be misrepresented; you must not 14 | claim that you wrote the original software. If you use this software 15 | in a product, an acknowledgment in the product documentation would be 16 | appreciated but is not required. 17 | 18 | 2. Altered source versions must be plainly marked as such, and must not be 19 | misrepresented as being the original software. 20 | 21 | 3. This notice may not be removed or altered from any source distribution. 22 | 23 | This version of box2d was developed by Yohei Yoshihara. It is based upon 24 | the original C++ code written by Erin Catto. 25 | */ 26 | 27 | import Foundation 28 | 29 | var b2_defaultFilter = b2ContactFilter() 30 | var b2_defaultListener = b2DefaultContactListener() 31 | 32 | // Delegate of b2World. 33 | open class b2ContactManager: b2BroadPhaseWrapper { 34 | init() { 35 | m_contactList = nil 36 | m_contactCount = 0 37 | m_contactFilter = b2_defaultFilter 38 | m_contactListener = b2_defaultListener 39 | } 40 | 41 | // Broad-phase callback. 42 | open func addPair(_ proxyUserDataA: inout b2FixtureProxy, _ proxyUserDataB: inout b2FixtureProxy) { 43 | let proxyA = proxyUserDataA 44 | let proxyB = proxyUserDataB 45 | 46 | var fixtureA = proxyA.fixture 47 | var fixtureB = proxyB.fixture 48 | 49 | var indexA = proxyA.childIndex 50 | var indexB = proxyB.childIndex 51 | 52 | var bodyA = fixtureA.body 53 | var bodyB = fixtureB.body 54 | 55 | // Are the fixtures on the same body? 56 | if bodyA === bodyB { 57 | return 58 | } 59 | 60 | // TODO_ERIN use a hash table to remove a potential bottleneck when both 61 | // bodies have a lot of contacts. 62 | // Does a contact already exist? 63 | var edge = bodyB.getContactList() 64 | 65 | while edge != nil { 66 | if edge!.other === bodyA { 67 | let fA = edge!.contact.fixtureA 68 | let fB = edge!.contact.fixtureB 69 | let iA = edge!.contact.childIndexA 70 | let iB = edge!.contact.childIndexB 71 | 72 | if fA === fixtureA && fB === fixtureB && iA == indexA && iB == indexB { 73 | // A contact already exists. 74 | return 75 | } 76 | 77 | if fA === fixtureB && fB === fixtureA && iA == indexB && iB == indexA { 78 | // A contact already exists. 79 | return 80 | } 81 | } 82 | 83 | edge = edge!.next 84 | } 85 | 86 | // Does a joint override collision? Is at least one body dynamic? 87 | if bodyB.shouldCollide(bodyA) == false { 88 | return 89 | } 90 | 91 | // Check user filtering. 92 | if m_contactFilter != nil && m_contactFilter!.shouldCollide(fixtureA, fixtureB) == false { 93 | return 94 | } 95 | 96 | // Call the factory. 97 | let c = b2Contact.create(fixtureA, indexA, fixtureB, indexB) 98 | if c == nil { 99 | return 100 | } 101 | 102 | // Contact creation may swap fixtures. 103 | fixtureA = c!.fixtureA 104 | fixtureB = c!.fixtureB 105 | indexA = c!.childIndexA 106 | indexB = c!.childIndexB 107 | bodyA = fixtureA.body 108 | bodyB = fixtureB.body 109 | 110 | // Insert into the world. 111 | c!.m_prev = nil 112 | c!.m_next = m_contactList 113 | if m_contactList != nil { 114 | m_contactList!.m_prev = c 115 | } 116 | m_contactList = c 117 | 118 | // Connect to island graph. 119 | 120 | // Connect to body A 121 | c!.m_nodeA.contact = c! 122 | c!.m_nodeA.other = bodyB 123 | 124 | c!.m_nodeA.prev = nil 125 | c!.m_nodeA.next = bodyA.m_contactList 126 | if bodyA.m_contactList != nil { 127 | bodyA.m_contactList!.prev = c!.m_nodeA 128 | } 129 | bodyA.m_contactList = c!.m_nodeA 130 | 131 | // Connect to body B 132 | c!.m_nodeB.contact = c! 133 | c!.m_nodeB.other = bodyA 134 | 135 | c!.m_nodeB.prev = nil 136 | c!.m_nodeB.next = bodyB.m_contactList 137 | if bodyB.m_contactList != nil { 138 | bodyB.m_contactList!.prev = c!.m_nodeB 139 | } 140 | bodyB.m_contactList = c!.m_nodeB 141 | 142 | // Wake up the bodies 143 | if fixtureA.isSensor == false && fixtureB.isSensor == false { 144 | bodyA.setAwake(true) 145 | bodyB.setAwake(true) 146 | } 147 | 148 | m_contactCount += 1 149 | } 150 | 151 | func findNewContacts() { 152 | m_broadPhase.updatePairs(callback: self) 153 | } 154 | 155 | func destroy(_ c: b2Contact) { 156 | let fixtureA = c.fixtureA 157 | let fixtureB = c.fixtureB 158 | let bodyA = fixtureA.body 159 | let bodyB = fixtureB.body 160 | 161 | if m_contactListener != nil && c.isTouching { 162 | m_contactListener!.endContact(c) 163 | } 164 | 165 | // Remove from the world. 166 | if c.m_prev != nil { 167 | c.m_prev!.m_next = c.m_next 168 | } 169 | 170 | if c.m_next != nil { 171 | c.m_next!.m_prev = c.m_prev 172 | } 173 | 174 | if c === m_contactList { 175 | m_contactList = c.m_next 176 | } 177 | 178 | // Remove from body 1 179 | if c.m_nodeA.prev != nil { 180 | c.m_nodeA.prev!.next = c.m_nodeA.next 181 | } 182 | 183 | if c.m_nodeA.next != nil { 184 | c.m_nodeA.next!.prev = c.m_nodeA.prev 185 | } 186 | 187 | if c.m_nodeA === bodyA.m_contactList { 188 | bodyA.m_contactList = c.m_nodeA.next 189 | } 190 | 191 | // Remove from body 2 192 | if c.m_nodeB.prev != nil { 193 | c.m_nodeB.prev!.next = c.m_nodeB.next 194 | } 195 | 196 | if c.m_nodeB.next != nil { 197 | c.m_nodeB.next!.prev = c.m_nodeB.prev 198 | } 199 | 200 | if c.m_nodeB === bodyB.m_contactList { 201 | bodyB.m_contactList = c.m_nodeB.next 202 | } 203 | 204 | // Call the factory. 205 | b2Contact.destroy(c) 206 | m_contactCount -= 1 207 | } 208 | 209 | // This is the top level collision call for the time step. Here 210 | // all the narrow phase collision is processed for the world 211 | // contact list. 212 | func collide() { 213 | // Update awake contacts. 214 | var c = m_contactList 215 | while c != nil { 216 | let fixtureA = c!.fixtureA 217 | let fixtureB = c!.fixtureB 218 | let indexA = c!.childIndexA 219 | let indexB = c!.childIndexB 220 | let bodyA = fixtureA.body 221 | let bodyB = fixtureB.body 222 | 223 | // Is this contact flagged for filtering? 224 | if (c!.m_flags & b2Contact.Flags.filterFlag) != 0 { 225 | // Should these bodies collide? 226 | if bodyB.shouldCollide(bodyA) == false { 227 | let cNuke = c! 228 | c = cNuke.getNext() 229 | destroy(cNuke) 230 | continue 231 | } 232 | 233 | // Check user filtering. 234 | if m_contactFilter != nil && m_contactFilter!.shouldCollide(fixtureA, fixtureB) == false { 235 | let cNuke = c! 236 | c = cNuke.getNext() 237 | destroy(cNuke) 238 | continue 239 | } 240 | 241 | // Clear the filtering flag. 242 | c!.m_flags &= ~b2Contact.Flags.filterFlag 243 | } 244 | 245 | let activeA = bodyA.isAwake && bodyA.m_type != b2BodyType.staticBody 246 | let activeB = bodyB.isAwake && bodyB.m_type != b2BodyType.staticBody 247 | // At least one body must be awake and it must be dynamic or kinematic. 248 | if activeA == false && activeB == false { 249 | c = c!.getNext() 250 | continue 251 | } 252 | 253 | let proxyIdA = fixtureA.m_proxies[indexA].proxyId 254 | let proxyIdB = fixtureB.m_proxies[indexB].proxyId 255 | let overlap = m_broadPhase.testOverlap(proxyIdA: proxyIdA, proxyIdB: proxyIdB) 256 | 257 | // Here we destroy contacts that cease to overlap in the broad-phase. 258 | if overlap == false { 259 | let cNuke = c! 260 | c = cNuke.getNext() 261 | destroy(cNuke) 262 | continue 263 | } 264 | 265 | // The contact persists. 266 | c!.update(m_contactListener!) 267 | c = c!.getNext() 268 | } 269 | } 270 | 271 | var m_broadPhase = b2BroadPhase() 272 | open var broadPhase: b2BroadPhase { 273 | return m_broadPhase 274 | } 275 | var m_contactList: b2Contact? = nil // ** owner ** 276 | var m_contactCount: Int = 0 277 | var m_contactFilter: b2ContactFilter? = b2_defaultFilter 278 | var m_contactListener: b2ContactListener? = b2_defaultListener 279 | } 280 | 281 | 282 | -------------------------------------------------------------------------------- /Sources/Box2D/Dynamics/b2Island.swift: -------------------------------------------------------------------------------- 1 | /** 2 | Copyright (c) 2006-2014 Erin Catto http://www.box2d.org 3 | Copyright (c) 2015 - Yohei Yoshihara 4 | 5 | This software is provided 'as-is', without any express or implied 6 | warranty. In no event will the authors be held liable for any damages 7 | arising from the use of this software. 8 | 9 | Permission is granted to anyone to use this software for any purpose, 10 | including commercial applications, and to alter it and redistribute it 11 | freely, subject to the following restrictions: 12 | 13 | 1. The origin of this software must not be misrepresented; you must not 14 | claim that you wrote the original software. If you use this software 15 | in a product, an acknowledgment in the product documentation would be 16 | appreciated but is not required. 17 | 18 | 2. Altered source versions must be plainly marked as such, and must not be 19 | misrepresented as being the original software. 20 | 21 | 3. This notice may not be removed or altered from any source distribution. 22 | 23 | This version of box2d was developed by Yohei Yoshihara. It is based upon 24 | the original C++ code written by Erin Catto. 25 | */ 26 | 27 | import Foundation 28 | 29 | /// This is an internal class. 30 | open class b2Island { 31 | init(_ bodyCapacity: Int, _ contactCapacity: Int, _ jointCapacity: Int, _ listener: b2ContactListener?) { 32 | m_bodyCapacity = bodyCapacity 33 | m_contactCapacity = contactCapacity 34 | m_jointCapacity = jointCapacity 35 | 36 | m_listener = listener 37 | 38 | m_bodies = [b2Body]() 39 | m_bodies.reserveCapacity(bodyCapacity) 40 | m_contacts = [b2Contact]() 41 | m_contacts.reserveCapacity(contactCapacity) 42 | m_joints = [b2Joint]() 43 | m_joints.reserveCapacity(jointCapacity) 44 | 45 | m_velocities = b2Array() 46 | m_velocities.reserveCapacity(m_bodyCapacity) 47 | m_positions = b2Array() 48 | m_positions.reserveCapacity(m_bodyCapacity) 49 | } 50 | 51 | func reset(_ bodyCapacity: Int, _ contactCapacity: Int, _ jointCapacity: Int, _ listener: b2ContactListener?) { 52 | m_bodyCapacity = bodyCapacity 53 | m_contactCapacity = contactCapacity 54 | m_jointCapacity = jointCapacity 55 | 56 | m_listener = listener 57 | 58 | m_bodies.removeAll(keepingCapacity: true) 59 | m_bodies.reserveCapacity(bodyCapacity) 60 | m_contacts.removeAll(keepingCapacity: true) 61 | m_contacts.reserveCapacity(contactCapacity) 62 | m_joints.removeAll(keepingCapacity: true) 63 | m_joints.reserveCapacity(jointCapacity) 64 | 65 | m_velocities.removeAll(true) 66 | m_velocities.reserveCapacity(m_bodyCapacity) 67 | m_positions.removeAll(true) 68 | m_positions.reserveCapacity(m_bodyCapacity) 69 | } 70 | 71 | func clear() { 72 | m_bodies.removeAll(keepingCapacity: true) 73 | m_contacts.removeAll(keepingCapacity: true) 74 | m_joints.removeAll(keepingCapacity: true) 75 | } 76 | 77 | func solve(_ profile: inout b2Profile, _ step: b2TimeStep, _ gravity: b2Vec2, _ allowSleep: Bool) { 78 | let timer = b2Timer() 79 | 80 | let h = step.dt 81 | 82 | // Integrate velocities and apply damping. Initialize the body state. 83 | m_positions.removeAll(true) 84 | m_velocities.removeAll(true) 85 | for i in 0 ..< m_bodyCount { 86 | let b = m_bodies[i] 87 | 88 | let c = b.m_sweep.c 89 | let a = b.m_sweep.a 90 | var v = b.m_linearVelocity 91 | var w = b.m_angularVelocity 92 | 93 | // Store positions for continuous collision. 94 | b.m_sweep.c0 = b.m_sweep.c 95 | b.m_sweep.a0 = b.m_sweep.a 96 | 97 | if b.m_type == b2BodyType.dynamicBody { 98 | // Integrate velocities. 99 | v += h * (b.m_gravityScale * gravity + b.m_invMass * b.m_force) 100 | w += h * b.m_invI * b.m_torque 101 | 102 | // Apply damping. 103 | // ODE: dv/dt + c * v = 0 104 | // Solution: v(t) = v0 * exp(-c * t) 105 | // Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt) 106 | // v2 = exp(-c * dt) * v1 107 | // Pade approximation: 108 | // v2 = v1 * 1 / (1 + c * dt) 109 | v *= 1.0 / (1.0 + h * b.m_linearDamping) 110 | w *= 1.0 / (1.0 + h * b.m_angularDamping) 111 | } 112 | 113 | m_positions.append(b2Position(c, a)) 114 | m_velocities.append(b2Velocity(v, w)) 115 | } 116 | 117 | timer.reset() 118 | 119 | // Solver data 120 | var solverData = b2SolverData() 121 | solverData.step = step 122 | solverData.positions = m_positions 123 | solverData.velocities = m_velocities 124 | 125 | // Initialize velocity constraints. 126 | var contactSolverDef = b2ContactSolverDef() 127 | contactSolverDef.step = step 128 | contactSolverDef.contacts = m_contacts 129 | contactSolverDef.count = m_contactCount 130 | contactSolverDef.positions = m_positions 131 | contactSolverDef.velocities = m_velocities 132 | 133 | let contactSolver = b2ContactSolver(contactSolverDef) 134 | contactSolver.initializeVelocityConstraints() 135 | 136 | if step.warmStarting { 137 | contactSolver.warmStart() 138 | } 139 | 140 | for i in 0 ..< m_jointCount { 141 | m_joints[i].initVelocityConstraints(&solverData) 142 | } 143 | 144 | profile.solveInit = timer.milliseconds 145 | 146 | // Solve velocity constraints 147 | timer.reset() 148 | for _ in 0 ..< step.velocityIterations { 149 | for j in 0 ..< m_jointCount { 150 | m_joints[j].solveVelocityConstraints(&solverData) 151 | } 152 | 153 | contactSolver.solveVelocityConstraints() 154 | } 155 | 156 | // Store impulses for warm starting 157 | contactSolver.storeImpulses() 158 | profile.solveVelocity = timer.milliseconds 159 | 160 | // Integrate positions 161 | for i in 0 ..< m_bodyCount { 162 | var c = m_positions[i].c 163 | var a = m_positions[i].a 164 | var v = m_velocities[i].v 165 | var w = m_velocities[i].w 166 | 167 | // Check for large velocities 168 | let translation = h * v 169 | if b2Dot(translation, translation) > b2_maxTranslationSquared { 170 | let ratio = b2_maxTranslation / translation.length() 171 | v *= ratio 172 | } 173 | 174 | let rotation = h * w 175 | if rotation * rotation > b2_maxRotationSquared { 176 | let ratio = b2_maxRotation / abs(rotation) 177 | w *= ratio 178 | } 179 | 180 | // Integrate 181 | c += h * v 182 | a += h * w 183 | 184 | m_positions[i].c = c 185 | m_positions[i].a = a 186 | m_velocities[i].v = v 187 | m_velocities[i].w = w 188 | } 189 | 190 | // Solve position constraints 191 | timer.reset() 192 | var positionSolved = false 193 | for _ in 0 ..< step.positionIterations { 194 | let contactsOkay = contactSolver.solvePositionConstraints() 195 | 196 | var jointsOkay = true 197 | for i2 in 0 ..< m_jointCount { 198 | let jointOkay = m_joints[i2].solvePositionConstraints(&solverData) 199 | jointsOkay = jointsOkay && jointOkay 200 | } 201 | 202 | if contactsOkay && jointsOkay { 203 | // Exit early if the position errors are small. 204 | positionSolved = true 205 | break 206 | } 207 | } 208 | 209 | // Copy state buffers back to the bodies 210 | for i in 0 ..< m_bodyCount { 211 | let body = m_bodies[i] 212 | body.m_sweep.c = m_positions[i].c 213 | body.m_sweep.a = m_positions[i].a 214 | body.m_linearVelocity = m_velocities[i].v 215 | body.m_angularVelocity = m_velocities[i].w 216 | body.synchronizeTransform() 217 | } 218 | 219 | profile.solvePosition = timer.milliseconds 220 | 221 | report(contactSolver.m_velocityConstraints) 222 | 223 | if allowSleep { 224 | var minSleepTime = b2_maxFloat 225 | 226 | let linTolSqr = b2_linearSleepTolerance * b2_linearSleepTolerance 227 | let angTolSqr = b2_angularSleepTolerance * b2_angularSleepTolerance 228 | 229 | for i in 0 ..< m_bodyCount { 230 | let b = m_bodies[i] 231 | if b.type == b2BodyType.staticBody { 232 | continue 233 | } 234 | 235 | if (b.m_flags & b2Body.Flags.autoSleepFlag) == 0 || 236 | b.m_angularVelocity * b.m_angularVelocity > angTolSqr || 237 | b2Dot(b.m_linearVelocity, b.m_linearVelocity) > linTolSqr { 238 | b.m_sleepTime = 0.0 239 | minSleepTime = 0.0 240 | } 241 | else { 242 | b.m_sleepTime += h 243 | minSleepTime = min(minSleepTime, b.m_sleepTime) 244 | } 245 | } 246 | 247 | if minSleepTime >= b2_timeToSleep && positionSolved { 248 | for i in 0 ..< m_bodyCount { 249 | let b = m_bodies[i] 250 | b.setAwake(false) 251 | } 252 | } 253 | } 254 | } 255 | 256 | func solveTOI(_ subStep: b2TimeStep, _ toiIndexA: Int, _ toiIndexB: Int) { 257 | assert(toiIndexA < m_bodyCount) 258 | assert(toiIndexB < m_bodyCount) 259 | 260 | // Initialize the body state. 261 | m_positions.removeAll(true) 262 | m_velocities.removeAll(true) 263 | for i in 0 ..< m_bodyCount { 264 | let b = m_bodies[i] 265 | m_positions.append(b2Position(b.m_sweep.c, b.m_sweep.a)) 266 | m_velocities.append(b2Velocity(b.m_linearVelocity, b.m_angularVelocity)) 267 | } 268 | 269 | var contactSolverDef = b2ContactSolverDef() 270 | contactSolverDef.contacts = m_contacts 271 | contactSolverDef.count = m_contactCount 272 | contactSolverDef.step = subStep 273 | contactSolverDef.positions = m_positions 274 | contactSolverDef.velocities = m_velocities 275 | let contactSolver = b2ContactSolver(contactSolverDef) 276 | 277 | // Solve position constraints. 278 | for _ in 0 ..< subStep.positionIterations { 279 | let contactsOkay = contactSolver.solveTOIPositionConstraints(toiIndexA, toiIndexB) 280 | if contactsOkay { 281 | break 282 | } 283 | } 284 | 285 | #if false 286 | // Is the new position really safe? 287 | for i in 0 ..< m_contactCount { 288 | let c = m_contacts[i] 289 | let fA = c.fixtureA 290 | let fB = c.fixtureB 291 | 292 | let bA = fA.body 293 | let bB = fB.body 294 | 295 | let indexA = c.childIndexA 296 | let indexB = c.childIndexB 297 | 298 | var input = b2DistanceInput() 299 | input.proxyA.set(fA.shape, indexA) 300 | input.proxyB.set(fB.shape, indexB) 301 | input.transformA = bA.transform 302 | input.transformB = bB.transform 303 | input.useRadii = false 304 | 305 | var output = b2DistanceOutput() 306 | var cache = b2SimplexCache() 307 | cache.count = 0 308 | b2Distance(&output, &cache, input) 309 | 310 | if output.distance == 0 || cache.count == 3 { 311 | cache.count += 0 312 | } 313 | } 314 | #endif 315 | 316 | // Leap of faith to new safe state. 317 | m_bodies[toiIndexA].m_sweep.c0 = m_positions[toiIndexA].c 318 | m_bodies[toiIndexA].m_sweep.a0 = m_positions[toiIndexA].a 319 | m_bodies[toiIndexB].m_sweep.c0 = m_positions[toiIndexB].c 320 | m_bodies[toiIndexB].m_sweep.a0 = m_positions[toiIndexB].a 321 | 322 | // No warm starting is needed for TOI events because warm 323 | // starting impulses were applied in the discrete solver. 324 | contactSolver.initializeVelocityConstraints() 325 | 326 | // Solve velocity constraints. 327 | for _ in 0 ..< subStep.velocityIterations { 328 | contactSolver.solveVelocityConstraints() 329 | } 330 | 331 | // Don't store the TOI contact forces for warm starting 332 | // because they can be quite large. 333 | 334 | let h = subStep.dt 335 | 336 | // Integrate positions 337 | for i in 0 ..< m_bodyCount { 338 | var c = m_positions[i].c 339 | var a = m_positions[i].a 340 | var v = m_velocities[i].v 341 | var w = m_velocities[i].w 342 | 343 | // Check for large velocities 344 | let translation = h * v 345 | if b2Dot(translation, translation) > b2_maxTranslationSquared { 346 | let ratio = b2_maxTranslation / translation.length() 347 | v *= ratio 348 | } 349 | 350 | let rotation = h * w 351 | if rotation * rotation > b2_maxRotationSquared { 352 | let ratio = b2_maxRotation / abs(rotation) 353 | w *= ratio 354 | } 355 | 356 | // Integrate 357 | c += h * v 358 | a += h * w 359 | 360 | m_positions[i].c = c 361 | m_positions[i].a = a 362 | m_velocities[i].v = v 363 | m_velocities[i].w = w 364 | 365 | // Sync bodies 366 | let body = m_bodies[i] 367 | body.m_sweep.c = c 368 | body.m_sweep.a = a 369 | body.m_linearVelocity = v 370 | body.m_angularVelocity = w 371 | body.synchronizeTransform() 372 | } 373 | 374 | report(contactSolver.m_velocityConstraints) 375 | } 376 | 377 | func add(_ body: b2Body) { 378 | assert(m_bodyCount < m_bodyCapacity) 379 | body.m_islandIndex = m_bodyCount 380 | m_bodies.append(body) 381 | } 382 | 383 | func add(_ contact: b2Contact) { 384 | assert(m_contactCount < m_contactCapacity) 385 | m_contacts.append(contact) 386 | } 387 | 388 | func add(_ joint: b2Joint) { 389 | assert(m_jointCount < m_jointCapacity) 390 | m_joints.append(joint) 391 | } 392 | 393 | func report(_ constraints: [b2ContactVelocityConstraint]) { 394 | if m_listener == nil { 395 | return 396 | } 397 | 398 | for i in 0 ..< m_contactCount { 399 | let c = m_contacts[i] 400 | 401 | let vc = constraints[i] 402 | 403 | var impulse = b2ContactImpulse() 404 | impulse.count = vc.pointCount 405 | for j in 0 ..< vc.pointCount { 406 | impulse.normalImpulses[j] = vc.points[j].normalImpulse 407 | impulse.tangentImpulses[j] = vc.points[j].tangentImpulse 408 | } 409 | 410 | m_listener?.postSolve(c, impulse: impulse) 411 | } 412 | } 413 | 414 | var m_listener: b2ContactListener? 415 | 416 | var m_bodies: [b2Body] 417 | var m_contacts: [b2Contact] 418 | var m_joints: [b2Joint] 419 | 420 | var m_positions: b2Array 421 | var m_velocities: b2Array 422 | 423 | var m_bodyCount: Int { return m_bodies.count } 424 | var m_jointCount: Int { return m_joints.count } 425 | var m_contactCount: Int { return m_contacts.count } 426 | 427 | var m_bodyCapacity: Int 428 | var m_contactCapacity: Int 429 | var m_jointCapacity: Int 430 | } 431 | -------------------------------------------------------------------------------- /Sources/Box2D/Dynamics/b2TimeStep.swift: -------------------------------------------------------------------------------- 1 | /** 2 | Copyright (c) 2006-2014 Erin Catto http://www.box2d.org 3 | Copyright (c) 2015 - Yohei Yoshihara 4 | 5 | This software is provided 'as-is', without any express or implied 6 | warranty. In no event will the authors be held liable for any damages 7 | arising from the use of this software. 8 | 9 | Permission is granted to anyone to use this software for any purpose, 10 | including commercial applications, and to alter it and redistribute it 11 | freely, subject to the following restrictions: 12 | 13 | 1. The origin of this software must not be misrepresented; you must not 14 | claim that you wrote the original software. If you use this software 15 | in a product, an acknowledgment in the product documentation would be 16 | appreciated but is not required. 17 | 18 | 2. Altered source versions must be plainly marked as such, and must not be 19 | misrepresented as being the original software. 20 | 21 | 3. This notice may not be removed or altered from any source distribution. 22 | 23 | This version of box2d was developed by Yohei Yoshihara. It is based upon 24 | the original C++ code written by Erin Catto. 25 | */ 26 | 27 | import Foundation 28 | 29 | /// Profiling data. Times are in milliseconds. 30 | public struct b2Profile { 31 | public init() {} 32 | public var step: b2Float = 0.0 33 | public var collide: b2Float = 0.0 34 | public var solve: b2Float = 0.0 35 | public var solveInit: b2Float = 0.0 36 | public var solveVelocity: b2Float = 0.0 37 | public var solvePosition: b2Float = 0.0 38 | public var broadphase: b2Float = 0.0 39 | public var solveTOI: b2Float = 0.0 40 | } 41 | 42 | public struct b2TimeStep { 43 | var dt: b2Float = 0.0 // time step 44 | var inv_dt: b2Float = 0.0 // inverse time step (0 if dt == 0). 45 | var dtRatio: b2Float = 0.0 // dt * inv_dt0 46 | var velocityIterations: Int = 0 47 | var positionIterations: Int = 0 48 | var warmStarting: Bool = false 49 | } 50 | 51 | open class b2Array : CustomStringConvertible { 52 | var array = [T]() 53 | 54 | public init() { 55 | } 56 | 57 | public init(count: Int, repeatedValue: T) { 58 | array = [T](repeating: repeatedValue, count: count) 59 | } 60 | 61 | open func reserveCapacity(_ minimumCapacity: Int) { 62 | array.reserveCapacity(minimumCapacity) 63 | } 64 | 65 | open func append(_ newElement: T) { 66 | array.append(newElement) 67 | } 68 | 69 | open func insert(_ newElement: T, atIndex i: Int) { 70 | array.insert(newElement, at: i) 71 | } 72 | 73 | open func removeAtIndex(_ index: Int) -> T { 74 | return array.remove(at: index) 75 | } 76 | 77 | open func removeLast() { 78 | array.removeLast() 79 | } 80 | 81 | open func removeAll(_ keepCapacity: Bool = true) { 82 | array.removeAll(keepingCapacity: keepCapacity) 83 | } 84 | 85 | open subscript(index: Int) -> T { 86 | get { 87 | return array[index] 88 | } 89 | set { 90 | array[index] = newValue 91 | } 92 | } 93 | 94 | open func clone() -> b2Array { 95 | let clone = b2Array() 96 | clone.reserveCapacity(self.count) 97 | for e in self.array { 98 | clone.array.append(e) 99 | } 100 | return clone 101 | } 102 | 103 | open var count : Int { 104 | get { 105 | return array.count 106 | } 107 | } 108 | 109 | open var description: String { 110 | var s = "b2Array[" 111 | for i in 0 ..< array.count { 112 | s += "\(array[i])" 113 | if i != array.count - 1 { 114 | s += ", " 115 | } 116 | } 117 | s += "]" 118 | return s 119 | } 120 | } 121 | 122 | /// This is an internal structure. 123 | public struct b2Position { 124 | init(_ c: b2Vec2, _ a: b2Float) { 125 | self.c = c 126 | self.a = a 127 | } 128 | var c : b2Vec2 129 | var a : b2Float 130 | } 131 | 132 | /// This is an internal structure. 133 | public struct b2Velocity { 134 | init(_ v: b2Vec2, _ w: b2Float) { 135 | self.v = v 136 | self.w = w 137 | } 138 | var v : b2Vec2 139 | var w : b2Float 140 | } 141 | 142 | /// Solver Data 143 | public struct b2SolverData { 144 | var step = b2TimeStep() 145 | var positions = b2Array() 146 | var velocities = b2Array() 147 | } 148 | 149 | 150 | -------------------------------------------------------------------------------- /Sources/Box2D/Dynamics/b2WorldCallbacks.swift: -------------------------------------------------------------------------------- 1 | /** 2 | Copyright (c) 2006-2014 Erin Catto http://www.box2d.org 3 | Copyright (c) 2015 - Yohei Yoshihara 4 | 5 | This software is provided 'as-is', without any express or implied 6 | warranty. In no event will the authors be held liable for any damages 7 | arising from the use of this software. 8 | 9 | Permission is granted to anyone to use this software for any purpose, 10 | including commercial applications, and to alter it and redistribute it 11 | freely, subject to the following restrictions: 12 | 13 | 1. The origin of this software must not be misrepresented; you must not 14 | claim that you wrote the original software. If you use this software 15 | in a product, an acknowledgment in the product documentation would be 16 | appreciated but is not required. 17 | 18 | 2. Altered source versions must be plainly marked as such, and must not be 19 | misrepresented as being the original software. 20 | 21 | 3. This notice may not be removed or altered from any source distribution. 22 | 23 | This version of box2d was developed by Yohei Yoshihara. It is based upon 24 | the original C++ code written by Erin Catto. 25 | */ 26 | 27 | import Foundation 28 | 29 | /// Joints and fixtures are destroyed when their associated 30 | /// body is destroyed. Implement this listener so that you 31 | /// may nullify references to these joints and shapes. 32 | public protocol b2DestructionListener { 33 | /// Called when any joint is about to be destroyed due 34 | /// to the destruction of one of its attached bodies. 35 | func sayGoodbye(_ joint: b2Joint) 36 | 37 | /// Called when any fixture is about to be destroyed due 38 | /// to the destruction of its parent body. 39 | func sayGoodbye(_ fixture: b2Fixture) 40 | } 41 | 42 | /// Implement this class to provide collision filtering. In other words, you can implement 43 | /// this class if you want finer control over contact creation. 44 | open class b2ContactFilter { 45 | public init(){} 46 | /// Return true if contact calculations should be performed between these two shapes. 47 | /// @warning for performance reasons this is only called when the AABBs begin to overlap. 48 | open func shouldCollide(_ fixtureA: b2Fixture, _ fixtureB: b2Fixture) -> Bool { 49 | let filterA = fixtureA.filterData 50 | let filterB = fixtureB.filterData 51 | 52 | if filterA.groupIndex == filterB.groupIndex && filterA.groupIndex != 0 { 53 | return filterA.groupIndex > 0 54 | } 55 | 56 | let collide = (filterA.maskBits & filterB.categoryBits) != 0 && (filterA.categoryBits & filterB.maskBits) != 0 57 | return collide 58 | } 59 | } 60 | 61 | /// Contact impulses for reporting. Impulses are used instead of forces because 62 | /// sub-step forces may approach infinity for rigid body collisions. These 63 | /// match up one-to-one with the contact points in b2Manifold. 64 | public struct b2ContactImpulse { 65 | public var normalImpulses = [b2Float](repeating: 0, count: b2_maxManifoldPoints) 66 | public var tangentImpulses = [b2Float](repeating: 0, count: b2_maxManifoldPoints) 67 | public var count = 0 68 | } 69 | 70 | /// things like sounds and game logic. You can also get contact results by 71 | /// traversing the contact lists after the time step. However, you might miss 72 | /// some contacts because continuous physics leads to sub-stepping. 73 | /// Additionally you may receive multiple callbacks for the same contact in a 74 | /// single time step. 75 | /// You should strive to make your callbacks efficient because there may be 76 | /// many callbacks per time step. 77 | /// @warning You cannot create/destroy Box2D entities inside these callbacks. 78 | public protocol b2ContactListener { 79 | /// Called when two fixtures begin to touch. 80 | func beginContact(_ contact: b2Contact) 81 | 82 | /// Called when two fixtures cease to touch. 83 | func endContact(_ contact: b2Contact) 84 | 85 | /// This is called after a contact is updated. This allows you to inspect a 86 | /// contact before it goes to the solver. If you are careful, you can modify the 87 | /// contact manifold (e.g. disable contact). 88 | /// A copy of the old manifold is provided so that you can detect changes. 89 | /// Note: this is called only for awake bodies. 90 | /// Note: this is called even when the number of contact points is zero. 91 | /// Note: this is not called for sensors. 92 | /// Note: if you set the number of contact points to zero, you will not 93 | /// get an EndContact callback. However, you may get a BeginContact callback 94 | /// the next step. 95 | func preSolve(_ contact: b2Contact, oldManifold: b2Manifold) 96 | 97 | /// This lets you inspect a contact after the solver is finished. This is useful 98 | /// for inspecting impulses. 99 | /// Note: the contact manifold does not include time of impact impulses, which can be 100 | /// arbitrarily large if the sub-step is small. Hence the impulse is provided explicitly 101 | /// in a separate data structure. 102 | /// Note: this is only called for contacts that are touching, solid, and awake. 103 | func postSolve(_ contact: b2Contact, impulse: b2ContactImpulse) 104 | } 105 | 106 | open class b2DefaultContactListener : b2ContactListener { 107 | open func beginContact(_ contact : b2Contact) {} 108 | open func endContact(_ contact: b2Contact) {} 109 | open func preSolve(_ contact: b2Contact, oldManifold: b2Manifold) {} 110 | open func postSolve(_ contact: b2Contact, impulse: b2ContactImpulse) {} 111 | } 112 | 113 | /// Callback class for AABB queries. 114 | /// See b2World::Query 115 | public protocol b2QueryCallback { 116 | /** 117 | Called for each fixture found in the query AABB. 118 | 119 | - returns: false to terminate the query. 120 | */ 121 | func reportFixture(_ fixture: b2Fixture) -> Bool 122 | } 123 | 124 | public typealias b2QueryCallbackFunction = (_ fixture: b2Fixture) -> Bool 125 | 126 | class b2QueryCallbackProxy: b2QueryCallback { 127 | var callback: b2QueryCallbackFunction 128 | init(callback: @escaping b2QueryCallbackFunction) { 129 | self.callback = callback 130 | } 131 | func reportFixture(_ fixture: b2Fixture) -> Bool { 132 | return self.callback(fixture) 133 | } 134 | } 135 | 136 | /// Callback class for ray casts. 137 | /// See b2World::RayCast 138 | public protocol b2RayCastCallback { 139 | /** 140 | Called for each fixture found in the query. You control how the ray cast 141 | proceeds by returning a float: 142 | return -1: ignore this fixture and continue 143 | return 0: terminate the ray cast 144 | return fraction: clip the ray to this point 145 | return 1: don't clip the ray and continue 146 | 147 | - parameter fixture: the fixture hit by the ray 148 | - parameter point: the point of initial intersection 149 | - parameter normal: the normal vector at the point of intersection 150 | 151 | - returns: -1 to filter, 0 to terminate, fraction to clip the ray for closest hit, 1 to continue 152 | */ 153 | func reportFixture(_ fixture: b2Fixture, point: b2Vec2, normal: b2Vec2, fraction: b2Float) -> b2Float 154 | } 155 | 156 | public typealias b2RayCastCallbackFunction = (_ fixture: b2Fixture, _ point: b2Vec2, _ normal: b2Vec2, _ fraction: b2Float) -> b2Float 157 | 158 | class b2RayCastCallbackProxy: b2RayCastCallback { 159 | var callback: b2RayCastCallbackFunction 160 | init(callback: @escaping b2RayCastCallbackFunction) { 161 | self.callback = callback 162 | } 163 | func reportFixture(_ fixture: b2Fixture, point: b2Vec2, normal: b2Vec2, fraction: b2Float) -> b2Float { 164 | return self.callback(fixture, point, normal, fraction) 165 | } 166 | } 167 | -------------------------------------------------------------------------------- /Sources/Box2D/Info.plist: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | CFBundleDevelopmentRegion 6 | en 7 | CFBundleExecutable 8 | $(EXECUTABLE_NAME) 9 | CFBundleIdentifier 10 | $(PRODUCT_BUNDLE_IDENTIFIER) 11 | CFBundleInfoDictionaryVersion 12 | 6.0 13 | CFBundleName 14 | $(PRODUCT_NAME) 15 | CFBundlePackageType 16 | FMWK 17 | CFBundleShortVersionString 18 | 1.0 19 | CFBundleSignature 20 | ???? 21 | CFBundleVersion 22 | $(CURRENT_PROJECT_VERSION) 23 | NSPrincipalClass 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /Sources/Box2D/Rope/b2Rope.swift: -------------------------------------------------------------------------------- 1 | /** 2 | Copyright (c) 2006-2014 Erin Catto http://www.box2d.org 3 | Copyright (c) 2015 - Yohei Yoshihara 4 | 5 | This software is provided 'as-is', without any express or implied 6 | warranty. In no event will the authors be held liable for any damages 7 | arising from the use of this software. 8 | 9 | Permission is granted to anyone to use this software for any purpose, 10 | including commercial applications, and to alter it and redistribute it 11 | freely, subject to the following restrictions: 12 | 13 | 1. The origin of this software must not be misrepresented; you must not 14 | claim that you wrote the original software. If you use this software 15 | in a product, an acknowledgment in the product documentation would be 16 | appreciated but is not required. 17 | 18 | 2. Altered source versions must be plainly marked as such, and must not be 19 | misrepresented as being the original software. 20 | 21 | 3. This notice may not be removed or altered from any source distribution. 22 | 23 | This version of box2d was developed by Yohei Yoshihara. It is based upon 24 | the original C++ code written by Erin Catto. 25 | */ 26 | 27 | import Foundation 28 | 29 | /// 30 | public struct b2RopeDef { 31 | init() { 32 | vertices = nil 33 | count = 0 34 | masses = nil 35 | gravity = b2Vec2() 36 | damping = 0.1 37 | k2 = 0.9 38 | k3 = 0.1 39 | } 40 | 41 | /// 42 | var vertices: [b2Vec2]! 43 | 44 | /// 45 | var count: Int 46 | 47 | /// 48 | var masses: [b2Float]! 49 | 50 | /// 51 | var gravity: b2Vec2 52 | 53 | /// 54 | var damping: b2Float 55 | 56 | /// Stretching stiffness 57 | var k2: b2Float 58 | 59 | /// Bending stiffness. Values above 0.5 can make the simulation blow up. 60 | var k3: b2Float 61 | } 62 | 63 | /// 64 | open class b2Rope { 65 | init() { 66 | m_count = 0 67 | m_ps = nil 68 | m_p0s = nil 69 | m_vs = nil 70 | m_ims = nil 71 | m_Ls = nil 72 | m_as = nil 73 | m_gravity = b2Vec2(0.0, 0.0) 74 | m_damping = 0.1 75 | m_k2 = 1.0 76 | m_k3 = 0.1 77 | } 78 | deinit { 79 | } 80 | 81 | /// 82 | func initialize(_ def: b2RopeDef) { 83 | assert(def.count >= 3) 84 | m_count = def.count 85 | m_ps = [b2Vec2](repeating: b2Vec2(), count: m_count) 86 | m_p0s = [b2Vec2](repeating: b2Vec2(), count: m_count) 87 | m_vs = [b2Vec2](repeating: b2Vec2(), count: m_count) 88 | m_ims = [b2Float](repeating: b2Float(0.0), count: m_count) 89 | 90 | for i in 0 ..< m_count { 91 | m_ps[i] = def.vertices[i] 92 | m_p0s[i] = def.vertices[i] 93 | m_vs[i].setZero() 94 | 95 | let m = def.masses[i] 96 | if m > 0.0 { 97 | m_ims[i] = 1.0 / m 98 | } 99 | else { 100 | m_ims[i] = 0.0 101 | } 102 | } 103 | 104 | let count2 = m_count - 1 105 | let count3 = m_count - 2 106 | m_Ls = [b2Float](repeating: b2Float(0.0), count: count2) 107 | m_as = [b2Float](repeating: b2Float(0.0), count: count3) 108 | 109 | for i in 0 ..< count2 { 110 | let p1 = m_ps[i] 111 | let p2 = m_ps[i+1] 112 | m_Ls[i] = b2Distance(p1, p2) 113 | } 114 | 115 | for i in 0 ..< count3 { 116 | let p1 = m_ps[i] 117 | let p2 = m_ps[i + 1] 118 | let p3 = m_ps[i + 2] 119 | 120 | let d1 = p2 - p1 121 | let d2 = p3 - p2 122 | 123 | let a = b2Cross(d1, d2) 124 | let b = b2Dot(d1, d2) 125 | 126 | m_as[i] = b2Atan2(a, b) 127 | } 128 | 129 | m_gravity = def.gravity 130 | m_damping = def.damping 131 | m_k2 = def.k2 132 | m_k3 = def.k3 133 | } 134 | 135 | /// 136 | func step(timeStep h: b2Float, iterations: Int) { 137 | if h == 0.0 { 138 | return 139 | } 140 | 141 | let d = exp(-h * m_damping) 142 | 143 | for i in 0 ..< m_count { 144 | m_p0s[i] = m_ps[i] 145 | if m_ims[i] > 0.0 { 146 | m_vs[i] += h * m_gravity 147 | } 148 | m_vs[i] *= d 149 | m_ps[i] += h * m_vs[i] 150 | } 151 | 152 | for _ in 0 ..< iterations { 153 | solveC2() 154 | solveC3() 155 | solveC2() 156 | } 157 | 158 | let inv_h = 1.0 / h 159 | for i in 0 ..< m_count { 160 | m_vs[i] = inv_h * (m_ps[i] - m_p0s[i]) 161 | } 162 | } 163 | 164 | /// 165 | var vertexCount: Int { 166 | return m_count 167 | } 168 | 169 | /// 170 | var vertices: [b2Vec2] { 171 | return m_ps 172 | } 173 | 174 | /// 175 | func draw(_ draw: b2Draw) { 176 | let c = b2Color(0.4, 0.5, 0.7) 177 | 178 | for i in 0 ..< m_count - 1 { 179 | draw.drawSegment(m_ps[i], m_ps[i+1], c) 180 | } 181 | } 182 | 183 | /// 184 | func setAngle(_ angle: b2Float) { 185 | let count3 = m_count - 2 186 | for i in 0 ..< count3 { 187 | m_as[i] = angle 188 | } 189 | } 190 | 191 | func solveC2() { 192 | let count2 = m_count - 1 193 | 194 | for i in 0 ..< count2 { 195 | var p1 = m_ps[i] 196 | var p2 = m_ps[i + 1] 197 | 198 | var d = p2 - p1 199 | let L = d.normalize() 200 | 201 | let im1 = m_ims[i] 202 | let im2 = m_ims[i + 1] 203 | 204 | if im1 + im2 == 0.0 { 205 | continue 206 | } 207 | 208 | let s1 = im1 / (im1 + im2) 209 | let s2 = im2 / (im1 + im2) 210 | 211 | p1 -= m_k2 * s1 * (m_Ls[i] - L) * d 212 | p2 += m_k2 * s2 * (m_Ls[i] - L) * d 213 | 214 | m_ps[i] = p1 215 | m_ps[i + 1] = p2 216 | } 217 | } 218 | func solveC3() { 219 | let count3 = m_count - 2 220 | 221 | for i in 0 ..< count3 { 222 | var p1 = m_ps[i] 223 | var p2 = m_ps[i + 1] 224 | var p3 = m_ps[i + 2] 225 | 226 | let m1 = m_ims[i] 227 | let m2 = m_ims[i + 1] 228 | let m3 = m_ims[i + 2] 229 | 230 | let d1 = p2 - p1 231 | let d2 = p3 - p2 232 | 233 | let L1sqr = d1.lengthSquared() 234 | let L2sqr = d2.lengthSquared() 235 | 236 | if L1sqr * L2sqr == 0.0 { 237 | continue 238 | } 239 | 240 | let a = b2Cross(d1, d2) 241 | let b = b2Dot(d1, d2) 242 | 243 | var angle = b2Atan2(a, b) 244 | 245 | let Jd1 = (-1.0 / L1sqr) * d1.skew() 246 | let Jd2 = (1.0 / L2sqr) * d2.skew() 247 | 248 | let J1 = -Jd1 249 | let J2 = Jd1 - Jd2 250 | let J3 = Jd2 251 | 252 | var mass = m1 * b2Dot(J1, J1) + m2 * b2Dot(J2, J2) + m3 * b2Dot(J3, J3) 253 | if mass == 0.0 { 254 | continue 255 | } 256 | 257 | mass = 1.0 / mass 258 | 259 | var C = angle - m_as[i] 260 | 261 | while C > b2_pi { 262 | angle -= 2 * b2_pi 263 | C = angle - m_as[i] 264 | } 265 | 266 | while C < -b2_pi { 267 | angle += 2.0 * b2_pi 268 | C = angle - m_as[i] 269 | } 270 | 271 | let impulse = -m_k3 * mass * C 272 | 273 | p1 += (m1 * impulse) * J1 274 | p2 += (m2 * impulse) * J2 275 | p3 += (m3 * impulse) * J3 276 | 277 | m_ps[i] = p1 278 | m_ps[i + 1] = p2 279 | m_ps[i + 2] = p3 280 | } 281 | } 282 | 283 | var m_count: Int 284 | var m_ps: [b2Vec2]! = nil 285 | var m_p0s: [b2Vec2]! = nil 286 | var m_vs: [b2Vec2]! = nil 287 | 288 | var m_ims: [b2Float]! = nil 289 | 290 | var m_Ls: [b2Float]! = nil 291 | var m_as: [b2Float]! = nil 292 | 293 | var m_gravity: b2Vec2 294 | var m_damping: b2Float 295 | 296 | var m_k2: b2Float 297 | var m_k3: b2Float 298 | } 299 | -------------------------------------------------------------------------------- /Tests/Box2DTests/Box2DTests.swift: -------------------------------------------------------------------------------- 1 | /** 2 | Copyright (c) 2006-2014 Erin Catto http://www.box2d.org 3 | Copyright (c) 2015 - Yohei Yoshihara 4 | 5 | This software is provided 'as-is', without any express or implied 6 | warranty. In no event will the authors be held liable for any damages 7 | arising from the use of this software. 8 | 9 | Permission is granted to anyone to use this software for any purpose, 10 | including commercial applications, and to alter it and redistribute it 11 | freely, subject to the following restrictions: 12 | 13 | 1. The origin of this software must not be misrepresented; you must not 14 | claim that you wrote the original software. If you use this software 15 | in a product, an acknowledgment in the product documentation would be 16 | appreciated but is not required. 17 | 18 | 2. Altered source versions must be plainly marked as such, and must not be 19 | misrepresented as being the original software. 20 | 21 | 3. This notice may not be removed or altered from any source distribution. 22 | 23 | This version of box2d was developed by Yohei Yoshihara. It is based upon 24 | the original C++ code written by Erin Catto. 25 | */ 26 | 27 | import XCTest 28 | import Box2D 29 | 30 | class A { 31 | var x = 0 32 | var y = 0 33 | } 34 | 35 | let expected: [b2Float] = [ 36 | 0.000000, 3.997222, 0.000000, 37 | 0.000000, 3.991667, 0.000000, 38 | 0.000000, 3.983333, 0.000000, 39 | 0.000000, 3.972222, 0.000000, 40 | 0.000000, 3.958333, 0.000000, 41 | 0.000000, 3.941667, 0.000000, 42 | 0.000000, 3.922222, 0.000000, 43 | 0.000000, 3.900000, 0.000000, 44 | 0.000000, 3.875000, 0.000000, 45 | 0.000000, 3.847222, 0.000000, 46 | 0.000000, 3.816667, 0.000000, 47 | 0.000000, 3.783333, 0.000000, 48 | 0.000000, 3.747222, 0.000000, 49 | 0.000000, 3.708333, 0.000000, 50 | 0.000000, 3.666667, 0.000000, 51 | 0.000000, 3.622222, 0.000000, 52 | 0.000000, 3.575000, 0.000000, 53 | 0.000000, 3.525000, 0.000000, 54 | 0.000000, 3.472222, 0.000000, 55 | 0.000000, 3.416667, 0.000000, 56 | 0.000000, 3.358333, 0.000000, 57 | 0.000000, 3.297222, 0.000000, 58 | 0.000000, 3.233333, 0.000000, 59 | 0.000000, 3.166667, 0.000000, 60 | 0.000000, 3.097222, 0.000000, 61 | 0.000000, 3.025000, 0.000000, 62 | 0.000000, 2.950000, 0.000000, 63 | 0.000000, 2.872222, 0.000000, 64 | 0.000000, 2.791667, 0.000000, 65 | 0.000000, 2.708333, 0.000000, 66 | 0.000000, 2.622222, 0.000000, 67 | 0.000000, 2.533333, 0.000000, 68 | 0.000000, 2.441667, 0.000000, 69 | 0.000000, 2.347222, 0.000000, 70 | 0.000000, 2.250000, 0.000000, 71 | 0.000000, 2.150000, 0.000000, 72 | 0.000000, 2.047222, 0.000000, 73 | 0.000000, 1.941667, 0.000000, 74 | 0.000000, 1.833333, 0.000000, 75 | 0.000000, 1.722222, 0.000000, 76 | 0.000000, 1.608334, 0.000000, 77 | 0.000000, 1.491667, 0.000000, 78 | 0.000000, 1.372223, 0.000000, 79 | 0.000000, 1.250000, 0.000000, 80 | 0.000000, 1.125000, 0.000000, 81 | 0.000000, 1.014582, 0.000141, 82 | 0.000000, 1.014651, 0.000110, 83 | 0.000000, 1.014708, 0.000086, 84 | 0.000000, 1.014756, 0.000067, 85 | 0.000000, 1.014796, 0.000052, 86 | 0.000000, 1.014830, 0.000041, 87 | 0.000000, 1.014858, 0.000032, 88 | 0.000000, 1.014881, 0.000025, 89 | 0.000000, 1.014900, 0.000020, 90 | 0.000000, 1.014917, 0.000016, 91 | 0.000000, 1.014930, 0.000012, 92 | 0.000000, 1.014942, 0.000010, 93 | 0.000000, 1.014951, 0.000008, 94 | 0.000000, 1.014959, 0.000006, 95 | 0.000000, 1.014966, 0.000005, 96 | ] 97 | 98 | class Box2DTests: XCTestCase { 99 | 100 | override func setUp() { 101 | super.setUp() 102 | // Put setup code here. This method is called before the invocation of each test method in the class. 103 | } 104 | 105 | override func tearDown() { 106 | // Put teardown code here. This method is called after the invocation of each test method in the class. 107 | super.tearDown() 108 | } 109 | 110 | func testExample() { 111 | // Define the gravity vector. 112 | let gravity = b2Vec2(0.0, -10.0) 113 | 114 | // Construct a world object, which will hold and simulate the rigid bodies. 115 | let world = b2World(gravity: gravity) 116 | 117 | // Define the ground body. 118 | let groundBodyDef = b2BodyDef() 119 | groundBodyDef.position.set(0.0, -10.0) 120 | 121 | // Call the body factory which allocates memory for the ground body 122 | // from a pool and creates the ground box shape (also from a pool). 123 | // The body is also added to the world. 124 | let groundBody = world.createBody(groundBodyDef) 125 | 126 | // Define the ground box shape. 127 | let groundBox = b2PolygonShape() 128 | 129 | // The extents are the half-widths of the box. 130 | groundBox.setAsBox(halfWidth: 50.0, halfHeight: 10.0) 131 | 132 | // Add the ground fixture to the ground body. 133 | groundBody.createFixture(shape: groundBox, density: 0.0) 134 | 135 | // Define the dynamic body. We set its position and call the body factory. 136 | let bodyDef = b2BodyDef() 137 | bodyDef.type = b2BodyType.dynamicBody 138 | bodyDef.position.set(0.0, 4.0) 139 | let body = world.createBody(bodyDef) 140 | 141 | // Define another box shape for our dynamic body. 142 | let dynamicBox = b2PolygonShape() 143 | dynamicBox.setAsBox(halfWidth: 1.0, halfHeight: 1.0) 144 | 145 | // Define the dynamic body fixture. 146 | let fixtureDef = b2FixtureDef() 147 | fixtureDef.shape = dynamicBox 148 | 149 | // Set the box density to be non-zero, so it will be dynamic. 150 | fixtureDef.density = 1.0 151 | 152 | // Override the default friction. 153 | fixtureDef.friction = 0.3 154 | 155 | // Add the shape to the body. 156 | body.createFixture(fixtureDef) 157 | 158 | // Prepare for simulation. Typically we use a time step of 1/60 of a 159 | // second (60Hz) and 10 iterations. This provides a high quality simulation 160 | // in most game scenarios. 161 | let timeStep: b2Float = 1.0 / 60.0 162 | let velocityIterations = 6 163 | let positionIterations = 2 164 | 165 | // This is our little game loop. 166 | for i in 0 ..< 60 { 167 | if i == 45 { 168 | print("stop") 169 | } 170 | // Instruct the world to perform a single step of simulation. 171 | // It is generally best to keep the time step and iterations fixed. 172 | world.step(timeStep: timeStep, velocityIterations: velocityIterations, positionIterations: positionIterations) 173 | //world.dump() 174 | 175 | // Now print the position and angle of the body. 176 | //body.dump() 177 | let position = body.position 178 | let angle = body.angle 179 | 180 | //world.dump() 181 | print("\(i): \(position.x) \(position.y) \(angle)") 182 | 183 | let expectedX: b2Float = expected[i * 3 + 0] 184 | let expectedY: b2Float = expected[i * 3 + 1] 185 | let expectedAngle: b2Float = expected[i * 3 + 2] 186 | XCTAssertEqual(body.position.x, expectedX, accuracy: 1e-5); 187 | XCTAssertEqual(body.position.y, expectedY, accuracy: 1e-5); 188 | XCTAssertEqual(body.angle, expectedAngle, accuracy: 1e-5); 189 | } 190 | XCTAssertEqual(body.position.x, b2Float(0.0), accuracy: 1e-4); 191 | XCTAssertEqual(body.position.y, b2Float(1.014966), accuracy: 1e-4); 192 | XCTAssertEqual(body.angle, b2Float(0.0), accuracy: 1e-4); 193 | // When the world destructor is called, all bodies and joints are freed. This can 194 | // When the world destructor is called, all bodies and joints are freed. This can 195 | // create orphaned pointers, so be careful about your world management. 196 | } 197 | } 198 | --------------------------------------------------------------------------------