void
LaserSensorModel2D::setInformationForVertexPoint(EdgeSE2PointXYCov*& edge, VertexPointXYCov*& point, LaserSensorParams& params)
{
double beamAngle = atan2(edge->measurement()(1), edge->measurement()(0));
///calculate incidence angle in point frame
Eigen::Vector2d beam = edge->measurement();
Eigen::Vector2d normal = point->normal();
beam = beam.normalized();
normal = normal.normalized();
double incidenceAngle = 0.0;
if(point->covariance() != Eigen::Matrix2d::Identity())
incidenceAngle = acos(normal.dot(beam));
double d = (tan(params.angularResolution * 0.5) * edge->measurement().norm());
///uncertainty of the surface measurement in direction of the beam
double dk = fabs(params.scale * (d / cos(incidenceAngle)));
edge->information().setIdentity();
edge->information()(0,0) = 1.0 / ((dk + params.sensorPrecision) * (dk + params.sensorPrecision));
double cError = 0.001 * edge->measurement().norm();
edge->information()(1,1) = 1.0 / (cError * cError);
Eigen::Rotation2Dd rot(beamAngle);
Eigen::Matrix2d mrot = rot.toRotationMatrix();
edge->information() = mrot * edge->information() * mrot.transpose();
}
Eigen::Vector2d snell_s (double n1, double n2, const Eigen::Vector2d &input_normal_vector, const Eigen::Vector2d &input_incident_vector)
{
Eigen::Vector2d normal_vector = input_normal_vector.normalized();
Eigen::Vector2d incident_vector = input_incident_vector.normalized();
if (n1 > n2) //check for critcal angle
{
double critical_angle = asin(n1/n2);
double angle_between_n_I = acos(normal_vector.dot(incident_vector)/(normal_vector.norm()*incident_vector.norm()));
if (angle_between_n_I>critical_angle)
{
throw 20;
}
}
double c = normal_vector.dot(incident_vector);
double r = n1/n2;
Eigen::Vector2d v_refract = (r*c - sqrt(1-r*r*(1-c*c)))*normal_vector - r*incident_vector;
return v_refract;
}
/**
Eigen::Vector2d internal_ray_lens_intersection( const Eigen::Vector2d &direction_of_internal, const Eigen::Vector2d &origin_of_internal, const Eigen::Vector2d ¢erpoint_of_sphere, double radius_of_sphere)
{
//https://people.cs.clemson.edu/~dhouse/courses/405/notes/raycast.pdf
Eigen::Vector2d return_intersection;
Eigen::Vector2d direction_of_internal_normalized = direction_of_internal.normalized();
Eigen::Vector2d o_minus_c = origin_of_internal-centerpoint_of_sphere;
double discriminant = (direction_of_internal_normalized.dot(o_minus_c))*(direction_of_internal_normalized.dot(o_minus_c))-o_minus_c.norm()*o_minus_c.norm()+radius_of_sphere*radius_of_sphere;
if (discriminant < 0)
{
throw 30;
}
else
{
double d_1 = -1*(direction_of_internal_normalized.dot(o_minus_c))+sqrt((direction_of_internal_normalized.dot(o_minus_c))*(direction_of_internal_normalized.dot(o_minus_c))-o_minus_c.norm()*o_minus_c.norm()+radius_of_sphere*radius_of_sphere);
double d_2 = -1*(direction_of_internal_normalized.dot(o_minus_c))-sqrt((direction_of_internal_normalized.dot(o_minus_c))*(direction_of_internal_normalized.dot(o_minus_c))-o_minus_c.norm()*o_minus_c.norm()+radius_of_sphere*radius_of_sphere);
if (d_2=d_1)
{
return_intersection = origin_of_internal+d_2*direction_of_internal_normalized;
}
else
{
Eigen::Vector2d return_intersection_1 = origin_of_internal+d_1*direction_of_internal_normalized;
Eigen::Vector2d return_intersection_2 = origin_of_internal+d_2*direction_of_internal_normalized;
if (return_intersection_1.y()<return_intersection_2.y())
{
return_intersection = return_intersection_1;
}
else
{
return_intersection = return_intersection_2;
}
}
cout << "lens choice 1: " << d_1 << endl;
cout << "lens choice 2: " << d_2 << endl;
}
return return_intersection;
}
**/
Eigen::Vector2d internal_ray_lens_intersection( const Eigen::Vector2d &direction_of_internal, const Eigen::Vector2d &origin_of_internal, const Eigen::Vector2d ¢erpoint_of_sphere, double radius_of_sphere)
{
// https://people.cs.clemson.edu/~dhouse/courses/405/notes/raycast.pdf
Eigen::Vector2d return_intersection;
Eigen::Vector2d direction_of_internal_normalized = direction_of_internal.normalized();
Eigen::Vector2d o_minus_c = origin_of_internal-centerpoint_of_sphere;
double t_close = direction_of_internal.dot(centerpoint_of_sphere-origin_of_internal);
Eigen::Vector2d x_close = origin_of_internal + t_close*direction_of_internal;
double discriminant = (x_close-centerpoint_of_sphere).norm();
if (discriminant < radius_of_sphere)
{
double a = sqrt(radius_of_sphere*radius_of_sphere-discriminant*discriminant);
return_intersection = origin_of_internal + (t_close-a)*direction_of_internal;
//cout << "Centerpoint of sphere : " << centerpoint_of_sphere << endl;
//cout << "radius of sphere : " << radius_of_sphere << endl;
}
else
{
throw 30;
}
return return_intersection;
}
