vrutil.cpp

3D reconstruction, medical image processing from colons, using intel image processing for based clas

/*
 * $Id: util.c,v 1.5 1998/12/05 06:02:34 yjlee Exp $
 *
 * Copyright (c) 1998 by Lee yongjae, in dept. of Computer Science,
 * in Seoul National University, Korea.
 *
 * Permission to use, copy, modify, and distribute this software and its
 * documentation for any purpose and without fee is hereby granted, 
 * provided that the above copyright notice appear in all copies and
 * that both that copyright notice and this permission notice appear in
 * supporting documentation, and that the name of the copyright holder
 * not be used in advertising or publicity pertaining to distribution
 * of the software without specific, written prior permission.
 * 
 * This software is provided "as is" without express or implied warranty.
 *
 */

//#include <sys/time.h>
#include "stdafx.h"
#include <string.h>
#include <math.h>

#include "VRUtil.h"
#include "FastInlineFuncs.h"

/*
 * Time checking tool ---------------------------------------------------------
 */

/*
 * Vector operations ----------------------------------------------------------
 */

double v3nrm(double v[3])
{
  double l;

  l = sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);

  if(l!=0) {
	  v[0] /= l;
	  v[1] /= l;
	  v[2] /= l;
  }

  return l;
}

float v3nrmf(float v[3])
{
  float l;

  l = Sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
  if(l!=0) {
	  v[0] /= l;
	  v[1] /= l;
	  v[2] /= l;
  }
  return l;
}

/*
 * Vector/Matrix Operation
 */

void m4v4mul(double _r[4], double m[4][4], double v[4])
{
  double r[4];

  r[0] = m[0][0] * v[0] + m[0][1] * v[1] + m[0][2] * v[2] + m[0][3] * v[3];
  r[1] = m[1][0] * v[0] + m[1][1] * v[1] + m[1][2] * v[2] + m[1][3] * v[3];
  r[2] = m[2][0] * v[0] + m[2][1] * v[1] + m[2][2] * v[2] + m[2][3] * v[3];
  r[3] = m[3][0] * v[0] + m[3][1] * v[1] + m[3][2] * v[2] + m[3][3] * v[3];

  memcpy(_r, r, 32);
}

void m4v4mulf(float _r[4], float m[4][4], float v[4])
{
  float r[4];

  r[0] = m[0][0] * v[0] + m[0][1] * v[1] + m[0][2] * v[2] + m[0][3] * v[3];
  r[1] = m[1][0] * v[0] + m[1][1] * v[1] + m[1][2] * v[2] + m[1][3] * v[3];
  r[2] = m[2][0] * v[0] + m[2][1] * v[1] + m[2][2] * v[2] + m[2][3] * v[3];
  r[3] = m[3][0] * v[0] + m[3][1] * v[1] + m[3][2] * v[2] + m[3][3] * v[3];

  memcpy(_r, r, 16);
}

void m4v3mul(double _r[3], double m[4][4], double v[3])
{
  double r[4], d;

  r[0] = m[0][0] * v[0] + m[0][1] * v[1] + m[0][2] * v[2] + m[0][3];
  r[1] = m[1][0] * v[0] + m[1][1] * v[1] + m[1][2] * v[2] + m[1][3];
  r[2] = m[2][0] * v[0] + m[2][1] * v[1] + m[2][2] * v[2] + m[2][3];
  d = 1.0 / (m[3][0] * v[0] + m[3][1] * v[1] + m[3][2] * v[2] + m[3][3]);

  memcpy(_r, r, 24);

  _r[0] *= d;
  _r[1] *= d;
  _r[2] *= d;
}

void m4v3mulf(float _r[3], float m[4][4], float v[3])
{
  float r[4], d;

  r[0] = m[0][0] * v[0] + m[0][1] * v[1] + m[0][2] * v[2] + m[0][3];
  r[1] = m[1][0] * v[0] + m[1][1] * v[1] + m[1][2] * v[2] + m[1][3];
  r[2] = m[2][0] * v[0] + m[2][1] * v[1] + m[2][2] * v[2] + m[2][3];
  d = 1.0f / (m[3][0] * v[0] + m[3][1] * v[1] + m[3][2] * v[2] + m[3][3]);

  memcpy(_r, r, 12);

  _r[0] *= d;
  _r[1] *= d;
  _r[2] *= d;
}