public override void Init(int nGPU)
{
m_point = new MyVector3D();
m_anchor = new MyVector3D();
m_axis = new MyVector3D();
m_rateOfChange = new MyVector3D();
m_matrixOps = new MyMatrixAutoOps(Owner, MatOperation.Multiplication);
}
public override void Execute()
{
Owner.Anchors.SafeCopyToHost();
Owner.RotationAxes.SafeCopyToHost();
Owner.Point.SafeCopyToHost();
for (int i = 0; i < Owner.ROWS; i++)
{
m_point.X = Owner.Point.Host[0];
m_point.Y = Owner.Point.Host[1];
m_point.Z = Owner.Point.Host[2];
m_anchor.X = Owner.Anchors.Host[3 * i];
m_anchor.Y = Owner.Anchors.Host[3 * i + 1];
m_anchor.Z = Owner.Anchors.Host[3 * i + 2];
m_axis.X = Owner.RotationAxes.Host[3 * i];
m_axis.Y = Owner.RotationAxes.Host[3 * i + 1];
m_axis.Z = Owner.RotationAxes.Host[3 * i + 2];
m_rateOfChange = MyVector3D.CrossProduct(m_axis, m_point - m_anchor);
Owner.JacobianTranspose.Host[3 * i] = (float)m_rateOfChange.X;
Owner.JacobianTranspose.Host[3 * i + 1] = (float)m_rateOfChange.Y;
Owner.JacobianTranspose.Host[3 * i + 2] = (float)m_rateOfChange.Z;
}
for (int i = 0; i < Owner.JacobianTranspose.Count; i++)
{
if (float.IsNaN(Owner.JacobianTranspose.Host[i]))
{
Owner.JacobianTranspose.Host[i] = 0;
}
}
Owner.JacobianTranspose.SafeCopyToDevice();
m_matrixOps.Run(Matrix.MatOperation.Multiplication, Owner.JacobianTranspose, Owner.Force, Owner.Output);
}
public static MyVector3D CrossProduct(MyVector3D a, MyVector3D b)
{
return(new MyVector3D((a.Y * b.Z) - (b.Y * a.Z), (a.Z * b.X) - (b.Z * a.X), (a.X * b.Y) - (b.X * a.Y)));
}
public MyVector3D(MyVector3D vector)
{
X = vector.X;
Y = vector.Y;
Z = vector.Z;
}