001: /*
002: * $RCSfile: PolhemusTracker.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 PolhemusTracker Class defines the code to make a polhemus input
054: * device work correclty
055: *
056: * @version 1.12, 99/09/15 13:47:19
057: * @author Henry Sowizral
058: */
059:
060: /**
061: * A PolhemusTracker object encapsulates the Polhemus's I/O information
062: */
063:
064: class PolhemusTracker extends Tracker {
065:
066: /**
067: * This device's file descriptor index
068: */
069: int fileDescriptor;
070:
071: /**
072: * Holds a sensor's Euler Angles
073: */
074: Vector3d deviceEulerAngles = new Vector3d();
075:
076: /**
077: * Holds a sensor's Position Information
078: */
079: Vector3d deviceTranslateValues = new Vector3d();
080:
081: /**
082: * The number of sensors associated with this device.
083: */
084: int SensorCount;
085:
086: /**
087: * This device's sensor mask. Which sensor's to initialize.
088: */
089: int sensorMask;
090:
091: /**
092: * The number of buttons associated with this device.
093: */
094: static final int ButtonCount = 0;
095:
096: static float PositionOrientation[] = new float[6];
097: static int[] ButtonArray = new int[1];
098:
099: native int StartUp(int fd, int baudRate);
100:
101: native int PollInput(int fd, float PosOrient[], int Buttons[]);
102:
103: /**
104: * Construct a new PolhemusTracker, consisting of a
105: */
106: PolhemusTracker(View view, String deviceFilename, int mode,
107: int sensorCount, int buttonCount, int seonsorMask) {
108: this .super (view, deviceFilename, mode, sensorCount, ButtonCount);
109: }
110:
111: native void pollPolhemus(int fd);
112:
113: native int polhmeusOpen(String deviceFilename, int baudRate,
114: int sensorMask);
115:
116: native boolean readUpdate(Vector3d deviceEulerAngles,
117: Vector3d deviceTranslateValues);
118:
119: /**
120: * Code to initialize the device
121: * @param deviceFilename
122: */
123: public boolean initialize() {
124: return this .initialize(19200);
125: }
126:
127: /**
128: * Initializes the Polhemus deviceFilename by opening the
129: * device and sending the Polhemus the initialization information.
130: * @param deviceFilename the Polhemus's deviceFilename
131: * @param baudRate the speed we want the Polhemus to run at
132: */
133: public boolean initialize(int baudRate) {
134: fileDescriptor = this .polhmeusOpen(deviceFilename, baudRate,
135: sensorMask);
136: if (fileDescriptor < 0) {
137: System.out.println("Unable to initialize Polhemus on "
138: + deviceFilename + " error code " + fileDescriptor);
139: return false;
140: }
141:
142: try {
143: Thread.sleep(2000L);
144: } catch (InterruptedException e) {
145: System.out.println(e);
146: }
147:
148: return (0 == this .StartUp(fileDescriptor, baudRate));
149:
150: }
151:
152: /**
153: * Code to set the device's current position and orientation as the devices
154: * nominal position and orientation(establish its reference frame relative
155: * to the "Tracker base" reference frame).
156: */
157: public void setNominalPositionAndOrientation() {
158: TrackerBaseToDevice.setIdentity();
159: }
160:
161: static float translationalScale = .0005f;
162: static float rotationScale = 0.5f;
163:
164: static Transform3D currentReading = new Transform3D();
165:
166: /**
167: * Code to poll and then process the input from a polhemus.
168: */
169: public void pollAndProcessInput() {
170:
171: this
172: .PollInput(fileDescriptor, PositionOrientation,
173: ButtonArray);
174: /*
175: System.out.println("PositionOrientation: "
176: + PositionOrientation[0] +
177: " " + PositionOrientation[0] +
178: " " + PositionOrientation[1] +
179: " " + PositionOrientation[2] +
180: " " + PositionOrientation[3] +
181: " " + PositionOrientation[4] +
182: " " + PositionOrientation[5]);
183: System.out.println("Buttons " + ButtonArray[0]);
184: */
185: }
186:
187: /**
188: * Code to parse a Polhemus Record and update the associated sensor.
189: */
190: void updateSensorRead(Sensor sensor, float posOrient[]) {
191: }
192:
193: /**
194: * Code to construct a delta Matrix from Polhemus inputs
195: */
196: void setMatrixFromValues(float posOrient[], Matrix3d Delta) {
197: double sina, sinb, sinc, cosa, cosb, cosc;
198:
199: sina = Math.sin(posOrient[3]);
200: sinb = Math.sin(posOrient[4]);
201: sinc = Math.sin(posOrient[5]);
202:
203: cosa = Math.cos(posOrient[3]);
204: cosb = Math.cos(posOrient[4]);
205: cosc = Math.cos(posOrient[5]);
206:
207: Delta.m00 = cosb * cosc;
208: Delta.m01 = cosb * sinc;
209: Delta.m02 = -sinb;
210:
211: Delta.m10 = -(cosa * sinc) + (sina * sinb * sinc);
212: Delta.m11 = (cosa * cosc) + (sina * sinb * sinc);
213: Delta.m12 = sina * cosb;
214:
215: Delta.m20 = (sina * sinc) + (cosa * sinb * cosc);
216: Delta.m21 = -(sina * cosc) + (cosa * sinb * sinc);
217: Delta.m22 = cosa * cosb;
218: }
219:
220: Matrix3d oldMatrix = new Matrix3d();
221: Matrix3d newMatrix = new Matrix3d();
222: Matrix3d tmpMatrix = new Matrix3d();
223:
224: Transform3D oldTransform = new Transform3D();
225: Vector3d oldLocation = new Vector3d();
226:
227: Transform3D newTransform = new Transform3D();
228: Vector3d newLocation = new Vector3d();
229:
230: Vector3d tmpVector = new Vector3d();
231:
232: /**
233: * Code to process the device's streaming input.
234: */
235: public void processStreamInput() {
236: long time;
237: SensorRead currentRead;
238: int currentSensor = 0;
239:
240: // The following must also set currentSensor
241: this
242: .PollInput(fileDescriptor, PositionOrientation,
243: ButtonArray);
244:
245: time = System.currentTimeMillis();
246:
247: setMatrixFromValues(PositionOrientation, newMatrix);
248:
249: currentRead = sensors[currentSensor].getCurrentSensorRead();
250:
251: currentRead.get(oldTransform);
252:
253: oldTransform.get(oldLocation);
254: tmpVector.set(PositionOrientation[0] / 1000,
255: PositionOrientation[1] / 1000,
256: PositionOrientation[2] / 1000);
257: oldLocation.sub(tmpVector);
258: newTransform.setTranslation(oldLocation);
259:
260: oldTransform.getRotationScale(oldMatrix);
261:
262: tmpMatrix.mul(newMatrix, oldMatrix);
263:
264: newTransform.setRotationScale(tmpMatrix);
265: sensors[currentSensor].setNextSensorRead(time, newTransform,
266: ButtonArray);
267:
268: }
269:
270: }
|