public virtual void Calibrate()
{
var conjugate = new Quaternion(RawRotation.X, RawRotation.Y, RawRotation.Z, RawRotation.W) * RotationOffset;
conjugate.Conjugate();
BaseRotation = conjugate;
BasePosition = -(RawPosition * PositionScaleFactor) + _positionOffset;
}
private void update_needle_holder_position(Vector3D forearm_orientation)
{
Vector3D axis = new Vector3D();
axis = Vector3D.CrossProduct(circle_normal, forearm_orientation);
axis = axis / axis.Length;
double angle;
angle = Math.Acos(Vector3D.DotProduct(circle_normal,forearm_orientation)/(circle_normal.Length*forearm_orientation.Length));
//needle_holder_position = 2 * circle_center - needle_tip_position;
Vector3D temp = new Vector3D();
temp = 2 * (circle_center - needle_tip_position);
Quaternion q_needle_holder_position = new Quaternion(temp.X, temp.Y, temp.Z, 0);
Quaternion q_axis = new Quaternion(Math.Sin(angle / 2) * axis.X, Math.Sin(angle / 2) * axis.Y, Math.Sin(angle / 2) * axis.Z, Math.Cos(angle / 2));
Quaternion q_axis_conjugate = new Quaternion();
q_axis_conjugate = q_axis;
q_axis_conjugate.Conjugate();
q_needle_holder_position = q_axis * q_needle_holder_position * q_axis_conjugate;
needle_holder_position.X = q_needle_holder_position.X + needle_tip_position.X;
needle_holder_position.Y = q_needle_holder_position.Y + needle_tip_position.Y;
needle_holder_position.Z = q_needle_holder_position.Z + needle_tip_position.Z;
}
/*private void update_needle_holder_twist()
{
Vector3D temp = new Vector3D();
temp = needle_holder_position - circle_center;
needle_holder_twist = Math.Acos(Vector3D.DotProduct(temp, e_x) / (temp.Length * e_x.Length));
}*/
private void update_needle_holder_position()
{
Vector3D temp = new Vector3D();
temp = needle_holder_position - circle_center;
Quaternion q_needle_holder_position = new Quaternion(temp.X, temp.Y, temp.Z, 0);
//print_quaternion(q_needle_holder_position);
Quaternion q_circle_normal = new Quaternion(Math.Sin(-t_incr / 2) * circle_normal.X, Math.Sin(-t_incr / 2) * circle_normal.Y, Math.Sin(-t_incr / 2) * circle_normal.Z, Math.Cos(-t_incr / 2));
Quaternion q_circle_normal_conjugate = new Quaternion();
q_circle_normal_conjugate = q_circle_normal;
q_circle_normal_conjugate.Conjugate();
q_needle_holder_position = q_circle_normal * q_needle_holder_position * q_circle_normal_conjugate;
needle_holder_position.X = q_needle_holder_position.X + circle_center.X;
needle_holder_position.Y = q_needle_holder_position.Y + circle_center.Y;
needle_holder_position.Z = q_needle_holder_position.Z + circle_center.Z;
//print_quaternion(q_circle_normal);
//print_quaternion(q_needle_holder_position);
}
public Vector3D update_needle_tip_position()
{
Vector3D circle_normal = new Vector3D();
circle_normal = local_coordinate.e_z;
Vector3D circle_center = new Vector3D();
circle_center = local_coordinate.origin;
Vector3D temp = new Vector3D();
temp = needle_tip_position - circle_center;
Quaternion q_needle_tip_position = new Quaternion(temp.X, temp.Y, temp.Z, 0);
Quaternion q_circle_normal = new Quaternion(Math.Sin(-incr / 2) * circle_normal.X, Math.Sin(-incr / 2) * circle_normal.Y, Math.Sin(-incr / 2) * circle_normal.Z, Math.Cos(-incr / 2));
Quaternion q_circle_normal_conjugate = new Quaternion();
q_circle_normal_conjugate = q_circle_normal;
q_circle_normal_conjugate.Conjugate();
q_needle_tip_position = q_circle_normal * q_needle_tip_position * q_circle_normal_conjugate;
needle_tip_position.X = q_needle_tip_position.X + circle_center.X;
needle_tip_position.Y = q_needle_tip_position.Y + circle_center.Y;
needle_tip_position.Z = q_needle_tip_position.Z + circle_center.Z;
/*
needle_tip_position = rotateAboutAxis(normal, needle_tip_position - center, incr);
needle_tip_position = needle_tip_position + center;
*/
return needle_tip_position;
}