bool Camera2::on_event(Event const& e)
{
switch (e.type) {
case MOUSE_PRESS:
{
if (e.button != MOUSE_RIGHT)
return false;
m_grab_position = e.pos;
m_translation_at_grab = m_translation;
m_dragging = true;
return true;;
}
case MOUSE_SCROLL:
{
scale(pow(1.1, e.delta));
return true;
}
case MOUSE_RELEASE:
{
if (e.button == MOUSE_RIGHT)
{
m_dragging = false;
return true;
}
return false;
}
case MOUSE_MOVE:
{
if (!m_dragging)
return false;
set_translation(m_translation_at_grab + canvas2worldv(e.pos - m_grab_position));
return true;
}
case KEY_PRESS:
{
switch (e.key) {
case 'q':
{
rotate(0.1f);
return true;
}
case 'e':
{
rotate(-0.1f);
return true;
}}
return false;
}
case RESIZE:
{
set_size(e.size);
return true;
}
default:
{
return false;
}}
}
void CameraObject::orbit (DTfloat angle_x, DTfloat angle_y, const Vector3 &around_pt)
{
Matrix3 roty, rotx;
roty = Matrix3::set_rotation_y(angle_y);
rotx = Matrix3::set_rotation_x(angle_x);
Matrix3 o = orientation();
Vector3 offset = o.inversed() * (translation() - around_pt);
o = o * roty * rotx;
set_translation(o * offset + around_pt);
set_orientation(o);
}
template<class TV> static Matrix<T,TV::m+1> affine_register_helper(RawArray<const TV> X0, RawArray<const TV> X1, RawArray<const T> mass) {
typedef typename TV::Scalar T;
static const int d = TV::m;
const int n = X0.size();
GEODE_ASSERT(n==X1.size() && (mass.size()==0 || mass.size()==n));
const bool weighted = mass.size() == n;
const T total = weighted ? mass.sum() : n;
if (!total)
return Matrix<T,d+1>::identity_matrix();
// Compute centers of mass
TV c0, c1;
if (weighted) {
for (int i=0;i<n;i++) {
c0 += mass[i]*X0[i];
c1 += mass[i]*X1[i];
}
} else {
c0 = X0.sum();
c1 = X1.sum();
}
c0 /= total;
c1 /= total;
// Compute covariances
SymmetricMatrix<T,d> cov00 = scaled_outer_product(-total,c0);
Matrix<T,d> cov01 = outer_product(-total*c1,c0);
if (weighted)
for(int i=0;i<n;i++) {
cov00 += scaled_outer_product(mass[i],X0[i]);
cov01 += outer_product(mass[i]*X1[i],X0[i]);
}
else
for (int i=0;i<n;i++) {
cov00 += outer_product(X0[i]);
cov01 += outer_product(X1[i],X0[i]);
}
// Compute transform
const Matrix<T,d> A = cov01*cov00.inverse();
const TV t = c1-A*c0;
auto tA = Matrix<T,d+1>::from_linear(A);
tA.set_translation(t);
return tA;
}
//
// Given the goal matrix and the projection axis, find the position
// of the end effector and the equation of the circle that defines
// how the R joint can swivel.
//
// Also compute the matrix S*RY*T and save it for future computations
//
int SRS::SetGoal(const Matrix GG, float &rangle)
{
Matrix RY;
float s[3];
cpmatrix(G, GG);
get_translation(G, ee);
get_translation(T, p_r1);
get_translation(S, s);
if (project_to_workspace && scale_goal(p_r1,s,ee))
set_translation(G,ee);
EvaluateCircle(ee);
//radius = get_circle_equation(ee, proj_axis, pos_axis,
// upper_len, lower_len, c, u, v, n);
//
// Build rotation matrix about the R joint
//
if (!solve_R_angle(ee, s, p_r1, T, r_angle))
return 0;
r_angle=-r_angle;
rangle = r_angle;
// Find RY, and store the positions of the R jt and
// the ee in the R1 frame as p_r1 and ee_r1
// Also save matrix product S*RY*T
rotation_principal_axis_to_matrix('y',-r_angle, RY);
hmatmult(SRT, S, RY);
hmatmult(SRT, SRT, T);
get_translation(SRT, ee_r1);
#ifdef SRSDEBUG
printf("EE distance error is %lf\n", norm(ee_r1) - norm(ee));
#endif
return 1;
}
//----------------------------------------------------------------------------------------------
Matrix::Matrix(const Quaternion& rot, const Vector& pos)
{
set_rot(rot);
set_translation(pos);
}
