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  }