void UpdateCoords(vector<Vec3f>& v, Vec3f position, Vec3f velocity, Vec4f rotation, Vec4f wvelocity, float dt, float scale, bool pers, float inter, float xScale, float yScale, float zFar, float zNear){
float angle = rotation[0];
angle += wvelocity[0]*dt*inter;
Vector3f newPos = Vector3f(position[0] + velocity[0]*dt*inter, position[1] + velocity[1]*dt*inter, position[2] + velocity[2]*dt*inter);
Vector3f axis(rotation[1], rotation[2], rotation[3]);
axis.normalize();
Translation3f move =Translation3f(newPos.x(), newPos.y(), newPos.z());
angle = angle*2*M_PI/360;
AngleAxisf turn = AngleAxisf(angle, axis);
Matrix4f pmat;
if(pers){
pmat << xScale, 0, 0, 0,
0, yScale, 0, 0,
0, 0, -(zFar+zNear)/(zFar-zNear), -1,
0, 0, -2*zNear*zFar/(zFar-zNear), 0;
}else{
pmat = Matrix4f::Identity();
}
for(unsigned int boxV = 0; boxV < v.size(); boxV++){
Vector3f p = Vector3f(v[boxV][0], v[boxV][1], v[boxV][2]);
/*Vector4f pt(p.x(), p.y(), p.z(), 1);
pt = pmat*pt;
p = Vector3f(pt.x(), pt.y(), pt.z());*/
p = scale* p;
if(axis.norm() > 0)
p = turn* p;
p = move* p;
v[boxV] = Vec3f(p.x(), p.y(), p.z());
}
}
void GetTransformedPoint(Vec3f& pt, Vec3f position, Vec3f velocity, Vec4f rotation, Vec4f wvelocity, float dt, float scale = 1.f, float inter = 1.0){
float angle = rotation[0];
angle += wvelocity[0]*dt*inter;
Vector3f newPos = Vector3f(position[0] + velocity[0]*dt*inter, position[1] + velocity[1]*dt*inter, position[2] + velocity[2]*dt*inter);
Vector3f axis(rotation[1], rotation[2], rotation[3]);
axis.normalize();
Translation3f move =Translation3f(newPos.x(), newPos.y(), newPos.z());
angle = angle*2*M_PI/360;
AngleAxisf turn = AngleAxisf(angle, axis);
Vector3f p = Vector3f(pt[0], pt[1], pt[2]);
p = p*scale;
p = turn*p;
p = move*p;
pt = Vec3f(p.x(), p.y(), p.z());
}
Matrix4f Model::getModelMatrix() const {
Matrix4f M;
Matrix3f R = rotation.toRotationMatrix();
M.setIdentity();
// then rotate
M.block<3,3>(0,0) = R;
// then scale
M = M * Scaling(Vector4f(scale, scale, scale, 1));
// translate to center first
Affine3f a(Translation3f(-center));
M = M * a.matrix();
// finally set position
M.block<3,1>(0,3) += position;
return M;
}