ORF2012

ORFで使った「服の型紙を自動でリサイズしてくれるプログラム」のコードをとりあえず公開
ライブラリはSimpleOpenNIとControlP5を使用

あまりにも日本語のリソースが少ないので、
そのうちSimpleOpenNIのドキュメント的な何かを書けたら書く予定

import SimpleOpenNI.*;
SimpleOpenNI context;
import controlP5.*;
ControlP5 controlP5;
import processing.pdf.*;
boolean saveOneFrame = false;

RadioButton r;

int          mode=0;

float        zoomF =0.5f;
float        rotX = radians(180);
float        rotY = radians(0);
boolean      autoCalib=true;
PVector      bodyCenter = new PVector();//体の中央
PVector      bodyDir = new PVector();//体の向き
PVector realWorldPoint;
color[]      userColors = {
color(255, 0, 0), color(0, 255, 0), color(0, 0, 255), color(255, 255, 0), color(255, 0, 255), color(0, 255, 255)
};
int topIndex;
int waistIndex;
int waistRightIndex;
int waistLeftIndex;
int[] userMap = null;

float mitake;
float yuki;
float kosimawari;
float sodetake;
float sodehaba;
float sodeguti;
float sodemarumi;
float kitake;
float sodetuke;
float miyatuguti;
float kurikosi;
float okumisagari;
float erisita;
float kensaki;
float katahaba;
float atohaba;
float maehaba;
float okumihaba;
float aiduma;
float erikataaki;
float katamawari;
float erituke;
float Height=150;
float Waist=70;
float Arm=60;
float Height2=150;
float Waist2=70;
float Arm2=60;

PFrame f;
secondApplet s;

void setup() {
size(1024, 768, P3D);
PFrame f = new PFrame();
context = new SimpleOpenNI(this);
context.setMirror(false);
//キネクト未接続時の例外処理
if (context.enableDepth() == false)
{
println("Can't open the depthMap, maybe the camera is not connected!");
exit();
return;
}
context.enableUser(SimpleOpenNI.SKEL_PROFILE_ALL);
stroke(255, 255, 255);
smooth();
}

void draw() {
if (mode==0) {
//println("mode:Kinect");
mitake=Height;
kosimawari=Waist;
yuki=Arm;
}
if (mode==1) {
//println("mode:Slider");
mitake=Height2;
kosimawari=Waist2;
yuki=Arm2;
}
kitake=mitake*0.83*2;
sodetake=55*2;
sodehaba=(yuki/2+1)*2;
sodeguti=23*2;
sodemarumi=2*2;
sodetuke=23*2;
miyatuguti=15*2;
kurikosi=2*2;
okumisagari=22*2;
erisita=88*2;
kensaki=7.5*2;

//pushMatrix();
perspective(radians(45), float(width)/float(height), 10, 150000);
context.update();
background(0, 0, 0);

// set the scene pos
translate(width/2, height/2, 0);
rotateX(rotX);
rotateY(rotY);
scale(zoomF);

int[]   depthMap = context.depthMap();
int     steps   = 3;  // to speed up the drawing, draw every third point
int     index;

translate(0, 0, -1000);  // set the rotation center of the scene 1000 infront of the camera

int userCount = context.getNumberOfUsers();
if (userCount > 0)
{
userMap = context.getUsersPixels(SimpleOpenNI.USERS_ALL);
}

//デプスマップ表示
stroke(100);
for (int y=0;y < context.depthHeight();y+=steps)
{
for (int x=0;x < context.depthWidth();x+=steps)
{
index = x + y * context.depthWidth();
if (depthMap[index] > 0)
{
// 点の描画
realWorldPoint = context.depthMapRealWorld()[index];
// check if there is a user
if (userMap != null && userMap[index] != 0)
{  // calc the user color
int colorIndex = userMap[index] % userColors.length;
stroke(userColors[colorIndex]);
}
else
// default color
stroke(100);
point(realWorldPoint.x, realWorldPoint.y, realWorldPoint.z);
}
}
}

//スケルトン表示
int[] userList = context.getUsers();
for (int i=0;i<userList.length;i++)
{
if (context.isTrackingSkeleton(userList[i]))
drawSkeleton(userList[i]);
}

// draw the kinect cam
//context.drawCamFrustum();
}
PVector cameraPos = new PVector(0, 0, 0);
PVector cameraRot = new PVector(0, 0, 0);
PVector cameraTargetPos = new PVector(0, 0, 0);
float ftmp = 1.0f;
float cameraTargetR = 1000.0f;
void setCamera() {
cameraPos.x = cameraTargetPos.x + ftmp*sin(cameraRot.y)* cameraTargetR * cos(cameraRot.x);
cameraPos.y = ftmp*sin(cameraRot.x) * cameraTargetR;
cameraPos.z = cameraTargetPos.z + ftmp*cos(cameraRot.y) * cameraTargetR *cos(cameraRot.x);

camera(cameraPos.x, cameraPos.y, cameraPos.z,
cameraTargetPos.x, cameraTargetPos.y, cameraTargetPos.z,
0, 1, 0);
perspective(PI/2.0, (float)width/(float)height, 1, 5000);
}

