// Filename: smoothMover.cxx
// Created by:  drose (19Oct01)
//
////////////////////////////////////////////////////////////////////
//
// PANDA 3D SOFTWARE
// Copyright (c) Carnegie Mellon University.  All rights reserved.
//
// All use of this software is subject to the terms of the revised BSD
// license.  You should have received a copy of this license along
// with this source code in a file named "LICENSE."
//
////////////////////////////////////////////////////////////////////

#include "smoothMover.h"
#include "pnotify.h"
#include "config_deadrec.h"

////////////////////////////////////////////////////////////////////
//     Function: SmoothMover::Constructor
//       Access: Published
//  Description: 
////////////////////////////////////////////////////////////////////
SmoothMover::
SmoothMover() {
  _sample._pos.set(0.0, 0.0, 0.0);
  _sample._hpr.set(0.0, 0.0, 0.0);
  _sample._timestamp = 0.0;

  _smooth_pos.set(0.0, 0.0, 0.0);
  _smooth_hpr.set(0.0, 0.0, 0.0);
  _forward_axis.set(0.0, 1.0, 0.0);
  _smooth_timestamp = 0.0;
  _smooth_position_known = false;
  _smooth_position_changed = true;
  _computed_forward_axis = true;

  _smooth_forward_velocity = 0.0;
  _smooth_lateral_velocity = 0.0;
  _smooth_rotational_velocity = 0.0;

  _has_most_recent_timestamp = false;

  _last_point_before = -1;
  _last_point_after = -1;

  _net_timestamp_delay = 0;
  // Record one delay of 0 on the top of the delays array, just to
  // guarantee that the array is never completely empty.
  _timestamp_delays.push_back(0);

  _last_heard_from = 0.0;

  _smooth_mode = SM_off;
  _prediction_mode = PM_off;
  _delay = 0.2;
  _accept_clock_skew = accept_clock_skew;
  _directional_velocity = true;
  _default_to_standing_still = true;
  _max_position_age = 0.25;
  _expected_broadcast_period = 0.2;
  _reset_velocity_age = 0.3;
}

////////////////////////////////////////////////////////////////////
//     Function: SmoothMover::Destructor
//       Access: Published
//  Description: 
////////////////////////////////////////////////////////////////////
SmoothMover::
~SmoothMover() {
}

////////////////////////////////////////////////////////////////////
//     Function: SmoothMover::mark_position
//       Access: Published
//  Description: Stores the position, orientation, and timestamp (if
//               relevant) indicated by previous calls to set_pos(),
//               set_hpr(), and set_timestamp() in a new position
//               report.
//
//               When compute_smooth_position() is called, it uses
//               these stored position reports to base its computation
//               of the known position.
////////////////////////////////////////////////////////////////////
void SmoothMover::
mark_position() {
  /*
  if (deadrec_cat.is_debug()) {
    deadrec_cat.debug() << "mark_position\n";
  }
  */
  if (_smooth_mode == SM_off) {
    // With smoothing disabled, mark_position() simply stores its
    // current position in the smooth_position members.

    // In this mode, we also ignore the supplied timestamp, and just
    // use the current frame time--there's no need to risk trusting
    // the timestamp from another client.
    double timestamp = ClockObject::get_global_clock()->get_frame_time();

    // We also need to compute the velocity here.
    if (_smooth_position_known) {
      LVector3 pos_delta = _sample._pos - _smooth_pos;
      LVecBase3 hpr_delta = _sample._hpr - _smooth_hpr;
      double age = timestamp - _smooth_timestamp;
      age = min(age, _max_position_age);

      set_smooth_pos(_sample._pos, _sample._hpr, timestamp);
      if (age != 0.0) {
        compute_velocity(pos_delta, hpr_delta, age);
      }

    } else {
      // No velocity is possible, just position and orientation.
      set_smooth_pos(_sample._pos, _sample._hpr, timestamp);
    }

  } else {
    // Otherwise, smoothing is in effect and we store a true position
    // report.

    if (!_points.empty() && _points.back()._timestamp > _sample._timestamp) {
      if (deadrec_cat.is_debug()) {
        deadrec_cat.debug()
          << "*** timestamp out of order " << _points.back()._timestamp << " "
          << _sample._timestamp << "\n";
      }

      // If we get a timestamp out of order, one of us must have just
      // reset our clock.  Flush the sequence and start again.
      _points.clear();

      // That invalidates the index numbers.
      _last_point_before = -1;
      _last_point_after = -1;

      _points.push_back(_sample);

    } else if (!_points.empty() && _points.back()._timestamp == _sample._timestamp) {
      if (deadrec_cat.is_debug()) {
        deadrec_cat.debug()
          << "*** same timestamp\n";
      }
      // If the new timestamp is the same as the last timestamp, the
      // value simply replaces the previous value.
      _points.back() = _sample;

    } else if ((int)_points.size() >= max_position_reports) {
      if (deadrec_cat.is_debug()) {
        deadrec_cat.debug()
          << "*** dropped oldest position report\n";
      }
      // If we have too many position reports, throw away the oldest
      // one.
      _points.pop_front();

      --_last_point_before;
      --_last_point_after;

      _points.push_back(_sample);

    } else {
      // In the ordinary case, just add another sample.
      _points.push_back(_sample);
    }
  }
  //cout << "mark_position: " << _points.back()._pos << endl;
}

