hesuicong
/
openMVS.build
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
1 Commit
1 Branch
0 Tags
488 MiB
 
 
 
 
 
Branch: main
Branches Tags
${ item.name }
Create tag ${ searchTerm }
Create branch ${ searchTerm }
from 'main'
${ noResults }
openMVS.build/vcglib/wrap/gui/trackmode.cpp

872 lines
25 KiB
Raw Permalink Blame History

/****************************************************************************
* VCGLib o o *
* Visual and Computer Graphics Library o o *
* _ O _ *
* Copyright(C) 2004-2016 \/)\/ *
* Visual Computing Lab /\/| *
* ISTI - Italian National Research Council | *
* \ *
* All rights reserved. *
* *
* This program is free software; you can redistribute it and/or modify *
* it under the terms of the GNU General Public License as published by *
* the Free Software Foundation; either version 2 of the License, or *
* (at your option) any later version. *
* *
* This program is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
* GNU General Public License (http://www.gnu.org/licenses/gpl.txt) *
* for more details. *
* *
****************************************************************************/
#ifdef QT_OPENGL_LIB
#include <QtOpenGL/qgl.h>
#else
#include <GL/glew.h>
#endif
#include <vcg/space/distance3.h>
#include <wrap/gui/trackmode.h>
#include <wrap/gui/trackball.h>
#include <wrap/gui/trackutils.h>
using namespace vcg;
using namespace vcg::trackutils;
// Track mode implementation, dummy.
void TrackMode::Apply (Trackball * , float ){}
void TrackMode::Apply (Trackball * , Point3f ){}
void TrackMode::Draw(Trackball * ){}
void TrackMode::SetAction (){}
void TrackMode::Reset (){}
bool TrackMode::IsAnimating(const Trackball *){
return false;
}
void TrackMode::Animate(unsigned int, Trackball *){
}
bool TrackMode::isSticky() {
return false;
}
void TrackMode::Undo(){}
// draw an inactive trackball
void InactiveMode::Draw(Trackball * tb){
DrawSphereIcon(tb,false);
}
// Sphere mode implementation.
// the most important function; given a new point in window coord,
// it update the transformation computed by the trackball.
// General scheme : the transformation is a function of just
// the begin and current mouse positions, with greater precision
// is function of just two 3d points over the manipulator.
void SphereMode::Apply (Trackball * tb, Point3f new_point)
{
Point3f hitOld = HitSphere (tb, tb->last_point);
Point3f hitNew = HitSphere (tb, new_point);
tb->Hits.push_back (hitNew);
Point3f center = tb->center;
Point3f axis = (hitNew - center) ^ (hitOld - center);
vcg::Normalize(axis);
// Figure out how much to rotate around that axis.
// float phi = Distance (hitNew, hitOld) / tb->radius;
// float phi = vcg::Angle(hitNew - center,hitOld - center)*(Distance(hitNew,center)/tb->radius);
float phi = std::max(vcg::Angle(hitNew - center,hitOld - center),(Distance(hitNew,hitOld)/tb->radius)) ;
tb->track.rot = Quaternionf (-phi, axis) * tb->last_track.rot;
}
void SphereMode::Draw(Trackball * tb){
DrawSphereIcon(tb,true );
}
// Pan mode implementation.
void PanMode::Apply (Trackball * tb, Point3f new_point)
{
Point3f hitOld = HitViewPlane (tb, tb->last_point);
Point3f hitNew = HitViewPlane (tb, new_point);
tb->Translate (hitNew - hitOld);
}
void PanMode::Draw(Trackball * tb){
DrawSphereIcon(tb,true);
DrawSphereAxis(tb);
DrawUglyPanMode(tb);
}
// Z mode implementation.
void ZMode::Apply (Trackball * tb, float WheelNotch)
{
Point3f dir= (GetViewPlane (tb->camera, tb->center)).Direction();
dir.Normalize();
tb->Translate (dir * (-WheelNotch));
}
void ZMode::Apply (Trackball * tb, Point3f new_point)
{
Point3f dir= (GetViewPlane (tb->camera, tb->center)).Direction();
dir.Normalize();
tb->Translate (dir * ( -2.0f * getDeltaY(tb,new_point)));
}
void ZMode::Draw(Trackball * tb){
DrawSphereIcon(tb,true );
DrawUglyZMode(tb);
}
// Scale mode implementation.
void ScaleMode::Apply (Trackball * tb, float WheelNotch)
{
tb->track.sca *= std::pow (1.2f, -WheelNotch);
}
void ScaleMode::Apply (Trackball * tb, Point3f new_point)
{
tb->track.sca = tb->last_track.sca * std::pow (3.0f, -(getDeltaY(tb,new_point)));
}
void ScaleMode::Draw(Trackball * tb){
DrawSphereIcon(tb,true );
DrawUglyScaleMode(tb);
}
// Axis mode implementation.
void AxisMode::Apply (Trackball * tb, float WheelNotch)
{
tb->Translate (axis.Direction () * (WheelNotch / 10.0f));
}
void AxisMode::Apply (Trackball * tb, Point3f new_point)
{
std::pair< Point3f,bool > hitOld = HitNearestPointOnAxis (tb, axis, tb->last_point);
std::pair< Point3f,bool > hitNew = HitNearestPointOnAxis (tb, axis, new_point);
if (hitOld.second && hitNew.second){
tb->Translate (hitNew.first - hitOld.first);
}
}
void AxisMode::Draw(Trackball * tb){
DrawSphereIcon(tb,true );
DrawUglyAxisMode(tb,axis);
}
// Plane mode implementation.
void PlaneMode::Apply (Trackball * tb, Point3f new_point)
{
std::pair< Point3f, bool > hitOld = HitPlane(tb,tb->last_point,plane);
std::pair< Point3f, bool > hitNew = HitPlane(tb,new_point,plane);
if(hitOld.second && hitNew.second){
tb->Translate (hitNew.first - hitOld.first);
}
}
void PlaneMode::Draw(Trackball * tb){
DrawSphereIcon(tb,true );
DrawUglyPlaneMode(tb, plane);
}
// Cylinder mode implementation.
void CylinderMode::Apply (Trackball * tb, float WheelNotch)
{
const float PI2=6.283185307179586232f;
float angle= (snap==0.0) ? WheelNotch/(tb->radius * PI2) : WheelNotch * snap;
tb->track.rot = tb->last_track.rot * Quaternionf (angle,axis.Direction());
}
void CylinderMode::Apply (Trackball * tb, Point3f new_point)
{
Plane3f viewplane=GetViewPlane (tb->camera, tb->center);
Line3f axisproj;
axisproj=ProjectLineOnPlane(axis,viewplane);
float angle;
const float EPSILON=0.005f; // this IS scale independent
if(axisproj.Direction().Norm() < EPSILON){
angle=(10.0f * getDeltaY(tb,new_point)) / tb->radius;
} else {
Point3f hitOld = HitViewPlane (tb, tb->last_point);
Point3f hitNew = HitViewPlane (tb, new_point);
axisproj.Normalize();
Point3f plusdir= viewplane.Direction() ^ axisproj.Direction();
float distOld = signedDistance(axisproj,hitOld,plusdir);
float distNew = signedDistance(axisproj,hitNew,plusdir);
angle= (distNew-distOld) / tb->radius;
}
if(snap>0.0){
angle = ((angle<0)?-1:1)* floor((((angle<0)?-angle:angle)/snap)+0.5f)*snap;
}
// tb->track.rot = tb->last_track.rot * Quaternionf (angle,axis.Direction());
tb->track.rot = Quaternionf (-angle,axis.Direction()) * tb->last_track.rot;
}
void CylinderMode::Draw(Trackball * tb){
DrawSphereIcon(tb,true );
DrawUglyCylinderMode(tb,axis);
}
// Path mode implementation.
void PathMode::Init(const std::vector < Point3f > &pts)
{
unsigned int npts = int(pts.size());
assert(npts >= 2);
points.reserve(npts);
for(unsigned int i=0;i<npts;i++){
points.push_back(pts[i]);
}
path_length=0.0f;
min_seg_length=Distance(points[0],points[1]);
float seg_length;
for(unsigned int i=1;i<npts;i++){
seg_length=Distance(points[i-1],points[i]);
path_length += seg_length;
min_seg_length = (std::min)(seg_length,min_seg_length);
}
if(wrap){
seg_length=Distance(points[npts-1],points[0]);
path_length += seg_length;
min_seg_length = (std::min)(seg_length,min_seg_length);
}
}
void PathMode::Reset()
{
current_state=initial_state;
}
Point3f PathMode::SetStartNear(Point3f point)
{
float p0_state=0;
Point3f p0,p1;
float nearest_state=0;
Point3f nearest_point=points[0];
float nearest_distance=Distance(nearest_point,point);
unsigned int npts = int(points.size());
for(unsigned int i = 1;i <= npts;i++){
if( i == npts){
if (wrap){
p0=points[npts-1];
p1=points[0];
} else {
break;
}
} else {
p0=points[i-1];
p1=points[i];
}
//Point3f segment_point=ClosestPoint(Segment3f(p0,p1),point);
Point3f segment_point;
float distance;
vcg::SegmentPointDistance<float>(Segment3f(p0,p1),point,segment_point,distance);
// float distance=Distance(segment_point,point);
if(distance<nearest_distance){
nearest_point=segment_point;
nearest_distance=distance;
nearest_state=p0_state+(Distance(p0,nearest_point)/path_length);
}
float segment_norm= Distance(p0,p1) / path_length;
p0_state+=segment_norm;
}
assert( nearest_state >= 0.0 );
if(nearest_state > 1.0){
nearest_state=1.0;
nearest_point=( wrap ? points[0] : points[npts-1] );
}
initial_state=nearest_state;
return nearest_point;
}
void PathMode::GetPoints(float state, Point3f & point, Point3f & prev_point, Point3f & next_point)
{
assert(state >= 0.0f);
assert(state <= 1.0f);
float remaining_norm=state;
Point3f p0(0,0,0),p1(0,0,0);
unsigned int npts = int(points.size());
for(unsigned int i = 1;i <= npts;i++){
if( i == npts){
if (wrap){
p0=points[npts-1];
p1=points[0];
} else {
break;
}
} else {
p0=points[i-1];
p1=points[i];
}
float segment_norm= Distance(p0,p1) / path_length;
if (segment_norm < remaining_norm){
remaining_norm -= segment_norm;
continue;
}
prev_point = p0;
next_point = p1;
float ratio= remaining_norm / segment_norm;
point = prev_point + (( next_point - prev_point ) * ratio);
const float EPSILON=min_seg_length * 0.01f;
if(Distance(point,prev_point) < EPSILON){
point=prev_point;
if (i > 1){
prev_point=points[i-2];
} else if (wrap){
prev_point=points[npts-1];
}
} else if (Distance(point,next_point) < EPSILON){
point=next_point;
if( i < (npts-1)){
next_point=points[i+1];
} else {
if (wrap){
next_point=points[1];
} else {
next_point=points[npts-1];
}
}
}
return;
}
// rounding errors can lead out of the for..
prev_point = p0;
point = p1;
if (wrap){
next_point=points[1];
}else{
next_point = points[npts-1];
}
}
void PathMode::Apply (Trackball * tb, float WheelNotch)
{
undo_current_state=current_state;
undo_old_hitpoint=old_hitpoint;
const float STEP_COEFF = min_seg_length * 0.5f;
float delta=(WheelNotch*STEP_COEFF)/path_length;
Point3f old_point,new_point,prev_point,next_point;
GetPoints(current_state,old_point,prev_point,next_point);
current_state=Normalize(current_state+delta);
GetPoints(current_state,new_point,prev_point,next_point);
tb->Translate (new_point - old_point);
}
float PathMode::Normalize(float state)
{
if ( wrap ) {
double intpart;
float fractpart;
fractpart =(float) modf(state,&intpart);
if( fractpart < 0.0f )
fractpart += 1.0f;
return fractpart;
}
if ( state < 0.0f )
return 0.0f;
if ( state > 1.0f )
return 1.0f;
return state;
}
int PathMode::Verse(Point3f reference_point,Point3f current_point,Point3f prev_point,Point3f next_point)
{
Point3f reference_dir = reference_point - current_point ;
Point3f prev_dir = prev_point - current_point ;
Point3f next_dir = next_point - current_point ;
const float EPSILON=min_seg_length * 0.005f;
if (reference_dir.Norm() < EPSILON)
reference_dir = Point3f(0,0,0);
if (prev_dir.Norm() < EPSILON)
prev_dir = Point3f(0,0,0);
if (next_dir.Norm() < EPSILON)
next_dir = Point3f(0,0,0);
reference_dir.Normalize();
prev_dir.Normalize();
next_dir.Normalize();
float prev_coeff,next_coeff;
prev_coeff = prev_dir.dot(reference_dir);
next_coeff = next_dir.dot(reference_dir);
if (prev_coeff < 0.0f)
prev_coeff = 0.0f;
if (next_coeff < 0.0f)
next_coeff = 0.0f;
if( (prev_coeff == 0.0f) && (next_coeff == 0.0f)){
return 0;
}
if ( prev_coeff <= next_coeff ){
return 1;
}
return -1;
}
float PathMode::HitPoint(float state, Ray3fN ray, Point3f &hit_point)
{
Point3f current_point, next_point, prev_point;
GetPoints(state,current_point,prev_point,next_point);
Point3f closest_point;
closest_point=ray.ClosestPoint(current_point);
int verse=Verse(closest_point,current_point,prev_point,next_point);
if (verse == 0){
hit_point=current_point;
return 0.0f;
}
Segment3f active_segment;
if (verse > 0){
active_segment=Segment3f(current_point,next_point);
} else {
active_segment= Segment3f(current_point,prev_point);
}
//hit_point=ClosestPoint(active_segment,closest_point);
float dist;
vcg::SegmentPointDistance<float>(active_segment,closest_point,hit_point,dist);
return verse * ((hit_point-current_point).Norm() / path_length);
}
void PathMode::SetAction (){
Point3f temp1,temp2;
GetPoints(current_state,old_hitpoint,temp1,temp2);
}
void PathMode::Apply (Trackball * tb, Point3f new_point)
{
undo_current_state=current_state;
undo_old_hitpoint=old_hitpoint;
Ray3fN ray = line2ray(tb->camera.ViewLineFromWindow (new_point));
Point3f hit_point;
float delta_state=HitPoint(current_state,ray,hit_point);
current_state=Normalize(current_state+delta_state);
tb->Translate (hit_point - old_hitpoint);
}
bool PathMode::isSticky() {
return true;
}
void PathMode::Undo(){
current_state=undo_current_state;
old_hitpoint=undo_old_hitpoint;
}
void PathMode::Draw(Trackball * tb){
DrawSphereIcon(tb,true );
Point3f current_point,prev_point,next_point;
GetPoints(current_state,current_point,prev_point,next_point);
DrawUglyPathMode(tb,points,current_point,prev_point,
next_point,old_hitpoint,wrap);
}
// Area mode implementation.
void AreaMode::Init(const std::vector < Point3f > &pts)
{
unsigned int npts = int(pts.size());
assert(npts >= 3);
//get the plane
Point3f p0=pts[0];
unsigned int onethird=(unsigned int)floor(npts/3.0);
const float EPSILON = 0.005f;
bool pts_not_in_line=false;
Point3f a,b;
for(unsigned int i=0;i<onethird;i++){
a=(pts[(i+ onethird )%npts] - pts[i%npts]).normalized();
b=(pts[(i+(2*onethird))%npts] - pts[i%npts]).normalized();
pts_not_in_line = (a ^ b).Norm() > EPSILON;
if(pts_not_in_line){
plane.Init( pts[i%npts],
pts[(i+(onethird))%npts],
pts[(i+(2*onethird))%npts]);
break;
}
}
assert(pts_not_in_line);
float ncx,ncy,ncz;
ncx=fabs(plane.Direction()[0]);
ncy=fabs(plane.Direction()[1]);
ncz=fabs(plane.Direction()[2]);
if(( ncx > ncy ) && ( ncx > ncz )){
first_coord_kept=1;
second_coord_kept=2;
} else if(( ncy > ncx ) && ( ncy > ncz)){
first_coord_kept=0;
second_coord_kept=2;
} else {
first_coord_kept=0;
second_coord_kept=1;
}
points.reserve(npts);
for(unsigned int i=0;i<npts;i++){
points.push_back(plane.Projection(pts[i]));
}
min_side_length=Distance(points[0],points[1]);
for(unsigned int i=1;i<npts;i++){
min_side_length=(std::min)(Distance(points[i-1],points[i]),min_side_length);
}
rubberband_handle=old_status=status=initial_status=p0;
}
void AreaMode::Reset()
{
rubberband_handle=old_status=status=initial_status;
path.clear();
}
void AreaMode::Apply (Trackball * tb, Point3f new_point)
{
undo_begin_action=begin_action;
undo_status=status;
undo_delta_mouse=delta_mouse;
undo_old_status=old_status;
undo_rubberband_handle=rubberband_handle;
undo_path_index=path.size();
if(begin_action){
delta_mouse=tb->camera.Project(status)-new_point;
begin_action=false;
}
std::pair< Point3f, bool > hitNew = HitPlane(tb,new_point+delta_mouse,plane);
if(! hitNew.second){
return;
}
Point3f hit_point=hitNew.first;
Point3f delta_status=Move(status,hit_point);
status += delta_status;
tb->Translate (status - old_status);
rubberband_handle=hit_point;
}
void AreaMode::SetAction ()
{
begin_action=true;
old_status=status;
path.clear();
path.push_back(status);
rubberband_handle=status;
}
Point3f AreaMode::Move(Point3f start,Point3f end)
{
const float EPSILON=min_side_length*0.001f;
Point3f pt=start;
bool done=false;
bool end_inside=Inside(end);
while(!done){
path.push_back(pt);
Segment3f segment(pt,end);
bool p_on_side = false;
bool hit=false;
Point3f pside(0,0,0),phit(0,0,0);
bool slide=false,mid_inside=false;
int np = int(points.size()), i, j;
for (i = 0, j = np-1; i < np; j = i++) {
Segment3f side(points[i],points[j]);
Point3f pseg,psid;
//std::pair<float,bool> res=SegmentSegmentDistance(segment,side,pseg,psid);
std::pair<float,bool> res;
vcg::SegmentSegmentDistance(segment,side,res.first,res.second,pseg,psid);
if(res.first < EPSILON && ! res.second){
float dist= Distance(pt,pseg);
if(dist < EPSILON){
//Point3f pn=ClosestPoint(side,end);
Point3f pn;
float dist;
vcg::SegmentPointDistance<float>(side,end,pn,dist);
if(!p_on_side || (Distance(pn,end)<Distance(end,pside))){
pside=pn;
p_on_side=true;
}
} else {
if (!hit || Distance(pt,pseg) < Distance(pt,phit)){
phit=pseg;
hit=true;
}
}
}
}
if (p_on_side)
slide = Distance(pside,pt) > EPSILON;
if (hit)
mid_inside = Inside( pt + ( ( phit - pt ) / 2) );
if ( !hit && end_inside ){
pt = end;
done = true;
} else if ( hit && (!p_on_side || (p_on_side && mid_inside))) {
pt = phit;
} else if ( p_on_side && slide) {
pt = pside;
} else {
done = true;
}
}
path.push_back(pt);
return pt - start;
}
// adapted from the original C code by W. Randolph Franklin
// http://www.ecse.rpi.edu/Homepages/wrf/Research/Short_Notes/pnpoly.html
bool AreaMode::Inside(Point3f point)
{
bool inside=false;
float x=point[first_coord_kept];
float y=point[second_coord_kept];
float yi, yj, xi, xj;
int i, j, np=int(points.size());
for (i = 0, j = np-1; i < np; j = i++) {
xi=points[i][first_coord_kept];
yi=points[i][second_coord_kept];
xj=points[j][first_coord_kept];
yj=points[j][second_coord_kept];
if ( ( ( (yi<=y) && (y<yj) ) || ( (yj<=y) && (y<yi) ) ) &&
( x < ( xj - xi ) * ( y - yi ) / ( yj - yi ) + xi ) )
{
inside=!inside;
}
}
return inside;
}
Point3f AreaMode::SetStartNear(Point3f point)
{
Point3f candidate=plane.Projection(point);
if (Inside(candidate)){
initial_status=candidate;
return initial_status;
}
Point3f nearest_point=initial_status;
float nearest_distance=Distance(nearest_point,candidate);
int i, j, np=int(points.size());
for (i = 0, j = np-1; i < np; j = i++) {
Segment3f side(points[i],points[j]);
//Point3f side_point=ClosestPoint(side,candidate);
//float distance=Distance(side_point,candidate);
Point3f side_point;
float distance;
vcg::SegmentPointDistance<float>(side,candidate,side_point,distance);
if( distance < nearest_distance ){
nearest_point=side_point;
nearest_distance=distance;
}
}
initial_status=nearest_point;
return initial_status;
}
bool AreaMode::isSticky() {
return true;
}
void AreaMode::Undo(){
begin_action=undo_begin_action;
status=undo_status;
delta_mouse=undo_delta_mouse;
old_status=undo_old_status;
rubberband_handle=undo_rubberband_handle;
for(size_t i=path.size() - 1; i > undo_path_index; --i)
path.pop_back();
}
void AreaMode::Draw(Trackball * tb)
{
DrawSphereIcon(tb,true );
DrawUglyAreaMode(tb,points,status,old_status,plane,path,rubberband_handle);
}
// Polar mode implementation.
void PolarMode::Apply (Trackball * tb, Point3f new_point)
{
Point3f hitOld = HitViewPlane (tb, tb->last_point);
Point3f hitNew = HitViewPlane (tb, new_point);
float dx = (hitNew.X() - hitOld.X());
float dy = (hitNew.Y() - hitOld.Y());
const float scale = float(0.5*M_PI); //sensitivity of the mouse
const float top = float(0.9*M_PI/2); //maximum top view angle
float anglex = dx/(tb->radius * scale);
float angley = -dy/(tb->radius * scale);
enda = alpha + anglex;
endb = beta + angley;
if(endb > top) endb = top;
if(endb < -top) endb = -top;
tb->track.rot = Quaternionf (endb, Point3f(1,0,0)) *
Quaternionf (enda, Point3f(0,1,0)) ;
}
void PolarMode::SetAction() {
alpha = enda;
beta = endb;
}
void PolarMode::Reset() {
alpha = beta = enda = endb = 0;
}
void PolarMode::Draw(Trackball * tb){
DrawSphereIcon(tb,true );
}
// Navigator WASD implementation
NavigatorWasdMode::NavigatorWasdMode() {
_flipH=1; _flipV=1;
SetTopSpeedsAndAcc(1,1,4);
step_height = step_length = 0;
Reset();
};
void NavigatorWasdMode::Reset() {
alpha=0;
beta=0;
current_speed.SetZero();
step_x=0.0f;
step_current = step_last = 0.0;
}
void NavigatorWasdMode::FlipH(){
_flipH*=-1;
}
void NavigatorWasdMode::FlipV(){
_flipV*=-1;
}
void NavigatorWasdMode::SetAction() {
}
bool NavigatorWasdMode::IsAnimating(const Trackball * tb){
const unsigned int MOVEMENT_KEY_MASK = (const unsigned int)(~Trackball::MODIFIER_MASK);
if (tb->current_button & MOVEMENT_KEY_MASK) return true;
if (current_speed!=Point3f(0,0,0)) return true;
if (step_current>0.0) return true;
return false;
}
void NavigatorWasdMode::Animate(unsigned int msec, Trackball * tb){
vcg::Point3f acc(0,0,0);
float sa = sin(-alpha);
float ca = cos(-alpha);
if (tb->current_button & Trackball::KEY_UP ) acc += vcg::Point3f( sa,0,ca)*(accY*_flipH);
if (tb->current_button & Trackball::KEY_DOWN ) acc -= vcg::Point3f( sa,0,ca)*(accY*_flipH);
if (tb->current_button & Trackball::KEY_LEFT ) acc -= vcg::Point3f(-ca,0,sa)*accX;
if (tb->current_button & Trackball::KEY_RIGHT ) acc += vcg::Point3f(-ca,0,sa)*accX;
if (tb->current_button & Trackball::KEY_PGUP ) acc -= vcg::Point3f( 0,1, 0)*accZ;
if (tb->current_button & Trackball::KEY_PGDOWN) acc += vcg::Point3f( 0,1, 0)*accZ;
float sec = msec/1.0f;
current_speed += acc*sec;
tb->track.tra+=current_speed*sec;
// compute step height.
Point3f current_speed_h = current_speed;
current_speed_h[1]=0;
float vel = current_speed_h.Norm();
if (vel<topSpeedH*0.05) {
// stopped: decrease step heigth to zero
step_current*=std::pow(dumping,sec);
if (step_current<step_height*0.06) { step_current=0; step_x=0.0f;}
} else {
// running: rise step heigth
vel = current_speed.Norm();
step_x += vel*sec;
float step_current_min = (float)fabs(sin( step_x*M_PI / step_length ))*step_height;
if (step_current<step_current_min) step_current=step_current_min;
}
current_speed*=std::pow(dumping,sec);
if (current_speed.Norm()<topSpeedH*0.005) current_speed.SetZero(); // full stop
tb->track.tra[1]+=step_last;
tb->track.tra[1]-=step_current;
step_last=step_current;
//tb->track.tra[1]+=0.01;
}
void NavigatorWasdMode::Apply (Trackball * tb, Point3f new_point)
{
Point3f hitOld = tb->last_point;
Point3f hitNew = new_point;
tb->last_point=new_point;
float dx = (hitNew.X() - hitOld.X());
float dy = (hitNew.Y() - hitOld.Y());
const float scale = float(150*M_PI); //sensitivity of the mouse
const float top = float(0.9f*M_PI/2); //maximum top view angle
float anglex = dx/(tb->radius * scale);
float angley = -dy/(tb->radius * scale * 0.5f);
alpha+= anglex*_flipH;
beta += angley*_flipV;
if(beta > +top) beta = +top;
if(beta < -top) beta = -top;
Point3f viewpoint = tb->track.InverseMatrix()*Point3f(0,0,0);
tb->track.tra = tb->track.rot.Inverse().Rotate(tb->track.tra + viewpoint ) ;
tb->track.rot = Quaternionf (beta , Point3f(1,0,0)) *
Quaternionf (alpha, Point3f(0,1,0)) ;
tb->track.tra = tb->track.rot.Rotate(tb->track.tra) - viewpoint ;
tb->track.tra[1]+=step_last;
tb->track.tra[1]-=step_current;
step_last=step_current;
}
void NavigatorWasdMode::SetTopSpeedsAndAcc(float hspeed, float vspeed, float acc){
// conversion to msec
hspeed /= 1000;
vspeed /= 1000;
acc /= 1000000;
accX = accY = acc;
dumping = hspeed / ( hspeed + acc );
accZ = ( vspeed / dumping ) - vspeed;
if (acc==0) {
accX = accY = hspeed;
accZ = vspeed;
dumping=0.0;
}
topSpeedH = hspeed; topSpeedV=vspeed;
}
void NavigatorWasdMode::SetStepOnWalk(float width, float height){
step_length = width;
step_height = height;
}
void NavigatorWasdMode::Apply (Trackball * tb, float WheelNotch)
{
tb->Translate(Point3f(0,topSpeedV,0)*(-WheelNotch*100));
}
bool NavigatorWasdMode::isSticky(){
return false;
}