public class PFrame extends Frame {
public PFrame() {
setBounds(100, 100, 768, 768);
s = new secondApplet();
add(s);
s.init();
show();
}
}

public class secondApplet extends PApplet {
public void setup() {
size(768, 768);
controlP5 = new ControlP5(this);
r = controlP5.addRadioButton("radioButton")
.setPosition(500, 580)
.setSize(20, 20)
.setColorForeground(color(120))
.setColorActive(color(255))
.setColorLabel(color(255))
.setItemsPerRow(5)
.setSpacingColumn(50)
.addItem("Kinect", 0)
.addItem("Slider", 1)
.activate(mode)
;
controlP5.addButton("Save")
.setValue(0)
.setPosition(500, 700)
.setSize(30, 20)
;

controlP5.addSlider("Height", 150, 190, 500, 610, 200, 20);
controlP5.addSlider("Waist", 70, 100, 500, 640, 200, 20);
controlP5.addSlider("Arm", 60, 90, 500, 670, 200, 20);
// noLoop();
}

public void draw() {
background(0);
stroke(255);
noFill();
controlP5.controller("Height").setValue(mitake);
controlP5.controller("Waist").setValue(kosimawari);
controlP5.controller("Arm").setValue(yuki);
if (saveOneFrame == true) {
beginRecord(PDF, "Line.pdf");
}
drawsode(50, 200);
drawsode(50, 500);
drawmigoro(200, 350, 1);
drawmigoro(400, 350, -1);
drawokumi(500, 100, 1);
drawokumi(600, 100, -1);
draweri();
if (saveOneFrame == true) {
endRecord();
saveOneFrame = false;
}
redraw();
}
void drawsode(int x, int y) {
PVector sode1=new PVector(x, y);
PVector sode2=new PVector(sode1.x+sodehaba, sode1.y);
PVector sode3=new PVector(sode2.x, sode2.y+sodetuke);
PVector sode4=new PVector(sode2.x, sode2.y+sodetake);
PVector sode5=new PVector(sode4.x-sodehaba+sodemarumi, sode4.y);
PVector sode6=new PVector(sode5.x-sodemarumi, sode5.y-sodemarumi);
PVector sode7=new PVector(sode1.x, sode1.y+sodeguti);
PVector sode8=new PVector(sode5.x, sode5.y-sodemarumi);
PVector sode13=new PVector(sode2.x, sode2.y-sodetuke);
PVector sode14=new PVector(sode2.x, sode2.y-sodetake);
PVector sode15=new PVector(sode14.x-sodehaba+sodemarumi, sode14.y);
PVector sode16=new PVector(sode15.x-sodemarumi, sode15.y+sodemarumi);
PVector sode17=new PVector(sode1.x, sode1.y+sodeguti);
PVector sode18=new PVector(sode15.x, sode15.y+sodemarumi);
line(sode1.x, sode1.y, sode2.x, sode2.y);
line(sode2.x, sode2.y, sode3.x, sode3.y);
line(sode3.x, sode3.y, sode4.x, sode4.y);
line(sode4.x, sode4.y, sode5.x, sode5.y);
line(sode6.x, sode6.y, sode7.x, sode7.y);
line(sode7.x, sode7.y, sode1.x, sode1.y);
arc(sode8.x, sode8.y, sodemarumi*2, sodemarumi*2, HALF_PI, PI);
line(sode2.x, sode2.y, sode13.x, sode13.y);
line(sode13.x, sode13.y, sode14.x, sode14.y);
line(sode14.x, sode14.y, sode15.x, sode15.y);
line(sode16.x, sode16.y, sode17.x, sode17.y);
line(sode17.x, sode17.y, sode1.x, sode1.y);
arc(sode18.x, sode18.y, sodemarumi*2, sodemarumi*2, PI, PI+HALF_PI);
}

void drawmigoro(int x, int y, int dir) {
katahaba=(yuki/2-1)*2*dir;
maehaba=(kosimawari/2-7-15)*2*dir;
atohaba=((kosimawari/2+7)/2+2)*2*dir;
erikataaki=8.5*2*dir;
PVector migoro1=new PVector(x, y);
PVector migoro2=new PVector(migoro1.x+katahaba-erikataaki, migoro1.y);
PVector migoro3=new PVector(migoro2.x, migoro2.y+kurikosi);
PVector migoro4=new PVector(migoro3.x+erikataaki, migoro3.y);
PVector migoro5=new PVector(migoro4.x, migoro4.y+kitake-kurikosi);
PVector migoro6=new PVector(migoro5.x-atohaba, migoro5.y);
PVector migoro7=new PVector(migoro1.x+katahaba-atohaba, migoro1.y+sodetuke+miyatuguti);
PVector migoro8=new PVector(migoro7.x, migoro1.y-sodetuke-miyatuguti);
PVector migoro9=new PVector(migoro8.x, migoro8.y-kitake+sodetuke+miyatuguti);
PVector migoro10=new PVector(migoro9.x+maehaba, migoro9.y);
PVector migoro11=new PVector(migoro10.x-1, migoro10.y+kitake-okumisagari);
PVector migoro12=new PVector(migoro1.x+maehaba, migoro1.y);

line(migoro1.x, migoro1.y, migoro2.x, migoro2.y);
line(migoro2.x, migoro2.y, migoro3.x, migoro3.y);
line(migoro3.x, migoro3.y, migoro4.x, migoro4.y);
line(migoro4.x, migoro4.y, migoro5.x, migoro5.y);
line(migoro5.x, migoro5.y, migoro6.x, migoro6.y);
line(migoro6.x, migoro6.y, migoro7.x, migoro7.y);
line(migoro7.x, migoro7.y, migoro1.x, migoro1.y);
line(migoro1.x, migoro1.y, migoro8.x, migoro8.y);
line(migoro8.x, migoro8.y, migoro9.x, migoro9.y);
line(migoro9.x, migoro9.y, migoro10.x, migoro10.y);
line(migoro10.x, migoro10.y, migoro11.x, migoro11.y);
line(migoro11.x, migoro11.y, migoro12.x, migoro12.y);
}

void drawokumi(int x, int y, int dir) {
okumihaba=15.5*2*dir;
aiduma=14*2*dir;
PVector okumi1=new PVector(x, y);
PVector okumi2=new PVector(okumi1.x+okumihaba, okumi1.y);
PVector okumi3=new PVector(okumi2.x, okumi2.y+kitake-okumisagari);
PVector okumi4=new PVector(okumi3.x-aiduma, okumi1.y+erisita);
line(okumi1.x, okumi1.y, okumi2.x, okumi2.y);
line(okumi2.x, okumi2.y, okumi3.x, okumi3.y);
line(okumi3.x, okumi3.y, okumi4.x, okumi4.y);
line(okumi4.x, okumi4.y, okumi1.x, okumi1.y);
}

void draweri() {
erikataaki=8.5*2;
kensaki=6.5*2;
katamawari=5.5*2;
erituke=30.5*2;
PVector eri1=new PVector(700, 350);
PVector eri2=new PVector(eri1.x, eri1.y+erikataaki+0.6*2);
PVector eri3=new PVector(eri1.x, eri2.y+okumisagari);
PVector eri4=new PVector(eri1.x, eri3.y+erituke);
PVector eri5=new PVector(eri1.x+kensaki, eri4.y);
PVector eri6=new PVector(eri3.x+kensaki, eri3.y);
PVector eri7=new PVector(eri2.x+katamawari, eri2.y);
PVector eri8=new PVector(eri7.x, eri1.y);
PVector eri9=new PVector(eri1.x, eri1.y-erikataaki-0.6*2);
PVector eri10=new PVector(eri1.x, eri9.y-okumisagari);
PVector eri11=new PVector(eri1.x, eri10.y-erituke);
PVector eri12=new PVector(eri1.x+kensaki, eri11.y);
PVector eri13=new PVector(eri10.x+kensaki, eri10.y);
PVector eri14=new PVector(eri9.x+katamawari, eri9.y);

line(eri1.x, eri1.y, eri2.x, eri2.y);
line(eri2.x, eri2.y, eri3.x, eri3.y);
line(eri3.x, eri3.y, eri4.x, eri4.y);
line(eri4.x, eri4.y, eri5.x, eri5.y);
line(eri5.x, eri5.y, eri6.x, eri6.y);
line(eri6.x, eri6.y, eri7.x, eri7.y);
line(eri7.x, eri7.y, eri8.x, eri8.y);
line(eri8.x, eri8.y, eri1.x, eri1.y);

line(eri1.x, eri1.y, eri9.x, eri9.y);
line(eri9.x, eri9.y, eri10.x, eri10.y);
line(eri10.x, eri10.y, eri11.x, eri11.y);
line(eri11.x, eri11.y, eri12.x, eri12.y);
line(eri12.x, eri12.y, eri13.x, eri13.y);
line(eri13.x, eri13.y, eri14.x, eri14.y);
line(eri14.x, eri14.y, eri8.x, eri8.y);
}

public void Height(int v) {
Height2 = v;
}
public void Waist(int v) {
Waist2 = v;
if(kosimawari>yuki+24){
Waist2=yuki+24;
}
}
public void Arm(int v) {
Arm2 = v;
if(kosimawari>yuki+24){
Arm2=kosimawari-24;
}
}
void radioButton(int a) {
mode=a;
}

public void Save(int theValue) {
saveOneFrame = true;
}
}

