@Override
public Joint find(String id) {
if (getId().equals(id)) return this;
for (int i = 0; i < children.size(); i++) {
Joint child = children.get(i);
Joint subChild = child.find(id);
if (subChild != null) return subChild;
}
return null;
}
public void calcBaseMatrix() {
for (int i = 0; i < joints.size(); i++) {
Joint j = joints.get(i);
j.base = new Matrix4f();
j.base.setTransform(j.pose);
j.inverseBase = j.base.invert();
if (j.parent >= 0) {
joints.get(j.parent).base.mult(j.base, j.base);
j.inverseBase.mult(joints.get(j.parent).inverseBase, j.inverseBase);
}
}
}
public void update(Joint updatedBy) {
for (MovementStep s : savedMovementFlow) {
s.getMove().updateWorldOrientation();
}
// check if updateBy is parent of currently observed joint
// if yes its not neccessary to update
if (observedJoint.hasParent(updatedBy)) {
return;
}
StoredJointState newState = new StoredJointState(observedJoint, observedJoint.parent, true);
if (lastChanged != null) {
StoredJointState lastState = lastChanged.getMove();
if (!newState.hasAngelDifferenceGreaterThan(lastState, MIN_ANGLE_DIFFERENCE)) {
// LOGGER.debug("Difference to low");
return;
}
}
// check if an almost same position is already saved and its not
// the last one (not increasing counter if we move slightly on
// position)
// if yes increase counter
for (int i = 0; i < savedMovementFlow.size() - 1; i++) {
MovementStep m = savedMovementFlow.get(i);
if (m != lastChanged && m.getMove().equals(newState)) {
m.incCount();
maxCount = Math.max(maxCount, m.getCount());
// LOGGER.debug("Find existing position - Increase count");
return;
}
}
// LOGGER.debug("Created new one");
lastChanged = new MovementStep(newState);
savedMovementFlow.addLast(lastChanged);
// get right parent
// save extrema
checkExtrema(newState);
}
/** Check whether a hand (of any user) touches the radio. Play radio if it does. */
public void checkHands() {
MyWorld world = (MyWorld) getWorld();
UserData[] users = world.getTrackedUsers();
for (UserData user : users) {
Joint rightHand = user.getJoint(Joint.RIGHT_HAND);
Radio radio = getRadio(rightHand.getX(), rightHand.getY());
if (radio != null) { // yes, there was a radio
radio.play(1);
} else {
Joint leftHand = user.getJoint(Joint.LEFT_HAND);
radio = getRadio(leftHand.getX(), leftHand.getY());
if (radio != null) {
radio.play(2);
}
}
}
}
/** Display a tracked user on screen */
private void trackUser(UserData user) {
for (int i = 0; i < Joint.NUM_JOINTS; i++) {
Joint joint = user.getJoint(i);
dot[i].set(joint.getX(), joint.getY());
}
}
/**
* Step the simulation. Currently anything other than 1/60f as a step leads to unpredictable
* results - hence the default step fixes us to this step
*
* @param dt The amount of time to step
*/
public void step(float dt) {
for (int i = 0; i < bodies.size(); ++i) {
for (int j = 0; j < sources.size(); j++) {
((ForceSource) sources.get(j)).apply(bodies.get(i), dt);
}
}
BodyList bodies = getActiveBodies();
JointList joints = getActiveJoints();
float invDT = dt > 0.0f ? 1.0f / dt : 0.0f;
if (restingBodyDetection) {
for (int i = 0; i < bodies.size(); ++i) {
Body b = bodies.get(i);
b.startFrame();
}
for (int i = 0; i < joints.size(); ++i) {
Joint j = joints.get(i);
j.getBody1().setIsResting(false);
j.getBody2().setIsResting(false);
}
}
broadPhase(dt);
for (int i = 0; i < bodies.size(); ++i) {
Body b = bodies.get(i);
if (b.getInvMass() == 0.0f) {
continue;
}
if (b.isResting() && restingBodyDetection) {
continue;
}
Vector2f temp = new Vector2f(b.getForce());
temp.scale(b.getInvMass());
if (b.getGravityEffected()) {
temp.add(gravity);
}
temp.scale(dt);
b.adjustVelocity(temp);
Vector2f damping = new Vector2f(b.getVelocity());
damping.scale(-b.getDamping() * b.getInvMass());
b.adjustVelocity(damping);
b.adjustAngularVelocity(dt * b.getInvI() * b.getTorque());
b.adjustAngularVelocity(-b.getAngularVelocity() * b.getInvI() * b.getRotDamping());
}
for (int i = 0; i < arbiters.size(); i++) {
Arbiter arb = arbiters.get(i);
if (!restingBodyDetection || !arb.hasRestingPair()) {
arb.preStep(invDT, dt, damping);
}
}
for (int i = 0; i < joints.size(); ++i) {
Joint j = joints.get(i);
j.preStep(invDT);
}
for (int i = 0; i < iterations; ++i) {
for (int k = 0; k < arbiters.size(); k++) {
Arbiter arb = arbiters.get(k);
if (!restingBodyDetection || !arb.hasRestingPair()) {
arb.applyImpulse();
} else {
arb.getBody1().collided(arb.getBody2());
arb.getBody2().collided(arb.getBody1());
}
}
for (int k = 0; k < joints.size(); ++k) {
Joint j = joints.get(k);
j.applyImpulse();
}
}
for (int i = 0; i < bodies.size(); ++i) {
Body b = bodies.get(i);
if (b.getInvMass() == 0.0f) {
continue;
}
if (restingBodyDetection) {
if (b.isResting()) {
continue;
}
}
b.adjustPosition(b.getVelocity(), dt);
b.adjustPosition(b.getBiasedVelocity(), dt);
b.adjustRotation(dt * b.getAngularVelocity());
b.adjustRotation(dt * b.getBiasedAngularVelocity());
b.resetBias();
b.setForce(0, 0);
b.setTorque(0);
}
if (restingBodyDetection) {
for (int i = 0; i < bodies.size(); ++i) {
Body b = bodies.get(i);
b.endFrame();
}
}
cleanUpArbiters();
}
@Override
public void setAngle(float angle) {
this.angle = angle - parent.getAngle();
}
@Override
public float getAngle() {
return angle + parent.getAngle();
}
@Override
public void setPosition(float x, float y) {
float localX = parent.projectX(x, y);
float localY = parent.projectY(x, y);
localPosition.set(localX, localY);
}
private Vector2 getLocalPosition() {
absolutePosition.set(localPosition.x, localPosition.y);
absolutePosition.rotate(parent.getAngle());
absolutePosition.set(absolutePosition.x + parent.getX(), absolutePosition.y + parent.getY());
return absolutePosition;
}
public JointImpl(Joint joint) {
this.parent = joint.getParent();
setId(joint.getId());
setPosition(joint.getX(), joint.getY());
setAngle(joint.getAngle());
}
public MotionAnalysis(Hand hand, Joint observedJoint) {
this.hand = hand;
this.observedJoint = observedJoint;
addInitialSavedMove();
infoName = "Motion " + observedJoint.getInfoName();
}
