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; }