Vec2f Maze::next_step(int start_x, int start_y, int trail_size)
{
start_x = min(start_x, (int)this->path[0].size() - 1);
start_y = min(start_y, (int)this->path.size() - 1);
// cout << "len : " << this->path.size() << " " << this->path[start_y].size() << endl;
// cout << start_x << " and " << start_y << endl;
pair<int, int> current = this->path[start_y][start_x],
next;
// cout << current.first << " or " << current.second << endl;
if (current.first == -1) return Vec2f();
for (int i = 0; i < trail_size; i++)
{
next = this->path[current.second][current.first];
if (next.first == -1) break;
current = next;
}
Vec2f target_vector(current.first - start_x, current.second - start_y);
return target_vector / norm(target_vector);
}
void SharedDomainMap<Mesh,CoordinateField>::apply(
const Teuchos::RCP< FieldEvaluator<GlobalOrdinal,SourceField> >& source_evaluator,
Teuchos::RCP< FieldManager<TargetField> >& target_space_manager )
{
typedef FieldTraits<SourceField> SFT;
typedef FieldTraits<TargetField> TFT;
// Set existence values for the source and target.
bool source_exists = true;
if ( source_evaluator.is_null() ) source_exists = false;
bool target_exists = true;
if ( target_space_manager.is_null() ) target_exists = false;
// Evaluate the source function at the target points and construct a view
// of the function evaluations.
int source_dim = 0;
Teuchos::ArrayRCP<typename SFT::value_type> source_field_copy(0,0);
if ( source_exists )
{
SourceField function_evaluations =
source_evaluator->evaluate(
Teuchos::arcpFromArray( d_source_geometry ),
Teuchos::arcpFromArray( d_target_coords ) );
source_dim = SFT::dim( function_evaluations );
source_field_copy =
FieldTools<SourceField>::copy( function_evaluations );
}
Teuchos::broadcast<int,int>( *d_comm, d_source_indexer.l2g(0),
Teuchos::Ptr<int>(&source_dim) );
// Build a multivector for the function evaluations.
Tpetra::MultiVector<typename SFT::value_type, int, GlobalOrdinal>
source_vector( d_source_map, source_dim );
source_vector.get1dViewNonConst().deepCopy( source_field_copy() );
// Construct a view of the target space.
int target_dim = 0;
Teuchos::ArrayRCP<typename TFT::value_type> target_field_view(0,0);
if ( target_exists )
{
target_field_view = FieldTools<TargetField>::nonConstView(
*target_space_manager->field() );
target_dim = TFT::dim( *target_space_manager->field() );
}
Teuchos::broadcast<int,int>( *d_comm, d_target_indexer.l2g(0),
Teuchos::Ptr<int>(&target_dim) );
// Check that the source and target have the same field dimension.
DTK_REQUIRE( source_dim == target_dim );
// Verify that the target space has the proper amount of memory allocated.
if ( target_exists )
{
DTK_REQUIRE(
target_field_view.size() == Teuchos::as<GlobalOrdinal>(
d_target_map->getNodeNumElements()) * target_dim );
}
// Build a multivector for the target space.
Tpetra::MultiVector<typename TFT::value_type, int, GlobalOrdinal>
target_vector( d_target_map, target_dim );
// Fill the target space with zeros so that points we didn't map get some
// data.
if ( target_exists )
{
FieldTools<TargetField>::putScalar(
*target_space_manager->field(), 0.0 );
}
// Move the data from the source decomposition to the target
// decomposition.
target_vector.doImport( source_vector, *d_source_to_target_importer,
Tpetra::INSERT );
target_field_view.deepCopy( target_vector.get1dView()() );
}
void
Camera::move()
{
Vector3<float> target_vector(0,0,1); // x, y, z to target before rotating to planes axis, values are in meters
//decide what happens to camera depending on camera mode
switch(mode)
{
case 0:
//do nothing, i.e lock camera in place
return;
break;
case 1:
//stabilize
target_vector.x=0; //east to west gives +tive value (i.e. longitude)
target_vector.y=0; //south to north gives +tive value (i.e. latitude)
target_vector.z=100; //downwards is +tive
break;
case 2:
//track target
if(g_gps->fix)
{
target_vector=get_location_vector(¤t_loc,&camera_target);
}
break;
case 3: // radio manual control
case 4: // test (level the camera and point north)
break; // see code 25 lines bellow
}
Matrix3f m = dcm.get_dcm_transposed();
Vector3<float> targ = m*target_vector; //to do: find out notion of x y convention
switch(gimbal_type)
{
case 0: // pitch & roll (tilt & roll)
cam_pitch = degrees(atan2(-targ.x, targ.z)); //pitch
cam_roll = degrees(atan2(targ.y, targ.z)); //roll
break;
case 1: // yaw & pitch (pan & tilt)
cam_pitch = atan2((sqrt(sq(targ.y) + sq(targ.x)) * .01113195), targ.z) * -1;
cam_yaw = 9000 + atan2(-targ.y, targ.x) * 5729.57795;
break;
/* case 2: // pitch, roll & yaw - not started
cam_ritch = 0;
cam_yoll = 0;
cam_paw = 0;
break; */
}
//some camera modes overwrite the gimbal_type calculations
switch(mode)
{
case 3: // radio manual control
if (rc_function[CAM_PITCH])
cam_pitch = map(rc_function[CAM_PITCH]->radio_in,
rc_function[CAM_PITCH]->radio_min,
rc_function[CAM_PITCH]->radio_max,
rc_function[CAM_PITCH]->angle_min,
rc_function[CAM_PITCH]->radio_max);
if (rc_function[CAM_ROLL])
cam_roll = map(rc_function[CAM_ROLL]->radio_in,
rc_function[CAM_ROLL]->radio_min,
rc_function[CAM_ROLL]->radio_max,
rc_function[CAM_ROLL]->angle_min,
rc_function[CAM_ROLL]->radio_max);
if (rc_function[CAM_YAW])
cam_yaw = map(rc_function[CAM_YAW]->radio_in,
rc_function[CAM_YAW]->radio_min,
rc_function[CAM_YAW]->radio_max,
rc_function[CAM_YAW]->angle_min,
rc_function[CAM_YAW]->radio_max);
break;
case 4: // test (level the camera and point north)
cam_pitch = -dcm.pitch_sensor;
cam_yaw = dcm.yaw_sensor; // do not invert because the servo is mounted upside-down on my system
// TODO: the "trunk" code can invert using parameters, but this branch still can't
cam_roll = -dcm.roll_sensor;
break;
}
#if CAM_DEBUG == ENABLED
//for debugging purposes
Serial.println();
Serial.print("current_loc: lat: ");
Serial.print(current_loc.lat);
Serial.print(", lng: ");
Serial.print(current_loc.lng);
Serial.print(", alt: ");
Serial.print(current_loc.alt);
Serial.println();
Serial.print("target_loc: lat: ");
Serial.print(camera_target.lat);
Serial.print(", lng: ");
Serial.print(camera_target.lng);
Serial.print(", alt: ");
Serial.print(camera_target.alt);
Serial.print(", distance: ");
Serial.print(get_distance(¤t_loc,&camera_target));
Serial.print(", bearing: ");
Serial.print(get_bearing(¤t_loc,&camera_target));
Serial.println();
Serial.print("dcm_angles: roll: ");
Serial.print(degrees(dcm.roll));
Serial.print(", pitch: ");
Serial.print(degrees(dcm.pitch));
Serial.print(", yaw: ");
Serial.print(degrees(dcm.yaw));
Serial.println();
Serial.print("target_vector: x: ");
Serial.print(target_vector.x,2);
Serial.print(", y: ");
Serial.print(target_vector.y,2);
Serial.print(", z: ");
Serial.print(target_vector.z,2);
Serial.println();
Serial.print("rotated_target_vector: x: ");
Serial.print(targ.x,2);
Serial.print(", y: ");
Serial.print(targ.y,2);
Serial.print(", z: ");
Serial.print(targ.z,2);
Serial.println();
Serial.print("gimbal type 0: roll: ");
Serial.print(roll);
Serial.print(", pitch: ");
Serial.print(pitch);
Serial.println();
/* Serial.print("gimbal type 1: pitch: ");
Serial.print(pan);
Serial.print(", roll: ");
Serial.print(tilt);
Serial.println(); */
/* Serial.print("gimbal type 2: pitch: ");
Serial.print(ritch);
Serial.print(", roll: ");
Serial.print(yoll);
Serial.print(", yaw: ");
Serial.print(paw);
Serial.println(); */
#endif
}
