diff options
Diffstat (limited to 'src/main/scala/sims/dynamics/constraints')
3 files changed, 180 insertions, 0 deletions
diff --git a/src/main/scala/sims/dynamics/constraints/Constraining.scala b/src/main/scala/sims/dynamics/constraints/Constraining.scala new file mode 100644 index 0000000..f1834a2 --- /dev/null +++ b/src/main/scala/sims/dynamics/constraints/Constraining.scala @@ -0,0 +1,28 @@ +/* _____ _ __ ________ ___ *\ +** / ___/(_) |/ / ___/ |__ \ Simple Mechanics Simulator 2 ** +** \__ \/ / /|_/ /\__ \ __/ / copyright (c) 2011 Jakob Odersky ** +** ___/ / / / / /___/ / / __/ ** +** /____/_/_/ /_//____/ /____/ ** +\* */ + +package sims.dynamics.constraints + +import sims.dynamics._ + +/**A base trait implemented by objects representing constraints (such as joints or collisions). + * @see sims.dynamics.constraints.Constraint + */ +trait Constraining { + + /**All constraints represented by this constraining object.*/ + def constraints: Seq[Constraint] + + /**Invoke `preSolve()` on each constraint. + * @see sims.dynamics.constraints.Constraint*/ + def preSolve() = for (c <- constraints) c.preSolve() + + /**Solves all constraints of this constraining object. + * @see sims.dynamics.constraints.Constraint*/ + def correctVelocity(h: Double, erp: Double): Unit = for (c <- constraints) c.correctVelocity(h, erp) + +}
\ No newline at end of file diff --git a/src/main/scala/sims/dynamics/constraints/Constraint.scala b/src/main/scala/sims/dynamics/constraints/Constraint.scala new file mode 100644 index 0000000..367c84d --- /dev/null +++ b/src/main/scala/sims/dynamics/constraints/Constraint.scala @@ -0,0 +1,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) + } + +}
\ No newline at end of file diff --git a/src/main/scala/sims/dynamics/constraints/Jacobian.scala b/src/main/scala/sims/dynamics/constraints/Jacobian.scala new file mode 100644 index 0000000..3b16bde --- /dev/null +++ b/src/main/scala/sims/dynamics/constraints/Jacobian.scala @@ -0,0 +1,26 @@ +/* _____ _ __ ________ ___ *\ +** / ___/(_) |/ / ___/ |__ \ Simple Mechanics Simulator 2 ** +** \__ \/ / /|_/ /\__ \ __/ / copyright (c) 2011 Jakob Odersky ** +** ___/ / / / / /___/ / / __/ ** +** /____/_/_/ /_//____/ /____/ ** +\* */ + +package sims.dynamics.constraints + +import sims.math.Vector2D + +/** A Jacobian matrix used for solving constraints. + * + * In SiMS 2 constraints are considered to remove one degree of freedom for two bodies. + * Let '''v''',,1,,, w,,1,,, '''v''',,2,,, w,,2,, be the linear and angular velocities of both bodies + * respectively. + * Let v = ['''v''',,1,,, w,,1,,, '''v''',,2,,, w,,2,,]. + * The velocity constraint function is then given by 'Ċ='''Jv'''+b' and + * the Jacobian ('''J''') is given by '''J''' = ['''Jv''',,1,,, Jw,,1,,, '''Jv''',,2,,, Jw,,2,,] = [Jv,,1,x,,, Jv,,1,y,,, Jw,,1,,, Jv,,2,x,,, Jv,,2,y,,, Jw,,2,,] + * + * @param v1 corresponds to '''Jv''',,1,, in the description above + * @param w1 corresponds to Jw,,1,, in the description above + * @param v2 corresponds to '''Jv''',,2,, in the description above + * @param w2 corresponds to Jw,,2,, in the description above + * @see sims.dynamics.constraints.Constraint */ +class Jacobian(val v1: Vector2D, val w1: Double, val v2: Vector2D, val w2: Double)
\ No newline at end of file |