void MyEntityClass::Init(void)
{
m_v3Position = vector3();
m_qOrientation = quaternion();
m_v3Scale = vector3();
m_v3Velocity = vector3();
m_v3Acceleration = vector3();
m_sName = "";
m_fMaxAcc = 10.0f;
m_fMass = 1.0f;
m_pColliderManager = MyBOManager::GetInstance();
m_pMeshManager = MeshManagerSingleton::GetInstance();
m_pScoreMngr = ScoreManager::GetInstance();
}
//---
void EnemyObject::Init(void)
{
//Set initial values
m_v3Position = vector3();
m_qOrientation = quaternion();
m_v3Scale = vector3();
m_v3Velocity = vector3();
m_v3Acceleration = vector3();
m_fMass = 1.0f;
m_fMaxAcc = 0.1f;
m_pColliderManager = BoundingObjectManager::GetInstance();
m_pMeshManager = MeshManagerSingleton::GetInstance();
boIndex = m_pColliderManager->AddBox("Mine", m_pMeshManager->GetVertexList("Mine")); //initialize a bounding object and save its index
}
Foam::triad::operator Foam::quaternion() const
{
tensor R;
R.xx() = x().x();
R.xy() = y().x();
R.xz() = z().x();
R.yx() = x().y();
R.yy() = y().y();
R.yz() = z().y();
R.zx() = x().z();
R.zy() = y().z();
R.zz() = z().z();
return quaternion(R);
}
Foam::tmp<Foam::pointField> Foam::sixDoFRigidBodyMotion::transform
(
const pointField& initialPoints,
const scalarField& scale
) const
{
// Calculate the transformation septerion from the initial state
septernion s
(
centreOfRotation() - initialCentreOfRotation(),
quaternion(Q().T() & initialQ())
);
tmp<pointField> tpoints(new pointField(initialPoints));
pointField& points = tpoints.ref();
forAll(points, pointi)
{
// Move non-stationary points
if (scale[pointi] > SMALL)
{
// Use solid-body motion where scale = 1
if (scale[pointi] > 1 - SMALL)
{
points[pointi] = transform(initialPoints[pointi]);
}
// Slerp septernion interpolation
else
{
septernion ss(slerp(septernion::I, s, scale[pointi]));
points[pointi] =
initialCentreOfRotation()
+ ss.invTransformPoint
(
initialPoints[pointi]
- initialCentreOfRotation()
);
}
}
}
return tpoints;
}
void SceneNodeWidget::updateSceneNode()
{
auto target=m_node.lock();
if(!target){
return;
}
// position
auto &position=target->position();
ui->q_x->setValue(position[0]);
ui->q_y->setValue(position[1]);
ui->q_z->setValue(position[2]);
// rotation
auto &quternion=target->quaternion();
// mesh
auto mesh=target->getMesh();
if(mesh){
ui->meshWidget->show();
}
else{
ui->meshWidget->hide();
}
// camera
auto camera=target->getCamera();
if(camera){
ui->cameraWidget->show();
}
else{
ui->cameraWidget->hide();
}
// light
auto light=target->getLight();
if(light){
ui->lightWidget->show();
}
else{
ui->lightWidget->hide();
}
}
void ARMultiPublisher::publishMarker(const int index,
const tf::Transform& transform,
const ros::Time& time_stamp)
{
const double MARKER_THIKNESS_SCALE = 0.1;
const double WIDTH = multi_marker_config_->marker[index].width;
tf::Vector3 marker_origin(0, 0, (MARKER_THIKNESS_SCALE / (double)2.0) * WIDTH * AR_TO_ROS);
tf::Transform quaternion(tf::Quaternion::getIdentity(), marker_origin);
tf::Transform marker_pose = transform * quaternion; // marker pose in the camera frame
visualization_msgs::Marker rviz_marker;
tf::poseTFToMsg(marker_pose, rviz_marker.pose);
rviz_marker.header.frame_id = camera_frame_;
rviz_marker.header.stamp = time_stamp;
rviz_marker.id = marker_info_[index].id;
rviz_marker.scale.x = 1.0 * WIDTH * AR_TO_ROS;
rviz_marker.scale.y = 1.0 * WIDTH * AR_TO_ROS;
rviz_marker.scale.z = MARKER_THIKNESS_SCALE * WIDTH * AR_TO_ROS;
// rviz_marker_.ns.assign(node_handle_.getNamespace());
rviz_marker.ns.assign(marker_frame_);
rviz_marker.type = visualization_msgs::Marker::CUBE;
rviz_marker.action = visualization_msgs::Marker::ADD;
if(marker_info_[marker_indizes_[index]].cf > 0.9)
{
rviz_marker.color.r = marker_red_;
rviz_marker.color.g = marker_green_;
rviz_marker.color.b = marker_blue_;
rviz_marker.color.a = 1.0;
}
else
{
rviz_marker.color.r = 1.0f;
rviz_marker.color.g = 0.0f;
rviz_marker.color.b = 0.0f;
rviz_marker.color.a = 1.0;
}
rviz_marker.lifetime = ros::Duration(0.1);
rviz_marker_pub_.publish(rviz_marker);
ROS_DEBUG ("Published visual marker");
}
quaternion slerp( const quaternion& src, const quaternion& dest, float t )
{
float cos_omega = dot_prod4(src.comps(), dest.comps());
vec4 near_dest_v = dest.comps() * sign(cos_omega);
cos_omega = abs(cos_omega);
float k0(0.0f), k1(0.0f);
if( equal(cos_omega, 1.0f) ){
k0 = 1.0f - t;
k1 = t;
}else{
float sin_omega = sqrt(1.0f - cos_omega*cos_omega);
float omega = atan2(sin_omega, cos_omega);
float inv_sin_omega = 1.0f / sin_omega;
k0 = sin( (1.0f - t) * omega ) * inv_sin_omega;
k1 = sin( t * omega ) * inv_sin_omega;
}
return quaternion( src.comps() * k0 + near_dest_v * k1);
}
void key_press(int key) {
if (key == 'A') {
thrustPeriod = 2;
rigidBody.applyForce(matrix3x1(0, -1, 0), matrix3x1(-1, 0, 0));
rigidBody.applyForce(matrix3x1(0, 1, 0), matrix3x1( 1, 0, 0));
}
else if (key == 'D') {
thrustPeriod = 2;
rigidBody.applyForce(matrix3x1(0, 1, 0), matrix3x1(-1, 0, 0));
rigidBody.applyForce(matrix3x1(0, -1, 0), matrix3x1( 1, 0, 0));
}
else if (key == 'W') {
thrustPeriod = 2;
rigidBody.applyForce(matrix3x1(0, 0, 1), matrix3x1(0, 1, 0));
rigidBody.applyForce(matrix3x1(0, 0, -1), matrix3x1(0,-1, 0));
}
else if (key == 'S') {
thrustPeriod = 2;
rigidBody.applyForce(matrix3x1(0, 0, -1), matrix3x1(0, 1, 0));
rigidBody.applyForce(matrix3x1(0, 0, 1), matrix3x1(0,-1, 0));
}
else if (key == 'Z') {
thrustPeriod = 2;
rigidBody.applyForce(matrix3x1(-1, 0, 0), matrix3x1(0, 0, 1));
rigidBody.applyForce(matrix3x1( 1, 0, 0), matrix3x1(0, 0, -1));
}
else if (key == 'X') {
thrustPeriod = 2;
rigidBody.applyForce(matrix3x1(-1, 0, 0), matrix3x1(0, 0, -1));
rigidBody.applyForce(matrix3x1( 1, 0, 0), matrix3x1(0, 0, 1));
}
else if (key == ' ') {
rigidBody.netTorque = matrix3x1(0, 0, 0);
rigidBody.angularVelocity = matrix3x1(0, 0, 0);
}
else if (key == 'R') {
rigidBody.netTorque = matrix3x1(0, 0, 0);
rigidBody.angularVelocity = matrix3x1(0, 0, 0);
rigidBody.orientation = quaternion(1, 0, 0, 0);
}
}
void RigidBody::start() {
const Collider *collider = gameObject->get<Collider>();
if (!collider)
return;
glm::vec3 translation = gameObject->getTranslation();
glm::vec3 rotation = gameObject->getRotation();
btVector3 position(translation.x, translation.y, translation.z);
btQuaternion quaternion(rotation.x, rotation.y, rotation.z);
btVector3 inertia(0, 0, 0);
collider->shape->calculateLocalInertia(mass, inertia);
btDefaultMotionState *motionState = new btDefaultMotionState(btTransform(quaternion, position));
btRigidBody::btRigidBodyConstructionInfo rigidBodyCI(mass, motionState, collider->shape, inertia);
rigidBody = new btRigidBody(rigidBodyCI);
Command::physics.push(new AddRigidBodyCommand(rigidBody));
rigidBodies.emplace(rigidBody, this);
}
void trackball::set_2d_coords(float const x0,float const y0,float const x1,float const y1)
{
//security to not move if start=end
float epsilon=1e-5f;
if( std::pow((x0-x1)*(x0-x1)+(y0-y1)*(y0-y1),0.5f)<epsilon)
d_q=quaternion(1.0f,0.0f,0.0f,0.0f);
else
{
vec3 const p1=vec3(x0,y0,project_to_disc(x0,y0));
vec3 const p2=vec3(x1,y1,project_to_disc(x1,y1));
vec3 const axis = normalized(cross(p1,p2));
vec3 const u=p1-p2;
float t=norm(u)/(2.0f*disc_radius);
t=std::min(std::max(t,-1.0f),1.0f); //clamp
float const phi = 2.0f*asin(t);
//compute quaternion to apply
d_q.set_axis_angle(axis,phi);
}
}
float *
psmove_fusion_get_modelview_matrix(PSMoveFusion *fusion, PSMove *move)
{
psmove_return_val_if_fail(fusion != NULL, NULL);
psmove_return_val_if_fail(move != NULL, NULL);
float q0, q1, q2, q3;
psmove_get_orientation(move, &q0, &q1, &q2, &q3);
if (psmove_tracker_get_mirror(fusion->tracker)) {
/* Need to invert these two axes if mirroring is enabled */
q3 *= -1.;
q2 *= -1.;
}
glm::quat quaternion(q3, q2, q1, q0);
float x, y, z;
psmove_fusion_get_position(fusion, move, &x, &y, &z);
fusion->modelview = glm::translate(glm::mat4(),
glm::vec3(x, y, z)) * glm::mat4_cast(quaternion);
return glm::value_ptr(fusion->modelview);
}
vector<rigid_sys::dynamic_info> rigid_sys::get_dynamic(number_t t,vector<dynamic_info> start)
{
vector<rigid_sys::dynamic_info> dynamic_dot;
for(int i=0;i<start.size();i++)
{
rigid_sys::dynamic_info change;
rigid_sys::dynamic_info origin;
origin=start[i];
//dx
change.x=origin.P/bodies[i].total_mass;
//dq
matrix3 R(origin.q);
matrix3 I_inv=R*bodies[i].Ibody_Inv*R.transpose();
vec omega=I_inv*origin.L;
change.q=quaternion(omega)*origin.q;
//dP
vec total_force(0,0,0);
for(int j=0;j<bodies[i].mass.size();j++)
total_force=total_force+extern_force(t,i,j);
change.P=total_force;
//dL
vec total_torque(0,0,0);
for(int j=0;j<bodies[i].mass.size();j++)
total_torque=total_torque+cross(matrix3(origin.q)*bodies[i].pos[j]-origin.x,extern_force(t,i,j));
change.L=total_torque;
dynamic_dot.push_back(change);
}
return dynamic_dot;
}
Quaternion EulerToQuaternion(const vector3df& euler)
{
vector3df rhEuler(euler.X, euler.Y, euler.Z);
return ConvertQuaternion(quaternion(rhEuler * irr::core::DEGTORAD));
}
quaternion conjugated(quaternion const& q)
{
return quaternion(-q.x(),-q.y(),-q.z(),q.w());
}
quaternion ConvertQuaternion(const Quaternion& quat)
{
return quaternion(quat.x(), quat.y(), quat.z(), quat.w());
}
static int quaternion_new(lua_State* L)
{
LuaStack stack(L);
stack.push_quaternion(quaternion(stack.get_vector3(1), stack.get_float(2)));
return 1;
}
quaternion operator-(quaternion const& q)
{
return quaternion(-q.x(),-q.y(),-q.z(),-q.w());
}
bool ProbeCalibrationWidget::calibrateLSQR()
{
typedef lsqrRecipes::SingleUnknownPointTargetUSCalibrationParametersEstimator::DataType
DataType;
std::cout << "Cross wire phantom, experimental data:\n";
std::cout << "--------------------------------------\n";
DataType dataElement;
std::vector<DataType> data;
std::vector<double> estimatedUSCalibrationParameters;
// data structures to store iamge data
std::vector <lsqrRecipes::Frame> transformations;
std::vector<lsqrRecipes::Point2D> imagePoints;
for (uint i = 0; i < translations.rows(); i++)
{
// lsqrRecipes::Frame f(
// translations[i][0],
// translations[i][1],
// translations[i][2],
// rotations[i][0],
// rotations[i][1],
// rotations[i][2],
// rotations[i][3],
// false
// );
lsqrRecipes::Frame f;
vnl_quaternion<double> quaternion(
rotations[i][1],
rotations[i][2],
rotations[i][3],
rotations[i][0]);
vnl_matrix<double> rotationMatrix = quaternion.rotation_matrix_transpose();
// rotationMatrix = rotationMatrix.transpose();
f.setRotationMatrix(rotationMatrix);
vnl_vector<double> translation = translations.get_row(i);
f.setTranslation(translation);
transformations.push_back(f);
std::cout << transformations.at(i) << std::endl;
}
for (uint i = 0; i < coords.rows(); i++)
{
lsqrRecipes::Point2D p;
p[0] = coords[i][0];
p[1] = coords[i][1];
imagePoints.push_back(p);
}
for (uint i = 0; i < transformations.size(); i++)
{
dataElement.T2 = transformations[i];
dataElement.q = imagePoints[i];
data.push_back(dataElement);
}
double maxDistanceBetweenPoints = 5.0;
lsqrRecipes::SingleUnknownPointTargetUSCalibrationParametersEstimator
usCalibration(maxDistanceBetweenPoints);
// //estimate using the RANSAC algorithm
// double desiredProbabilityForNoOutliers = 0.999;
// double percentageOfDataUsed;
// std::vector<bool> consensusSet;
// percentageOfDataUsed =
// lsqrRecipes::RANSAC< DataType, double>::compute(estimatedUSCalibrationParameters,
// &usCalibration,
// data,
// desiredProbabilityForNoOutliers,
// &consensusSet);
usCalibration.setLeastSquaresType(
lsqrRecipes::SingleUnknownPointTargetUSCalibrationParametersEstimator::ANALYTIC);
usCalibration.leastSquaresEstimate(data, estimatedUSCalibrationParameters);
// std::cout << "us calibration \n" << usCalibration << std::endl;
if (estimatedUSCalibrationParameters.size() == 0)
{
std::cout << "FAILED CALIBRATION, possibly degenerate configuration\n\n\n";
return EXIT_FAILURE;
}
else
{
std::cout << "t1[x,y,z]:\n";
std::cout << "\t[" << estimatedUSCalibrationParameters[0] << ", " << estimatedUSCalibrationParameters[1];
std::cout << ", " << estimatedUSCalibrationParameters[2] << "]\n";
std::cout << "t3[x,y,z]:\n";
std::cout << "\t[" << estimatedUSCalibrationParameters[3] << ", " << estimatedUSCalibrationParameters[4];
std::cout << ", " << estimatedUSCalibrationParameters[5] << "]\n";
std::cout << "omega[z,y,x]:\n";
std::cout << "\t[" << estimatedUSCalibrationParameters[6] << ", " << estimatedUSCalibrationParameters[7];
std::cout << ", " << estimatedUSCalibrationParameters[8] << "]\n";
std::cout << "m[x,y]:\n";
std::cout << "\t[" << estimatedUSCalibrationParameters[9];
std::cout << ", " << estimatedUSCalibrationParameters[10] << "]\n";
return true;
}
}
vec3 operator*(quaternion const& lhs,vec3 const& rhs)
{
quaternion q=conjugated(lhs)*quaternion(rhs.x(),rhs.y(),rhs.z(),0.0f)*lhs;
return {q.x(),q.y(),q.z()};
}
void key_press(int key) {
if (key == 'A') {
thrustPeriod = 2;
rigidBody.applyForce(matrix3x1(0, -1, 0), matrix3x1(-1, 0, 0));
rigidBody.applyForce(matrix3x1(0, 1, 0), matrix3x1( 1, 0, 0));
}
else if (key == 'D') {
thrustPeriod = 2;
rigidBody.applyForce(matrix3x1(0, 1, 0), matrix3x1(-1, 0, 0));
rigidBody.applyForce(matrix3x1(0, -1, 0), matrix3x1( 1, 0, 0));
}
else if (key == 'W') {
thrustPeriod = 2;
rigidBody.applyForce(matrix3x1(0, 0, 1), matrix3x1(0, 1, 0));
rigidBody.applyForce(matrix3x1(0, 0, -1), matrix3x1(0,-1, 0));
}
else if (key == 'S') {
thrustPeriod = 2;
rigidBody.applyForce(matrix3x1(0, 0, -1), matrix3x1(0, 1, 0));
rigidBody.applyForce(matrix3x1(0, 0, 1), matrix3x1(0,-1, 0));
}
else if (key == 'Z') {
thrustPeriod = 2;
rigidBody.applyForce(matrix3x1(-1, 0, 0), matrix3x1(0, 0, 1));
rigidBody.applyForce(matrix3x1( 1, 0, 0), matrix3x1(0, 0, -1));
}
else if (key == 'X') {
thrustPeriod = 2;
rigidBody.applyForce(matrix3x1(-1, 0, 0), matrix3x1(0, 0, -1));
rigidBody.applyForce(matrix3x1( 1, 0, 0), matrix3x1(0, 0, 1));
}
else if (key == ' ') {
rigidBody.netTorque = matrix3x1(0, 0, 0);
rigidBody.angularVelocity = matrix3x1(0, 0, 0);
rigidBody.velocity = matrix3x1(0, 0, 0);
}
else if (key == 'R') {
rigidBody.netTorque = matrix3x1(0, 0, 0);
rigidBody.angularVelocity = matrix3x1(0, 0, 0);
rigidBody.orientation = quaternion(1, 0, 0, 0);
}
else if (key == 262) {
// right
thrustPeriod = 2;
rigidBody.applyForce(matrix3x1(1, 0, 0), matrix3x1(0,0,0));
}
else if (key == 263) {
// left
thrustPeriod = 2;
rigidBody.applyForce(matrix3x1(-1, 0, 0), matrix3x1(0,0,0));
}
else if (key == 264) {
// back
thrustPeriod = 2;
rigidBody.applyForce(matrix3x1(0, 0, -1), matrix3x1(0,0,0));
}
else if (key == 265) {
thrustPeriod = 2;
rigidBody.applyForce(matrix3x1(0, 0, 1), matrix3x1(0,0,0));
}
else {
std::cout << "keypress: " << key << std::endl;
}
}
quaternion quaternion::operator +(const quaternion &q) {
return quaternion(x+q.x, y+q.y, z+q.z, w+q.w);
}
quaternion quaternion::conjugated() const
{
quaternion res=quaternion(s,-x,-y,-z);
return res;
}
quaternion quaternion::operator *(const quaternion &q) {
return quaternion(w * q.x + x * q.w + y * q.z - z * q.y,
w * q.y - x * q.z + y * q.w + z * q.x,
w * q.z + x * q.y - y * q.x + z * q.w,
w * q.w - x * q.x - y * q.y - z * q.z);
}
quaternion conjugate(quaternion &q) {
return quaternion(-q.x, -q.y, -q.z, q.w);
}
friend quaternion operator* (real fScalar, const quaternion& rkQ)
{
return quaternion(fScalar*rkQ.x, fScalar*rkQ.y, fScalar*rkQ.z, fScalar*rkQ.w);
}
You should have received a copy of the GNU General Public License
along with OpenFOAM. If not, see <http://www.gnu.org/licenses/>.
\*---------------------------------------------------------------------------*/
#include "septernion.H"
#include "IOstreams.H"
#include "OStringStream.H"
// * * * * * * * * * * * * * * Static Data Members * * * * * * * * * * * * * //
const char* const Foam::septernion::typeName = "septernion";
const Foam::septernion Foam::septernion::zero
(
vector(0, 0, 0),
quaternion(0, vector(0, 0, 0))
);
const Foam::septernion Foam::septernion::I
(
vector(0, 0, 0),
quaternion(1, vector(0, 0, 0))
);
// * * * * * * * * * * * * * * * * Constructors * * * * * * * * * * * * * * //
Foam::septernion::septernion(Istream& is)
{
is >> *this;
}
void ProbeCalibrationWidget::calibrate()
{
// generate the transformation (rotation and translation) matrixes for each image
std::cout << "Transformation matrices" << std::endl;
std::cout << std::endl;
const int points_size = imageStack.size();
vnl_matrix<double> transformationArray [points_size];
std::vector<vnl_matrix<double>* > transformationSet(points_size);
for (int i = 0; i < points_size; i++)
{
// the given parameters is [0]=scale factor, [1]=x, [2]=y, [3]=z
vnl_quaternion<double> quaternion(
rotations[i][1],
rotations[i][2],
rotations[i][3],
rotations[i][0]);
vnl_matrix<double> transformation = quaternion.rotation_matrix_transpose_4();
transformation = transformation.transpose();
transformation.put(0, 3, translations[i][0]);
transformation.put(1, 3, translations[i][1]);
transformation.put(2, 3, translations[i][2]);
transformationArray[i] = transformation;
transformationSet[i] = &transformationArray[i];
std::cerr << transformation;
std::cout << std::endl;
}
coords.print(std::cout);
std::cout << std::endl;
// const int point_size = 7;
//
// // vnl_matrix<int> coords(point_size, 2);
// this->coords.set_size(point_size, 2);
// coords[0][0] = 199;
// coords[0][1] = 164;
// coords[1][0] = 211;
// coords[1][1] = 193;
// coords[2][0] = 84;
// coords[2][1] = 175;
// coords[3][0] = 226;
// coords[3][1] = 180;
// coords[4][0] = 190;
// coords[4][1] = 200;
// coords[5][0] = 326;
// coords[5][1] = 212;
// coords[6][0] = 257;
// coords[6][1] = 179;
//
// // vnl_matrix<double> rotations(point_size, 4);
// this->rotations.set_size(point_size, 4);
//
// double r0 [] = {-0.372, 0.894, -0.233, 0.094};
// double r1 [] = {-0.228, 0.523, -0.783, 0.248};
// double r2 [] = {-0.078, -0.012, 0.925, -0.372};
// double r3 [] = {-0.333, 0.777, -0.495, 0.199};
// double r4 [] = {-0.225, 0.288, -0.882, 0.297};
// double r5 [] = {-0.357, 0.931, 0.073, 0.024};
// double r6 [] = {-0.451, 0.890, -0.046, 0.047};
//
// rotations.set_row(0, r0);
// rotations.set_row(1, r1);
// rotations.set_row(2, r2);
// rotations.set_row(3, r3);
// rotations.set_row(4, r4);
// rotations.set_row(5, r5);
// rotations.set_row(6, r6);
//
// // vnl_matrix<double> translations(point_size, 3);
// this->translations.set_size(point_size, 3);
//
// double t0 [] = {279.010, 157.851, 63.233};
// double t1 [] = {305.487, 149.455, 66.491};
// double t2 [] = {291.314, 126.426, 62.004};
// double t3 [] = {291.800, 162.819, 64.538};
// double t4 [] = {304.505, 136.624, 68.660};
// double t5 [] = {255.105, 151.850, 63.597};
// double t6 [] = {266.262, 151.553, 66.177};
//
// translations.set_row(0, t0);
// translations.set_row(1, t1);
// translations.set_row(2, t2);
// translations.set_row(3, t3);
// translations.set_row(4, t4);
// translations.set_row(5, t5);
// translations.set_row(6, t6);
// //
// //
// // std::cout << "\n Rotations \n" << rotations << "\n" << std::endl;
// //
// // std::cout << "\n Transtaltions \n" << translations << "\n" << std::endl;
// //
// //
// // // gnerate the transformation (rotation and translation) matrixes for each image
// // std::cout << "Transformation matrices" << std::endl;
// // std::cout << std::endl;
// //
// // vnl_matrix<double> transformationArray [point_size];
// // std::vector<vnl_matrix<double>* > transformationSet(point_size);
// //
// // for (int i = 0; i < point_size; i++) {
// //
// // // the given parameters is [0]=scale factor, [1]=x, [2]=y, [3]=z
// // vnl_quaternion<double> quaternion(rotations[i][1], rotations[i][2], rotations[i][3], rotations[i][0]);
// // vnl_matrix<double> transformation = quaternion.rotation_matrix_transpose_4();
// // transformation = transformation.transpose();
// //
// // transformation.put(0, 3, translations[i][0]);
// // transformation.put(1, 3, translations[i][1]);
// // transformation.put(2, 3, translations[i][2]);
// //
// // transformationArray[i] = transformation;
// // transformationSet[i] = &transformationArray[i];
// //
// // std::cerr << transformation;
// // std::cout << std::endl;
// //
// // }
// //
// // coords.print(std::cout);
// // std::cout << std::endl;
// //
//
//
std::cout << "------------- LSQRRecipies --------------" << std::endl;
this->calibrateLSQR();
//
// std::cout << "------------- Traditional --------------" << std::endl;
//
// CalibrationPointsSquaresFunction optimizationFunction(&transformationSet, &coords);
//
//
// vnl_vector<double> x0(11);
// x0.fill(1.0);
// vnl_vector<double> x1 = x0.as_ref();
//
// x1.fill(1.0);
//
// vnl_levenberg_marquardt LM(optimizationFunction);
// LM.set_verbose(TRUE);
//
// LM.set_f_tolerance(10e-10);
// LM.set_x_tolerance(10e-10);
//
// // max iterations 5000
// LM.set_max_function_evals(5000);
//
// bool okOptimization = false;
//
// try
// {
// okOptimization = LM.minimize(x1);
// }
// catch (std::exception& e)
// {
// qCritical() << "Exception thrown:" << e.what();
// }
//
// LM.diagnose_outcome(std::cout);
// cout << "x1 = " << x1 << endl;
}
void AppClass::ProcessKeyboard(void)
{
bool bModifier = false;
float fSpeed = 0.01f;
#pragma region ON PRESS/RELEASE DEFINITION
static bool bLastF1 = false, bLastF2 = false, bLastF3 = false, bLastF4 = false, bLastF5 = false,
bLastF6 = false, bLastF7 = false, bLastF8 = false, bLastF9 = false, bLastF10 = false,
bLastEscape = false;
#define ON_KEY_PRESS_RELEASE(key, pressed_action, released_action){ \
bool pressed = sf::Keyboard::isKeyPressed(sf::Keyboard::key); \
if(pressed){ \
if(!bLast##key) pressed_action;}/*Just pressed? */\
else if(bLast##key) released_action;/*Just released?*/\
bLast##key = pressed; } //remember the state
#pragma endregion
#pragma region Modifiers
if(sf::Keyboard::isKeyPressed(sf::Keyboard::LShift) || sf::Keyboard::isKeyPressed(sf::Keyboard::RShift))
bModifier = true;
#pragma endregion
if (sf::Keyboard::isKeyPressed(sf::Keyboard::R))
{
m_v3Orientation = vector3(0.0f);
something = quaternion(vector3(0.0f, 0.0f, 0.0f));
}
if (sf::Keyboard::isKeyPressed(sf::Keyboard::X))
{
if (!bModifier) {
m_v3Orientation.x += 1.0f;
something = something * quaternion(vector3(PI * 0.01f, 0, 0));
}
else {
m_v3Orientation.x -= 1.0f;
something = something * quaternion(-vector3(PI * 0.01f, 0, 0));
}
}
if (sf::Keyboard::isKeyPressed(sf::Keyboard::Y))
{
if (!bModifier) {
m_v3Orientation.y += 1.0f;
something = something * quaternion(vector3(PI * 0.01f, 0, 0));
}
else {
m_v3Orientation.y -= 1.0f;
something = something * quaternion(-vector3(0.0f, -PI * 0.01f, 0));
}
}
if (sf::Keyboard::isKeyPressed(sf::Keyboard::Z))
{
if (!bModifier) {
m_v3Orientation.z += 1.0f;
something = something * quaternion(vector3(PI * 0.01f, 0, 0));
}
else {
m_v3Orientation.z -= 1.0f;
something = something * quaternion(-vector3(0, 0, -PI * 0.01f));
}
}
#pragma region Camera Positioning
if(bModifier)
fSpeed *= 10.0f;
if(sf::Keyboard::isKeyPressed(sf::Keyboard::W))
m_pCameraMngr->MoveForward(fSpeed);
if(sf::Keyboard::isKeyPressed(sf::Keyboard::S))
m_pCameraMngr->MoveForward(-fSpeed);
if(sf::Keyboard::isKeyPressed(sf::Keyboard::A))
m_pCameraMngr->MoveSideways(-fSpeed);
if(sf::Keyboard::isKeyPressed(sf::Keyboard::D))
m_pCameraMngr->MoveSideways(fSpeed);
m_pCameraMngr->CalculateView();
#pragma endregion
#pragma region Other Actions
ON_KEY_PRESS_RELEASE(Escape, NULL, PostMessage(m_pWindow->GetHandler(), WM_QUIT, NULL, NULL))
#pragma endregion
}
fcl::Transform3f toFCLTransform() const {
fcl::Vec3f pose(stateVars[0], stateVars[1], stateVars[2]);
fcl::Quaternion3f quaternion(stateVars[3], stateVars[4], stateVars[5], stateVars[6]);
quaternion = math::normalize(quaternion);
return fcl::Transform3f(quaternion, pose);
}
SO3 Sim3
::so3() const
{
return SO3(quaternion());
}