LTriangle2 LMesh::GetTriangle2(uint index)
{
LTriangle2 f;
LTriangle t = GetTriangle(index);
f.vertices[0] = GetVertex(t.a);
f.vertices[1] = GetVertex(t.b);
f.vertices[2] = GetVertex(t.c);
f.vertexNormals[0] = GetNormal(t.a);
f.vertexNormals[1] = GetNormal(t.b);
f.vertexNormals[2] = GetNormal(t.c);
f.textureCoords[0] = GetUV(t.a);
f.textureCoords[1] = GetUV(t.b);
f.textureCoords[2] = GetUV(t.c);
LVector3 a, b;
a = SubtractVectors(_4to3(f.vertices[1]), _4to3(f.vertices[0]));
b = SubtractVectors(_4to3(f.vertices[1]), _4to3(f.vertices[2]));
f.faceNormal = CrossProduct(b, a);
f.faceNormal = NormalizeVector(f.faceNormal);
f.materialId = m_tris[index].materialId;
return f;
}
TVector3 MakeNormal (const TPolygon& p, TVector3 *v) {
TVector3 v1 = SubtractVectors (v[p.vertices[1]], v[p.vertices[0]]);
TVector3 v2 = SubtractVectors (v[p.vertices[p.num_vertices-1]], v[p.vertices[0]]);
TVector3 normal = CrossProduct (v1, v2);
NormVector (normal);
return normal;
}
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;
}
TVector3 MakeNormal (TPolygon p, TVector3 *v) {
TVector3 normal, v1, v2;
v1 = SubtractVectors (v[p.vertices[1]], v[p.vertices[0]]);
v2 = SubtractVectors (v[p.vertices[p.num_vertices-1]], v[p.vertices[0]]);
normal = CrossProduct (v1, v2);
NormVector (&normal);
return normal;
}
TVector3 ProjectToPlane (TVector3 nml, TVector3 v){
TVector3 nmlComp;
double dotProd;
dotProd = DotProduct (nml, v);
nmlComp = ScaleVector (dotProd, nml);
return SubtractVectors (v, nmlComp);
}
int DexApparatus::CheckCorrectStartPosition( int target_id, float tolX, float tolY, float tolZ, int max_bad_positions,
const char *msg, const char *picture ) {
const char *fmt;
bool error = false;
int bad_positions = 0, movements = 0;
int first, last;
int i, index;
Vector3 delta;
// 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 );
// Step through each marked movement trigger and verify that the velocity is
// close to zero when the trigger was sent.
FindAnalysisEventRange( first, last );
for ( i = first; i < last; i++ ) {
if ( eventList[i].event == TRIGGER_MOVEMENT ) {
movements++;
index = TimeToFrame( eventList[i].time );
SubtractVectors( delta, acquiredManipulandumState[index].position, targetPosition[target_id] );
if ( fabs( delta[X] ) > tolX
|| fabs( delta[Y] ) > tolY
|| fabs( delta[Z] ) > tolZ ) bad_positions++;
}
}
// Check if the computed number of incorrect starting positions is in the desired range.
error = ( bad_positions > max_bad_positions );
// 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 = "Starting position not respected.";
// The message could be different depending on whether the maniplulandum
// was not moved enough, or if it just could not be seen.
fmt = "%s\n Total Movements: %d\n Erroneous Positions: %d\nMaximum Allowed: %d";
int response = fSignalError( MB_ABORTRETRYIGNORE, picture, fmt, msg, movements, bad_positions, max_bad_positions );
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( fmt, "Start positions OK.", movements, bad_positions, max_bad_positions );
return( NORMAL_EXIT );
}
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;
}
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 );
}
// --------------------------------------------------------------------
// add_track_mark
// --------------------------------------------------------------------
void add_track_mark (CControl *ctrl, int *id) {
TVector3 width_vector;
TVector3 left_vector;
TVector3 right_vector;
double magnitude;
track_quad_t *q, *qprev, *qprevprev;
TVector3 vel;
double speed;
TVector3 left_wing, right_wing;
double left_y, right_y;
double dist_from_surface;
TPlane surf_plane;
double comp_depth;
double tex_end;
double dist_from_last_mark;
TVector3 vector_from_last_mark;
TTerrType *TerrList = Course.TerrList;
if (param.perf_level < 3) return;
q = &track_marks.quads[track_marks.current_mark%MAX_TRACK_MARKS];
qprev = &track_marks.quads[(track_marks.current_mark-1)%MAX_TRACK_MARKS];
qprevprev = &track_marks.quads[(track_marks.current_mark-2)%MAX_TRACK_MARKS];
vector_from_last_mark = SubtractVectors (ctrl->cpos, track_marks.last_mark_pos);
dist_from_last_mark = NormVector (&vector_from_last_mark);
*id = Course.GetTerrainIdx (ctrl->cpos.x, ctrl->cpos.z, 0.5);
if (*id < 1) {
break_track_marks();
return;
}
if (TerrList[*id].trackmarks < 1) {
break_track_marks();
return;
}
vel = ctrl->cvel;
speed = NormVector (&vel);
if (speed < SPEED_TO_START_TRENCH) {
break_track_marks();
return;
}
width_vector = CrossProduct (ctrl->cdirection, MakeVector (0, 1, 0));
magnitude = NormVector (&width_vector);
if (magnitude == 0) {
break_track_marks();
return;
}
left_vector = ScaleVector (TRACK_WIDTH/2.0, width_vector);
right_vector = ScaleVector (-TRACK_WIDTH/2.0, width_vector);
left_wing = SubtractVectors (ctrl->cpos, left_vector);
right_wing = SubtractVectors (ctrl->cpos, right_vector);
left_y = Course.FindYCoord (left_wing.x, left_wing.z);
right_y = Course.FindYCoord (right_wing.x, right_wing.z);
if (fabs(left_y-right_y) > MAX_TRACK_DEPTH) {
break_track_marks();
return;
}
surf_plane = Course.GetLocalCoursePlane (ctrl->cpos);
dist_from_surface = DistanceToPlane (surf_plane, ctrl->cpos);
// comp_depth = get_compression_depth(Snow);
comp_depth = 0.1;
if (dist_from_surface >= (2 * comp_depth)) {
break_track_marks();
return;
}
if (!continuing_track) {
break_track_marks();
q->track_type = TRACK_HEAD;
q->v1 = MakeVector (left_wing.x, left_y + TRACK_HEIGHT, left_wing.z);
q->v2 = MakeVector (right_wing.x, right_y + TRACK_HEIGHT, right_wing.z);
q->n1 = Course.FindCourseNormal (q->v1.x, q->v1.z);
q->n2 = Course.FindCourseNormal (q->v2.x, q->v2.z);
q->t1 = MakeVector2(0.0, 0.0);
q->t2 = MakeVector2(1.0, 0.0);
track_marks.next_mark = track_marks.current_mark + 1;
} else {
if (track_marks.next_mark == track_marks.current_mark) {
q->v1 = qprev->v3;
q->v2 = qprev->v4;
q->n1 = qprev->n3;
q->n2 = qprev->n4;
q->t1 = qprev->t3;
q->t2 = qprev->t4;
if (qprev->track_type != TRACK_HEAD) qprev->track_type = TRACK_MARK;
q->track_type = TRACK_MARK;
}
q->v3 = MakeVector (left_wing.x, left_y + TRACK_HEIGHT, left_wing.z);
q->v4 = MakeVector (right_wing.x, right_y + TRACK_HEIGHT, right_wing.z);
q->n3 = Course.FindCourseNormal (q->v3.x, q->v3.z);
q->n4 = Course.FindCourseNormal (q->v4.x, q->v4.z);
tex_end = speed*g_game.time_step/TRACK_WIDTH;
if (q->track_type == TRACK_HEAD) {
q->t3= MakeVector2 (0.0, 1.0);
q->t4= MakeVector2 (1.0, 1.0);
} else {
q->t3 = MakeVector2 (0.0, q->t1.y + tex_end);
q->t4 = MakeVector2 (1.0, q->t2.y + tex_end);
}
track_marks.current_mark++;
track_marks.next_mark = track_marks.current_mark;
}
q->alpha = min ((2*comp_depth-dist_from_surface)/(4*comp_depth), 1.0);
track_marks.last_mark_time = g_game.time;
continuing_track = true;
}
TVector3 ProjectToPlane (const TVector3& nml, const TVector3& v) {
double dotProd = DotProduct (nml, v);
TVector3 nmlComp = ScaleVector (dotProd, nml);
return SubtractVectors (v, nmlComp);
}
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);
}
int DexApparatus::CheckMovementDirection( int max_false_directions, float dirX, float dirY, float dirZ, float threshold,
const char *msg, const char *picture ) {
const char *fmt;
bool error = false;
int bad_movements = 0, movements = 0, starts = 0;
int first, last;
int i, index;
Vector3 dir;
dir[X] = dirX;
dir[Y] = dirY;
dir[Z] = dirZ;
float displacement;
Vector3 start_position, delta;
// 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 );
// Step through each marked upward movement trigger and count if
// the initial movement was in the right direction.
FindAnalysisEventRange( first, last );
for ( i = first; i < last; i++ ) {
if ( eventList[i].event == TRIGGER_MOVE_UP ) {
movements++;
index = TimeToFrame( eventList[i].time );
CopyVector( start_position, acquiredManipulandumState[index].position );
while ( index < nAcqFrames ) {
SubtractVectors( delta, acquiredManipulandumState[index].position, start_position );
displacement = DotProduct( dir, delta );
// 'UP' here means in the same direction and the specified vector.
// If we move past the threshold in that direction before moving past the
// same threshold distance in the other direction, then this movement is good.
if ( displacement > threshold ) {
starts++;
break;
}
// If we go the threshold distance in the opposite direction first,
// then consider this to have been an erroneous start.
if ( displacement < - threshold ) {
bad_movements++;
starts++;
break;
}
index++;
}
}
else if ( eventList[i].event == TRIGGER_MOVE_DOWN ) {
// Now do the same thing for downward movements.
movements++;
index = TimeToFrame( eventList[i].time );
CopyVector( start_position, acquiredManipulandumState[index].position );
while ( index < nAcqFrames ) {
SubtractVectors( delta, acquiredManipulandumState[index].position, start_position );
displacement = DotProduct( dir, delta );
// Here, a negative movement is good ...
if ( displacement < - threshold ) {
starts++;
break;
}
// ... and a positive movement is bad.
if ( displacement > threshold ) {
bad_movements++;
starts++;
break;
}
index++;
}
}
}
// Check if the computed number of incorrect starting positions is in the desired range.
error = ( bad_movements > max_false_directions );
// This format string is used to add debugging information to the event notification.
// It is used whether there is an error or not.
fmt = "%s\n Total Movements: %d\n Actual Starts: %d\n Errors Detected: %d\n Maximum Allowed: %d";
// 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 = "To many starts in wrong direction.";
int response = fSignalError( MB_ABORTRETRYIGNORE, picture, fmt, msg, movements, starts, bad_movements, max_false_directions );
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 to ground.
monitor->SendEvent( fmt, "Start directions OK.", movements, starts, bad_movements, max_false_directions );
return( NORMAL_EXIT );
}
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;
}
}
}
// --------------------------------------------------------------------
// add_track_mark
// --------------------------------------------------------------------
void add_track_mark(const CControl *ctrl, int *id) {
if (param.perf_level < 3)
return;
TTerrType *TerrList = &Course.TerrList[0];
*id = Course.GetTerrainIdx (ctrl->cpos.x, ctrl->cpos.z, 0.5);
if (*id < 1) {
break_track_marks();
return;
}
if (!TerrList[*id].trackmarks) {
break_track_marks();
return;
}
TVector3 vel = ctrl->cvel;
double speed = NormVector (vel);
if (speed < SPEED_TO_START_TRENCH) {
break_track_marks();
return;
}
TVector3 width_vector = CrossProduct (ctrl->cdirection, TVector3 (0, 1, 0));
double magnitude = NormVector (width_vector);
if (magnitude == 0) {
break_track_marks();
return;
}
TVector3 left_vector = ScaleVector (TRACK_WIDTH/2.0, width_vector);
TVector3 right_vector = ScaleVector (-TRACK_WIDTH/2.0, width_vector);
TVector3 left_wing = SubtractVectors (ctrl->cpos, left_vector);
TVector3 right_wing = SubtractVectors (ctrl->cpos, right_vector);
double left_y = Course.FindYCoord (left_wing.x, left_wing.z);
double right_y = Course.FindYCoord (right_wing.x, right_wing.z);
if (fabs(left_y-right_y) > MAX_TRACK_DEPTH) {
break_track_marks();
return;
}
TPlane surf_plane = Course.GetLocalCoursePlane (ctrl->cpos);
double dist_from_surface = DistanceToPlane (surf_plane, ctrl->cpos);
// comp_depth = get_compression_depth(Snow);
double comp_depth = 0.1;
if (dist_from_surface >= (2 * comp_depth)) {
break_track_marks();
return;
}
if(track_marks.quads.size() < MAX_TRACK_MARKS)
track_marks.quads.push_back(track_quad_t());
list<track_quad_t>::iterator qprev = track_marks.current_mark;
if(track_marks.current_mark == track_marks.quads.end())
track_marks.current_mark = track_marks.quads.begin();
else
track_marks.current_mark = incrementRingIterator(track_marks.current_mark);
list<track_quad_t>::iterator q = track_marks.current_mark;
if (!continuing_track) {
q->track_type = TRACK_HEAD;
q->v1 = TVector3 (left_wing.x, left_y + TRACK_HEIGHT, left_wing.z);
q->v2 = TVector3 (right_wing.x, right_y + TRACK_HEIGHT, right_wing.z);
q->v3 = TVector3 (left_wing.x, left_y + TRACK_HEIGHT, left_wing.z);
q->v4 = TVector3 (right_wing.x, right_y + TRACK_HEIGHT, right_wing.z);
q->n1 = Course.FindCourseNormal (q->v1.x, q->v1.z);
q->n2 = Course.FindCourseNormal (q->v2.x, q->v2.z);
q->t1 = TVector2(0.0, 0.0);
q->t2 = TVector2(1.0, 0.0);
} else {
q->track_type = TRACK_TAIL;
if (qprev != track_marks.quads.end()) {
q->v1 = qprev->v3;
q->v2 = qprev->v4;
q->n1 = qprev->n3;
q->n2 = qprev->n4;
q->t1 = qprev->t3;
q->t2 = qprev->t4;
if (qprev->track_type == TRACK_TAIL) qprev->track_type = TRACK_MARK;
}
q->v3 = TVector3 (left_wing.x, left_y + TRACK_HEIGHT, left_wing.z);
q->v4 = TVector3 (right_wing.x, right_y + TRACK_HEIGHT, right_wing.z);
q->n3 = Course.FindCourseNormal (q->v3.x, q->v3.z);
q->n4 = Course.FindCourseNormal (q->v4.x, q->v4.z);
double tex_end = speed*g_game.time_step/TRACK_WIDTH;
if (q->track_type == TRACK_HEAD) {
q->t3= TVector2 (0.0, 1.0);
q->t4= TVector2 (1.0, 1.0);
} else {
q->t3 = TVector2 (0.0, q->t1.y + tex_end);
q->t4 = TVector2 (1.0, q->t2.y + tex_end);
}
}
q->alpha = min ((2*comp_depth-dist_from_surface)/(4*comp_depth), 1.0);
continuing_track = true;
}
int DexApparatus::CheckEarlyStarts( int max_early_starts, float hold_time, float threshold, float filter_constant,
const char *msg, const char *picture ) {
const char *fmt;
bool error = false;
int early_starts = 0;
int first, last;
int i, j, index, frm;
int hold_frames = (int) floor( hold_time / tracker->samplePeriod );
int N = 0;
double tangential_velocity[DEX_MAX_MARKER_FRAMES];
Vector3 delta;
// 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 instantaneous tangential velocity.
for ( i = first + 1; i < last; i++ ) {
if ( acquiredManipulandumState[i].visibility && acquiredManipulandumState[i-1].visibility) {
SubtractVectors( delta, acquiredManipulandumState[i].position, acquiredManipulandumState[i-1].position );
ScaleVector( delta, delta, 1.0 / tracker->GetSamplePeriod() );
tangential_velocity[i] = VectorNorm( delta );
N++;
}
else {
tangential_velocity[i] = tangential_velocity[i-1];
}
}
// If there is no valid position data, signal an error.
if ( N <= 0.0 ) {
monitor->SendEvent( "No valid data." );
error = true;
}
else {
// Smooth the tangential velocity using a recursive filter.
for ( frm = 1; frm < nAcqFrames; frm++ ) {
tangential_velocity[frm] = ( filter_constant * tangential_velocity[frm-1] + tangential_velocity[frm] ) / ( 1.0 + filter_constant );
}
// Run the filter backwards to eliminate the phase lag.
for ( frm = nAcqFrames - 2; frm >= 0; frm-- ) {
tangential_velocity[frm] = ( filter_constant * tangential_velocity[frm+1] + tangential_velocity[frm] ) / ( 1.0 + filter_constant );
}
// Step through each marked movement trigger and verify that the velocity is
// close to zero when the trigger was sent.
FindAnalysisEventRange( first, last );
for ( i = first; i < last; i++ ) {
if ( eventList[i].event == TRIGGER_MOVEMENT ) {
index = TimeToFrame( eventList[i].time );
for ( j = index; j > index - hold_frames && j > first; j-- ) {
if ( tangential_velocity[i] > threshold ) {
early_starts++;
break;
}
}
}
}
// Check if the computed number of cycles is in the desired range.
error = ( early_starts > max_early_starts );
}
// 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 = "To many false starts.";
// 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 False Starts Detected: %d\nMaximum Allowed: %d";
int response = fSignalError( MB_ABORTRETRYIGNORE, picture, fmt, msg, early_starts, max_early_starts );
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( "Early Starts OK.\n Measured: %d\n Maximum Allowed: %d", early_starts, max_early_starts );
return( NORMAL_EXIT );
}
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 );
}
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 LMesh::CalcNormals(bool useSmoothingGroups)
{
uint i;
// first calculate the face normals
for (i=0; i<m_triangles.size(); i++)
{
LVector3 a, b;
a = SubtractVectors(_4to3(m_vertices[m_tris[i].b]), _4to3(m_vertices[m_tris[i].a]));
b = SubtractVectors(_4to3(m_vertices[m_tris[i].b]), _4to3(m_vertices[m_tris[i].c]));
m_tris[i].normal = NormalizeVector(CrossProduct(b, a));
}
std::vector< std::vector<int> > array;
array.resize(m_vertices.size());
for (i=0; i<m_triangles.size(); i++)
{
uint k = m_tris[i].a;
array[k].push_back(i);
k = m_tris[i].b;
array[k].push_back(i);
k = m_tris[i].c;
array[k].push_back(i);
}
LVector3 temp;
if (!useSmoothingGroups)
{
// now calculate the normals without using smoothing groups
for (i=0; i<m_vertices.size(); i++)
{
temp = zero3;
int t = int(array[i].size());
for (int k=0; k<t; k++)
{
temp.x += m_tris[array[i][k]].normal.x;
temp.y += m_tris[array[i][k]].normal.y;
temp.z += m_tris[array[i][k]].normal.z;
}
m_normals[i] = NormalizeVector(temp);
}
}
else
{
// now calculate the normals _USING_ smoothing groups
// I'm assuming a triangle can only belong to one smoothing group at a time!
std::vector<ulong> smGroups;
std::vector< std::vector <uint> > smList;
uint loop_size = uint(m_vertices.size());
for (i=0; i<loop_size; i++)
{
int t = uint(array[i].size());
if (t == 0)
continue;
smGroups.clear();
smList.clear();
smGroups.push_back(m_tris[array[i][0]].smoothingGroups);
smList.resize(smGroups.size());
smList[smGroups.size()-1].push_back(array[i][0]);
// first build a list of smoothing groups for the vertex
for (int k=0; k<t; k++)
{
bool found = false;
for (uint j=0; j<smGroups.size(); j++)
{
if (m_tris[array[i][k]].smoothingGroups == smGroups[j])
{
smList[j].push_back(array[i][k]);
found = true;
}
}
if (!found)
{
smGroups.push_back(m_tris[array[i][k]].smoothingGroups);
smList.resize(smGroups.size());
smList[smGroups.size()-1].push_back(array[i][k]);
}
}
// now we have the list of faces for the vertex sorted by smoothing groups
// now duplicate the vertices so that there's only one smoothing group "per vertex"
if (smGroups.size() > 1)
for (uint j=1; j< smGroups.size(); j++)
{
m_vertices.push_back(m_vertices[i]);
m_normals.push_back(m_normals[i]);
m_uv.push_back(m_uv[i]);
m_tangents.push_back(m_tangents[i]);
m_binormals.push_back(m_binormals[i]);
uint ts = uint(m_vertices.size())-1;
for (uint h=0; h<smList[j].size(); h++)
{
if (m_tris[smList[j][h]].a == i)
m_tris[smList[j][h]].a = ts;
if (m_tris[smList[j][h]].b == i)
m_tris[smList[j][h]].b = ts;
if (m_tris[smList[j][h]].c == i)
m_tris[smList[j][h]].c = ts;
}
}
}
// now rebuild a face list for each vertex, since the old one is invalidated
for (i=0; i<array.size(); i++)
array[i].clear();
array.clear();
array.resize(m_vertices.size());
for (i=0; i<m_triangles.size(); i++)
{
uint k = m_tris[i].a;
array[k].push_back(i);
k = m_tris[i].b;
array[k].push_back(i);
k = m_tris[i].c;
array[k].push_back(i);
}
// now compute the normals
for (i=0; i<m_vertices.size(); i++)
{
temp = zero3;
int t = uint(array[i].size());
for (int k=0; k<t; k++)
{
temp.x += m_tris[array[i][k]].normal.x;
temp.y += m_tris[array[i][k]].normal.y;
temp.z += m_tris[array[i][k]].normal.z;
}
m_normals[i] = NormalizeVector(temp);
}
}
// copy m_tris to m_triangles
for (i=0; i<m_triangles.size(); i++)
{
m_triangles[i].a = m_tris[i].a;
m_triangles[i].b = m_tris[i].b;
m_triangles[i].c = m_tris[i].c;
}
}
