bool Sphere::Intersect( Ray const &ray, float *tHit, float *epsilon, DifferentialGeometry *geom ) const {
Transform tf = Tform();
Ray r = ray * Inverse( tf );
float t;
if( !Intersect( ray, &t ) )
return false;
// compute differential geometry
Vec4 p = ray.Point( t );
float x = p.X();
float y = p.Y();
float z = p.Z();
if( x == 0.0f && z == 0.0f ) {
// can't have both atan2 arguments be zero
z = kEpsilon * m_radius;
}
float theta = atan2( p.X(), p.Z() );
if( theta < 0.0f ) {
// remap theta to [0, 2pi] to match sphere's definition
theta += k2Pi;
}
float phi = Acos( Clamp( z / m_radius, -1.0f, 1.0f ) );
// parameterize sphere hit
float u = theta * kInv2Pi;
float v = phi * kInvPi;
float sTheta, cTheta;
float sPhi, cPhi;
SinCos( theta, &sTheta, &cTheta );
SinCos( phi, &sPhi, &cPhi );
Vec4 dpdu( k2Pi * z, 0.0f, -k2Pi * x, 0.0f );
Vec4 dpdv( kPi * y * sTheta, -kPi * m_radius * sPhi, kPi * y * cTheta, 0.0f );
Vec4 d2pdu2( -k2Pi * k2Pi * x, 0.0f, -k2Pi * k2Pi * z, 0.0f );
Vec4 d2pduv( k2Pi * kPi * y * cTheta, 0.0f, -k2Pi * kPi * y * sTheta, 0.0f );
Vec4 d2pdv2( -kPi * kPi * x, -kPi * kPi * y, -kPi * kPi * z, 0.0f );
// change in normal is computed using Weingarten equations
Scalar E = Dot( dpdu, dpdu );
Scalar F = Dot( dpdu, dpdv );
Scalar G = Dot( dpdv, dpdv );
Vec4 N = Normalize( Cross( dpdu, dpdv ) );
Scalar e = Dot( N, d2pdu2 );
Scalar f = Dot( N, d2pduv );
Scalar g = Dot( N, d2pdv2 );
Scalar h = 1.0f / ( E * G - F * F );
Vec4 dndu = ( f * F - e * G ) * h * dpdu + ( e * F - f * E ) * h * dpdv;
Vec4 dndv = ( g * F - f * G ) * h * dpdu + ( f * F - g * E ) * h * dpdv;
*tHit = t;
*epsilon = 5e-4f * t;
// return world space differential geometry
*geom = DifferentialGeometry( Handle(), p * tf, dpdu * tf, dpdv * tf, Normal( dndu ) * tf, Normal( dndv ) * tf, u, v );
return true;
}
float Quaternion::Angle() const
{
return 2 * Acos(w_);
}
// define adjacency rules
//
void MyGlWindow::define_adjacency_rules( std::vector< std::pair< std::pair<int,int>, std::pair<int,int> > > &rules )
{
rules.clear();
for (int i=0;i<SM_Size();i++) {
corres c_i = SM_Get(i);
EdgePlane line_i = _camera->fromWorldFrameToCameraFrame( c_i.line );
for (int j=0;j<SM_Size();j++) {
if ( i == j )
continue;
corres c_j = SM_Get(j);
EdgePlane line_j = _camera->fromWorldFrameToCameraFrame( c_j.line );
if ( line_i.angle( line_j ) > SPHERE_TESSELLATION_RESOLUTION )
continue;
if ( line_i.overlap( line_j ) < EPS && line_j.overlap( line_i ) < EPS )
continue;
Vec3d n1 = line_i._normal;
Vec3d n2 = line_j._normal;
Vec3d n = cross(n1,n2);
if ( len(n) < EPS )
continue;
n = norm(n);
double angle_i_j = Acos(dot(n1,n2));
if ( M_PI - angle_i_j < angle_i_j ) {
n2 = -n2;
angle_i_j = M_PI - angle_i_j;
}
if ( dot(n,cross(n1,n2)) < 0)
angle_i_j = - angle_i_j;
for (int ii = 0; ii < c_i.eps.size(); ii++) {
Vec3d n_i = c_i.eps[ii]._normal;
double best_score = M_PI;
int best_jj = -1;
for (int jj=0;jj < c_j.eps.size(); jj++) {
Vec3d n_j = c_j.eps[jj]._normal;
double angle_ii_jj = Acos(dot(n_i,n_j));
if ( M_PI - angle_ii_jj < angle_ii_jj ) {
n_j = -n_j;
angle_ii_jj = M_PI - angle_ii_jj;
}
if ( dot(n,cross(n_i,n_j)) < 0)
angle_ii_jj = - angle_ii_jj;
double score = angle_ii_jj*angle_i_j > 0 ? fabs(angle_ii_jj - angle_i_j) : M_PI;
if ( score < best_score ) {
best_score = score;
best_jj = jj;
}
}
if ( best_jj == -1 )
continue;
std::pair<int, int> p1;
p1.first = i;
p1.second = ii;
std::pair<int, int> p2;
p2.first = j;
p2.second = best_jj;
std::pair< std::pair<int,int>, std::pair<int,int> > element;
element.first = p1;
element.second = p2;
rules.push_back( element );
}
}
}
// mutual consistency check
std::vector< std::pair< std::pair<int,int>, std::pair<int,int> > > consistent_rules;
for (std::vector< std::pair< std::pair<int,int>, std::pair<int,int> > >::iterator iter = rules.begin(); iter != rules.end(); iter++) {
bool ok = false;
for (std::vector< std::pair< std::pair<int,int>, std::pair<int,int> > >::iterator iter2 = rules.begin(); iter2 != rules.end(); iter2++) {
if ( iter2->first.first == iter->second.first && iter2->first.second == iter->second.second &&
iter2->second.first == iter->first.first && iter2->second.second == iter->first.second ) {
ok = true;
break;
}
}
if ( ok ) {
std::pair< std::pair<int,int>, std::pair<int,int> > element = *iter;
std::pair< std::pair<int,int>, std::pair<int,int> > transpose;
transpose.first.first = element.second.first;
transpose.first.second = element.second.second;
transpose.second.first = element.first.first;
transpose.second.second = element.first.second;
consistent_rules.push_back( element );
LOG(LEVEL_INFO, "introducing pair (%d,%d) (%d,%d)", element.first.first, element.first.second, element.second.first, element.second.second );
}
}
rules.clear();
rules = consistent_rules;
LOG(LEVEL_INFO, "rules done");
}
/* chain the edge planes for each camera
*/
void Frame::chainEdges (int topK)
{
double angle_normal_threshold = toRadians( 2.0 );
double angle_normal_threshold2 = toRadians( 4.0 );
double separation_criteria = toRadians( 5.0 );
double max_color_distance = 0.25;
int img;
double score=0,score_alignment=0;
int i,j,k;
if ( topK == -1 ) {
for (img=0;img<_nImages;img++) {
_edgeplanes_chained[img] = _edgeplanes[img];
}
} else {
for (img=0;img<_nImages;img++) {
_edgeplanes_chained[img].clear();
edgePlaneVector edges = _edgeplanes[img];
std::vector< intVector > buckets;
// first split edges into buckets
for (i=0;i<_edgeplanes[img].size();i++) {
EdgePlane edge = _edgeplanes[img][i];
if ( buckets.empty() ) {
intVector vs;
vs.push_back( i );
buckets.push_back( vs );
continue;
}
// check each bucket
bool matched = false;
for (j=0;j<buckets.size();j++) {
bool accept = true;
for (k=0;k<buckets[j].size();k++) {
if ( _edgeplanes[img][buckets[j][k]].angle( edge ) > angle_normal_threshold || _edgeplanes[img][buckets[j][k]].colorDistance( edge ) > max_color_distance) {
accept = false;
break;
}
if ( _edgeplanes[img][buckets[j][k]].overlap( edge ) > EPS || edge.overlap( _edgeplanes[img][buckets[j][k]] ) > EPS ) {
accept = false;
break;
}
}
if ( accept ) {
buckets[j]. push_back( i );
matched = true;
break;
}
}
if ( matched )
continue;
// if did not find any bucket, create a new one
intVector vs;
vs.push_back( i );
buckets.push_back( vs );
}
// second, merge edges within each bucket
for (i=0;i<buckets.size();i++) {
bool done = buckets[i].size() < 2;
while ( !done ) {
done = true;
for (j=0;j<buckets[i].size();j++) {
if ( buckets[i][j] == -1 )
continue;
for (k=j+1;k<buckets[i].size();k++) {
if ( buckets[i][k] == -1 )
continue;
double d1 = Acos(dot( edges[buckets[i][j]]._a, edges[buckets[i][k]]._a ));
double d2 = Acos(dot( edges[buckets[i][j]]._a, edges[buckets[i][k]]._b ));
double d3 = Acos(dot( edges[buckets[i][j]]._b, edges[buckets[i][k]]._a ));
double d4 = Acos(dot( edges[buckets[i][j]]._b, edges[buckets[i][k]]._b ));
if ( d1 < separation_criteria ) {
edges[buckets[i][j]]._a = edges[buckets[i][k]]._b;
buckets[i][k] = -1;
done = false;
break;
}
if ( d2 < separation_criteria ) {
edges[buckets[i][j]]._a = edges[buckets[i][k]]._a;
buckets[i][k] = -1;
done = false;
break;
}
if ( d3 < separation_criteria ) {
edges[buckets[i][j]]._b = edges[buckets[i][k]]._b;
buckets[i][k] = -1;
done = false;
break;
}
if ( d4 < separation_criteria ) {
edges[buckets[i][j]]._b = edges[buckets[i][k]]._a;
buckets[i][k] = -1;
done = false;
break;
}
}
if ( !done ) {
edges[buckets[i][j]]._normal = norm(cross(edges[buckets[i][j]]._a, edges[buckets[i][j]]._b));
break;
}
}
}
}
// create a new edge for each bucket
for (i=0;i<buckets.size();i++) {
for (j=0;j<buckets[i].size();j++) {
if ( buckets[i][j] == -1 )
continue;
edges[buckets[i][j]]._chaineId = i;
if ( edges[buckets[i][j]].length() > 1E-7 )
_edgeplanes_chained[img].push_back( edges[buckets[i][j]] );
}
}
std::sort(_edgeplanes_chained[img].begin(),_edgeplanes_chained[img].end());
if ( topK > 0 && _edgeplanes_chained[img].size() > topK )
_edgeplanes_chained[img].resize( topK );
}
}
// reset edge planes IDs
n_edgeplanes_chained = 0;
for (i=0;i<_edgeplanes_chained.size();i++) {
for (j=0;j<_edgeplanes_chained[i].size();j++) {
_edgeplanes_chained[i][j]._uid = n_edgeplanes_chained;
n_edgeplanes_chained++;
}
}
}
float Quat::Angle() const
{
return Acos(w) * 2.f;
}
/* Update corners with connected edges (double max_dist is in radians)
* for each edge end point, look for close corners and attach them together if distance < max_dist
* min_angle: min angle (in radians) between two edges to validate a corner
* max_dist: max distance between a point and a line to make a connection (in radians)
*/
void Frame::update_corners_connected_edges( std::vector< EdgePlane > &edges, std::vector < intVector > &cells, std::vector < intVector > &cells_corners,
std::vector< corner > &corners, double max_dist, double min_angle, Model *spherets )
{
int i,j,k;
std::map < std::pair<int, int>, int > book; // book keeping of pairs of end points
std::map < std::pair<int, int>, int >::iterator iter; // iterator in the book
for (i=0;i<cells.size();i++) {
for (j=0;j<cells[i].size()/2;j++) {
int marker = cells[i][2*j+1]; // 0 if start point, 1 if end point
int edgeid = cells[i][2*j];
double x,y,z; // edge end point (x,y,z)
double x2,y2,z2; // other edge end point (x2,y2,z2)
if ( marker == 0 ) {
x = edges[edgeid]._a[0];
y = edges[edgeid]._a[1];
z = edges[edgeid]._a[2];
x2 = edges[edgeid]._b[0];
y2 = edges[edgeid]._b[1];
z2 = edges[edgeid]._b[2];
} else {
x = edges[edgeid]._b[0];
y = edges[edgeid]._b[1];
z = edges[edgeid]._b[2];
x2 = edges[edgeid]._a[0];
y2 = edges[edgeid]._a[1];
z2 = edges[edgeid]._a[2];
}
double min_dist = 1E10;
int closest_corner = -1;
// for each corner in the neighborhood
for (k=0;k<cells_corners[i].size();k++) {
int corner_id = cells_corners[i][k];
iter = book.find( std::pair<int,int>( edgeid, corner_id )); // don't compare if already processed
if ( iter != book.end() )
continue;
corner c = corners[corner_id];
Vec3d p = c.getpoint();
double d = Acos( dot( p, Vec3d(x,y,z) ) );
if ( d < min_dist ) {
min_dist = d;
closest_corner = corner_id;
}
}
// if we found a point within the range
// we add the edge to that corner
if ( closest_corner != -1 && min_dist < max_dist ) {
corners[closest_corner].insert_edge( x, y, z, x2, y2, z2, min_angle );
std::pair<int,int> p; // update the book
p.first = edgeid;
p.second = closest_corner;
std::pair< std::pair<int, int>, int> element;
element.first = p;
element.second = 0;
book.insert( element );
}
}
}
}
/* Create new corners where lines intersect (max_dist is in radians)
*/
void Frame::create_corners_2_lines( std::vector< EdgePlane > &edges, std::vector < intVector > &cells, std::vector< corner > &corners, double max_dist )
{
int i,j,k;
std::map < std::pair<int, int>, int > book; // book keeping of pairs of end points
std::map < std::pair<int, int>, int >::iterator iter; // iterator in the book
for (i=0;i<cells.size();i++) { // within each cell
for (j=0;j<cells[i].size() / 2;i++) { // for each end point
int marker = cells[i][2*j+1]; // 0 if start point, 1 if end point
int edgeid = cells[i][2*j];
double x,y,z; // end point (x,y,z)
if ( marker == 0 ) {
x = edges[edgeid]._a[0];
y = edges[edgeid]._a[1];
z = edges[edgeid]._a[2];
} else {
x = edges[edgeid]._b[0];
y = edges[edgeid]._b[1];
z = edges[edgeid]._b[2];
}
int closest_id = -1;
int closest_marker = -1;
double min_dist = 1.0;
for (k=0;k<cells[i].size()/2;k++) { // for each other end point in the neighborhood
if ( j == k ) // don't compare to itself
continue;
int marker2 = cells[i][2*k+1];
int edgeid2 = cells[i][2*k];
iter = book.find( std::pair<int,int>( edgeid, edgeid2 )); // don't compare if already processed
if ( iter != book.end() )
continue;
iter = book.find( std::pair<int,int>( edgeid2, edgeid ));
if ( iter != book.end() )
continue;
double x2,y2,z2; // end point (x2,y2)
if ( marker2 == 0 ) {
x2 = edges[edgeid2]._a[0];
y2 = edges[edgeid2]._a[1];
z2 = edges[edgeid2]._a[2];
} else {
x2 = edges[edgeid2]._b[0];
y2 = edges[edgeid2]._b[1];
z2 = edges[edgeid2]._b[2];
}
double dist = Acos( dot( Vec3d(x,y,z), Vec3d(x2,y2,z2) ) );
if ( dist < min_dist ) { // keep the closest end point
min_dist = dist;
closest_id = edgeid2;
closest_marker = marker2;
}
}
if ( closest_id != -1 && min_dist < max_dist ) {
double x2,y2,z2;
if ( closest_marker == 0 ) {
x2 = edges[closest_id]._a[0];
y2 = edges[closest_id]._a[1];
z2 = edges[closest_id]._a[2];
} else {
x2 = edges[closest_id]._b[0];
y2 = edges[closest_id]._b[1];
z2 = edges[closest_id]._b[2];
}
std::pair<int,int> p; // update the book
p.first = edgeid;
p.second = closest_id;
std::pair< std::pair<int, int>, int> element;
element.first = p;
element.second = 0;
book.insert( element );
// compute the two intersections (poles) of the two edgeplanes and test each of them
Vec3d pole_1, pole_2;
if ( ! edges[edgeid].poles( edges[closest_id], pole_1, pole_2 ) )
continue;
double l1 = Acos( dot( pole_1, Vec3d(x2,y2,z2) ) );
double l2 = Acos( dot( pole_1, Vec3d(x, y, z ) ) );
if ( l1 < max_dist && l2 < max_dist ) {
corner c( pole_1[0], pole_1[1], pole_1[2] ); // create a new corner
corners.push_back( c );
continue;
}
l1 = Acos( dot( pole_2, Vec3d(x2,y2,z2) ) );
l2 = Acos( dot( pole_2, Vec3d(x, y, z ) ) );
if ( l1 < max_dist && l2 < max_dist ) {
corner c( pole_2[0], pole_2[1], pole_2[2] ); // create a new corner
corners.push_back( c );
continue;
}
}
}
}
}
/* Compute the CCW angle between two lines in R3. */
M_Real
M_LineLineAngle3(M_Line3 L1, M_Line3 L2)
{
return Acos(M_VecDot3p(&L1.d, &L2.d));
}
/* Computes abs(targetHipAngle) for the hip, such that the swing ankle is virtually constrained
* to move only in the vertical direction, with the distance locked at the target step length */
float getTargetHipAngle(float thStance, float thSwing, float stepLen) {
float d = stepLen; // target step length
float l = PARAM_l; // robot leg length
float h = getAnkleJointRelHeight(thStance, thSwing);
return Acos( 1 - ( d * d + h * h ) / ( 2 * l * l ) );
}
// Angle (in radians) between two adjacent face normals
// (or 0, if < 2 faces):
// XXX - name problem: it is not actually the dihedral angle!
double dihedral_angle() const { return Acos(dot()); }
