github.com/ByteArena/box2d@v1.0.2/DynamicsB2JointRope.go (about) 1 package box2d 2 3 import ( 4 "fmt" 5 "math" 6 ) 7 8 /// Rope joint definition. This requires two body anchor points and 9 /// a maximum lengths. 10 /// Note: by default the connected objects will not collide. 11 /// see collideConnected in b2JointDef. 12 type B2RopeJointDef struct { 13 B2JointDef 14 15 /// The local anchor point relative to bodyA's origin. 16 LocalAnchorA B2Vec2 17 18 /// The local anchor point relative to bodyB's origin. 19 LocalAnchorB B2Vec2 20 21 /// The maximum length of the rope. 22 /// Warning: this must be larger than b2_linearSlop or 23 /// the joint will have no effect. 24 MaxLength float64 25 } 26 27 func MakeB2RopeJointDef() B2RopeJointDef { 28 res := B2RopeJointDef{ 29 B2JointDef: MakeB2JointDef(), 30 } 31 res.Type = B2JointType.E_ropeJoint 32 res.LocalAnchorA.Set(-1.0, 0.0) 33 res.LocalAnchorB.Set(1.0, 0.0) 34 res.MaxLength = 0.0 35 return res 36 } 37 38 /// A rope joint enforces a maximum distance between two points 39 /// on two bodies. It has no other effect. 40 /// Warning: if you attempt to change the maximum length during 41 /// the simulation you will get some non-physical behavior. 42 /// A model that would allow you to dynamically modify the length 43 /// would have some sponginess, so I chose not to implement it 44 /// that way. See b2DistanceJoint if you want to dynamically 45 /// control length. 46 type B2RopeJoint struct { 47 *B2Joint 48 49 // Solver shared 50 M_localAnchorA B2Vec2 51 M_localAnchorB B2Vec2 52 M_maxLength float64 53 M_length float64 54 M_impulse float64 55 56 // Solver temp 57 M_indexA int 58 M_indexB int 59 M_u B2Vec2 60 M_rA B2Vec2 61 M_rB B2Vec2 62 M_localCenterA B2Vec2 63 M_localCenterB B2Vec2 64 M_invMassA float64 65 M_invMassB float64 66 M_invIA float64 67 M_invIB float64 68 M_mass float64 69 M_state uint8 70 } 71 72 /// The local anchor point relative to bodyA's origin. 73 func (joint B2RopeJoint) GetLocalAnchorA() B2Vec2 { 74 return joint.M_localAnchorA 75 } 76 77 /// The local anchor point relative to bodyB's origin. 78 func (joint B2RopeJoint) GetLocalAnchorB() B2Vec2 { 79 return joint.M_localAnchorB 80 } 81 82 /// Set/Get the maximum length of the rope. 83 func (joint *B2RopeJoint) SetMaxLength(length float64) { 84 joint.M_maxLength = length 85 } 86 87 // // Limit: 88 // // C = norm(pB - pA) - L 89 // // u = (pB - pA) / norm(pB - pA) 90 // // Cdot = dot(u, vB + cross(wB, rB) - vA - cross(wA, rA)) 91 // // J = [-u -cross(rA, u) u cross(rB, u)] 92 // // K = J * invM * JT 93 // // = invMassA + invIA * cross(rA, u)^2 + invMassB + invIB * cross(rB, u)^2 94 95 func MakeB2RopeJoint(def *B2RopeJointDef) *B2RopeJoint { 96 res := B2RopeJoint{ 97 B2Joint: MakeB2Joint(def), 98 } 99 100 res.M_localAnchorA = def.LocalAnchorA 101 res.M_localAnchorB = def.LocalAnchorB 102 103 res.M_maxLength = def.MaxLength 104 105 res.M_mass = 0.0 106 res.M_impulse = 0.0 107 res.M_state = B2LimitState.E_inactiveLimit 108 res.M_length = 0.0 109 110 return &res 111 } 112 113 func (joint *B2RopeJoint) InitVelocityConstraints(data B2SolverData) { 114 joint.M_indexA = joint.M_bodyA.M_islandIndex 115 joint.M_indexB = joint.M_bodyB.M_islandIndex 116 joint.M_localCenterA = joint.M_bodyA.M_sweep.LocalCenter 117 joint.M_localCenterB = joint.M_bodyB.M_sweep.LocalCenter 118 joint.M_invMassA = joint.M_bodyA.M_invMass 119 joint.M_invMassB = joint.M_bodyB.M_invMass 120 joint.M_invIA = joint.M_bodyA.M_invI 121 joint.M_invIB = joint.M_bodyB.M_invI 122 123 cA := data.Positions[joint.M_indexA].C 124 aA := data.Positions[joint.M_indexA].A 125 vA := data.Velocities[joint.M_indexA].V 126 wA := data.Velocities[joint.M_indexA].W 127 128 cB := data.Positions[joint.M_indexB].C 129 aB := data.Positions[joint.M_indexB].A 130 vB := data.Velocities[joint.M_indexB].V 131 wB := data.Velocities[joint.M_indexB].W 132 133 qA := MakeB2RotFromAngle(aA) 134 qB := MakeB2RotFromAngle(aB) 135 136 joint.M_rA = B2RotVec2Mul(qA, B2Vec2Sub(joint.M_localAnchorA, joint.M_localCenterA)) 137 joint.M_rB = B2RotVec2Mul(qB, B2Vec2Sub(joint.M_localAnchorB, joint.M_localCenterB)) 138 joint.M_u = B2Vec2Sub(B2Vec2Sub(B2Vec2Add(cB, joint.M_rB), cA), joint.M_rA) 139 140 joint.M_length = joint.M_u.Length() 141 142 C := joint.M_length - joint.M_maxLength 143 if C > 0.0 { 144 joint.M_state = B2LimitState.E_atUpperLimit 145 } else { 146 joint.M_state = B2LimitState.E_inactiveLimit 147 } 148 149 if joint.M_length > B2_linearSlop { 150 joint.M_u.OperatorScalarMulInplace(1.0 / joint.M_length) 151 } else { 152 joint.M_u.SetZero() 153 joint.M_mass = 0.0 154 joint.M_impulse = 0.0 155 return 156 } 157 158 // Compute effective mass. 159 crA := B2Vec2Cross(joint.M_rA, joint.M_u) 160 crB := B2Vec2Cross(joint.M_rB, joint.M_u) 161 invMass := joint.M_invMassA + joint.M_invIA*crA*crA + joint.M_invMassB + joint.M_invIB*crB*crB 162 163 if invMass != 0.0 { 164 joint.M_mass = 1.0 / invMass 165 } else { 166 joint.M_mass = 0.0 167 } 168 169 if data.Step.WarmStarting { 170 // Scale the impulse to support a variable time step. 171 joint.M_impulse *= data.Step.DtRatio 172 173 P := B2Vec2MulScalar(joint.M_impulse, joint.M_u) 174 vA.OperatorMinusInplace(B2Vec2MulScalar(joint.M_invMassA, P)) 175 wA -= joint.M_invIA * B2Vec2Cross(joint.M_rA, P) 176 vB.OperatorPlusInplace(B2Vec2MulScalar(joint.M_invMassB, P)) 177 wB += joint.M_invIB * B2Vec2Cross(joint.M_rB, P) 178 } else { 179 joint.M_impulse = 0.0 180 } 181 182 data.Velocities[joint.M_indexA].V = vA 183 data.Velocities[joint.M_indexA].W = wA 184 data.Velocities[joint.M_indexB].V = vB 185 data.Velocities[joint.M_indexB].W = wB 186 } 187 188 func (joint *B2RopeJoint) SolveVelocityConstraints(data B2SolverData) { 189 vA := data.Velocities[joint.M_indexA].V 190 wA := data.Velocities[joint.M_indexA].W 191 vB := data.Velocities[joint.M_indexB].V 192 wB := data.Velocities[joint.M_indexB].W 193 194 // Cdot = dot(u, v + cross(w, r)) 195 vpA := B2Vec2Add(vA, B2Vec2CrossScalarVector(wA, joint.M_rA)) 196 vpB := B2Vec2Add(vB, B2Vec2CrossScalarVector(wB, joint.M_rB)) 197 C := joint.M_length - joint.M_maxLength 198 Cdot := B2Vec2Dot(joint.M_u, B2Vec2Sub(vpB, vpA)) 199 200 // Predictive constraint. 201 if C < 0.0 { 202 Cdot += data.Step.Inv_dt * C 203 } 204 205 impulse := -joint.M_mass * Cdot 206 oldImpulse := joint.M_impulse 207 joint.M_impulse = math.Min(0.0, joint.M_impulse+impulse) 208 impulse = joint.M_impulse - oldImpulse 209 210 P := B2Vec2MulScalar(impulse, joint.M_u) 211 vA.OperatorMinusInplace(B2Vec2MulScalar(joint.M_invMassA, P)) 212 wA -= joint.M_invIA * B2Vec2Cross(joint.M_rA, P) 213 vB.OperatorPlusInplace(B2Vec2MulScalar(joint.M_invMassB, P)) 214 wB += joint.M_invIB * B2Vec2Cross(joint.M_rB, P) 215 216 data.Velocities[joint.M_indexA].V = vA 217 data.Velocities[joint.M_indexA].W = wA 218 data.Velocities[joint.M_indexB].V = vB 219 data.Velocities[joint.M_indexB].W = wB 220 } 221 222 func (joint *B2RopeJoint) SolvePositionConstraints(data B2SolverData) bool { 223 224 cA := data.Positions[joint.M_indexA].C 225 aA := data.Positions[joint.M_indexA].A 226 cB := data.Positions[joint.M_indexB].C 227 aB := data.Positions[joint.M_indexB].A 228 229 qA := MakeB2RotFromAngle(aA) 230 qB := MakeB2RotFromAngle(aB) 231 232 rA := B2RotVec2Mul(qA, B2Vec2Sub(joint.M_localAnchorA, joint.M_localCenterA)) 233 rB := B2RotVec2Mul(qB, B2Vec2Sub(joint.M_localAnchorB, joint.M_localCenterB)) 234 u := B2Vec2Sub(B2Vec2Sub(B2Vec2Add(cB, rB), cA), rA) 235 236 length := u.Normalize() 237 C := length - joint.M_maxLength 238 239 C = B2FloatClamp(C, 0.0, B2_maxLinearCorrection) 240 241 impulse := -joint.M_mass * C 242 P := B2Vec2MulScalar(impulse, u) 243 244 cA.OperatorMinusInplace(B2Vec2MulScalar(joint.M_invMassA, P)) 245 aA -= joint.M_invIA * B2Vec2Cross(rA, P) 246 cB.OperatorPlusInplace(B2Vec2MulScalar(joint.M_invMassB, P)) 247 aB += joint.M_invIB * B2Vec2Cross(rB, P) 248 249 data.Positions[joint.M_indexA].C = cA 250 data.Positions[joint.M_indexA].A = aA 251 data.Positions[joint.M_indexB].C = cB 252 data.Positions[joint.M_indexB].A = aB 253 254 return length-joint.M_maxLength < B2_linearSlop 255 } 256 257 func (joint B2RopeJoint) GetAnchorA() B2Vec2 { 258 return joint.M_bodyA.GetWorldPoint(joint.M_localAnchorA) 259 } 260 261 func (joint B2RopeJoint) GetAnchorB() B2Vec2 { 262 return joint.M_bodyB.GetWorldPoint(joint.M_localAnchorB) 263 } 264 265 func (joint B2RopeJoint) GetReactionForce(inv_dt float64) B2Vec2 { 266 F := B2Vec2MulScalar((inv_dt * joint.M_impulse), joint.M_u) 267 return F 268 } 269 270 func (joint B2RopeJoint) GetReactionTorque(inv_dt float64) float64 { 271 return 0.0 272 } 273 274 func (joint B2RopeJoint) GetMaxLength() float64 { 275 return joint.M_maxLength 276 } 277 278 func (joint B2RopeJoint) GetLimitState() uint8 { 279 return joint.M_state 280 } 281 282 func (joint *B2RopeJoint) Dump() { 283 indexA := joint.M_bodyA.M_islandIndex 284 indexB := joint.M_bodyB.M_islandIndex 285 286 fmt.Printf(" b2RopeJointDef jd;\n") 287 fmt.Printf(" jd.bodyA = bodies[%d];\n", indexA) 288 fmt.Printf(" jd.bodyB = bodies[%d];\n", indexB) 289 fmt.Printf(" jd.collideConnected = bool(%d);\n", joint.M_collideConnected) 290 fmt.Printf(" jd.localAnchorA.Set(%.15lef, %.15lef);\n", joint.M_localAnchorA.X, joint.M_localAnchorA.Y) 291 fmt.Printf(" jd.localAnchorB.Set(%.15lef, %.15lef);\n", joint.M_localAnchorB.X, joint.M_localAnchorB.Y) 292 fmt.Printf(" jd.maxLength = %.15lef;\n", joint.M_maxLength) 293 fmt.Printf(" joints[%d] = m_world.CreateJoint(&jd);\n", joint.M_index) 294 }