public VPoint(VShape shape, VPoint camera) { ID = shape.ID; healthy = true; if (shape._isSquare) tag = "Square"; if (shape._isTriangle) tag = "Triangle"; Point camXY = shape.FindCenter(); //Right so this is where things get ugly. Camera X and Y are divided by the total to get a percentage. //50% is subtracted to make the coordinant a percentage of total width relative to middle. //This is multiplied by the Camera constant (viewing angle) times the height of the double xToCam = ((double)camXY.X / 480 - 0.5) * (EveDef.CameraWidthOverZ * 1.00000 * (camera.z + EveDef.WristToEnd_Z - EveDef.WristToCam_Z)); double yToCam = ((double)camXY.Y / 640 - 0.5) * (EveDef.CameraWidthOverZ * 1.33333 * (camera.z + EveDef.WristToEnd_Z - EveDef.WristToCam_Z)); double rToCam = Geometry.XYtoR(xToCam, yToCam); double thetaToCam = Geometry.XYtoTheta(xToCam, yToCam); //these are the distance from the center of camera to the object in correctly oriented mm int dX = (int)Math.Round(Geometry.PolarToX(thetaToCam + camera.Eve.A, rToCam)); int dY = -(int)Math.Round(Geometry.PolarToY(thetaToCam + camera.Eve.A, rToCam)); //the real camera position x and y, plus the dX and dY to the object, minus the extra x and y to get to the end-effector from the camera x = camera.x + dX - (int)Math.Round(Geometry.PolarToX(camera.Eve.A + Math.PI / 2, EveDef.WristToCam_L)); y = camera.y + dY - (int)Math.Round(Geometry.PolarToY(camera.Eve.A + Math.PI / 2, EveDef.WristToCam_L)); y = (int)(1.11*y - 30); if(y > 250 && y < 500) { z = (int)-((y - 250) / 30); } else z = 0; TransformXYZtoEVE(); TransformXYZtoMARTY(); Eve.D = Math.PI / 2; //NEEDS MORE Marty.D = 0; }
public bool IsAnalogous(VShape other) { return (Distance(FindCenter(), other.FindCenter()) < 32 && _shape.Length == other._shape.Length); }