projection.c
Go to the documentation of this file.
1 #include "arith.h"
2 
3 projection(x,n,motion_in,motion_out)
4 REAL x[MAX][MAXRANK];
5 int n;
6 REAL motion_in[MAXRANK],motion_out[MAXRANK];
7 {
8  register int i,j,k;
9  register unsigned int planetab,planework;
10  int l[MAXRANK];
11  REAL v_norm,distance,max_negative_distance,inner_product;
13 
14  /* orthogonalize the normal vectors of the hyperplanes. */
15 
16  for(i=0; i<n; i++){
17  v_norm = 0.0e0;
18  for(j=0; j<MAXRANK; j++){
19  v_norm += x[i][j] * x[i][j];
20  }
21  v_norm = sqrt(v_norm);
22  for(j=0; j<MAXRANK; j++){
23  h[i][j][0] = x[i][j] / v_norm;
24  }
25  }
26 
27  /* return motion_in if it is inside the cone */
28 
29  for(i=0; i<n; i++){
30  distance = 0.0e0;
31  for(j=0; j<MAXRANK; j++){
32  distance += x[i][j] * motion_in[j];
33  }
34  if(distance < -EPS2) break;
35  }
36  if(distance > -EPS2){
37  /* printf("in the cone\n"); */
38  for(j=0; j<MAXRANK; j++) motion_out[j] = motion_in[j];
39  return;
40  }
41 
42 
43  /* project the query vector onto the closest hyperplane */
44 
45  max_negative_distance = 0.0e0;
46  for(i=0; i<n; i++){
47  distance = 0.0e0;
48  for(j=0; j<MAXRANK; j++){
49  distance += h[i][j][0] * motion_in[j];
50  }
51  if(distance < max_negative_distance){
52  max_negative_distance = distance;
53  l[1] = i;
54  }
55  }
56  for(j=0; j<MAXRANK; j++){
57  q[1][j] = motion_in[j] - max_negative_distance * h[l[1]][j][0];
58  }
59 
60  /* output the projection onto the hyperplane if it is relatively inside. */
61 
62  for(i=0; i<n; i++){
63  distance = 0.0e0;
64  for(j=0; j<MAXRANK; j++){
65  distance += h[i][j][0] * q[1][j];
66  }
67  if(distance < -EPS2) break;
68  }
69  if(distance > -EPS2){
70  /* printf("on the first hyperplane\n"); */
71  for(j=0; j<MAXRANK; j++) motion_out[j]=q[1][j];
72  return;
73  }
74 
75  /* start the recursive routine. */
76 
77  for(k=1; k<=MAXRANK-2; k++){
78  for(i=0; i<n; i++){
79  inner_product = 0.0e0;
80  for(j=0; j<MAXRANK; j++)
81  inner_product += h[l[k]][j][k-1] * h[i][j][k-1];
82  v_norm = 0.0e0;
83  for(j=0; j<MAXRANK; j++){
84  h[i][j][k] = h[i][j][k-1] - inner_product * h[l[k]][j][k-1];
85  v_norm += h[i][j][k] * h[i][j][k];
86  }
87  if(v_norm > EPS){
88  v_norm = sqrt(v_norm);
89  for(j=0; j<MAXRANK; j++) h[i][j][k] = h[i][j][k] / v_norm;
90  }
91  }
92 
93  max_negative_distance = 0.0e0;
94 
95  for(i=0; i<n; i++){
96  distance = 0.0e0;
97  for(j=0; j<MAXRANK; j++){
98  distance += h[i][j][k] * q[k][j];
99  }
100  if(distance < max_negative_distance){
101  max_negative_distance = distance;
102  l[k+1] = i;
103  }
104  }
105  distance = 0.0e0; inner_product = 0.0e0;
106  for(j=0; j<MAXRANK; j++){
107  distance += h[l[k+1]][j][0] * q[k][j];
108  inner_product += h[l[k+1]][j][0] * h[l[k+1]][j][k];
109  }
110  for(j=0; j<MAXRANK; j++){
111  q[k+1][j] = q[k][j] - distance * h[l[k+1]][j][k] / inner_product;
112  }
113 
114  /* output the projection onto the (d-k) face if it is relatively inside. */
115 
116  for(i=0; i<n; i++){
117  distance = 0.0e0;
118  for(j=0; j<MAXRANK; j++){
119  distance += h[i][j][0] * q[k+1][j];
120  }
121  if(distance < -EPS2) break;
122  }
123  if(distance > -EPS2){
124  /* printf("on the d-%d face\n",k+1); */
125  for(j=0; j<MAXRANK; j++) motion_out[j]=q[k+1][j];
126  return;
127  }
128  }
129 
130  /* printf("Error - The projection could not be found.\n"); */
131 
132  /* the input is in the dual polyhedral convex cone. */
133  for(j=0; j<MAXRANK; j++) motion_out[j]=0.0e0;
134 
135  return;
136 }
GLfloat n[6][3]
Definition: cube.c:15
double REAL
Definition: arith.h:25
#define MAX
Definition: arith.h:12
long l
Definition: structsize.c:3
double sqrt()
#define MAXRANK
Definition: arith.h:13
projection(x, int n, motion_in, motion_out)
Definition: projection.c:3
#define EPS2
Definition: /arith.h:19
#define EPS
Definition: arith.h:15


euslisp
Author(s): Toshihiro Matsui
autogenerated on Thu Jun 6 2019 20:00:44