// Update is called once per frame
void Update()
{
bool button1State = PluginImport.GetButton1State();
if (button1State && !previousButton1State)
{
penColorNum = (penColorNum + 1) % penColors.Length;
}
previousButton1State = button1State;
//if (PluginImport.GetButton2State()) cleanBoard();
bool eraseState = PluginImport.GetButton2State();
if (eraseState)
{
changePenColor(new Color(0.25f, 0.25f, 0.25f));
}
else
{
changePenColor(penColorNum);
}
bool shouldDraw = PluginImport.GetContact() && (myCounter > 0);
if (shouldDraw)
{
double[] pos = ConverterClass.ConvertIntPtrToDouble3(PluginImport.GetProxyPosition());
double[] dir = ConverterClass.ConvertIntPtrToDouble3(PluginImport.GetProxyDirection());
Vector3 position = new Vector3((float)pos[0], (float)pos[1], (float)pos[2]);
Vector3 direction = new Vector3((float)dir[0], (float)dir[1], (float)dir[2]);
double[] realPos = ConverterClass.ConvertIntPtrToDouble3(PluginImport.GetDevicePosition());
Vector3 realPosition = new Vector3((float)realPos[0], (float)realPos[1], (float)realPos[2]);
float force = (realPosition - position).magnitude;
if (force > 1.0f)
{
force = 1.0f;
}
RaycastHit hitInfo = new RaycastHit();
bool hasHit = Physics.Raycast(position, direction, out hitInfo);
if (previousShouldDraw)
{
drawBlobLine(previousCoord, hitInfo.textureCoord, blobRadius, penColors[penColorNum], force, blobSteps, eraseState);
}
else
{
drawBlob(hitInfo.textureCoord, blobRadius, penColors[penColorNum], force, eraseState);
}
previousCoord = hitInfo.textureCoord;
boardTexture.SetPixels(boardPixels);
boardTexture.Apply();
}
previousShouldDraw = shouldDraw;
myCounter++;
}
/******************************************************************************************************************************************************************/
//Get Proxy Position and Orientation generic function
public void GetProxyValues()
{
/*Proxy Position*/
//Convert IntPtr to Double3Array
myProxyPosition = ConverterClass.ConvertIntPtrToDouble3(PluginImport.GetProxyPosition());
Debug.Log(myProxyPosition);
//Attach the Cursor Node
Vector3 positionCursor = new Vector3();
positionCursor = ConverterClass.ConvertDouble3ToVector3(myProxyPosition);
//Assign Haptic Values to Cursor
myHapticClassScript.hapticCursor.transform.position = positionCursor;
//Proxy Right - Not use in that case
//Convert IntPtr to Double3Array
myProxyRight = ConverterClass.ConvertIntPtrToDouble3(PluginImport.GetProxyRight());
//Attach the Cursor Node
Vector3 rightCursor = new Vector3();
rightCursor = ConverterClass.ConvertDouble3ToVector3(myProxyRight);
//Proxy Direction
//Convert IntPtr to Double3Array
myProxyDirection = ConverterClass.ConvertIntPtrToDouble3(PluginImport.GetProxyDirection());
//Attach the Cursor Node
Vector3 directionCursor = new Vector3();
directionCursor = ConverterClass.ConvertDouble3ToVector3(myProxyDirection);
//Proxy Torque
myProxyTorque = ConverterClass.ConvertIntPtrToDouble3(PluginImport.GetProxyTorque());
//Attach the Cursor Node
Vector3 torqueCursor = new Vector3();
torqueCursor = ConverterClass.ConvertDouble3ToVector3(myProxyTorque);
//Set Orientation
myHapticClassScript.hapticCursor.transform.rotation = Quaternion.LookRotation(directionCursor, torqueCursor);
//Proxy Orientation
//Convert IntPtr to Double4Array
/*myProxyOrientation = ConverterClass.ConvertIntPtrToDouble4(PluginImport.GetProxyOrientation());
*
* //Attach the Cursor Node
* Vector4 OrientationCursor = new Vector4();
* OrientationCursor = ConverterClass.ConvertDouble4ToVector4(myProxyOrientation);
*
* //Assign Haptic Values to Cursor
* myHapticClassScript.hapticCursor.transform.rotation = new Quaternion(OrientationCursor.x,OrientationCursor.y,OrientationCursor.z,OrientationCursor.w);
* Debug.Log(OrientationCursor.x + " " + OrientationCursor.y + " " + OrientationCursor.z + " " + OrientationCursor.w);*/
}
// Update is called once per frame
void Update()
{
double[] pos = ConverterClass.ConvertIntPtrToDouble3(PluginImport.GetDevicePosition());
Vector3 position = new Vector3((float)pos [0], (float)pos [1], (float)pos [2]);
button1 = PluginImport.GetButton1State(); // getting state of button 1 on phantom
Debug.Log(button1 + " " + button2); // printing button 1 and button 2 state on console
if (button1 && !previousbutton1state) // routine for writing coordinate values to text file
{
//////////////////////////////////////////////////////////////////
using (StreamWriter writer = new StreamWriter("G:\\log.txt", true))
// Loop through ten numbers.
for (int i = 0; i < 3; i++)
{
//Write format string to file.
//writer.Write ("{0:0.0} ", position [i]);
writer.WriteLine(position);
} // for loop
using (StreamWriter writer =
new StreamWriter("G:\\log.txt", true)) {
writer.WriteLine("\r\n");
// //writer.WriteLine("First target coordinates");
} // stream
//
}
previousbutton1state = button1;
/////////////////////////////////////////////////////////////////////
button2 = PluginImport.GetButton2State();
if (button2 && !previousbutton2state) // Controlling invisibility of the pointer object
{
visibility.GetComponent <MeshRenderer> ().enabled = false; // making object invisible
} // update
previousbutton2state = button2;
if (Input.GetKeyDown(KeyCode.A))
{
visibility.GetComponent <MeshRenderer> ().enabled = true; // Making object visible again on pressing button 2
}
}
/******************************************************************************************************************************************************************/
/*************************************************************/
// Generic functionnalities
/*************************************************************/
/******************************************************************************************************************************************************************/
//generic function that returns the current mode
/******************************************************************************************************************************************************************/
//Haptic workspace generic functions
/******************************************************************************************************************************************************************/
//Get Proxy Position and Orientation generic function
public void GetProxyValues()
{
/*Proxy Position*/
//Convert IntPtr to Double3Array
myProxyPosition = ConverterClass.ConvertIntPtrToDouble3(PluginImport.GetProxyPosition());
//Attach the Cursor Node
Vector3 positionCursor = new Vector3();
positionCursor = ConverterClass.ConvertDouble3ToVector3(myProxyPosition);
//Assign Haptic Values to Cursor
//myHapticClassScript.hapticCursor.transform.position = positionCursor;
//Proxy Right - Not use in that case
//Convert IntPtr to Double3Array
/*myProxyRight = ConverterClass.ConvertIntPtrToDouble3(PluginImport.GetProxyRight());
* //Attach the Cursor Node
* Vector3 rightCursor = new Vector3();
* rightCursor = ConverterClass.ConvertDouble3ToVector3(myProxyRight);
*
* //Proxy Direction
* //Convert IntPtr to Double3Array
* myProxyDirection = ConverterClass.ConvertIntPtrToDouble3( PluginImport.GetProxyDirection());
* //Attach the Cursor Node
* Vector3 directionCursor = new Vector3();
* directionCursor = ConverterClass.ConvertDouble3ToVector3(myProxyDirection);
*
* //Proxy Torque
* myProxyTorque = ConverterClass.ConvertIntPtrToDouble3(PluginImport.GetProxyTorque());
* //Attach the Cursor Node
* Vector3 torqueCursor = new Vector3();
* torqueCursor = ConverterClass.ConvertDouble3ToVector3(myProxyTorque);
*
* //Set Orientation
* myHapticClassScript.hapticCursor.transform.rotation = Quaternion.LookRotation(directionCursor,torqueCursor);*/
}
/******************************************************************************************************************************************************************/
public void GetProxyValues()
{
//Convert IntPtr to Double3Array
myProxyPosition = ConverterClass.ConvertIntPtrToDouble3(PluginImport.GetProxyPosition());
//Attach the Cursor Node
Vector3 positionCursor = new Vector3();
positionCursor = ConverterClass.ConvertDouble3ToVector3(myProxyPosition);
positionCursor.z *= 2f;
myHapticClassScript.hapticCursor.transform.position = positionCursor;
myProxyOrientation = ConverterClass.ConvertIntPtrToDouble4(PluginImport.GetProxyOrientation());
//Attach the Cursor Node
Vector4 OrientationCursor = new Vector4();
OrientationCursor = ConverterClass.ConvertDouble4ToVector4(myProxyOrientation);
//Assign Haptic Values to Cursor
myHapticClassScript.hapticCursor.transform.rotation = new Quaternion(OrientationCursor.x, OrientationCursor.y, OrientationCursor.z, OrientationCursor.w);
}
void Update()
{
/////////////////////////////////// getting position of Haptic device ///////////////////////
double[] pos = ConverterClass.ConvertIntPtrToDouble3(PluginImport.GetProxyPosition());
//Vector3 position1 = new Vector3 ((float)pos [2], (float)pos [1], -(float)pos [0]); // applying rotation as device is rotated 90 degrees to left... swapping coordinates
/////////////////////////////////////////////////////////////////////////////////////////////
Vector3 position1 = new Vector3((float)pos [0], (float)pos [1], (float)pos [2]);
position1 = rotation * position1;
///////////////////////////////// Instantiation of Targets as Key B is pressed + writing target coordinates to text file //////////////////////////
button1 = PluginImport.GetButton1State();
if (button1 && !previousbutton1stateI)
{
target [i] = Instantiate(ball);
target [i].transform.position = new Vector3(i * 0.1f, j * 1.0f, -1.0f);
using (StreamWriter writer = new StreamWriter("D:\\log.txt", true)) {
writer.WriteLine("");
writer.Write("Position for Target ");
writer.Write(i + 1);
writer.Write(" = ");
writer.Write(target[i].transform.position);
}
if (i > 0)
{
Destroy(target [i - 1]); // destroying previous targets as the next target is displayed
}
i++;
j = j + 0.1f;
}
previousbutton1stateI = button1;
//////////////////////////////////////////////////////////////////////////////////////////////////////////
button1 = PluginImport.GetButton1State(); // getting state of button 1 on phantom
///////////////////////////// routine for writing pointer current position to text file/////////////////////////////////////
if (button1 && !previousbutton1state)
{
if (targetwritecounter > 0)
{
using (StreamWriter writer = new StreamWriter("D:\\log.txt", true)) {
writer.WriteLine("");
writer.Write("Hand position for target ");
writer.Write(targetwritecounter);
writer.Write(" = ");
writer.Write(gameObject.transform.position);
}
}
targetwritecounter++;
}
previousbutton1state = button1;
//////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////// Controlling visibility of haptic sphere /////////////////
if (Input.GetKeyDown(KeyCode.A)) // Controlling invisibility of the pointer object
{
visibility.GetComponent <MeshRenderer> ().enabled = false; // making object invisible on pressing A
}
if (Input.GetKeyDown(KeyCode.S))
{
visibility.GetComponent <MeshRenderer> ().enabled = true; // Making object visible again on pressing S
}
//////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////// Recentering of Head Set (Occulus)/////////////////////////////////////////
if (Input.GetKeyDown(KeyCode.F12))
{
InputTracking.Recenter();
}
//////////////////////////////////////////////////////////////////////////////////////////////////////
/***************************************************************/
//Haptic Rendering Loop
/***************************************************************/
PluginImport.RenderHaptic();
///////////// Origin offset for phantom local coordinates/////////////////////
position1 [0] = position1 [0] - (-0.2178f);
position1 [1] = position1 [1] - (-0.8274f);
position1 [2] = position1 [2] - (-0.0001f);
///////////////////////////////////////////////////////////////////////////////
Debug.Log("Phantom Coordinates" + position1.ToString("F4"));
////////////////// scaling between real world distance and unity distance ///////////////
position1 [0] = position1 [0] * 0.33f; //Real world 10cm = 30cm in unity // scaling factor10/30
position1 [1] = position1 [1] * 0.5882f; //Real world 10cm = 17cm in unity // scaling factor10/17
position1 [2] = position1 [2] * 0.625f; //Real world 10cm = 16cm in unity // scaling factor10/16
//////////////////////////////////////////////////////////////////////////////////////////
//////////////// Linear transformation from phantom origin to occulus origin /////////////////
// position1 [0] = position1 [0] + 0.006f+ 0.08f;
position1 [1] = position1 [1] + 0.63f;
position1 [2] = position1 [2] + 0.3f;
transform.position = position1;
//Debug.Log ("Phantom Coordinates" + position1.ToString ("F4"));
//////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////// Printing Phantom coordinates to console //////////////////////
//Debug.Log ("Phantom Coordinates" + position1.ToString ("F4"));
//////////////////////////////////////////////////////////////////////////////////////////////
//Debug.Log ("Button 1: " + PluginImport.GetButton1State());
//Debug.Log ("Button 2: " + PluginImport.GetButton2State());
}
void Update()
{
/***************************************************************/
//Update Workspace as function of camera
/***************************************************************/
//PluginImport.UpdateWorkspace(myHapticCamera.transform.rotation.eulerAngles.y);//To be deprecated
//Update the Workspace as function of camera
for (int i = 0; i < workspaceUpdateValue.Length; i++)
{
workspaceUpdateValue[i] = myHapticCamera.transform.rotation.eulerAngles.y;
}
PluginImport.UpdateHapticWorkspace(ConverterClass.ConvertFloatArrayToIntPtr(workspaceUpdateValue));
/***************************************************************/
//Update cube workspace
/***************************************************************/
myGenericFunctionsClassScript.UpdateGraphicalWorkspace();
/***************************************************************/
//Haptic Rendering Loop
/***************************************************************/
PluginImport.RenderHaptic();
//Associate the cursor object with the haptic proxy value
myGenericFunctionsClassScript.GetProxyValues();
//For the Puncture Mode effect
if (PluginImport.GetMode() == 3)
{
//Debug.Log ("Contact state is set to " + PluginImport.GetContact());
//Debug.Log ("Penetration State " + PluginImport.GetPenetrationRatio());
double[] myScp = new double[3];
myScp = ConverterClass.ConvertIntPtrToDouble3(PluginImport.GetFirstScpPt());
//Debug.Log (" SCP " + myScp[0] + " " + myScp[1] + " " + myScp[2]);
Vector3 posInjectionHole;
posInjectionHole = ConverterClass.ConvertDouble3ToVector3(myScp);
GameObject.Find("InjectionMarker").transform.position = posInjectionHole;
/*double[] myProx = new double[3];
* myProx = ConverterClass.ConvertIntPtrToDouble3(PluginImport.GetProxyPosition());
*
* Vector3 posProx;
* posProx = ConverterClass.ConvertDouble3ToVector3(myProx);
* GameObject.Find ("ProxyTipMarker").transform.position = posProx;*/
/*double[] myDev = new double[3];
* myDev = ConverterClass.ConvertIntPtrToDouble3(PluginImport.GetDevicePosition());
*
* Vector3 posDevice;
* posDevice = ConverterClass.ConvertDouble3ToVector3(myDev);
* GameObject.Find ("DeviceTipMarker").transform.position = posDevice;*/
double[] myPinch = new double[3];
myPinch = ConverterClass.ConvertIntPtrToDouble3(PluginImport.GetPunctureDirection());
Vector3 start = new Vector3();
start = ConverterClass.ConvertDouble3ToVector3(myScp);
Vector3 end = new Vector3();
end = ConverterClass.ConvertDouble3ToVector3(myPinch);
end.Normalize();
Debug.DrawLine(start, start + end * maxPenetration, Color.green);
//Ray Cast so we can determine the limitation of the puncture
RaycastHit[] hits;
hits = Physics.RaycastAll(start, end, maxPenetration);
if (hits.Length != 0)
{
//Declare a float array to store the tissue layer
float[] tissueLayers = new float[hits.Length];
//Declare a string array to store the name of the tissue layer
string[] punctObjects = new string[hits.Length];
int nbLayer = 0;
for (int i = 0; i < hits.Length; i++)
{
RaycastHit hit = hits[i];
//Only if the object is declared as touchable
if (hit.collider.gameObject.tag == "Touchable")
{
tissueLayers[nbLayer] = hit.distance;
punctObjects[nbLayer] = hit.collider.name;
nbLayer++;
}
}
/*Declaration of the Puncture Stack
* Additionally, on the basis of the puncture stack components, the plugin setup a penetration restriction
* - due to the fact that Proxy Method along such constraint line is not accurate - most probably due to the fact
* that device position and proxy position differ because the constraint applies forces onto the device.
* So, the plugin impedes the proxy to penetrate in underlying layer when their popthrough values is null
*/
SetPunctureStack(nbLayer, punctObjects, tissueLayers);
}
}
}
//Get Proxy Position and Orientation generic function
public void GetProxyValues(bool useTransform)
{
/*Proxy Position*/
//Convert IntPtr to Double3Array
myProxyPosition = ConverterClass.ConvertIntPtrToDouble3(PluginImport.GetProxyPosition());
//Attach the Cursor Node
Vector3 positionCursor = new Vector3();
positionCursor = ConverterClass.ConvertDouble3ToVector3(myProxyPosition);
positionCursor.z *= 2f;
//Vector3 movement = positionCursor - rb.position;
if (useTransform)
{
myHapticClassScript.hapticCursor.transform.position = positionCursor;
}
else
{
rb.MovePosition(positionCursor);
}
//myHapticClassScript.hapticCursor.transform.position = positionCursor;
/*
* if (movement.magnitude > 0.1)
* {
* positionCursor = myHapticClassScript.hapticCursor.transform.position;//movement.normalized * 0.01f + myHapticClassScript.hapticCursor.transform.position;
* }*/
//rb.velocity = new Vector3(0,0,0);
//if (movement.magnitude > 0.1)
//rb.velocity = movement * Time.deltaTime * 500;
//rb.AddForce(movement);
//else rb.velocity = new Vector3(0, 0, 0);
//rb.MovePosition(positionCursor);
//rb.position = positionCursor;
//rb.MovePosition(positionCursor*2);
//myHapticClassScript.hapticCursor.gameObject.GetComponent<Rigidbody>().MovePosition(positionCursor);
//rb.AddForce((rb.gameObject.transform.position - positionCursor) * 5, ForceMode.VelocityChange);
//Proxy Right - Not use in that case
//Convert IntPtr to Double3Array
/*myProxyRight = ConverterClass.ConvertIntPtrToDouble3(PluginImport.GetProxyRight());
* //Attach the Cursor Node
* Vector3 rightCursor = new Vector3();
* rightCursor = ConverterClass.ConvertDouble3ToVector3(myProxyRight);
*
* //Proxy Direction
* //Convert IntPtr to Double3Array
* myProxyDirection = ConverterClass.ConvertIntPtrToDouble3( PluginImport.GetProxyDirection());
* //Attach the Cursor Node
* Vector3 directionCursor = new Vector3();
* directionCursor = ConverterClass.ConvertDouble3ToVector3(myProxyDirection);
*
* //Proxy Torque
* myProxyTorque = ConverterClass.ConvertIntPtrToDouble3(PluginImport.GetProxyTorque());
* //Attach the Cursor Node
* Vector3 torqueCursor = new Vector3();
* torqueCursor = ConverterClass.ConvertDouble3ToVector3(myProxyTorque);
*
* //Set Orientation
* myHapticClassScript.hapticCursor.transform.rotation = Quaternion.LookRotation(directionCursor,torqueCursor);*/
//Proxy Orientation
//Convert IntPtr to Double4Array
myProxyOrientation = ConverterClass.ConvertIntPtrToDouble4(PluginImport.GetProxyOrientation());
//Attach the Cursor Node
Vector4 OrientationCursor = new Vector4();
OrientationCursor = ConverterClass.ConvertDouble4ToVector4(myProxyOrientation);
//Assign Haptic Values to Cursor
if (useTransform)
{
myHapticClassScript.hapticCursor.transform.rotation = new Quaternion(OrientationCursor.x, OrientationCursor.y, OrientationCursor.z, OrientationCursor.w);
}
else
{
rb.MoveRotation(new Quaternion(OrientationCursor.x, OrientationCursor.y, OrientationCursor.z, OrientationCursor.w));
}
}