bool ISeg::Match( ISeg* seg )
{
double tol = 0.01 * 0.01;
Puw* s1uw0 = m_IPnt[0]->GetPuw( m_SurfA );
Puw* s1uw1 = m_IPnt[1]->GetPuw( m_SurfA );
Puw* s2uw0 = seg->m_IPnt[0]->GetPuw( m_SurfA );
Puw* s2uw1 = seg->m_IPnt[1]->GetPuw( m_SurfA );
double d00 = dist_squared( s1uw0->m_UW, s2uw0->m_UW );
double d11 = dist_squared( s1uw1->m_UW, s2uw1->m_UW );
double d10 = dist_squared( s1uw1->m_UW, s2uw0->m_UW );
double d01 = dist_squared( s1uw0->m_UW, s2uw1->m_UW );
if ( d00 < tol && d11 < tol )
{
return true;
}
else if ( d10 < tol && d01 < tol )
{
return true;
}
return false;
}
void ISeg::JoinFront( ISeg* seg )
{
double df = dist_squared( m_IPnt[0]->m_Pnt, seg->m_IPnt[0]->m_Pnt );
double db = dist_squared( m_IPnt[0]->m_Pnt, seg->m_IPnt[1]->m_Pnt );
if ( df < db )
seg->FlipDir();
}
void ISeg::JoinBack( ISeg* seg )
{
double df = dist_squared( m_IPnt[1]->m_Pnt, seg->m_IPnt[0]->m_Pnt );
double db = dist_squared( m_IPnt[1]->m_Pnt, seg->m_IPnt[1]->m_Pnt );
if ( db < df )
seg->FlipDir();
}
double ISeg::MinDist( IPnt* ip )
{
Puw* uw = ip->GetPuw( m_SurfA );
Puw* uw0 = m_IPnt[0]->GetPuw( m_SurfA );
Puw* uw1 = m_IPnt[1]->GetPuw( m_SurfA );
double min_dist = dist_squared( uw->m_UW, uw0->m_UW );
min_dist = min( min_dist, dist_squared( uw->m_UW, uw1->m_UW) );
return min_dist;
}
double ISeg::MinDist( ISeg* seg )
{
Puw* s1uw0 = m_IPnt[0]->GetPuw( m_SurfA );
Puw* s1uw1 = m_IPnt[1]->GetPuw( m_SurfA );
Puw* s2uw0 = seg->m_IPnt[0]->GetPuw( m_SurfA );
Puw* s2uw1 = seg->m_IPnt[1]->GetPuw( m_SurfA );
double min_dist = dist_squared( s1uw0->m_UW, s2uw0->m_UW );
min_dist = min( min_dist, dist_squared( s1uw0->m_UW, s2uw1->m_UW ) );
min_dist = min( min_dist, dist_squared( s1uw1->m_UW, s2uw0->m_UW ) );
min_dist = min( min_dist, dist_squared( s1uw1->m_UW, s2uw1->m_UW ) );
return min_dist;
}
void Tri::ComputeCosAngles( Node* n0, Node* n1, Node* n2, double* ang0, double* ang1, double* ang2 )
{
double dsqr01 = dist_squared( n0->pnt, n1->pnt );
double dsqr12 = dist_squared( n1->pnt, n2->pnt );
double dsqr20 = dist_squared( n2->pnt, n0->pnt );
double d01 = sqrt( dsqr01 );
double d12 = sqrt( dsqr12 );
double d20 = sqrt( dsqr20 );
*ang0 = ( -dsqr12 + dsqr01 + dsqr20 ) / ( 2.0 * d01 * d20 );
*ang1 = ( -dsqr20 + dsqr01 + dsqr12 ) / ( 2.0 * d01 * d12 );
*ang2 = ( -dsqr01 + dsqr12 + dsqr20 ) / ( 2.0 * d12 * d20 );
}
void RulerLabel::calculateOffset()
{
if (vertex1.isSet() && vertex2.isSet())
{
vec2d vs = vertex1.pos2d();
vec2d ve = vertex2.pos2d();
//=== calculate shortest vector between mousept and line
double len2 = dist_squared(vs,ve);
if (len2 != 0)
{
double u = dot(cursor - vs, ve - vs) / len2;
vec2d vm = vs + (ve - vs) * u;
vec2d c1 = ve-vs;
vec2d c2 = cursor-vs;
if (c1.x()*c2.y() - c1.y()*c2.x() > 0) // cross product
rulerOffset = dist(vm, cursor) / viewScale;
else
rulerOffset = -dist(vm, cursor) / viewScale;
}
else
{
rulerOffset = 0;
}
}
}
double pointLineDistSquared( vec3d& X0, vec3d& X1, vec3d& X2, double* t )
{
vec3d X10 = X1 - X0;
vec3d X21 = X2 - X1;
double denom = dist_squared( X2, X1 );
if ( denom < 0.000000001 )
*t = 0.0;
else
*t = -dot(X10, X21)/dist_squared( X2, X1 );
vec3d Xon = X1 + X21 * (*t);
return dist_squared( Xon, X0 );
}
void addDisc(int num, float4 center, float4 u, float4 v, float radius, float spacing, std::vector<float4>& rvec)
{
printf("num: %d\n", num);
spacing *= 1.999f; //should probably just figure out whats up with my spacing
printf("spacing: %f\n", spacing);
float4 umin = -radius*u;
float4 vmin = -radius*v;
printf("u %f %f %f %f\n", u.x, u.y, u.z, u.w);
printf("v %f %f %f %f\n", v.x, v.y, v.z, v.w);
printf("umin %f %f %f %f\n", umin.x, umin.y, umin.z, umin.w);
printf("vmin %f %f %f %f\n", vmin.x, vmin.y, vmin.z, vmin.w);
int i = 0;
float d2 = 0.;
float r2 = radius*radius;
for(float du = 0.; du < 2.*radius; du += spacing) {
for(float dv = 0.; dv < 2.*radius; dv += spacing) {
if(i >= num) break;
float4 part = center + umin + u*du + vmin + v*dv;
part.w = 1.0f;
printf("part %f %f %f %f\n", part.x, part.y, part.z, part.w);
d2 = dist_squared(part-center);
printf("d2: %f, r2: %f\n", d2, r2);
if(d2 < r2)
{
rvec.push_back(part);
i++;
}
}
}
}
IPnt* IPntBin::Match( IPnt* ip, map< int, IPntBin > & binMap )
{
IPnt* close_ipnt = NULL;
if ( ip->m_Puws.size() != 2 )
return close_ipnt;
vector< IPnt* > compareIPntVec;
AddCompareIPnts( ip, compareIPntVec ); // Load IPnts From This Bin
for ( int b = 0 ; b < (int)m_AdjBins.size() ; b++ ) // Load From Adjancent Bins
{
int id = m_AdjBins[b];
binMap[id].AddCompareIPnts( ip, compareIPntVec );
}
//==== Find Closest IPnt ====//
double tol = 0.000001*0.000001;
double close_d = 1.0e12;
for ( int i = 0 ; i < (int)compareIPntVec.size() ; i++ )
{
if ( compareIPntVec[i]->m_Puws[0]->m_Surf == ip->m_Puws[0]->m_Surf &&
compareIPntVec[i]->m_Puws[1]->m_Surf == ip->m_Puws[1]->m_Surf )
{
double d = dist_squared( ip->m_Pnt, compareIPntVec[i]->m_Pnt );
if ( d < close_d && d < tol )
{
close_d = d;
close_ipnt = compareIPntVec[i];
}
}
}
return close_ipnt;
}
float OUTER::Wpoly6(float4 r, float h)
{
float h9 = h*h*h * h*h*h * h*h*h;
float alpha = 315.f/64.0f/params.PI/h9;
float r2 = dist_squared(r);
float hr2 = (h*h - r2);
float Wij = alpha * hr2*hr2*hr2;
return Wij;
}
// Test if ISegChain B matches this ISegChain.
bool ISegChain::Match( ISegChain* B )
{
double tol = 1e-8;
// Check that parent surfaces are the same.
if( m_SurfA->GetSurfID() != B->m_SurfA->GetSurfID() )
{
return false;
}
// Find 3d x,y,z coordinates of each chain's end points.
ISeg* frontSegA = m_ISegDeque.front();
frontSegA->m_IPnt[0]->CompPnt();
vec3d pA0 = frontSegA->m_IPnt[0]->m_Pnt;
ISeg* frontSegB = B->m_ISegDeque.front();
frontSegB->m_IPnt[0]->CompPnt();
vec3d pB0 = frontSegB->m_IPnt[0]->m_Pnt;
ISeg* backSegA = m_ISegDeque.back();
backSegA->m_IPnt[1]->CompPnt();
vec3d pA1 = backSegA->m_IPnt[1]->m_Pnt;
ISeg* backSegB = B->m_ISegDeque.back();
backSegB->m_IPnt[1]->CompPnt();
vec3d pB1 = backSegB->m_IPnt[1]->m_Pnt;
// Test for matching end points.
if( dist_squared( pA0, pB0 ) < tol && dist_squared( pA1, pB1 ) < tol )
{
return true;
}
// Check for flipped matching end points.
if( dist_squared( pA0, pB1 ) < tol && dist_squared( pA1, pB0 ) < tol )
{
this->FlipDir();
printf( "Flipping\n" );
return true;
}
// No match.
return false;
}
double pointSegDistSquared( vec3d& p, vec3d& sp0, vec3d& sp1, double* t )
{
double dSqr = pointLineDistSquared( p, sp0, sp1, t );
if ( *t < 0 )
{
*t = 0;
vec3d vec = p - sp0;
dSqr = dist_squared( p, sp0 );
}
else if ( *t > 1 )
{
*t = 1;
vec3d vec = p - sp1;
dSqr = dist_squared( p, sp1 );
}
return dSqr;
}
bool Surf::BorderCurveMatch( vector< vec3d > & curveA, vector< vec3d > & curveB )
{
double tol = 0.00000001;
if ( curveA.size() != curveB.size() )
return false;
bool match = true;
//==== Check Same Way ====//
for ( int i = 0 ; i < (int)curveA.size() ; i++ )
{
if ( dist_squared( curveA[i], curveB[i] ) > tol )
{
match = false;
break;
}
}
if ( match )
return true;
match = true;
int max_ind = (int)curveB.size() - 1;
//==== Check Opposite Way ====//
for ( int i = 0 ; i < (int)curveA.size() ; i++ )
{
if ( dist_squared( curveA[i], curveB[max_ind-i] ) > tol )
{
match = false;
break;
}
}
if ( match )
return true;
return false;
}
SkyObject *PlanetCatalog::findClosest(const SkyPoint *p, double &r) const {
QPtrListIterator<KSPlanetBase> it(planets);
SkyObject *found = 0;
double trialr = 0.0;
double rmin = 100000.0;
for (KSPlanetBase *ksp = it.toFirst(); ksp != 0; ksp = ++it) {
trialr = dist_squared(ksp, p);
if (trialr < rmin) {
rmin = trialr;
found = ksp;
}
}
r = rmin;
return found;
}
bool SurfCore::MatchThisOrientation( const piecewise_surface_type &osurf ) const
{
int ip, jp, nupatch, nvpatch, onupatch, onvpatch;
double tol = 1.0e-8;
nupatch = m_Surface.number_u_patches();
onupatch = osurf.number_u_patches();
if ( nupatch != onupatch )
{
return false;
}
nvpatch = m_Surface.number_v_patches();
onvpatch = osurf.number_v_patches();
if ( nvpatch != onvpatch )
{
return false;
}
for ( ip = 0; ip < nupatch; ip++ )
{
const surface_patch_type *patch = m_Surface.get_patch( ip, 0 );
const surface_patch_type *opatch = osurf.get_patch( ip, 0 );
if ( patch->degree_u() != opatch->degree_u() )
{
return false;
}
}
for ( jp = 0; jp < nvpatch; jp++ )
{
const surface_patch_type *patch = m_Surface.get_patch( 0, jp );
const surface_patch_type *opatch = osurf.get_patch( 0, jp );
if ( patch->degree_v() != opatch->degree_v() )
{
return false;
}
}
for( ip = 0; ip < nupatch; ++ip )
{
for( jp = 0; jp < nvpatch; ++jp )
{
surface_patch_type::index_type icp, jcp;
const surface_patch_type *patch = m_Surface.get_patch( ip, jp );
const surface_patch_type *opatch = osurf.get_patch( 0, jp );
for( icp = 0; icp <= patch->degree_u(); ++icp )
{
for( jcp = 0; jcp <= patch->degree_v(); ++jcp )
{
vec3d cp, ocp;
cp = patch->get_control_point( icp, jcp );
ocp = opatch->get_control_point( icp, jcp );
if ( dist_squared( cp, ocp ) > tol )
{
return false;
}
}
}
}
}
return true;
}
void SCurve::Tesselate( vector< vec3d > & target_pnts )
{
assert( m_Surf );
m_UTess.clear();
m_UWTess.clear();
vector< double > u_vec;
vector< vec3d > pnt_vec;
//==== Build U to Pnt Table ====//
int num_segs = 10000;
vec3d uw = m_UWCrv.comp_pnt( 0 );
vec3d last_p = m_Surf->CompPnt( uw.x(), uw.y() );
for ( int i = 0 ; i < num_segs ; i++ )
{
double u = (double)i/(double)(num_segs-1);
uw = m_UWCrv.comp_pnt( u );
vec3d p = m_Surf->CompPnt( uw.x(), uw.y() );
pnt_vec.push_back( p );
u_vec.push_back( u );
last_p = p;
}
//===== Look For Closest U For Each Target Point ====//
for ( int i = 0 ; i < (int)target_pnts.size() ; i++ )
{
int close_ind = 0;
double close_d2 = 1.0e12;
for ( int j = 1 ; j < (int)pnt_vec.size()-1 ; j++ )
{
double d2 = dist_squared( target_pnts[i], pnt_vec[j] );
if ( d2 < close_d2 )
{
close_d2 = d2;
close_ind = j;
}
}
//==== Make Sure Ind is Valid ====//
double f0, f1;
double d0 = pointSegDistSquared( target_pnts[i], pnt_vec[close_ind], pnt_vec[close_ind-1], &f0 );
double d1 = pointSegDistSquared( target_pnts[i], pnt_vec[close_ind], pnt_vec[close_ind+1], &f1 );
double u;
if ( d0 < d1 )
u = u_vec[close_ind] + (u_vec[close_ind-1] - u_vec[close_ind])*f0;
else
u = u_vec[close_ind] + (u_vec[close_ind+1] - u_vec[close_ind])*f1;
m_UTess.push_back( u );
m_UWTess.push_back( m_UWCrv.comp_pnt( u ) );
//vec3d uw = m_UWCrv.comp_pnt( u );
//double d = dist( target_pnts[i], m_Surf->CompPnt( uw[0], uw[1] ) );
//if ( d > 0.01 )
// printf( "SCurve Tess Target %f %f \n", f0, f1 );
}
// if ( m_UTess.size() == 1 )
// {
//int junk = 23;
//
// }
//==== Reset Begin and End Points ====//
m_UTess.front() = 0.0;
m_UWTess.front() = m_UWCrv.comp_pnt( 0.0 );
m_UTess.back() = 1.0;
m_UWTess.back() = m_UWCrv.comp_pnt( 1.0 );
}
double CellGrid::dist(loc l1, loc l2) {
return sqrt( dist_squared(l1, l2));
}
vec2d Surf::ClosestUW( vec3d & pnt, double guess_u, double guess_w, double guess_del_u, double guess_del_w, double tol )
{
double u = guess_u;
double w = guess_w;
double del_u = guess_del_u;
double del_w = guess_del_w;
double dist = dist_squared( pnt, CompPnt(u, w) );
if ( dist < tol )
return vec2d( u, w );
int iter = 20;
while ( iter > 0 )
{
double u_plus = u + del_u;
double u_minus = u - del_u;
if ( u_plus > m_MaxU )
{
u_plus = m_MaxU;
del_u *= 0.25;
}
if ( u_minus < 0 )
{
u_minus = 0.0;
del_u *= 0.25;
}
double dist_plus_u = dist_squared( pnt, CompPnt(u_plus, w) );
double dist_minus_u = dist_squared( pnt, CompPnt(u_minus, w) );
if ( dist_plus_u < dist )
{
u = u_plus;
del_u *= 2.0;
dist = dist_plus_u;
}
else if ( dist_minus_u < dist )
{
u = u_minus;
del_u *= 2.0;
dist = dist_minus_u;
}
else
{
del_u *= 0.5;
iter--;
}
double w_plus = w + del_w;
double w_minus = w - del_w;
if ( w_plus > m_MaxW )
{
w_plus = m_MaxW;
del_w *= 0.25;
}
if ( w_minus < 0 )
{
w_minus = 0;
del_w *= 0.25;
}
double dist_plus_w = dist_squared( pnt, CompPnt(u, w_plus ) );
double dist_minus_w = dist_squared( pnt, CompPnt(u, w_minus ) );
if ( dist_plus_w < dist )
{
w = w_plus;
del_w *= 2.0;
dist = dist_plus_w;
}
else if ( dist_minus_w < dist )
{
w = w_minus;
del_w *= 2.0;
dist = dist_minus_w;
}
else
{
del_w *= 0.5;
iter--;
}
}
return vec2d( u, w );
}
int findStableMatches( CvSeq *seq, float minRad, float maxRad, CandidatePtrVector& kps, IplImage* bin ) {
// Return value
int retVal = -1;
// Threshold Contour entries size
int elements = seq->total;
if( elements < 8 ) {
return retVal;
}
// Gather statistics
CvRect rect = cvBoundingRect( seq );
int high = ( rect.height < rect.width ? rect.width : rect.height );
int low = ( rect.height < rect.width ? rect.height : rect.width );
// If bounding box is very small simply return
if( low < minRad*2 ) {
return retVal;
}
// Allocate Contour array
CvPoint *group_pos = (CvPoint*) malloc(elements * sizeof(CvPoint));
cvCvtSeqToArray(seq, group_pos, CV_WHOLE_SEQ);
// Calculate arc and downsampling statistics
double arc_length = cvArcLength( seq );
double arc_approx = arc_length / 10;
double rect_approx = 12*(float)high / (float)low;
double downsample = 2 * elements / (rect_approx + arc_approx);
double ds_length = arc_length / 4;
// Perform downsampling
int maxSize = downsample * elements;
int newSize = 0;
CvPoint *dsed = (CvPoint*) malloc(maxSize * sizeof(CvPoint));
dsed[0] = CvPoint( group_pos[0] );
CvPoint last = CvPoint( dsed[0] );
newSize++;
for( int i = 1; i < elements; i++ ) {
double dist_so_far = dist_squared( group_pos[i], last );
if( dist_so_far > ds_length && newSize < maxSize ) {
dsed[newSize] = CvPoint( group_pos[i] );
newSize++;
last = CvPoint( group_pos[i] );
}
}
// Check to make sure reduced Contour size is sufficient [quickfix: todo revise above]
if( newSize < 6 ) {
free(group_pos);
free(dsed);
return -1;
}
// Fit Ellipse
CvPoint2D32f* input = (CvPoint2D32f*)malloc(newSize*sizeof(CvPoint2D32f));
for( int i=0; i<newSize; i++ ) {
input[i].x = dsed[i].x;
input[i].y = dsed[i].y;
}
CvBox2D* box = (CvBox2D*)malloc(sizeof(CvBox2D));
cvFitEllipse( input, newSize, box );
// Threshold size
float esize = PI*box->size.height*box->size.width/4.0f;
if( esize < PI*maxRad*maxRad ) {
// Add
Candidate *kp = new Candidate;
kp->angle = box->angle;
kp->r = box->center.y;
kp->c = box->center.x;
kp->minor = box->size.width/2;
kp->major = box->size.height/2;
kp->magnitude = 0;
kp->method = ADAPTIVE;
kps.push_back( kp );
retVal = 0;
} else {
// Interest point too large
retVal = 1;
}
// Deallocations
free(box);
free(input);
free(group_pos);
free(dsed);
return retVal;
}
