private static readonly double TOLERANCE_ANGULAIRE_STANDARD = 5; // A fixer, radian ou degrès ?
/// <summary>
/// La surface de contrôle sert ?valider la position du robot : cf IsAtTheRightPlace
/// Attention : la surface doit avoir pour origine (0,0), elle est clonée puis translatée automatiquement.
/// </summary>
/// <param name="type"></param>
/// <param name="pt"></param>
/// <param name="surfaceControle"></param>
/// <param name="toleranceAngulaire"></param>
public LieuCle(TypeDeLieu type, PointOriente pt, ElementSurface surfaceControle, double toleranceAngulaire)
{
this.surfaceDeControle = surfaceControle.clone();
this.pointDeReference = pt;
this.toleranceAngulaire = System.Math.Abs(toleranceAngulaire);
this.typeDeLieu = type;
this.surfaceDeControle.translater(pt);
}
public LensRayTransferFunction.Parameters ConvertFrontSurfaceRayToParameters(Ray ray)
{
Vector3d canonicalNormal = -Vector3d.UnitZ;
double surfaceSinTheta = frontSurfaceSinTheta;
Sphere sphericalSurface = frontSphericalSurface;
ElementSurface surface = ElementSurfaces.Last();
return(ConvertSurfaceRayToParameters(ray, canonicalNormal,
surfaceSinTheta, sphericalSurface, surface));
}
public Ray ConvertParametersToFrontSurfaceRay(
LensRayTransferFunction.Parameters parameters)
{
Vector3d canonicalNormal = -Vector3d.UnitZ;
double surfaceSinTheta = frontSurfaceSinTheta;
Sphere sphericalSurface = frontSphericalSurface;
ElementSurface surface = ElementSurfaces.Last();
return(ConvertParametersToSurfaceRay(parameters,
canonicalNormal, surfaceSinTheta, sphericalSurface, surface));
}
/// <summary>
/// Creates and initializes a new instance of a complex lens.
/// </summary>
/// <param name="surfaces">List of element surfaces, ordered from
/// back to front.</param>
public ComplexLens(IList <ElementSurface> surfaces)
{
ElementSurfaces = surfaces;
ElementSurface frontSurface = surfaces.LastOrDefault((surface) => surface.Surface is Sphere);
frontSphericalSurface = (Sphere)frontSurface.Surface;
frontSurfaceSinTheta = frontSphericalSurface.GetCapElevationAngleSine(frontSurface.ApertureRadius);
frontSurfaceApertureRadius = frontSurface.ApertureRadius;
ElementSurface backSurface = surfaces.FirstOrDefault((surface) => surface.Surface is Sphere);
backSphericalSurface = (Sphere)backSurface.Surface;
backSurfaceSinTheta = backSphericalSurface.GetCapElevationAngleSine(backSurface.ApertureRadius);
backSurfaceApertureRadius = backSurface.ApertureRadius;
MediumRefractiveIndex = Materials.Fixed.AIR;
frontSurface.NextRefractiveIndex = MediumRefractiveIndex;
}
public Obstacle(ElementSurface surface, TypeObstacle type)
{
surfaceInterdite = surface;
typeObstacle = type;
}
public static LieuCle buildAndAddLieuCle(TypeDeLieu type, CouleurEquipe couleur, PointOriente pt, ElementSurface surfaceControle, double toleranceAngulaire)
{
LieuCle retour = new LieuCle(type, pt, surfaceControle, toleranceAngulaire);
addLieuCle(type, couleur, retour);
return(retour);
}
public LensRayTransferFunction.Parameters ConvertSurfaceRayToParameters(
Ray ray,
Vector3d canonicalNormal, double surfaceSinTheta,
Sphere sphericalSurface, ElementSurface surface)
{
//Console.WriteLine("ray->parameters");
// - convert origin
// *- transform to hemispherical coordinates
// - find out if it is on the surface
// *- scale with respect to the spherical cap
// *- normalize
Vector3d unitSpherePos = (ray.Origin - sphericalSurface.Center) * sphericalSurface.RadiusInv;
unitSpherePos.Z *= canonicalNormal.Z;
//Console.WriteLine("unit sphere position: {0}", unitSpherePos);
Vector2d originParametric = Sampler.SampleSphereWithUniformSpacingInverse(
unitSpherePos, surfaceSinTheta, 1);
// - convert direction
// *- transform from camera space to local frame
// *- compute normal at origin
//Console.WriteLine("ray origin: {0}", ray.Origin);
Vector3d normalLocal = surface.SurfaceNormalField.GetNormal(ray.Origin);
normalLocal.Normalize(); // TODO: check if it is unnecessary
//Console.WriteLine("local normal: {0}", normalLocal);
// *- create rotation quaternion from canonical normal to local
// normal
Vector3d direction = ray.Direction;
//Console.WriteLine("local direction: {0}", direction);
if (surface.Convex)
{
direction = -direction;
}
Vector3d rotationAxis = Vector3d.Cross(canonicalNormal, normalLocal);
//Console.WriteLine("rotation axis: {0}", rotationAxis);
if (rotationAxis.Length > 0)
{
double angle = Math.Acos(Vector3d.Dot(normalLocal, canonicalNormal));
//Console.WriteLine("angle: {0}", angle);
double positionPhi = originParametric.Y;
// first transformed to the frame of the local normal
//Console.WriteLine("position phi: {0}", positionPhi);
Matrix4d rotMatrix = Matrix4d.CreateFromAxisAngle(rotationAxis, -angle);
// then rotate the local direction around Z using the position phi
rotMatrix = rotMatrix * Matrix4d.CreateRotationZ(2 * Math.PI * -positionPhi);
direction = Vector3d.Transform(direction, rotMatrix);
}
//Console.WriteLine("abs. direction: {0}", direction);
// *- transform to hemispherical coordinates
// - find out if it is within the local hemisphere
double sinTheta = direction.Z / canonicalNormal.Z;
double dirTheta = Math.Asin(sinTheta);
double cosTheta = Math.Sqrt(1 - sinTheta * sinTheta);
double dirPhi = Math.Atan2(direction.Y, direction.X);
if (dirPhi < 0)
{
// map [-PI; PI] to [0; 2*PI]
dirPhi += 2 * Math.PI;
}
// *- normalize
Vector2d directionParametric = new Vector2d(
dirTheta / (0.5 * Math.PI),
dirPhi / (2 * Math.PI));
//Console.WriteLine("position parameters: {0}", new Vector2d(
// originParametric.X, originParametric.Y));
return new LensRayTransferFunction.Parameters(
originParametric.X, originParametric.Y,
directionParametric.X, directionParametric.Y);
}
/// <summary>
/// Convert a ray with origin at the back or front lens surface from
/// its parametric representation.
/// </summary>
/// <param name="position">Position on lens surface in parameteric
/// representation (normalized hemispherical coordinates).</param>
/// <param name="direction">Direction of the ray with respect to the
/// local frame in parameteric representation (normalized hemispherical
/// coordinates).
/// </param>
/// <param name="canonicalNormal">Normal of the lens surface
/// hemisphere (typically (0,0,1) for the back surface or (0,0,-1) for
/// the front surface).</param>
/// <param name="surfaceSinTheta">Sine of the surface spherical cap
/// theta angle.</param>
/// <param name="sphericalSurface">Lens surface represented as a
/// sphere.</param>
/// <param name="surface">Lens surface with its normal field.</param>
/// <returns>Ray corresponding to its parametric representation.
/// </returns>
public Ray ConvertParametersToSurfaceRay(
LensRayTransferFunction.Parameters parameters,
Vector3d canonicalNormal, double surfaceSinTheta,
Sphere sphericalSurface, ElementSurface surface)
{
//Console.WriteLine("parameters->ray");
//Console.WriteLine("position parameters: {0}", parameters.Position);
// uniform spacing sampling for LRTF sampling
Vector3d unitSpherePos = Sampler.SampleSphereWithUniformSpacing(
parameters.Position, surfaceSinTheta, 1);
//Console.WriteLine("unit sphere position: {0}", unitSpherePos);
unitSpherePos.Z *= canonicalNormal.Z;
Vector3d lensPos = sphericalSurface.Center + sphericalSurface.Radius * unitSpherePos;
//Console.WriteLine("ray origin: {0}", lensPos);
// - get normal N at P
Vector3d normalLocal = surface.SurfaceNormalField.GetNormal(lensPos);
// - compute direction D from spherical coordinates (wrt normal Z = (0,0,+/-1))
double theta = 0.5 * Math.PI * parameters.DirectionTheta;
double phi = 2 * Math.PI * parameters.DirectionPhi;
double cosTheta = Math.Cos(theta);
Vector3d directionZ = new Vector3d(
Math.Cos(phi) * cosTheta,
Math.Sin(phi) * cosTheta,
Math.Sin(theta) * canonicalNormal.Z);
// - rotate D from Z to N frame
// - using a (normalized) quaternion Q
// - N and Z should be assumed to be already normalized
// - more efficient method: Efficiently building a matrix to
// rotate one vector to another [moller1999]
normalLocal.Normalize(); // TODO: check if it is unnecessary
//Console.WriteLine("abs. direction: {0}", directionZ);
//Console.WriteLine("local normal: {0}", normalLocal);
Vector3d rotationAxis = Vector3d.Cross(canonicalNormal, normalLocal);
//Console.WriteLine("rotation axis: {0}", rotationAxis);
Vector3d rotatedDir = directionZ;
if (rotationAxis.Length > 0)
{
double angle = Math.Acos(Vector3d.Dot(canonicalNormal, normalLocal));
//Console.WriteLine("angle: {0}", angle);
// first the local direction must be rotated around using the position phi!
//Console.WriteLine("position phi: {0}", parameters.PositionPhi);
Matrix4d rotMatrix = Matrix4d.CreateRotationZ(2 * Math.PI * parameters.PositionPhi);
// only then can be transformed to the frame of the local normal
rotMatrix = rotMatrix * Matrix4d.CreateFromAxisAngle(rotationAxis, angle);
rotatedDir = Vector3d.Transform(directionZ, rotMatrix);
}
if (surface.Convex)
{
rotatedDir = -rotatedDir;
}
//Console.WriteLine("local direction: {0}", rotatedDir);
Ray result = new Ray(lensPos, rotatedDir);
return result;
}
/// <summary>
/// Creates a new instance of a complex lens using a definition of
/// elements.
/// </summary>
/// <remarks>
/// The first and last surfaces have to be spherical. TODO: this is
/// needed only for simpler sampling. In general planar surfaces or
/// stops could be sampled too.
/// </remarks>
/// <param name="surfaceDefs">List of definitions of spherical or
/// planar element surfaces or stops. Ordered from front to back.
/// Must not be empty or null.
/// </param>
/// <param name="mediumRefractiveIndex">Index of refraction of medium
/// outside the lens. It is assumed there is one medium on the scene
/// side, senzor side and inside the lens.</param>
/// <returns>The created complex lens instance.</returns>
public static ComplexLens Create(
IList<SphericalElementSurfaceDefinition> surfaceDefs,
double mediumRefractiveIndex,
double scale)
{
var surfaces = new List<ElementSurface>();
var surfaceDefsReverse = surfaceDefs.Reverse().ToList();
// scale the lens if needed
if (Math.Abs(scale - 1.0) > epsilon)
{
surfaceDefsReverse = surfaceDefsReverse.Select(surface => surface.Scale(scale)).ToList();
}
// thickness of the whole lens (from front to back apex)
// (without the distance to the senzor - backmost surface def.)
double lensThickness = surfaceDefsReverse.Skip(1).Sum(def => def.Thickness);
double elementBasePlaneShiftZ = lensThickness;
double lastCapHeight = 0;
double capHeight = 0;
// definition list is ordered from front to back, working list
// must be ordered from back to front, so a conversion has to be
// performed
int defIndex = 0;
foreach (var definition in surfaceDefsReverse)
{
if (defIndex > 0)
{
elementBasePlaneShiftZ -= definition.Thickness;
}
ElementSurface surface = new ElementSurface();
surface.ApertureRadius = 0.5 * definition.ApertureDiameter;
if (defIndex + 1 < surfaceDefsReverse.Count)
{
surface.NextRefractiveIndex = surfaceDefsReverse[defIndex + 1].NextRefractiveIndex;
}
else
{
surface.NextRefractiveIndex = mediumRefractiveIndex;
}
if (definition.CurvatureRadius.HasValue)
{
// spherical surface
double radius = definition.CurvatureRadius.Value;
// convexity reverses when converting from front-to-back
// back-to-front ordering
surface.Convex = radius < 0;
Sphere sphere = new Sphere()
{
Radius = Math.Abs(radius)
};
sphere.Center = Math.Sign(radius) *
sphere.GetCapCenter(surface.ApertureRadius, Vector3d.UnitZ);
capHeight = Math.Sign(radius) * sphere.GetCapHeight(sphere.Radius, surface.ApertureRadius);
elementBasePlaneShiftZ -= lastCapHeight - capHeight;
sphere.Center += new Vector3d(0, 0, elementBasePlaneShiftZ);
surface.Surface = sphere;
surface.SurfaceNormalField = sphere;
}
else
{
// planar surface
// both media are the same -> circular stop
// else -> planar element surface
surface.NextRefractiveIndex = definition.NextRefractiveIndex;
surface.Convex = true;
capHeight = 0;
elementBasePlaneShiftZ -= lastCapHeight - capHeight;
Circle circle = new Circle()
{
Radius = 0.5 * definition.ApertureDiameter,
Z = elementBasePlaneShiftZ,
};
surface.Surface = circle;
surface.SurfaceNormalField = circle;
}
lastCapHeight = capHeight;
surfaces.Add(surface);
defIndex++;
}
//DEBUG
//foreach (var surface in surfaces)
//{
// Console.WriteLine("{0}, {1}, {2}", surface.ApertureRadius,
// surface.Convex, surface.NextRefractiveIndex);
//}
ComplexLens lens = new ComplexLens(surfaces)
{
MediumRefractiveIndex = mediumRefractiveIndex
};
return lens;
}
/// <summary>
/// Creates a new instance of a complex lens using a definition of
/// elements.
/// </summary>
/// <remarks>
/// The first and last surfaces have to be spherical. TODO: this is
/// needed only for simpler sampling. In general planar surfaces or
/// stops could be sampled too.
/// </remarks>
/// <param name="surfaceDefs">List of definitions of spherical or
/// planar element surfaces or stops. Ordered from front to back.
/// Must not be empty or null.
/// </param>
/// <param name="mediumRefractiveIndex">Index of refraction of medium
/// outside the lens. It is assumed there is one medium on the scene
/// side, senzor side and inside the lens.</param>
/// <returns>The created complex lens instance.</returns>
public static ComplexLens Create(
IList <SphericalElementSurfaceDefinition> surfaceDefs,
double mediumRefractiveIndex,
double scale)
{
var surfaces = new List <ElementSurface>();
var surfaceDefsReverse = surfaceDefs.Reverse().ToList();
// scale the lens if needed
if (Math.Abs(scale - 1.0) > epsilon)
{
surfaceDefsReverse = surfaceDefsReverse.Select(surface => surface.Scale(scale)).ToList();
}
// thickness of the whole lens (from front to back apex)
// (without the distance to the senzor - backmost surface def.)
double lensThickness = surfaceDefsReverse.Skip(1).Sum(def => def.Thickness);
double elementBasePlaneShiftZ = lensThickness;
double lastCapHeight = 0;
double capHeight = 0;
// definition list is ordered from front to back, working list
// must be ordered from back to front, so a conversion has to be
// performed
int defIndex = 0;
foreach (var definition in surfaceDefsReverse)
{
if (defIndex > 0)
{
elementBasePlaneShiftZ -= definition.Thickness;
}
ElementSurface surface = new ElementSurface();
surface.ApertureRadius = 0.5 * definition.ApertureDiameter;
if (defIndex + 1 < surfaceDefsReverse.Count)
{
surface.NextRefractiveIndex = surfaceDefsReverse[defIndex + 1].NextRefractiveIndex;
}
else
{
surface.NextRefractiveIndex = mediumRefractiveIndex;
}
if (definition.CurvatureRadius.HasValue)
{
// spherical surface
double radius = definition.CurvatureRadius.Value;
// convexity reverses when converting from front-to-back
// back-to-front ordering
surface.Convex = radius < 0;
Sphere sphere = new Sphere()
{
Radius = Math.Abs(radius)
};
sphere.Center = Math.Sign(radius) *
sphere.GetCapCenter(surface.ApertureRadius, Vector3d.UnitZ);
capHeight = Math.Sign(radius) * sphere.GetCapHeight(sphere.Radius, surface.ApertureRadius);
elementBasePlaneShiftZ -= lastCapHeight - capHeight;
sphere.Center += new Vector3d(0, 0, elementBasePlaneShiftZ);
surface.Surface = sphere;
surface.SurfaceNormalField = sphere;
}
else
{
// planar surface
// both media are the same -> circular stop
// else -> planar element surface
surface.NextRefractiveIndex = definition.NextRefractiveIndex;
surface.Convex = true;
capHeight = 0;
elementBasePlaneShiftZ -= lastCapHeight - capHeight;
Circle circle = new Circle()
{
Radius = 0.5 * definition.ApertureDiameter,
Z = elementBasePlaneShiftZ,
};
surface.Surface = circle;
surface.SurfaceNormalField = circle;
}
lastCapHeight = capHeight;
surfaces.Add(surface);
defIndex++;
}
//DEBUG
//foreach (var surface in surfaces)
//{
// Console.WriteLine("{0}, {1}, {2}", surface.ApertureRadius,
// surface.Convex, surface.NextRefractiveIndex);
//}
ComplexLens lens = new ComplexLens(surfaces)
{
MediumRefractiveIndex = mediumRefractiveIndex
};
return(lens);
}
public LensRayTransferFunction.Parameters ConvertSurfaceRayToParameters(
Ray ray,
Vector3d canonicalNormal, double surfaceSinTheta,
Sphere sphericalSurface, ElementSurface surface)
{
//Console.WriteLine("ray->parameters");
// - convert origin
// *- transform to hemispherical coordinates
// - find out if it is on the surface
// *- scale with respect to the spherical cap
// *- normalize
Vector3d unitSpherePos = (ray.Origin - sphericalSurface.Center) * sphericalSurface.RadiusInv;
unitSpherePos.Z *= canonicalNormal.Z;
//Console.WriteLine("unit sphere position: {0}", unitSpherePos);
Vector2d originParametric = Sampler.SampleSphereWithUniformSpacingInverse(
unitSpherePos, surfaceSinTheta, 1);
// - convert direction
// *- transform from camera space to local frame
// *- compute normal at origin
//Console.WriteLine("ray origin: {0}", ray.Origin);
Vector3d normalLocal = surface.SurfaceNormalField.GetNormal(ray.Origin);
normalLocal.Normalize(); // TODO: check if it is unnecessary
//Console.WriteLine("local normal: {0}", normalLocal);
// *- create rotation quaternion from canonical normal to local
// normal
Vector3d direction = ray.Direction;
//Console.WriteLine("local direction: {0}", direction);
if (surface.Convex)
{
direction = -direction;
}
Vector3d rotationAxis = Vector3d.Cross(canonicalNormal, normalLocal);
//Console.WriteLine("rotation axis: {0}", rotationAxis);
if (rotationAxis.Length > 0)
{
double angle = Math.Acos(Vector3d.Dot(normalLocal, canonicalNormal));
//Console.WriteLine("angle: {0}", angle);
double positionPhi = originParametric.Y;
// first transformed to the frame of the local normal
//Console.WriteLine("position phi: {0}", positionPhi);
Matrix4d rotMatrix = Matrix4d.CreateFromAxisAngle(rotationAxis, -angle);
// then rotate the local direction around Z using the position phi
rotMatrix = rotMatrix * Matrix4d.CreateRotationZ(2 * Math.PI * -positionPhi);
direction = Vector3d.Transform(direction, rotMatrix);
}
//Console.WriteLine("abs. direction: {0}", direction);
// *- transform to hemispherical coordinates
// - find out if it is within the local hemisphere
double sinTheta = direction.Z / canonicalNormal.Z;
double dirTheta = Math.Asin(sinTheta);
double cosTheta = Math.Sqrt(1 - sinTheta * sinTheta);
double dirPhi = Math.Atan2(direction.Y, direction.X);
if (dirPhi < 0)
{
// map [-PI; PI] to [0; 2*PI]
dirPhi += 2 * Math.PI;
}
// *- normalize
Vector2d directionParametric = new Vector2d(
dirTheta / (0.5 * Math.PI),
dirPhi / (2 * Math.PI));
//Console.WriteLine("position parameters: {0}", new Vector2d(
// originParametric.X, originParametric.Y));
return(new LensRayTransferFunction.Parameters(
originParametric.X, originParametric.Y,
directionParametric.X, directionParametric.Y));
}
/// <summary>
/// Convert a ray with origin at the back or front lens surface from
/// its parametric representation.
/// </summary>
/// <param name="position">Position on lens surface in parameteric
/// representation (normalized hemispherical coordinates).</param>
/// <param name="direction">Direction of the ray with respect to the
/// local frame in parameteric representation (normalized hemispherical
/// coordinates).
/// </param>
/// <param name="canonicalNormal">Normal of the lens surface
/// hemisphere (typically (0,0,1) for the back surface or (0,0,-1) for
/// the front surface).</param>
/// <param name="surfaceSinTheta">Sine of the surface spherical cap
/// theta angle.</param>
/// <param name="sphericalSurface">Lens surface represented as a
/// sphere.</param>
/// <param name="surface">Lens surface with its normal field.</param>
/// <returns>Ray corresponding to its parametric representation.
/// </returns>
public Ray ConvertParametersToSurfaceRay(
LensRayTransferFunction.Parameters parameters,
Vector3d canonicalNormal, double surfaceSinTheta,
Sphere sphericalSurface, ElementSurface surface)
{
//Console.WriteLine("parameters->ray");
//Console.WriteLine("position parameters: {0}", parameters.Position);
// uniform spacing sampling for LRTF sampling
Vector3d unitSpherePos = Sampler.SampleSphereWithUniformSpacing(
parameters.Position, surfaceSinTheta, 1);
//Console.WriteLine("unit sphere position: {0}", unitSpherePos);
unitSpherePos.Z *= canonicalNormal.Z;
Vector3d lensPos = sphericalSurface.Center + sphericalSurface.Radius * unitSpherePos;
//Console.WriteLine("ray origin: {0}", lensPos);
// - get normal N at P
Vector3d normalLocal = surface.SurfaceNormalField.GetNormal(lensPos);
// - compute direction D from spherical coordinates (wrt normal Z = (0,0,+/-1))
double theta = 0.5 * Math.PI * parameters.DirectionTheta;
double phi = 2 * Math.PI * parameters.DirectionPhi;
double cosTheta = Math.Cos(theta);
Vector3d directionZ = new Vector3d(
Math.Cos(phi) * cosTheta,
Math.Sin(phi) * cosTheta,
Math.Sin(theta) * canonicalNormal.Z);
// - rotate D from Z to N frame
// - using a (normalized) quaternion Q
// - N and Z should be assumed to be already normalized
// - more efficient method: Efficiently building a matrix to
// rotate one vector to another [moller1999]
normalLocal.Normalize(); // TODO: check if it is unnecessary
//Console.WriteLine("abs. direction: {0}", directionZ);
//Console.WriteLine("local normal: {0}", normalLocal);
Vector3d rotationAxis = Vector3d.Cross(canonicalNormal, normalLocal);
//Console.WriteLine("rotation axis: {0}", rotationAxis);
Vector3d rotatedDir = directionZ;
if (rotationAxis.Length > 0)
{
double angle = Math.Acos(Vector3d.Dot(canonicalNormal, normalLocal));
//Console.WriteLine("angle: {0}", angle);
// first the local direction must be rotated around using the position phi!
//Console.WriteLine("position phi: {0}", parameters.PositionPhi);
Matrix4d rotMatrix = Matrix4d.CreateRotationZ(2 * Math.PI * parameters.PositionPhi);
// only then can be transformed to the frame of the local normal
rotMatrix = rotMatrix * Matrix4d.CreateFromAxisAngle(rotationAxis, angle);
rotatedDir = Vector3d.Transform(directionZ, rotMatrix);
}
if (surface.Convex)
{
rotatedDir = -rotatedDir;
}
//Console.WriteLine("local direction: {0}", rotatedDir);
Ray result = new Ray(lensPos, rotatedDir);
return(result);
}