public Needle0(Vector3D needle_tip_position)
{
Vector3D circle_center = new Vector3D();
circle_center = local_coordinate.origin;
this.needle_tip_position = needle_tip_position;
needle_holder_position = 2 * circle_center - needle_tip_position;
Print0.print_vector(needle_holder_position);
}
public void SELECT_EXIT(double x, double y, double z)
{
Console.Write("\nState 2 EXIT POINT selected\n");
Vector3D exit_point = new Vector3D(x, y, z);
//Vector3D exit_point = new Vector3D(-25, 0, 130);
trajectory.exit_point = exit_point;
//trajectory.exit_point = find_tip_location(exit_point);
Print0.print_vector(exit_point);
state++;
}
public void SELECT_ENTRY(double x, double y, double z)
{
Console.Write("\nState 1: ENTRY POINT selected\n");
Vector3D entry_point = new Vector3D(x, y, z);
//Vector3D entry_point = new Vector3D(-30, 0, 130);
trajectory.entry_point = entry_point;
//trajectory.entry_point = find_tip_location(entry_point);
Print0.print_vector(entry_point);
state++;
}
public void create() // enrty and exit point must be given beforehand
{
incr = Math.PI / number_of_points;
local_coordinate.origin = new Vector3D();
local_coordinate.e_x = new Vector3D();
local_coordinate.e_y = new Vector3D();
local_coordinate.e_z = new Vector3D();
twisted_local_coordinate.origin = new Vector3D();
twisted_local_coordinate.e_x = new Vector3D();
twisted_local_coordinate.e_y = new Vector3D();
twisted_local_coordinate.e_z = new Vector3D();
twisted_local_coordinate.e_x = new Vector3D(); //e: unit vector
twisted_local_coordinate.e_x = exit_point - entry_point;
local_coordinate.e_x = new Vector3D(twisted_local_coordinate.e_x.X, 0, twisted_local_coordinate.e_x.Z);
local_coordinate.e_x = local_coordinate.e_x / local_coordinate.e_x.Length;
local_coordinate.e_y = new Vector3D(0, 1, 0);
local_coordinate.e_z = Vector3D.CrossProduct(local_coordinate.e_y, twisted_local_coordinate.e_x);
local_coordinate.e_z = local_coordinate.e_z / local_coordinate.e_z.Length;
twisted_local_coordinate.e_y = Vector3D.CrossProduct(local_coordinate.e_z, twisted_local_coordinate.e_x);
Vector3D offset = new Vector3D();
offset = Math.Sqrt(needle_radius * needle_radius - .25 * twisted_local_coordinate.e_x.Length * twisted_local_coordinate.e_x.Length) * twisted_local_coordinate.e_y / twisted_local_coordinate.e_y.Length;
local_coordinate.origin = new Vector3D(0.5 * (entry_point.X + exit_point.X), 0.5 * (entry_point.Y + exit_point.Y), 0.5 * (entry_point.Z + exit_point.Z));
local_coordinate.origin = local_coordinate.origin - offset;
twisted_local_coordinate.origin = local_coordinate.origin;
twisted_local_coordinate.e_z = local_coordinate.e_z;
needle_tip_position = entry_point;
needle_tip_twist = Math.Acos(Vector3D.DotProduct((needle_tip_position - local_coordinate.origin), local_coordinate.e_x) / ((needle_tip_position - local_coordinate.origin).Length * local_coordinate.e_x.Length));
needle_tip_twist = Math.PI - needle_tip_twist;
center = new Vector3D();
center = local_coordinate.origin;
normal = new Vector3D();
normal = local_coordinate.e_z;
Console.Write("\nreal entry\n");
Print0.print_vector(entry_point);
Console.Write("\nreal exit\n");
Print0.print_vector(exit_point);
Console.Write("\ncenter\n");
Print0.print_vector(center);
}
