package mocap.figure;
|
|
import java.util.ArrayList;
|
import java.util.HashMap;
|
import java.util.List;
|
|
import javax.media.j3d.Node;
|
import javax.media.j3d.SceneGraphPath;
|
import javax.media.j3d.Switch;
|
import javax.media.j3d.Transform3D;
|
import javax.media.j3d.TransformGroup;
|
import javax.vecmath.Point3d;
|
import javax.vecmath.Vector3d;
|
|
import j3d.Util;
|
|
/**
|
* Bone representation tailored to Acclaim format. Transform groups in this order (first is base):
|
*
|
* 1) base translation 2) translation (only used for root) 2) base rotation (joint geom attached
|
* here) 3) current rotation 4) inverse base rotation (bone geom attached here) 3) final
|
* translation, using direction + length from ASF definition (the translation could also be
|
* performed at the root of the child node -> then you could omit the first base translation)
|
*
|
* Note: Euler rotations performed with fixed axes.
|
*
|
* @author Michael Kipp
|
*/
|
public class Bone implements java.io.Serializable
|
{
|
|
public static final int FIXED_AXES = 0, MOVING_AXES = 1;
|
public static final int RX = 0, RY = 1, RZ = 2; // for specifying rotation order
|
public static final int TX = 3, TY = 4, TZ = 5;
|
public static final int NO_NAME = -1, SMALL_NAME = 0, BIG_NAME = 1;
|
private String _name;
|
private int _index;
|
private Bone _parent;
|
private Bone[] _children = new Bone[0];
|
transient BoneGeom _boneGeom; // the displayed geometry of the bone/joint
|
transient JointGeom _jointGeom;
|
transient private Switch _textSwitch;
|
transient TransformGroup _baseTransTG
|
,_transTG
|
,_baseRotTG
|
,_rotTG
|
,_invBaseRotTG
|
;
|
transient private Transform3D _t1 = new Transform3D();
|
transient private Transform3D _t2 = new Transform3D();
|
transient private Transform3D _transTF = new Transform3D();
|
transient private Transform3D _tworld = new Transform3D();
|
transient private Vector3d _trans = new Vector3d();
|
public int[] _dof = new int[0]; // rotation order
|
public int _numDOF = 0;
|
private boolean _translationEnabled = true; // translation can be switched off
|
private boolean _rotationEnabled = true; // rotation can be switched off
|
public int _rotationType = FIXED_AXES; // Euler rotation type
|
public double _scale = 1d; // scale is applied to all translational components
|
private Vector3d _geomDirection;
|
private double _geomRadius;
|
|
public Bone()
|
{
|
//Init();
|
}
|
|
void Init()
|
{
|
if (_baseTransTG != null)
|
return;
|
|
// _baseTransTG = new TransformGroup();
|
// _baseTransTG.setCapability(TransformGroup.ALLOW_TRANSFORM_WRITE);
|
// _baseTransTG.setCapability(TransformGroup.ALLOW_TRANSFORM_READ);
|
|
_baseRotTG = new TransformGroup();
|
_baseRotTG.setCapability(TransformGroup.ALLOW_TRANSFORM_WRITE);
|
_baseRotTG.setCapability(TransformGroup.ALLOW_TRANSFORM_READ);
|
// _baseRotTG.setCapability(TransformGroup.ALLOW_CHILDREN_READ);
|
_baseRotTG.setCapability(TransformGroup.ALLOW_LOCAL_TO_VWORLD_READ);
|
_rotTG = new TransformGroup();
|
_rotTG.setCapability(TransformGroup.ALLOW_TRANSFORM_WRITE);
|
_invBaseRotTG = new TransformGroup();
|
_invBaseRotTG.setCapability(TransformGroup.ALLOW_TRANSFORM_WRITE);
|
|
// wire up TG's
|
if (false) // _parent == null)
|
{
|
_transTG = new TransformGroup();
|
_transTG.setCapability(TransformGroup.ALLOW_TRANSFORM_WRITE);
|
|
_baseTransTG = _transTG; // .addChild(_transTG);
|
_transTG.addChild(_baseRotTG);
|
}
|
else
|
_baseTransTG = _baseRotTG; // .addChild(_baseRotTG);
|
|
_baseRotTG.addChild(_rotTG);
|
_rotTG.addChild(_invBaseRotTG);
|
|
_baseTransTG.setUserData(_name);
|
}
|
|
public Bone(String name, int index, int[] dof)
|
{
|
this();
|
_name = name;
|
_index = index;
|
_dof = dof;
|
_numDOF = _dof.length;
|
}
|
|
public void scale(double factor)
|
{
|
_scale = factor;
|
_baseTransTG.getTransform(_t1);
|
_t1.get(_trans);
|
_trans.scale(factor);
|
_t1.set(_trans);
|
_baseTransTG.setTransform(_t1);
|
for (Bone b : _children)
|
{
|
b.scale(factor);
|
}
|
// _boneGeom.detach();
|
// _jointGeom.detach();
|
}
|
|
public double getScale()
|
{
|
return _scale;
|
}
|
|
public void getWorldPosition(Point3d p)
|
{
|
p.set(0, 0, 0);
|
Util.getFullTransform(_baseRotTG, _tworld);
|
// getWorldTransform(_tworld);
|
_tworld.transform(p);
|
}
|
|
/**
|
* Ignores rotations and assumes that coord system is only translated from joint to joint.
|
*
|
* @param ref
|
* @param p
|
*/
|
public void getRelativeTransPosition(Bone ref, Point3d p)
|
{
|
getWorldPosition(p);
|
Point3d p2 = new Point3d();
|
ref.getWorldPosition(p2);
|
p.sub(p2);
|
}
|
|
/**
|
* Returns position of this bone in frame of reference of given bone.
|
*
|
* Note that rotations in the kinematic chain can render this information useless.
|
*
|
* @param ref
|
* @param p
|
*/
|
public void getRelativePosition(Bone ref, Point3d p)
|
{
|
p.set(0, 0, 0);
|
Util.getRelativeTransform(_baseRotTG, ref.getBaseRotTG(), _t1);
|
_t1.transform(p);
|
|
// Transform3D t1 = new Transform3D();
|
// Transform3D t2 = new Transform3D();
|
// Transform3D t3 = new Transform3D();
|
// getWorldTransform(t1);
|
// ref.getWorldTransform(t2);
|
// t2.invert();
|
// t3.mul(t2, t1);
|
//// t1.mul(t1, t2);
|
// t3.transform(p);
|
}
|
|
// public void getWorldTransform(Transform3D tf) {
|
// _rotTG.getLocalToVworld(tf);
|
// }
|
public void getPosition(SceneGraphPath path, Point3d p)
|
{
|
p.set(0, 0, 0);
|
_baseRotTG.getLocalToVworld(path, _tworld);
|
_tworld.transform(p);
|
}
|
|
/**
|
* Determines whether to rotate in fixed or moving axes manner (Euler rotation).
|
*
|
* @param type
|
* Either FIXED_AXES or MOVING_AXES.
|
*/
|
public void setRotationType(int type)
|
{
|
_rotationType = type;
|
}
|
|
public void setSelected(boolean val)
|
{
|
_boneGeom.setSelected(val);
|
}
|
|
public String getDOFString()
|
{
|
String res = "";
|
for (int i = 0; i < _dof.length; i++)
|
{
|
switch (_dof[i])
|
{
|
case RX:
|
res += "RX ";
|
break;
|
case RY:
|
res += "RY ";
|
break;
|
case RZ:
|
res += "RZ ";
|
break;
|
case TX:
|
res += "TX ";
|
break;
|
case TY:
|
res += "TY ";
|
break;
|
case TZ:
|
res += "TZ ";
|
break;
|
}
|
}
|
return res;
|
}
|
|
/**
|
* Sets bone and children to zero rotation.
|
*/
|
public void reset()
|
{
|
_t1.setIdentity();
|
_rotTG.setTransform(_t1);
|
for (Bone b : _children)
|
{
|
b.reset();
|
}
|
}
|
|
/**
|
* Switches on/off translation.
|
*/
|
public void setTranslationEnabled(boolean val)
|
{
|
_translationEnabled = val;
|
}
|
|
/**
|
* Switches on/off orientation/rotation.
|
*/
|
public void setRotationEnabled(boolean val)
|
{
|
_rotationEnabled = val;
|
}
|
|
public double getLength()
|
{
|
return _boneGeom != null ? _boneGeom.getLength() : 0d;
|
}
|
|
public void selectGeom(int style)
|
{
|
if (_boneGeom != null)
|
{
|
_boneGeom.select(style);
|
}
|
for (int i = 0; i < _children.length; i++)
|
{
|
_children[i].selectGeom(style);
|
}
|
}
|
|
public void selectJointGeom(int style)
|
{
|
selectJointGeom(style, true);
|
}
|
|
public void selectJointGeom(int style, boolean recursiv)
|
{
|
if (_jointGeom != null)
|
{
|
_jointGeom.select(style);
|
}
|
|
if (recursiv)
|
{
|
for (int i = 0; i < _children.length; i++)
|
{
|
_children[i].selectJointGeom(style);
|
}
|
}
|
}
|
|
public double getMaxDistance()
|
{
|
// System.out.println(">>>>>> Branch >>>>>");
|
double dDistance = 0;
|
if (_children.length > 0)
|
{
|
Point3d p3dCurrentBone = new Point3d();
|
getWorldPosition(p3dCurrentBone);
|
double[] arrayDistances = new double[_children.length];
|
int i = 0;
|
|
for (Bone bone : _children)
|
{
|
double dTmp = 0;
|
if (bone.getChildren().length == 0)
|
{
|
// System.out.println(">>>>>> leaf >>>>>\n\n");
|
Point3d p3dChild = new Point3d();
|
bone.getWorldPosition(p3dChild);
|
dTmp = p3dChild.distanceSquared(p3dCurrentBone);
|
// System.out.println("Distance to leaf (" + _name + ", "
|
// + bone.getName() + ") : " + dTmp);
|
//
|
|
|
} else
|
{
|
|
Point3d p3dChild = new Point3d();
|
bone.getWorldPosition(p3dChild);
|
dTmp = p3dChild.distanceSquared(p3dCurrentBone);
|
// System.out.println("Distance to node (" + _name + ", "
|
// + bone.getName() + ") : " + dTmp);
|
// double dRest = bone.getMaxDistance();
|
// System.out.println("Distance of rest (" + _name + ", "
|
// + bone.getName() + ") :: " + dRest);
|
dTmp += bone.getMaxDistance();
|
;
|
// System.out.println("Distance of node (" + _name + ", "
|
// + bone.getName() + ") + rest:: " + dTmp +"\n\n");
|
}
|
arrayDistances[i++] = dTmp;
|
}
|
|
for (double distance : arrayDistances)
|
{
|
dDistance = Math.max(distance, dDistance);
|
}
|
|
}
|
|
return dDistance;
|
|
}
|
|
public Bone getParent()
|
{
|
return _parent;
|
}
|
|
public void setParent(Bone parent)
|
{
|
_parent = parent;
|
}
|
|
public Bone[] getChildren()
|
{
|
return _children;
|
}
|
|
public String getName()
|
{
|
return _name;
|
}
|
|
public void setName(String name)
|
{
|
_name = name;
|
}
|
|
private List<Node> collectScenegraphPath(Bone initialBone, boolean all)
|
{
|
// System.out.println(">> collect " + getName() + " initial=" + initialBone);
|
List<Node> result = new ArrayList<Node>();
|
if (!this.equals(initialBone))
|
{
|
result.addAll(_parent.collectScenegraphPath(initialBone, true));
|
result.add(_baseTransTG);
|
result.add(_transTG);
|
}
|
|
if (all)
|
{
|
result.add(_baseRotTG);
|
result.add(_rotTG);
|
result.add(_invBaseRotTG);
|
}
|
return result;
|
}
|
|
public SceneGraphPath getSceneGraphPath(Bone initialBone)
|
{
|
List<Node> path = collectScenegraphPath(initialBone, false);
|
Node[] p = new Node[path.size()];
|
p = (Node[]) path.toArray(p);
|
SceneGraphPath sgp = new SceneGraphPath();
|
sgp.setNodes(p);
|
return sgp;
|
}
|
|
public void setChildren(Bone[] children)
|
{
|
_children = children;
|
for (int i = 0; i < children.length; i++)
|
{
|
Bone bone = children[i];
|
getEndTG().addChild(bone.getBaseTG());
|
}
|
}
|
|
/**
|
* This does not work for some reason.
|
*/
|
public void setRot(float x, float y, float z)
|
{
|
System.out.println("setRot " + x + " " + " " + y + " " + z);
|
_t1.setIdentity();
|
_t2.setIdentity();
|
for (int i = 0; i < _numDOF; i++)
|
{
|
switch (_dof[i])
|
{
|
case RX:
|
_t2.rotX(x);
|
break;
|
case RY:
|
_t2.rotY(y);
|
break;
|
case RZ:
|
_t2.rotZ(z);
|
break;
|
}
|
// multiply rotations
|
// fixed axes: multiply from left
|
if (_rotationType == FIXED_AXES)
|
{
|
if (i > 0)
|
{
|
_t2.mul(_t1);
|
}
|
_t1 = new Transform3D(_t2);
|
} else
|
{
|
// moving axes: multiply from right
|
_t1.mul(_t2);
|
_t2.setIdentity();
|
}
|
// set rotation
|
if (_rotationEnabled)
|
{
|
_rotTG.setTransform(_t1);
|
}
|
}
|
}
|
|
public void setFrame(int frame, float[] data)
|
{
|
setPose(frame, data, null);
|
}
|
|
/**
|
* Sets this bone/joint to the translation/rotation of the given frame.
|
* Called in the animation loop.
|
*
|
* @param frame
|
* Frame number
|
*/
|
public void setPose(int frame, float[] data, Point3d offsetTrans)
|
{
|
// System.out.println("setPose " + frame);
|
// _lastFrame = frame;
|
// do something if joint has at least one DOF
|
if (_numDOF > 0)
|
{
|
int offset = frame * _numDOF;
|
if (offset + _numDOF - 1 < data.length)
|
{
|
boolean hasTranslation = false;
|
if (_parent == null)
|
{
|
_trans.set(0, 0, 0);
|
}
|
_t1.setIdentity();
|
|
// go through DOF in the specified order
|
for (int i = 0; i < _numDOF; i++)
|
{
|
boolean isRotation = false;
|
switch (_dof[i])
|
{
|
case RX:
|
_t2.rotX(data[offset + i]);
|
isRotation = true;
|
break;
|
case RY:
|
_t2.rotY(data[offset + i]);
|
isRotation = true;
|
break;
|
case RZ:
|
_t2.rotZ(data[offset + i]);
|
isRotation = true;
|
break;
|
case TX:
|
_trans.x += _scale * data[offset + i];
|
hasTranslation = true;
|
isRotation = false;
|
break;
|
case TY:
|
_trans.y += _scale * data[offset + i];
|
hasTranslation = true;
|
isRotation = false;
|
break;
|
case TZ:
|
_trans.z += _scale * data[offset + i];
|
hasTranslation = true;
|
isRotation = false;
|
break;
|
}
|
|
// multiply rotations
|
if (isRotation)
|
{
|
// fixed axes: multiply from left
|
if (_rotationType == FIXED_AXES)
|
{
|
if (i > 0)
|
{
|
_t2.mul(_t1);
|
}
|
_t1 = new Transform3D(_t2);
|
} else
|
{
|
// moving axes: multiply from right
|
_t1.mul(_t2);
|
_t2.setIdentity();
|
}
|
}
|
}
|
|
// set rotation
|
if (_rotationEnabled)
|
{
|
_rotTG.setTransform(_t1);
|
}
|
|
// add offset to translation of root
|
if (offsetTrans != null && _parent == null)
|
{
|
_trans.add(offsetTrans);
|
hasTranslation = true;
|
}
|
|
// set translation
|
// (currently only possible for root bone! see mocapdata.com data)
|
if (_parent == null)
|
{
|
if (hasTranslation && _translationEnabled)
|
{
|
_transTF.setIdentity();
|
_transTF.setTranslation(_trans);
|
_transTG.setTransform(_transTF);
|
}
|
}
|
}
|
}
|
}
|
|
public int getIndex()
|
{
|
return _index;
|
}
|
|
public int[] getDOF()
|
{
|
return _dof;
|
}
|
|
/**
|
* Attaches bone geometry (e.g. line) which has orientation and length of the given vector, also
|
* attaches joint geometry to the beginning of this bone.
|
*/
|
public void attachGeom(Vector3d direction, double maxRadius, boolean joint)
|
{
|
_geomDirection = new Vector3d(direction);
|
_geomRadius = maxRadius;
|
_boneGeom = new BoneGeom(_invBaseRotTG, direction);
|
|
// double maxlength = _parent == null ? getLength() : Math.min(
|
// getLength(), _parent.getLength());
|
// _jointGeom = new JointGeom(_baseRotTG, (float) Math.min(maxRadius,
|
// maxlength / 4));
|
|
if (joint)
|
_jointGeom = new JointGeom(_baseRotTG, (float)(BoneGeom.CYLINDER_RADIUS*direction.length()));
|
}
|
|
public Bone findBone(String name)
|
{
|
if (_name.equals(name))
|
{
|
return this;
|
}
|
for (int i = 0; i < _children.length; i++)
|
{
|
Bone bone = _children[i].findBone(name);
|
if (bone != null)
|
{
|
return bone;
|
}
|
}
|
return null;
|
}
|
|
public void collectBones(HashMap<String, Bone> map)
|
{
|
map.put(_name, this);
|
for (int i = 0; i < _children.length; i++)
|
{
|
Bone bone = _children[i];
|
bone.collectBones(map);
|
}
|
}
|
|
public void collectBones(List<Bone> list)
|
{
|
list.add(this);
|
for (int i = 0; i < _children.length; i++)
|
{
|
Bone bone = _children[i];
|
bone.collectBones(list);
|
}
|
}
|
|
public void setBaseTranslation(Vector3d vec)
|
{
|
Transform3D tf = new Transform3D();
|
tf.setTranslation(vec);
|
getBaseTG(). //_baseTransTG.
|
setTransform(tf);
|
}
|
|
public TransformGroup getBaseTransTG()
|
{
|
Init();
|
return _baseTransTG;
|
}
|
|
public TransformGroup getBaseTG()
|
{
|
Init();
|
return _baseTransTG; // ?
|
}
|
|
public TransformGroup getBaseRotTG()
|
{
|
Init();
|
return _baseRotTG;
|
}
|
|
public TransformGroup getEndTG()
|
{
|
Init();
|
return _rotTG; // _invBaseRotTG;
|
}
|
|
public Vector3d getUpVector()
|
{
|
Transform3D xform = new Transform3D();
|
_baseTransTG.getLocalToVworld(xform);
|
|
/// transform the northward vector from world to local space
|
Vector3d v3dUpVector = new Vector3d(0, 1, 0);
|
// xform.invert();
|
xform.transform(v3dUpVector);
|
System.out.println("UpVector: " + v3dUpVector);
|
Util.getFullTransform(_baseRotTG, _tworld);
|
// getWorldTransform(_tworld);
|
return v3dUpVector;
|
|
}
|
|
/**
|
* Sets the base rotation of the joint (specified as ":axis" in ASF). Note that this operation
|
* is reversed in _invBaseRotTG so that the position of the child limb is not affected by the
|
* base rotation.
|
*/
|
public void setBaseRotDeg(Vector3d eulerDegrees)
|
{
|
_t1.rotX(Math.toRadians(eulerDegrees.x));
|
_t2.rotY(Math.toRadians(eulerDegrees.y));
|
_t2.mul(_t1);
|
|
Transform3D baseRot = new Transform3D();
|
|
baseRot.rotZ(Math.toRadians(eulerDegrees.z));
|
baseRot.mul(_t2);
|
_baseRotTG.setTransform(baseRot);
|
|
Transform3D invBaseRot = new Transform3D(baseRot);
|
invBaseRot.invert();
|
_invBaseRotTG.setTransform(invBaseRot);
|
}
|
|
/**
|
* Select display style of bone's name.
|
*/
|
public void displayName(int type)
|
{
|
if (_textSwitch != null)
|
{
|
_textSwitch.setWhichChild(type == NO_NAME
|
? Switch.CHILD_NONE
|
: type);
|
}
|
for (int i = 0; i < _children.length; i++)
|
{
|
_children[i].displayName(type);
|
}
|
}
|
|
/**
|
* Adds geometry to the joints.
|
*/
|
// public void init() {
|
// _jointGeom = new JointGeom(_baseRotTG);
|
// for (Bone bone : _children) {
|
// bone.init();
|
// }
|
// }
|
public String toString()
|
{
|
return "<Bone: " + _name + ">";
|
}
|
|
public void print()
|
{
|
System.out.println(" bone: " + _name + " dof=" + _numDOF + " -> "
|
+ (_parent == null ? "VOID" : _parent.getName()));
|
for (int i = 0; i < _children.length; i++)
|
{
|
Bone bone = _children[i];
|
bone.print();
|
}
|
}
|
}
|
