Subversion Repositories wimsdev

Rev

Rev 18432 | Rev 18455 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 18432 Rev 18447
Line 1883... Line 1883...
1883
{
1883
{
1884
  double r,cx,cy,a1,a2,a3,tmp,
1884
  double r,cx,cy,a1,a2,a3,tmp,
1885
    nx = -q[0]*q[2]*q[2]+(q[0]*q[0]+q[1]*q[1]+1)*q[2]-q[0]*q[3]*q[3]-q[0],
1885
    nx = -q[0]*q[2]*q[2]+(q[0]*q[0]+q[1]*q[1]+1)*q[2]-q[0]*q[3]*q[3]-q[0],
1886
    ny = -q[1]*q[2]*q[2]-q[1]*q[3]*q[3]+(q[0]*q[0]+q[1]*q[1]+1)*q[3]-q[1],
1886
    ny = -q[1]*q[2]*q[2]-q[1]*q[3]*q[3]+(q[0]*q[0]+q[1]*q[1]+1)*q[3]-q[1],
1887
    dy = -2*q[1]*q[2]+2*q[0]*q[3];
1887
    dy = -2*q[1]*q[2]+2*q[0]*q[3];
1888
  if (dy*dy*1e4 < nx*nx+ny*ny){
1888
  if (dy*dy*1e4 <= nx*nx+ny*ny){
1889
    res[5]=(q[0]+q[2])/2;
1889
    res[5]=(q[0]+q[2])/2;
1890
    res[6]=(q[1]+q[3])/2;
1890
    res[6]=(q[1]+q[3])/2;
1891
    return 0;}
1891
    return 0;}
1892
  cx = ny/dy; cy=-nx/dy;
1892
  cx = ny/dy; cy=-nx/dy;
1893
  a1 = atan2(q[1]-cy, q[0]-cx);
1893
  a1 = atan2(q[1]-cy, q[0]-cx);