forked from g3n/engine
/
rotationalmotor.go
80 lines (58 loc) · 1.94 KB
/
rotationalmotor.go
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
// Copyright 2016 The G3N Authors. All rights reserved.
// Use of this source code is governed by a BSD-style
// license that can be found in the LICENSE file.
package equation
import (
"github.com/g3n/engine/math32"
)
// RotationalMotor is a rotational motor constraint equation.
// Tries to keep the relative angular velocity of the bodies to a given value.
type RotationalMotor struct {
Equation // TODO maybe this should embed Rotational instead ?
axisA *math32.Vector3 // World oriented rotational axis
axisB *math32.Vector3 // World oriented rotational axis
targetSpeed float32 // Target speed
}
// NewRotationalMotor creates and returns a pointer to a new RotationalMotor equation object.
func NewRotationalMotor(bodyA, bodyB IBody, maxForce float32) *RotationalMotor {
re := new(RotationalMotor)
re.Equation.initialize(bodyA, bodyB, -maxForce, maxForce)
return re
}
// SetAxisA sets the axis of body A.
func (ce *RotationalMotor) SetAxisA(axisA *math32.Vector3) {
ce.axisA = axisA
}
// AxisA returns the axis of body A.
func (ce *RotationalMotor) AxisA() math32.Vector3 {
return *ce.axisA
}
// SetAxisB sets the axis of body B.
func (ce *RotationalMotor) SetAxisB(axisB *math32.Vector3) {
ce.axisB = axisB
}
// AxisB returns the axis of body B.
func (ce *RotationalMotor) AxisB() math32.Vector3 {
return *ce.axisB
}
// SetTargetSpeed sets the target speed.
func (ce *RotationalMotor) SetTargetSpeed(speed float32) {
ce.targetSpeed = speed
}
// TargetSpeed returns the target speed.
func (ce *RotationalMotor) TargetSpeed() float32 {
return ce.targetSpeed
}
// ComputeB
func (re *RotationalMotor) ComputeB(h float32) float32 {
// g = 0
// gdot = axisA * wi - axisB * wj
// gdot = G * W = G * [vi wi vj wj]
// =>
// G = [0 axisA 0 -axisB]
re.jeA.SetRotational(re.axisA.Clone())
re.jeB.SetRotational(re.axisB.Clone().Negate())
GW := re.ComputeGW() - re.targetSpeed
GiMf := re.ComputeGiMf()
return -GW*re.b - h*GiMf
}