1 void cal_q ( double par_x, double par_y, double par_z, double par_r, double bpx, double bpy, double bpz, double spx, double spy, double spz ) {
 2     double x_1, x_2, y_1, y_2, z_1, z_2;
 3
 4     if ( bpx != spx ) {
 5         double k_xy = ( spy - bpy ) / ( spx - bpx );
 6         double b_xy = bpy - k_xy * bpx;
 7
 8         double k_zx = ( spz - bpz ) / ( spx - bpx );
 9         double b_zx = bpz - k_zx * bpx;
10
11         double A = 1 + k_xy * k_xy + k_zx * k_zx;
12         double B = 2 * k_xy * ( b_xy - par_y ) + 2 * k_zx * ( b_zx - par_z ) - 2 * par_x;
13         double C = par_x * par_x + ( b_xy - par_y ) * ( b_xy - par_y ) + ( b_zx - par_z ) * ( b_zx - par_z ) - par_r * par_r;
14
15         x_1 = ( -B + sqrt ( B * B - 4 * A * C ) ) / ( 2 * A );
16         y_1 = k_xy * x_1 + b_xy;
17         z_1 = k_zx * x_1 + b_zx;
18
19         x_2 = ( -B - sqrt ( B * B - 4 * A * C ) ) / ( 2 * A );
20         y_2 = k_xy * x_2 + b_xy;
21         z_2 = k_zx * x_2 + b_zx;
22
23         printf ( "x_1: %10.5f   y_1: %10.5f   z_1: %10.5f \n", x_1, y_1, z_1 );
24         printf ( "x_2: %10.5f   y_2: %10.5f   z_2: %10.5f \n", x_2, y_2, z_2 );
25
26         printf ( "bpx != spx \n" );
27     } else {
28         if ( bpy != spy ) {
29             double k_zy = ( spz - bpz ) / ( spy - bpy );
30             double b_zy = spz - k_zy * spy;
31
32             double A = 1 + k_zy * k_zy;
33             double B = 2 * k_zy * ( b_zy - par_z ) - 2 * par_y;
34             double C = par_y * par_y + ( b_zy - par_z ) * ( b_zy - par_z ) + ( bpx - par_x ) * ( bpx - par_x ) - par_r * par_r;
35
36             x_1 = bpx;
37             y_1 = ( -B + sqrt ( B * B - 4 * A * C ) ) / ( 2 * A );
38             z_1 = k_zy * y_1 + b_zy;
39
40             x_2 = bpx;
41             y_2 = ( -B - sqrt ( B * B - 4 * A * C ) ) / ( 2 * A );
42             z_2 = k_zy * y_2 + b_zy;
43
44             printf ( "x_1: %10.5f   y_1: %10.5f   z_1: %10.5f \n", x_1, y_1, z_1 );
45             printf ( "x_2: %10.5f   y_2: %10.5f   z_2: %10.5f \n", x_2, y_2, z_2 );
46
47             printf ( "bpx == spx && bpy != spy \n" );
48         } else {
49             if ( bpz != spz ) {
50                 x_1 = bpx;
51                 y_1 = bpy;
52                 z_1 = par_z + sqrt ( par_r * par_r - ( bpx - par_x ) * ( bpx - par_x ) - ( bpy - par_y ) * ( bpy - par_y ) );
53
54                 x_2 = bpx;
55                 y_2 = bpy;
56                 z_2 = par_z - sqrt ( par_r * par_r - ( bpx - par_x ) * ( bpx - par_x ) - ( bpy - par_y ) * ( bpy - par_y ) );
57
58                 printf ( "x_1: %10.5f   y_1: %10.5f   z_1: %10.5f \n", x_1, y_1, z_1 );
59                 printf ( "x_2: %10.5f   y_2: %10.5f   z_2: %10.5f \n", x_2, y_2, z_2 );
60
61                 printf ( "bpx == spx && bpy == spy \n" );
62             } else {
63                 printf ( "Warning: two points overlap !!!! \n" );64             }
65         }
66     }
67 }

 1 #include <cstdio>
 2 #include <cmath>
 3
 4 void cal_q ( double, double, double, double, double, double, double, double, double, double );
 5
 6 int main() {
 7     double par_x = 20;
 8     double par_y = 20;
 9     double par_z = 20;
10     double par_r = 15;
11
12     double bpx = 20;
13     double bpy = 30;
14     double bpz = 40;
15
16     double spx = 20;
17     double spy = 20;
18     double spz = 20;
19
20     printf ( "bpx: %10.5f   bpy: %10.5f   bpz: %10.5f \n", bpx, bpy, bpz );
21     printf ( "spx: %10.5f   spy: %10.5f   spz: %10.5f \n", spx, spy, spz );
22
23     cal_q ( par_x, par_y, par_z, par_r, bpx, bpy, bpz, spx, spy, spz );
24
25     return 0;
26 }