001: /*
002: * $RCSfile: SharedMemoryMagellanTracker.java,v $
003: *
004: * Copyright 1996-2007 Sun Microsystems, Inc. All Rights Reserved.
005: *
006: * Redistribution and use in source and binary forms, with or without
007: * modification, are permitted provided that the following conditions
008: * are met:
009: *
010: * - Redistribution of source code must retain the above copyright
011: * notice, this list of conditions and the following disclaimer.
012: *
013: * - Redistribution in binary form must reproduce the above copyright
014: * notice, this list of conditions and the following disclaimer in
015: * the documentation and/or other materials provided with the
016: * distribution.
017: *
018: * Neither the name of Sun Microsystems, Inc. or the names of
019: * contributors may be used to endorse or promote products derived
020: * from this software without specific prior written permission.
021: *
022: * This software is provided "AS IS," without a warranty of any
023: * kind. ALL EXPRESS OR IMPLIED CONDITIONS, REPRESENTATIONS AND
024: * WARRANTIES, INCLUDING ANY IMPLIED WARRANTY OF MERCHANTABILITY,
025: * FITNESS FOR A PARTICULAR PURPOSE OR NON-INFRINGEMENT, ARE HEREBY
026: * EXCLUDED. SUN MICROSYSTEMS, INC. ("SUN") AND ITS LICENSORS SHALL
027: * NOT BE LIABLE FOR ANY DAMAGES SUFFERED BY LICENSEE AS A RESULT OF
028: * USING, MODIFYING OR DISTRIBUTING THIS SOFTWARE OR ITS
029: * DERIVATIVES. IN NO EVENT WILL SUN OR ITS LICENSORS BE LIABLE FOR
030: * ANY LOST REVENUE, PROFIT OR DATA, OR FOR DIRECT, INDIRECT, SPECIAL,
031: * CONSEQUENTIAL, INCIDENTAL OR PUNITIVE DAMAGES, HOWEVER CAUSED AND
032: * REGARDLESS OF THE THEORY OF LIABILITY, ARISING OUT OF THE USE OF OR
033: * INABILITY TO USE THIS SOFTWARE, EVEN IF SUN HAS BEEN ADVISED OF THE
034: * POSSIBILITY OF SUCH DAMAGES.
035: *
036: * You acknowledge that this software is not designed, licensed or
037: * intended for use in the design, construction, operation or
038: * maintenance of any nuclear facility.
039: *
040: * $Revision: 1.1 $
041: * $Date: 2007/09/25 20:01:19 $
042: * $State: Exp $
043: */
044:
045: package com.sun.j3d.utils.trackers;
046:
047: import java.lang.String;
048: import javax.vecmath.*;
049: import javax.media.j3d.*; // Note the following is necessary only for the sleep calls.
050: import java.lang.Thread;
051:
052: /**
053: * The SharedMemoryMagellanTracker Class defines the code to make a
054: * sharedMemoryMagellan input device work correclty
055: *
056: * @version 1.8, 98/08/14 21:49:19
057: * @author Henry Sowizral
058: */
059:
060: /**
061: * A SharedMemoryMagellanTracker object encapsulates the SharedMemoryMagellan's I/O information
062: */
063: public class SharedMemoryMagellanTracker extends Tracker {
064:
065: // This device's slave filename
066: String slaveFilename;
067:
068: // Current Sensor Read (last one we generated)
069: SensorRead currentRead;
070:
071: // Next SensorRead (the one we're now generating)
072: SensorRead nextRead;
073:
074: // The current sensor
075: int currentSensor = 0;
076:
077: // Holds a sensor's Euler Angles
078: Vector3d deviceEulerAngles = new Vector3d();
079:
080: // Holds the sensor's current Position
081: Vector3d deviceTranslateValues = new Vector3d();
082:
083: // The number of sensors associated with this device.
084: static final int SensorCount = 8;
085:
086: // The number of buttons associated with this device.
087: static final int ButtonCount = 5;
088:
089: // The initial position and orientation
090: Transform3D initialPosOrient = new Transform3D();
091:
092: static float PositionOrientation[] = new float[6];
093: static int[] ButtonArray = new int[1];
094:
095: /**
096: * Construct a new SharedMemoryMagellanTracker
097: * @param env the PhysicalEnvironment object
098: * @paran deviceFilename the SharedMemoryMagellan's devide name
099: * @param mode the mode of access one of POLLED or STREAMING
100: * @param sensorCount the number of sensors associated with this sharedMemoryMagellan
101: * @param buttonCount the number of buttons associated with each sensor
102: */
103: public SharedMemoryMagellanTracker(PhysicalEnvironment env,
104: String deviceFilename, int mode, int sensorCount,
105: int buttonCount) {
106: this .super (env, deviceFilename, mode, sensorCount, buttonCount);
107: }
108:
109: /**
110: * Construct a new SharedMemoryMagellanTracker with a hand tracker
111: * @param env the PhysicalEnvironment object
112: * @param masterFilename the SharedMemoryMagellan's master port device name
113: * @paran slaveFilename the SharedMemoryMagellan's slave port device name
114: * @param mode the mode of access one of POLLED or STREAMING
115: * @param sensorCount the number of sensors associated with this sharedMemoryMagellan
116: * @param buttonCount the number of buttons associated with each sensor
117: */
118: public SharedMemoryMagellanTracker(PhysicalEnvironment env,
119: String masterFilename, String slaveFilename, int mode,
120: int sensorCount, int buttonCount) {
121: this .super (env, masterFilename, mode, sensorCount, buttonCount);
122: this .slaveFilename = slaveFilename;
123: }
124:
125: native int StartUp(String filename);
126:
127: native int ProcessStream(int sensorIndex, float PosOrient[],
128: int Buttons[]);
129:
130: /**
131: * Code to initialize the device
132: * @param deviceFilename
133: */
134: public boolean initialize() {
135: return this .initialize(19200);
136: }
137:
138: /**
139: * Initializes the SharedMemoryMagellan deviceFilename by opening the
140: * device and sending the SharedMemoryMagellan the initialization information.
141: * @param deviceFilename the SharedMemoryMagellan's deviceFilename
142: * @param baudRate the speed we want the SharedMemoryMagellan to run at
143: */
144: public boolean initialize(int baudRate) {
145: return (0 == this .StartUp(""));
146: /*
147: if (processingMode == STREAMING) {
148: return (0 == this.StartUp(""));
149: } else if (processingMode == POLLED) {
150: return (0 == this.StartUp("")); // IEM
151: }
152:
153: System.out.println("Polling not supported");
154: initialPosOrient.setIdentity();
155: return false;
156: */
157: }
158:
159: /**
160: * Code to set the device's current position and orientation as the devices
161: * nominal position and orientation(establish its reference frame relative
162: * to the "Tracker base" reference frame).
163: */
164: public void setNominalPositionAndOrientation() {
165:
166: Transform3D baseTransform = new Transform3D();
167: // TODO: this will need some work
168: /*
169: (sensors[currentSensor].getCurrentSensorRead()).get(BaseTransform);
170: initialPosOrient.invert(BaseTransform);
171: System.out.println("New PositionOrientation transform");
172: System.out.println(initialPosOrient);
173: */
174: // make it simple -- tbryson
175: baseTransform.setIdentity();
176: ButtonArray[0] = 0;
177: sensors[currentSensor].setNextSensorRead(System
178: .currentTimeMillis(), baseTransform, ButtonArray);
179:
180: }
181:
182: /**
183: * Code to poll and then process the input from a sharedMemoryMagellan.
184: */
185: public void pollAndProcessInput() {
186: long time;
187:
188: this .ProcessStream(fileDescriptor, PositionOrientation,
189: ButtonArray);
190: time = System.currentTimeMillis();
191:
192: PositionOrientation[0] /= 1000.0;
193: PositionOrientation[1] /= 1000.0;
194: PositionOrientation[2] /= 1000.0;
195:
196: PositionOrientation[3] = PositionOrientation[3] * .0030f;
197: PositionOrientation[4] = PositionOrientation[4] * .0030f;
198: PositionOrientation[5] = PositionOrientation[5] * .0030f;
199:
200: setMatrixFromValues(PositionOrientation, newMatrix);
201:
202: currentRead = sensors[currentSensor].getCurrentSensorRead();
203: currentRead.get(oldTransform);
204:
205: oldTransform.get(oldLocation);
206: tmpVector.set(PositionOrientation[0], PositionOrientation[1],
207: PositionOrientation[2]);
208:
209: oldTransform.transform(tmpVector);
210: oldLocation.add(tmpVector);
211:
212: Vector3d zeroVec = new Vector3d(0.0, 0.0, 11.0);
213:
214: //newTransform.setTranslation(oldLocation);
215: newTransform.setTranslation(zeroVec);
216:
217: oldTransform.getRotationScale(oldMatrix);
218: tmpMatrix.mulTransposeRight(oldMatrix, newMatrix);
219:
220: // make the matrix orthogonal
221: SVD(tmpMatrix);
222:
223: newTransform.setRotationScale(tmpMatrix);
224:
225: // TODO: tbryson -- allocate buttons!!
226: sensors[currentSensor].setNextSensorRead(time, newTransform,
227: ButtonArray);
228:
229: }
230:
231: /**
232: * Code to construct a delta Matrix from SharedMemoryMagellan inputs
233: */
234: void setMatrixFromValues(float posOrient[], Matrix3d Delta) {
235: double sina, sinb, sinc, cosa, cosb, cosc;
236:
237: sina = Math.sin(posOrient[3]);
238: sinb = Math.sin(posOrient[4]);
239: sinc = Math.sin(posOrient[5]);
240:
241: cosa = Math.cos(posOrient[3]);
242: cosb = Math.cos(posOrient[4]);
243: cosc = Math.cos(posOrient[5]);
244:
245: Delta.m00 = cosb * cosc;
246: Delta.m01 = cosb * sinc;
247: Delta.m02 = -sinb;
248:
249: Delta.m10 = -(cosa * sinc) + (sina * sinb * sinc);
250: Delta.m11 = (cosa * cosc) + (sina * sinb * sinc);
251: Delta.m12 = sina * cosb;
252:
253: Delta.m20 = (sina * sinc) + (cosa * sinb * cosc);
254: Delta.m21 = -(sina * cosc) + (cosa * sinb * sinc);
255: Delta.m22 = cosa * cosb;
256:
257: if (false) {
258: // Normalize the matrix (THis does not work! returns a bad matrix)
259:
260: // forward x vertical => right (2x) x (1x) => (0x)
261: // normalize (right)
262: // right x forward => vertical (0x) x (2x) => (1x)
263: // normalize (vertical)
264: // vertical x right => forward (1x) x (0x) => (2x)
265: // normalize (forward)
266: // 0, 0 = r.x
267: // 0, 1 = r.y
268: // 0, 2 = r.z
269:
270: // 1, 0 = v.x
271: // 1, 1 = v.y
272: // 1, 2 = v.z
273:
274: // 2, 0 = -f.x
275: // 2, 1 = -f.y
276: // 2, 2 = -f.z
277:
278: Delta.m00 = Delta.m21 * Delta.m12 - Delta.m22 * Delta.m11;
279: Delta.m01 = Delta.m10 * Delta.m22 - Delta.m12 * Delta.m20;
280: Delta.m02 = Delta.m20 * Delta.m11 - Delta.m21 * Delta.m10;
281:
282: double norm = 1.0 / Math.sqrt(Delta.m00 * Delta.m00
283: + Delta.m01 * Delta.m01 + Delta.m02 * Delta.m02);
284: Delta.m00 = Delta.m00 * norm;
285: Delta.m01 = Delta.m01 * norm;
286: Delta.m02 = Delta.m02 * norm;
287:
288: Delta.m10 = Delta.m01 * Delta.m22 - Delta.m02 * Delta.m21;
289: Delta.m11 = Delta.m20 * Delta.m02 - Delta.m22 * Delta.m00;
290: Delta.m12 = Delta.m00 * Delta.m21 - Delta.m01 * Delta.m20;
291:
292: norm = 1.0 / Math.sqrt(Delta.m10 * Delta.m10 + Delta.m11
293: * Delta.m11 + Delta.m12 * Delta.m12);
294: Delta.m10 = Delta.m10 * norm;
295: Delta.m11 = Delta.m11 * norm;
296: Delta.m12 = Delta.m12 * norm;
297:
298: Delta.m20 = Delta.m11 * Delta.m12 - Delta.m12 * Delta.m11;
299: Delta.m21 = Delta.m00 * Delta.m02 - Delta.m02 * Delta.m00;
300: Delta.m22 = Delta.m10 * Delta.m11 - Delta.m11 * Delta.m10;
301:
302: norm = 1.0 / Math.sqrt(Delta.m20 * Delta.m20 + Delta.m21
303: * Delta.m21 + Delta.m22 * Delta.m22);
304: Delta.m20 = Delta.m20 * norm;
305: Delta.m21 = Delta.m21 * norm;
306: Delta.m22 = Delta.m22 * norm;
307: }
308: }
309:
310: Matrix3d oldMatrix = new Matrix3d();
311: Matrix3d newMatrix = new Matrix3d();
312: Matrix3d tmpMatrix = new Matrix3d();
313:
314: Transform3D oldTransform = new Transform3D();
315: Vector3d oldLocation = new Vector3d();
316:
317: Transform3D newTransform = new Transform3D();
318: Vector3d newLocation = new Vector3d();
319:
320: Vector3d tmpVector = new Vector3d();
321:
322: /**
323: * Code to process the device's streaming input.
324: */
325: public void processStreamInput() {
326: long time;
327:
328: this .ProcessStream(fileDescriptor, PositionOrientation,
329: ButtonArray);
330: time = System.currentTimeMillis();
331:
332: PositionOrientation[0] /= 1000.0;
333: PositionOrientation[1] /= 1000.0;
334: PositionOrientation[2] /= 1000.0;
335: //PositionOrientation[3] *= 0.15;
336: //PositionOrientation[4] *= 0.15;
337: //PositionOrientation[5] *= 0.15;
338:
339: // IEM
340: // First Normalize the Numbers (K1's)
341: if (false) {
342: PositionOrientation[0] /= 8.0; // X
343: PositionOrientation[1] /= 1.0; // Y
344: PositionOrientation[2] /= 8.0; // Z
345: PositionOrientation[3] *= 66.0; // PITCH
346: PositionOrientation[4] *= 66.0; // PAN
347: PositionOrientation[5] *= 125.0; // ROLL
348:
349: // Second level of a`gain adjustment
350: PositionOrientation[0] *= 1.0; // X
351: PositionOrientation[1] *= 1.0; // Y
352: PositionOrientation[2] *= 1.0; // Z
353: PositionOrientation[3] *= 1.0; // PITCH
354: PositionOrientation[4] *= 1.0; // PAN
355: PositionOrientation[5] *= 1.0 / 1.5; // ROLL
356:
357: // FAST level of a`gain adjustment
358: PositionOrientation[0] *= 0.6; // X
359: PositionOrientation[1] *= 0.2; // Y
360: PositionOrientation[2] *= 0.6; // Z
361: PositionOrientation[3] *= 0.3; // PITCH
362: PositionOrientation[4] *= 0.3; // PAN
363: PositionOrientation[5] *= 0.3; // ROLL
364:
365: // Up - down needs to be non symetric -
366: // up is too fast relative to down
367: PositionOrientation[1] *= (PositionOrientation[1] < 0.0) ? 3.0
368: : 1.0;
369:
370: System.out.println("PositionOrientation: " + " "
371: + (PositionOrientation[0] * 1000) + " "
372: + (PositionOrientation[1] * 1000) + " "
373: + (PositionOrientation[2] * 1000) + " ORI "
374: + (PositionOrientation[3] * 1000) + " "
375: + (PositionOrientation[4] * 1000) + " "
376: + (PositionOrientation[5] * 1000));
377:
378: // Formula for tuning is:
379: // new val = k2 * val + k3 *val *val
380: // MARKER1
381:
382: PositionOrientation[0] = (float) (PositionOrientation[0] * 1.0 + 1.5 * 0.065 * (PositionOrientation[0]
383: * PositionOrientation[0] * PositionOrientation[0]));
384: PositionOrientation[1] = (float) (PositionOrientation[1] * 1.0 + 0.010 * (PositionOrientation[1]
385: * PositionOrientation[1] * PositionOrientation[1]));
386: PositionOrientation[2] = (float) (PositionOrientation[2] * 1.0 + 0.025 * (PositionOrientation[2]
387: * PositionOrientation[2] * PositionOrientation[2]));
388:
389: PositionOrientation[3] = (float) (PositionOrientation[3] * 0.01 * 0.25 + 0.00015 * (PositionOrientation[3]
390: * PositionOrientation[3] * PositionOrientation[3]));
391: PositionOrientation[4] = (float) (PositionOrientation[4] * 0.01 * 0.50 + 0.001 * (PositionOrientation[4]
392: * PositionOrientation[4] * PositionOrientation[4]));
393: PositionOrientation[5] = (float) (PositionOrientation[5] * 0.01 * 0.33 * 1.5 + 0.001 * (PositionOrientation[5]
394: * PositionOrientation[5] * PositionOrientation[5]));
395: }
396:
397: // the real code is much simpler!! -- tbryson
398: PositionOrientation[3] = PositionOrientation[3] * .0030f;
399: PositionOrientation[4] = PositionOrientation[4] * .0030f;
400: PositionOrientation[5] = PositionOrientation[5] * .0030f;
401:
402: setMatrixFromValues(PositionOrientation, newMatrix);
403:
404: currentRead = sensors[currentSensor].getCurrentSensorRead();
405: currentRead.get(oldTransform);
406:
407: oldTransform.get(oldLocation);
408: tmpVector.set(PositionOrientation[0], PositionOrientation[1],
409: PositionOrientation[2]);
410:
411: oldTransform.transform(tmpVector);
412: oldLocation.add(tmpVector);
413:
414: Vector3d zeroVec = new Vector3d(0.0, 0.0, 11.0);
415:
416: //newTransform.setTranslation(oldLocation);
417: newTransform.setTranslation(zeroVec);
418:
419: oldTransform.getRotationScale(oldMatrix);
420: tmpMatrix.mulTransposeRight(oldMatrix, newMatrix);
421:
422: // make the matrix orthogonal
423: SVD(tmpMatrix);
424:
425: newTransform.setRotationScale(tmpMatrix);
426:
427: // TODO: tbryson -- allocate buttons!!
428: sensors[currentSensor].setNextSensorRead(time, newTransform,
429: ButtonArray);
430:
431: }
432:
433: // perform a first order SVD calculation
434:
435: void SVD(Matrix3d matArg) {
436:
437: Matrix3d mat = new Matrix3d(matArg);
438: Matrix3d matTranspose = new Matrix3d();
439:
440: matTranspose.transpose(matArg);
441:
442: mat.mul(matTranspose);
443: mat.mul(matArg);
444: mat.mul(-0.5);
445:
446: matArg.mul(1.5);
447: matArg.add(mat);
448:
449: }
450:
451: }
|