//スケルトン
void drawSkeleton(int userId) {
strokeWeight(1);

// to get the 3d joint data
drawLimb(userId, SimpleOpenNI.SKEL_HEAD, SimpleOpenNI.SKEL_NECK);

drawLimb(userId, SimpleOpenNI.SKEL_NECK, SimpleOpenNI.SKEL_LEFT_SHOULDER);
drawLimb(userId, SimpleOpenNI.SKEL_LEFT_SHOULDER, SimpleOpenNI.SKEL_LEFT_ELBOW);
drawLimb(userId, SimpleOpenNI.SKEL_LEFT_ELBOW, SimpleOpenNI.SKEL_LEFT_HAND);

drawLimb(userId, SimpleOpenNI.SKEL_NECK, SimpleOpenNI.SKEL_RIGHT_SHOULDER);
drawLimb(userId, SimpleOpenNI.SKEL_RIGHT_SHOULDER, SimpleOpenNI.SKEL_RIGHT_ELBOW);
drawLimb(userId, SimpleOpenNI.SKEL_RIGHT_ELBOW, SimpleOpenNI.SKEL_RIGHT_HAND);

drawLimb(userId, SimpleOpenNI.SKEL_LEFT_SHOULDER, SimpleOpenNI.SKEL_TORSO);
drawLimb(userId, SimpleOpenNI.SKEL_RIGHT_SHOULDER, SimpleOpenNI.SKEL_TORSO);

drawLimb(userId, SimpleOpenNI.SKEL_TORSO, SimpleOpenNI.SKEL_LEFT_HIP);
drawLimb(userId, SimpleOpenNI.SKEL_LEFT_HIP, SimpleOpenNI.SKEL_LEFT_KNEE);
drawLimb(userId, SimpleOpenNI.SKEL_LEFT_KNEE, SimpleOpenNI.SKEL_LEFT_FOOT);

drawLimb(userId, SimpleOpenNI.SKEL_TORSO, SimpleOpenNI.SKEL_RIGHT_HIP);
drawLimb(userId, SimpleOpenNI.SKEL_RIGHT_HIP, SimpleOpenNI.SKEL_RIGHT_KNEE);
drawLimb(userId, SimpleOpenNI.SKEL_RIGHT_KNEE, SimpleOpenNI.SKEL_RIGHT_FOOT);

PVector torso = new PVector();
PVector waistR = new PVector();
PVector waistL = new PVector();
PVector waistC = new PVector();
PVector waistProj= new PVector();
PVector head = new PVector();
PVector footR = new PVector();
PVector footL = new PVector();
PVector footC = new PVector();
PVector shoulderC = new PVector();
PVector shoulderR = new PVector();
PVector shoulderL = new PVector();
PVector elbowR = new PVector();
PVector handR = new PVector();
PVector top = new PVector();

context.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_TORSO, torso);
context.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_RIGHT_HIP, waistR);
context.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_LEFT_HIP, waistL);
context.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_HEAD, head);
context.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_RIGHT_FOOT, footR);
context.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_LEFT_FOOT, footL);
context.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_RIGHT_SHOULDER, shoulderR);
context.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_LEFT_SHOULDER, shoulderL);
context.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_RIGHT_ELBOW, elbowR);
context.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_RIGHT_HAND, handR);
waistC.set(((waistR.x+waistL.x)/2+torso.x)/2,(waistR.y+waistL.y+torso.y)/3,((waistR.z+waistL.z)/2+torso.z)/2);

