I'm using XNA to simulate a robot in a
3D world and then do image analysis on what the camera sees.
I have my camera looking down in front of the direction that the robot is going, and I have the robot detecting white pixels. I'm trying to take the white pixels that it finds and project them back into the
3D world so that I can see if it is actually detecting the correct pixels.
I almost have it working, but there is an offset between where the white is in in the World and were I put my orange triangles (which represent what the robot things is white).
/// <summary>
/// Takes a bool map of and makes vertex positions based on the map.
/// </summary>
/// <param name="c"> The bool map</param>
private void ProjectBoolMapOnGroundAnthony2(bool[,] c)
{
float triangleSize = 0.04f;
// Point of interest in World W cordinate system.
Vector3 pointOfInterest_W = Vector3.Zero;
// Point of interest in Robot Cordinate system R
Vector3 pointOfInterest_R = Vector3.Zero;
// alpha is the angle from the robot camera to where it is looking in the center.
//double alpha = Math.Atan(1.8f / 1);
/// Matrix representation of the view determined by the position, target, and updirection.
Matrix View = ((SimulationMain)Game).mainRobot.robotCameraView.View;
/// Matrix representation of the view determined by the angle of the field of view (Pi/4), aspectRatio, nearest plane visible (1), and farthest plane visible (1200)
Matrix Projection = ((SimulationMain)Game).mainRobot.robotCameraView.Projection;
/// Matrix representing how the real world cordinates differ from that of the rendering by the camera.
Matrix World = ((SimulationMain)Game).mainRobot.robotCameraView.World;
Plane groundPlan = new Plane(Vector3.UnitZ, 0.0f);
for (int x = 0; x < this.screenWidth; x++)
{
for (int y = 0; y < this.screenHeight; )
{
if (c[x, y] == true && this.count1D < 62000)
{
int j = 1;
Vector3 nearPlanePoint = Game.GraphicsDevice.Viewport.Unproject(new Vector3(x, y, 0), Projection, View, World);
Vector3 farPlanePoint = Game.GraphicsDevice.Viewport.Unproject(new Vector3(x, y, 1), Projection, View, World);
//Vector3 pointOfInterest_W = Vector3.in
Ray ray = new Ray(nearPlanePoint, farPlanePoint);
pointOfInterest_W = ray.Position + ray.Direction * (float) ray.Intersects(groundPlan);
this.vertexArray2[this.count1D + 0].Position.X = pointOfInterest_W.X - triangleSize;
this.vertexArray2[this.count1D + 0].Position.Y = pointOfInterest_W.Y - triangleSize * j;
this.vertexArray2[this.count1D + 0].Position.Z = pointOfInterest_W.Z;
this.vertexArray2[this.count1D + 0].Color = Color.DarkOrange;
// Put another vertex a the position but +1 in the X direction triangleSize
//this.vertexArray2[this.count1D + 1].Position.X = pointOnGroud.X + 3;
//this.vertexArray2[this.count1D + 1].Position.Y = pointOnGroud.Y + j;
this.vertexArray2[this.count1D + 1].Position.X = pointOfInterest_W.X;
this.vertexArray2[this.count1D + 1].Position.Y = pointOfInterest_W.Y + triangleSize * j;
this.vertexArray2[this.count1D + 1].Position.Z = pointOfInterest_W.Z;
this.vertexArray2[this.count1D + 1].Color = Color.Red;
// Put another vertex a the position but +1 in the X direction
//this.vertexArray2[this.count1D + 0].Position.X = pointOnGroud.X;
//this.vertexArray2[this.count1D + 0].Position.Y = pointOnGroud.Y + 3 + j;
this.vertexArray2[this.count1D + 2].Position.X = pointOfInterest_W.X + triangleSize;
this.vertexArray2[this.count1D + 2].Position.Y = pointOfInterest_W.Y - triangleSize * j;
this.vertexArray2[this.count1D + 2].Position.Z = pointOfInterest_W.Z;
this.vertexArray2[this.count1D + 2].Color = Color.Orange;
this.count1D += 3;
y += j;
}
else
{
y++;
}
}
}
}
The world is a grass texture with lines on it. The world plane is normal at (0,0,1).
Any ideas on why there is an offset?
Any Ideas?
Thanks for the help,
Anthony G.
