예제 #1
0
/*  num_deriv_exterior() calculates the partial numerical derivative of image
    coordinates of a given 3D position, over each of the 6 exterior orientation
    parameters (3 position parameters, 3 rotation angles).

    Arguments:
    Calibration* cal_in  -      camera calibration object
    control_par *cpar -     control parameters

    double dpos, double dang - the step size for numerical differentiation,
        dpos for the metric variables, dang for the angle variables. Units
        are same as the units of the variables derived.
    vec3d pos - the current 3D position represented on the image.
    
    Return parameters:
    double x_ders[6], y_ders[6] respectively the derivatives of the x and y
        image coordinates as function of each of the orientation parameters.
 */
void num_deriv_exterior(Calibration* cal, control_par *cpar, double dpos, double dang, vec3d pos,
    double x_ders[6], double y_ders[6])
{
    int pd; 
    double step, xs, ys, xpd, ypd;
    double* vars[6];
    
    vars[0] = &(cal->ext_par.x0);
    vars[1] = &(cal->ext_par.y0);
    vars[2] = &(cal->ext_par.z0);
    vars[3] = &(cal->ext_par.omega);
    vars[4] = &(cal->ext_par.phi);
    vars[5] = &(cal->ext_par.kappa);
    
    /* Starting image position */
    rotation_matrix(&(cal->ext_par));
    img_coord (pos, cal, cpar->mm, &xs, &ys);
    
    for (pd = 0; pd < 6; pd++) {
        step = (pd > 2) ? dang : dpos;
        
        *(vars[pd]) += step;
        if (pd > 2) rotation_matrix(&(cal->ext_par));
        img_coord (pos, cal, cpar->mm, &xpd, &ypd);
        x_ders[pd] = (xpd - xs) / step;
        y_ders[pd] = (ypd - ys) / step;
        *(vars[pd]) -= step;
    }
    rotation_matrix(&(cal->ext_par));
}
예제 #2
0
scene::scene( const vec &camera ) :
		camera_width( 3 ), camera_height( 3 ), camera_center( 3 ), camera_slope(
				3 ) {
	camera_center = camera;

	float x = 0.9285;
	float y = 0;
	float z = -0.3714;
	float angle = 1.5;

	camera_slope( 0 ) = x;
	camera_slope( 1 ) = y;
	camera_slope( 2 ) = z;

	vec camera_up( 3 );
	camera_up( 0 ) = -z;
	camera_up( 1 ) = y;
	camera_up( 2 ) = x;

	vec y_cross( 3 );
	y_cross( 0 ) = 0;
	y_cross( 1 ) = -1;
	y_cross( 2 ) = 0;

	camera_width = (rotation_matrix( camera_up, -angle / 2.0 ) * camera_slope)
			- (rotation_matrix( camera_up, angle / 2.0 ) * camera_slope);
	camera_height = (rotation_matrix( y_cross, -angle / 2.0 ) * camera_slope)
			- (rotation_matrix( y_cross, angle / 2.0 ) * camera_slope);
}
예제 #3
0
/// rotate selection
void selection_rotate(const vec3f& ea) {
    if(selected_point) return;
    if(selected_frame) {
        auto m = translation_matrix(selected_frame->o) *
                 rotation_matrix(ea.x, selected_frame->x) *
                 rotation_matrix(ea.y, selected_frame->y) *
                 rotation_matrix(ea.z, selected_frame->z) *
                 translation_matrix(- selected_frame->o);
        *selected_frame = transform_frame(m, *selected_frame);
    }
}
예제 #4
0
void Infill::generateLineInfill(Polygons& result, int line_distance, const double& fill_angle, int64_t shift)
{
    PointMatrix rotation_matrix(fill_angle);
    NoZigZagConnectorProcessor lines_processor(rotation_matrix, result);
    bool connected_zigzags = false;
    generateLinearBasedInfill(outline_offset, result, line_distance, rotation_matrix, lines_processor, connected_zigzags, shift);
}
예제 #5
0
bool checkBoundaries(double range, double laser_theta){

  //put robot position in frame where bottom is aligned with 0deg.
  //rotation_matrix(cur_pos,-orientation);

  geometry_msgs::Pose2D laserPoint;

  laserPoint.x = cur_pos.x + range*cos(laser_theta+cur_pos.theta);
  laserPoint.y = cur_pos.y + range*sin(laser_theta+cur_pos.theta);
  

  //ROS_INFO("Current: (%f, %f) -> %f\n", cur_pos.x, cur_pos.y, cur_pos.theta);
  //ROS_INFO("Laser point: (%f, %f)", laserPoint.x, laserPoint.y);
  //ROS_INFO("Range/theta: %f/%f",range, laser_theta);

  rotation_matrix(laserPoint, -orientation);

  if(single_i){
    if(laserPoint.x > SI_snow_length_max || laserPoint.x < SI_snow_length_min)
      return false;
    if(laserPoint.y > SI_snow_top || laserPoint.y < SI_snow_bottom)
      return false;
  }else{    
    if(laserPoint.x >  TI_snow_length_max || laserPoint.x < TI_snow_length_min)
      return false;
    if(laserPoint.y > TI_snow_top || laserPoint.y < TI_snow_bottom)
      return false;
  }

  return true;
}
예제 #6
0
void ComputeOrientation(const osg::Vec3& lv,const osg::Vec3& up, osg::Quat &rotationDest)
{
osg::Vec3 f(lv);
f.normalize();
osg::Vec3 s(f^up);
s.normalize();
osg::Vec3 u(s^f);
u.normalize();


{
__asm NOP
}


osg::Matrix rotation_matrix(s[0],     u[0],     -f[0],     0.0f,
                            s[1],     u[1],     -f[1],     0.0f,
                            s[2],     u[2],     -f[2],     0.0f,
                            0.0f,     0.0f,     0.0f,      1.0f);
rotationDest.set(rotation_matrix);
//debug_matrix = rotation_matrix;
//debug_quat = rotationDest;
rotationDest = rotationDest.inverse();

} // ComputeOrientation
예제 #7
0
// compute the frame from an animation
frame3f animate_compute_frame(FrameAnimation* animation, int time) {
    // find keyframe interval and t
        auto interval_t = get_keyframe_details(animation->keytimes, time);
        auto interval   = interval_t.first;
        auto t          = interval_t.second;


        // get translation and rotation matrices
        auto trans=translation_matrix(t*(animation->translation)[interval+1]+(1-t)*(animation->translation)[interval]);
        auto rot=t*((animation->rotation)[interval+1])+(1-t)*((animation->rotation)[interval]);
        // compute combined xform matrix
        auto rot_x=rotation_matrix(rot[0], x3f);
        auto rot_y=rotation_matrix(rot[1], y3f);
        auto rot_z=rotation_matrix(rot[2], z3f);
        // return the transformed rest frame
        return transform_frame(trans*rot_z*rot_y*rot_x, animation->rest_frame);
}
예제 #8
0
const glm::mat4 MovableObject::matrix() const{
	if(translation_matrix_dirty_ 
			|| rotation_matrix_dirty_ 
			|| scale_matrix_dirty_)  {
		matrix_ = translation_matrix()*rotation_matrix()*scale_matrix();
	}
	return matrix_;
}
예제 #9
0
Wind _prevailing_wind (Vector2 pressure_gradient_force, double coriolis_coefficient, double friction_coefficient) {
	double angle_offset = std::atan2(coriolis_coefficient, friction_coefficient);
	double speed = length(pressure_gradient_force) / length(Vector2(coriolis_coefficient, friction_coefficient));
	Vector2 v = rotation_matrix(angle(pressure_gradient_force) - angle_offset) * Vector2(1.0, 0.0);
	Wind w;
	w.speed = speed;
	w.direction = std::atan2(v.y, v.x);
	return w;
}
// convert a vector from earth to body frame
void Quaternion_D::earth_to_body(Vector3f &v)
{
    Matrix3f m;
    // we reverse z before and afterwards because of the differing
    // quaternion conventions from APM conventions.
    v.z = -v.z;
    rotation_matrix(m);
    v = m * v;
    v.z = -v.z;
}
예제 #11
0
파일: sim3.cpp 프로젝트: EI2012zyq/OpenMVO
Matrix4d Sim3
::matrix() const
{
  Matrix4d homogenious_matrix;
  homogenious_matrix.setIdentity();
  homogenious_matrix.block(0,0,3,3) = scale()*rotation_matrix();
  homogenious_matrix.col(3).head(3) = translation();
  homogenious_matrix(3,3) = 1;
  return homogenious_matrix;
}
예제 #12
0
void LLCoordFrame::setAxes(const LLQuaternion &q )
{
	LLMatrix3 rotation_matrix(q);
	setAxes(rotation_matrix);
	if( !isFinite() )
	{
		reset();
		llwarns << "Non Finite in LLCoordFrame::setAxes()" << llendl;
	}
}
예제 #13
0
void LLCoordFrame::yaw(F32 angle)
{
	LLQuaternion q(angle, mZAxis);
	LLMatrix3 rotation_matrix(q);
	rotate(rotation_matrix);

	if( !mXAxis.isFinite() || !mYAxis.isFinite() )
	{
		reset();
		llwarns << "Non Finite in LLCoordFrame::yaw()" << llendl;
	}
}
예제 #14
0
// create eulers from a quaternion
void Quaternion::to_vector312(float &roll, float &pitch, float &yaw) const
{    
    Matrix3f m;
    rotation_matrix(m);
    float T21 = m.a.y;
    float T22 = m.b.y;
    float T23 = m.c.y;
    float T13 = m.c.x;
    float T33 = m.c.z;
    yaw = atan2f(-T21, T22);
    roll = safe_asin(T23);
    pitch = atan2f(-T13, T33);
}
예제 #15
0
LLCoordFrame::LLCoordFrame(const LLVector3 &origin, const LLQuaternion &q) :
	mOrigin(origin)
{
	LLMatrix3 rotation_matrix(q);
	mXAxis.setVec(rotation_matrix.mMatrix[VX]);
	mYAxis.setVec(rotation_matrix.mMatrix[VY]);
	mZAxis.setVec(rotation_matrix.mMatrix[VZ]);

	if( !isFinite() )
	{
		reset();
		llwarns << "Non Finite in LLCoordFrame::LLCoordFrame()" << llendl;
	}
}
예제 #16
0
/**************************************************
 * read_calibration() reads orientation fiels and creates a Calibration object
 * that represents the files' data.
 * 
 * Note: for now it uses read_ori(). This is scheduled to change soon.
 * 
 * Arguments:
 * char *ori_file - name of the file containing interior, exterior, and glass
 *   parameters.
 * char *add_file - name of the file containing additional orientation
 *   parameters.
 * char *fallback_file - name of file to use if add_file can't be opened.
 *
 * Returns:
 * On success, a pointer to a new Calibration object. On failure, NULL.
 */
