bool IntersectPolygon (TPolygon p, TVector3 *v) {
TRay ray;
TVector3 nml, edge_nml, edge_vec;
TVector3 pt;
double d, s, nuDotProd, wec;
double edge_len, t, distsq;
int i;
nml = MakeNormal (p, v);
ray.pt = MakeVector (0., 0., 0.);
ray.vec = nml;
nuDotProd = DotProduct (nml, ray.vec);
if (fabs(nuDotProd) < EPS)
return false;
d = - (nml.x * v[p.vertices[0]].x +
nml.y * v[p.vertices[0]].y +
nml.z * v[p.vertices[0]].z);
if (fabs (d) > 1) return false;
for (i=0; i < p.num_vertices; i++) {
TVector3 *v0, *v1;
v0 = &v[p.vertices[i]];
v1 = &v[p.vertices[ (i+1) % p.num_vertices ]];
edge_vec = SubtractVectors (*v1, *v0);
edge_len = NormVector (&edge_vec);
t = - DotProduct (*((TVector3 *) v0), edge_vec);
if (t < 0) {
distsq = MAG_SQD2 (*v0);
} else if (t > edge_len) {
distsq = MAG_SQD2 (*v1);
} else {
*v0 = AddVectors (*v0, ScaleVector (t, edge_vec));
distsq = MAG_SQD2 (*v0);
}
if (distsq <= 1) return true;
}
s = - (d + DotProduct (nml, MakeVector (ray.pt.x, ray.pt.y, ray.pt.z))) / nuDotProd;
pt = AddVectors (ray.pt, ScaleVector (s, ray.vec));
for (i=0; i < p.num_vertices; i++) {
edge_nml = CrossProduct (nml,
SubtractVectors (v[p.vertices[ (i+1) % p.num_vertices ]], v[p.vertices[i]]));
wec = DotProduct (SubtractVectors (pt, v[p.vertices[i]]), edge_nml);
if (wec < 0) return false;
}
return true;
}
complex stb_InnerProduct(stabiliser *phi1, stabiliser *phi2)
{
affine_sp a = afp_Copy(phi1->a); //This won't work. I need to define a copy method
stabiliser stab;
stab.a = &a;
stab.q = phi1->q;
affine_sp *a1 = phi1->a, *a2 = phi2->a;
quadratic_fm *q1 = phi1->q, *q2 = phi2->q;
for (int i = a2->k +1; i < a2->n; i++){
alpha = InnerProduct(a2->h, a2->GBar[i])
res = shrink(&stab, a2->HBar[i], alpha);
if (res == EMPTY){
return (complex) 0;
}
}
short *y = (short *)calloc(a2->k, sizeof(short));
short *scratch_space = (short *)calloc(a2->n, sizeof(short));
short res = 0;
AddVectors(a2->n, scratch_space, a1->h);
AddVectors(a2->n, scratch_space, a2->h);
short **R = (short **)calloc(a2->n, sizeof(short*));
for (int i=0; i<a2->n; i++){R[i] = (short *)calloc(a2->n, sizeof(short));}
for (int i = 0; i < a2->k; i++){
y[i] = InnerProduct(scratch_space, a2->Gbar[i]);
for (int j = 0; j < a.k; j++){
R[j][i] = InnerProduct(a.G[j], a2.GBar[i]);
}
}
qfm_BasisChange(q2, R);
qfm_ShiftChange(q2, y);
for (int i = 0; i<a2->n; i++){a2->h[i] = a1->h[i];}
//Cleanup
for(int i = 0; i<q2->k; i++){free(R[i]);}
free(scratch_space);
free (R);
free(y);
//Find the final quadratic form q
q.Q = Modulo(q1->Q - q2->Q, 8);
for (int i = 0; i < q.K; i++){
q.D[i] = Modulo(q1->D[i]-q2->D[i], 8);
for (int j = 0; j<q.k; j++){
q.J[i][j] = Modulo(q1->J[i][j] - q2->J[i][j], 8);
}
}
return pow(2, -1*(a1->k - a2->k)/2)*ExponentialSum(q);
}
double DexAnalogMixin::FilterLoadForce( Vector3 load_force ) {
// Combine the new force sample with previous filtered value (recursive filtering).
ScaleVector( filteredLoadForce, filteredLoadForce, filterConstant );
AddVectors( filteredLoadForce, filteredLoadForce, load_force );
ScaleVector( filteredLoadForce, filteredLoadForce, 1.0 / (1.0 + filterConstant ));
// Return the filtered value in place.
CopyVector( load_force, filteredLoadForce );
return( VectorNorm( filteredLoadForce ) );
}
double DexAnalogMixin::FilterManipulandumRotations( Vector3 rotations ) {
// Combine the new rotations sample with previous filtered value (recursive filtering).
ScaleVector( filteredManipulandumRotations, filteredManipulandumRotations, filterConstant );
AddVectors( filteredManipulandumRotations, filteredManipulandumRotations, rotations );
ScaleVector( filteredManipulandumRotations, filteredManipulandumRotations, 1.0 / (1.0 + filterConstant ));
// Return the filtered value in place.
CopyVector( rotations, filteredManipulandumRotations );
return( VectorNorm( rotations ) );
}
double DexAnalogMixin::FilterCoP( int which_ati, Vector3 center_of_pressure ) {
// Combine the new force sample with previous filtered value (recursive filtering).
ScaleVector( filteredCoP[which_ati], filteredCoP[which_ati], filterConstant );
AddVectors( filteredCoP[which_ati], filteredCoP[which_ati], center_of_pressure );
ScaleVector( filteredCoP[which_ati], filteredCoP[which_ati], 1.0 / (1.0 + filterConstant ));
// Return the filtered value in place.
CopyVector( center_of_pressure, filteredCoP[which_ati] );
return( VectorNorm( filteredCoP[which_ati] ) );
}
double DexAnalogMixin::FilterAcceleration( Vector3 acceleration ) {
// Combine the new position sample with previous filtered value (recursive filtering).
ScaleVector( filteredAcceleration, filteredAcceleration, filterConstant );
AddVectors( filteredAcceleration, filteredAcceleration, acceleration );
ScaleVector( filteredAcceleration, filteredAcceleration, 1.0 / (1.0 + filterConstant ));
// Return the filtered value in place.
CopyVector( acceleration, filteredAcceleration );
return( VectorNorm( acceleration ) );
}
int DexMouseTracker::RetrieveMarkerFrames( CodaFrame frames[], int max_frames ) {
int n_frames;
int previous_frame;
int next_frame;
Vector3 jump, delta;
double time, interval, offset, relative;
// Copy data into an array.
previous_frame = 0;
next_frame = 0;
// Fill an array of frames at a constant frequency by interpolating the
// frames that were taken at a variable frequency in real time.
for ( n_frames = 0; n_frames < max_frames; n_frames++ ) {
// Compute the time of each slice at a constant sampling frequency.
time = (double) n_frames * samplePeriod;
frames[n_frames].time = (float) time;
// See if we have caught up with the real-time data.
if ( time > recordedMarkerFrames[next_frame].time ) previous_frame = next_frame;
// Find the next real-time frame that has a time stamp later than this one.
// It could be that this true already.
while ( recordedMarkerFrames[next_frame].time <= time && next_frame < nAcqFrames ) next_frame++;
// If we reached the end of the real-time samples, then we are done.
if ( next_frame >= nAcqFrames ) break;
// Compute the time difference between the two adjacent real-time frames.
interval = recordedMarkerFrames[next_frame].time - recordedMarkerFrames[previous_frame].time;
// Compute the time between the current frame and the previous real_time frame.
offset = time - recordedMarkerFrames[previous_frame].time;
// Use the relative time to interpolate.
relative = offset / interval;
for ( int j = 0; j < nMarkers; j++ ) {
frames[n_frames].marker[j].visibility =
recordedMarkerFrames[previous_frame].marker[j].visibility &&
recordedMarkerFrames[next_frame].marker[j].visibility;
if ( frames[n_frames].marker[j].visibility ) {
SubtractVectors( jump,
recordedMarkerFrames[next_frame].marker[j].position,
recordedMarkerFrames[previous_frame].marker[j].position );
ScaleVector( delta, jump, (float) relative );
AddVectors( frames[n_frames].marker[j].position, recordedMarkerFrames[previous_frame].marker[j].position, delta );
}
}
}
return( n_frames );
}
TVector3 CCourse::FindCourseNormal (double x, double z) const {
double *elevation = Course.elevation;
int x0, x1, y0, y1;
GetIndicesForPoint (x, z, &x0, &y0, &x1, &y1);
TIndex2 idx0, idx1, idx2;
double u, v;
FindBarycentricCoords (x, z, &idx0, &idx1, &idx2, &u, &v);
const TVector3& n0 = Course.nmls[ idx0.i + nx * idx0.j ];
const TVector3& n1 = Course.nmls[ idx1.i + nx * idx1.j ];
const TVector3& n2 = Course.nmls[ idx2.i + nx * idx2.j ];
TVector3 p0 = COURSE_VERTX (idx0.i, idx0.j);
TVector3 p1 = COURSE_VERTX (idx1.i, idx1.j);
TVector3 p2 = COURSE_VERTX (idx2.i, idx2.j);
TVector3 smooth_nml = AddVectors (
ScaleVector (u, n0),
AddVectors (ScaleVector (v, n1), ScaleVector (1.-u-v, n2)));
TVector3 tri_nml = CrossProduct (
SubtractVectors (p1, p0), SubtractVectors (p2, p0));
NormVector (tri_nml);
double min_bary = min (u, min (v, 1. - u - v));
double interp_factor = min (min_bary / NORM_INTERPOL, 1.0);
TVector3 interp_nml = AddVectors (
ScaleVector (interp_factor, tri_nml),
ScaleVector (1.-interp_factor, smooth_nml));
NormVector (interp_nml);
return interp_nml;
}
/* Take current data in s, make it the intial inputs for the transformed vector ns
* return the updated values in NS to calling function */
void transform_NS(ns_stance *s, vector *ns){
vector working_vector;
vector working_vector_2;
//initialize current ns_vector
ns->v[0] = (s->x);
ns->v[1] = (s->y);
ns->v[2] = (s->theta);
//PrintVector(ns);//diagnostic
//shift
AddVectors(ns, ns_shift_vector, &working_vector);
//diagnostic
//printf("Shift result = ");
//PrintVector(&working_vector);
//rotate
MultMatVec(ns_rot_matrix, &working_vector, &working_vector_2);
//diagnostic
//printf("Rotate Result = ");
//PrintVector(&working_vector_2);
//scale
// Update Scaling Matrix based on current signal strength [NOT WORKING RIGHT]
if(s->sig > 15000) {
scale_matrix->v[0][0] = 1.0 / (NS_TICKS_PER_CM - 15);
scale_matrix->v[1][1] = scale_matrix->v[0][0];
}
else if(s->sig > 4000) {
scale_matrix->v[0][0] = 1.0 / ( ((-3.0 * s->sig) / 1100.0) + 86.0 );
scale_matrix->v[1][1] = scale_matrix->v[0][0];
}
else {
scale_matrix->v[0][0] = 1.0 / (NS_TICKS_PER_CM + 15);
scale_matrix->v[1][1] = scale_matrix->v[0][0];
}
MultMatVec(scale_matrix, &working_vector_2, ns);
//diagnostic
//printf("Scaling Result = ");
//PrintVector(ns);
}
void SpringSys::UpdateParticleState(TimeValue t, Tab<Matrix3> tmArray, int index, TimeValue Delta)
{
Point3 t_pos, t_vel, orig_pos, orig_vel;
Matrix3 tm;
for (int b=0;b<parts[index].GetSprings()->length();b++)
{
if (parts[index].GetSpring(b)->GetPointConstraint()->GetIndex() < tmArray.Count())
{
tm = tmArray[parts[index].GetSpring(b)->GetPointConstraint()->index];
ComputeControlledParticleForce(tm, index, b); /* compute bone deriv used by ApplySpring */
}
}
Clear_Forces(index); /* zero the force accumulators */
Compute_Forces(t, index); /* magic force function */
ComputeDerivative(index, t_pos, t_vel); /* get deriv */
//ComputeDerivative(pIndex, t_pos, t_vel, Delta); /* get deriv */
ScaleVectors(t_pos, t_vel, Delta); /* scale it */
GetParticleState(index, orig_pos, orig_vel); /* get state */
AddVectors(orig_pos, orig_vel, t_pos, t_vel); /* add -> temp2 */
SetParticleState(index, t_pos, t_vel); /* update state */
}
int DexApparatus::CheckMovementAmplitude( double min, double max,
double dirX, double dirY, double dirZ,
const char *msg, const char *picture ) {
const char *fmt;
bool error = false;
int i, k, m;
int first, last;
double N = 0.0, sd;
Matrix3x3 Sxy;
Vector3 delta, mean;
Vector3 direction, vect;
// TODO: Should normalize the direction vector here.
direction[X] = dirX;
direction[Y] = dirY;
direction[Z] = dirZ;
// First we should look for the start and end of the actual movement based on
// events such as when the subject reaches the first target.
FindAnalysisFrameRange( first, last );
// Compute the mean position.
N = 0.0;
CopyVector( mean, zeroVector );
for ( i = first; i < last; i++ ) {
if ( acquiredManipulandumState[i].visibility ) {
if ( abs( acquiredManipulandumState[i].position[X] ) > 1000 ) {
i = i;
}
N++;
AddVectors( mean, mean, acquiredManipulandumState[i].position );
}
}
// If there is no valid position data, signal an error.
if ( N <= 0.0 ) {
monitor->SendEvent( "Movement extent - No valid data." );
sd = 0.0;
error = true;
}
else {
// This is the mean.
ScaleVector( mean, mean, 1.0 / N );;
// Compute the sums required for the variance calculation.
CopyMatrix( Sxy, zeroMatrix );
N = 0.0;
for ( i = first; i < last; i ++ ) {
if ( acquiredManipulandumState[i].visibility ) {
N++;
SubtractVectors( delta, acquiredManipulandumState[i].position, mean );
for ( k = 0; k < 3; k++ ) {
for ( m = 0; m < 3; m++ ) {
Sxy[k][m] += delta[k] * delta[m];
}
}
}
}
// If we have some data, compute the directional variance and then
// the standard deviation along that direction;
// This is just a scalar times a matrix.
// Sxy has to be a double or we risk underflow when computing the sums.
for ( k = 0; k < 3; k++ ) {
vect[k] = 0;
for ( m = 0; m < 3; m++ ) {
Sxy[k][m] /= N;
}
}
// This is a matrix multiply.
for ( k = 0; k < 3; k++ ) {
for ( m = 0; m < 3; m++ ) {
vect[k] += Sxy[m][k] * direction[m];
}
}
// Compute the length of the vector, which is the variance along
// the specified direction. Then take the square root of that
// magnitude to get the standard deviation along that direction.
sd = sqrt( VectorNorm( vect ) );
// Check if the computed value is in the desired range.
error = ( sd < min || sd > max );
}
// If not, signal the error to the subject.
// Here I take the approach of calling a method to signal the error.
// We agree instead that the routine should either return a null pointer
// if there is no error, or return the error message as a static string.
if ( error ) {
// If the user provided a message to signal a visibilty error, use it.
// If not, generate a generic message.
if ( !msg ) msg = "Movement extent outside range.";
// The message could be different depending on whether the maniplulandum
// was not moved enough, or if it just could not be seen.
if ( N <= 0.0 ) fmt = "%s\n Manipulandum not visible.";
else fmt = "%s\n Measured: %f\n Desired range: %f - %f\n Direction: < %.2f %.2f %.2f>";
int response = fSignalError( MB_ABORTRETRYIGNORE, picture, fmt, msg, sd, min, max, dirX, dirY, dirZ );
if ( response == IDABORT ) return( ABORT_EXIT );
if ( response == IDRETRY ) return( RETRY_EXIT );
if ( response == IDIGNORE ) return( IGNORE_EXIT );
}
// This is my means of signalling the event.
monitor->SendEvent( "Movement extent OK.\n Measured: %f\n Desired range: %f - %f\n Direction: < %.2f %.2f %.2f>", sd, min, max, dirX, dirY, dirZ );
return( NORMAL_EXIT );
}
void CEnvironment::DrawFog () {
if (!fog.is_on)
return;
TPlane bottom_plane, top_plane;
TVector3 left, right, vpoint;
TVector3 topleft, topright;
TVector3 bottomleft, bottomright;
// the clipping planes are calculated by view frustum (view.cpp)
const TPlane& leftclip = get_left_clip_plane ();
const TPlane& rightclip = get_right_clip_plane ();
const TPlane& farclip = get_far_clip_plane ();
const TPlane& bottomclip = get_bottom_clip_plane ();
// --------------- calculate the planes ---------------------------
float slope = tan (ANGLES_TO_RADIANS (Course.GetCourseAngle()));
// TPlane left_edge_plane = MakePlane (1.0, 0.0, 0.0, 0.0);
// TPlane right_edge_plane = MakePlane (-1.0, 0.0, 0.0, Course.width);
bottom_plane.nml = TVector3(0.0, 1, -slope);
float height = Course.GetBaseHeight (0);
bottom_plane.d = -height * bottom_plane.nml.y;
top_plane.nml = bottom_plane.nml;
height = Course.GetMaxHeight (0) + fog.height;
top_plane.d = -height * top_plane.nml.y;
if (!IntersectPlanes (bottom_plane, farclip, leftclip, &left)) return;
if (!IntersectPlanes (bottom_plane, farclip, rightclip, &right)) return;
if (!IntersectPlanes (top_plane, farclip, leftclip, &topleft)) return;
if (!IntersectPlanes (top_plane, farclip, rightclip, &topright)) return;
if (!IntersectPlanes (bottomclip, farclip, leftclip, &bottomleft)) return;
if (!IntersectPlanes (bottomclip, farclip, rightclip, &bottomright)) return;
TVector3 leftvec = SubtractVectors (topleft, left);
TVector3 rightvec = SubtractVectors (topright, right);
// --------------- draw the fog plane -----------------------------
ScopedRenderMode rm(FOG_PLANE);
glEnable (GL_FOG);
// only the alpha channel is used
float bottom_dens[4] = {0, 0, 0, 1.0};
float top_dens[4] = {0, 0, 0, 0.9};
float leftright_dens[4] = {0, 0, 0, 0.3};
float top_bottom_dens[4] = {0, 0, 0, 0.0};
glBegin (GL_QUAD_STRIP);
glColor4fv (bottom_dens);
glVertex3f (bottomleft.x, bottomleft.y, bottomleft.z);
glVertex3f (bottomright.x, bottomright.y, bottomright.z);
glVertex3f (left.x, left.y, left.z);
glVertex3f (right.x, right.y, right.z);
glColor4fv (top_dens);
glVertex3f (topleft.x, topleft.y, topleft.z);
glVertex3f (topright.x, topright.y, topright.z);
glColor4fv (leftright_dens);
vpoint = AddVectors (topleft, leftvec);
glVertex3f (vpoint.x, vpoint.y, vpoint.z);
vpoint = AddVectors (topright, rightvec);
glVertex3f (vpoint.x, vpoint.y, vpoint.z);
glColor4fv (top_bottom_dens);
vpoint = AddVectors (topleft, ScaleVector (3.0, leftvec));
glVertex3f (vpoint.x, vpoint.y, vpoint.z);
vpoint = AddVectors (topright, ScaleVector (3.0, rightvec));
glVertex3f (vpoint.x, vpoint.y, vpoint.z);
glEnd();
}
bool DexMouseTracker::GetCurrentMarkerFrame( CodaFrame &frame ) {
POINT mouse_position;
GetCursorPos( &mouse_position );
RECT rect;
GetWindowRect( GetDesktopWindow(), &rect );
Vector3 position, rotated;
Vector3 x_dir, y_span, z_span;
Vector3 x_displacement, y_displacement, z_displacement;
double x, y, z;
Quaternion Ry, Rz, Q, nominalQ, midQ;
Matrix3x3 xform = {{-1.0, 0.0, 0.0},{0.0, 0.0, 1.0},{0.0, 1.0, 0.0}};
int mrk, id;
// Just set the target frame markers at their nominal fixed positions.
for ( mrk = 0; mrk < nFrameMarkers; mrk++ ) {
id = FrameMarkerID[mrk];
CopyVector( frame.marker[id].position, TargetFrameBody[mrk] );
}
// Shift the vertical bar markers to simulate being in the left position.
if ( IsDlgButtonChecked( dlg, IDC_LEFT ) ) {
frame.marker[DEX_NEGATIVE_BAR_MARKER].position[X] += 300.0;
frame.marker[DEX_POSITIVE_BAR_MARKER].position[X] += 300.0;
}
// Transform the marker positions of the target box and frame according
// to whether the system was upright or supine when it was aligned and
// according to whether the system is currently installed in the upright
// or supine configuration.
if ( ( IsDlgButtonChecked( dlg, IDC_SUPINE ) && TRACKER_ALIGNED_SUPINE != SendDlgItemMessage( dlg, IDC_ALIGNMENT, CB_GETCURSEL, 0, 0 ) ) ||
( IsDlgButtonChecked( dlg, IDC_SEATED ) && TRACKER_ALIGNED_UPRIGHT != SendDlgItemMessage( dlg, IDC_ALIGNMENT, CB_GETCURSEL, 0, 0 ) ) ) {
for ( mrk = 0; mrk < nFrameMarkers; mrk++ ) {
id = FrameMarkerID[mrk];
position[X] = - frame.marker[id].position[X];
position[Z] = frame.marker[id].position[Y];
position[Y] = frame.marker[id].position[Z];
CopyVector( frame.marker[id].position, position );
}
MatrixToQuaternion( nominalQ, xform );
}
else CopyQuaternion( nominalQ, nullQuaternion );
// Now set the visibility flag as a funciton of the GUI.
if ( IsDlgButtonChecked( dlg, IDC_BAR_OCCLUDED ) ) {
frame.marker[DEX_NEGATIVE_BAR_MARKER].visibility = false;
frame.marker[DEX_POSITIVE_BAR_MARKER].visibility = false;
}
else {
frame.marker[DEX_NEGATIVE_BAR_MARKER].visibility = true;
frame.marker[DEX_POSITIVE_BAR_MARKER].visibility = true;
}
if ( IsDlgButtonChecked( dlg, IDC_BOX_OCCLUDED ) ) {
frame.marker[DEX_NEGATIVE_BOX_MARKER].visibility = false;
frame.marker[DEX_POSITIVE_BOX_MARKER].visibility = false;
}
else {
frame.marker[DEX_NEGATIVE_BOX_MARKER].visibility = true;
frame.marker[DEX_POSITIVE_BOX_MARKER].visibility = true;
}
// Map mouse coordinates to world coordinates. The factors used here are empirical.
y = (double) ( mouse_position.y - rect.top ) / (double) ( rect.bottom - rect.top );
z = (double) (mouse_position.x - rect.right) / (double) ( rect.left - rect.right );
x = 0.0;
// By default, the orientation of the manipulandum is the nominal orientation.
CopyQuaternion( Q, nominalQ );
// Simulate wobbly movements of the manipulandum. This has not really been tested.
if ( IsDlgButtonChecked( dlg, IDC_CODA_WOBBLY ) ) {
// We will make the manipulandum rotate in a strange way as a function of the distance from 0.
// This is just so that we can test the routines that compute the manipulandum position and orientation.
double theta = y * 45.0;
double gamma = - z * 45.0;
SetQuaterniond( Ry, theta, iVector );
SetQuaterniond( Rz, gamma, jVector );
MultiplyQuaternions( midQ, Rz, nominalQ );
MultiplyQuaternions( Q, Ry, midQ );
// Make the movement a little bit in X as well so that we test the routines in 3D.
x = 0.0 + 5.0 * sin( y / 80.0);
}
// Map screen position of the mouse pointer to 3D position of the wrist and manipulandum.
// Top of the screen corresponds to the bottom of the bar and vice versa. It's inverted to protect the right hand rule.
// Right of the screen correponds to the nearest horizontal target and left corresponds to the farthest.
// The X position is set to be just to the right of the box.
SubtractVectors( y_span, frame.marker[DEX_POSITIVE_BAR_MARKER].position, frame.marker[DEX_NEGATIVE_BAR_MARKER].position );
SubtractVectors( x_dir, frame.marker[DEX_POSITIVE_BOX_MARKER].position, frame.marker[DEX_NEGATIVE_BOX_MARKER].position );
NormalizeVector( x_dir );
ComputeCrossProduct( z_span, x_dir, y_span );
ScaleVector( y_displacement, y_span, y );
ScaleVector( z_displacement, z_span, z );
ScaleVector( x_displacement, x_dir, x );
// Reference position is the bottom target on the vertical target bar.
CopyVector( position, frame.marker[DEX_NEGATIVE_BAR_MARKER].position );
// Place the manipulandum to the right of the box, even if the target bar is in the left position.
position[X] = frame.marker[DEX_NEGATIVE_BOX_MARKER].position[X];
// Shift the position in X if the is any wobble to it.
AddVectors( position, position, x_displacement );
// Shift the position in Y and Z according to the displacements computed from the mouse position.
AddVectors( position, position, y_displacement );
AddVectors( position, position, z_displacement );
frame.time = DexTimerElapsedTime( acquisitionTimer );
// Displace the manipulandum with the mouse and make it rotate.
for ( mrk = 0; mrk < nManipulandumMarkers; mrk++ ) {
id = ManipulandumMarkerID[mrk];
RotateVector( rotated, Q, ManipulandumBody[mrk] );
AddVectors( frame.marker[id].position, position, rotated );
frame.marker[id].visibility = true;
}
// Displace the wrist with the mouse, but don't rotate it.
for ( mrk = 0; mrk < nWristMarkers; mrk++ ) {
id = WristMarkerID[mrk];
AddVectors( frame.marker[id].position, position, WristBody[mrk] );
frame.marker[id].visibility = true;
}
// Output the position and orientation used to compute the simulated
// marker positions. This is for testing only.
fprintf( fp, "%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\n",
frame.time,
position[X], position[Y], position[Z],
Q[X], Q[Y], Q[Z], Q[M] );
return( true );
}
int DexMouseTracker::RetrieveMarkerFrames( CodaFrame frames[], int max_frames, int unit ) {
int previous;
int next;
int frm;
Vector3f jump, delta;
double time, interval, offset, relative;
// Copy data into an array.
previous = 0;
next = 1;
// Fill an array of frames at a constant frequency by interpolating the
// frames that were taken at a variable frequency in real time.
for ( frm = 0; frm < max_frames; frm++ ) {
// Compute the time of each slice at a constant sampling frequency.
time = (double) frm * samplePeriod;
frames[frm].time = (float) time;
// Fill the first frames with the same position as the first polled frame;
if ( time < polledMarkerFrames[previous].time ) {
CopyMarkerFrame( frames[frm], polledMarkerFrames[previous] );
}
else {
// See if we have caught up with the real-time data.
if ( time > polledMarkerFrames[next].time ) {
previous = next;
// Find the next real-time frame that has a time stamp later than this one.
while ( polledMarkerFrames[next].time <= time && next < nPolled ) next++;
// If we reached the end of the real-time samples, then we are done.
if ( next >= nPolled ) {
break;
}
}
// Compute the time difference between the two adjacent real-time frames.
interval = polledMarkerFrames[next].time - polledMarkerFrames[previous].time;
// Compute the time between the current frame and the previous real_time frame.
offset = time - polledMarkerFrames[previous].time;
// Use the relative time to interpolate.
relative = offset / interval;
for ( int j = 0; j < nMarkers; j++ ) {
// Both the previous and next polled frame must be visible for the
// interpolated sample to be visible.
frames[frm].marker[j].visibility =
( polledMarkerFrames[previous].marker[j].visibility
&& polledMarkerFrames[next].marker[j].visibility );
if ( frames[frm].marker[j].visibility ) {
SubtractVectors( jump,
polledMarkerFrames[next].marker[j].position,
polledMarkerFrames[previous].marker[j].position );
ScaleVector( delta, jump, (float) relative );
AddVectors( frames[frm].marker[j].position, polledMarkerFrames[previous].marker[j].position, delta );
}
}
}
}
nAcqFrames = frm;
return( nAcqFrames );
}
void CEnvironment::DrawFog () {
TPlane bottom_plane, top_plane;
TVector3 left, right;
TVector3 topleft, topright;
TVector3 bottomleft = NullVec;
TVector3 bottomright = NullVec;
float height;
if (!fog.is_on) return;
// the clipping planes are calculated by view frustum (view.cpp)
leftclip = get_left_clip_plane ();
rightclip = get_right_clip_plane ();
farclip = get_far_clip_plane ();
bottomclip = get_bottom_clip_plane ();
// --------------- calculate the planes ---------------------------
float slope = tan (ANGLES_TO_RADIANS (Course.GetCourseAngle()));
// TPlane left_edge_plane = MakePlane (1.0, 0.0, 0.0, 0.0);
// TPlane right_edge_plane = MakePlane (-1.0, 0.0, 0.0, Course.width);
bottom_plane.nml = MakeVector (0.0, 1, -slope);
height = Course.GetBaseHeight (0);
bottom_plane.d = -height * bottom_plane.nml.y;
top_plane.nml = bottom_plane.nml;
height = Course.GetMaxHeight (0) + fog.height;
top_plane.d = -height * top_plane.nml.y;
if (!IntersectPlanes (bottom_plane, farclip, leftclip, &left)) return;
if (!IntersectPlanes (bottom_plane, farclip, rightclip, &right)) return;
if (!IntersectPlanes (top_plane, farclip, leftclip, &topleft)) return;
if (!IntersectPlanes (top_plane, farclip, rightclip, &topright)) return;
if (!IntersectPlanes (bottomclip, farclip, leftclip, &bottomleft)) return;
if (!IntersectPlanes (bottomclip, farclip, rightclip, &bottomright)) return;
TVector3 leftvec = SubtractVectors (topleft, left);
TVector3 rightvec = SubtractVectors (topright, right);
TVector3 vpoint1 = AddVectors (topleft, leftvec);
TVector3 vpoint2 = AddVectors (topright, rightvec);
TVector3 vpoint3 = AddVectors (topleft, ScaleVector (3.0, leftvec));
TVector3 vpoint4 = AddVectors (topright, ScaleVector (3.0, rightvec));
// --------------- draw the fog plane -----------------------------
set_gl_options (FOG_PLANE);
glEnable (GL_FOG);
glEnableClientState(GL_COLOR_ARRAY);
glEnableClientState(GL_VERTEX_ARRAY);
// only the alpha channel is used
const GLfloat col[] = {
// bottom density
0, 0, 0, 1.0,
0, 0, 0, 1.0,
0, 0, 0, 1.0,
0, 0, 0, 1.0,
// top density
0, 0, 0, 0.9,
0, 0, 0, 0.9,
// left/right density
0, 0, 0, 0.3,
0, 0, 0, 0.3,
// top/bottom density
0, 0, 0, 0.0,
0, 0, 0, 0.0
};
const GLfloat vtx[] = {
bottomleft.x, bottomleft.y, bottomleft.z,
bottomright.x, bottomright.y, bottomright.z,
left.x, left.y, left.z,
right.x, right.y, right.z,
topleft.x, topleft.y, topleft.z,
topright.x, topright.y, topright.z,
vpoint1.x, vpoint1.y, vpoint1.z,
vpoint2.x, vpoint2.y, vpoint2.z,
vpoint3.x, vpoint3.y, vpoint3.z,
vpoint4.x, vpoint4.y, vpoint4.z
};
glColorPointer(4, GL_FLOAT, 0, col);
glVertexPointer(3, GL_FLOAT, 0, vtx);
glDrawArrays(GL_TRIANGLE_STRIP, 0, 4);
glDisableClientState(GL_VERTEX_ARRAY);
glDisableClientState(GL_COLOR_ARRAY);
}
void CCourse::CalcNormals () {
for (int y=0; y<ny; y++) {
for (int x=0; x<nx; x++) {
TVector3 nml(0.0, 0.0, 0.0);
TVector3 p0 (XCD(x), ELEV(x,y), ZCD(y));
if ((x + y) % 2 == 0) {
if (x > 0 && y > 0) {
TVector3 p1 = NMLPOINT(x, y-1);
TVector3 p2 = NMLPOINT(x-1,y-1);
TVector3 v1 = SubtractVectors (p1, p0);
TVector3 v2 = SubtractVectors (p2, p0);
TVector3 n = CrossProduct (v2, v1);
NormVector (n);
nml = AddVectors (nml, n);
p1 = NMLPOINT (x-1, y-1);
p2 = NMLPOINT (x-1, y);
v1 = SubtractVectors (p1, p0);
v2 = SubtractVectors (p2, p0);
n = CrossProduct (v2, v1);
NormVector (n);
nml = AddVectors (nml, n);
}
if (x > 0 && y < ny-1) {
TVector3 p1 = NMLPOINT(x-1,y);
TVector3 p2 = NMLPOINT(x-1,y+1);
TVector3 v1 = SubtractVectors (p1, p0);
TVector3 v2 = SubtractVectors (p2, p0);
TVector3 n = CrossProduct (v2, v1);
NormVector (n);
nml = AddVectors (nml, n);
p1 = NMLPOINT(x-1,y+1);
p2 = NMLPOINT(x ,y+1);
v1 = SubtractVectors (p1, p0);
v2 = SubtractVectors (p2, p0);
n = CrossProduct (v2, v1);
NormVector (n);
nml = AddVectors (nml, n);
}
if (x < nx-1 && y > 0) {
TVector3 p1 = NMLPOINT(x+1,y);
TVector3 p2 = NMLPOINT(x+1,y-1);
TVector3 v1 = SubtractVectors (p1, p0);
TVector3 v2 = SubtractVectors (p2, p0);
TVector3 n = CrossProduct (v2, v1);
NormVector (n);
nml = AddVectors (nml, n);
p1 = NMLPOINT(x+1,y-1);
p2 = NMLPOINT(x ,y-1);
v1 = SubtractVectors (p1, p0);
v2 = SubtractVectors (p2, p0);
n = CrossProduct (v2, v1);
NormVector (n);
nml = AddVectors (nml, n);
}
if (x < nx-1 && y < ny-1) {
TVector3 p1 = NMLPOINT(x+1,y);
TVector3 p2 = NMLPOINT(x+1,y+1);
TVector3 v1 = SubtractVectors (p1, p0);
TVector3 v2 = SubtractVectors (p2, p0);
TVector3 n = CrossProduct (v1, v2);
NormVector (n);
nml = AddVectors (nml, n);
p1 = NMLPOINT(x+1,y+1);
p2 = NMLPOINT(x ,y+1);
v1 = SubtractVectors (p1, p0);
v2 = SubtractVectors (p2, p0);
n = CrossProduct (v1, v2);
NormVector (n);
nml = AddVectors (nml, n);
}
} else {
if (x > 0 && y > 0) {
TVector3 p1 = NMLPOINT(x, y-1);
TVector3 p2 = NMLPOINT(x-1,y);
TVector3 v1 = SubtractVectors (p1, p0);
TVector3 v2 = SubtractVectors (p2, p0);
TVector3 n = CrossProduct (v2, v1);
NormVector (n);
nml = AddVectors (nml, n);
}
if (x > 0 && y < ny-1) {
TVector3 p1 = NMLPOINT(x-1,y);
TVector3 p2 = NMLPOINT(x ,y+1);
TVector3 v1 = SubtractVectors (p1, p0);
TVector3 v2 = SubtractVectors (p2, p0);
TVector3 n = CrossProduct (v2, v1);
NormVector (n);
nml = AddVectors (nml, n);
}
if (x < nx-1 && y > 0) {
TVector3 p1 = NMLPOINT(x+1,y);
TVector3 p2 = NMLPOINT(x ,y-1);
TVector3 v1 = SubtractVectors (p1, p0);
TVector3 v2 = SubtractVectors (p2, p0);
TVector3 n = CrossProduct (v2, v1);
NormVector (n);
nml = AddVectors (nml, n);
}
if (x < nx-1 && y < ny-1) {
TVector3 p1 = NMLPOINT(x+1,y);
TVector3 p2 = NMLPOINT(x ,y+1);
TVector3 v1 = SubtractVectors (p1, p0);
TVector3 v2 = SubtractVectors (p2, p0);
TVector3 n = CrossProduct (v1, v2);
NormVector (n);
nml = AddVectors (nml, n);
}
}
NormVector (nml);
nmls [x + nx * y] = nml;
continue;
}
}
}
int DexApparatus::CheckMovementCycles( int min_cycles, int max_cycles,
float dirX, float dirY, float dirZ,
float hysteresis, const char *msg, const char *picture ) {
const char *fmt;
bool error = false;
float displacement = 0.0;
bool positive = false;
int cycles = 0;
Vector3 direction, mean, delta;
int i;
int first, last;
double N = 0.0;
// Just make sure that the user gave a positive value for hysteresis.
hysteresis = fabs( hysteresis );
// TODO: Should normalize the direction vector here.
direction[X] = dirX;
direction[Y] = dirY;
direction[Z] = dirZ;
// First we should look for the start and end of the actual movement based on
// events such as when the subject reaches the first target.
FindAnalysisFrameRange( first, last );
for ( first = first; first < last; first++ ) if ( acquiredManipulandumState[first].visibility ) break;
// Compute the mean position.
N = 0.0;
CopyVector( mean, zeroVector );
for ( i = first; i < last; i++ ) {
if ( acquiredManipulandumState[i].visibility ) {
N++;
AddVectors( mean, mean, acquiredManipulandumState[i].position );
}
}
// If there is no valid position data, signal an error.
if ( N <= 0.0 ) {
monitor->SendEvent( "Movement cycles - No valid data." );
cycles = 0;
error = true;
}
else {
// This is the mean.
ScaleVector( mean, mean, 1.0 / N );
// Step through the trajectory, counting positive zero crossings.
for ( i = first; i < last; i ++ ) {
// Compute the displacements around the mean.
if ( acquiredManipulandumState[i].visibility ) {
SubtractVectors( delta, acquiredManipulandumState[i].position, mean );
displacement = DotProduct( delta, direction );
}
// If on the positive side of the mean, just look for the negative transition.
if ( positive ) {
if ( displacement < - hysteresis ) positive = false;
}
// If on the negative side, look for the positive transition.
// If we find one, count another cycle.
else {
if ( displacement > hysteresis ) {
positive = true;
cycles++;
}
}
}
// Check if the computed number of cycles is in the desired range.
error = ( cycles < min_cycles || cycles > max_cycles );
}
// If not, signal the error to the subject.
// Here I take the approach of calling a method to signal the error.
// We agree instead that the routine should either return a null pointer
// if there is no error, or return the error message as a static string.
if ( error ) {
// If the user provided a message to signal a visibilty error, use it.
// If not, generate a generic message.
if ( !msg ) msg = "Movement cycles outside range.";
// The message could be different depending on whether the maniplulandum
// was not moved enough, or if it just could not be seen.
if ( N <= 0.0 ) fmt = "%s\n Manipulandum not visible.";
else fmt = "%s\n Measured cycles: %d\n Desired range: %d - %d\n Direction: < %.2f %.2f %.2f>";
int response = fSignalError( MB_ABORTRETRYIGNORE, picture, fmt, msg, cycles, min_cycles, max_cycles, dirX, dirY, dirZ );
if ( response == IDABORT ) return( ABORT_EXIT );
if ( response == IDRETRY ) return( RETRY_EXIT );
if ( response == IDIGNORE ) return( IGNORE_EXIT );
}
// This is my means of signalling the event.
monitor->SendEvent( "Number of movements OK.\n Measured: %d\n Desired range: %d - %d\n Direction: < %.2f %.2f %.2f>",
cycles, min_cycles, max_cycles, dirX, dirY, dirZ );
return( NORMAL_EXIT );
}
