public void SetViewCenter(HorizontalCoordinate viewCenterCoord)
{
ViewCenterVector = viewCenterCoord.ToVector();
HorizontalCoordinate vertAxisCoord =
new HorizontalCoordinate(viewCenterCoord.Azimuth + 90, 0);
vertAxis = vertAxisCoord.ToVector();
horzAxis = Vector3.Cross(ViewCenterVector, vertAxis);
}
public void GetAngleOffsets(HorizontalCoordinate objectCoord,
out double horzAngle, out double vertAngle)
{
Vector3 objectVector = objectCoord.ToVector();
Vector3 horzObjectCross = Vector3.Cross(objectVector, -horzAxis);
Vector3 vertObjectCross = Vector3.Cross(objectVector, vertAxis);
horzObjectCross = Vector3.Normalize(horzObjectCross);
vertObjectCross = Vector3.Normalize(vertObjectCross);
double x = Vector3.Dot(horzObjectCross, vertAxis);
double y = Vector3.Dot(horzObjectCross, ViewCenterVector);
horzAngle = -180 * Math.Atan2(y, x) / Math.PI;
x = Vector3.Dot(vertObjectCross, horzAxis);
y = Vector3.Dot(vertObjectCross, ViewCenterVector);
vertAngle = -180 * Math.Atan2(y, x) / Math.PI;
}
public static HorizontalCoordinate FromRotationMatrix(Matrix4x4 matrix)
{
// Transform (0, 0, -1) -- the vector extending from the lens
Vector3 zAxisTransformed = Vector3.Transform(-Vector3.UnitZ, matrix);
// Get the horizontal coordinates
HorizontalCoordinate horzCoord = FromVector(zAxisTransformed);
// Find the theoretical HorizontalCoordinate for the transformed +Y vector if the phone is upright
double yUprightAltitude = 0;
double yUprightAzimuth = 0;
if (horzCoord.Altitude > 0)
{
yUprightAltitude = 90 - horzCoord.Altitude;
yUprightAzimuth = 180 + horzCoord.Azimuth;
}
else
{
yUprightAltitude = 90 + horzCoord.Altitude;
yUprightAzimuth = horzCoord.Azimuth;
}
Vector3 yUprightVector = new HorizontalCoordinate(yUprightAzimuth, yUprightAltitude).ToVector();
// Find the real transformed +Y vector
Vector3 yAxisTransformed = Vector3.Transform(Vector3.UnitY, matrix);
// Get the angle between the upright +Y vector and the real transformed +Y vector
double dotProduct = Vector3.Dot(yUprightVector, yAxisTransformed);
Vector3 crossProduct = Vector3.Cross(yUprightVector, yAxisTransformed);
crossProduct = Vector3.Normalize(crossProduct);
// Sometimes dotProduct is slightly greater than 1, which
// raises an exception in the angleBetween calculation, so....
dotProduct = Math.Min(dotProduct, 1);
double angleBetween = 180 * Vector3.Dot(zAxisTransformed, crossProduct) * Math.Acos(dotProduct) / Math.PI;
horzCoord.Tilt = angleBetween;
return(horzCoord);
}