projection.c
Go to the documentation of this file.
00001 #include "arith.h"
00002 
00003 projection(x,n,motion_in,motion_out)
00004 REAL x[MAX][MAXRANK];
00005 int n;
00006 REAL motion_in[MAXRANK],motion_out[MAXRANK];
00007 {
00008    register int i,j,k;
00009    register unsigned int planetab,planework;
00010    int l[MAXRANK];
00011    REAL v_norm,distance,max_negative_distance,inner_product;
00012    REAL q[MAXRANK][MAXRANK],h[MAX][MAXRANK][MAXRANK];
00013 
00014    /* orthogonalize the normal vectors of the hyperplanes. */
00015 
00016    for(i=0; i<n; i++){
00017      v_norm = 0.0e0;
00018      for(j=0; j<MAXRANK; j++){
00019        v_norm += x[i][j] * x[i][j];
00020      }
00021      v_norm = sqrt(v_norm);
00022      for(j=0; j<MAXRANK; j++){
00023        h[i][j][0] = x[i][j] / v_norm;
00024      }
00025    }
00026 
00027    /* return motion_in if it is inside the cone */
00028 
00029    for(i=0; i<n; i++){
00030      distance = 0.0e0;
00031      for(j=0; j<MAXRANK; j++){
00032        distance += x[i][j] * motion_in[j];
00033      }
00034      if(distance < -EPS2) break;
00035    }
00036    if(distance > -EPS2){
00037      /* printf("in the cone\n"); */
00038      for(j=0; j<MAXRANK; j++) motion_out[j] = motion_in[j];
00039      return;
00040    }
00041 
00042 
00043    /* project the query vector onto the closest hyperplane */
00044    
00045    max_negative_distance = 0.0e0;
00046    for(i=0; i<n; i++){
00047      distance = 0.0e0;
00048      for(j=0; j<MAXRANK; j++){
00049        distance += h[i][j][0] * motion_in[j];
00050      }
00051      if(distance < max_negative_distance){
00052        max_negative_distance = distance;
00053        l[1] = i;
00054      }
00055    }
00056    for(j=0; j<MAXRANK; j++){
00057      q[1][j] = motion_in[j] - max_negative_distance * h[l[1]][j][0];
00058    }
00059 
00060    /* output the projection onto the hyperplane if it is relatively inside. */
00061 
00062    for(i=0; i<n; i++){
00063      distance = 0.0e0;
00064      for(j=0; j<MAXRANK; j++){
00065        distance += h[i][j][0] * q[1][j];
00066      }
00067      if(distance < -EPS2) break;
00068    }
00069    if(distance > -EPS2){
00070      /* printf("on the first hyperplane\n"); */
00071      for(j=0; j<MAXRANK; j++) motion_out[j]=q[1][j];
00072      return;
00073    }
00074 
00075    /* start the recursive routine. */
00076 
00077    for(k=1; k<=MAXRANK-2; k++){
00078      for(i=0; i<n; i++){
00079        inner_product = 0.0e0;
00080        for(j=0; j<MAXRANK; j++)
00081            inner_product += h[l[k]][j][k-1] * h[i][j][k-1];
00082        v_norm = 0.0e0;
00083        for(j=0; j<MAXRANK; j++){
00084          h[i][j][k] = h[i][j][k-1] - inner_product * h[l[k]][j][k-1];
00085          v_norm += h[i][j][k] * h[i][j][k];
00086        }
00087        if(v_norm > EPS){
00088          v_norm = sqrt(v_norm);
00089          for(j=0; j<MAXRANK; j++) h[i][j][k] = h[i][j][k] / v_norm;
00090        }
00091      }
00092 
00093      max_negative_distance = 0.0e0;
00094 
00095      for(i=0; i<n; i++){
00096        distance = 0.0e0;
00097        for(j=0; j<MAXRANK; j++){
00098          distance += h[i][j][k] * q[k][j];
00099        }
00100        if(distance < max_negative_distance){
00101          max_negative_distance = distance;
00102          l[k+1] = i;
00103        }
00104      }
00105      distance = 0.0e0; inner_product = 0.0e0;
00106      for(j=0; j<MAXRANK; j++){
00107        distance += h[l[k+1]][j][0] * q[k][j];
00108        inner_product += h[l[k+1]][j][0] * h[l[k+1]][j][k];
00109      }
00110      for(j=0; j<MAXRANK; j++){
00111        q[k+1][j] = q[k][j] - distance * h[l[k+1]][j][k] / inner_product;
00112      }
00113     
00114    /* output the projection onto the (d-k) face if it is relatively inside. */
00115 
00116      for(i=0; i<n; i++){
00117        distance = 0.0e0;
00118        for(j=0; j<MAXRANK; j++){
00119          distance += h[i][j][0] * q[k+1][j];
00120        }
00121        if(distance < -EPS2) break;
00122      }
00123      if(distance > -EPS2){
00124        /* printf("on the d-%d face\n",k+1); */
00125        for(j=0; j<MAXRANK; j++) motion_out[j]=q[k+1][j];
00126        return;
00127      }
00128    }
00129 
00130    /* printf("Error - The projection could not be found.\n"); */
00131 
00132    /* the input is in the dual polyhedral convex cone. */
00133    for(j=0; j<MAXRANK; j++) motion_out[j]=0.0e0;
00134 
00135    return;
00136 }


euslisp
Author(s): Toshihiro Matsui
autogenerated on Thu Sep 3 2015 10:36:20