001: /*
002: * $RCSfile: MagellanTracker.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.16, 99/09/15 13:47:18
057: * @author Henry Sowizral
058: */
059:
060: /**
061: * A MagellanTracker object encapsulates the Magellan's I/O information
062: */
063: public class MagellanTracker extends Tracker {
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:
112: static int ButtonArray[] = new int[1];
113:
114: /**
115: * Construct a new MagellanTracker
116: * @param view the view object
117: * @paran deviceFilename the Magellan's devide name
118: * @param mode the mode of access one of POLLED or STREAMING
119: * @param sensorCount the number of sensors associated with this magellan
120: * @param buttonCount the number of buttons associated with each sensor
121: */
122: public MagellanTracker(View view, String deviceFilename, int mode,
123: int sensorCount, int buttonCount) {
124: this .super (view, deviceFilename, mode, sensorCount, buttonCount);
125: }
126:
127: native int StartUp(int fd, int baudRate);
128:
129: native int PollInput(int fd, float PosOrient[], int Buttons[]);
130:
131: /**
132: * Code to initialize the device
133: * @param deviceFilename
134: */
135: public boolean initialize() {
136: return this .initialize(9600);
137: }
138:
139: /**
140: * Initializes the Magellan deviceFilename by opening the
141: * device and sending the Magellan the initialization information.
142: * @param deviceFilename the Magellan's deviceFilename
143: * @param baudRate the speed we want the Magellan to run at
144: */
145: public boolean initialize(int baudRate) {
146: fileDescriptor = this .OpenSerialDeviceRaw(deviceFilename,
147: baudRate);
148: if (fileDescriptor < 0) {
149: System.out.println("Unable to initialize Magellan on "
150: + deviceFilename + " error code " + fileDescriptor);
151: return false;
152: }
153:
154: try {
155: Thread.sleep(2000L);
156: } catch (InterruptedException e) {
157: System.out.println(e);
158: }
159:
160: return (0 == this .StartUp(fileDescriptor, baudRate));
161:
162: }
163:
164: /**
165: * Code to set the device's current position and orientation as the devices
166: * nominal position and orientation(establish its reference frame relative
167: * to the "Tracker base" reference frame).
168: */
169: public void setNominalPositionAndOrientation() {
170: newTransform.setIdentity();
171: ButtonArray[0] = 0;
172: sensors[currentSensor].setNextSensorRead(System
173: .currentTimeMillis(), newTransform, ButtonArray);
174: }
175:
176: static float translationalScale = .0005f;
177: static float rotationScale = 0.5f;
178:
179: static Transform3D currentReading = new Transform3D();
180:
181: /**
182: * Code to poll and then process the input from a magellan.
183: */
184: public void pollAndProcessInput() {
185:
186: this
187: .PollInput(fileDescriptor, PositionOrientation,
188: ButtonArray);
189: /*
190: System.out.println("PositionOrientation: "
191: + PositionOrientation[0] +
192: " " + PositionOrientation[0] +
193: " " + PositionOrientation[1] +
194: " " + PositionOrientation[2] +
195: " " + PositionOrientation[3] +
196: " " + PositionOrientation[4] +
197: " " + PositionOrientation[5]);
198: System.out.println("Buttons " + ButtonArray[0]);
199: */
200: }
201:
202: /**
203: * Code to parse a Magellan Record and update the associated sensor.
204: */
205: void updateSensorRead(Sensor sensor, float posOrient[],
206: int buttons[]) {
207: }
208:
209: /**
210: * Code to construct a delta Matrix from Magellan inputs
211: */
212: void setMatrixFromValues(float posOrient[], Matrix3d Delta) {
213: double sina, sinb, sinc, cosa, cosb, cosc;
214:
215: sina = Math.sin(posOrient[3]);
216: sinb = Math.sin(posOrient[4]);
217: sinc = Math.sin(posOrient[5]);
218:
219: cosa = Math.cos(posOrient[3]);
220: cosb = Math.cos(posOrient[4]);
221: cosc = Math.cos(posOrient[5]);
222:
223: Delta.m00 = cosb * cosc;
224: Delta.m01 = cosb * sinc;
225: Delta.m02 = -sinb;
226:
227: Delta.m10 = -(cosa * sinc) + (sina * sinb * sinc);
228: Delta.m11 = (cosa * cosc) + (sina * sinb * sinc);
229: Delta.m12 = sina * cosb;
230:
231: Delta.m20 = (sina * sinc) + (cosa * sinb * cosc);
232: Delta.m21 = -(sina * cosc) + (cosa * sinb * sinc);
233: Delta.m22 = cosa * cosb;
234: }
235:
236: Matrix3d oldMatrix = new Matrix3d();
237: Matrix3d newMatrix = new Matrix3d();
238: Matrix3d tmpMatrix = new Matrix3d();
239:
240: Transform3D oldTransform = new Transform3D();
241: Vector3d oldLocation = new Vector3d();
242:
243: Transform3D newTransform = new Transform3D();
244: Vector3d newLocation = new Vector3d();
245:
246: Vector3d tmpVector = new Vector3d();
247:
248: /**
249: * Code to process the device's streaming input.
250: */
251: public void processStreamInput() {
252: long time;
253:
254: this
255: .PollInput(fileDescriptor, PositionOrientation,
256: ButtonArray);
257: time = System.currentTimeMillis();
258:
259: // Scale rotations
260: PositionOrientation[3] *= 0.15;
261: PositionOrientation[4] *= 0.15;
262: PositionOrientation[5] *= 0.15;
263:
264: setMatrixFromValues(PositionOrientation, newMatrix);
265:
266: currentRead = sensors[currentSensor].getCurrentSensorRead();
267: currentRead.get(oldTransform);
268:
269: oldTransform.get(oldLocation);
270: tmpVector.set(PositionOrientation[0] / 1000,
271: PositionOrientation[1] / 1000,
272: PositionOrientation[2] / 1000);
273:
274: oldTransform.transform(tmpVector);
275: oldLocation.add(tmpVector);
276: newTransform.setTranslation(oldLocation);
277:
278: oldTransform.getRotationScale(oldMatrix);
279: tmpMatrix.mulTransposeRight(oldMatrix, newMatrix);
280:
281: newTransform.setRotationScale(tmpMatrix);
282: sensors[currentSensor].setNextSensorRead(time, newTransform,
283: ButtonArray);
284:
285: }
286:
287: }
|