Commit 6a2440b5 authored by Patric Schmitz's avatar Patric Schmitz

fix torso transform calculation

parent 9e4c600c
#include <sstream>
#include <fstream>
#include <iostream>
#include <iomanip>
#include <glm/ext.hpp>
#include <glm/gtx/matrix_decompose.hpp>
#include "datareader.h"
#define DEBUG_OUTPUT 0
glm::mat4 realHeadTransform;
glm::mat4 realFrontTransform;
glm::mat4 realBackTransform;
glm::mat4 realTorsoTransform;
glm::mat4 realHeadTransformLast;
glm::mat4 realFrontTransformLast;
glm::mat4 realBackTransformLast;
glm::vec3 offsetTorsoFrontToCenter;
glm::vec3 offsetTorsoBackToCenter;
glm::mat4 correctBodyTransform(glm::mat4 transform);
glm::mat4 cleanMatrix(glm::mat4 matrix);
void computeTorsoTransform();
std::string getMatlabString(glm::mat4 v);
......@@ -41,27 +48,25 @@ int main()
std::getline(ssLine, token, ',');
std::getline(ssLine, token, ',');
std::getline(ssLine, token, ',');
realHeadTransform = reader.ReadMat4(token);
if(realHeadTransform[3] == realHeadTransformLast[3])
continue;
std::getline(ssLine, token, ',');
std::getline(ssLine, token, ',');
std::getline(ssLine, token, ',');
realFrontTransform = reader.ReadMat4(token);
std::getline(ssLine, token, ',');
realBackTransform = reader.ReadMat4(token);
realFrontTransform = correctBodyTransform(realFrontTransform);
glm::vec3 scale;
glm::quat rotation;
glm::vec3 translation;
glm::vec3 skew;
glm::vec4 perspective;
glm::decompose(realHeadTransform,
scale, rotation, translation, skew, perspective);
std::getline(ssLine, token, ',');
if(scale != glm::vec3(1,1,1)) {
std::cout << glm::to_string(scale) << std::endl;
}
realBackTransform = reader.ReadMat4(token);
realBackTransform = correctBodyTransform(realBackTransform);
#if 0
#if DEBUG_OUTPUT
std::cout << "+++++++++++++++++++++++++++++++" << std::endl;
std::cout << "realHeadTransform" << std::endl;
std::cout << glm::to_string(realHeadTransform) << std::endl;
......@@ -71,21 +76,28 @@ int main()
std::cout << "realBackTransform" << std::endl;
std::cout << glm::to_string(realBackTransform) << std::endl;
#endif
computeTorsoTransform();
#if DEBUG_OUTPUT
std::cout << "realTorsoTransform" << std::endl;
std::cout << glm::to_string(realTorsoTransform) << std::endl;
#endif
computeTorsoTransform();
realHeadTransformLast = realHeadTransform;
realFrontTransformLast = realFrontTransform;
realBackTransformLast = realBackTransform;
fileOut << line << std::endl;
std::string lineOut =
getMatlabString(realHeadTransform) + "," +
getMatlabString(realFrontTransform) + "," +
getMatlabString(realBackTransform) + "," +
getMatlabString(realTorsoTransform) + ",";
fileOut << lineOut << std::endl;
}
file.close();
// operations on the buffer...
file.close();
} else {
std::cout << "Record file not found." << std::endl;
}
......@@ -144,12 +156,16 @@ void computeTorsoTransform() {
realTorsoPosition = 0.5f *
(realFrontTransform[3] + realBackTransform[3]);
glm::fquat torsoFrontOrientationQuat =
glm::quat torsoFrontOrientationQuat =
glm::quat_cast(realFrontTransform);
glm::fquat torsoBackOrientationQuat =
glm::quat torsoBackOrientationQuat =
glm::quat_cast(realBackTransform);
glm::fquat realTorsoOrientationQuat = glm::mix(
if(glm::dot(torsoFrontOrientationQuat,
torsoBackOrientationQuat) < 0.f)
torsoFrontOrientationQuat = -torsoFrontOrientationQuat;
glm::quat realTorsoOrientationQuat = glm::mix(
torsoFrontOrientationQuat, torsoBackOrientationQuat, 0.5f);
realTorsoOrientation = glm::mat4_cast(realTorsoOrientationQuat);
......@@ -157,20 +173,17 @@ void computeTorsoTransform() {
if (hasBodyTorsoFront) {
offsetTorsoFrontToCenter =
// glm::inverse(glm::mat3(realFrontTransform)) *
glm::transpose(glm::mat3(realFrontTransform)) *
(realTorsoPosition - glm::vec3(realFrontTransform[3]));
// (glm::vec3(realFrontTransform[3]) - realTorsoPosition);
}
if (hasBodyTorsoBack) {
offsetTorsoBackToCenter =
// glm::inverse(glm::mat3(realBackTransform)) *
glm::transpose(glm::mat3(realBackTransform)) *
(realTorsoPosition - glm::vec3(realBackTransform[3]));
}
#if 0
#if DEBUG_OUTPUT
std::cout << std::boolalpha
<< "hasBodyTorsoFront: " << hasBodyTorsoFront << std::endl;
std::cout << std::boolalpha
......@@ -185,8 +198,6 @@ void computeTorsoTransform() {
<< glm::to_string(realTorsoPosition) << std::endl;
#endif
// realTorsoTransform = realTorsoOrientation;
// realTorsoTransform[3] = glm::vec4(realTorsoPosition, 1);
realTorsoTransform =
glm::translate(glm::mat4(1.0f), realTorsoPosition) *
realTorsoOrientation;
......@@ -194,3 +205,31 @@ void computeTorsoTransform() {
for(auto & element : realTorsoTransform)
assert(glm::isnan(element) == false);
}
glm::mat4 correctBodyTransform(glm::mat4 transform) {
for(int entry = 0 ; entry < 3 ; ++entry) {
transform[3][entry] /= 1000.0f;
}
transform[3] += glm::vec4(4, 0, -4, 0);
return transform;
}
glm::mat4 cleanMatrix(glm::mat4 matrix) {
glm::vec3 scale;
glm::quat rotation;
glm::vec3 translation;
glm::vec3 skew;
glm::vec4 perspective;
glm::decompose(matrix,
scale, rotation, translation, skew, perspective);
glm::mat4 result = glm::toMat4(rotation);
result[3] = glm::vec4(translation, 1);
for(int col = 0 ; col < 3 ; ++col) {
result[col] = glm::normalize(result[col]);
}
return result;
}
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment