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);
}
public int FindShapes()
{
source = source.Resize(480, 640, Emgu.CV.CvEnum.INTER.CV_INTER_LINEAR);
Random rnd = new Random();
working = source.Convert<Gray, Byte>();
working._SmoothGaussian(3);
working = working.Canny(100, 60);
Bgr red = new Bgr(0, 0, 255);
Bgr green = new Bgr(0, 255, 0);
Bgr blue = new Bgr(255, 0, 0);
using (MemStorage storage = new MemStorage()) //allocate storage for contour approximation
for (Contour<DrawingPoint> contours = working.FindContours(); contours != null; contours = contours.HNext)
{
if (contours.Area > 1000)
{
VShape newShape = new VShape(contours.ApproxPoly(contours.Perimeter * 0.04, storage).ToArray(), new Bgr(rnd.Next(32, 255), rnd.Next(32, 255), rnd.Next(32, 255)));
if (newShape._isTriangle || newShape._isSquare)
{
bool IsAdded = false;
//MessageBox.Show("Found something");
for (int i = 0; i < shapes.Count; i++)
{
if (shapes[i].IsAnalogous(newShape))
{
newShape.ID = shapes[i].ID;
shapes[i] = newShape;
//shapes.Add(newShape);
IsAdded = true;
//break;
}
}
if (!IsAdded)
{
shapes.Add(newShape);
}
}
}
}
List<VShape> damnedShapes = new List<VShape>();
foreach (VShape shape in shapes)
{
if (shape.health == 0)
{
damnedShapes.Add(shape);
continue;
}
else
shape.health -= 1;
//post.Draw(new CircleF(shape.FindCenter(), shape.health * (float)shape.scale / 50), shape.ID, 0);
//////frame.Draw(" Inches away: " + Math.Round((1750/(shape.scale+7))-1.0,2).ToString() + ">" + shape.scale.ToString(), ref newFont, shape.FindCenter(), new Bgr(255, 64, 128));
//post.Draw(" Center X: " + shape.FindCenter().X.ToString(), ref newFont, new Point(shape.FindCenter().X, shape.FindCenter().Y - 8), new Bgr(255, 64, 128));
//post.Draw(" Center Y: " + shape.FindCenter().Y.ToString(), ref newFont, new Point(shape.FindCenter().X, shape.FindCenter().Y + 8), new Bgr(255, 64, 128));
post.FillConvexPoly(shape._shape, shape.ID);
//post.Draw(new CircleF(new PointF(20, 20), 10), new Bgr(128,128,128), 0);
}
foreach (VShape damnedShape in damnedShapes)
{
shapes.Remove(damnedShape);
}
return shapes.Count;
}