001: /*
002: * $RCSfile: FakespaceMagellanTracker.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 yield instruction
050: import java.lang.Thread;
051:
052: /**
053: * The MagellanTracker Class defines the code to make a magellan input
054: * device work correclty
055: *
056: * @version 1.9, 99/09/15 13:47:13
057: * @author Henry Sowizral
058: */
059:
060: /**
061: * A MagellanTracker object encapsulates the Magellan's I/O information
062: */
063: public class FakespaceMagellanTracker extends MagellanTracker {
064:
065: // Switch to 3D mode
066: static final String initializationString = "\rz\rz\r";
067:
068: // get version string
069: static final String magellanVersion = "vQ\r";
070:
071: // no extra sensitivity
072: static final String noExtraSensitivity = "q00\r";
073:
074: // detect zero position
075: static final String detectZero = "z\r";
076:
077: // transmit every 60 ms data
078: static final String continuousMode = "pAA\r";
079:
080: // translation and rotation data ON
081: static final String sendTranslateAndRotate = "m3\r";
082:
083: // null radius to value 8
084: static final String smallNullRadius = "nH\r";
085:
086: // Short beep
087: static final String shortBeep = "b9\r";
088:
089: // Current Sensor Read (last one we generated)
090: SensorRead currentRead;
091:
092: // Next SensorRead (the one we're now generating)
093: SensorRead nextRead;
094:
095: // The current sensor
096: int currentSensor = 0;
097:
098: // Holds a sensor's Euler Angles
099: Vector3d deviceEulerAngles = new Vector3d();
100:
101: // Holds the sensor's current Position
102: Vector3d deviceTranslateValues = new Vector3d();
103:
104: // The number of sensors associated with this device.
105: static final int SensorCount = 8;
106:
107: // The number of buttons associated with this device.
108: static final int ButtonCount = 9;
109:
110: static float PositionOrientation[] = new float[6];
111: static int ButtonArray[] = new int[1];
112:
113: /**
114: * Construct a new MagellanTracker
115: * @param view the view object
116: * @paran deviceFilename the Magellan's devide name
117: * @param mode the mode of access one of POLLED or STREAMING
118: * @param sensorCount the number of sensors associated with this magellan
119: * @param buttonCount the number of buttons associated with each sensor
120: */
121: public FakespaceMagellanTracker(View view, String deviceFilename,
122: int mode, int sensorCount, int buttonCount) {
123: this .super (view, deviceFilename, mode, sensorCount, buttonCount);
124: }
125:
126: /**
127: * Code to initialize the device
128: * @param deviceFilename
129: */
130: public boolean initialize() {
131: return this .initialize(9600);
132: }
133:
134: /**
135: * Initializes the Magellan deviceFilename by opening the
136: * device and sending the Magellan the initialization information.
137: * @param deviceFilename the Magellan's deviceFilename
138: * @param baudRate the speed we want the Magellan to run at
139: */
140: public boolean initialize(int baudRate) {
141: fileDescriptor = this .OpenSerialDeviceRaw(deviceFilename,
142: baudRate);
143: if (fileDescriptor < 0) {
144: System.out.println("Unable to initialize Magellan on "
145: + deviceFilename + " error code " + fileDescriptor);
146: return false;
147: }
148: try {
149: Thread.sleep(2000L);
150: } catch (InterruptedException e) {
151: System.out.println(e);
152: }
153:
154: return (0 == this .StartUp(fileDescriptor, baudRate));
155: }
156:
157: /**
158: * Code to set the device's current position and orientation as the devices
159: * nominal position and orientation(establish its reference frame relative
160: * to the "Tracker base" reference frame).
161: */
162: public void setNominalPositionAndOrientation() {
163: newTransform.setIdentity();
164: ButtonArray[0] = 0;
165: sensors[currentSensor].setNextSensorRead(System
166: .currentTimeMillis(), newTransform, ButtonArray);
167: }
168:
169: static float translationalScale = .0005f;
170: static float rotationScale = 0.5f;
171:
172: static Transform3D currentReading = new Transform3D();
173:
174: /**
175: * Code to poll and then process the input from a magellan.
176: */
177: public void pollAndProcessInput() {
178:
179: this
180: .PollInput(fileDescriptor, PositionOrientation,
181: ButtonArray);
182: /*
183: System.out.println("PositionOrientation: " +
184: " " + (PositionOrientation[0]*1000) +
185: " " + (PositionOrientation[1]*1000) +
186: " " + (PositionOrientation[2]*1000) +
187: " ORI " + (PositionOrientation[3]*1000) +
188: " " + (PositionOrientation[4]*1000) +
189: " " + (PositionOrientation[5]*1000));
190: System.out.println("Buttons " + ButtonArray[0]);
191: */
192: }
193:
194: /**
195: * Code to parse a Magellan Record and update the associated sensor.
196: */
197: void updateSensorRead(Sensor sensor, float posOrient[],
198: int buttons[]) {
199: }
200:
201: /**
202: * Code to construct a delta Matrix from Magellan inputs
203: */
204: void setMatrixFromValues(float posOrient[], Matrix3d Delta) {
205: double sina, sinb, sinc, cosa, cosb, cosc;
206:
207: sina = Math.sin(posOrient[3]);
208: sinb = Math.sin(posOrient[4]);
209: sinc = Math.sin(posOrient[5]);
210:
211: cosa = Math.cos(posOrient[3]);
212: cosb = Math.cos(posOrient[4]);
213: cosc = Math.cos(posOrient[5]);
214:
215: Delta.m00 = cosb * cosc;
216: Delta.m01 = cosb * sinc;
217: Delta.m02 = -sinb;
218:
219: Delta.m10 = -(cosa * sinc) + (sina * sinb * sinc);
220: Delta.m11 = (cosa * cosc) + (sina * sinb * sinc);
221: Delta.m12 = sina * cosb;
222:
223: Delta.m20 = (sina * sinc) + (cosa * sinb * cosc);
224: Delta.m21 = -(sina * cosc) + (cosa * sinb * sinc);
225: Delta.m22 = cosa * cosb;
226: }
227:
228: Matrix3d oldMatrix = new Matrix3d();
229: Matrix3d newMatrix = new Matrix3d();
230: Matrix3d tmpMatrix = new Matrix3d();
231:
232: Transform3D oldTransform = new Transform3D();
233: Vector3d oldLocation = new Vector3d();
234:
235: Transform3D newTransform = new Transform3D();
236: Vector3d newLocation = new Vector3d();
237:
238: Vector3d tmpVector = new Vector3d();
239:
240: /**
241: * Code to process the device's streaming input.
242: */
243: public void processStreamInput() {
244: long time;
245:
246: this
247: .PollInput(fileDescriptor, PositionOrientation,
248: ButtonArray);
249: time = System.currentTimeMillis();
250:
251: PositionOrientation[0] /= 1000.0;
252: PositionOrientation[1] /= 1000.0;
253: PositionOrientation[2] /= 1000.0;
254: PositionOrientation[3] *= 0.15;
255: PositionOrientation[4] *= 0.15;
256: PositionOrientation[5] *= 0.15;
257:
258: // IEM
259: // First Normalize the Numbers (K1's)
260: PositionOrientation[0] /= 8.0; // X
261: PositionOrientation[1] /= 1.0; // Y
262: PositionOrientation[2] /= 8.0; // Z
263: PositionOrientation[3] *= 66.0; // PITCH
264: PositionOrientation[4] *= 66.0; // PAN
265: PositionOrientation[5] *= 125.0; // ROLL
266:
267: // Second level of a`gain adjustment
268: PositionOrientation[0] *= 1.0; // X
269: PositionOrientation[1] *= 1.0; // Y
270: PositionOrientation[2] *= 1.0; // Z
271: PositionOrientation[3] *= 1.0; // PITCH
272: PositionOrientation[4] *= 1.0; // PAN
273: PositionOrientation[5] *= 1.0 / 1.5; // ROLL
274:
275: // Up - down needs to be non symetric - up is too fast relative to down
276: PositionOrientation[1] *= (PositionOrientation[1] < 0.0) ? 3.0
277: : 1.0;
278:
279: /*
280: System.out.println("PositionOrientation: " +
281: " " + (PositionOrientation[0]*1000) +
282: " " + (PositionOrientation[1]*1000) +
283: " " + (PositionOrientation[2]*1000) +
284: " ORI " + (PositionOrientation[3]*1000) +
285: " " + (PositionOrientation[4]*1000) +
286: " " + (PositionOrientation[5]*1000));
287: */
288:
289: // Formula for tuning is:
290: // new val = k2 * val + k3 *val *val
291: // MARKER1
292: PositionOrientation[0] = (float) (PositionOrientation[0] * 1.0 + 1.5 * 0.065 * (PositionOrientation[0]
293: * PositionOrientation[0] * PositionOrientation[0]));
294: PositionOrientation[1] = (float) (PositionOrientation[1] * 1.0 + 0.010 * (PositionOrientation[1]
295: * PositionOrientation[1] * PositionOrientation[1]));
296: PositionOrientation[2] = (float) (PositionOrientation[2] * 1.0 + 0.025 * (PositionOrientation[2]
297: * PositionOrientation[2] * PositionOrientation[2]));
298:
299: PositionOrientation[3] = (float) (PositionOrientation[3] * 0.01 * 0.25 + 0.00015 * (PositionOrientation[3]
300: * PositionOrientation[3] * PositionOrientation[3]));
301: PositionOrientation[4] = (float) (PositionOrientation[4] * 0.01 * 0.50 + 0.001 * (PositionOrientation[4]
302: * PositionOrientation[4] * PositionOrientation[4]));
303: PositionOrientation[5] = (float) (PositionOrientation[5] * 0.01 * 0.33 * 1.5 + 0.001 * (PositionOrientation[5]
304: * PositionOrientation[5] * PositionOrientation[5]));
305:
306: setMatrixFromValues(PositionOrientation, newMatrix);
307:
308: currentRead = sensors[currentSensor].getCurrentSensorRead();
309: currentRead.get(oldTransform);
310:
311: oldTransform.get(oldLocation);
312: tmpVector.set(PositionOrientation[0], PositionOrientation[1],
313: PositionOrientation[2]);
314:
315: oldTransform.transform(tmpVector);
316: oldLocation.add(tmpVector);
317: newTransform.setTranslation(oldLocation);
318:
319: oldTransform.getRotationScale(oldMatrix);
320: tmpMatrix.mulTransposeRight(oldMatrix, newMatrix);
321:
322: newTransform.setRotationScale(tmpMatrix);
323:
324: // We suspect that the mat is non othogonal & screwing up the lights so try to fix:
325:
326: sensors[currentSensor].setNextSensorRead(time, newTransform,
327: ButtonArray);
328:
329: }
330:
331: }
|