Source

kinect-viewer / utility.h

Full commit
/*
 * Copyright 2010 Cliff L. Biffle.  All Rights Reserved.
 * Use of this source code is governed by the Apache License 2.0,
 * which can be found in the LICENSE file.
 */

#ifndef __utility_h__
#define __utility_h__

#include "types.h"

static inline vec3f_t vector_subtract(vec3f_t a, vec3f_t b) {
	return (vec3f_t) {
		a.x - b.x,
		a.y - b.y,
		a.z - b.z
	};
}

static inline vec3f_t vector_scale(vec3f_t v, float scale) {
  return (vec3f_t) {
    v.x * scale,
    v.y * scale,
    v.z * scale,
  };
}

static inline vec3f_t cross_product(vec3f_t a, vec3f_t b) {
	return (vec3f_t) {
		a.y * b.z - a.z * b.y,
		a.z * b.x - a.x * b.z,
		a.x * b.y - a.y * b.x
	};		
}

static inline vec3f_t compute_normal(vec3f_t p1, vec3f_t p2, vec3f_t p3) {
	vec3f_t a = vector_subtract(p3, p2);
  vec3f_t b = vector_subtract(p1, p2);
	return cross_product(a, b);
}

// Converts depth image coordinates to color image coordinates
// using |xform|, given the linearized depth image reading |z|.
static inline void apply_projective_transform(const projective_transform_t *xform, float z, int *x, int *y) {
  static const float farScaleZ = 10.F;
  
  // Center the coordinates around the origin.
  vec2f_t centered = { *x - 320, *y - 240 };

  // Shift the origin.
  vec2f_t displaced = {
    centered.x + xform->offset.x,// / zRat,
    centered.y + xform->offset.y,// / zRat,
  };
  float shiftedZ = z + xform->offset.z;

  // Correct the field of view
  vec2f_t scale = {
    (shiftedZ / farScaleZ) * xform->scale.x,
    (shiftedZ / farScaleZ) * xform->scale.y,
  };
  vec2f_t scaled = {
    displaced.x * scale.x,
    displaced.y * scale.y,
  };
  
  *x = ((int) scaled.x) + 320;
  *y = ((int) scaled.y) + 240;
}

#endif __utility_h__