Initializing the Stock Material

Professor Horsch provided me with code to create a transformation matrix from reading in 3 points; origin, x-axis and y-axis. The code was included into the project but could not be tested until the haptic to screen mapping was working. After fixing the mapping last week the code was tested.

The matrix mulitplication needed is as follows:

Translation from default haptic TCP to desired Tooltip *
Current Transformation Matrix from world center point to TCP *
inverted Transformation Matrix from world center point to origin of stock material

After this multiplication, the coordinates can be used as input to the collision detection algorithms. This does not take into account orientation of the tooltip, and this doesn't matter is the tooltip is a sphere.