blob: 367c84d0a0c09c2f850eb5e298434394d128a386 (
plain) (
blame)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
|
/* _____ _ __ ________ ___ *\
** / ___/(_) |/ / ___/ |__ \ Simple Mechanics Simulator 2 **
** \__ \/ / /|_/ /\__ \ __/ / copyright (c) 2011 Jakob Odersky **
** ___/ / / / / /___/ / / __/ **
** /____/_/_/ /_//____/ /____/ **
\* */
package sims.dynamics.constraints
import sims.dynamics._
import sims.math._
/** A base trait for implementing and solving constraints.
* Constraints are solved using the 'sequential impulse' method presented by Erin Catto
* at Game Developpers Conference 2008 (see http://www.box2d.org/documentation.html).
*
* One instance of this trait represents one constraint for one degree of freedom between
* two bodies.
* @see sims.dynamics.constraints.Constrained */
trait Constraint {
/** First constrained body. */
def body1: Body
/** Second constrained body. */
def body2: Body
/** Value of the position constraint function ('C' in the presentation). */
def value: Double
/** Jacobian for the velocity constraint ('J' in 'Ċ=Jv+b' in the presentation). */
def jacobian: Jacobian
/** Velocity bias ('b' in 'Ċ=Jv+b' in the presentation).*/
def bias: Double = 0
/** Lower and upper limits of the total corrective impulse allowed
* when solving this constraint.
* The first element represents the lower limit,
* the second the upper limit.
* None represents no limits. */
def limit: Option[(Double, Double)] = None
/** Defines whether or not this constraint should be treated as an inequality constraint.
* An inequality constraint will only be solved if the constraint function is less than
* zero (i.e. `value < 0`). */
def inequality = false
/** Position error used for Baumgarte sabilization.
* Corresponds to `value` by default. */
def error = value
/** Accumulated impulse. */
private var accumulated = 0.0
/** Constraint mass cache, constant for all iterations. */
private var m = 0.0
/** Jacobian cache, constant for all iterations. */
private var J: Jacobian = null
/** Velocity bias cache, constant for all iterations. */
private var b = 0.0
/** Method that should be called before solving this constraint.
* Computes constraint masses and other invariable data for iterating over constraints. */
def preSolve() = {
//compute jacobian and bias for next solving
J = jacobian
b = bias
//invMass=(J*invert(M)*transpose(J))
val invMass: Double = (
(J.v1.x * J.v1.x + J.v1.y * J.v1.y) / body1.mass + J.w1 * J.w1 / body1.inertia +
(J.v2.x * J.v2.x + J.v2.y * J.v2.y) / body2.mass + J.w2 * J.w2 / body2.inertia
)
//if invMass == 0, both bodies have infinite mass (i.e. are fixed)
m = if (invMass == 0.0) 0.0 else 1.0 / invMass
//apply accumulated impulse from prevoius step
body1.applyImpulse(J.v1 * accumulated)
body1.applyAngularImpulse(J.w1 * accumulated)
body2.applyImpulse(J.v2 * accumulated)
body2.applyAngularImpulse(J.w2 * accumulated)
//initialize accumulated impulse
//accumulated = 0.0
}
/** Solves this constraint by applying corrective impulses to its constrained bodies.
* @todo implement error correction properly for collisiions (add slop tolerance, etc)
* @param h time interval over which the correction should be applied
* @param erp error reduction parameter. */
def correctVelocity(h: Double, erp: Double): Unit = {
//if m == 0, both bodies are fixed and there is no point in applying corrective impulse
if (m == 0) return ()
//C > 0 => ignore
if (inequality && value > 0.0) return ()
//lambda
var lambda = -m * (
J.v1.x * body1.linearVelocity.x + J.v1.y * body1.linearVelocity.y + J.w1 * body1.angularVelocity +
J.v2.x * body2.linearVelocity.x + J.v2.y * body2.linearVelocity.y + J.w2 * body2.angularVelocity +
b + (erp / h * error)
)
//clamp accumulated impulse
if (limit != None) {
val temp = accumulated
accumulated = math.max(limit.get._1, math.min(accumulated + lambda, limit.get._2))
lambda = accumulated - temp
}
//apply accumulated impulse
body1.applyImpulse(J.v1 * lambda)
body1.applyAngularImpulse(J.w1 * lambda)
body2.applyImpulse(J.v2 * lambda)
body2.applyAngularImpulse(J.w2 * lambda)
}
}
|
