#include <stdio.h>
#include <math.h>

#define UNDEFINED_PT	-1.0


/*
**	This routine converts an ABW range image to 3D cartesian coordinates.
*/

ConvertABWRangeToCartesian(RangeImage,P,cal)

unsigned char	*RangeImage;
double		*P[3];
int		cal;	/* 1 or 2, specifying which calibration to use */

{
int		r,c;
double		OFFSET;
double		SCAL;
double		F;
double		C;

if (cal == 1)
  {
  OFFSET = 785.410786;
  SCAL = 0.773545;
  F = -1610.981722;
  C = 1.4508;
  }
else
  {
  OFFSET = 771.016866;
  SCAL = 0.791686;
  F = -1586.072821;
  C = 1.4508;
  }

for (r=0; r<512; r++)
  {
  for (c=0; c<512; c++)
    {
    if (RangeImage[r*512+c] == 0)
      {
      P[0][r*512+c]=P[1][r*512+c]=P[2][r*512+c]=UNDEFINED_PT;
      continue;
      }
    P[0][r*512+c]=(double)(c-255)*((double)RangeImage[r*512+c]/
	SCAL+OFFSET)/fabs(F);
    P[1][r*512+c]=(double)(-r+255)/C*((double)RangeImage[r*512+c]/
	SCAL+OFFSET)/fabs(F);
    P[2][r*512+c]=(double)(255-RangeImage[r*512+c])/SCAL;
    }
  }
}