Calibration *read_calibration(char *ori_file, char *add_file,
    char *fallback_file)
{
    Calibration *ret = (Calibration *) malloc(sizeof(Calibration));
    
    if (read_ori(&(ret->ext_par), &(ret->int_par), &(ret->glass_par), ori_file,
        &(ret->added_par), add_file, fallback_file))
    {
        rotation_matrix(ret->ext_par, ret->ext_par.dm);
        return ret;
    } else {
        free(ret);
        return NULL;
    }
}
예제 #17
0
osg::Matrix CameraModel::get_rot() const {
    osg::Vec3d lv( _center - _eye );

    osg::Vec3d f( lv );
    f.normalize();
    osg::Vec3d s( f^_up );
    s.normalize();
    osg::Vec3d u( s^f );
    u.normalize();

    osg::Matrixd rotation_matrix( s[0], u[0], -f[0], 0.0f,
                            s[1], u[1], -f[1], 0.0f,
                            s[2], u[2], -f[2], 0.0f,
                            0.0f, 0.0f,  0.0f, 1.0f );
    return rotation_matrix;
}
예제 #18
0
/* Generate a calibration object with example values matching those in the
   files read by test_read_ori.
*/
Calibration test_cal(void) {
    Exterior correct_ext = {
        105.2632, 102.7458, 403.8822,
        -0.2383291, 0.2442810, 0.0552577, 
        {{0.9688305, -0.0535899, 0.2418587}, 
        {-0.0033422, 0.9734041, 0.2290704},
        {-0.2477021, -0.2227387, 0.9428845}}};
    Interior correct_int = {-2.4742, 3.2567, 100.0000};
    Glass correct_glass = {0.0001, 0.00001, 150.0};
    ap_52 correct_addp = {0., 0., 0., 0., 0., 1., 0.};
    Calibration correct_cal = {correct_ext, correct_int, correct_glass, 
        correct_addp};
    rotation_matrix(&(correct_cal.ext_par));
    
    return correct_cal;
}
예제 #19
0
void ReferenceFrame::Set(Eigen::Vector3d const& translation,
                         Eigen::Vector3d const& rotation) {
  Eigen::Matrix4d translation_matrix = Eigen::Matrix4d::Identity(),
                  rotation_matrix = Eigen::Matrix4d::Zero();

  rotation_matrix.block<3, 3>(0, 0) =
      (Eigen::AngleAxisd(rotation(2), Eigen::Vector3d::UnitZ()) *
       Eigen::AngleAxisd(rotation(1), Eigen::Vector3d::UnitY()) *
       Eigen::AngleAxisd(rotation(0), Eigen::Vector3d::UnitX()))
          .toRotationMatrix();
  rotation_matrix(3, 3) = 1;

  translation_matrix.block<3, 1>(0, 3) = translation;

  // Rotate first, then translate
  homogenous_transform_ = translation_matrix * rotation_matrix;
}
예제 #20
0
int SRS::R1Psi(Matrix C, Matrix s, Matrix o)
{
    Matrix R0;

    SolveR1((float) 0, R0);

    // 
    // R1(psi)  = R0*R(n,psi) 
    // R(n,psi) = cos(psi)*C + sin(psi)*s + o
    // 

    rotation_matrix(n, C, s, o);
    rmatmult(C,R0,C);
    rmatmult(s,R0,s);
    rmatmult(o,R0,o);

    return 1;
}
void QuadrotorAerodynamics::update(double dt)
{
  if (dt <= 0.0) return;
  boost::mutex::scoped_lock lock(mutex_);

  // fill input vector u for drag model
  drag_model_->u[0] =  (twist_.linear.x - wind_.x);
  drag_model_->u[1] = -(twist_.linear.y - wind_.y);
  drag_model_->u[2] = -(twist_.linear.z - wind_.z);
  drag_model_->u[3] =  twist_.angular.x;
  drag_model_->u[4] = -twist_.angular.y;
  drag_model_->u[5] = -twist_.angular.z;

  // We limit the input velocities to +-100. Required for numeric stability in case of collisions,
  // where velocities returned by Gazebo can be very high.
  limit(drag_model_->u, -100.0, 100.0);

  // convert input to body coordinates
  Eigen::Quaterniond orientation(orientation_.w, orientation_.x, orientation_.y, orientation_.z);
  Eigen::Matrix<double,3,3> rotation_matrix(orientation.toRotationMatrix());
  Eigen::Map<Eigen::Vector3d> linear(&(drag_model_->u[0]));
  Eigen::Map<Eigen::Vector3d> angular(&(drag_model_->u[3]));
  linear  = rotation_matrix * linear;
  angular = rotation_matrix * angular;

  ROS_DEBUG_STREAM_NAMED("quadrotor_aerodynamics", "aerodynamics.twist:  " << PrintVector<double>(drag_model_->u.begin(), drag_model_->u.begin() + 6));
  checknan(drag_model_->u, "drag model input");

  // update drag model
  f(drag_model_->u.data(), dt, drag_model_->y.data());

  ROS_DEBUG_STREAM_NAMED("quadrotor_aerodynamics", "aerodynamics.force:  " << PrintVector<double>(drag_model_->y.begin() + 0, drag_model_->y.begin() + 3));
  ROS_DEBUG_STREAM_NAMED("quadrotor_aerodynamics", "aerodynamics.torque: " << PrintVector<double>(drag_model_->y.begin() + 3, drag_model_->y.begin() + 6));
  checknan(drag_model_->y, "drag model output");

  // drag_model_ gives us inverted vectors!
  wrench_.force.x  = -( drag_model_->y[0]);
  wrench_.force.y  = -(-drag_model_->y[1]);
  wrench_.force.z  = -(-drag_model_->y[2]);
  wrench_.torque.x = -( drag_model_->y[3]);
  wrench_.torque.y = -(-drag_model_->y[4]);
  wrench_.torque.z = -(-drag_model_->y[5]);
}
예제 #22
0
/*
 * Ok, simulate a track-ball.  Project the points onto the virtual
 * trackball, then figure out the axis of rotation, which is the cross
 * product of P1 P2 and O P1 (O is the center of the ball, 0,0,0)
 * Note:  This is a deformed trackball-- is a trackball in the center,
 * but is deformed into a hyperbolic sheet of rotation away from the
 * center.  This particular function was chosen after trying out
 * several variations.
 *
 * It is assumed that the arguments to this routine are in the range
 * (-1.0 ... 1.0)
 */
