typedef struct { double x,y,z; } XYZ; /* Rotate a point p by angle theta around an arbitrary axis r Return the rotated point. Positive angles are anticlockwise looking down the axis towards the origin. Assume right hand coordinate system. */ XYZ ArbitraryRotate(XYZ p,double theta,XYZ r) { XYZ q = {0.0,0.0,0.0}; double costheta,sintheta; Normalise(&r); costheta = cos(theta); sintheta = sin(theta); q.x += (costheta + (1 - costheta) * r.x * r.x) * p.x; q.x += ((1 - costheta) * r.x * r.y - r.z * sintheta) * p.y; q.x += ((1 - costheta) * r.x * r.z + r.y * sintheta) * p.z; q.y += ((1 - costheta) * r.x * r.y + r.z * sintheta) * p.x; q.y += (costheta + (1 - costheta) * r.y * r.y) * p.y; q.y += ((1 - costheta) * r.y * r.z - r.x * sintheta) * p.z; q.z += ((1 - costheta) * r.x * r.z - r.y * sintheta) * p.x; q.z += ((1 - costheta) * r.y * r.z + r.x * sintheta) * p.y; q.z += (costheta + (1 - costheta) * r.z * r.z) * p.z; return(q); } /* Rotate a point p by angle theta around an arbitrary line segment p1-p2 Return the rotated point. Positive angles are anticlockwise looking down the axis towards the origin. Assume right hand coordinate system. */ XYZ ArbitraryRotate2(XYZ p,double theta,XYZ p1,XYZ p2) { XYZ q = {0.0,0.0,0.0}; double costheta,sintheta; XYZ r; r.x = p2.x - p1.x; r.y = p2.y - p1.y; r.z = p2.z - p1.z; p.x -= p1.x; p.y -= p1.y; p.z -= p1.z; Normalise(&r); costheta = cos(theta); sintheta = sin(theta); q.x += (costheta + (1 - costheta) * r.x * r.x) * p.x; q.x += ((1 - costheta) * r.x * r.y - r.z * sintheta) * p.y; q.x += ((1 - costheta) * r.x * r.z + r.y * sintheta) * p.z; q.y += ((1 - costheta) * r.x * r.y + r.z * sintheta) * p.x; q.y += (costheta + (1 - costheta) * r.y * r.y) * p.y; q.y += ((1 - costheta) * r.y * r.z - r.x * sintheta) * p.z; q.z += ((1 - costheta) * r.x * r.z - r.y * sintheta) * p.x; q.z += ((1 - costheta) * r.y * r.z + r.x * sintheta) * p.y; q.z += (costheta + (1 - costheta) * r.z * r.z) * p.z; q.x += p1.x; q.y += p1.y; q.z += p1.z; return(q); }