public Vector2D Normalized()
{
Vector2D result = new Vector2D(0, 0);
if (Length() != 0)
{
result.X = X / Length();
result.Y = Y / Length();
}
return result;
}
/// <summary>
/// Angle in degrees between two directions. The angle is positive if the direction passed as second parameter points at the left of the first one
/// (considering the first one as a vector pointing forward). The angle is negative otherwise.
/// </summary>
/// <param name="dir1"></param>
/// <param name="dir2"></param>
/// <returns> Returns an angles between two directions. </returns>
static public double AngleBetweenDirections(Vector2D dir1, Vector2D dir2)
{
Vector2D dir1norm = dir1.Normalized();
Vector2D dir2norm = dir2.Normalized();
double dot = dir1norm.Dot(dir2norm);
double angle = Math.Acos(dot) * 180 / Math.PI;
// If the coss product vector points down, than dir2 poitn to the left relatively to dir1
if (dir1.Cross(dir2) > 0)
angle *= -1;
return angle;
}
internal void GlanceToAngle(Vector2D angleCoordinates)
{
backFromGlance = true;
GenerateGaze(GazeTarget.Angle, angleCoordinates, DefaultGazeSpeed / 2);
}
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;
}
private double GetAngleOfLineBetweenTwoPoints(Vector2D p1, Vector2D p2)
{
double xDiff = p2.X - p1.X;
double yDiff = p2.Y - p1.Y;
return Math.Atan2(xDiff, yDiff) * 180 / Math.PI;
}
public ScreenSetup(Vector3D bottomLeftCorner, Vector2D size, Vector2D resolution)
{
_bottomLeftCorner = bottomLeftCorner;
_size = size;
_resolution = resolution;
}
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;
}
internal void Glance(TargetInfo target)
{
if (!started || ((GazeState == GazeTarget.Person || GazeState == GazeTarget.Person2) && target.GazeTarget != GazeTarget.Person && target.GazeTarget != GazeTarget.Person2)) return;
currentGazeInterval = GlanceInterval;
currentRandomAmplitude = GlanceRandomAmplitude;
if (target.GazeTarget == GazeTarget.ScreenPoint) GlanceToScreen(target.Coordinates);
else if (target.GazeTarget == GazeTarget.Angle) GlanceToAngle(target.Coordinates);
else Glance(target.GazeTarget);
NotifyGazeTargetChanged(target.GazeTarget);
}
private void Glance(GazeTarget gazeTarget)
{
if (!started || !((GazeState == GazeTarget.Person || GazeState == GazeTarget.Person2) && gazeTarget != GazeTarget.Person && gazeTarget != GazeTarget.Person2)) return;
currentGazeInterval = GlanceInterval;
currentRandomAmplitude = GlanceRandomAmplitude;
if (gazeTarget == GazeTarget.ThroughMap) glanceThroughMapToDo = GlanceThroughMapCount - 1;
if (gazeTarget == GazeTarget.AcrossRoom)
{
currentRandomAmplitude = GazeRandomAmplitude;
glanceAcrossRoomToDo = GlanceAcrossRoomCount - 1;
}
GlanceToTarget(gazeTarget);
}
internal void Gaze(GazeTarget GazeTarget, bool dontPerform = false)
{
if (!started) return;
ignoreClick = true;
currentGazeInterval = GazeInterval;
currentRandomAmplitude = GazeRandomAmplitude;
SwitchGazeTarget(GazeTarget, dontPerform);
}
internal void Gaze(TargetInfo target, bool dontPerform = false)
{
if (!started) return;
currentGazeInterval = GazeInterval;
currentRandomAmplitude = GazeRandomAmplitude;
if (target.GazeTarget == GazeTarget.ScreenPoint) GazeToScreen(target.Coordinates, dontPerform);
else if (target.GazeTarget == GazeTarget.Angle) GazeToAngle(target.Coordinates, dontPerform);
else Gaze(target.GazeTarget, dontPerform);
}
internal void HeadTracking(int userId, Vector2D PersonLocation)
{
if (((userId == 0 && GazeState == GazeTarget.Person) || (userId == 1 && GazeState == GazeTarget.Person2)) /*&& gazeId == ""*/)
{
Console.WriteLine("headtracking info: " + userId + ", " + PersonLocation.X);
currentGazeInterval = GazeInterval;
currentRandomAmplitude = GazeRandomAmplitude;
GenerateGaze(GazeState, PersonLocation, DefaultGazeSpeed / 2);
}
else Console.WriteLine("headtracking ignored: " + gazeId);
}
public void GazeToAngle(Vector2D p, bool dontPerform = false)
{
currentGazeAnglePoint = p;
SwitchGazeTarget(GazeTarget.Angle, dontPerform);
}
public void GazeToScreen(Vector2D p, bool dontPerform = false)
{
currentScreenPoint = p;
SwitchGazeTarget(GazeTarget.ScreenPoint, dontPerform);
}
private void NotifyPointAngleChanged(Vector2D point)
{
if (ScreenPointChanged != null) ScreenPointChanged(point);
}
public TargetInfo(Vector2D coordinates, string targetName = "", GazeTarget targetType = EmoteCommonMessages.GazeTarget.ScreenPoint)
{
this.GazeTarget = targetType;
this.Coordinates = coordinates;
this.TargetName = targetName;
Linked = false;
}
public ScreenSetup(Vector2D size, Vector2D resolution) : this(new Vector3D(0, 0, 0), size, resolution) { }
internal void SetAngleTarget(string targetName, Vector2D coordinates)
{
lock (Targets)
{
Targets[targetName.ToLower()] = new TargetInfo(coordinates, targetName, GazeTarget.Angle);
}
NotifyTargetsChanged();
}
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) {}
void IMapEvents.Click(double x, double y)
{
LastTouchOnScreenCoords = new Vector2D(x, y);
NotifyClickPointChanged(LastTouchOnScreenCoords);
GazeManager.Click(LastTouchOnScreenCoords);
}
public Vector2D PointToScreenPoint(double x, double y)
{
Vector2D angles = new Vector2D(0, 0);
if (IsAtRobotRight(x,y))
{
angles = AnglesToPoint(x, y, _rightShoulderPosition);
}
else
{
angles = AnglesToPoint(x, y, _leftShoulderPosition);
}
return angles;
}
void IMapEvents.Pan(double x, double y, double prevX, double prevY)
{
NotifyClickPointChanged(LastTouchOnScreenCoords);
GazeManager.Click(LastTouchOnScreenCoords);
LastTouchOnScreenCoords = new Vector2D((x + prevX) / 2, (y + prevY) / 2);
}
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;
}
void EmoteCommonMessages.IPerceptionEvents.HeadTracking(int userID, double X, double Y, double Z, bool DetectedSkeleton)
{
userID -= 1;
if (DetectedSkeleton)
{
Z += ZTrackingCompensation / 100;
Y += YTrackingCompensation / 100;
PersonLocation[userID] = new Vector2D(-Math.Atan2(X, Z) * (180 / Math.PI), Math.Atan2(Y, Z) * (180 / Math.PI));
if (userID == 0)
{
SetAngleTarget("env", PersonLocation[userID]);
SetAngleTarget("environmentalist", PersonLocation[userID]);
}
else if (userID == 1)
{
SetAngleTarget("eco", PersonLocation[userID]);
SetAngleTarget("economist", PersonLocation[userID]);
}
}
if ((IsPersonVisible && !DetectedSkeleton) || (!IsPersonVisible && DetectedSkeleton)) NotifyPersonVisibleChanged(userID, DetectedSkeleton);
IsPersonVisible = DetectedSkeleton;
if (IsPersonVisible) NotifyPersonAngleChanged(userID, PersonLocation[userID]);
}
static public Vector2D Direction(Vector2D fromPoint, Vector2D toPoint) {
Vector2D result = new Vector2D(0, 0);
result.X = toPoint.X - fromPoint.X;
result.Y = toPoint.Y - fromPoint.Y;
return result.Normalized();
}
private void NotifyPersonAngleChanged(int userId, Vector2D angle)
{
if (PersonAngleChanged != null) PersonAngleChanged(userId, angle);
}
public double Dot(Vector2D secondVector)
{
return X * secondVector.X + Y * secondVector.Y;
}
private void NotifyClickPointChanged(Vector2D point)
{
if (ClickPointChanged != null) ClickPointChanged(point);
}
/// <summary>
/// Calculate the cross product of two vectors
/// if you don't remember ---> http://en.wikipedia.org/wiki/Cross_product#Cross_visualization
/// </summary>
/// <param name="v2"></param>
/// <returns> since the vectors are bidimensional, it returns only the third coordinate of the resulting tridimensional cross vector </returns>
public double Cross(Vector2D v2)
{
return X * v2.Y - Y * v2.X; ;
}
internal void GlanceToScreen(Vector2D ScreenPoint)
{
SetGazeTime(GlanceInterval);
backFromGlance = true;
GenerateGaze(GazeTarget.ScreenPoint, ScreenPoint, DefaultGazeSpeed / 2);
}
