void Camera_Draw_back(Camera *self)
{
VECTOR cam = {0, 0, 0};
// 方角をカメラのyawに換算
float yaw = calc_yaw(self);
SetCameraPositionAndAngle(cam, self->pt.x, yaw, 0.0f);
}
float roll_calc(inertial *current,inertial *prev, int datarate)
{
float cur_bearing,prev_bearing,cur_speed,prev_speed,prev_roll;
int cur_time,prev_time;
cur_bearing = current->bearing;
prev_bearing = prev->bearing;
cur_speed = current->speed;
prev_speed = prev->speed;
prev_time = 10*(prev->time);
cur_time = 10*(current->time);
int td = cur_time-prev_time;
//check time diff between datapoints is as expected
if (td!=(10/datarate))
{
//return previous value of roll if not
return prev->roll;
}
//calc cross product to determin if turn is left or right
float turn = cos(prev_bearing*M_PI/180)*sin(cur_bearing*M_PI/180)
- sin(prev_bearing*M_PI/180)*cos(cur_bearing*M_PI/180);
//right
if (turn >= 0)
{
turn = 1;
}
//left
else
{
turn = -1;
}
float yaw = calc_yaw(cur_bearing,prev_bearing,turn);
float period = 360/(yaw*datarate);
float ave_speed = (prev_speed+cur_speed)/2;
if (ave_speed <1)
{
return 0.0;
}
float radius = ave_speed * period/(2*M_PI);
float roll = atan(ave_speed*ave_speed/(radius*9.8))*180/M_PI;
return roll;
}
/**
* @brief Interpret IR data into more user friendly variables.
*
* @param wm Pointer to a wiimote_t structure.
*/
static void interpret_ir_data(struct wiimote_t* wm) {
struct ir_dot_t* dot = wm->ir.dot;
int i;
float roll = 0.0f;
int last_num_dots = wm->ir.num_dots;
if (WIIMOTE_IS_SET(wm, WIIMOTE_STATE_ACC))
roll = wm->orient.roll;
/* count visible dots */
wm->ir.num_dots = 0;
for (i = 0; i < 4; ++i) {
if (dot[i].visible)
wm->ir.num_dots++;
}
switch (wm->ir.num_dots) {
case 0:
{
wm->ir.state = 0;
/* reset the dot ordering */
for (i = 0; i < 4; ++i)
dot[i].order = 0;
wm->ir.x = 0;
wm->ir.y = 0;
wm->ir.z = 0.0f;
return;
}
case 1:
{
fix_rotated_ir_dots(wm->ir.dot, roll);
if (wm->ir.state < 2) {
/*
* Only 1 known dot, so use just that.
*/
for (i = 0; i < 4; ++i) {
if (dot[i].visible) {
wm->ir.x = dot[i].x;
wm->ir.y = dot[i].y;
wm->ir.ax = wm->ir.x;
wm->ir.ay = wm->ir.y;
/* can't calculate yaw because we don't have the distance */
//wm->orient.yaw = calc_yaw(&wm->ir);
ir_convert_to_vres(&wm->ir.x, &wm->ir.y, wm->ir.aspect, wm->ir.vres[0], wm->ir.vres[1]);
break;
}
}
} else {
/*
* Only see 1 dot but know theres 2.
* Try to estimate where the other one
* should be and use that.
*/
for (i = 0; i < 4; ++i) {
if (dot[i].visible) {
int ox = 0;
int x, y;
if (dot[i].order == 1)
/* visible is the left dot - estimate where the right is */
ox = dot[i].x + wm->ir.distance;
else if (dot[i].order == 2)
/* visible is the right dot - estimate where the left is */
ox = dot[i].x - wm->ir.distance;
x = ((signed int)dot[i].x + ox) / 2;
y = dot[i].y;
wm->ir.ax = x;
wm->ir.ay = y;
wm->orient.yaw = calc_yaw(&wm->ir);
if (ir_correct_for_bounds(&x, &y, wm->ir.aspect, wm->ir.offset[0], wm->ir.offset[1])) {
ir_convert_to_vres(&x, &y, wm->ir.aspect, wm->ir.vres[0], wm->ir.vres[1]);
wm->ir.x = x;
wm->ir.y = y;
}
break;
}
}
}
break;
}
case 2:
case 3:
case 4:
{
/*
* Two (or more) dots known and seen.
* Average them together to estimate the true location.
*/
int x, y;
wm->ir.state = 2;
fix_rotated_ir_dots(wm->ir.dot, roll);
/* if there is at least 1 new dot, reorder them all */
if (wm->ir.num_dots > last_num_dots) {
reorder_ir_dots(dot);
wm->ir.x = 0;
wm->ir.y = 0;
}
wm->ir.distance = ir_distance(dot);
wm->ir.z = 1023 - wm->ir.distance;
get_ir_dot_avg(wm->ir.dot, &x, &y);
wm->ir.ax = x;
wm->ir.ay = y;
wm->orient.yaw = calc_yaw(&wm->ir);
if (ir_correct_for_bounds(&x, &y, wm->ir.aspect, wm->ir.offset[0], wm->ir.offset[1])) {
ir_convert_to_vres(&x, &y, wm->ir.aspect, wm->ir.vres[0], wm->ir.vres[1]);
wm->ir.x = x;
wm->ir.y = y;
}
break;
}
default:
{
break;
}
}
#ifdef WITH_WIIUSE_DEBUG
{
int ir_level;
WIIUSE_GET_IR_SENSITIVITY(wm, &ir_level);
WIIUSE_DEBUG("IR sensitivity: %i", ir_level);
WIIUSE_DEBUG("IR visible dots: %i", wm->ir.num_dots);
for (i = 0; i < 4; ++i)
if (dot[i].visible)
WIIUSE_DEBUG("IR[%i][order %i] (%.3i, %.3i) -> (%.3i, %.3i)", i, dot[i].order, dot[i].rx, dot[i].ry, dot[i].x, dot[i].y);
WIIUSE_DEBUG("IR[absolute]: (%i, %i)", wm->ir.x, wm->ir.y);
}
#endif
}
//--------------------------------------------------------------------------------
// follow the target as near as possible
//--------------------------------------------------------------------------------
bool zz_camera_follow::update_lookmode (float follow_yaw_last)
{
bool move_camera;
// distance between last_target and current target position
float target_diff = last_.target_pos.distance(final_.target_pos);
// distance between camera and current target
float target_dist = (target_) ? distance(target_) : get_position().distance(vec3_null);
// update by camera-target difference
// 1. we start moving camera position if the target_diff reaches a certain amount.
// 2. we stop moving camera position if the target_diff is too small.
if (now_following_) { // if we started moving camera position.
if (target_diff > MIN_DISTANCE_THRESHOLD) { // now moving
move_camera = true; // should move
}
else { // we are already very close to each other
now_following_ = false; // quit following mode
move_camera = true; // but keep in camera moving state for now
}
}
else if (target_diff > MAX_DISTANCE_THRESHOLD) { // we should start moving the camera position now.
now_following_ = true;
move_camera = true;
}
else { // we do not need to move the camera position
move_camera = false;
}
// force moving the camera for test
if (!znzin->get_use_time_weight()) move_camera = true;
if (move_camera) { // if we should move camera and target
vec3 displacement;
float damp = (target_diff / MAX_DISTANCE_THRESHOLD);
// get target displacement by last and current
displacement = .5f*time_weight_*(final_.target_pos - last_.target_pos); // .5f to make slower than 1.0f
// update last_target_pos
last_.target_pos += displacement;
move(displacement);
}
//camera_dir = target_->get_position() - this->get_eye(); // update camera_dir for later use in update()
final_.camera_dir = last_.target_pos - get_eye();
// recalc yaw, because eye position was changed in update_lookmode()
// and, we need the difference between yaw and yaw_last
float yaw_diff = final_.yaw - last_.yaw;
current_.yaw = calc_yaw();
final_.yaw = current_.yaw + yaw_diff;
// and apply new_value
current_.yaw += time_weight_*(final_.yaw - current_.yaw);
apply_yaw(final_.camera_dir, current_.yaw, final_.target_dir);
// save last_camera_dir(non-pitched) for back-mode
last_.camera_dir = final_.camera_dir;
// apply pitch
apply_pitch(final_.camera_dir, current_.pitch);
final_.camera_pos = last_.target_pos - current_.distance * final_.camera_dir;
look_at(final_.camera_pos, last_.target_pos, vec3(0, 0, 1));
last_.camera_pos = final_.camera_pos; // for back-mode
return move_camera;
}