private Vector3D ScreenToPhysicalCoords(Vector2D screenCoords)
{
Vector3D screenPointPhysicalCoords = new Vector3D(0, 0, 0);
screenPointPhysicalCoords.X = screenCoords.X * _screenSetup._size.X / _screenSetup._resolution.X;
screenPointPhysicalCoords.Y = screenCoords.Y * _screenSetup._size.Y / _screenSetup._resolution.Y;
screenPointPhysicalCoords.X += _screenSetup._bottomLeftCorner.X;
screenPointPhysicalCoords.Y += _screenSetup._bottomLeftCorner.Y;
screenPointPhysicalCoords.Z += _screenSetup._bottomLeftCorner.Z;
return screenPointPhysicalCoords;
}
private Vector2D AnglesToPoint(double x, double y, Vector3D origin)
{
Vector2D angles = new Vector2D(0, 0);
Vector3D screenPointPhysicalCoords = ScreenToPhysicalCoords(new Vector2D(x, y));
Vector2D directionToPointHorizzontal = Vector2D.Direction(new Vector2D(origin.X, origin.Y), new Vector2D(screenPointPhysicalCoords.X, screenPointPhysicalCoords.Y));
Vector2D directionToPointVertical = Vector2D.Direction(new Vector2D(origin.Z, origin.Y), new Vector2D(screenPointPhysicalCoords.Z, screenPointPhysicalCoords.Y));
angles.X = Vector2D.AngleBetweenDirections(_forward, directionToPointHorizzontal);
angles.Y = Vector2D.AngleBetweenDirections(_forward, directionToPointVertical);
// HACK to avoid the robot looking upward when he's located on the upper side of the screen
if (angles.Y > 0) angles.Y *= -1;
return angles;
}
public PhysicalSpace(ScreenSetup screenSetup, Vector3D headPosition, Vector2D forward, string name="") : this(screenSetup, headPosition, new Vector3D(0,0,0), new Vector3D(0,0,0), forward, name) {}
public PhysicalSpace(ScreenSetup screenSetup, Vector3D headPosition, Vector3D rightShoulderPosition, Vector3D leftShoulderPosition, Vector2D forward, string name = "")
{
_name = name;
_screenSetup = screenSetup;
_headPosition = headPosition;
_forward = forward;
_leftShoulderPosition = leftShoulderPosition;
_rightShoulderPosition = rightShoulderPosition;
}
public ScreenSetup(Vector3D bottomLeftCorner, Vector2D size, Vector2D resolution)
{
_bottomLeftCorner = bottomLeftCorner;
_size = size;
_resolution = resolution;
}
