001: /*
002: * Apollo - Motion capture and animation system
003: * Copyright (c) 2005 Apollo
004: *
005: * This program is free software; you can redistribute it and/or
006: * modify it under the terms of the GNU General Public License
007: * as published by the Free Software Foundation; either version 2
008: * of the License, or (at your option) any later version.
009: *
010: * This program is distributed in the hope that it will be useful,
011: * but WITHOUT ANY WARRANTY; without even the implied warranty of
012: * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
013: * GNU General Public License for more details.
014: *
015: * You should have received a copy of the GNU General Public License
016: * along with this program; if not, write to the Free Software
017: * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
018: *
019: * http://www.gnu.org/copyleft/gpl.html
020: *
021: * @author Giovane.Kuhn - brain@netuno.com.br
022: *
023: */
024: package org.apollo.trackmodel;
025:
026: import javax.vecmath.Vector3d;
027:
028: import org.apollo.ApolloUtil;
029: import org.apollo.bvh.Joint;
030: import org.apollo.datamodel.TrackingFrame;
031: import org.apollo.datamodel.TrackingJoint;
032: import org.apollo.datamodel.TrackingVideo;
033:
034: /**
035: * Calculate position for a tracking joint based on its grandparent and parent,
036: * since both have truth position
037: * @author Giovane.Kuhn on 02/06/2005
038: */
039: public final class AncestralPositioner extends AbstractPositioner {
040:
041: public AncestralPositioner(TrackingVideo video) {
042: super (video);
043: }
044:
045: public synchronized void track(TrackingFrame frame) {
046: this .actualTrackFrame = frame;
047: modelRoot.visit(this );
048: }
049:
050: public boolean accept(Joint node) {
051:
052: // analyze just joint accepted by the plan
053: if (!plan.getModelVisitor().accept(node)) {
054: return false;
055: }
056:
057: // calculate just for nodes with parent and grandparent
058: if (node.getParent() == null
059: || node.getParent().getParent() == null) {
060: return true;
061: }
062:
063: // cannot exists tracking joint for current node
064: TrackingJoint tnode = actualTrackFrame.getTrackingJoint(node
065: .getName());
066: if (tnode != null) {
067: return true;
068: }
069:
070: // get parent
071: Joint parent = node.getParent();
072: TrackingJoint tparent = actualTrackFrame
073: .getTrackingJoint(parent.getName());
074:
075: // get grandparent
076: Joint grand = parent.getParent();
077: TrackingJoint tgrand = actualTrackFrame.getTrackingJoint(grand
078: .getName());
079:
080: // must exists parent and child tracking
081: if (ApolloUtil.hasNull(tparent, tgrand)) {
082: return true;
083: }
084:
085: // calculate tnode position based on its parent and child
086: tnode = calculate(node, tgrand, parent, tparent);
087:
088: // if succed on creating a new tracking joint
089: if (tnode != null) {
090: tnode.setName(node.getName());
091: actualTrackFrame.addTrackingJoint(tnode);
092: }
093:
094: return true;
095: }
096:
097: /**
098: * Calculate joint position, based on its parent and grandparent
099: * @param node Model joint to be calculated
100: * @param tgrand Grandparent tracking joint
101: * @param parent Parent model joint
102: * @param tparent Parent tracking joint
103: * @return New tracking joint
104: */
105: private TrackingJoint calculate(Joint node, TrackingJoint tgrand,
106: Joint parent, TrackingJoint tparent) {
107:
108: // get vectors
109: Vector3d nodeVec = node.getVector3d();
110: Vector3d tgrandVec = tgrand.getVector3d();
111: Vector3d parentVec = parent.getVector3d();
112: Vector3d tparentVec = tparent.getVector3d();
113:
114: // calculate vector between grandparent and node
115: Vector3d thirdVec = new Vector3d();
116: thirdVec.add(parentVec, nodeVec);
117:
118: // calculate vector for parent in this frame
119: Vector3d nparentVec = new Vector3d();
120: nparentVec.sub(tparentVec, tgrandVec);
121:
122: // angle between parent and new parent and third
123: double angle = plan.angle(nparentVec, parentVec);
124:
125: // create new node vector, and calculate its global coordinate
126: Vector3d retVec = plan.rotateVector(thirdVec, angle);
127: retVec.add(tgrandVec);
128:
129: // TODO improve below code, really this way
130: // not exists enough information about coordinates
131: if (ApolloUtil.hasNaN(retVec.x, retVec.y, retVec.z)) {
132: retVec.add(tparentVec, nodeVec);
133: }
134:
135: // create new tracking joint
136: TrackingJoint ret = new TrackingJoint();
137: ret.setVector3d(retVec);
138: return ret;
139: }
140: }
|