private void calibrationVRtoRobot()
{
Vector3 vrPoint = Util.Math.GetTranslationVector3(UtilOld.getControllerTipPosition(ref mScene, true));
vrCallibrationPoints.Add(vrPoint);
Rhino.RhinoApp.WriteLine("add vrCallibrationPoints: " + vrPoint.ToString());
UtilOld.MarkPoint(ref mScene.staticGeometry, vrPoint, 1, 1, 0);
if (vrCallibrationPoints.Count == 8)
{
Util.Math.solveForAffineTransformOpenCV(vrCallibrationPoints, robotCallibrationPoints, ref mScene.vrToRobot);
foreach (Vector3 v in robotCallibrationPoints)
{
Vector4 v4 = new Vector4(v.X, v.Y, v.Z, 1);
v4 = mScene.vrToRobot.Inverted() * v4;
UtilOld.MarkPoint(ref mScene.staticGeometry, new Vector3(v4.X, v4.Y, v4.Z), 0, 1, 0);
}
Rhino.RhinoApp.WriteLine(mScene.vrToRobot.ToString());
Rhino.RhinoApp.WriteLine(mScene.vrToRobot.M11 + "f, " + mScene.vrToRobot.M12 + "f, " + mScene.vrToRobot.M13 + "f, " + mScene.vrToRobot.M14 + "f,");
Rhino.RhinoApp.WriteLine(mScene.vrToRobot.M21 + "f, " + mScene.vrToRobot.M22 + "f, " + mScene.vrToRobot.M23 + "f, " + mScene.vrToRobot.M24 + "f,");
Rhino.RhinoApp.WriteLine(mScene.vrToRobot.M31 + "f, " + mScene.vrToRobot.M32 + "f, " + mScene.vrToRobot.M33 + "f, " + mScene.vrToRobot.M34 + "f,");
Rhino.RhinoApp.WriteLine(mScene.vrToRobot.M41 + "f, " + mScene.vrToRobot.M42 + "f, " + mScene.vrToRobot.M43 + "f, " + mScene.vrToRobot.M44 + "f");
robotCallibrationPoints.Clear();
vrCallibrationPoints.Clear();
}
}
public bool init()
{
// Set up HMD
EVRInitError eError = EVRInitError.None;
mHMD = OpenVR.Init(ref eError, EVRApplicationType.VRApplication_Scene);
if (eError == EVRInitError.None)
{
Rhino.RhinoApp.WriteLine("Booted VR System");
renderPoseArray = new TrackedDevicePose_t[Valve.VR.OpenVR.k_unMaxTrackedDeviceCount];
gamePoseArray = new TrackedDevicePose_t[Valve.VR.OpenVR.k_unMaxTrackedDeviceCount];
}
else
{
Rhino.RhinoApp.WriteLine("Failed to boot");
mTitleBase = "SparrowHawk (No VR Detected)";
}
// // THIS IS FOR UNCLIPPED
// Width = 864;
// Height = 820;
// THIS IS FOR CLIPPED RECTANGLE
Width = 691;
Height = 692;
// Window Setup Info
mStrDriver = UtilOld.GetTrackedDeviceString(ref mHMD, OpenVR.k_unTrackedDeviceIndex_Hmd, ETrackedDeviceProperty.Prop_TrackingSystemName_String);
mStrDisplay = UtilOld.GetTrackedDeviceString(ref mHMD, OpenVR.k_unTrackedDeviceIndex_Hmd, ETrackedDeviceProperty.Prop_SerialNumber_String);
mTitleBase = "SparrowHawk - " + mStrDriver + " " + mStrDisplay;
Title = mTitleBase;
MakeCurrent();
setupScene();
if (eError == EVRInitError.None)
{
mRenderer = new VrRenderer(ref mHMD, ref mScene, mRenderWidth, mRenderHeight);
}
else
{
mRenderer = new VrRenderer(ref mHMD, ref mScene, mRenderWidth, mRenderHeight);
}
//use other 8 points for calibrartion
robotCallibrationPointsTest.Add(new Vector3(22, 15, -100) / 1000);
robotCallibrationPointsTest.Add(new Vector3(-10, 40, -153) / 1000);
robotCallibrationPointsTest.Add(new Vector3(25, -25, -181) / 1000);
foreach (Vector3 v in robotCallibrationPointsTest)
{
Vector4 v4 = new Vector4(v.X, v.Y, v.Z, 1);
v4 = mScene.vrToRobot.Inverted() * v4;
UtilOld.MarkPoint(ref mScene.staticGeometry, new Vector3(v4.X, v4.Y, v4.Z), 1, 1, 1);
}
robotCallibrationPointsTest.Clear();
//set default matrix
if (mRenderer.ovrvision_controller != null)
{
mRenderer.ovrvision_controller.setDefaultMatrixHC();
}
//detecting whether users in control or left
Rhino.DocObjects.ObjectAttributes attr = new Rhino.DocObjects.ObjectAttributes();
attr.Name = "user:out";
Point3d userP = new Point3d(0, 0, 0);
uGuid = mScene.rhinoDoc.Objects.AddPoint(userP, attr);
//testing - rotate rhino object as well
/*
* Transform transM = new Transform();
* for (int row = 0; row < 4; row++)
* {
* for (int col = 0; col < 4; col++)
* {
* transM[row, col] = mScene.platformRotation[row, col];
* }
* }
* Rhino.DocObjects.ObjectEnumeratorSettings settings = new Rhino.DocObjects.ObjectEnumeratorSettings();
* settings.ObjectTypeFilter = Rhino.DocObjects.ObjectType.Brep;
* foreach (Rhino.DocObjects.RhinoObject rhObj in mScene.rhinoDoc.Objects.GetObjectList(settings))
* {
* mDoc.Objects.Transform(rhObj.Id, transM, true);
* }
* mScene.rhinoDoc.Views.Redraw();
*/
//testing visualize printStroke
printStroke = new Geometry.RobotPrintStroke(ref mScene);
printStroke_m = new Material.SingleColorMaterial(1, 1, 0, 1.0f);
return(eError == EVRInitError.None);
}
private void initMeshGeometry(ref Mesh triMesh)
{
//get faces from mesh
List <MeshFace> faces = new List <MeshFace>();
List <int> indices_array = new List <int>();
foreach (MeshFace face in triMesh.Faces)
{
faces.Add(face);
//TODO-loop throgn the original mesh to optimize
if (face.IsQuad)
{
Rhino.RhinoApp.WriteLine("Triangulate error.");
}
else
{
indices_array.Add(face.A);
indices_array.Add(face.B);
indices_array.Add(face.C);
}
}
mGeometryIndices = indices_array.ToArray();
//get vertices from mesh
// rhino coordinate system is different from OpenGL
List <Point3d> vertices = new List <Point3d>();
List <float> vertices_array = new List <float>();
OpenTK.Matrix4 transToOrigin = new OpenTK.Matrix4();
// visualize the origin of the platform for debugging
if (!mScene.vrToRobot.Equals(OpenTK.Matrix4.Identity))
{
OpenTK.Vector3 robotO = UtilOld.platformToVRPoint(ref mScene, new OpenTK.Vector3(0, 0, 0));
UtilOld.MarkPoint(ref mScene.staticGeometry, robotO, 0, 1, 0);
}
foreach (Point3d vertex in triMesh.Vertices)
{
vertices.Add(vertex);
OpenTK.Vector3 rp = new OpenTK.Vector3((float)vertex.X, (float)vertex.Y, (float)vertex.Z);
/*
* if(transM != OpenTK.Matrix4.Identity)
* {
* rp = Util.transformPoint(transM, rp);
*
* }*/
//seperate the data and visualization
OpenTK.Vector3 p = UtilOld.platformToVRPoint(ref mScene, rp);
if (isRotate)
{
p = UtilOld.transformPoint(mScene.tableGeometry.transform.Inverted(), p);
}
vertices_array.Add(p.X);
vertices_array.Add(p.Y);
vertices_array.Add(p.Z);
}
mGeometry = vertices_array.ToArray();
mNumPrimitives = mGeometryIndices.Length / 3;
primitiveType = OpenTK.Graphics.OpenGL4.BeginMode.Triangles;
}
