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);