func fast_skewexp(m mesh.Mesher) (g mesh.Mesher) {
if r, c, _ := m.Size(); r != 3 && c != 1 {
panic("fast_skewexp: Length of input mesh should be 3x1.")
}
g = mesh.Gen(nil, 3, 3)
th := m.Norm(mesh.Two)
if th != 0 {
mscaled := mesh.Gen(nil, 3, 3)
mscaled.Vec().Skew(m.Vec())
mscaled.Scale(th)
g.Sum(math.Sin(th), mscaled, mesh.I(3))
mscaled.Powm(mscaled, 2)
g.Sum(1-math.Cos(th), mscaled, g)
} else {
g = mesh.I(3)
}
return g
}
func main() {
sk := mesh.Gen(nil, 3, 3)
w := mesh.Gen(vec.New(-1, 0, 0), 3, 1)
sk.Vec().Skew(w.Vec())
skexp := mesh.Gen(nil, 3, 3)
st := time.Now()
for i := 0; i < 1e3; i++ {
skexp.Expm(sk)
}
fmt.Println("since: ", time.Since(st)/1e3)
fmt.Println("skexp: ", skexp)
st = time.Now()
for i := 0; i < 1e3; i++ {
skexp = fast_skewexp(w)
}
fmt.Println("since: ", time.Since(st)/1e3)
fmt.Println("skexp: ", skexp)
}
func NewTwist(w, q vec.Vectorer) Twister {
t := new(twist)
t.w = w // set the axis of motion of joint
t.q = q // set the position a point on the axis of joint
t.v = vec.Zeros(AxisLength)
t.v.Cross(q, w) // qxw
t.t = mesh.Gen(nil, TwistLength, 1)
tvec := t.t.Vec().Slice()
copy(tvec[3:], w.Slice())
copy(tvec[:3], t.v.Slice())
return t
}