/* 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));
}
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);
}
/// 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);
}
}
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);
}
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;
}
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
// 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);
}
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_;
}
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;
}
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;
}
void LLCoordFrame::setAxes(const LLQuaternion &q )
{
LLMatrix3 rotation_matrix(q);
setAxes(rotation_matrix);
if( !isFinite() )
{
reset();
llwarns << "Non Finite in LLCoordFrame::setAxes()" << llendl;
}
}
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;
}
}
// 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);
}
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;
}
}
/**************************************************
* 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;
}
}
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;
}
/* 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;
}
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;
}
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]);
}
/*
* 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;
}
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();
}
/* 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;
}
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;
}
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();
}
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]);
}
}
}
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);
}
}
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;
}