context.convertRealWorldToProjective(waistC,waistProj);
//println(waistProj.x+","+waistProj.y+","+waistProj.z);

topIndex=int(head.x)+int(head.y)*context.depthWidth();

for(int i=0;i<context.depthWidth()*context.depthHeight();i++){
if(userMap[i]!=0){
top = context.depthMapRealWorld()[i];
break;
}
}
waistIndex=int(waistProj.x)+int(waistProj.y)*context.depthWidth();
if(waistIndex<0)waistIndex=1;
if(waistIndex>307200)waistIndex=307200-1;
while(userMap != null && userMap[waistIndex] != 0){
waistIndex++;
}
waistIndex--;
waistR=context.depthMapRealWorld()[waistIndex];
waistRightIndex=waistIndex;
point(waistR.x,waistR.y,waistR.z);

waistIndex=int(waistProj.x)+int(waistProj.y)*context.depthWidth();
if(waistIndex<0)waistIndex=1;
if(waistIndex>307200)waistIndex=307200-1;
while(userMap != null && userMap[waistIndex] != 0){
waistIndex--;
}
waistIndex++;
waistL=context.depthMapRealWorld()[waistIndex];
waistLeftIndex=waistIndex;
point(waistL.x,waistL.y,waistL.z);
strokeWeight(2);
Waist=0;
for(int i=waistLeftIndex;i<waistRightIndex-10;i=i+10){
Waist+=context.depthMapRealWorld()[i].dist(context.depthMapRealWorld()[i+10]);
}
Waist*=0.2;
if(Waist<60)Waist=60;
if(Waist>100)Waist=100;
if(Waist>Arm+24){
Arm=Waist-24;
}
println(Waist);
footC.set((footR.x+footL.x)/2, (footR.y+footL.y)/2, (footR.z+footL.z)/2);
Height=top.dist(footC)*0.105;
Arm=(shoulderR.dist(shoulderL)/2+shoulderR.dist(elbowR)+elbowR.dist(handR))*0.1;
//println(shoulderR.dist(shoulderL)/2+","+shoulderR.dist(elbowR)+","+elbowR.dist(handR));
//体の向き
getBodyDirection(userId, bodyCenter, bodyDir);

