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
}
// SetCol sets the c'th column of m with vector, v
func (m *mesh) SetCol(v vec.Vectorer, c int) {
for i, e := range v.Slice() {
m.elems.SetAt(i*m.off+c, e)
}
}
// GetCol returns the c'th column of m into vector, v
// of length equal to no. of rows of mesh
func (m mesh) GetCol(v vec.Vectorer, c int) {
off := m.off
for i := range v.Slice() {
v.SetAt(i, m.elems.GetAt(i*off+c))
}
}
// SetRow sets the r'th column of m with vector, v
func (m *mesh) SetRow(v vec.Vectorer, r int) {
off := r * m.off
for j, e := range v.Slice() {
m.elems.SetAt(off+j, e)
}
}
// GetRow returns the r'th column of m into vector, v
// of length equal to no. of cols of mesh
func (m mesh) GetRow(v vec.Vectorer, r int) {
off := r * m.off
for j := range v.Slice() {
v.SetAt(j, m.elems.GetAt(off+j))
}
}
// SetDiag puts the diagonal, d into the mesh
// m must be a square matrix
func (m *mesh) SetDiag(d vec.Vectorer) {
for e := range d.Slice() {
m.SetAtNode(d.GetAt(e), e, e)
}
}
// GetDiag puts the diagonal of the mesh into
// the vector, m must be a square matrix
func (m mesh) GetDiag(d vec.Vectorer) {
for e := range d.Slice() {
d.SetAt(e, m.GetAtNode(e, e))
}
}
// Solve Ux = y, replace b with solution
// column oriented backward substitution
func (U mesh) SolveUx(y vec.Vectorer) {
n := U.c
for j := n - 1; j >= 1; j-- {
y.SetAt(j, y.GetAt(j)/U.GetAtNode(j, j))
for k := 0; k < j; k++ {
y.SetAt(k, y.GetAt(k)-y.GetAt(j)*U.GetAtNode(k, j))
}
}
y.SetAt(0, y.GetAt(0)/U.GetAtNode(0, 0))
}
// Solve Ly = b for y, replace b with solution
// column oriented forward substitution
func (L mesh) SolveLy(b vec.Vectorer) {
n := L.c
for j := 0; j < n-1; j++ {
b.SetAt(j, b.GetAt(j)/L.GetAtNode(j, j))
for k := j + 1; k < n; k++ {
b.SetAt(k, b.GetAt(k)-b.GetAt(j)*L.GetAtNode(k, j))
}
}
b.SetAt(n-1, b.GetAt(n-1)/L.GetAtNode(n-1, n-1))
}