////////////////////////////////////////////////////////////////////
//     Function: SmoothMover::clear_positions
//       Access: Published
//  Description: Erases all the old position reports.  This should be
//               done, for instance, prior to teleporting the avatar
//               to a new position; otherwise, the smoother might try
//               to lerp the avatar there.  If reset_velocity is true,
//               the velocity is also reset to 0.
////////////////////////////////////////////////////////////////////
void SmoothMover::
clear_positions(bool reset_velocity) {
  if (deadrec_cat.is_debug()) {
    deadrec_cat.debug()
      << "clear_positions " << reset_velocity << "\n";
  }

  _points.clear();
  _last_point_before = -1;
  _last_point_after = -1;
  _smooth_position_known = false;
  _has_most_recent_timestamp = false;

  if (reset_velocity) {
    _smooth_forward_velocity = 0.0;
    _smooth_lateral_velocity = 0.0;
    _smooth_rotational_velocity = 0.0;
  }
}

////////////////////////////////////////////////////////////////////
//     Function: SmoothMover::compute_smooth_position
//       Access: Published
//  Description: Computes the smoothed position (and orientation) of
//               the mover at the indicated point in time, based on
//               the previous position reports.  After this call has
//               been made, get_smooth_pos() etc. may be called to
//               retrieve the smoothed position.
//
//               The return value is true if the value has changed (or
//               might have changed) since the last call to
//               compute_smooth_position(), or false if it remains the
//               same.
////////////////////////////////////////////////////////////////////
bool SmoothMover::
compute_smooth_position(double timestamp) {
  if (deadrec_cat.is_spam()) {
    deadrec_cat.spam()
      << _points.size() << " points\n";
  }

  if (_points.empty()) {
    // With no position reports available, this function does nothing,
    // except to make sure that our velocity gets reset to zero after
    // a period of time.

    if (_smooth_position_known) {
      double age = timestamp - _smooth_timestamp;
      if (age > _reset_velocity_age) {
        if (deadrec_cat.is_debug()) {
          deadrec_cat.debug()
            << "points empty; reset velocity, age = " << age << "\n";
        }
        _smooth_forward_velocity = 0.0;
        _smooth_lateral_velocity = 0.0;
        _smooth_rotational_velocity = 0.0;
      }
    }
    bool result = _smooth_position_changed;
    _smooth_position_changed = false;

    if (deadrec_cat.is_spam()) {
      deadrec_cat.spam()
        << "  no points: " << result << "\n";
    }
    return result;
  }
  if (_smooth_mode == SM_off) {
    // With smoothing disabled, this function also does nothing,
    // except to ensure that any old bogus position reports are
    // cleared.
    clear_positions(false);
    bool result = _smooth_position_changed;
    _smooth_position_changed = false;

    if (deadrec_cat.is_spam()) {
      deadrec_cat.spam()
        << "  disabled: " << result << "\n";
    }
    return result;
  }

  // First, back up in time by the specified delay factor.
  double orig_timestamp = timestamp;
  timestamp -= _delay;
  if (_accept_clock_skew) {
    timestamp -= get_avg_timestamp_delay();
  }

  if (deadrec_cat.is_spam()) {
    deadrec_cat.spam()
      << "time = " << timestamp << ", " << _points.size()
      << " points, last = " << _last_point_before << ", " 
      << _last_point_after << "\n";
    deadrec_cat.spam(false)
      << "  ";
    for (int pi = 0; pi < (int)_points.size(); pi++) {
      deadrec_cat.spam(false) << _points[pi]._timestamp << " ";
    }
    deadrec_cat.spam(false) << "\n";
  }

  // Now look for the two bracketing position reports.
  int point_way_before = -1;
  int point_before = -1;
  double timestamp_before = 0.0;
  int point_after = -1;
  double timestamp_after = 0.0;

  int num_points = _points.size();
  int i;

  // Find the newest of the points before the indicated time.  Assume
  // that this will be no older than _last_point_before.
  i = max(0, _last_point_before);
  while (i < num_points && _points[i]._timestamp < timestamp) {
    point_before = i;
    timestamp_before = _points[i]._timestamp;
    ++i;
  }
  point_way_before = max(point_before - 1, -1);

  // Now the next point is presumably the oldest point after the
  // indicated time.
  if (i < num_points) {
    point_after = i;
    timestamp_after = _points[i]._timestamp;
  }

  if (deadrec_cat.is_spam()) {
    deadrec_cat.spam()
      << "  found points (" << point_way_before << ") " << point_before
      << ", " << point_after << "\n";
  }

  if (point_before < 0) {
    nassertr(point_after >= 0, false);
    // If we only have an after point, we have to start there.
    bool result = !(_last_point_before == point_before && 
                    _last_point_after == point_after);
    const SamplePoint &point = _points[point_after];
    set_smooth_pos(point._pos, point._hpr, timestamp);
    _smooth_forward_velocity = 0.0;
    _smooth_lateral_velocity = 0.0;
    _smooth_rotational_velocity = 0.0;
    _last_point_before = point_before;
    _last_point_after = point_after;
    if (deadrec_cat.is_spam()) {
      deadrec_cat.spam()
        << "  only an after point: " << _last_point_before << ", " 
        << _last_point_after << "\n";
    }
    return result;
  }

  bool result = true;

  if (point_after < 0 && _prediction_mode != PM_off) {
    // With prediction in effect, we're allowed to anticipate where
    // the avatar is going by a tiny bit, if we don't have current
    // enough data.  This works only if we have at least two points of
    // old data.
    if (point_way_before >= 0) {
      // To implement simple prediction, we simply back up in time to
      // the previous two timestamps, and base our linear
      // interpolation off of those two, extending into the future.
      SamplePoint &point = _points[point_way_before];
      point_after = point_before;
      timestamp_after = timestamp_before;
      point_before = point_way_before;
      timestamp_before = point._timestamp;
      
      if (timestamp > timestamp_after + _max_position_age) {
        // Don't allow the prediction to get too far into the
        // future.
        timestamp = timestamp_after + _max_position_age;
      }
    }
  }

  if (point_after < 0) {
    // If we only have a before point even after we've checked for the
    // possibility of using prediction, then we have to stop there.
    if (point_way_before >= 0) {
      // Use the previous two points, if we've got 'em, so we can
      // still reflect the avatar's velocity.
      if (deadrec_cat.is_spam()) {
        deadrec_cat.spam()
          << "  previous two\n";
      }
      linear_interpolate(point_way_before, point_before, timestamp_before);

    } else {
      if (deadrec_cat.is_spam()) {
        deadrec_cat.spam()
          << "  one point\n";
      }
      // If we really only have one point, use it.
      const SamplePoint &point = _points[point_before];
      set_smooth_pos(point._pos, point._hpr, timestamp);
    }

    double age = timestamp - timestamp_before;
    if (age > _reset_velocity_age) {
      if (deadrec_cat.is_spam()) {
        deadrec_cat.spam()
          << "  reset_velocity, age = " << age << "\n";
      }
      _smooth_forward_velocity = 0.0;
      _smooth_lateral_velocity = 0.0;
      _smooth_rotational_velocity = 0.0;
    }

    result = !(_last_point_before == point_before && 
               _last_point_after == point_after);
  } else {
    // If we have two points, we can linearly interpolate between them.
    if (deadrec_cat.is_spam()) {
      deadrec_cat.spam()
        << "  normal interpolate\n";
    }
    SamplePoint &point_b = _points[point_before];
    const SamplePoint &point_a = _points[point_after];
    
    if (point_b._pos == point_a._pos && point_b._hpr == point_a._hpr) {
      // The points are equivalent, so just return that.
      if (deadrec_cat.is_spam()) {
        deadrec_cat.spam()
          << "Points are equivalent\n";
      }
      set_smooth_pos(point_b._pos, point_b._hpr, timestamp);

      // This implies that velocity is 0.
      _smooth_forward_velocity = 0.0;
      _smooth_lateral_velocity = 0.0;
      _smooth_rotational_velocity = 0.0;

    } else {
      // The points are different, so we have to do some work.
      double age = (point_a._timestamp - point_b._timestamp);
    
      if (_default_to_standing_still && (age > _max_position_age)) {
        // If the first point is too old, assume there were a lot of
        // implicit standing still messages that weren't sent.  Insert a new
        // sample point to reflect this.
        if (deadrec_cat.is_spam()) {
          deadrec_cat.spam()
            << "  first point too old: age = " << age << "\n";
        }
        SamplePoint new_point = point_b;
        new_point._timestamp = point_a._timestamp - _expected_broadcast_period;
        if (deadrec_cat.is_spam()) {
          deadrec_cat.spam()
            << "  constructed new timestamp at " << new_point._timestamp
            << "\n";
        }
        if (new_point._timestamp > point_b._timestamp) {
          _points.insert(_points.begin() + point_after, new_point);
          
          // Now we've monkeyed with the sequence.  Start over.
          if (deadrec_cat.is_spam()) {
            deadrec_cat.spam()
              << "  recursing after time adjustment.\n";
          }
          return compute_smooth_position(orig_timestamp);
        }
      }

      linear_interpolate(point_before, point_after, timestamp);
    }
  }

  if (deadrec_cat.is_spam()) {
    deadrec_cat.spam()
      << "  changing " << _last_point_before << ", " << _last_point_after
      << " to " << point_before << ", " << point_after << "\n";
  }
  _last_point_before = point_before;
  _last_point_after = point_after;


  // Assume we'll never get another compute_smooth_position() request
  // for an older time than this, and remove all the timestamps at the
  // head of the queue up to but not including point_way_before.
  while (point_way_before > 0) {
    nassertr(!_points.empty(), result);
    _points.pop_front();

    --point_way_before;
    --_last_point_before;
    --_last_point_after;
    if (deadrec_cat.is_spam()) {
      deadrec_cat.spam()
        << "  popping old point: " << _last_point_before << ", " 
        << _last_point_after << "\n";
    }
  }

  // If we are not using prediction mode, we can also remove
  // point_way_before.
  if (_prediction_mode == PM_off) {
    if (point_way_before == 0) {
      nassertr(!_points.empty(), result);
      _points.pop_front();
      
      --point_way_before;
      --_last_point_before;
      --_last_point_after;
      if (deadrec_cat.is_spam()) {
        deadrec_cat.spam()
          << "  popping way_before point: " << _last_point_before << ", " 
          << _last_point_after << "\n";
      }
    }

    // And if there's only one point left, remove even that one
    // after a while.
    /* jbutler: commented this out, seems to cause the smoothing pop that occurs
                when this object is stopped for a while then starts moving again
    if (_points.size() == 1) {
      double age = timestamp - _points.back()._timestamp;
      if (deadrec_cat.is_spam()) {
        deadrec_cat.spam()
          << "considering clearing all points, age = " << age << "\n";
      }
      if (age > _reset_velocity_age) {
        if (deadrec_cat.is_spam()) {
          deadrec_cat.spam()
            << "clearing all points.\n";
        }
        _points.clear();
      }
    }
    */
  }

  if (deadrec_cat.is_spam()) {
    deadrec_cat.spam()
      << "  result = " << result << "\n";
  }

  return result;
}

