Ejemplo n.º 1
0
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;
}
Ejemplo n.º 2
0
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) );
}
Ejemplo n.º 3
0
// 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);
}
Ejemplo n.º 4
0
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);
}
Ejemplo n.º 5
0
 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();
 }
Ejemplo n.º 6
0
// 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;
}
Ejemplo n.º 7
0
// 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;
}
Ejemplo n.º 8
0
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)));
}