// Called once after IsFinished returns true
void TargetCommand::End()
{
printf("TargetCommand::End\n");
ReadCamera(); // one last time, just to report
int aimFrames = m_cameraFrame - m_cameraStartFrame;
printf("+++ offset %d frames %d\n", m_tgtOffset, aimFrames);
SmartDashboard::PutBoolean("aiming", false);
SmartDashboard::PutNumber("aimFrames", aimFrames);
}
// Called just before this Command runs the first time
void TargetCommand::Initialize()
{
printf("TargetCommand::Initialize\n");
m_cameraFrame = -1;
// m_tgtHeight = 0;
// m_tgtWidth = 0;
m_tgtOffset = 0;
ReadCamera();
m_cameraStartFrame = m_cameraFrame;
// let the world know that somebody pushed the button
SmartDashboard::PutBoolean("aiming", true);
// figure out time/power
double power, time;
findPower(m_tgtOffset, &time, &power);
printf("frame %d offset %d time %g power %g\n", m_cameraFrame, m_tgtOffset, time, power);
Set(0.0, 0.0, power, time);
}
void TwoFrameModel::Read(FILE *f)
{
fscanf(f, "%d\n", &m_num_points);
fscanf(f, "%lf\n", &m_angle);
fscanf(f, "%lf\n", &m_error);
v3_t *points_tmp = new v3_t[m_num_points];
double *tracks_tmp = new double[m_num_points];
int *k1_tmp = new int[m_num_points];
int *k2_tmp = new int[m_num_points];
m_points = new v3_t[m_num_points];
m_tracks = new int[m_num_points];
for (int i = 0; i < m_num_points; i++) {
int tr, k1, k2;
fscanf(f, "%d %d %d %lf %lf %lf\n", &tr, &k1, &k2,
&(Vx(points_tmp[i])),
&(Vy(points_tmp[i])),
&(Vz(points_tmp[i])));
tracks_tmp[i] = (double) tr;
k1_tmp[i] = k1;
k2_tmp[i] = k2;
}
bool use_tracks = true;
if (tracks_tmp[0] < 0)
use_tracks = false;
if (use_tracks) {
int *perm = new int[m_num_points];
qsort_ascending();
qsort_perm(m_num_points, tracks_tmp, perm);
for (int i = 0; i < m_num_points; i++) {
m_tracks[i] = iround(tracks_tmp[i]);
m_points[i] = points_tmp[perm[i]];
}
m_keys1 = m_keys2 = NULL;
delete [] perm;
} else {
m_keys1 = new int[m_num_points];
m_keys2 = new int[m_num_points];
for (int i = 0; i < m_num_points; i++) {
m_keys1[i] = k1_tmp[i];
m_keys2[i] = k2_tmp[i];
m_points[i] = points_tmp[i];
}
m_tracks = NULL;
}
delete [] points_tmp;
delete [] tracks_tmp;
delete [] k1_tmp;
delete [] k2_tmp;
ReadCamera(f, m_camera0);
ReadCamera(f, m_camera1);
ReadVector(f, 9, m_C0);
ReadVector(f, 9, m_C1);
}
bool L3DS::Read3DS()
{
LChunk mainchunk;
LChunk edit;
edit.id = EDIT3DS;
mainchunk = ReadChunk();
if (mainchunk.id != MAIN3DS)
{
fprintf(stderr, "L3DS::Read3DS - wrong file format");
return false;
}
if (!FindChunk(edit, mainchunk))
return false;
LChunk obj;
LChunk ml;
GotoChunk(edit);
obj.id = MAT_ENTRY;
while (FindChunk(obj, edit))
{
ReadMaterial(obj);
SkipChunk(obj);
}
GotoChunk(edit);
obj.id = EDIT_OBJECT;
{
while (FindChunk(obj, edit))
{
ReadASCIIZ(m_objName, 99);
ml = ReadChunk();
if (ml.id == OBJ_TRIMESH)
ReadMesh(ml);
else
if (ml.id == OBJ_LIGHT)
ReadLight(ml);
else
if (ml.id == OBJ_CAMERA)
ReadCamera(ml);
SkipChunk(obj);
}
}
// read the keyframer data here to find out correct object orientation
LChunk keyframer;
keyframer.id = KFDATA;
LChunk objtrack;
objtrack.id = OBJECT_NODE_TAG;
GotoChunk(mainchunk);
if (FindChunk(keyframer, mainchunk))
{ // keyframer chunk is present
GotoChunk(keyframer);
while (FindChunk(objtrack, keyframer))
{
ReadKeyframeData(objtrack);
SkipChunk(objtrack);
}
}
for (uint i=0; i<m_meshes.size(); i++)
m_meshes[i].Optimize(m_optLevel);
m_pos = 0;
strcpy(m_objName, "");
return true;
}