////////////////////////////////////////////////////////////////////
//     Function: SmoothMover::get_latest_position
//       Access: Published
//  Description: Updates the smooth_pos (and smooth_hpr, etc.) members
//               to reflect the absolute latest position known for
//               this avatar.  This may result in a pop to the most
//               recent position.
//
//               Returns true if the latest position is known, false
//               otherwise.
////////////////////////////////////////////////////////////////////
bool SmoothMover::
get_latest_position() {
  if (deadrec_cat.is_debug()) {
    deadrec_cat.debug() << "get_latest_position\n";
  }
  if (_points.empty()) {
    // Nothing to do if there are no points.
    return _smooth_position_known;
  }

  const SamplePoint &point = _points.back();
  set_smooth_pos(point._pos, point._hpr, point._timestamp);
  _smooth_forward_velocity = 0.0;
  _smooth_lateral_velocity = 0.0;
  _smooth_rotational_velocity = 0.0;
  return true;
}

////////////////////////////////////////////////////////////////////
//     Function: SmoothMover::output
//       Access: Published
//  Description: 
////////////////////////////////////////////////////////////////////
void SmoothMover::
output(ostream &out) const {
  out << "SmoothMover, " << _points.size() << " sample points.";
}

////////////////////////////////////////////////////////////////////
//     Function: SmoothMover::write
//       Access: Published
//  Description: 
////////////////////////////////////////////////////////////////////
void SmoothMover::
write(ostream &out) const {
  out << "SmoothMover, " << _points.size() << " sample points:\n";
  int num_points = _points.size();
  for (int i = 0; i < num_points; i++) {
    const SamplePoint &point = _points[i];
    out << "  " << i << ". time = " << point._timestamp << " pos = " 
        << point._pos << " hpr = " << point._hpr << "\n";
  }
}

