void InterpolationHelper::fadeOut(int key){
if (bMerge){
if (mergeFactor<=1.0)
mergeFactor+=1.0 * renderer->deltaTime * 0.001 * mergeDirection;
if (mergeFactor<=0.0 ){
mergeFactor=1.0;
bFinished=true;
return;
}
//cout << "mergeFactor " << mergeFactor << endl;
}
SkeletalActor* skel=(SkeletalActor*)moveActor;
for (uint i=0;i<skel->bones.size();i++){
Matrix4f matrixOne=keyFrames[key]->boneMatrices[skel->bones[i]->name];
Matrix3f resultingRotation=getRotationMatrix(matrixOne);
Matrix3f sourceMatrix=getRotationMatrix(skel->bones[i]->transformMatrix);
resultingRotation = sourceMatrix.lerp(mergeFactor,resultingRotation);
resultingRotation =normalizeRotations(resultingRotation );
skel->bones[i]->transformMatrix.setRotation(resultingRotation);
}
}
//sample cylinder
void sampleCylinder(Point cenPoint0, Point cenPoint1, Eigen::Vector3f& direction, float r, float grid_length, MyPointCloud& mpt){
float height=sqrt(pow(cenPoint0.x-cenPoint1.x, 2)+pow(cenPoint0.y-cenPoint1.y, 2)+pow(cenPoint0.z-cenPoint1.z, 2));
int num_cir=(int)((2*PII*r)/grid_length);
int num_h=(int)(height/grid_length);
Eigen::Vector3d direction_normal;
direction_normal << direction[0], direction[1], direction[2];
direction_normal.normalize();
double angle0=acos(direction_normal.dot(Eigen::Vector3d(0,0,1)));
Eigen::Vector3d axis0=direction_normal.cross(Eigen::Vector3d(0,0,1));
axis0.normalize();
Eigen::Matrix4d matrix0;
getRotationMatrix(axis0, angle0, matrix0);
PointCloud cloud;
cloud.push_back(cenPoint0);
cloud.push_back(cenPoint1);
PointCloudPtr cloud_up(new PointCloud);
PointCloudPtr cloud_temp(new PointCloud);
Eigen::Matrix4f matrix_transform0 = matrix0.cast<float>();
pcl::copyPointCloud(cloud,*cloud_temp);
pcl::transformPointCloud (*cloud_temp, cloud, matrix_transform0);
Point firstPoint0(cloud.at(0).x,cloud.at(0).y+r,cloud.at(0).z);
Point firstPoint1(cloud.at(1).x,cloud.at(1).y+r,cloud.at(1).z);
float ang=grid_length/r;
int pos_neg_flag=1;
if(firstPoint0.z>firstPoint1.z){
pos_neg_flag=-1;
}
for(int i=0;i<num_cir;i++){
float new_x=0;
float new_y=0;
rotatePoint2ByPoint2(firstPoint0.x,firstPoint0.y,cloud.at(0).x,cloud.at(0).y,ang*i,&new_x,&new_y);
for(int j=0;j<num_h;j++){
cloud_up->push_back(Point(new_x,new_y,cloud.at(0).z+pos_neg_flag*(j+1)*grid_length));
}
}
Eigen::Matrix4d matrix1;
getRotationMatrix(axis0, -angle0, matrix1);
Eigen::Matrix4f matrix_transform1 = matrix1.cast<float>();
pcl::copyPointCloud(*cloud_up,*cloud_temp);
pcl::transformPointCloud (*cloud_temp, *cloud_up, matrix_transform1);
PointCloud2MyPointCloud(cloud_up, mpt);
}
void InterpolationHelper::interpolateActor(){
Matrix4f transformOne, transformTwo;
Matrix3f rotationOne, rotationTwo;
Matrix3f resultingRotation;
Matrix4f resultingTransform;
currentTime=renderer->currentTime-startTime;
float relativeTime=currentTime/moveTime;
transformOne=baseTransform;
transformTwo=targetActor->transformMatrix;
if (currentTime>moveTime){
resultingTransform=transformOne.lerp(1.0,transformTwo); //calculate resulting position
moveActor->transformMatrix=resultingTransform;
bFinished=true;
return;
}
float x=relativeTime*2.0f;
float y=0.0f;
if (x< 1)
y=x*x;
else
y=2-( (x-2) * (x-2) );
y=y*0.5;
if (bLinear)
y=relativeTime;
rotationOne=getRotationMatrix(transformOne);
rotationTwo=getRotationMatrix(transformTwo);
resultingRotation=rotationOne.lerp(y,rotationTwo);
//interpolate between them
resultingTransform=transformOne.lerp(y,transformTwo); //calculate resulting position
//normalize rotations!
resultingRotation=normalizeRotations(resultingRotation);
resultingTransform.setRotation(resultingRotation);
moveActor->transformMatrix=resultingTransform;
}
//get Rect For PlaneCloud
void getRectForPlaneCloud(PointCloudPtr_RGB plane_cloud, pcl::ModelCoefficients::Ptr plane_coefficients, PointCloudPtr rect_cloud){
PointCloudPtr_RGB cloud_in_plane(new PointCloud_RGB);
PointCloudPtr_RGB plane_cloud_tem(new PointCloud_RGB);
pcl::copyPointCloud(*plane_cloud,*plane_cloud_tem);
Point_RGB pr;
pr.x=0;
pr.y=0;
pr.z=(-plane_coefficients->values[3])/plane_coefficients->values[2];
plane_cloud_tem->push_back(pr);
Eigen::Vector3d plane_normal;
plane_normal << plane_coefficients->values[0], plane_coefficients->values[1], plane_coefficients->values[2];
plane_normal.normalize();
double angle=acos(plane_normal.dot(Eigen::Vector3d(0,0,1)));
Eigen::Vector3d axis=plane_normal.cross(Eigen::Vector3d(0,0,1));
axis.normalize();
Eigen::Matrix4d matrix;
getRotationMatrix(axis, angle, matrix);
Eigen::Matrix4f matrix_transform = matrix.cast<float>();
pcl::transformPointCloud (*plane_cloud_tem, *cloud_in_plane, matrix_transform);
Point_RGB new_pr=cloud_in_plane->at(cloud_in_plane->size()-1);
pointCloud_RGBPopUp(cloud_in_plane);
cv::Point2f p0;
cv::Point2f p1;
cv::Point2f p2;
cv::Point2f p3;
std::cout<<"cloud_in_plane->size:"<< cloud_in_plane->size() <<std::endl;
find_min_rect(cloud_in_plane, p0,p1,p2,p3);
PointCloudPtr points(new PointCloud());
points->push_back(Point(p0.x,p0.y,new_pr.z));
points->push_back(Point(p1.x,p1.y,new_pr.z));
points->push_back(Point(p2.x,p2.y,new_pr.z));
points->push_back(Point(p3.x,p3.y,new_pr.z));
Eigen::Matrix4d matrix_reverse;
getRotationMatrix(axis, -angle, matrix_reverse);
Eigen::Matrix4f matrix_transform_reverse = matrix_reverse.cast<float>();
pcl::transformPointCloud (*points, *rect_cloud, matrix_transform_reverse);
}
void Node::addRotation(float angle, glm::vec3 axis)
{
glm::mat4 newRotationMatrix = glm::rotate(getRotationMatrix(), angle, axis);
m_rotationMatrix = newRotationMatrix;
updateModelMatrix(m_rotationMatrix);
}
void calculateAccMagOrientation(void)
{
if (getRotationMatrix(rotationMatrix, NULL, accel, magnetom)) {
getOrientation(accMagOrientation, rotationMatrix);
}
else {
printf("NO CALCULO! ARGH!\r\n");
}
}
glm::mat4 DisplayObject::getLocalTransformation ()
{
if (_translationIsDirty || _rotationIsDirty || _scalingIsDirty)
{
_transformMatrixCache = getTranslationMatrix() * getRotationMatrix() * getScalingMatrix();
}
return _transformMatrixCache;
}
const glm::mat4& CTransformer::getModelMatrix() const
{
if (m_model.m_matrixDirty)
{
m_model.m_matrix = getTranslationMatrix() * getRotationMatrix() * getScaleMatrix();
m_model.m_matrixDirty = false;
}
return m_model.m_matrix;
}
CommandTransformLinear::CommandTransformLinear(TransformType type, Editor::Axis axis, Editor::RotationPivot pivot, float delta, const QSet<int> &selection, ldraw::model *model)
: CommandTransform(selection, model, pivot)
{
axis_ = axis;
type_ = type;
delta_ = delta;
/* no use for translating 'position' */
if (type == Position)
pivot_ = Editor::PivotEach;
if (type == Rotation)
postmult_ = getRotationMatrix(axis_, delta);
else
premult_ = getPositionMatrix(axis_, delta);
}
uint32_t rSimpleMesh::getMatrix( rMat4f **_mat, rObjectBase::MATRIX_TYPES _type ) {
switch ( _type ) {
case SCALE: *_mat = getScaleMatrix(); return 0;
case ROTATION: *_mat = getRotationMatrix(); return 0;
case TRANSLATION: *_mat = getTranslationMatrix(); return 0;
case CAMERA_MATRIX: *_mat = getViewProjectionMatrix(); return 0;
case MODEL_MATRIX: *_mat = getModelMatrix(); return 0;
case VIEW_MATRIX: *_mat = getViewMatrix(); return 0;
case PROJECTION_MATRIX: *_mat = getProjectionMatrix(); return 0;
case MODEL_VIEW_MATRIX: *_mat = getModelViewMatrix(); return 0;
case MODEL_VIEW_PROJECTION: *_mat = getModelViewProjectionMatrix(); return 0;
case NORMAL_MATRIX: break;
}
return INDEX_OUT_OF_RANGE;
}
void testApp::changePosition(const ofMatrix4x4 &changeMatrix,ofVec3f* finalPoints,ofVec3f* finalnorml)
{
ofMatrix3x3 cnormalMatrix = getRotationMatrix(changeMatrix);
for(int y = 0; y < KINECT_HEIGHT; y ++) {
for(int x = 0; x < KINECT_WIDTH; x ++) {
if(finalPoints[y*KINECT_WIDTH+x].z > 0) {
ofVec3f point = finalPoints[y*KINECT_WIDTH+x];
point = changeMatrix * point;
ofVec3f pnormal = finalnorml[y*KINECT_WIDTH+x];
pnormal = nmmul(cnormalMatrix,pnormal);
finalPoints[y*KINECT_WIDTH+x] = point;
finalnorml[y*KINECT_WIDTH+x] = pnormal;
}
}
}
}
//ALEX OWEN 10/04/15
XMMATRIX GameObject::getWorldMatrix()
{
if(!transformed) return XMLoadFloat4x4(&worldMatrix);
// create matrices to create a single world matrix for the GameObject's transform
XMMATRIX scaleMatrix = XMMatrixScaling( scale.x, scale.y, scale.z );
XMMATRIX rotationMatrix = getRotationMatrix();
XMMATRIX positionMatrix = getPositionMatrix();
// 1) scale
// 2) rotate
// 3) position
XMMATRIX _worldMatrix = XMMatrixTranspose( scaleMatrix * rotationMatrix * positionMatrix );
XMStoreFloat4x4(&worldMatrix,_worldMatrix);
transformed = false;
return _worldMatrix;
}
//ALEX OWEN - 30/03/15
void GameObject::updateVectors()
{
XMMATRIX rotationMatrix = getRotationMatrix();
FXMVECTOR _forwardVector = XMVector3Transform(FORWARD,rotationMatrix);
FXMVECTOR _rightVector = XMVector3Transform(RIGHT,rotationMatrix);
FXMVECTOR _upVector = XMVector3Transform(UP,rotationMatrix);
XMFLOAT3* _temp = &forwardVector;
XMStoreFloat3(_temp,_forwardVector);
_temp = &rightVector;
XMStoreFloat3(_temp,_rightVector);
_temp = &upVector;
XMStoreFloat3(_temp,_upVector);
updated = false;
//forwardVector = XMStoreFloat4(_forwardVector)
//forwardVector.x = XMVectorGetX(
}
bool CommandTransformLinear::mergeWith(const QUndoCommand *command)
{
if (command->id() != id())
return false;
const CommandTransformLinear *ctl = dynamic_cast<const CommandTransformLinear *>(command);
if (selection_ != ctl->selection())
return false;
delta_ += ctl->delta();
if (type_ == Position)
premult_ = getPositionMatrix(axis_, delta_);
else
postmult_ = getRotationMatrix(axis_, delta_);
return true;
}
glm::vec2 Camera::untransformPoint(const glm::vec2 point) const
{
return (1.0f/getZoom()) * (glm::inverse(getRotationMatrix()) * (point)) + getPosition();
}
glm::vec2 Camera::transformPoint(const glm::vec2 point) const
{
return getRotationMatrix() * (getZoom() * (point - getPosition()));
}
int main(){
initAndLoad(cv::Mat::eye(4,4,CV_32F), cv::Mat::eye(4,4,CV_32F), &vidRecord, &wcSkeletons, "map000000_aoto4_edit/video/", true);
zeroWorldCoordinateSkeletons(cv::Mat::eye(4,4,CV_32F), &vidRecord, &wcSkeletons);
setCameraMatrixTexture(KINECT::loadCameraParameters());
setCameraMatrixScene(KINECT::loadCameraParameters());
cylinderBody.Load("map000000-custCB/");
cv::namedWindow("rgb");
cv::namedWindow("3d", CV_GUI_NORMAL);
cv::setMouseCallback("3d", onMouse);
frame=0;
angle_y = 0;
angle_x = 0;
trans = cv::Mat::eye(4,4,CV_32F);
calculateSkeletonOffsetPoints(vidRecord, wcSkeletons, cylinderBody);
boundingBoxLerp = cv::Rect (0,0,WIDTH,HEIGHT);
lc = generateLerpCorners(boundingBoxLerp);
while(true){
cv::Mat tex_ = uncrop(vidRecord[frame].videoFrame);
cv::Mat tex(tex_.size(), CV_8UC4, cv::Scalar(255,255,255,255));
int from_to[] = { 0,0, 1,1, 2,2};
cv::mixChannels(&tex_,1,&tex,1,from_to,3);
cv::Mat _3d(HEIGHT, WIDTH, CV_8UC4, cv::Scalar(0,0,0,255));
cv::Vec3f sCenter = calcSkeleCenter(vidRecord[frame].kinectPoints);
trans = getTranslationMatrix(sCenter) * mat3_to_mat4(getRotationMatrix(cv::Vec3f(0,1,0), angle_y)) * mat3_to_mat4(getRotationMatrix(cv::Vec3f(1,0,0), angle_x)) * getTranslationMatrix(-sCenter);
ghostdraw_parallel(frame, cv::Mat::eye(4,4,CV_32F), vidRecord, wcSkeletons, cylinderBody, Limbrary(), tex, cv::Mat(), GD_CYL);
ghostdraw_parallel(frame, trans, vidRecord, wcSkeletons, cylinderBody, Limbrary(), _3d, cv::Mat(), GD_CYL);
cv::Scalar goodColor(50, 200, 250);
cv::Scalar badColor(20, 20, 250);
for(int joint=0;joint<NUMJOINTS;++joint)
{
cv::Scalar color;
if (wcSkeletons[frame].states[joint] < 1)
color = badColor;
else
color = goodColor;
{
cv::Vec3f jv = mat_to_vec3(wcSkeletons[frame].points.col(joint));
cv::Vec2f jv2 = mat4_to_vec2(getCameraMatrixScene() * vec3_to_mat4(jv));
cv::Point pj(jv2(0), jv2(1));
cv::circle(tex,pj,2,color,-1);
}
{
cv::Vec3f jv = mat_to_vec3(trans * wcSkeletons[frame].points.col(joint));
cv::Vec2f jv2 = mat4_to_vec2(getCameraMatrixScene() * vec3_to_mat4(jv));
cv::Point pj(jv2(0), jv2(1));
cv::circle(_3d,pj,3,color,-1);
}
}
std::stringstream frameSS;
frameSS << frame;
cv::putText(tex, frameSS.str(), cv::Point(30, 30), CV_FONT_HERSHEY_PLAIN, 2, cv::Scalar(0, 255, 0), 3);
cv::putText(_3d, frameSS.str(), cv::Point(30, 30), CV_FONT_HERSHEY_PLAIN, 2, cv::Scalar(255, 0, 255), 3);
cv::imshow("rgb", tex);
cv::imshow("3d", _3d);
char in = cv::waitKey(10);
switch(in){
case 'q':
return 0;
case 'z':
--frame;
while(vidRecord[frame].videoFrame.mat.empty()){
--frame;
}
if(frame < 0) frame = 0;
break;
case 'x':
++frame;
if (frame >= vidRecord.size()) frame = vidRecord.size() - 1;
while(vidRecord[frame].videoFrame.mat.empty()){
++frame;
if (frame >= vidRecord.size()) frame = vidRecord.size() - 1;
}
break;
case 'd':
if (lastjoint != -1){
if (wcSkeletons[frame].states[lastjoint] < 1){
wcSkeletons[frame].states[lastjoint] = 1;
}
else{
wcSkeletons[frame].states[lastjoint] = 0.5;
}
}
case 'r':
angle_y = 0;
angle_x = 0;
break;
case 'c':
alljoints = !alljoints;
break;
case 's':
for(int i=0;i<vidRecord.size();++i){
vidRecord[i].kinectPoints = wcSkeletons[i];
//if(!vidRecord[i].cam2World.empty())
//{
// vidRecord[i].kinectPoints.points = vidRecord[i].cam2World.inv() * vidRecord[i].kinectPoints.points;
//}
}
SaveVideo(&vidRecord, getCameraMatrixScene(), "map000000_aoto4_edit/video/");
break;
}
}
}
int main(){
std::ofstream of;
of.open("benchmark/score_out.txt");
std::cerr.rdbuf(of.rdbuf());
cv::Mat K2P = cv::Mat::eye(4,4,CV_32F);
#if PTAMM_STYLE == 1
K2P = getScaleMatrix(-1,-1,1);
#endif
initAndLoad(cv::Mat::eye(4,4,CV_32F),K2P,&vidRecord,&wcSkeletons,"map000000_whitesweater_edit/video/",false);
#if PTAMM_STYLE == 1
LoadWorldCoordinateSkeletons(wcSkeletons, "map000000_whitesweater/GhostGame.xml");
#else
zeroWorldCoordinateSkeletons(cv::Mat::eye(4,4,CV_32F),&vidRecord,&wcSkeletons);
#endif
cv::Vec3f skeleCenter = calcSkeleCenter(wcSkeletons[0]);
//cv::Mat shittyTransformMatrix = /*getTranslationMatrix(cv::Vec3f(0,0,1000)); */ getRotationMatrix4(cv::Vec3f(1,0,1), CV_PI);
//cv::FileStorage fs("ptammat.yml", cv::FileStorage::READ);
//fs["matCfW"] >> shittyTransformMatrix;
//for(int i=0;i<wcSkeletons.size();++i){
// wcSkeletons[i].points = shittyTransformMatrix * wcSkeletons[i].points;
//}
cylinderBody.Load("map000000-custCB/");
#if PTAMM_STYLE == 1
cylinderBody.radiusModifier = 0.658017;
#endif
calculateSkeletonOffsetPoints(vidRecord,wcSkeletons,cylinderBody);
//limbrary.Load("map000000_whitesweater-custCB-clust/");
limbrary.Load("map000000_whitesweater_edit-custCB-clean-test/");
//LoadStatusRecord("map000000_whitesweater/estim/", estimRecord);
//std::vector<Skeleton> wcSkeletonsEstim = wcSkeletons;
//interpolate(estimRecord, wcSkeletonsEstim);
cv::Vec3f translation(0,0,0);
cv::Vec3f rotation(0,CV_PI/2,0);
cv::Mat im(480, 640, CV_8UC4, cv::Scalar(255,255,255,0));
int frame = 100;
int r = 8;
int maxr = 16-4;
rotation(1) = CV_PI/2 - r * (CV_PI/16);
int p = 8;
rotation(0) = CV_PI/2 - (CV_PI/16) * p;
rotation(2) = 0;
//smoothKinectPoints(wcSkeletons, 0.5);
bool play = true;
while(true){
{
unsigned char options = GD_DRAW ;// | GD_NOWEIGHT | GD_NOLIMBRARY;
cv::Mat draw = im.clone();
cv::Mat zBuf;
cv::Mat transform = getTranslationMatrix(translation) * getTranslationMatrix(skeleCenter) *
mat3_to_mat4(getRotationMatrix(cv::Vec3f(1,0,0),rotation(0)) * getRotationMatrix(cv::Vec3f(0,1,0),rotation(1)) * getRotationMatrix(cv::Vec3f(0,0,1),rotation(2)))
* getTranslationMatrix(-skeleCenter);
#if PTAMM_STYLE == 1
cv::FileStorage fs("ptammat.yml", cv::FileStorage::READ);
fs["matCfW"] >> transform;
transform = getTranslationMatrix(translation) * getTranslationMatrix(skeleCenter) *
mat3_to_mat4(getRotationMatrix(cv::Vec3f(1,0,0),rotation(0)) * getRotationMatrix(cv::Vec3f(0,1,0),rotation(1)) * getRotationMatrix(cv::Vec3f(0,0,1),rotation(2)))
* getTranslationMatrix(-skeleCenter) * transform;
#endif
ScoreList scoreList[NUMLIMBS];
ghostdraw_parallel(frame, K2P.inv() * transform /* shittyTransformMatrix.inv()*/, vidRecord, wcSkeletons, cylinderBody, limbrary, draw, zBuf, options, scoreList);
/*
//hybrid
cv::Mat draw2 = im.clone();
options = GD_CYL ;
ghostdraw_parallel(frame, transform, vidRecord, wcSkeletons, cylinderBody, limbrary, draw2, options);
cv::Mat candidate = vidRecord[scoreList[0].front().first].videoFrame.mat;
cv::Mat cand_resize(HEIGHT, WIDTH, CV_8UC3, cv::Scalar(200,200,200));
int gap_y = (HEIGHT-candidate.rows)/2;
int gap_x = (HEIGHT-candidate.cols)/2;
//candidate.copyTo(cand_resize(cv::Range(gap_y,gap_y + candidate.rows), cv::Range(gap_x,gap_x+candidate.cols)));
candidate.copyTo(draw(cv::Range(0,candidate.rows), cv::Range(0,candidate.cols)));
cv::rectangle(draw,cv::Rect(0,0,candidate.cols,candidate.rows),cv::Scalar(200,150,100));
cv::Mat combine(HEIGHT*2,WIDTH,CV_8UC3);
draw2.copyTo(combine(cv::Range(0,draw2.rows),cv::Range(0,draw2.cols)));
draw.copyTo(combine(cv::Range(draw2.rows,draw.rows+draw2.rows),cv::Range(0,draw.cols)));
//cand_resize.copyTo(combine(cv::Range(draw.rows+draw2.rows,draw.rows+draw2.rows+cand_resize.rows),cv::Range(0,cand_resize.cols)));*/
//hybrid
//cv::Mat draw2 = im.clone();
//
//ghostdraw_parallel(frame, transform, vidRecord, wcSkeletonsEstim, cylinderBody, limbrary, draw2, zBuf, options);
//
//cv::Mat combine(HEIGHT*2,WIDTH,CV_8UC4);
//
//draw2.copyTo(combine(cv::Range(0,draw2.rows),cv::Range(0,draw2.cols)));
//draw.copyTo(combine(cv::Range(draw2.rows,draw.rows+draw2.rows),cv::Range(0,draw.cols)));
cv::imshow("pic", draw);
cv::Mat zBufNorm;
cv::normalize(zBuf, zBufNorm, 0, 255, cv::NORM_MINMAX);
cv::Mat zBuf8(zBuf.rows, zBuf.cols, CV_8U);
for(int i=0;i<zBuf.rows*zBuf.cols;++i){
zBuf8.ptr<unsigned char>()[i] = 255-zBufNorm.ptr<unsigned short>()[i];
}
cv::imshow("depth", zBuf8);
std::string dir = "out_ws_interpcmp/";
CreateDirectoryA(dir.c_str(), NULL);
std::stringstream ss;
ss << dir <<
"frame" << std::setfill('0') << std::setw(3) << frame <<
"r" << std::setw(2) << r <<
"p" << p << ".png";
//cv::imwrite(ss.str(), draw);
if(play)
{
if(++frame >= vidRecord.size()){
frame = 100;
++r;
rotation(1) = CV_PI/2 - r * (CV_PI/16);
if(r > maxr)
{
return 0;
}
}
}
}
char q = cv::waitKey(10);
if(q=='q') return 0;
else if(q=='w'){
translation(2) -= 1;
}else if(q=='s'){
translation(2) += 1;
}else if(q=='a'){
rotation(1) += CV_PI/16;
}else if(q=='d'){
rotation(1) -= CV_PI/16;
}else if(q=='z'){
rotation (2) += CV_PI/64;
}else if(q=='x'){
rotation (2) -= CV_PI/64;
}else if(q=='p'){
play = !play;
}
}
}
int main(void)
{
//Set the error callback
glfwSetErrorCallback(error_callback);
//Initialize GLFW
if (!glfwInit())
{
exit(EXIT_FAILURE);
}
//Set the GLFW window creation hints - these are optional
//glfwWindowHint(GLFW_CONTEXT_VERSION_MAJOR, 3); //Request a specific OpenGL version
//glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 3); //Request a specific OpenGL version
//glfwWindowHint(GLFW_SAMPLES, 4); //Request 4x antialiasing
//glfwWindowHint(GLFW_OPENGL_PROFILE, GLFW_OPENGL_CORE_PROFILE);
//Create a window and create its OpenGL context
window = glfwCreateWindow(960, 720, "Test Window", NULL, NULL);
//If the window couldn't be created
if (!window)
{
fprintf(stderr, "Failed to open GLFW window.\n");
glfwTerminate();
//exit(EXIT_FAILURE);
}
//This function makes the context of the specified window current on the calling thread.
glfwMakeContextCurrent(window);
//Sets the key callback
glfwSetKeyCallback(window, key_callback);
//Initialize GLEW
GLenum err = glewInit();
//If GLEW hasn't initialized
if (err != GLEW_OK)
{
fprintf(stderr, "Error: %s\n", glewGetErrorString(err));
return -1;
}
//Set a background color
glClearColor(0.0f, 0.0f, 0.0f, 0.0f);
glfwSetCursorPos(window, 1024 / 2, 768 / 2);
GLuint VertexArrayID;
glGenVertexArrays(1, &VertexArrayID);
glBindVertexArray(VertexArrayID);
// Create and compile our GLSL program from the shaders
GLuint red = LoadShaders("SimpleTransform.vertexshader", "SingleColorRed.fragmentshader");
GLuint grid = LoadShaders("SimpleTransform.vertexshader", "SingleColorGrid.fragmentshader");
glBindFragDataLocation(red, 0, "red");
glBindFragDataLocation(grid, 1, "grid");
// Get a handle for our "MVP" uniform
GLuint MatrixID = glGetUniformLocation(red, "MVP");
// Projection matrix : 45° Field of View, 4:3 ratio, display range : 0.1 unit <-> 100 units
glm::mat4 Projection = glm::perspective(45.0f, 4.0f / 3.0f, 0.1f, 1000.0f);
// Or, for an ortho camera :
//glm::mat4 Projection = glm::ortho(-10.0f,10.0f,-10.0f,10.0f,0.0f,100.0f); // In world coordinates
// Camera matrix
glm::mat4 View = glm::lookAt(
glm::vec3(4, 3, 3), // Camera is at (4,3,3), in World Space
glm::vec3(0, 0, 0), // and looks at the origin
glm::vec3(0, 1, 0) // Head is up (set to 0,-1,0 to look upside-down)
);
static const GLfloat g_vertex_buffer_data[] = {
-1.0f, -1.0f, 0.0f,
1.0f, -1.0f, 0.0f,
0.0f, 1.0f, 0.0f,
};
static const GLushort g_element_buffer_data[] = { 0, 1, 2 };
GLuint vertexbuffer;
glGenBuffers(1, &vertexbuffer);
glBindBuffer(GL_ARRAY_BUFFER, vertexbuffer);
glBufferData(GL_ARRAY_BUFFER, sizeof(g_vertex_buffer_data), g_vertex_buffer_data, GL_STATIC_DRAW);
static const GLfloat g_triangle_buffer_data[] = {
-1.0f, -1.0f, -1.0f,
1.0f, -1.0f, -1.0f,
0.0f, 1.0f, -1.0f,
};
GLuint triangle;
glGenBuffers(1, &triangle);
glBindBuffer(GL_ARRAY_BUFFER, triangle);
glBufferData(GL_ARRAY_BUFFER, sizeof(g_triangle_buffer_data), g_triangle_buffer_data, GL_STATIC_DRAW);
// Enable depth test
glEnable(GL_DEPTH_TEST);
// Accept fragment if it closer to the camera than the former one
glDepthFunc(GL_LESS);
glEnable(GL_CULL_FACE);
glEnable(GL_LIGHTING);
glEnable(GL_SMOOTH);//OPENGL INSTANTIATION
HRESULT hr;
NUI_IMAGE_FRAME depthFrame;
HANDLE hDepth;
INuiSensor* pNuiSensor = NULL;
int iSensorCount = 0;
hr = NuiGetSensorCount(&iSensorCount);
if (FAILED(hr))
return hr;
for (int i = 0; i < iSensorCount; i++)
{
INuiSensor* tempSensor;
hr = NuiCreateSensorByIndex(i, &tempSensor);
if (FAILED(hr))
continue;
hr = tempSensor->NuiStatus();
if (S_OK == hr)
{
pNuiSensor = tempSensor;
break;
}
tempSensor->Release();
}
for (int i = 0; i < 2048; i++) {
depthLookUp[i] = rawDepthToMeters(i);
}
rotation = getRotationMatrix(theta, psi, fi);
pNuiSensor->NuiInitialize(NUI_INITIALIZE_FLAG_USES_DEPTH);
pNuiSensor->NuiImageStreamOpen(
NUI_IMAGE_TYPE_DEPTH,
NUI_IMAGE_RESOLUTION_320x240,
0,
2,
NULL,
&hDepth);//KINECT INSTANTIATION
cout << "Starting Main Loop";
static double lastTime = glfwGetTime();
//Main Loop
do
{
double currentTime = glfwGetTime();
float deltaTime = float(currentTime - lastTime);
//Clear color buffer
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glUseProgram(grid);
modelMatrix(MatrixID);
hr = pNuiSensor->NuiImageStreamGetNextFrame(hDepth, 0, &depthFrame);
if (!FAILED(hr))
{
INuiFrameTexture* pTexture;
NUI_LOCKED_RECT LockedRect;
hr = pNuiSensor->NuiImageFrameGetDepthImagePixelFrameTexture(
hDepth, &depthFrame, false, &pTexture);
if (FAILED(hr))
{
pNuiSensor->NuiImageStreamReleaseFrame(hDepth, &depthFrame);
continue;
}
pTexture->LockRect(0, &LockedRect, NULL, 0);//Kinect Image Grab
int skipX = 1;
int skipY = 1;
float scalar = 4.0f;
if (LockedRect.Pitch != 0)
{
for (int x = 0; x < width; x += skipX)
{
for (int y = 0; y < height; y += skipY)
{
const NUI_DEPTH_IMAGE_PIXEL * pBufferRun = reinterpret_cast<const NUI_DEPTH_IMAGE_PIXEL *>(LockedRect.pBits) + x + y * width;
//float depth = (float)(pBufferRun->depth);
//glm::vec3 location = realWorld(depth, height - y, x, 500.0f, 1000.0f);
//createCube(0.006f, location);
Vector4 locationDepth = NuiTransformDepthImageToSkeleton(x, y, (short)(pBufferRun->depth << 3));
glm::vec3 locationDepthxyz = glm::vec3(locationDepth.x * scalar, locationDepth.y * scalar, locationDepth.z * scalar);
createCube(0.009f, locationDepthxyz);
}
}
}
pTexture->UnlockRect(0);
pTexture->Release();
pNuiSensor->NuiImageStreamReleaseFrame(hDepth, &depthFrame);
}
createGrid();
//Test drawings
/*
glUseProgram(red);
modelMatrix(MatrixID);
//createCube(0.05f, glm::vec3(1.0f,1.0f,1.0f));
// 1rst attribute buffer : vertices
glEnableVertexAttribArray(0);
//createObject(vertexbuffer, GL_TRIANGLES, 3);
//createObject(triangle, GL_TRIANGLES, 3);
glDisableVertexAttribArray(0);
*/
//Swap buffers
glfwSwapBuffers(window);
//Get and organize events, like keyboard and mouse input, window resizing, etc...
glfwPollEvents();
std::string title = "Title | DELTA TIME " + std::to_string(1.0f/deltaTime);
const char* pszConstString = title.c_str();
glfwSetWindowTitle(window, pszConstString);
lastTime = currentTime;
} //Check if the ESC key had been pressed or if the window had been closed
while (!glfwWindowShouldClose(window));
//Close OpenGL window and terminate GLFW
glfwDestroyWindow(window);
//Finalize and clean up GLFW
glfwTerminate();
exit(EXIT_SUCCESS);
}
//for any one-time only initialization of the
// virtual world before any rendering takes place
// BUT after OpenGL has been initialized
void init()
{
glEnable(GL_LIGHTING);
/*
points = {
{{ -4.0f, -5.0f }}, {{ -4.0f, 5.0f }},
{{ 0.0f, 7.0f }}, {{ 4.0f, 5.0f }},
{{ 4.0f, -5.0f }}, {{ 0.0f, -7.0f }}
};
*/
points3d = {
{{ -4.0f, -5.0f,5.0f }},
{{ 4.0f, 5.0f,5.0f }},
{{ 8.0f, 7.0f,5.0f }},
{{ 12.0f, 5.0f,5.0f }},
{{ 16.0f, -5.0f,5.0f }},
{{ 20.0f, -7.0f,5.0f }}
};
auto circle = getCircle(2, 10);
for (auto &v : circle) points.push_back({{ v[0], v[2] }});
//spring:
points3d = generateSpline(-50, 50, 150,
[](float z)->float { return sin(z/2.0) * 15; },
[](float x)->float { return cos(x/2.0) * 15; },
[](float y)->float { return y; });
// heart:
/*
points3d = generateSpline(-50, 50, 150,
[](float z)->float {
float t = z/5.0;
return 16 * sin(t) * sin(t) * sin(t);
},
[](float x)->float {
float t = x/5.0;
return 13 * cos(t) - 5*cos(2*t) - 2*cos(3*t) - cos(4*t);
});
*/
extrude = new Extrusion(points);
extrude->setDepth(8);
loft = new Loft(points, points3d);
//Low-polygons dragon (5835 triangles)
mymodelloader.load("data/model_lowpolygonstanforddragon.txt",100);
deer = new Mesh("data/deer.obj");
deer->setFlatColor({{.8, .2, .8}});
deer->setTranslateX(10.5f);
deer->setScale(0.5f);
elephant = new Mesh("data/elephant-triangulated.obj");
elephant->setFlatColor({{ .8, .1, .15 }});
elephant->setTranslateX(-10.5f);
elephant->setRotateY(-45.0f);
pts = getCircle(8, 7);
vec3 startNormal = {{ 0, 1, 0 }};
vec3 targetNormal = {{ 0.3333, 0.3333, 0.3333 }};
mat3 rotationMatrix = getRotationMatrix(startNormal, targetNormal);
for (auto &p : pts) {
ptsTransformed.push_back(mult(rotationMatrix, p));
}
//Try this:
//High-polygons dragon (original model of Stanford Dragon)
// (871414 triangles) will take some minutes for it to get loaded
//mymodelloader.load("data/model_highpolygonstanforddragon.txt",100);
//mymodelloader.load("data/model_rose.txt", 0.2);
//mymodelloader.load("data/model_shuttle.txt", 0.1);
timeold = glutGet(GLUT_ELAPSED_TIME);
}
void SpelchkCamera::calculateTranslationVector() {
// calculate displacement based on current angles (note rotations done in reverse order and negative to move model in opposite direction)
vec4 calculateDisplacement = getRotationMatrix() * vec4( -_xDepth, -_yDepth, -_zDepth, 0.0 );
_translationVector = (_oldTranslationVector + calculateDisplacement);
}
void InterpolationHelper::interpolateMatrix(){
if (bMerge){
if (mergeFactor<=1.0)
mergeFactor+=2.0 * renderer->deltaTime * 0.001 * mergeDirection;
if (mergeFactor>1.0 ){
mergeFactor=1.0;
}
if (mergeFactor<0.0 ){
mergeFactor=1.0;
bFinishedMatrix=true;
return;
}
}
Matrix4f matrixOne;
Matrix4f matrixTwo;
Matrix4f resultingMatrix;
Matrix3f rotationOne;
Matrix3f rotationTwo;
Matrix3f resultingRotation;
double timeKeyOne;
double timeKeyTwo;
currentTime=(renderer->currentTime-startTime) + inPoint;
//currentTime=currentTime*100.0;
currentTime=(int)currentTime/100;
currentTime=currentTime*100;
currentTime*=timeScale;
//currentTime=startTime+1;
//currentTime++;
//move forward if we are at end of key
while (currentTime> keyFrames[currentKeyMatrix+1]->timeKey ){
currentKeyMatrix++;
if ((uint)currentKeyMatrix+1 >= keyFrames.size()){
if (bLooping){
currentKeyMatrix=0;
startTime=renderer->currentTime;
currentTime=0.0;
//cout << "looped keyframes size: " << keyFrames.size() << endl;
//return;
}else if (bMerge){
if (mergeDirection > 0.0){
mergeDirection=-1.0 * mergeDirection;
mergeFactor=1.0f;
}
currentKeyMatrix=keyFrames.size()-2;
fadeOut(keyFrames.size()-1);
return;
}else{
bFinishedMatrix=true;
return;
}
}
}
//cout << "key: " << currentKeyMatrix << " max Keys: " << (*timeKeys).size() << endl;
//if we are at the end of all keys, we're finished!
//get the two closest interpolation points
timeKeyOne=keyFrames[currentKeyMatrix]->timeKey;
timeKeyTwo=keyFrames[currentKeyMatrix+1]->timeKey;
//get the time difference
double timeDifference=timeKeyTwo-timeKeyOne;
//map time difference to 0.0 - 1.0
double keyTime=currentTime-timeKeyOne; //-> current Position, in the beginning 0.0;
float relativeTime=(float) (keyTime/timeDifference); //-> will go from 0.0 to 1.0 between the keys
SkeletalActor* skel=(SkeletalActor*)moveActor;
for (uint i=0;i<skel->bones.size();i++){
matrixOne=keyFrames[currentKeyMatrix]->boneMatrices[skel->bones[i]->name];
matrixTwo=keyFrames[currentKeyMatrix+1]->boneMatrices[skel->bones[i]->name];
//ease in - ease out
float x=relativeTime*2.0f;
float y=0.0f;
if (x< 1)
y=x*x;
else
y=2-( (x-2) * (x-2) );
y=y* 0.5;
if (bLinear)
y=relativeTime;
//interpolate between them
//resultingMatrix=matrixOne.lerp(relativeTime,matrixTwo); //calculate resulting position
rotationOne=getRotationMatrix(matrixOne);
rotationTwo=getRotationMatrix(matrixTwo);
resultingRotation=rotationOne.lerp(y,rotationTwo);
resultingMatrix=matrixOne.lerp(y,matrixTwo); //calculate resulting position
if (bMerge && mergeFactor < 1.0 && mergeFactor > 0.0){
Matrix3f sourceMatrix=getRotationMatrix(skel->bones[i]->transformMatrix);
resultingRotation = sourceMatrix.lerp(mergeFactor,resultingRotation);
}
//normalize rotations!
resultingRotation=normalizeRotations(resultingRotation);
//apply rotation
skel->bones[i]->transformMatrix.setRotation(resultingRotation);
}
}
void InterpolationHelper::interpolateTransform(){
Matrix4f transformOne, transformTwo;
Matrix4f resultingTransform;
Matrix3f rotationOne;
Matrix3f rotationTwo;
Matrix3f resultingRotation;
double timeKeyOne;
double timeKeyTwo;
currentTime=renderer->currentTime-startTime;
//move forward if we are at end of key
while (currentTime> keyFrames[currentKeyTransform+1]->timeKey ){
//if (currentTime> keyFrames[currentKey+1]->timeKey ){
currentKeyTransform++;
if (currentKeyTransform+1 >= (int)keyFrames.size()){
bFinishedTransform=true;
return;
}
}
timeKeyOne=keyFrames[currentKeyTransform]->timeKey;
timeKeyTwo=keyFrames[currentKeyTransform+1]->timeKey;
//get the time difference
double timeDifference=timeKeyTwo-timeKeyOne;
//map time difference to 0.0 - 1.0
double keyTime=currentTime-timeKeyOne; //-> current Position, in the beginning 0.0;
float relativeTime=(float) (keyTime/timeDifference); //-> will go from 0.0 to 1.0 between the keys
/*
relativeTime=relativeTime*100.0;
relativeTime=(int)relativeTime/30;
relativeTime=relativeTime*0.3;
*/
if (bAdditive){
transformOne=moveActor->transformMatrix; //
relativeTime*=relativeTime; //
}else{
transformOne=keyFrames[currentKeyTransform]->transformKey;
}
transformTwo=keyFrames[currentKeyTransform+1]->transformKey;
float x=relativeTime*2.0f;
float y=0.0f;
if (x< 1)
y=x*x;
else
y=2-( (x-2) * (x-2) );
y=y* 0.5;
if (bLinear)
y=relativeTime;
rotationOne=getRotationMatrix(transformOne);
rotationTwo=getRotationMatrix(transformTwo);
resultingRotation=rotationOne.lerp(y,rotationTwo);
//interpolate between them
resultingTransform=transformOne.lerp(y,transformTwo); //calculate resulting position
//normalize rotations!
resultingRotation=normalizeRotations(resultingRotation);
resultingTransform.setRotation(resultingRotation);
//apply resulting position
if (bRelative){
//apply rotation
moveActor->transformMatrix=baseTransform* resultingTransform;
}
else{
//apply rotation
moveActor->transformMatrix=resultingTransform;
}
}
void testApp::compute_pda(const ofMatrix4x4 &preTmatrix,int timeZ,ofMatrix4x4 &newTmatrix)//,const ofVec3f* pointmapNow,const ofVec3f* pointmap_pre)
{
ofMatrix4x4 nowTmatrix;
if(timeZ==0)
nowTmatrix = preTmatrix;
else
{
// 迭代
nowTmatrix = newTmatrix;
//cout<<"z is big than 0ne"<<endl;
}
//ofMatrix4x4 invPreT = nowTmatrix.getInverse();
ofMatrix4x4 invPreT = preTmatrix.getInverse();
ofMatrix3x3 nowRmatrix = getRotationMatrix(nowTmatrix);
ofMatrix3x3 preRmatrix = getRotationMatrix(preTmatrix);
int i=0,j=0;
int dnum = 0;
double total_b = 0;
double total_bb = 0;
final_A = Eigen::MatrixXf::Zero(6,6);
final_B = Eigen::MatrixXf::Zero(6,1);
// 找到对应点对
for(j=0;j<KINECT_HEIGHT;j++)
for(i=0;i<KINECT_WIDTH;i++)
{
int num = j*KINECT_WIDTH+i;
if(pointsmap_orignal[num].z>0)//isvalid_orignal[num])
{
ofVec4f pointNow = ofVec4f(pointsmap_orignal[num].x,pointsmap_orignal[num].y,pointsmap_orignal[num].z,1);
ofVec4f spacePointNow = nowTmatrix*pointNow;
ofVec3f spacenormalNow = nmmul(nowRmatrix,normalmap_orignal[num]);
//cout<<i<<" "<<j<<endl;
ofVec4f pointPreC = invPreT*spacePointNow;
ofVec3f dpointPreC = ofVec3f(pointPreC.x,pointPreC.y,pointPreC.z);
ofVec3f pointPreZ = nmmul(the_K_cam,dpointPreC);
ofVec2f pointPreU = ofVec2f(pointPreZ.x/pointPreZ.z,pointPreZ.y/pointPreZ.z);
//cout<<endl;
int num_p = (int)(pointPreU.x+0.5)+(int)(pointPreU.y+0.5)*KINECT_WIDTH;
int kk = num;
if(pointsmap_final[num_p].z>0)
{
ofVec4f pointPre = ofVec4f(pointsmap_final[num_p].x,pointsmap_final[num_p].y,pointsmap_final[num_p].z,1);
ofVec4f spacePointPre = pointPre;
//ofVec4f spacePointPre = preTmatrix*pointPre;
ofVec3f spacenormalPre = normalmap_final[num_p];
//ofVec3f spacenormalPre = nmmul(preRmatrix,normalmap_final[num_p]);
float pointDistance = spacePointNow.distance(spacePointPre);
float pointAngle = spacenormalNow.angle(spacenormalPre);
if(pointDistance<=THRESHOLD_D&&pointAngle<=THRESHOLD_A)
{
ofVec3f dspacePointNow = ofVec3f(spacePointNow.x,spacePointNow.y,spacePointNow.z);
ofVec3f dspacePointPre = ofVec3f(spacePointPre.x,spacePointPre.y,spacePointPre.z);
ofVec3f minsPoint = (dspacePointPre - dspacePointNow);
float b = minsPoint.dot(spacenormalPre);
if(b > 0)
total_b += b;
else
total_bb += b;
compute_icp(b,dspacePointNow,spacenormalPre);
++dnum;
}
}
}
}
cout<<"dnum = "<<dnum<<" total_b = "<<total_b<<" total_bb = "<<total_bb<<endl;
//double deta = final_A.determinant();
//if(deta>0) // 条件待修改
Eigen::Matrix<float,6,1> result = final_A.llt().solve(final_B).cast<float>();
cout<<"result = "<<endl;
cout<<result<<endl;
float alpha = result(0);
float beta = result(1);
float gamma = result(2);
Eigen::Matrix3f rinc = (Eigen::Matrix3f)Eigen::AngleAxisf(gamma,Eigen::Vector3f::UnitZ())*Eigen::AngleAxisf(beta,Eigen::Vector3f::UnitY())*Eigen::AngleAxisf(alpha,Eigen::Vector3f::UnitX());
ofMatrix4x4 changeMatrix = ofMatrix4x4(rinc(0,0),rinc(0,1),rinc(0,2),result(3),
rinc(1,0),rinc(1,1),rinc(1,2),result(4),
rinc(2,0),rinc(2,1),rinc(2,2),result(5),
0,0,0,1);
cout<<"change = "<<endl;
cout<<changeMatrix<<endl;
newTmatrix = changeMatrix * newTmatrix;
cout<<"newTM = "<<endl;
cout<<newTmatrix<<endl;
}
Eigen::Matrix<double, 3, 3> getInverseRotationMatrix(Eigen::Matrix<double, 3, 1> l, double cos, double sin) {
return getRotationMatrix(l, cos, -sin);
}
glm::mat4 getViewMatrix(float u) const
{
glm::mat4 m = getRotationMatrix(u);
m = glm::translate(m, -lerp(u, prevPosition, position));
return m;
}
glm::vec3 getDirection()
{
return glm::normalize((glm::inverse(getRotationMatrix(0.0f)) * glm::vec4(0.0f, 0.0f, -1.0f, 1.0f)).xyz());
}
void SpelchkCamera::calculateCameraPosition() {
vec4 cameraDisplacement = getRotationMatrix() * vec4( _xDepth, _yDepth, _zDepth, 0.0 );
_cameraPosition = (_oldCameraPosition + cameraDisplacement);
}
State::RotationMatrix State::getRotationMatrix() const {
RotationMatrix R;
getRotationMatrix(R);
return R;
}
void RemoteControl::caminatas (byte comando) {
// todo esto está programado como el orto, hay que reorganizarlo
// por ejemplo este "anguloso"... talvez las otras variables también deban estar acá
static float anguloso = 0;
if (anguloso != 0) {
COORD2D matrix [2];
getRotationMatrix (matrix, anguloso);
centro_caminata = applyMatrix (centro_caminata, matrix);
anguloso = 0;
}
// switch de seteo
switch (comando) {
/// mega provisorio
///////////////////////////
case 5:
mov.salto (velocidad, HALF_PI);
break;
///////////////////////////
/// termina mega provisorio
case RC_UP:
texto1 = "UP";
angulo = angulo_offset + HALF_PI;
break;
case RC_DOWN:
texto1 = "DN";
angulo = angulo_offset - HALF_PI;
break;
case RC_LEFT:
texto1 = "LEFT";
if (modo == CAMINATAS1) {
angulo = angulo_offset + PI;
} else {
anguloso = 0;
mov.mon_angulo = &anguloso; // aca el ángulo offset se usaría para rotar el centro
mov.curva (velocidad, desplazamiento, (COORD2D) {0, 0} , CCW, marcha, largo_pasos);
}
break;
case RC_RIGHT:
texto1 = "RIGHT";
if (modo == CAMINATAS1) {
angulo = angulo_offset;
} else {
anguloso = 0;
mov.mon_angulo = &anguloso; // aca el ángulo offset se usaría para rotar el centro
mov.curva (velocidad, desplazamiento, (COORD2D) {0, 0} , CW, marcha, largo_pasos);
}
break;
case RC_MENU:
texto1 = "MENU";
mov.mon_angulo = NULL;
mov.curva (velocidad, desplazamiento, centro_caminata, CCW, marcha, largo_pasos);
break;
case RC_EXIT:
texto1 = "EXIT";
mov.mon_angulo = NULL;
mov.curva (velocidad, desplazamiento, centro_caminata, CW, marcha, largo_pasos);
break;
case RC_MTS:
texto1 = "MTS";
mov.mon_angulo = &angulo_offset;
mov.curva (velocidad, desplazamiento, centro_caminata, CCW, marcha, largo_pasos);
break;
case RC_CCTTX:
mov.mon_angulo = &angulo_offset;
mov.curva (velocidad, desplazamiento, centro_caminata, CW, marcha, largo_pasos);
texto1 = "CC_TTX";
break;
case RC_ENTER1:
texto1 = "STOP";
mov.stop();
isMoving = false;
break;
case RC_VOL_UP:
if (pantalla.isBusy()) {break;}
velocidad = constrain (velocidad+inc, 1, 50);
texto1 = "Vel " + float2string (velocidad);
if (isMoving) {mov.set_vel (velocidad);}
retardo = true;
break;
case RC_VOL_DN:
if (pantalla.isBusy()) {break;}
velocidad = constrain (velocidad-inc, 1, 50);
texto1 = "Vel " + float2string (velocidad);
if (isMoving) {mov.set_vel (velocidad);}
retardo = true;
break;
case RC_CH_UP:
if (pantalla.isBusy()) {break;}
if (!isMoving) {
largo_pasos = constrain (largo_pasos+inc, 0, 20);
texto1 = "Paso ";
if (largo_pasos == 0) {texto1 += "AUTO";} else {texto1 += float2string (largo_pasos);}
} else {
texto1 = "Escala " + String (mov.dec_escala(), DEC);
}
retardo = true;
break;
case RC_CH_DN:
if (pantalla.isBusy()) {break;}
if (!isMoving) {
largo_pasos = constrain (largo_pasos-inc, 0, 20);
texto1 = "Paso ";
if (largo_pasos == 0) {texto1 += "AUTO";} else {texto1 += float2string (largo_pasos);}
} else {
texto1 = "Escala " + String (mov.inc_escala(), DEC);
}
retardo = true;
break;
}
// switch de ejecución (y puede haber más; talvez la variable swicheada en segunda instancia no sea "comando")
switch (comando) {
case RC_UP: case RC_DOWN: case RC_LEFT: case RC_RIGHT:
if (modo == CAMINATAS1) {mov.mon_desplazamiento = NULL;}
else if (modo == CAMINATAS2) {
if (comando == RC_LEFT || comando == RC_RIGHT) {break;} // la lógica hay que reformularla toda
mov.mon_desplazamiento = ¢ro_caminata;
}
mov.recta (velocidad, desplazamiento, angulo, marcha, largo_pasos);
isMoving = true;
break;
}
}
