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