void CameraManipulator::trackball(osg::Vec3& axis,float& angle, float p1x, float p1y, float p2x, float p2y)
{
    /*
     * First, figure out z-coordinates for projection of P1 and P2 to
     * deformed sphere
     */

    osg::Matrix rotation_matrix(_rotation);


    osg::Vec3 uv = osg::Vec3(0.0f,1.0f,0.0f)*rotation_matrix;
    osg::Vec3 sv = osg::Vec3(1.0f,0.0f,0.0f)*rotation_matrix;
    osg::Vec3 lv = osg::Vec3(0.0f,0.0f,-1.0f)*rotation_matrix;

    osg::Vec3 p1 = sv * p1x + uv * p1y - lv * tb_project_to_sphere(_trackballSize, p1x, p1y);
    osg::Vec3 p2 = sv * p2x + uv * p2y - lv * tb_project_to_sphere(_trackballSize, p2x, p2y);

    /*
     *  Now, we want the cross product of P1 and P2
     */

// Robert,
//
// This was the quick 'n' dirty  fix to get the trackball doing the right 
// thing after fixing the Quat rotations to be right-handed.  You may want
// to do something more elegant.
//   axis = p1^p2;
axis = p2^p1;
    axis.normalize();

    /*
     *  Figure out how much to rotate around that axis.
     */
    float t = (p2 - p1).length() / (2.0 * _trackballSize);

    /*
     * Avoid problems with out-of-control values...
     */
    if (t > 1.0) t = 1.0;
    if (t < -1.0) t = -1.0;
    angle = osg::inRadians(asin(t));

}
cv::Mat OpenCVCameraEstimation::estimate(std::vector<imageio::ModelLandmark> imagePoints, cv::Mat intrinsicCameraMatrix, std::vector<int> vertexIds /*= std::vector<int>()*/)
{
	if (imagePoints.size() < 3) {
		Loggers->getLogger("morphablemodel").error("CameraEstimation: Number of points given is smaller than 3.");
		throw std::runtime_error("CameraEstimation: Number of points given is smaller than 3.");
	}

	// Todo: Currently, the optional vertexIds is not used
	vector<Point2f> points2d;
	vector<Point3f> points3d;
	for (const auto& landmark : imagePoints) {
		points2d.emplace_back(landmark.getPoint2D());
		points3d.emplace_back(morphableModel.getShapeModel().getMeanAtPoint(landmark.getName()));
	}

	//Estimate the pose
	Mat rvec(3, 1, CV_64FC1);
	Mat tvec(3, 1, CV_64FC1);
	if (points2d.size() == 3) {
		cv::solvePnP(points3d, points2d, intrinsicCameraMatrix, vector<float>(), rvec, tvec, false, CV_ITERATIVE); // CV_ITERATIVE (3pts) | CV_P3P (4pts) | CV_EPNP (4pts)
	} else {
		cv::solvePnP(points3d, points2d, intrinsicCameraMatrix, vector<float>(), rvec, tvec, false, CV_EPNP); // CV_ITERATIVE (3pts) | CV_P3P (4pts) | CV_EPNP (4pts)
		// Alternative, more outlier-resistant:
		// cv::solvePnPRansac(modelPoints, imagePoints, camMatrix, distortion, rvec, tvec, false); // min 4 points
		// has an optional argument 'inliers' - might be useful
	}

	// Convert rvec/tvec to matrices, etc... return 4x4 extrinsic camera matrix
	Mat rotation_matrix(3, 3, CV_64FC1);
	cv::Rodrigues(rvec, rotation_matrix);
	rotation_matrix.convertTo(rotation_matrix, CV_32FC1);
	Mat translation_vector = tvec;
	translation_vector.convertTo(translation_vector, CV_32FC1);

	Mat extrinsicCameraMatrix = Mat::zeros(4, 4, CV_32FC1);
	Mat extrRot = extrinsicCameraMatrix(cv::Range(0, 3), cv::Range(0, 3));
	rotation_matrix.copyTo(extrRot);
	Mat extrTrans = extrinsicCameraMatrix(cv::Range(0, 3), cv::Range(3, 4));
	translation_vector.copyTo(extrTrans);
	extrinsicCameraMatrix.at<float>(3, 3) = 1; // maybe set (3, 2) = 1 here instead so that the renderer can do divByW as well? (see Todo in libRender)

	return extrinsicCameraMatrix;
}
예제 #24
0
void CSulManipulatorCamera::computePosition( const osg::Vec3& eye,const osg::Vec3& center,const osg::Vec3& up )
{
    osg::Vec3 lv(center-eye);

    osg::Vec3 f(lv);
    f.normalize();
    osg::Vec3 s(f^up);
    s.normalize();
    osg::Vec3 u(s^f);
    u.normalize();
    
    osg::Matrix rotation_matrix(s[0],     u[0],     -f[0],     0.0f,
                                s[1],     u[1],     -f[1],     0.0f,
                                s[2],     u[2],     -f[2],     0.0f,
                                0.0f,     0.0f,     0.0f,      1.0f);
                   
    m_c = center;
    m_d = lv.length();
    m_q = rotation_matrix.getRotate().inverse();
}
예제 #25
0
파일: opengl.cpp 프로젝트: ammen99/wayfire
/* look up the actual values of wl_output_transform enum
 * All _flipped transforms have values (regular_transfrom + 4) */
