Snippet of Kinect Math

public float angleOf(PVector one, PVector two, PVector axis) {
		PVector limb = PVector.sub(two, one);
		return degrees(PVector.angleBetween(limb, axis));
	}

	static public final float degrees(float radians) {
		return radians * RAD_TO_DEG;
	}

/**
* Calculate the angle between two vectors, using the dot product
* 
* @param v1
*            a vector
* @param v2
*            another vector
* @return the angle between the vectors
*/
static public float angleBetween(PVector v1, PVector v2) {
double dot = v1.x * v2.x + v1.y * v2.y + v1.z * v2.z;
double v1mag = Math.sqrt(v1.x * v1.x + v1.y * v1.y + v1.z * v1.z);
double v2mag = Math.sqrt(v2.x * v2.x + v2.y * v2.y + v2.z * v2.z);
return (float) Math.acos(dot / (v1mag * v2mag));
}

/**

	 * Subtract one vector from another
	 * 
	 * @param v1
	 *            a vector
	 * @param v2
	 *            another vector
	 * @return a new vector that is v1 - v2
	 */
	static public PVector sub(PVector v1, PVector v2) {
		return sub(v1, v2, null);
	}

	static public PVector sub(PVector v1, PVector v2, PVector target) {
		if (target == null) {
			target = new PVector(v1.x - v2.x, v1.y - v2.y, v1.z - v2.z);
		} else {
			target.set(v1.x - v2.x, v1.y - v2.y, v1.z - v2.z);
		}
		return target;
	}

/**

* initially started from "Making Things See" a excellent book and I recommend

* buying it http://shop.oreilly.com/product/0636920020684.do

// reduce our joint vectors to two dimensions

PVector rightHandXY = new PVector(skeleton.rightHand.x, skeleton.rightHand.y);

PVector rightElbowXY = new PVector(skeleton.rightElbow.x, skeleton.rightElbow.y);

PVector rightElbowYZ = new PVector(skeleton.rightElbow.y, skeleton.rightElbow.z);

PVector rightShoulderXY = new PVector(skeleton.rightShoulder.x, skeleton.rightShoulder.y);

PVector rightShoulderYZ = new PVector(skeleton.rightShoulder.y, skeleton.rightShoulder.z);

PVector rightHipXY = new PVector(skeleton.rightHip.x, skeleton.rightHip.y);

PVector leftHandXY = new PVector(skeleton.leftHand.x, skeleton.leftHand.y);

PVector leftElbowXY = new PVector(skeleton.leftElbow.x, skeleton.leftElbow.y);

PVector leftElbowYZ = new PVector(skeleton.leftElbow.y, skeleton.leftElbow.z);

PVector leftShoulderXY = new PVector(skeleton.leftShoulder.x, skeleton.leftShoulder.y);

PVector leftShoulderYZ = new PVector(skeleton.leftShoulder.y, skeleton.leftShoulder.z);

PVector leftHipXY = new PVector(skeleton.leftHip.x, skeleton.leftHip.y);

// calculate the axis against which we want to measure our angles

// dunno if this needs all the defintion it has :P - normal of the

// "person" is pretty much XY

PVector rightTorsoOrientationXY = PVector.sub(rightShoulderXY, rightHipXY);

PVector rightUpperArmOrientationXY = PVector.sub(rightElbowXY, rightShoulderXY);

PVector leftTorsoOrientationXY = PVector.sub(leftShoulderXY, leftHipXY);

PVector leftUpperArmOrientationXY = PVector.sub(leftElbowXY, leftShoulderXY);

// FIXME !! - IS THIS CORRECT - CAN XY JUST BE RE-USED - SINCE THE

// NORMAL OF THE BODY IS IN THE Z ?

PVector leftTorsoOrientationYZ = PVector.sub(leftShoulderXY, leftHipXY);

PVector rightTorsoOrientationYZ = PVector.sub(rightShoulderXY, rightHipXY);

// calculate the angles between our joints

float rightShoulderAngleXY = angleOf(rightElbowXY, rightShoulderXY, rightTorsoOrientationXY);

float rightShoulderAngleYZ = angleOf(rightElbowYZ, rightShoulderYZ, rightTorsoOrientationYZ);

float rightElbowAngleXY = angleOf(rightHandXY, rightElbowXY, rightUpperArmOrientationXY);

float leftShoulderAngleXY = angleOf(leftElbowXY, leftShoulderXY, leftTorsoOrientationXY);

float leftShoulderAngleYZ = angleOf(leftElbowYZ, leftShoulderYZ, leftTorsoOrientationYZ);

float leftElbowAngleXY = angleOf(leftHandXY, leftElbowXY, leftUpperArmOrientationXY);

skeleton.rightShoulder.setAngleXY(rightShoulderAngleXY);

skeleton.rightElbow.setAngleXY(rightElbowAngleXY);

skeleton.rightShoulder.setAngleYZ(rightShoulderAngleYZ);

skeleton.leftShoulder.setAngleXY(leftShoulderAngleXY);

skeleton.leftElbow.setAngleXY(leftElbowAngleXY);

skeleton.leftShoulder.setAngleYZ(leftShoulderAngleYZ);

g2d.drawString(String.format("shoulder %d %d", Math.round(leftShoulderAngleYZ), Math.round(rightShoulderAngleYZ)), 20, 30);

g2d.drawString(String.format("omoplate %d %d", Math.round(rightShoulderAngleXY), Math.round(leftShoulderAngleXY)), 20, 40);

g2d.drawString(String.format("bicep %d %d", Math.round(rightElbowAngleXY), Math.round(leftElbowAngleXY)), 20, 50);

invoke("publish", skeleton);