////////////////////////////////////////////////////////////////////
//     Function: SmoothMover::set_smooth_pos
//       Access: Private
//  Description: Sets the computed smooth position and orientation for
//               the indicated timestamp.
////////////////////////////////////////////////////////////////////
void SmoothMover::
set_smooth_pos(const LPoint3 &pos, const LVecBase3 &hpr,
               double timestamp) {
  if (deadrec_cat.is_spam()) {
    deadrec_cat.spam()
      << "set_smooth_pos(" << pos << ", " << hpr << ", "
      << timestamp << ")\n";
  }

  if (_smooth_pos != pos) {
    _smooth_pos = pos;
    _smooth_position_changed = true;
  }
  if (_smooth_hpr != hpr) {
    _smooth_hpr = hpr;
    _smooth_position_changed = true;
    _computed_forward_axis = false;
  }

  _smooth_timestamp = timestamp;
  _smooth_position_known = true;
}

////////////////////////////////////////////////////////////////////
//     Function: SmoothMover::linear_interpolate
//       Access: Private
//  Description: Interpolates the smooth position linearly between the
//               two bracketing position reports.
////////////////////////////////////////////////////////////////////
void SmoothMover::
linear_interpolate(int point_before, int point_after, double timestamp) {
  SamplePoint &point_b = _points[point_before];
  const SamplePoint &point_a = _points[point_after];

  double age = (point_a._timestamp - point_b._timestamp);

  /*
  Points::const_iterator pi;
  cout << "linear_interpolate: ";
  for (pi = _points.begin(); pi != _points.end(); ++pi) {
    cout << "(" << (*pi)._pos << "), ";
  }
  cout << endl;
  */

  if (point_before == _last_point_before && 
      point_after == _last_point_after) {
    if (deadrec_cat.is_spam()) {
      deadrec_cat.spam()
        << "  same two points\n";
    }

    // If these are the same two points we found last time (which is
    // likely), we can save a bit of work.
    double t = (timestamp - point_b._timestamp) / age;

    if (deadrec_cat.is_spam()) {
      deadrec_cat.spam()
        << "   interp " << t << ": " << point_b._pos << " to " << point_a._pos
        << "\n";
    }
    set_smooth_pos(point_b._pos + t * (point_a._pos - point_b._pos),
                   point_b._hpr + t * (point_a._hpr - point_b._hpr),
                   timestamp);

    // The velocity remains the same as last time.

  } else {
    // To interpolate the hpr's, we must first make sure that both
    // angles are on the same side of the discontinuity.
    for (int j = 0; j < 3; j++) {
      if ((point_b._hpr[j] - point_a._hpr[j]) > 180.0) {
        point_b._hpr[j] -= 360.0;
      } else if ((point_b._hpr[j] - point_a._hpr[j]) < -180.0) {
        point_b._hpr[j] += 360.0;
      }
    }
    
    double t = (timestamp - point_b._timestamp) / age;
    LVector3 pos_delta = point_a._pos - point_b._pos;
    LVecBase3 hpr_delta = point_a._hpr - point_b._hpr;

    if (deadrec_cat.is_spam()) {
      deadrec_cat.spam()
        << "   interp " << t << ": " << point_b._pos << " to " << point_a._pos
        << "\n";
    }
    set_smooth_pos(point_b._pos + t * pos_delta, 
                   point_b._hpr + t * hpr_delta, 
                   timestamp);
    compute_velocity(pos_delta, hpr_delta, age);
  }
}

