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)
}
}
// 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)
}
}
// 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))
}
}
// 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))
}
}