bodyDir.mult(200);  // 200mm length
bodyDir.add(bodyCenter);

stroke(255, 200, 200);
line(bodyCenter.x, bodyCenter.y, bodyCenter.z, bodyDir.x, bodyDir.y, bodyDir.z);
strokeWeight(1);
}

//骨格線
void drawLimb(int userId, int jointType1, int jointType2) {
PVector jointPos1 = new PVector();
PVector jointPos2 = new PVector();
float  confidence;

// draw the joint position
confidence = context.getJointPositionSkeleton(userId, jointType1, jointPos1);
confidence = context.getJointPositionSkeleton(userId, jointType2, jointPos2);

stroke(255, 0, 0, confidence * 200 + 55);
line(jointPos1.x, jointPos1.y, jointPos1.z,
jointPos2.x, jointPos2.y, jointPos2.z);

drawJointOrientation(userId, jointType1, jointPos1, 50);
drawJointOrientation(userId, jointType2, jointPos2, 50);
}

//RGBの3軸表示
void drawJointOrientation(int userId, int jointType, PVector pos, float length)
{
// draw the joint orientation
PMatrix3D  orientation = new PMatrix3D();
float confidence = context.getJointOrientationSkeleton(userId, jointType, orientation);
if (confidence < 0.001f)
// nothing to draw, orientation data is useless
return;

pushMatrix();
translate(pos.x, pos.y, pos.z);

// set the local coordsys
applyMatrix(orientation);

// coordsys lines are 100mm long
// x - r
stroke(255, 0, 0, confidence * 200 + 55);
line(0, 0, 0,
length, 0, 0);
// y - g
stroke(0, 255, 0, confidence * 200 + 55);
line(0, 0, 0,
0, length, 0);
// z - b
stroke(0, 0, 255, confidence * 200 + 55);
line(0, 0, 0,
0, 0, length);
popMatrix();
}

