public ImageSensorData Measure()
{
var tmpLocation = GetCameraLocation(_robot);
var result = new KinectData(_settings.VerticalResolution, _settings.HorisontalResolution);
var horisontalAngle = -_settings.HorisontalViewAngle / 2.0;
var verticalAngle = -_settings.VerticalViewAngle / 2.0;
for (int i = 0; i < _settings.VerticalResolution; i++)
{
Frame3D mediateDirection = SensorRotation.VerticalFrameRotation(tmpLocation, -verticalAngle);
horisontalAngle = -_settings.HorisontalViewAngle / 2.0;
for (int j = 0; j < _settings.HorisontalResolution; j++)
{
Frame3D direction = SensorRotation.HorisontalFrameRotation(mediateDirection, horisontalAngle);
Ray ray = new Ray(tmpLocation.ToPoint3D(), SensorRotation.GetFrontDirection(direction));
var dist = double.PositiveInfinity;
foreach (var body in _worldRoot.GetSubtreeChildrenFirst())
{
if (_settings.Exclude.All(a => a != body))
{
var inter = Intersector.Intersect(body, ray);
dist = Math.Min(dist, inter);
}
}
result.Depth[i, j] = dist;
//verticalAngle += _settings.VStep;
horisontalAngle += _settings.HStep;
}
verticalAngle += _settings.VStep;
}
return(new ImageSensorData(result.GetBytes()));
}
public void VerticalTest(Frame3D frame, Angle angle, Point3D rotated)
{
Point3D computed = SensorRotation.VerticalRotation(frame, angle);
Assert.AreEqual(rotated, computed);
Console.WriteLine("Expected: {0}. But was: {1}", rotated, computed);
}