////////////////////////////////////////////////////////////////////
//     Function: SmoothMover::compute_velocity
//       Access: Private
//  Description: Computes the forward and rotational velocities of the
//               moving object.
////////////////////////////////////////////////////////////////////
void SmoothMover::
compute_velocity(const LVector3 &pos_delta, const LVecBase3 &hpr_delta,
                 double age) {
  _smooth_rotational_velocity = hpr_delta[0] / age;

  if (_directional_velocity) {
    // To get just the forward component of velocity, we need to project
    // the velocity vector onto the y axis, as rotated by the current
    // hpr.
    if (!_computed_forward_axis) {
      LMatrix3 rot_mat;
      compose_matrix(rot_mat, LVecBase3(1.0, 1.0, 1.0), _smooth_hpr);
      _forward_axis = LVector3(0.0, 1.0, 0.0) * rot_mat;
      
      if (deadrec_cat.is_spam()) {
        deadrec_cat.spam()
          << "  compute forward_axis = " << _forward_axis << "\n";
      }
    }
    
    LVector3 lateral_axis = _forward_axis.cross(LVector3(0.0,0.0,1.0));
    
    PN_stdfloat forward_distance = pos_delta.dot(_forward_axis);
    PN_stdfloat lateral_distance = pos_delta.dot(lateral_axis);
    
    _smooth_forward_velocity = forward_distance / age;
    _smooth_lateral_velocity = lateral_distance / age;

  } else {
    _smooth_forward_velocity = pos_delta.length();
    _smooth_lateral_velocity = 0.0f;
  }   

  if (deadrec_cat.is_spam()) {
    deadrec_cat.spam()
      << "  compute_velocity = " << _smooth_forward_velocity << "\n";
  }
}