// ここからおまじない
// SimpleOpenNI user events

void onNewUser(int userId)
{
println("onNewUser - userId: " + userId);
println("  start pose detection");

if (autoCalib)
context.requestCalibrationSkeleton(userId, true);
else
context.startPoseDetection("Psi", userId);
}

void onLostUser(int userId)
{
println("onLostUser - userId: " + userId);
}

void onExitUser(int userId)
{
println("onExitUser - userId: " + userId);
}

void onReEnterUser(int userId)
{
println("onReEnterUser - userId: " + userId);
}

void onStartCalibration(int userId)
{
println("onStartCalibration - userId: " + userId);
}

void onEndCalibration(int userId, boolean successfull)
{
println("onEndCalibration - userId: " + userId + ", successfull: " + successfull);
Height=150;
Waist=70;
Arm=60;

if (successfull)
{
println("  User calibrated !!!");
context.startTrackingSkeleton(userId);
}
else
{
println("  Failed to calibrate user !!!");
println("  Start pose detection");
context.startPoseDetection("Psi", userId);
}
}

void onStartPose(String pose, int userId)
{
println("onStartdPose - userId: " + userId + ", pose: " + pose);
println(" stop pose detection");

context.stopPoseDetection(userId);
context.requestCalibrationSkeleton(userId, true);
}

void onEndPose(String pose, int userId)
{
println("onEndPose - userId: " + userId + ", pose: " + pose);
}

// -----------------------------------------------------------------
// Keyboard events

void keyPressed()
{
switch(key)
{
case ' ':
context.setMirror(!context.mirror());
break;
}

switch(keyCode)
{
case LEFT:
rotY += 0.1f;
break;
case RIGHT:
// zoom out
rotY -= 0.1f;
break;
case UP:
if (keyEvent.isShiftDown())
zoomF += 0.01f;
else
rotX += 0.1f;
break;
case DOWN:
if (keyEvent.isShiftDown())
{
zoomF -= 0.01f;
if (zoomF < 0.01)
zoomF = 0.01;
}
else
rotX -= 0.1f;
break;
}
}

void getBodyDirection(int userId, PVector centerPoint, PVector dir)
{
PVector jointL = new PVector();
PVector jointH = new PVector();
PVector jointR = new PVector();
float  confidence;

// draw the joint position
confidence = context.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_LEFT_SHOULDER, jointL);
confidence = context.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_HEAD, jointH);
confidence = context.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_RIGHT_SHOULDER, jointR);

// take the neck as the center point
confidence = context.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_NECK, centerPoint);

/*  // manually calc the centerPoint
PVector shoulderDist = PVector.sub(jointL,jointR);
centerPoint.set(PVector.mult(shoulderDist,.5));
centerPoint.add(jointR);
*/

PVector up = new PVector();
PVector left = new PVector();

up.set(PVector.sub(jointH, centerPoint));
left.set(PVector.sub(jointR, centerPoint));

dir.set(up.cross(left));
dir.normalize();
}