import mocap.figure.BoneGeom; import javax.media.j3d.Transform3D; import javax.vecmath.Vector3d; import javax.vecmath.Vector2d; import javax.vecmath.Quat4d; import javax.vecmath.Matrix4d; import com.jme.math.Quaternion; import java.io.*; //import com.bulletphysics.linearmath.QuaternionUtil; /* * To change this template, choose Tools | Templates * and open the template in the editor. */ /** * * @author nbriere */ public class Mocap extends Object3D { static final long serialVersionUID = 7437391692559853707L; void Fade() { if (currentbones == null || CameraPane.fullreset) return; cVector temp = new cVector(); int numframes = bvh.animation.getNumFrames(); int b; float[] data; Quat4d quatstart = new Quat4d(); Quat4d quatend = new Quat4d(); Quat4d quat = new Quat4d(); Matrix4d mat4d = new Matrix4d(); double[][] rot = new double[4][4]; for (b=numbones; --b>=0;) // HIP ROTATION: 0;) { // int hipindex = get(0)._index; // assert(get(0).get(0)._isHip); // if (b == hipindex) // continue; data = bvh.animation.getBoneData(b); int dof = 3; // data.length/numframes; if (b == 0) { dof = 6; } // assert(dof == 3); int boneframe3 = (b-1)*3; // dof; // System.err.println("Bone #" + b + ": dof = " + dof); int fadein = 45; // 120; // if (b == hipindex) // fadein *= 5; if (fadein > numframes) fadein = 0; // numframes; for (int fi=fadein; --fi>=0;) //for (int f=numframes; --f>=0;) { int f = fi + GetFirstFrame(); int f3 = f*dof; float k = fi; k /= fadein - 1; if (Float.isNaN(k)) k = 1; // 0/0 k = (float) (-Math.cos(k*Math.PI)); k += 1; k /= 2; int start = 3; int end = 0; if (dof == 6) { start = 6; end = 3; } if (b == 0) { for (int i=start; --i>=end;) { //data[f3 + i] += pos[i] - data[frame3 + i]; // data[f3 + i] = k*data[f3+i] + (1-k)* // currentbones[boneframe3 + i-end]; // data[f3 + i] = (float)CurveAngle(data[f3+i], currenthip[i], 1-k); } // TODO: translation // _t1.setIdentity(); // _t2.setIdentity(); // _t2.rotZ(currentbones[boneframe3 + start-1-end]); // _t1.mul(_t2); // _t2.setIdentity(); // if (b == 0) // { // _t2.rotY(currentbones[boneframe3 + start-2-end]); // _t1.mul(_t2); // _t2.setIdentity(); // _t2.rotX(currentbones[boneframe3 + start-3-end]); // } // else // { // _t2.rotX(currentbones[boneframe3 + start-2-end]); // _t1.mul(_t2); // _t2.setIdentity(); // _t2.rotY(currentbones[boneframe3 + start-3-end]); // } // _t1.mul(_t2); // _t2.setIdentity(); // // _t1.get(mat4d); // // mat4d.get(quatstart); // // _t1.setIdentity(); // _t2.setIdentity(); // _t2.rotZ(data[f3 + start-1]); // _t1.mul(_t2); // _t2.setIdentity(); // if (b == 0) // { // _t2.rotY(data[f3 + start-2]); // _t1.mul(_t2); // _t2.setIdentity(); // _t2.rotX(data[f3 + start-3]); // } // else // { // _t2.rotX(data[f3 + start-2]); // _t1.mul(_t2); // _t2.setIdentity(); // _t2.rotY(data[f3 + start-3]); // } // // _t1.mul(_t2); // _t2.setIdentity(); // // _t1.get(mat4d); // // mat4d.get(quatend); // // double dot = quatstart.x*quatend.x + // quatstart.y*quatend.y + // quatstart.z*quatend.z + // quatstart.w*quatend.w // ; // // if (dot < 0) // { // quatend.x *= -1; // quatend.y *= -1; // quatend.z *= -1; // quatend.w *= -1; // } // // // k = 1-k; // // quat.x = (1-k)*quatstart.x + k*quatend.x; // quat.y = (1-k)*quatstart.y + k*quatend.y; // quat.z = (1-k)*quatstart.z + k*quatend.z; // quat.w = (1-k)*quatstart.w + k*quatend.w; // // quat.normalize(); // // mat4d.set(quat); // // cJ3D.SetTransform(rot, mat4d); // // matrixToEulerXYZ(rot, temp); // // data[f3 + start-1] = -(float)temp.z; // // if (b == 0) // { // data[f3 + start-2] = -(float)temp.y; // data[f3 + start-3] = -(float)temp.x; // } // else // { // data[f3 + start-2] = -(float)temp.x; // data[f3 + start-3] = -(float)temp.y; // } } else { for (int i=start; --i>=end;) { //data[f3 + i] += pos[i] - data[frame3 + i]; // data[f3 + i] = k*data[f3+i] + (1-k)* // currentbones[boneframe3 + i-end]; data[f3 + i] = (float)CurveAngle(data[f3+i], currentbones[boneframe3 + i/*-end*/], 1-k); } } } } } // Updates the toParent matrix to keep the same position and orientation // before resetting the mocap data. void SetGlobalTransform() { //SetCurrentBones(frame); cVector temp = new cVector(); cVector pos = new cVector(); cVector poship = new cVector(); double angleY = 0; double angleYhip = 0; Object3D hip = get(0); if (hip.get(0).toParent == null) { hip.get(0).toParent = LA.newMatrix(); hip.get(0).fromParent = LA.newMatrix(); } LA.matConcat(toParent, hip.get(0).toParent, matrix); poship.x = matrix[3][0]; poship.y = matrix[3][1]; poship.z = matrix[3][2]; temp.x = 1; temp.y = 0; temp.z = 0; LA.xformDir(temp, matrix, temp); angleYhip = Math.atan2(-temp.z, temp.x); LA.matIdentity(toParent); LA.matYRotate(toParent, angleYhip); LA.matTranslate(toParent, poship.x, poship.y, poship.z); // LA.matConcat(toParent, hip.get(0).toParent, toParent); CameraPane.CreateSelectedPoint(); CameraPane.debugpointG.toParent[3][0] = poship.x; CameraPane.debugpointG.toParent[3][1] = poship.y; CameraPane.debugpointG.toParent[3][2] = poship.z; LA.matInvert(toParent, fromParent); LA.matIdentity(hip.get(0).toParent); LA.matIdentity(hip.get(0).fromParent); // if (true) // return; // Updates hip.get(0).toParent setPose(hip, GetFirstFrame(), bvh.animation.getBoneData(hip._index)); // A = toParent; B = hip.get(0).toParent // A'*B = A // A' = A * B-1 poship.x = hip.get(0).toParent[3][0]; poship.y = hip.get(0).toParent[3][1]; poship.z = hip.get(0).toParent[3][2]; temp.x = 1; temp.y = 0; temp.z = 0; LA.xformDir(temp, hip.get(0).toParent, temp); angleYhip = Math.atan2(-temp.z, temp.x); LA.matIdentity(matrix); LA.matYRotate(matrix, angleYhip); LA.matTranslate(matrix, poship.x, poship.y, poship.z); //LA.matInvert(hip.get(0).toParent, matrix); LA.matInvert(matrix, matrix); //LA.matIdentity(matrix); //LA.matTranslate(matrix, -poship.x, -poship.y, -poship.z); LA.matConcat(toParent, matrix, toParent); LA.matConcat(toParent, hip.get(0).toParent, matrix); pos.x = hip.get(0).toParent[3][0]; pos.y = hip.get(0).toParent[3][1]; pos.z = hip.get(0).toParent[3][2]; LA.xformPos(new cVector(), hip.get(0).toParent, pos); LA.xformPos(pos, toParent, pos); // temp.x = 1; // temp.y = 0; // temp.z = 0; // // LA.xformDir(temp, toParent, temp); // // angleY = Math.atan2(-temp.z, temp.x); // // LA.matIdentity(toParent); // LA.matYRotate(toParent, angleY - angleYhip); // LA.matTranslate(toParent, pos.x - poship.x, pos.y - poship.y, pos.z - poship.z); CameraPane.debugpointP.toParent[3][0] = pos.x; CameraPane.debugpointP.toParent[3][1] = pos.y; CameraPane.debugpointP.toParent[3][2] = pos.z; CameraPane.debugpointC.toParent[3][0] = poship.x; CameraPane.debugpointC.toParent[3][1] = poship.y; CameraPane.debugpointC.toParent[3][2] = poship.z; poship.x = toParent[3][0]; poship.y = toParent[3][1]; poship.z = toParent[3][2]; CameraPane.debugpointR.toParent[3][0] = poship.x; CameraPane.debugpointR.toParent[3][1] = poship.y; CameraPane.debugpointR.toParent[3][2] = poship.z; LA.matInvert(toParent, fromParent); } void LoadData() { float[] thedata = bvh.animation.getBoneData(0); // thedata[0] = 0; // thedata[1] = 0; // thedata[2] = 0; // thedata[3] = 0; // thedata[4] = 0; // thedata[5] = 0; if (true) return; for (int b=numbones; --b>=0;) //int b = 0; { thedata = bvh.animation.getBoneData(b); int numframes = bvh.animation.getNumFrames(); int dof = 6; // thedata.length/numframes; if (b > 0) dof = 3; if (dof != thedata.length/numframes) { System.err.println("Bone #" + b + ": dof = " + dof + " ---> " + (thedata.length/numframes)); dof = thedata.length/numframes; } //for (int f=numframes; --f>=offset;) int firstframe = GetFirstFrame(); for (int f=0; f=0;) for (int i=dof; --i>=0;) { //thedata[f3 + i] += thedata[i] - thedata[offset*dof + i]; thedata[f3 + i] = thedata[f3 + firstframe*dof + i]; } } } } void ResetZero() { for (int b=numbones; --b>=0;) { float[] thedata = bvh.animation.getBoneData(b); int numframes = bvh.animation.getNumFrames(); int dof = 6; if (b > 0) dof = 3; for (int i=dof; --i>=0;) { thedata[i] = 0; } } } void SetCurrentBones(int frame) { if (frame == 0) { //frame = 1; } if (currentbones == null) { currentbones = new double[(numbones-1)*3]; currenthip = new double[6]; } for (int b=numbones; --b>=1;) // HIP ROTATION: 0;) //int b = 0; { float[] data = bvh.animation.getBoneData(b); // int numframes = bvh.animation.getNumFrames(); int dof = 6; // data.length/numframes; if (b > 0) dof = 3; //assert(dof == 3); // System.err.println("Bone #" + b + ": dof = " + dof); int theframe = // 1; // baseframe; frame; int frame3 = theframe*dof; int bone3 = (b-1)*dof; int start = 3; int end = 0; if (dof == 6) { start = 6; //end = 3; } for (int i=start; --i>=end;) { currentbones[bone3 + i/*-end*/] = data[frame3 + i]; } } float[] data = bvh.animation.getBoneData(0); int dof = 6; int theframe = frame; int frame3 = theframe*dof; int start = 6; int end = 0; for (int i=start; --i>=end;) { currenthip[i] = data[frame3 + i]; } } static cVector centroid = new cVector(); static cVector mocaporigin = new cVector(); void SetHipOrientation() { Object3D hip = get(0); if (toParent == null) { toParent = LA.newMatrix(); fromParent = LA.newMatrix(); } LA.matConcat(toParent, hip.get(0).toParent, matrix); mocaporigin.x = matrix[3][0]; mocaporigin.y = matrix[3][1]; mocaporigin.z = matrix[3][2]; centroid.x = matrix[3][0]; centroid.y = matrix[3][1]; centroid.z = matrix[3][2]; // this.getCentroid(centroid, true); CameraPane.CreateSelectedPoint(); CameraPane.debugpointG.name = ""; CameraPane.debugpointG.toParent[3][0] = mocaporigin.x; CameraPane.debugpointG.toParent[3][1] = mocaporigin.y; CameraPane.debugpointG.toParent[3][2] = mocaporigin.z; CameraPane.debugpointP.name = ""; CameraPane.debugpointP.toParent[3][0] = centroid.x; CameraPane.debugpointP.toParent[3][1] = centroid.y; CameraPane.debugpointP.toParent[3][2] = centroid.z; cVector goal = GetGoal(centroid); if (goal != null) { System.err.println("GOAL change (" + this + "): " + goal + " (was " + goalposx + ", " + goalposz + ")"); goalposx = goal.x; goalposz = goal.z; targetdirx = targetdirz = 0; } if (goalposx == 0 && goalposz == 0) { // No target if (ScriptNode.speaker != null) { Object3D sourcenode = GetFileRoot(); Object3D speakernode = ScriptNode.speaker.GetFileRoot(); //System.err.println("speaker is " + speakernode + "; last was " + ScriptNode.lastspeaker); Object3D speaker = ScriptNode.speaker; if (speakernode == sourcenode) { if (ScriptNode.lastspeaker != null) { speakernode = ScriptNode.lastspeaker.GetFileRoot(); speaker = ScriptNode.lastspeaker; } else return; } cVector dst = new cVector(); boolean success = speakernode.getCentroid(dst); // , true); if (!success) new Exception().printStackTrace(); //speakernode.parent.TransformToWorld(dst); //sourcenode.parent.TransformToLocal(dst); this.parent.TransformToLocal(dst); goalposx = dst.x; goalposz = dst.z; } else return; } cVector temp = new cVector(); temp.x = 1; temp.y = 0; temp.z = 0; LA.xformDir(temp, matrix, temp); double angleYhip = Math.atan2(-temp.z, temp.x); double angleYtarget = Math.atan2(goalposx - centroid.x, goalposz - centroid.z); LA.matIdentity(matrix); LA.matTranslate(matrix, -centroid.x, -centroid.y, -centroid.z); double angle = CurveAngle(0, angleYtarget - angleYhip, 0.1f); LA.matYRotate(matrix, angle); LA.matTranslate(matrix, centroid.x, centroid.y, centroid.z); LA.matConcat(matrix, toParent, toParent); LA.matInvert(toParent, fromParent); LA.matConcat(toParent, hip.get(0).toParent, matrix); centroid.x = matrix[3][0]; centroid.y = matrix[3][1]; centroid.z = matrix[3][2]; double dist = LA.distance(centroid, mocaporigin); CheckForAction(centroid); } void WriteTo(java.io.Writer writer) throws Exception { writer.write("MOTION\n"); int ff = firstframe; int lf = lastframe; if (lf == 0) lf = bvh.animation.getNumFrames(); float[][] data = bvh.animation._data; writer.write("Frames: " + (lf-ff)); writer.write("\nFrame Time: 0.00833333\n"); for (int f=ff; f= 3) val = (float)Math.toDegrees(val); else val /= bvh.animation.scale; writer.write(Float.toString(val)); writer.write(" "); } } writer.write("\n"); } } String fullname; int beginframe, endframe; // transient // aout 2013 mocap.reader.BVHReader.BVHResult bvh; // transient // aout 2013 Object3D skeleton; void ExtractBigData(Object3D o) { super.ExtractBigData(o); o.savebvh = this.bvh; o.saveskeleton = this.skeleton; this.bvh = null; this.skeleton = null; } void RestoreBigData(Object3D o) { super.RestoreBigData(o); this.bvh = o.savebvh; this.skeleton = o.saveskeleton; } boolean smoothed; //boolean touched; int baseframe = 0; // playback frame int frame = 0; // real frame after dynamic offset for crowds int firstframe = 0; // for local playback and export int lastframe = 0; String wavname; double myvolume = 0; transient Wav wav; Object3D floorobject; double ground; double pinx; double pinz; double targetdirx; double targetdirz; double goalposx = 0; // -20; double goalposz = 0; // -10; boolean followpath; class FromTo { int inside; cVector from; cVector to; FromTo(cVector f, cVector t) { from = f; to = t; } } class ScriptAction { int inside; cVector from; ScriptNode script; // String filename; String[] command; int offset; // ScriptAction(cVector f, ScriptNode s, String fn) // { // from = f; // script = s; // filename = fn; // command = null; // } ScriptAction(cVector f, ScriptNode s, String[] cmd, int off) { from = f; script = s; // filename = null; command = cmd; offset = off; } } transient java.util.Vector fromtos; transient java.util.Vector scriptactions; static cVector testvect = new cVector(); void ClearPath() { if (fromtos == null) { fromtos = new java.util.Vector(); } else fromtos.clear(); } void ClearActions() { if (scriptactions == null) { scriptactions = new java.util.Vector(); } else scriptactions.clear(); } static double EPSACTION = 0.1; // 0.075; // 0.1;Came void AddFromTo(cVector from, cVector to) { if (fromtos == null) { fromtos = new java.util.Vector(); } for (int i=0; i " + to); fromtos.add(new FromTo(from,to)); } // void AddAction(cVector from, ScriptNode to, String fn) // { // if (scriptactions == null) // { // scriptactions = new java.util.Vector(); // } // //// for (int i=0; i(); } // for (int i=0; i 1; // hip orientation if (get(0).link2master) { rotateonce = 2; } int loop = 0; // 1; while (--loop>=0) { this.count++; // currently drawing itself cVector centroid = new cVector(); cVector floor = new cVector(); Object3D sourcenode = GetFileRoot(); boolean success = sourcenode.getCentroid(centroid); if (!success) { //assert(success); //new Exception().printStackTrace(); System.out.println("No centroid for " + sourcenode); } long floorid = sourcenode.getFloor(floor); //assert(floorid != 0); if (floorid == 0) { //new Exception().printStackTrace(); System.out.println("No floor for " + sourcenode); } sourcenode.Dump(true); this.count--; // currently drawing itself } } cVector GetGoal(cVector from) { if (fromtos == null) { fromtos = new java.util.Vector(); } for (int i=0; i 0) continue; fromto.inside = 100; // true; return fromto.to; } else { if (fromto.inside > 0) fromto.inside--; // = false; } } return null; } void CheckForAction(cVector from) { if (scriptactions == null) { scriptactions = new java.util.Vector(); } ScriptNode script = null; //String filename = null; String[] command = null; for (int i=0; i 0) continue; fromto.inside = 100; // true; script = fromto.script; //filename = fromto.filename; command = fromto.command; if (command != null) { //System.err.println("action command: " + command[6]); System.err.print("run command " + fromto.from + " :"); for (int c=fromto.offset; c 0) fromto.inside--; // = false; } } if (script == null) return; // if (filename != null) // { // // script file // script.stack++; // System.err.println("action file: " + filename); // try // { // script.reader = script.readers[script.stack] = new ScriptNode.ScriptReader(new BufferedReader(new FileReader(filename)), script.duration, script.linecount); // script.duration = 0; // script.linecount = 0; // } // catch(Exception e) // { // e.printStackTrace(); // System.err.println(" ******* ERROR IN ACTION COMMAND *******"); // } // } // else // { // assert(command != null); // // //script.ComputerCommand(command, 6); // } } Mocap(String s) { super(s); link2master = true; } // SET SUPPORT void linkVerticesThis(Object3D other) { support = other; } void InitIndices(Object3D input, String dataname, mocap.figure.Bone[] bones) { // assert(input.Size() == 1); input.material = null; if (input.name != null) { for (int i=bones.length; --i>=0;) { if (/*input.*/dataname.equals(bones[i].getName())) { mocap.figure.Bone bone = bones[i]; // // skip first node: translation Object3D b = input; //.get(0); b._index = bone.getIndex(); b._name = bone.getName(); b._isBone = true; // !b._name.equals("null"); input.CreateMaterial(); // private Bone _parent; b._isRoot = bone.getParent() == null; //b._isHip = false; //if (!b._isRoot) // b._isHip = bone.getParent().getParent() == null; // private Bone[] _children = new Bone[0]; // private Switch _textSwitch; // transient TransformGroup _baseTransTG // ,_transTG // ,_baseRotTG // ,_rotTG // ,_invBaseRotTG // ; // Transform3D _t1 = new Transform3D(); // Transform3D _t2 = new Transform3D(); // Transform3D _transTF = new Transform3D(); // Transform3D _tworld = new Transform3D(); // Vector3d _trans = new Vector3d(); b._dof = bone._dof; // rotation order b._numDOF = bone._numDOF; //b._translationEnabled = true; // translation can be switched off //b._rotationEnabled = true; // rotation can be switched off b._rotationType = bone._rotationType; // Euler rotation type //b._scale = bone._scale; // scale is applied to all translational components break; } } } // mocap.figure.Bone[] children = bone.getChildren(); // assert(children.length == input.Size()); for (int i = 0; i < input.Size(); i++) { //mocap.figure.Bone subbone = children[i]; Object3D subb = input.get(i); String dname = subb.name; if (subb.name != null && subb.name.equals("neck")) dname = "head"; InitIndices(subb, dname, bones); } } int CollectBones(mocap.figure.Bone b, mocap.figure.Bone[] bones, int index) { bones[index++] = b; mocap.figure.Bone[] children = b.getChildren(); for (int i = 0; i < children.length; i++) { mocap.figure.Bone bone = children[i]; index = CollectBones(bone, bones, index); } return index; } int NumBones(mocap.figure.Bone b) { int numbones = 1; mocap.figure.Bone[] children = b.getChildren(); for (int i = 0; i < children.length; i++) { mocap.figure.Bone bone = children[i]; numbones += NumBones(bone); } return numbones; } // void SetPose(mocap.figure.Bone b, mocap.figure.AnimData data, int f) // { //// if (touched) //// { //// skeleton = null; //// GetSkeleton(); //// touched = false; //// } // // b.setPose(f, data.getBoneData(b.getIndex()), null); // // mocap.figure.Bone[] children = b.getChildren(); // // for (int i = 0; i < children.length; i++) // { // mocap.figure.Bone bone = children[i]; // SetPose(bone, data, f); // } // // } void SetPose(Object3D input, mocap.figure.AnimData data, int f) { if (input._isBone) { // skip first node: translation // Object3D b = input.get(0); setPose(input, f, data.getBoneData(input._index)); //, false); //, null); } //mocap.figure.Bone[] children = b.getChildren(); for (int i = 0; i < input.Size(); i++) { Object3D bone = input.get(i); SetPose(bone, data, f); } } //mocap.figure.Bone[] bones; int numbones; transient double groundx,groundz; transient double hipx,hipz; transient long groundid; // for exact foot contact //transient boolean contactfound; // same as groundid == 0 transient int lastsoundtime; // static int currentwave = 1; // transient int mywave = 0; void SetPosition(Object3D sourcenode, cVector floor, long floorid, cVector centroid) { assert(false); //assert(CameraPane.drawMode == CameraPane.SHADOW); Object3D transformnode = new Object3D(); //sourcenode.parent; // may 2014 //LA.matIdentity(transformnode.toParent); //cVector floor = new cVector(); //cVector centroid = new cVector(); cVector tmp = new cVector(); this.count++; // currently drawing itself //boolean success = sourcenode.getFloor(floor, true); //long // june 2014 floorid = sourcenode.getFloor(floor); // , sourcenode.parent); CameraPane.debugpoints[0].name = ""; // "Floor = " + 0; // floorid; CameraPane.debugpoints[0].toParent[3][0] = floor.x; CameraPane.debugpoints[0].toParent[3][1] = floor.y; CameraPane.debugpoints[0].toParent[3][2] = floor.z; if (floorid == 0) // !success) new Exception().printStackTrace(); //LA.xformPos(floor, sourcenode.parent.toParent, floor); // Object3D hip = get(0); //success = sourcenode.getCentroid(centroid, true); boolean success = true; // june 2014 sourcenode.getCentroid(centroid); //, sourcenode.parent); this.count--; // currently drawing itself CameraPane.debugpoints[7].name = ""; // Centroid = " + 0; // groundid; // centroid; CameraPane.debugpoints[7].toParent[3][0] = centroid.x; CameraPane.debugpoints[7].toParent[3][1] = centroid.y; CameraPane.debugpoints[7].toParent[3][2] = centroid.z; if (!success) new Exception().printStackTrace(); //System.err.println("this = " + sourcenode + ": floor = " + floor.y + ", ground = " + ground); if (transformnode.toParent == null) { transformnode.toParent = LA.newMatrix(); transformnode.fromParent = LA.newMatrix(); } // hum... // LA.xformPos(centroid, sourcenode.parent.toParent, centroid); // // if (sourcenode.parent.parent != null) // sourcenode.parent.parent.TransformToWorld(centroid, centroid); // sourcenode.parent.TransformToWorld(centroid, centroid); tmp.set(centroid); // jan 2014 sourcenode.parent.TransformToWorld(tmp); CameraPane.debugpoints[1].name = ""; // "C+"; CameraPane.debugpoints[1].toParent[3][0] = tmp.x; CameraPane.debugpoints[1].toParent[3][1] = tmp.y; CameraPane.debugpoints[1].toParent[3][2] = tmp.z; // tmp.set(pinx,0,pinz); if (pinx != 0 && pinx != -123456) { tmp.x = pinx; } if (ground != -123456) { tmp.y = ground; } if (pinz != 0 && pinz != -123456) { tmp.z = pinz; } CameraPane.debugpoints[2].name = ""; // "Pin"; CameraPane.debugpoints[2].toParent[3][0] = tmp.x; CameraPane.debugpoints[2].toParent[3][1] = tmp.y; CameraPane.debugpoints[2].toParent[3][2] = tmp.z; //if (sourcenode.parent.parent != null) // jan 2014 sourcenode.parent.TransformToLocal(tmp); //, tmp); CameraPane.debugpoints[3].name = ""; // "C-"; CameraPane.debugpoints[3].toParent[3][0] = tmp.x; CameraPane.debugpoints[3].toParent[3][1] = tmp.y; CameraPane.debugpoints[3].toParent[3][2] = tmp.z; // double posx = sourcenode.parent.toParent[3][0]; // double posz = sourcenode.parent.toParent[3][2]; double w = 0.00015; //if (!sourcenode.link2master) // strong pin on floor if (parent.link2master) // strong pin on floor { w = 0.005; // .001; } if (CameraPane.FAST || CameraPane.fullreset) w = 1; // 0.05; // 1; LA.matIdentity(tempmatrix); if (pinx != 0 && pinx != -123456) { tempmatrix[3][0] = w * (tmp.x - centroid.x); // sourcenode.parent.toParent[3][0] = (pinx - centroid.x)*w + posx*(1-w); //sourcenode.parent.toParent[3][0] = pinx*w + centroid.x*(1-w); // LA.xformPos(centroid, sourcenode.parent.toParent, tmp); // groundx = (pinx + floor.x - tmp.x) * w + groundx * (1-w); groundx += tempmatrix[3][0]; hipx += tempmatrix[3][0]; } if (ground != -123456) { double w0 = w; if (Math.abs(tmp.y - floor.y) > 0.3) // floor too far away, jump to it w0 = 1; tempmatrix[3][1] = Math.sqrt(Math.sqrt(w0)) * (tmp.y - floor.y); } if (pinz != 0 && pinz != -123456) { tempmatrix[3][2] = w * (tmp.z - centroid.z); // sourcenode.parent.toParent[3][2] = (pinz - centroid.z)*w + posz*(1-w); //sourcenode.parent.toParent[3][2] = pinz*w + centroid.z*(1-w); // LA.xformPos(centroid, sourcenode.parent.toParent, tmp); // groundz = (pinz + floor.z - tmp.z) * w + groundz * (1-w); //groundz += (pinz - tmp.z) * w; //groundz += sourcenode.parent.toParent[3][2] - posz; groundz += tempmatrix[3][2]; hipz += tempmatrix[3][2]; } // LA.matConcat(tempmatrix, sourcenode.parent.toParent, sourcenode.parent.toParent); LA.matConcat(transformnode.toParent, tempmatrix, transformnode.toParent); // sourcenode.parent.TransformToWorld(tmp, tmp); double tempg = floor.y; // jan 2014 // LA.xformPos(floor, sourcenode.parent.toParent, floor); // // CameraPane.debugpoints[4].name = "F+"; // CameraPane.debugpoints[4].toParent[3][0] = floor.x; // CameraPane.debugpoints[4].toParent[3][1] = floor.y; // CameraPane.debugpoints[4].toParent[3][2] = floor.z; // // if (sourcenode.parent.parent != null) // sourcenode.parent.parent.TransformToWorld(floor); //, floor); CameraPane.debugpoints[5].name = ""; // "F++"; CameraPane.debugpoints[5].toParent[3][0] = floor.x; CameraPane.debugpoints[5].toParent[3][1] = floor.y; CameraPane.debugpoints[5].toParent[3][2] = floor.z; floor.y = tempg; CameraPane.debugpoints[6].name = ""; // "F+-"; CameraPane.debugpoints[6].toParent[3][0] = floor.x; CameraPane.debugpoints[6].toParent[3][1] = floor.y; CameraPane.debugpoints[6].toParent[3][2] = floor.z; if (pinx != -123456) { // LA.xformPos(centroid, tempmatrix, tmp); // LA.xformPos(centroid, sourcenode.parent.toParent, tmp); // groundx = (pinx + floor.x - tmp.x) * w + groundx * (1-w); } if (pinz != -123456) { // LA.xformPos(centroid, sourcenode.parent.toParent, tmp); // groundz = (pinz + floor.z - tmp.z) * w + groundz * (1-w); //groundz += (pinz - tmp.z) * w; //groundz += sourcenode.parent.toParent[3][2] - posz; } boolean footcontact = false; // CameraPane.FOOTCONTACT; // false; if (footcontact && ground != -123456) { // foot contact // double ox = groundx - floor.x; // double oz = groundz - floor.z; // // double dist2 = ox*ox + oz*oz; if (groundid != floorid) // !contactfound || dist2 > 0.0075) // .005) // .05) { groundx = floor.x; // + posx; groundz = floor.z; // + posz; groundid = floorid; // green CameraPane.debugpointG.toParent[3][0] = groundx; CameraPane.debugpointG.toParent[3][1] = floor.y; CameraPane.debugpointG.toParent[3][2] = groundz; if (true) // slow && stepout && onein) { // System.out.println("Play sound (" + dist2 + ") " + sourcenode); // if (mywave == 0) // { // mywave = currentwave++; // } // sound cVector eye = Globals.theRenderer.EyeCamera().location; // if (sourcenode.parent.parent != null) // sourcenode.parent.parent.TransformToWorld(floor, tmp); // else tmp.set(floor); tmp.sub(eye); if (groundid != 0 && wavname != null) // CameraPane.framecount - lastsoundtime > 30) // 30) // ~0.25 secs { double volume = 1 // (Math.random()+0.5) / Math.max(tmp.length2(), 1); // 0.2); double dx = hipx - centroid.x; double dz = hipz - centroid.z; double hipmotion2 = Math.sqrt(dx*dx + dz*dz); hipmotion2 *= 4; if (hipmotion2 > 1) { // System.err.println("WARNING hipmotion = " + hipmotion2); //new Exception().printStackTrace(); hipmotion2 = 1; } // System.out.print(" volume = " + volume); // System.out.println("; hipmotion = " + hipmotion2); volume *= hipmotion2; if (wav == null) wav = new Wav(wavname); double usedvolume = 1; if (myvolume != 0) usedvolume = myvolume; // System.out.println("play sound: " + (volume * usedvolume)); //GraphreeD. wav.play(volume * usedvolume); //, mywave); lastsoundtime = Globals.framecount; Grafreed.hassound = true; } // else // System.out.println("skipped"); hipx = centroid.x; hipz = centroid.z; //stepout = false; } //contactfound = true; } // floor.x += posx; // floor.z += posz; // System.out.println("Allo " + sourcenode); // purple CameraPane.debugpointP.toParent[3][0] = floor.x;// + posx; CameraPane.debugpointP.toParent[3][1] = ground; // floor.y;// + sourcenode.parent.toParent[3][1]; CameraPane.debugpointP.toParent[3][2] = floor.z;// + posz; if (false) // dist2 > 0.1) { groundx = floor.x; groundz = floor.z; } double oldposx = transformnode.toParent[3][0]; double oldposy = transformnode.toParent[3][1]; double oldposz = transformnode.toParent[3][2]; double K = 1; // 0.25; // 1; // 0.05; // LA.matIdentity(tempmatrix); // tempmatrix[3][0] = K * (groundx - floor.x); // tempmatrix[3][1] = K * (ground - floor.y); // tempmatrix[3][2] = K * (groundz - floor.z); // // LA.matConcat(tempmatrix, sourcenode.parent.toParent, sourcenode.parent.toParent); // sourcenode.parent.toParent[3][0] += groundx - floor.x; // sourcenode.parent.toParent[3][1] = ground - floor.y; // sourcenode.parent.toParent[3][2] += groundz - floor.z; // sourcenode.parent.toParent[3][1] = K * (ground - floor.y) + (1 - K) * oldposy; if (footcontact && (sourcenode.parent != null && !sourcenode.parent.link2master)) // strong pin on floor { v0.x = groundx - floor.x; v0.y = 0; // groundx - floor.x; v0.z = groundz - floor.z; double distance = Math.sqrt(v0.dot(v0)); double maxdist = 0.04; if (distance > maxdist) { // probably wrong foot K = 0; // maxdist/distance; } //K = 1; transformnode.toParent[3][0] = K * (oldposx + v0.x) + (1 - K) * oldposx; transformnode.toParent[3][2] = K * (oldposz + v0.z) + (1 - K) * oldposz; } } LA.matInvert(transformnode.toParent, transformnode.fromParent); if (transformnode != sourcenode.parent) { //LA.matConcat(transformnode.toParent, sourcenode.toParent, sourcenode.toParent); // ????????? sourcenode = this; // may 2014 if (sourcenode.toParent == null) { // june 2014 sourcenode.toParent = LA.newMatrix(); sourcenode.fromParent = LA.newMatrix(); } v0.x = v0.z = 0; this.get(0).TransformToWorld(v0); // cyan CameraPane.debugpointC.toParent[3][0] = v0.x; CameraPane.debugpointC.toParent[3][1] = ground; CameraPane.debugpointC.toParent[3][2] = v0.z; LA.matConcat(sourcenode.toParent, transformnode.toParent, sourcenode.toParent); LA.matInvert(sourcenode.toParent, sourcenode.fromParent); v0.x = v0.z = 0; this.get(0).TransformToWorld(v0); // red CameraPane.debugpointR.toParent[3][0] = v0.x; CameraPane.debugpointR.toParent[3][1] = ground; CameraPane.debugpointR.toParent[3][2] = v0.z; } } void SetRotation(Object3D sourcenode, cVector floor, cVector centroid) { _t1.setIdentity(); Object3D rotsourcenode = sourcenode; // get(0).get(0); // Object3D tempobj = new Object3D(); // LA.matCopy(rotsourcenode.toParent, tempobj.toParent); // cJ3D.ResetTransform(get(0).get(0), _t1); cVector src = new cVector(); cVector dst = new cVector(); cVector orig = new cVector(); cVector test = new cVector(); //cVector euler = new cVector(); //cVector floor = new cVector(); //cVector centroid = new cVector(); // this.count++; // currently drawing itself // // //boolean success = sourcenode.getFloor(floor, true); // long floorid = sourcenode.getFloor(floor); // , sourcenode.parent); // assert(floorid != 0); // boolean success = sourcenode.getCentroid(centroid); // , sourcenode.parent); // assert(success); // // this.count--; // currently drawing itself centroid.y = floor.y; // 0; cVector goal = GetGoal(centroid); if (goal != null) { System.err.println("GOAL change (" + sourcenode + "): " + goal + " (was " + goalposx + ", " + goalposz + ")"); //sourcenode.parent.parent.Dump(); goalposx = goal.x; goalposz = goal.z; targetdirx = targetdirz = 0; followpath = true; } else { // if (followpath) // { // goalx = goalz = 0; // followpath = false; // } } sourcenode.TransformToLocal(floor); // sourcenode.parent.TransformToLocal(floor); // dst.x = 0; // dst.y = 0; // dst.z = -1; Object3D speakernode = null; if (ScriptNode.speaker != null) { speakernode = ScriptNode.speaker.GetFileRoot(); boolean uselast = false; if (speakernode == sourcenode && ScriptNode.lastspeaker != null) { speakernode = ScriptNode.lastspeaker.GetFileRoot(); uselast = true; } boolean success = speakernode.getCentroid(dst, true); if (!success) new Exception().printStackTrace(); speakernode.parent.TransformToWorld(dst); //, dst); // aout 2013 sourcenode.parent.TransformToLocal(dst); //, dst); // LA.xformPos(dst, speakernode.parent.toParent, dst); // // if (speakernode.parent.parent != null) // aout 2013 // { // speakernode.parent.parent.TransformToWorld(dst, dst); //// LA.xformPos(dst, speakernode.parent.parent.toParent, dst); // } if (false) // !uselast) { CameraPane.debugpointG.toParent[3][0] = dst.x; CameraPane.debugpointG.toParent[3][1] = dst.y; CameraPane.debugpointG.toParent[3][2] = dst.z; } } if (goalposx != 0 || goalposz != 0) // TODO { // overwrite speaker orientation dst.set(goalposx,0,goalposz); if (sourcenode.parent != null) // july 2014 sourcenode.parent. TransformToLocal(dst); } //LA.xformDir(temp, rot.toParent, temp); // LA.xformPos(dst, fromParent, dst); // src.x = 0; // src.y = 0; // src.z = 0; src.x = 0; src.y = 0; src.z = 0; LA.xformPos(src, get(0).get(0).toParent, src); LA.xformPos(src, get(0).toParent, src); LA.xformPos(src, toParent, src); // LA.xformPos(src, rotsourcenode.toParent, src); orig.set(src); // rotsourcenode.TransformToWorld(orig); if (rotsourcenode != this) LA.xformPos(src, rotsourcenode.toParent, src); // CameraPane.debugpoints[4].name = "ORIGIN"; // CameraPane.debugpoints[4].toParent[3][0] = orig.x; // CameraPane.debugpoints[4].toParent[3][1] = orig.y; // CameraPane.debugpoints[4].toParent[3][2] = orig.z; // orig.set(src); if (false) // aout 2013 rotsourcenode.parent != null) { LA.xformPos(src, rotsourcenode.parent.toParent, src); if (rotsourcenode.parent.parent != null) // aout 2013 { rotsourcenode.parent.parent.TransformToWorld(src); //, src); //LA.xformPos(src, rot.parent.parent.toParent, src); } } else { // new Exception().printStackTrace(); } // mars 2014 if (false) // goalx != 0 || goalz != 0) { targetdirx = dst.x - src.x; targetdirz = dst.z - src.z; if (Math.abs(targetdirx) > 0.1 || Math.abs(targetdirz) > 0.1) { // far enough from goal. keep the goal position. targetdirx = 0; targetdirz = 0; } else { // too close to goal. switch to target direction instead. goalposx = 0; goalposz = 0; } } //sourcenode.getCentroid(src, true); if (speakernode == sourcenode && goalposx == 0 && goalposz == 0) { if (ScriptNode.lastspeaker != null) new Exception().printStackTrace(); return; } // LA.xformPos(src, fromParent, src); if ((ScriptNode.speaker != null && CameraPane.SPEAKERMOCAP || goalposx != 0 || goalposz != 0) && targetdirx == 0 && targetdirz == 0) dst.sub(src); else // vector mode { dst.x = targetdirx; dst.z = targetdirz; // TEST TARGET // dst.x = CameraPane.selectedpoint.toParent[3][0]; // dst.y = CameraPane.selectedpoint.toParent[3][1]; // dst.z = CameraPane.selectedpoint.toParent[3][2]; } if (dst.x == 0 && dst.z == 0) { return; } double angleY = Math.atan2(dst.x, dst.z); // src.x = 0; // src.y = 1; // src.z = 0; // LA.xformDir(src, rot.toParent, src); // // double angleX = Math.atan2(-src.x, src.y); if (rotsourcenode.toParent == null) { rotsourcenode.toParent = LA.newMatrix(); rotsourcenode.fromParent = LA.newMatrix(); } double tx = rotsourcenode.toParent[3][0]; double ty = rotsourcenode.toParent[3][1]; double tz = rotsourcenode.toParent[3][2]; // dst.x = 0; // dst.y = 0; // dst.z = 0; // LA.xformPos(dst, get(0).get(0).get(0).get(0).get(0).get(0).get(0).toParent, dst); // LA.xformPos(dst, get(0).get(0).get(0).get(0).get(0).get(0).toParent, dst); // LA.xformPos(dst, get(0).get(0).get(0).get(0).get(0).toParent, dst); // LA.xformPos(dst, get(0).get(0).get(0).get(0).toParent, dst); // LA.xformPos(dst, get(0).get(0).get(0).toParent, dst); // LA.xformPos(dst, get(0).get(0).toParent, dst); // LA.xformPos(dst, get(0).toParent, dst); // hip // LA.xformPos(dst, toParent, dst); dst.set(floor); // rotation center test.set(dst); rotsourcenode.TransformToWorld(test); CameraPane.debugpoints[4].name = ""; // "ROT = "; // + test; CameraPane.debugpoints[4].toParent[3][0] = test.x; CameraPane.debugpoints[4].toParent[3][1] = test.y; CameraPane.debugpoints[4].toParent[3][2] = test.z; // LA.xformPos(src, rot.toParent, src); // tx = src.x; // ty = src.y; // tz = src.z; // assert(tx == 0); // assert(ty == 0); // assert(tz == 0); // _trans.x = src.x; // _trans.y = src.y; // _trans.z = src.z; // // _t2.setIdentity(); // _t2.setTranslation(_trans); // // _t1.mul(_t2); src.x = 1; src.y = 0; src.z = 0; LA.xformDir(src, get(0).get(0).toParent, src); // LA.xformDir(src, get(0).toParent, src); // LA.xformDir(src, toParent, src); double angleDY = Math.atan2(-src.z, src.x); src.x = 1; src.y = 0; src.z = 0; LA.xformDir(src, toParent, src); double angleRY = Math.atan2(-src.z, src.x); // 0 double targetangle = angleY - (angleRY + angleDY); src.x = 1; src.y = 0; src.z = 0; LA.xformDir(src, rotsourcenode.toParent, src); double currentangle = Math.atan2(-src.z, src.x); if (ScriptNode.speaker == null && dst.x == 0 && dst.z == 0 || postponeangle != 0) targetangle = currentangle; _t2.setIdentity(); // while (targetangle < -Math.PI) // targetangle += 2*Math.PI; // while (currentangle < -Math.PI) // currentangle += 2*Math.PI; // while (targetangle > Math.PI) // targetangle -= 2*Math.PI; // while (currentangle > Math.PI) // currentangle -= 2*Math.PI; double sinc = Math.sin(currentangle/2); double sint = Math.sin(targetangle/2); double cosc = Math.cos(currentangle/2); double cost = Math.cos(targetangle/2); double K = 0.01; // 0.005; // TARGET ROTATION SPEED // if (sourcenode.link2master) // if (goalx != 0 || goalz != 0) // K = 0.02; // .0625; if (parent.link2master) // strong pin on floor { K = 0.05; // 0.02; } if (rotateonce > 0 || CameraPane.FAST) K = 1; // 0.25; // 1; // *= 16; rotateonce--; double finalangle = Math.atan2(sinc*(1-K) + sint*K, cosc*(1-K) + cost*K); // _t2.rotY(finalangle*2); if (postponeangle != 0) { //System.err.println("sourcenode = " + sourcenode); //System.err.println("currentangle = " + currentangle); //System.err.println("targetangle = " + targetangle); //System.err.println("postponeangle = " + postponeangle); //_t2.rotY(CurveAngle(currentangle, targetangle, K)); _t2.rotY(targetangle); // + postponeangle); // _t2.rotY(postponeangle); postponeangle = 0; } else { double angle = CurveAngle(currentangle, targetangle, K); //System.err.println("angle = " + angle); _t2.rotY(angle); } // _t2.rotY(currentangle*K + targetangle*(1-K)); // _t2.rotX(angleX); orig.set(dst); if (true) // false) { src.set(dst); LA.xformPos(src, rotsourcenode.toParent, src); // LA.matCopy(rotsourcenode.toParent, matrix); test.set(src); cJ3D.ResetTransform(rotsourcenode, _t2, false); // LA.matInvert(rotsourcenode.toParent, matrix2); // LA.matConcat(matrix, matrix2, matrix); LA.xformPos(dst, rotsourcenode.toParent, dst); tx = orig.x; // rotsourcenode.toParent[3][0]; // ty = rotsourcenode.toParent[3][1]; tz = orig.z; // rotsourcenode.toParent[3][2]; //if (!sourcenode.link2master) // pin on floor { // mar 2013 tx = src.x - dst.x; // ty = src.y - dst.y; tz = src.z - dst.z; // } //else { // tx -= dst.x; // ty -= dst.y; // tz -= dst.z; } // javax.vecmath.Matrix4d m = new javax.vecmath.Matrix4d(); // // m.m00 = rot.toParent[0][0]; // m.m10 = rot.toParent[0][1]; // m.m20 = rot.toParent[0][2]; // m.m30 = rot.toParent[0][3]; // m.m01 = rot.toParent[1][0]; // m.m11 = rot.toParent[1][1]; // m.m21 = rot.toParent[1][2]; // m.m31 = rot.toParent[1][3]; // m.m02 = rot.toParent[2][0]; // m.m12 = rot.toParent[2][1]; // m.m22 = rot.toParent[2][2]; // m.m32 = rot.toParent[2][3]; // m.m03 = rot.toParent[3][0]; // m.m13 = rot.toParent[3][1]; // m.m23 = rot.toParent[3][2]; // m.m33 = rot.toParent[3][3]; // // _t1.set(m); // _t1.mul(_t2); // // _t1.get(m); // // tx = m.m03; // ty = m.m13; // tz = m.m23; // // _t2.setIdentity(); // _t2.rotY(angleY - (angleRY + angleDY)); // _trans.x = -src.x; // _trans.y = -src.y; // _trans.z = -src.z; // // _t2.setIdentity(); // _t2.setTranslation(_trans); // // _t1.mul(_t2); //matrixToEulerXYZ(rot.toParent, euler); // cJ3D.ResetTransform(rot, _t2); // // LA.matIdentity(matrix); // LA.matTranslate(matrix, -tx, 0, -tz); // // LA.matConcat(rotsourcenode.toParent, matrix, rotsourcenode.toParent); // // LA.matIdentity(matrix); // LA.matTranslate(matrix, tx, 0, tz); // // LA.matConcat(matrix, rotsourcenode.toParent, rotsourcenode.toParent); //// jan 2014 //// rotsourcenode.toParent[3][0] = tx; // rotsourcenode.toParent[3][1] = ty; //// rotsourcenode.toParent[3][2] = tz; LA.matIdentity(matrix); LA.matTranslate(matrix, tx, ty, tz); // ????????? LA.matConcat(rotsourcenode.toParent, matrix, rotsourcenode.toParent); // LA.matConcat(matrix, rotsourcenode.toParent, rotsourcenode.toParent); // LA.xformPos(orig, rotsourcenode.toParent, orig); if (!orig.toString().equals(test.toString())) new Exception().printStackTrace(); LA.matInvert(rotsourcenode.toParent, rotsourcenode.fromParent); //LA.matConcat(transformnode.toParent, sourcenode.toParent, sourcenode.toParent); // ????????? if (toParent == null) { toParent = LA.newMatrix(); fromParent = LA.newMatrix(); } LA.matConcat(toParent, rotsourcenode.toParent, toParent); LA.matInvert(toParent, fromParent); LA.matIdentity(rotsourcenode.toParent); LA.matIdentity(rotsourcenode.fromParent); } else { if (transformnode == null) { transformnode = new Object3D(); } cJ3D.ResetTransform(transformnode, _t2, false); tx = orig.x; // rotsourcenode.toParent[3][0]; // ty = rotsourcenode.toParent[3][1]; tz = orig.z; // rotsourcenode.toParent[3][2]; LA.matIdentity(matrix); LA.matTranslate(matrix, -tx, 0, -tz); LA.matConcat(rotsourcenode.parent.toParent, matrix, rotsourcenode.parent.toParent); LA.matConcat(rotsourcenode.parent.toParent, transformnode.toParent, rotsourcenode.parent.toParent); LA.matIdentity(matrix); LA.matTranslate(matrix, tx, 0, tz); LA.matConcat(rotsourcenode.parent.toParent, matrix, rotsourcenode.parent.toParent); LA.matInvert(rotsourcenode.parent.toParent, rotsourcenode.parent.fromParent); } } static double[] trackedarray; static int trackedindex; static double trackedvalue; static DebugThread debugthread; static class DebugThread extends Thread { public void run() { for (;;) { try { CheckBreakPoint(); sleep(1); } catch (Exception e) { e.printStackTrace(); } } } } static void SetBreakPoint(double[] ta, int ti) { //if (debugthread != null) // return; trackedarray = ta; trackedindex = ti; if (ta != null) { trackedvalue = ta[ti]; if (debugthread != null) debugthread.stop(); (debugthread = new DebugThread()).start(); } } static void CheckBreakPoint() { if (trackedarray == null) return; if (Math.abs(trackedarray[trackedindex] - trackedvalue) > 1) { trackedvalue = trackedarray[trackedindex]; return; } } Object3D GetSkeleton() { // CheckBreakPoint(); // if (bvh.skeleton == null) // { // bvh.reader._indexCounter = 0; // // bvh.skeleton = bvh.reader.processJoint(bvh.joints, null, bvh.joints.findMax(0) / 15, false); // // touched = true; // } try { if (skeleton == null) // size() == 0) { // if (bvh == null) // ReadBVH(); assert (bvh.skeleton == null); //if (bvh.skeleton == null) { bvh.reader._indexCounter = 0; bvh.skeleton = bvh.reader.processJoint(bvh.joints, null, bvh.joints.findMax(0) / 15, true); // false); // touched = true; } //addChild(new cJ3D().Read(bvh.skeleton.getBaseTransTG())); skeleton = new cJ3D().Read(bvh.skeleton.getBaseTransTG()); mocap.figure.Bone[] bones = new mocap.figure.Bone[numbones = NumBones(bvh.skeleton)]; int index = CollectBones(bvh.skeleton, bones, 0); assert(index == bones.length); InitIndices(skeleton, skeleton.name, bones); skeleton.parent = this; // dec 2012 // // aout 2013 // for (int i=8; --i>=0;) // SmoothAnimData(); // much reduces shakiness } if (!smoothed) for (int i=10; --i>=0;) SmoothAnimData(); // much reduces shakiness smoothed = true; } catch (Exception e) { e.printStackTrace(); } return skeleton; } // void setPose(int f) // { // GetSkeleton(); // // //bvh.skeleton.setPose(f, bvh.animation.getBoneData(bvh.skeleton.getIndex()), null); // SetPose(bvh.skeleton, bvh.animation, f); // // //new cJ3D().ResetTransform(get(0), bvh.skeleton.getBaseTransTG()); // new cJ3D().ResetTransform(GetSkeleton(), bvh.skeleton.getBaseTransTG()); // } static Vector2d fromVector = new Vector2d(); static Vector2d toVector = new Vector2d(); static Vector2d vec = new Vector2d(); static double CurveAngle(double from, double to, double step) { if (step == 0) return from; if (from == to || step == 1) return to; fromVector.x = LA.cos(from); fromVector.y = LA.sin(from); toVector.x = LA.cos(to); toVector.y = LA.sin(to); double fromA = Math.atan2(fromVector.y, fromVector.x); double toA = Math.atan2(toVector.y, toVector.x); Vector2d currentVector = Slerp(fromVector, toVector, step); double angle = Math.atan2(currentVector.y, currentVector.x); double angle2 = (1-step) * fromA + step * toA; return angle; } public static Vector2d Slerp(Vector2d from, Vector2d to, double step) { double cosa = from.dot(to); if (cosa >= 1) return to; double theta = Math.acos(cosa); if (theta == 0) return to; double sinTheta = LA.sin(theta); if (sinTheta == 0) return to; double A = LA.sin((1 - step) * theta) / sinTheta; double B = LA.sin(step * theta) / sinTheta; // return A * from + B * to; vec.x = A*from.x + B*to.x; vec.y = A*from.y + B*to.y; return vec; } double postponeangle; int rotateonce; void setPose(int f) { // if (frame == f) // return; // june 2014 // System.err.println("setPose Mocap frame # " + frame); // System.err.println("setPose Mocap baseframe # " + baseframe); // System.err.println("setPose Mocap new frame # " + f); // baseframe = // !! frame = f; if (!live) // aout 2013 return; //bvh.skeleton.setPose(f, bvh.animation.getBoneData(bvh.skeleton.getIndex()), null); try { SetPose(GetSkeleton(), bvh.animation, f); SetCurrentBones(frame); } catch (Exception e) { e.printStackTrace(); } SetHipOrientation(); if (true) return; cVector centroid = new cVector(); cVector floor = new cVector(); Object3D sourcenode = GetFileRoot(); boolean success; // = sourcenode.getCentroid(centroid); // if (!success) // { // //assert(success); // //new Exception().printStackTrace(); // System.out.println("No centroid for " + sourcenode); // } if (sourcenode.parent == null && sourcenode.fileparent == null) { // No GFD // july 2014 return; } // if (sourcenode.name.contains("turtle")) // { // SetBreakPoint(get(0).toParent[0], 0); // } boolean setrotation = get(0).link2master && !firsttimeafterload && frame > 0; boolean setposition = sourcenode != null && // sourcenode.parent != null && // july 2014 sourcenode./*parent.*/link2master && !firsttimeafterload && frame > 0; // cVector centroid = new cVector(); // cVector floor = new cVector(); long floorid = 0; if (setrotation || setposition) { this.count++; // currently drawing itself //boolean success = sourcenode.getCentroid(centroid); if (!success) { //assert(success); //new Exception().printStackTrace(); System.out.println("No centroid for " + sourcenode); } floorid = sourcenode.getFloor(floor); //assert(floorid != 0); if (floorid == 0) { //new Exception().printStackTrace(); System.out.println("No floor for " + sourcenode); } this.count--; // currently drawing itself } centroid.y = floor.y; //new cJ3D().ResetTransform(get(0), bvh.skeleton.getBaseTransTG()); // new cJ3D().ResetTransform(GetSkeleton(), bvh.skeleton.getBaseTransTG()); if (setrotation) { SetRotation(sourcenode, new cVector(floor), new cVector(centroid)); } if (setposition) { if (floorobject != null) { float g = floorobject.GetGround(new cVector(floor)); if (g != Float.NEGATIVE_INFINITY) { //System.out.println("Ground of " + sourcenode + " = " + g + "(was " + ground + ")"); ground = g; } } SetPosition(sourcenode, new cVector(floor), floorid, new cVector(centroid)); } CheckForAction(centroid); } /** * 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); // } // } // } // } // } private static double getMatrixElem(double[][] mat, int index) { int i = index % 3; int j = index / 3; return mat[j][i]; } /** * MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html */ private static boolean matrixToEulerXYZ(double[][] mat, cVector xyz) { // // rot = cy*cz -cy*sz sy // // cz*sx*sy+cx*sz cx*cz-sx*sy*sz -cy*sx // // -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy // if (getMatrixElem(mat, 2) < 1.0f) { if (getMatrixElem(mat, 2) > -1.0f) { xyz.x = (float) Math.atan2(-getMatrixElem(mat, 5), getMatrixElem(mat, 8)); xyz.y = (float) Math.asin(getMatrixElem(mat, 2)); xyz.z = (float) Math.atan2(-getMatrixElem(mat, 1), getMatrixElem(mat, 0)); return true; } else { // WARNING. Not unique. XA - ZA = -atan2(r10,r11) xyz.x = -(float) Math.atan2(getMatrixElem(mat, 3), getMatrixElem(mat, 4)); xyz.y = -Math.PI/2; xyz.z = 0.0f; return false; } } else { // WARNING. Not unique. XAngle + ZAngle = atan2(r10,r11) xyz.x = (float) Math.atan2(getMatrixElem(mat, 3), getMatrixElem(mat, 4)); xyz.y = Math.PI/2; xyz.z = 0.0f; } return false; } static //transient Transform3D _t1 = new Transform3D(); static //transient Transform3D _t2 = new Transform3D(); static //transient Transform3D _transTF = new Transform3D(); static //transient Transform3D _tworld = new Transform3D(); static //transient Vector3d _trans = new Vector3d(); static //transient Vector3d _trans0 = new Vector3d(); /** * Sets this bone/joint to the translation/rotation of the given frame. * Called in the animation loop. * * @param frame * Frame number */ public void setPose(Object3D bone, int frame, float[] data) //, boolean fade) //, Point3d offsetTrans) { if (!live) // aout 2013 return; cVector temp = new cVector(); cVector temp0 = new cVector(); double pos[] = new double[3]; double angleY = 0; double sinY = 0; double cosY = 0; if (false) // bone._isRoot && firsttimeafterload && frame > 0) { bone.get(0).GlobalTransformInv(); temp.x = bone.get(0).globalTransform[3][0]; temp.y = bone.get(0).globalTransform[3][1]; temp.z = bone.get(0).globalTransform[3][2]; pos[0] = temp.x; pos[1] = temp.y; pos[2] = temp.z; temp.x = 1; temp.y = 0; temp.z = 0; LA.xformDir(temp, bone.get(0).globalTransform, temp); sinY = temp.z; cosY = temp.x; angleY = Math.atan2(-sinY, cosY); } // Object3D obj = get(0); // Object3D gp = this.parent; // this.parent = null; // obj.GlobalTransformInv(); // this.parent = gp; // // CameraPane.selectedpoint.toParent[3][0] = obj.globalTransform[3][0]; // CameraPane.selectedpoint.toParent[3][1] = obj.globalTransform[3][1]; // CameraPane.selectedpoint.toParent[3][2] = obj.globalTransform[3][2]; // System.out.println("setPose " + frame); // _lastFrame = frame; // do something if joint has at least one DOF if (bone._numDOF > 0) { int offset = frame * bone._numDOF; //if (offset + bone._numDOF - 1 < data.length) { boolean hasTranslation = false; if (bone._isRoot) { _trans.set(0, 0, 0); } _t1.setIdentity(); float boost = 1; // .2f; // go through DOF in the specified order for (int i = 0; i < bone._numDOF; i++) { boolean isRotation = false; switch (bone._dof[i]) { case RX: _t2.rotX(data[offset + i] * boost); bone.rx = data[offset + i]; isRotation = true; break; case RY: double angle = data[offset + i]; // if (bone._isRoot && !firstframe && frame > 0) // { // // Object3D rot = bone.get(0); // // // // _t2.setIdentity(); // // // // double tx = rot.toParent[3][0]; // // double ty = rot.toParent[3][1]; // // double tz = rot.toParent[3][2]; // // temp.x = 1; // temp.y = 0; // temp.z = 0; // //LA.xformDir(temp, rot.toParent, temp); // // // double // angle = Math.atan2(-temp.z, temp.x); // // // _t2.rotY(angle); // // // // cJ3D.ResetTransform(rot, _t2); // // // // rot.toParent[3][0] = tx; // // rot.toParent[3][1] = ty; // // rot.toParent[3][2] = tz; // } //continue; _t2.rotY(angle * boost); bone.ry = angle; isRotation = true; break; case RZ: _t2.rotZ(data[offset + i] * boost); bone.rz = data[offset + i]; isRotation = true; break; case TX: _trans.x += //bone._scale * data[offset + i]; bone.tx = data[offset + i]; hasTranslation = true; //isRotation = false; break; case TY: _trans.y += //bone._scale * data[offset + i]; bone.ty = data[offset + i]; hasTranslation = true; //isRotation = false; break; case TZ: _trans.z += //bone._scale * data[offset + i]; bone.tz = data[offset + i]; hasTranslation = true; //isRotation = false; break; } // multiply rotations if (isRotation) { // fixed axes: multiply from left if (bone._rotationType == FIXED_AXES) { if (i > 0) { _t2.mul(_t1); } _t1.set(_t2); // = new Transform3D(_t2); } else { // moving axes: multiply from right _t1.mul(_t2); _t2.setIdentity(); } } } if (bone._isRoot && firsttimeafterload && frame > 0) { assert(bone == get(0)); } // set rotation //if (bone._rotationEnabled) if (//frame > 0 && !bone.skipmocap && !bone.name.contains("head") && !bone.name.contains("Head") // && // !bone.name.contains("rFoot") && // !bone.name.contains("lFoot") && // !bone.name.contains("Pinky") && // !bone.name.contains("Ring") && // !bone.name.contains("Mid") && // !bone.name.contains("Index") && // !bone.name.contains("Thumb") ) { //_rotTG.setTransform(_t1); if (!bone.name.contains("abdomen")) { cJ3D.ResetTransform(bone.get(0), _t1, false); // skip translation node } else { // if (get(0).link2master) // "hip" rotation // _t1.setIdentity(); cJ3D.ResetTransform(bone.get(0), _t1, false); // skip translation node } } // 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 (bone._isRoot && link2master) // && !GetFileRoot().link2master) { if (hasTranslation) // && _translationEnabled) { // CROWD //cJ3D.GetTranslation(bone, _trans0); //_trans.add(_trans0); //_transTF.setIdentity(); _transTF.set(_t1); _transTF.setTranslation(_trans); // System.err.println("Translate: " + _transTF); //_transTG.setTransform(_transTF); cJ3D.ResetTransform(bone.get(0), _transTF, true); // translation node //cJ3D.ResetTransform(bone, _transTF, true); // translation node } } if (false) // bone._isRoot && firsttimeafterload && frame > 0) { assert(bone == get(0)); // LA.matConcat(bone.toParent, bone.get(0).toParent, tempmatrix); // B' * C' // // // find A' : A'B'C' == ABC // LA.matInvert(tempmatrix, tempmatrixinv); // LA.matConcat(originmatrix, tempmatrixinv, toParent); // ABC * (B'*C')^-1 // // LA.matConcat(bone.fromParent, bone.get(0).fromParent, tempmatrix); // B' * C' // // LA.matInvert(tempmatrix, tempmatrixinv); // LA.matConcat(originmatrixinv, tempmatrixinv, fromParent); // ABC * (B'*C')^-1 // Invariants(); bone.Invariants(); bone.get(0).Invariants(); // if (origin == null) // origin = null; origin.Invariants(); origin0.Invariants(); //?? LA.matConcat(bone.get(0).fromParent, bone.fromParent, tempmatrix); // B' * C' //?? LA.matConcat(origin.toParent, tempmatrix, toParent); // matrixToEulerXYZ(toParent, temp); // matrixToEulerXYZ(bone.get(0).toParent, temp); // //// Quaternion quat = new Quaternion(); //// float[] angles = new float[3]; //// angles[0] = (float)-temp.x; //// angles[1] = (float)-temp.y; //// angles[2] = (float)-temp.z; //// quat.fromAngles(angles); //// quat.toAngles(angles); // _t1.setIdentity(); // _t2.setIdentity(); // _t2.rotZ(-temp.z); // _t1.mul(_t2); // _t2.setIdentity(); // _t2.rotY(-temp.y); // _t1.mul(_t2); // _t2.setIdentity(); // _t2.rotX(-temp.x); // _t1.mul(_t2); // _t2.setIdentity(); // // // double[][] rot = bone.get(0). // // toParent; // // Quat4d quat = new Quat4d(); // Matrix4d mat4d = new Matrix4d(); // // _t1.get(mat4d); // // mat4d.get(quat); if (false) // hip correction { _t2.setIdentity(); double tx = toParent[3][0]; double ty = toParent[3][1]; double tz = toParent[3][2]; temp.x = 1; temp.y = 0; temp.z = 0; LA.xformDir(temp, toParent, temp); double angle = Math.atan2(-temp.z, temp.x); // HIP ROTATION!!! _t2.rotY(angle); postponeangle = angle; // _t1.setIdentity(); // _t1.rotX(temp.x); // _t2.mul(_t1); // // _t1.setIdentity(); // _t1.rotY(temp.y); // _t2.mul(_t1); // // _t1.setIdentity(); // _t1.rotZ(temp.z); // _t2.mul(_t1); cJ3D.ResetTransform(this, _t2, false); toParent[3][0] = tx; toParent[3][1] = ty; toParent[3][2] = tz; } //????? LA.matConcat(bone.get(0).toParent, bone.toParent, tempmatrix); // B' * C' // LA.matConcat(origin.fromParent, tempmatrix, fromParent); //?? LA.matInvert(toParent, fromParent); Invariants(); bone.Invariants(); bone.get(0).Invariants(); origin.Invariants(); origin0.Invariants(); // LA.matInvert(fromParent, tempmatrix); // //// LA.matConcat(originmatrix0, bone.fromParent, tempmatrix); //// toParent[3][0] = tempmatrix[3][0]; //// toParent[3][1] = tempmatrix[3][1]; //// toParent[3][2] = tempmatrix[3][2]; //// LA.matConcat(originmatrixinv0, bone.toParent, tempmatrix); //// fromParent[3][0] = tempmatrix[3][0]; //// fromParent[3][1] = tempmatrix[3][1]; //// fromParent[3][2] = tempmatrix[3][2]; // LA.matConcat(originmatrix0, bone.fromParent, toParent); // LA.matConcat(originmatrixinv0, bone.toParent, fromParent); firsttimeafterload = false; // temp.x = data[0]; // temp.y = data[1]; // temp.z = data[2]; LA.xformPos(temp, origin0.toParent, temp0); // NU // bone.get(0).GlobalTransformInv(); // // temp0.x = bone.get(0).globalTransform[3][0]; // temp0.y = bone.get(0).globalTransform[3][1]; // temp0.z = bone.get(0).globalTransform[3][2]; // // MEGA TEST // june 2014: necessary for precision? otherwise comment works //fromParent[3][0] = 0; //fromParent[3][1] = 0; //fromParent[3][2] = 0; //LA.matInvert(fromParent, toParent); LA.xformPos(temp0, fromParent, temp); // NU //LA.xformPos(temp, bone.fromParent, temp); // float[] data; // pos[0] = temp.x; // pos[1] = temp.y; // pos[2] = temp.z; // // temp.x = 1; // temp.y = 0; // temp.z = 0; // // LA.xformDir(temp, bone.get(0).globalTransform, temp); // CameraPane.selectedpoint.toParent[3][0] = temp0.x; // CameraPane.selectedpoint.toParent[3][1] = temp0.y; // CameraPane.selectedpoint.toParent[3][2] = temp0.z; int numframes = bvh.animation.getNumFrames(); int b = 0; { assert(data == bvh.animation.getBoneData(b)); int dof = 6; /// data.length/numframes; // july 2014 if (dof != data.length/numframes) { System.err.println("Bone #" + b + ": dof = " + dof + " ---> " + (data.length/numframes)); dof = data.length/numframes; } int theframe = 0; // baseframe; int frame3 = theframe*dof; // for (int f=theframe; --f>=0;) // { // int f3 = f*dof; // // for (int i=dof/*3!*/; --i>=0;) // { // data[f3 + i] += pos[i] - data[frame3 + i]; // } // } for (int f=numframes; --f>=theframe;) { int f3 = f*dof; if (f == theframe) f = theframe; // temp.x = data[f3]; // temp.y = data[f3 + 1]; // temp.z = data[f3 + 2]; // LA.xformPos(temp, originmatrix0, temp); // LA.xformPos(temp, fromParent, temp); // data[f3] = (float)temp.x; // data[f3 + 1] = (float)temp.y; // data[f3 + 2] = (float)temp.z; for (int i=3/*dof*/; --i>=0;) { //if (i == 4) // continue; // if (i >= 4) // { // while (data[frame3 + i] < 0) // { // data[frame3 + i] += Math.PI*2; // } // while (data[f3 + i] < 0) // { // data[f3 + i] += Math.PI*2; // } // while (pos[i] < 0) // { // pos[i] += Math.PI*2; // } // } data[f3 + i] += /*pos[i]*/ - data[frame3 + i]; // if (f < 3) // { // System.out.print(data[f3 + i] + " "); // } } double t1 = data[f3 + 0]; double t2 = data[f3 + 2]; data[f3 + 0] = (float)(cosY*t1 - sinY*t2); data[f3 + 2] = (float)(sinY*t1 + cosY*t2); for (int i=3/*dof*/; --i>=0;) { data[f3 + i] += pos[i]; } data[f3 + 4] += angleY - data[frame3 + 4]; } } // temp.x = data[0]; // temp.y = data[1]; // temp.z = data[2]; // // LA.xformPos(temp, toParent, temp); // // temp.x = data[0]; // temp.y = data[1]; // temp.z = data[2]; // if (fade && currentbones != null && !CameraPane.fullreset) // { // Fade(); // } } if (false) // doesn't change anything bone._isRoot) { // Reset orientation _t2.setIdentity(); double tx = bone.toParent[3][0]; double ty = bone.toParent[3][1]; double tz = bone.toParent[3][2]; temp.x = 1; temp.y = 0; temp.z = 0; LA.xformDir(temp, bone.toParent, temp); double angle = Math.atan2(-temp.z, temp.x); _t2.rotY(angle); // _t1.setIdentity(); // _t1.rotX(temp.x); // _t2.mul(_t1); // // _t1.setIdentity(); // _t1.rotY(temp.y); // _t2.mul(_t1); // // _t1.setIdentity(); // _t1.rotZ(temp.z); // _t2.mul(_t1); cJ3D.ResetTransform(bone, _t2, false); bone.toParent[3][0] = tx; bone.toParent[3][1] = ty; bone.toParent[3][2] = tz; } } // SetCurrentBones(frame); } } // skip initial "T" static int offset = 1; // 5; // 10; // 5; // 100; void ReadBVH() { try { mocap.reader.BVHReader rd = new mocap.reader.BVHReader(0.75); //System.err.println("Reset MOCAP fullname = " + fullname); if (random && !CameraPane.FAST) { // suppose a random crowd. We need longer mocap data. System.err.println("MOCAP EXTENSION"); rd.mocapextension = rd.EXTENSION; endframe = 0; // aout 2013 } bvh = rd.readFile(new java.io.File(fullname)); smoothed = false; // aout 2013 } catch(Exception e) { e.printStackTrace(); } } void Reset() { // if (fullname != null) // ReadBVH(); Object3D hip = get(0); // assert(obj.name.equals("hip")); //Object3D rot = hip.get(0); // assert(obj.name == null); // .equals("hip#")); if (hip.toParent == null) { hip.toParent = LA.newMatrix(); hip.fromParent = LA.newMatrix(); } //hip.toParent[3][0] = hip.toParent[3][1] = hip.toParent[3][2] = 0; //hip.fromParent[3][0] = hip.fromParent[3][1] = hip.fromParent[3][2] = 0; if (toParent == null) { toParent = LA.newMatrix(); fromParent = LA.newMatrix(); } LA.matIdentity(toParent); LA.matIdentity(fromParent); // LA.matIdentity(hip.toParent); // LA.matIdentity(hip.fromParent); // if (rot.toParent == null) // { // // june 2014 // rot.toParent = LA.newMatrix(); // rot.fromParent = LA.newMatrix(); // } // LA.matIdentity(rot.toParent); // LA.matIdentity(rot.fromParent); // july 2014 // goalx = goalz = 0; targetdirx = targetdirz = 0; //SetPositionDelta(true, true, true, false); // LoadData(); // Rewind(); baseframe = frame = 0; // T pose. No offset. // aout 2013 endframe = 0; // june 2013 ResetZero(); //setPose(frame); } static int mocapsupport = 0; int GetFirstFrame() { //assert(offset + beginframe == 0); return offset + beginframe; } void Step() { // if (CameraPane.ABORTED) // return; int step = 1; // patch for running hare if (speedup) // fev 2014 step *= 2; // 4; if (rewind) // mars 2014 step *= 4; //if (CameraPane.FAST) // && !CameraPane.HOLD) step *= CameraPane.STEP; //System.err.println("Step Mocap frame # " + frame); //System.err.println("Step Mocap baseframe # " + baseframe); baseframe += step; //frame = baseframe; //frame += step; // if (frame != baseframe) // { // //assert(frame == baseframe); // System.err.print("Step Mocap frame # " + frame); // System.err.println("; baseframe # " + baseframe); // } int lastframetest = endframe; if (lastframetest == 0) lastframetest = bvh.animation.getNumFrames(); // WARNING: RESET DESTROYS EVERYTHING if (baseframe >= lastframetest) // july 2013 // - GetFirstFrame()) { System.out.println("MOCAP reset: " + this.GetFileRoot() + "; baseframe = " + baseframe + "; beginframe = " + beginframe + "; lastframe = " + lastframetest + "; fullname = " + fullname); //int delta = frame - baseframe; if (Globals.CROWD) { baseframe = GetFirstFrame(); // 0 initial point } else //frame = baseframe + delta; // SetPositionDelta(false, true, false); // ?? false); { if (support == null) { if (bvh == null && new File(fullname).exists()) ObjEditor.LoadBVHFile(fullname, this, false, beginframe, lastframetest); else { SetGlobalTransform(); Rewind(); Fade(); } } else { // try // { bvh = ((Mocap)support.get(mocapsupport++ % support.Size())).bvh; // SetPositionDelta(false, true, true, true); LoadData(); Rewind(); Fade(); // } // catch (Exception e) // { // System.err.println("An error occured while loading data. Use fallback by loading the file."); // //e.printStackTrace(); // try // { // ObjEditor.LoadBVHFile(fullname, this, false, beginframe, lastframetest); // } // catch (Exception e2) // { // System.err.println("Motion file not found: " + fullname); // } // } } } frame = GetFirstFrame(); } else frame += step; //SetPositionDelta(false); // if (frame >= bvh.animation.getNumFrames()) // //baseframe = // frame = GetFirstFrame(); // 0; // offset; // initial point //System.err.println("frame = " + frame); if (lastframe != 0) { // may 2014 if (baseframe > lastframe || baseframe < firstframe) { frame = baseframe = firstframe; } } setPose(frame); } // filters out bad input data void SmoothAnimData() { //if (true) // return; float[] data; // = bvh.animation.getBoneData(0); //int dim = data.length; int numframes = bvh.animation.getNumFrames(); //assert(dim == numframes*6); for (int b=numbones; --b>=0;) // 0;) { data = bvh.animation.getBoneData(b); int loops = 1; if (b == 0) // if (random) // aout 2013 supersmooth for crowds loops = 8; float[] tmp = new float[data.length]; Quaternion quat_1 = new Quaternion(); Quaternion quat = new Quaternion(); Quaternion quat1 = new Quaternion(); float[] angles = new float[3]; for (int loop=loops; --loop>=0;) { System.arraycopy(data, 0, tmp, 0, tmp.length); // if (dim == -1) // dim = data.length; // else // assert(dim == data.length); int dof = 6; // data.length/numframes; if (b > 0) dof = 3; //boolean standard = true; // july 2014 if (dof != data.length/numframes) { System.err.println("Bone #" + b + ": dof = " + dof + " ---> " + (data.length/numframes)); //dof = data.length/numframes; //standard = false; return; } float clamp = 0.1f; for (int f=numframes-1; --f>=1;) // offset;) // 1;) { int f3_1 = (f-1)*dof; int f3 = f*dof; int f31 = (f+1)*dof; // for (int i=dof; --i>=0;) // angles[i] = data[f3_1 + i]; // quat_1.fromAngles(angles); // // for (int i=dof; --i>=0;) // angles[i] = data[f3 + i]; // quat.fromAngles(angles); // // for (int i=dof; --i>=0;) // angles[i] = data[f31 + i]; // quat1.fromAngles(angles); // // quat.x = 0.25f*quat_1.x + 0.5f*quat.x + 0.25f*quat1.x; // quat.y = 0.25f*quat_1.y + 0.5f*quat.y + 0.25f*quat1.y; // quat.z = 0.25f*quat_1.z + 0.5f*quat.z + 0.25f*quat1.z; // quat.w = 0.25f*quat_1.w + 0.5f*quat.w + 0.25f*quat1.w; // // quat.normalize(); // quat.toAngles(angles); int start = 3; int end = 3; if (b == 0) { end = 0; } for (int i=start; --i>=end;) { tmp[f3 + i] = 0.25f*data[f3_1 + i] + 0.5f*data[f3 + i] + 0.25f*data[f31 + i]; // if (dof == 3) // tmp[f3 + i] = angles[i]; // else // { // if (i < 3) // { // tmp[f3 + i] = 0.25f*data[f3_1 + i] + 0.5f*data[f3 + i] + 0.25f*data[f31 + i]; // } // else // { // tmp[f3 + i] = angles[i-3]; // } // } // if (Math.abs((tmp[f3 + i] - data[f3 + i]) / data[f3 + i]) > 0.1) // { // // value is too different. probably bad data // tmp[f3 + i] = tmp[f31 + i]; // } } // angles if (b == 0) { start = 6; end = 3; } else { start = 3; end = 0; } for (int i=start; --i>=end;) { double A = data[f3_1 + i]; double B = data[f3 + i]; double C = data[f31 + i]; double T = CurveAngle(A,C,0.5); tmp[f3 + i] = (float)CurveAngle(T,B,0.5); // double AB = CurveAngle(A,B,0.5); // double BC = CurveAngle(B,C,0.5); // // double AC = CurveAngle(AB,BC,0.5); // // assert(tmp[f3 + i] == AC); } } System.arraycopy(tmp, 0, data, 0, tmp.length); } } } // filters out bad input data void FilterAnimData() { new Exception().printStackTrace(); System.exit(0); float[] data; // = bvh.animation.getBoneData(0); //int dim = data.length; int numframes = bvh.animation.getNumFrames(); //assert(dim == numframes*6); for (int b=numbones; --b>=0;) { data = bvh.animation.getBoneData(b); int loops = 1; // if (b == 0) // loops = 10; for (int loop=loops; --loop>=0;) { int dof = 6; // data.length/numframes; if (b > 0) dof = 3; // System.err.println("Bone #" + b + ": dof = " + dof); float[] average = new float[dof]; float[] variance = new float[dof]; for (int f=numframes; --f>1;) { int f3_1 = (f-1)*dof; int f3 = f*dof; for (int i=dof; --i>=0;) { average[i] += (data[f3 + i] - data[f3_1 + i]) / (numframes-1); } } for (int f=numframes; --f>1;) { int f3_1 = (f-1)*dof; int f3 = f*dof; for (int i=dof; --i>=0;) { double diff = (data[f3 + i] - data[f3_1 + i]) - average[i]; variance[i] += diff*diff / (numframes-1); } } // for (int f=numframes-1; --f>1;) for (int f=2; f=0;) { double diff = (data[f3 + i] - data[f3_1 + i]) - average[i]; if (diff*diff > variance[i]/100) { // we consider the data as invalid int f31 = (f+1)*dof; float prev = data[f3_1 + i]; float next = data[f31 + i]; data[f3 + i] = (next + prev) / 2; } } } } } } void TranslateData(float[] offset) { int b = 0; { float[] thedata = bvh.animation.getBoneData(b); int numframes = bvh.animation.getNumFrames(); int dof = 6; for (int f=0; f=3;) for (int i=3; --i>=0;) { thedata[f3 + i] += offset[i]; } } } } transient boolean firsttimeafterload; // be ready to set "this" matrix //double[][] originmatrix = LA.newMatrix(); // ABC //double[][] originmatrixinv = LA.newMatrix(); //double[][] originmatrix0 = LA.newMatrix(); // ABC //double[][] originmatrixinv0 = LA.newMatrix(); //static Object3D origin; // = new Object3D(); //static Object3D origin0; // = new Object3D(); //static Object3D transformnode; // = new Object3D(); //static double[][] tempmatrix; // = LA.newMatrix(); // B'C' //static double[][] tempmatrixinv; // = LA.newMatrix(); static double[][] matrix = LA.newMatrix(); // = LA.newMatrix(); static double[][] matrix2 = LA.newMatrix(); // = LA.newMatrix(); //static transient double[] currentbones; double[] currenthip; transient boolean inbetween; void SetPositionDelta(boolean reset, boolean rewind, boolean load, boolean fade) { if (marked) return; //assert(dim == numframes*6); if (load) { LoadData(); } // assert(thedata[0] == 0); // assert(thedata[1] == 0); // assert(thedata[2] == 0); // assert(thedata[6] == 0); // assert(thedata[7] == 0); // assert(thedata[8] == 0); Object3D hip = get(0); // assert(obj.name.equals("hip")); Object3D rot = hip.get(0); // if (toParent == null) // { // toParent = LA.newMatrix(); // fromParent = LA.newMatrix(); // } // LA.matCopy(rot.toParent, fromParent); // LA.matCopy(rot.fromParent, toParent); // // assert(obj.name == null); // .equals("hip#")); Invariants(); hip.Invariants(); rot.Invariants(); if (origin == null) { origin = new Object3D(); origin0 = new Object3D(); //transformnode = new Object3D(); tempmatrix = LA.newMatrix(); tempmatrixinv = LA.newMatrix(); } origin.Invariants(); origin0.Invariants(); if (toParent == null) // mars 2014 { toParent = LA.newMatrix(); fromParent = LA.newMatrix(); } if (hip.toParent == null) // june 2014 { hip.toParent = LA.newMatrix(); hip.fromParent = LA.newMatrix(); } LA.matConcat(toParent, hip.toParent, origin0.toParent); // AB Object3D gp = this.parent; this.parent = null; hip.GlobalTransformInv(); this.parent = gp; if (rot.toParent == null) { // june 2014 rot.toParent = LA.newMatrix(); rot.fromParent = LA.newMatrix(); } // LA.matConcat(obj.fromParent, fromParent, origin0.toParent); // AB LA.matConcat(origin0.toParent, rot.toParent, origin.toParent); // ABC // CameraPane.selectedpoint.toParent[3][0] = obj.globalTransform[3][0]; // CameraPane.selectedpoint.toParent[3][1] = obj.globalTransform[3][1]; // CameraPane.selectedpoint.toParent[3][2] = obj.globalTransform[3][2]; // // LA.matConcat(fromParent, obj.fromParent, origin0.fromParent); // LA.matConcat(origin0.fromParent, rot.fromParent, origin.fromParent); LA.matInvert(origin0.toParent, origin0.fromParent); LA.matInvert(origin.toParent, origin.fromParent); Invariants(); hip.Invariants(); rot.Invariants(); origin.Invariants(); origin0.Invariants(); firsttimeafterload = false; // true; if (rewind) Rewind(); setPose(hip, frame, bvh.animation.getBoneData(hip._index)); //, fade); if (true) return; double pos[] = new double[6]; if (!reset) { assert(hip.tx == hip.toParent[3][0]); assert(hip.ty == hip.toParent[3][1]); assert(hip.tz == hip.toParent[3][2]); pos[0] = hip.tx; // obj.toParent[3][0]; pos[1] = hip.ty; // obj.toParent[3][1]; pos[2] = hip.tz; // obj.toParent[3][2]; pos[3] = hip.rz; pos[4] = hip.ry; pos[5] = hip.rx; } cVector temp = new cVector(); temp.x = hip.globalTransform[3][0]; temp.y = hip.globalTransform[3][1]; temp.z = hip.globalTransform[3][2]; // LA.xformPos(temp, origin0.toParent, temp); // System.err.println("POS0 = " + temp.x + ", " + temp.y + ", " + temp.z + ";"); pos[0] = temp.x; pos[1] = temp.y; pos[2] = temp.z; // System.err.println("POS1 = " + pos[0] + ", " + pos[1] + ", " + pos[2] + ";"); float[] data; // = bvh.animation.getBoneData(0); //int dim = data.length; int numframes = bvh.animation.getNumFrames(); //assert(dim == numframes*6); for (int b=numbones; --b>=1;) //int b = 0; { data = bvh.animation.getBoneData(b); int dof = 3; // data.length/numframes; assert(dof == 3); // System.err.println("Bone #" + b + ": dof = " + dof); int theframe = // 1; // baseframe; frame; int frame3 = theframe*dof; int boneframe3 = b*dof; for (int i=3/*dof*/; --i>=0;) { currentbones[boneframe3 + i] = data[frame3 + i]; } if (true) continue; // for (int f=theframe; --f>=0;) // { // int f3 = f*dof; // // for (int i=dof/*3!*/; --i>=0;) // { // data[f3 + i] += pos[i] - data[frame3 + i]; // } // } for (int f=numframes; --f>=theframe;) { int f3 = f*dof; if (f == theframe) f = theframe; for (int i=3/*dof*/; --i>=0;) { //if (i == 4) // continue; // if (i >= 4) // { // while (data[frame3 + i] < 0) // { // data[frame3 + i] += Math.PI*2; // } // while (data[f3 + i] < 0) // { // data[f3 + i] += Math.PI*2; // } // while (pos[i] < 0) // { // pos[i] += Math.PI*2; // } // } data[f3 + i] += pos[i] - data[frame3 + i]; } } } // Rewind(); setPose(hip, frame, bvh.animation.getBoneData(hip._index)); // , true); } void Rewind() { baseframe = frame = GetFirstFrame(); } String GetToolTip() { int seconds = (frame * 1000 / 120); return Integer.toString(frame) + " frames : " + (seconds/1000) + "." + (seconds/100%10) + "" + (seconds/10%10) + "" + (seconds%10) + " seconds"; // / 100.0; } transient boolean restarted; // CROWD issue? void drawSelf(iCameraPane display, Object3D /*Composite*/ root, boolean selected, boolean blocked) { //System.err.println("drawSelf Mocap frame # " + frame); //System.err.println("drawSelf Mocap baseframe # " + baseframe); // frame = baseframe; // if (hide) // return; if (count <= 0) // || display.IsFreezed()) { return; } // if (firstframe) // return; if (//!restarted && /*display.restartframe &&*/ Globals.isLIVE() && live && (display.DrawMode() == iCameraPane.SHADOW || !Globals.RENDERSHADOW || !Globals.COMPUTESHADOWWHENLIVE)) { //display.restartframe = false; restarted = true; //System.err.println("restarted = true"); Step(); } else { //System.err.println("restarted"); if (Globals.isLIVE() && live && display.DrawMode() != iCameraPane.SHADOW) { restarted = false; //System.err.println("restarted = false"); } } super.drawSelf(display, root, selected, blocked); } ////////////// public int Size() { //if (count == 0) // return 0; //if (filecontent == null) // return 0; return count>0?1:0; // GetObject().Size(); } public int size() { //if (filecontent == null) // return 0; return count>0?1:0; // GetObject().Size(); } public int size0() { //if (filecontent == null) // return 0; return count>=0?1:0; // GetObject().Size(); } public Object3D reserve(int i) { // assert(i==0); if (i != 0) System.exit(-1); return GetSkeleton(); // .reserve(i); } public Object3D reserve0(int i) { // assert(i==0); if (i != 0) System.exit(-1); return GetSkeleton(); // .reserve(i); } public void release(int i) { if (i != 0) System.exit(-1); // assert(i==0); // GetObject().release(i); } boolean HasBigData() { return skeleton != null; } public Object3D get(int i) { if (i != 0) System.exit(-1); // assert(i==0); return GetSkeleton(); //.get(i); } public int indexOf(Object3D obj) { if (obj == GetSkeleton()) return 0; return -1; //return GetObject().indexOf(obj); } void createEditWindow(GroupEditor callee, boolean newWindow) { //editWindow = (new SphereEditor(this, deepCopy(), callee)).GetEditor(); if (newWindow) { objectUI = new MocapEditor(this, deepCopy(), callee); } else { objectUI = new MocapEditor(this, callee); } editWindow = objectUI.GetEditor(); } }