float calcDihed(vector<float> &a, vector<float> &b, vector<float> &c, vector<float> &d) {
//cout << "a " << a[0] <<" "<< a[1] <<" "<< a[2] << endl;
//cout << "b " << b[0] <<" "<< b[1] <<" "<< b[2] << endl;
//cout << "c " << c[0] <<" "<< c[1] <<" "<< c[2] << endl;
//cout << "d " << d[0] <<" "<< d[1] <<" "<< d[2] << endl;
vector<float> ba(3), bc(3), cb(3), cd(3);
point_sub(ba, a, b); point_sub(bc, c, b);
point_sub(cb, b, c); point_sub(cd, d, c);
//cout << "ba " << ba[0] <<" "<< ba[1] <<" "<< ba[2] << endl;
//cout << "bc " << bc[0] <<" "<< bc[1] <<" "<< bc[2] << endl;
//cout << "cb " << cb[0] <<" "<< cb[1] <<" "<< cb[2] << endl;
//cout << "cd " << cd[0] <<" "<< cd[1] <<" "<< cd[2] << endl;
vector<float> ba_bc(3), cb_cd(3);
cross_product(cb_cd, cb, cd); normalize_point(cb_cd, cb_cd);
cross_product(ba_bc, ba, bc); normalize_point(ba_bc, ba_bc);
//cout << "cb_cd " << cb_cd[0] <<" "<< cb_cd[1] <<" "<< cb_cd[2] << endl;
//cout << "ba_bc " << ba_bc[0] <<" "<< ba_bc[1] <<" "<< ba_bc[2] << endl;
float dp = dot_product(cb_cd, ba_bc);
if(dp > 1) dp = 1;
if(dp < -1) dp = -1;
float angle = RADIANS_TO_DEGREES ( acos(dp) );
//cout << angle <<" "<< dp << endl;
vector<float> cp(3); cross_product(cp, ba_bc,cb_cd);
if ( dot_product(cp,bc) < 0 ) angle = -1*angle;
return angle;
}
float calcAngle(vector<float> & A, vector<float> & B, vector<float> & C) {
vector<float> BA(3), BC(3);
linear_combination(BA, 1, A, -1, B);
linear_combination(BC, 1, C, -1, B);
normalize_point(BA, BA); normalize_point(BC, BC);
float dp = dot_product(BA,BC);
if(dp > 1) dp = 1;
else if(dp < -1) dp = -1;
return RADIANS_TO_DEGREES( acos(dp) );
}
// given a vector X, find normal vectors Y,Z such that X,Y,Z are mutually perpendicular
void findYZgivenX( vector<float> & givenX, vector<float> & Y, vector<float> & Z ) {
vector<float> X(3);
normalize_point(X, givenX);
int a=0, b=1, c=2;
if( fabs(X[a]) < 1e-2 ) { a=1; b=2; c=0; }
if( fabs(X[a]) < 1e-2 ) { a=2; b=0; c=1; }
assert ( fabs(X[a]) > 1e-2 );
Y[b] = -1. + (2.*ran01()); Y[c] = -1. + (2.*ran01());
//cout << "rands " << Y[b] << "\nrands " << Y[c] << endl;
Y[a] = -1 * (X[b]*Y[b] + X[c]*Y[c]) / X[a];
normalize_point(Y, Y);
cross_product(Z, X, Y);
normalize_point(Z, Z);
}
void find4thPoint(vector<float>& p4,
vector<float>& p1, vector<float>& p2, vector<float>& p3,
float dist, float ang, float dihed) {
vector<float> n1(3), n2(3), a(3), b(3);
point_sub(a, p1,p2); point_sub(b, p3,p2);
cross_product(n1, a,b); normalize_point(n1,n1);
cross_product(n2, b,n1); normalize_point(n2,n2);
double Sang = sin( DEGREES_TO_RADIANS(ang) );
double Cang = cos( DEGREES_TO_RADIANS(ang) );
double Sdihed = sin( DEGREES_TO_RADIANS(dihed) );
double Cdihed = cos( DEGREES_TO_RADIANS(dihed) );
normalize_point(b,b);
//cout << point_string(b) << point_string(n1) << point_string(n2) << endl;
linear_combination(p4, Sang*Cdihed, n2, -1*Cang, b);
linear_combination(p4, -1*Sang*Sdihed, n1, 1, p4);
//cout << point_string(p4) << endl;
linear_combination(p4, dist, p4, 1, p3);
}
virtual void on_idle()
{
int i;
for(i = 0; i < 6; i++)
{
move_point(m_poly1.xn(i), m_poly1.yn(i), m_dx1[i], m_dy1[i]);
move_point(m_poly2.xn(i), m_poly2.yn(i), m_dx2[i], m_dy2[i]);
normalize_point(i);
}
force_redraw();
}
// on intersection of spheres c1,r1 and c2,r2 find p such that p-c1-q is of certain value, and lies on intersection of 2 spheres
// this is solved by sampling points on the circle of intersection and checking the angle
int findSphereSphereAngleIntx(float r1, vector<float> & c1, float r2, vector<float> & c2, vector<float> & q, float desiredAngle,
vector<float>& p1, vector<float>& p2) {
vector<float> c1c2(3), Y(3), Z(3), c(3);
linear_combination(c1c2, 1, c2, -1, c1);
double cc = magnitude(c1c2);
cout << cc << endl;
if(cc > r1+r2) return 0;
normalize_point(c1c2, c1c2);
findYZgivenX(c1c2, Y, Z);
cout << "c1c2 " << c1c2[0] << " " << c1c2[1] << " " << c1c2[2] << endl;
cout << "Y " << Y[0] << " " << Y[1] << " " << Y[2] << endl;
cout << "Z " << Z[0] << " " << Z[1] << " " << Z[2] << endl;
double c1c = (r1*r1 + cc*cc - r2*r2) / (2*cc);
double r = sqrt(r1*r1 - c1c*c1c);
linear_combination(c, 1, c1, c1c, c1c2);
cout << "C " << c[0] << " " << c[1] << " " << c[2] << endl;
float lastAngle = -999, angle, lastTheta = -999;
float startTheta = 0, stopTheta = 360, step = 5; // clockwise
int nsol = 0;
vector<float> p(3);
for(float theta = startTheta; theta <= stopTheta; theta += step) {
double th = M_PI * theta / 180;
linear_combination(p, 1, c, r*sin(th), Y);
linear_combination(p, 1, p, r*cos(th), Z);
angle = calcAngle(q, c1, p);
cout << "P " << lastAngle << " " << desiredAngle << " " << angle << " " << p[0] << " " << p[1] << " " << p[2] << endl;
if(lastAngle != -999 && (desiredAngle-angle)*(desiredAngle-lastAngle) <= 0) {
double th = M_PI * (theta + lastTheta)/360.;
if(nsol==0) { linear_combination(p1, 1, c, r*sin(th), Y); linear_combination(p1, 1, p1, r*cos(th), Z); }
if(nsol==1) { linear_combination(p2, 1, c, r*sin(th), Y); linear_combination(p2, 1, p2, r*cos(th), Z); }
assert(nsol!=2);
nsol++;
cout << "NSOL " << nsol << endl;
}
lastAngle = angle;
lastTheta = theta;
}
return nsol;
}
// as mentioned in AxisRotation.pdf in the same directory
void findRotnOperator(vector<vector<float> > & op, vector<float> & axis, float angle) {
op.resize(3); op[0].resize(3); op[1].resize(3); op[2].resize(3);
vector<float> n(3);
normalize_point(n, axis);
angle = DEGREES_TO_RADIANS(angle);
float ct = cos(angle), st = sin(angle);
op[0][0] = n[0]*n[0] + ct * (n[1]*n[1] + n[2]*n[2]);
op[1][1] = n[1]*n[1] + ct * (n[2]*n[2] + n[0]*n[0]);
op[2][2] = n[2]*n[2] + ct * (n[0]*n[0] + n[1]*n[1]);
op[0][1] = n[0]*n[1]*(1-ct) - n[2]*st;
op[1][0] = n[0]*n[1]*(1-ct) + n[2]*st;
op[0][2] = n[0]*n[2]*(1-ct) + n[1]*st;
op[2][0] = n[0]*n[2]*(1-ct) - n[1]*st;
op[1][2] = n[1]*n[2]*(1-ct) - n[0]*st;
op[2][1] = n[1]*n[2]*(1-ct) + n[0]*st;
}
float calcAngle(vector<float> & A, vector<float> & B) {
vector<float> a = A, b = B;
normalize_point(a, A); normalize_point(b, B);
return RADIANS_TO_DEGREES(acos(dot_product(A,B)));
}