////////////////////////////////////////////////////////////////////
//     Function: SmoothMover::record_timestamp_delay
//       Access: Private
//  Description: Records the delay measured in receiving this
//               particular timestamp.  The average delay of the last
//               n timestamps will be used to smooth the motion
//               properly.
////////////////////////////////////////////////////////////////////
void SmoothMover::
record_timestamp_delay(double timestamp) {
  double now = ClockObject::get_global_clock()->get_frame_time();

  // Convert the delay to an integer number of milliseconds.  Integers
  // are better than doubles because they don't accumulate errors over
  // time.
  int delay = (int)((now - timestamp) * 1000.0);
  if (_timestamp_delays.full()) {
    _net_timestamp_delay -= _timestamp_delays.front();
    _timestamp_delays.pop_front();
  }
  _net_timestamp_delay += delay;
  _timestamp_delays.push_back(delay);

  _last_heard_from = now;
}

////////////////////////////////////////////////////////////////////
//     Function: SmoothMover::handle_wrt_reparent
//       Access: Private
//  Description: Node is being wrtReparented, update recorded
//               sample positions to reflect new parent
////////////////////////////////////////////////////////////////////
void SmoothMover::
handle_wrt_reparent(NodePath &old_parent, NodePath &new_parent) {
  Points::iterator pi;
  NodePath np = old_parent.attach_new_node("smoothMoverWrtReparent");

  //cout << "handle_wrt_reparent: ";
  for (pi = _points.begin(); pi != _points.end(); pi++) {
    np.set_pos_hpr((*pi)._pos, (*pi)._hpr);
    (*pi)._pos = np.get_pos(new_parent);
    (*pi)._hpr = np.get_hpr(new_parent);
    //cout << "(" << (*pi)._pos << "), ";
  }
  //cout << endl;
  
  np.set_pos_hpr(_sample._pos, _sample._hpr);
  _sample._pos = np.get_pos(new_parent);
  _sample._hpr = np.get_hpr(new_parent);

  np.set_pos_hpr(_smooth_pos, _smooth_hpr);
  _smooth_pos = np.get_pos(new_parent);
  _smooth_hpr = np.get_hpr(new_parent);

  _computed_forward_axis = false;

  np.detach_node();
}
