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); |