blob: 09e72d967312076c69caf527dc1112894ef85ba0 (
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
|
/*
* Simple Mechanics Simulator (SiMS)
* copyright (c) 2009 Jakob Odersky
* made available under the MIT License
*/
package sims.dynamics.joints.test
import sims.dynamics._
import sims.dynamics.joints._
import sims.geometry._
class UnitCircleJoint(body: Body, anchor: Vector2D) extends Joint{
val node1 = body
val node2 = body
private val a = anchor - body.pos
private val initRotation = body.rotation
def connection = (a rotate (body.rotation - initRotation)) + body.pos
def x = connection
def v = body.velocityOfPoint(connection)
/*
* C = ||x|| - 1
* Cdot = x/||x|| dot v = u dot v
* J = [u (r cross u)]
*/
def correctVelocity(h: Double) = {
val r = connection - body.pos
val u = x.unit
val cr = r cross u
val mc = 1.0/(1/body.mass + 1/body.I * cr * cr)
val lambda = -mc * (u dot v)
val Pc = u * lambda
val vupdate = u * lambda / body.mass
val wupdate = (r cross u) * lambda / body.I
println("dv = " + vupdate + " dw = " + wupdate)
body.linearVelocity = body.linearVelocity + u * lambda / body.mass
body.angularVelocity = body.angularVelocity + (r cross u) * lambda / body.I
}
def correctPosition(h: Double) = {}
}
|
