src/sims/dynamics/joints/test/PrismaticJoint.scala


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

package sims.dynamics.joints.test

import sims.dynamics._
import sims.dynamics.joints._
import sims.geometry._

case class PrismaticJoint(node1: Body, anchor1: Vector2D, node2: Body, anchor2: Vector2D) extends Joint{
  def this(node1: Body, node2: Body) = this(node1, node1.pos, node2, node2.pos)
  
  val angle = node2.rotation - node1.rotation
  
  private val a1 = anchor1 - node1.pos
  private val a2 = anchor2 - node2.pos
  private val initRotation1 = node1.rotation
  private val initRotation2 = node2.rotation
  
  def connection1 = (a1 rotate (node1.rotation - initRotation1)) + node1.pos
  def connection2 = (a2 rotate (node2.rotation - initRotation2)) + node2.pos
  
  def x = connection2 - connection1
  
  def v = node2.velocityOfPoint(connection2) - node1.velocityOfPoint(connection1)
  
  
  def correctVelocity(h: Double) = {
    correctLinear(h)
    //correctAngular(h)
  }
  
  def correctLinear(h: Double) = {
    val x = this.x.unit
    val n0 = x.leftNormal
    val v = this.v
    val r1 = (connection1 - node1.pos)
    val r2 = (connection2 - node2.pos)
    val cr1 = r1 cross n0
    val cr2 = r2 cross n0
    val Cdot = n0 dot v
    val invMass = 1/node1.mass + 1/node1.I * cr1 * cr1 + 1/node2.mass + 1/node2.I * cr2 * cr2
    val m = if (invMass == 0.0) 0.0 else 1/invMass
    val impulse = -n0 * m * Cdot
    node1.applyImpulse(-impulse, connection1)
    node2.applyImpulse(impulse, connection2)
  }
  
  //J=[-1,1]
  
  def correctAngular(h: Double) = {
    val w = node2.angularVelocity - node1.angularVelocity
    val Cdot = w
    val invMass = node1.I + node2.I
    val m = 1 / invMass
    val lambda = m * Cdot
    node1.angularVelocity += lambda / node1.I
    node2.angularVelocity += -lambda / node2.I
  }
  
  def correctPosition(h: Double) = {
    /*
    val x = this.x.unit
    val n0 = x.leftNormal
    val v = this.v
    val r1 = (connection1 - node1.pos)
    val r2 = (connection2 - node2.pos)
    val cr1 = r1 cross n0
    val cr2 = r2 cross n0
    val C = n0 dot x
    val invMass = 1/node1.mass + 1/node1.I * cr1 * cr1 + 1/node2.mass + 1/node2.I * cr2 * cr2
    val m = if (invMass == 0.0) 0.0 else 1/invMass
    val impulse = -n0 * m * C
    node1.pos += -impulse
    node1.rotation += -impulse cross r1
    node2.pos += impulse
    node2.rotation += impulse cross r2
    
    val relOmega = node2.angularVelocity - node2.angularVelocity
    val invMassOmega = node1.I + node2.I
    val mOmega = if (invMassOmega == 0.0) 0.0 else 1/invMassOmega
    val Crot = node2.rotation - node2.rotation + angle 
    node1.rotation += mOmega * Crot
    node2.rotation += -mOmega * Crot
      */
  }
}