protected void notifyRobotIfSafe()
{
//Vector3 tableOrigin = Util.transformPoint(mScene.vrToRobot.Inverted(), new Vector3(0, 0, 0));
Vector3 tableOrigin = UtilOld.platformToVRPoint(ref mScene, new Vector3(0, 0, 0));
Vector3 headOrigin = UtilOld.transformPoint(mScene.mHMDPose.Inverted(), new Vector3(0, 0, 0));
Vector3 displacement = tableOrigin - headOrigin;
displacement.Y = 0;
if (displacement.Length > 1.0f && isUserIn == true)
{
Rhino.DocObjects.RhinoObject rhobj = mScene.rhinoDoc.Objects.Find(uGuid);
rhobj.Attributes.Name = "user:out";
rhobj.CommitChanges();
isUserIn = false;
Rhino.RhinoApp.WriteLine("User out. " + displacement.Length);
}
else if (displacement.Length < 1.0f && isUserIn == false)
{
Rhino.DocObjects.RhinoObject rhobj = mScene.rhinoDoc.Objects.Find(uGuid);
rhobj.Attributes.Name = "user:in";
rhobj.CommitChanges();
isUserIn = true;
Rhino.RhinoApp.WriteLine("User in. " + displacement.Length);
}
}
protected void registerPositon(uint deviceIndex)
{
// Find and record position of controller
Matrix4 M = UtilOld.getLeftControllerTipPosition(ref mScene, mScene.leftControllerIdx == deviceIndex);
Vector3 origin = UtilOld.transformPoint(M, new Vector3(0, 0, 0));
if (mPoints != null)
{
mPoints.Add(origin);
}
if (mPoses != null)
{
mPoses.Add(M);
}
Rhino.RhinoApp.WriteLine("RegisterPoint: " + origin.ToString());
// Add a Point Rep object to the mScene
Geometry.Geometry point = new Geometry.PointMarker(new Vector3());
Material.Material material = new Material.SingleColorMaterial(0f, .5f, 1f, 1f);
SceneNode sceneNode = new SceneNode("point", ref point, ref material);
sceneNode.transform = M;
mScene.tableGeometry.add(ref sceneNode);
}
public void addEdge(OpenTK.Vector3 p1, Vector3 p2)
{
mPoints.Add(p1);
mPoints.Add(p2);
//when visualizing, stroke is in tableGeometry so we need to apply tableGeomeotry.transfrom inverted first
p1 = UtilOld.transformPoint(mScene.tableGeometry.transform.Inverted(), p1);
p2 = UtilOld.transformPoint(mScene.tableGeometry.transform.Inverted(), p2);
vertices_array.Add(p1.X);
vertices_array.Add(p1.Y);
vertices_array.Add(p1.Z);
mNumPoints++;
vertices_array.Add(p2.X);
vertices_array.Add(p2.Y);
vertices_array.Add(p2.Z);
mNumPoints++;
mGeometry = vertices_array.ToArray();
//mNumPrimitives = mNumPoints -1 ;
mNumPrimitives = mNumPoints / 2;
if (mPoints.Count % 2 == 0)
{
indices_array.Add(mNumPoints - 2);
indices_array.Add(mNumPoints - 1);
mGeometryIndices = indices_array.ToArray();
}
}
public override void draw(bool isTop)
{
//visualize the point on the plane
if (onPlane && isTop)
{
//ray casting to the pre-defind planes
OpenTK.Vector4 controller_p = UtilOld.getControllerTipPosition(ref mScene, primaryDeviceIndex == mScene.leftControllerIdx) * new OpenTK.Vector4(0, 0, 0, 1);
OpenTK.Vector4 controller_pZ = UtilOld.getControllerTipPosition(ref mScene, primaryDeviceIndex == mScene.leftControllerIdx) * new OpenTK.Vector4(0, 0, -1, 1);
Point3d controller_pRhino = UtilOld.openTkToRhinoPoint(UtilOld.vrToPlatformPoint(ref mScene, new OpenTK.Vector3(controller_p.X, controller_p.Y, controller_p.Z)));
Point3d controller_pZRhin = UtilOld.openTkToRhinoPoint(UtilOld.vrToPlatformPoint(ref mScene, new OpenTK.Vector3(controller_pZ.X, controller_pZ.Y, controller_pZ.Z)));
Rhino.Geometry.Vector3d direction = new Rhino.Geometry.Vector3d(controller_pZRhin.X - controller_pRhino.X, controller_pZRhin.Y - controller_pRhino.Y, controller_pZRhin.Z - controller_pRhino.Z);
Ray3d ray = new Ray3d(controller_pRhino, direction);
Rhino.DocObjects.ObjectEnumeratorSettings settings = new Rhino.DocObjects.ObjectEnumeratorSettings();
settings.ObjectTypeFilter = Rhino.DocObjects.ObjectType.Brep;
//settings.NameFilter = "plane";
float mimD = 1000000f;
hitPlane = false;
if (!lockPlane)
{
foreach (Rhino.DocObjects.RhinoObject rhObj in mScene.rhinoDoc.Objects.GetObjectList(settings))
{
//only drawing on planes for now rhObj.Attributes.Name.Contains("brepMesh") || rhObj.Attributes.Name.Contains("aprint") || rhObj.Attributes.Name.Contains("plane")
if (rhObj.Attributes.Name.Contains("plane"))
{
List <GeometryBase> geometries = new List <GeometryBase>();
geometries.Add(rhObj.Geometry);
//must be a brep or surface, not mesh
Point3d[] rayIntersections = Rhino.Geometry.Intersect.Intersection.RayShoot(ray, geometries, 1);
if (rayIntersections != null)
{
//get the nearest one
OpenTK.Vector3 tmpP = UtilOld.platformToVRPoint(ref mScene, new OpenTK.Vector3((float)rayIntersections[0].X, (float)rayIntersections[0].Y, (float)rayIntersections[0].Z));
float distance = (float)Math.Sqrt(Math.Pow(tmpP.X - controller_p.X, 2) + Math.Pow(tmpP.Y - controller_p.Y, 2) + Math.Pow(tmpP.Z - controller_p.Z, 2));
if (distance < mimD)
{
hitPlane = true;
targetPRhObj = rhObj;
mimD = distance;
projectP = UtilOld.platformToVRPoint(ref mScene, new OpenTK.Vector3((float)rayIntersections[0].X, (float)rayIntersections[0].Y, (float)rayIntersections[0].Z));
}
}
}
}
}
else
{
if (targetPRhObj != null)
{
List <GeometryBase> geometries = new List <GeometryBase>();
geometries.Add(targetPRhObj.Geometry);
//must be a brep or surface, not mesh
Point3d[] rayIntersections = Rhino.Geometry.Intersect.Intersection.RayShoot(ray, geometries, 1);
if (rayIntersections != null)
{
//get the nearest one
OpenTK.Vector3 tmpP = UtilOld.platformToVRPoint(ref mScene, new OpenTK.Vector3((float)rayIntersections[0].X, (float)rayIntersections[0].Y, (float)rayIntersections[0].Z));
float distance = (float)Math.Sqrt(Math.Pow(tmpP.X - controller_p.X, 2) + Math.Pow(tmpP.Y - controller_p.Y, 2) + Math.Pow(tmpP.Z - controller_p.Z, 2));
if (distance < mimD)
{
hitPlane = true;
mimD = distance;
projectP = UtilOld.platformToVRPoint(ref mScene, new OpenTK.Vector3((float)rayIntersections[0].X, (float)rayIntersections[0].Y, (float)rayIntersections[0].Z));
}
}
}
}
if (!hitPlane)
{
targetPRhObj = null;
projectP = new OpenTK.Vector3(100, 100, 100); //make it invisable
}
//visualize the projection points
// inverted rotation first
OpenTK.Matrix4 t = OpenTK.Matrix4.CreateTranslation(UtilOld.transformPoint(mScene.tableGeometry.transform.Inverted(), projectP));
t.Transpose();
drawPoint.transform = t;
}
if (currentState != State.PAINT || !isTop)
{
return;
}
// drawing curve
Vector3 pos = new Vector3();
if (onPlane)
{
pos = projectP;
if (hitPlane)
{
//GeometryStroke handle rotation
((Geometry.GeometryStroke)stroke_g).addPoint(pos);
rhihoPointList.Add(UtilOld.openTkToRhinoPoint(UtilOld.vrToPlatformPoint(ref mScene, pos)));
}
}
else
{
pos = Util.Math.GetTranslationVector3(UtilOld.getControllerTipPosition(ref mScene, primaryDeviceIndex == mScene.leftControllerIdx));
//GeometryStroke handle rotation already
((Geometry.GeometryStroke)stroke_g).addPoint(pos);
rhihoPointList.Add(UtilOld.openTkToRhinoPoint(UtilOld.vrToPlatformPoint(ref mScene, pos)));
}
if (((Geometry.GeometryStroke)stroke_g).mNumPrimitives == 1)
{
SceneNode stroke = new SceneNode("Stroke", ref stroke_g, ref stroke_m);
mScene.tableGeometry.add(ref stroke);
//mScene.staticGeometry.add(ref stroke);
strokeId = stroke.guid;
}
//testing the performance of rhino curve
if (rhihoPointList.Count == 2)
{
rcurve = Curve.CreateInterpolatedCurve(rhihoPointList.ToArray(), 3);
}
else if (rhihoPointList.Count > 2)
{
rcurve.Extend(Rhino.Geometry.CurveEnd.End, Rhino.Geometry.CurveExtensionStyle.Line, rhihoPointList[rhihoPointList.Count - 1]);
}
}
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;
}