glm::mat4 get_output_matrix_from_transform(wl_output_transform transform)
{
    glm::mat4 scale = glm::mat4(1.0);

    if (transform >= 4)
        scale = glm::scale(scale, {-1, 1, 0});

    /* remove the third bit if it's set */
    uint32_t rotation = transform & (~4);
    glm::mat4 rotation_matrix(1.0);

    if (rotation == WL_OUTPUT_TRANSFORM_90)
        rotation_matrix = glm::rotate(rotation_matrix, -WF_PI / 2.0f, {0, 0, 1});
    if (rotation == WL_OUTPUT_TRANSFORM_180)
        rotation_matrix = glm::rotate(rotation_matrix,  WF_PI,        {0, 0, 1});
    if (rotation == WL_OUTPUT_TRANSFORM_270)
        rotation_matrix = glm::rotate(rotation_matrix,  WF_PI / 2.0f, {0, 0, 1});

    return rotation_matrix * scale;
}
예제 #26
0
Eigen::MatrixXd ObjectsToCostmap::makeRectanglePoints(const autoware_msgs::DetectedObject& in_object,
                                                     const double expand_rectangle_size)
{
  double length = in_object.dimensions.x + expand_rectangle_size;
  double width = in_object.dimensions.y + expand_rectangle_size;
  Eigen::MatrixXd origin_points(NUMBER_OF_DIMENSIONS, NUMBER_OF_POINTS);
  origin_points << length / 2, length / 2, -length / 2, -length / 2, width / 2, -width / 2, -width / 2, width / 2;

  double yaw = tf::getYaw(in_object.pose.orientation);
  Eigen::MatrixXd rotation_matrix(NUMBER_OF_DIMENSIONS, NUMBER_OF_DIMENSIONS);
  rotation_matrix << std::cos(yaw), -std::sin(yaw), std::sin(yaw), std::cos(yaw);
  Eigen::MatrixXd rotated_points = rotation_matrix * origin_points;

  double dx = in_object.pose.position.x;
  double dy = in_object.pose.position.y;
  Eigen::MatrixXd transformed_points(NUMBER_OF_DIMENSIONS, NUMBER_OF_POINTS);
  Eigen::MatrixXd ones = Eigen::MatrixXd::Ones(1, NUMBER_OF_POINTS);
  transformed_points.row(0) = rotated_points.row(0) + dx * ones;
  transformed_points.row(1) = rotated_points.row(1) + dy * ones;
  return transformed_points;
}
예제 #27
0
void FPSManipulator::computePosition(const osg::Vec3& eye,const osg::Vec3& center,const osg::Vec3& up)
{

    osg::Vec3 lv(center-eye);

    osg::Vec3 f(lv);
    f.normalize();
    osg::Vec3 s(f^up);
    s.normalize();
    osg::Vec3 u(s^f);
    u.normalize();
    
    osg::Matrix rotation_matrix(s[0],     u[0],     -f[0],     0.0f,
                                s[1],     u[1],     -f[1],     0.0f,
                                s[2],     u[2],     -f[2],     0.0f,
                                0.0f,     0.0f,     0.0f,      1.0f);
                   
    _center = eye;
    //_distance = lv.length();
    _rotation = rotation_matrix.getRotate().inverse();
}
예제 #28
0
void _set_wind (const Planet& planet, const Climate_parameters&, Climate_generation_season& season) {
	for (auto& t : tiles(planet)) {
		season.tiles[id(t)].wind = _default_wind(planet, id(t), season.tropical_equator);
		season.tiles[id(t)].wind.direction += north(planet, &t);
	}
	for (auto& t : tiles(planet)) {
		//tile shape in 2d, rotated according to wind direction
		std::vector<Vector2> corners =
			rotation_matrix(north(planet, &t) - season.tiles[id(t)].wind.direction) * polygon(&t, rotation_to_default(planet));

		int e = edge_count(t);
		for (int k=0; k<e; k++) {
			int direction = sign(nth_edge(t, k), &t);
			if (corners[k].x + corners[(k+1)%e].x < 0) direction *= -1;
			season.edges[id(nth_edge(t, k))].wind_velocity -=
				0.5 * direction
				* season.tiles[id(t)].wind.speed
				* std::abs(corners[k].y - corners[(k+1)%e].y)
				/ length(corners[k] - corners[(k+1)%e]);
		}
	}
}
예제 #29
0
void Infill::generateZigZagInfill(Polygons& result, const int line_distance, const double& fill_angle, const bool connected_zigzags, const bool use_endpieces)
{

    PointMatrix rotation_matrix(fill_angle);
    if (use_endpieces)
    {
        if (connected_zigzags)
        {
            ZigzagConnectorProcessorConnectedEndPieces zigzag_processor(rotation_matrix, result);
            generateLinearBasedInfill(outline_offset - infill_line_width / 2, result, line_distance, rotation_matrix, zigzag_processor, connected_zigzags, 0);
        }
        else
        {
            ZigzagConnectorProcessorDisconnectedEndPieces zigzag_processor(rotation_matrix, result);
            generateLinearBasedInfill(outline_offset - infill_line_width / 2, result, line_distance, rotation_matrix, zigzag_processor, connected_zigzags, 0);
        }
    }
    else 
    {
        ZigzagConnectorProcessorNoEndPieces zigzag_processor(rotation_matrix, result);
        generateLinearBasedInfill(outline_offset - infill_line_width / 2, result, line_distance, rotation_matrix, zigzag_processor, connected_zigzags, 0);
    }
}
예제 #30
0
		void Manipulator::calcRotationArgs(osg::Vec3d& axis, float& angle, 
			float p1x, float p1y,float p2x, float p2y) {
			osg::Matrixd rotation_matrix(_rotation);

			osg::Vec3d uv = osg::Vec3d(0.0f,1.0f,0.0f)*rotation_matrix;
			osg::Vec3d sv = osg::Vec3d(1.0f,0.0f,0.0f)*rotation_matrix;
			osg::Vec3d lv = osg::Vec3d(0.0f,0.0f,-1.0f)*rotation_matrix;

			osg::Vec3d p1 = sv * p1x + uv * p1y - lv * tb_project_to_sphere(_trackball_size, p1x, p1y);
			osg::Vec3d p2 = sv * p2x + uv * p2y - lv * tb_project_to_sphere(_trackball_size, p2x, p2y);

			axis = p2^p1;
			axis.normalize();

			float t = (p2 - p1).length() / (2.0 * _trackball_size);

			if (t > 1.0) t = 1.0;
			if (t < -1.0) t = -1.0;
			angle = osg::inRadians(asin(t));
			//2014/4/28
			//2014/10/26,0.1
			angle = 0.02 * angle;
		}