Quat &Quat::operator= (const Vec3f &RV)
{
Float _w=RV.GetNorm();
if (_w<Float_Eps)
{
xyzw().SetDefault();
return *this;
}
Vec2f Toto;
SinCos(Toto,_w*0.5f);
xyz()=RV*(Toto.x/_w);
w=Toto.y;
return *this;
}
/*inline*/ void Quat::SetEular(const Vec4f &Eular)
{
Vec2f sincos;
Quat QWork;
// Rotation en Z.
Vec4f vSin = VEC4F_NULL;
Vec4f vCos = VEC4F_NULL;
Vec4f vHalfEular = VecFloatMul( Eular, VEC4F_HALF );
VecFloatSinCos( vSin, vCos, vHalfEular);
xyzw().Set( 0.f, 0.f, vSin.z, vCos.z );
// Rotation en Y.
QWork.xyzw().Set( 0.f, vSin.y, 0.f, vCos.y );
(*this) *= QWork;
// Rotation en X.
QWork.xyzw().Set( vSin.x, 0.f, 0.f, vCos.x );
(*this) *= QWork;
}
void shader_uniforms_think(struct shader *s, float delta_time)
{
glUseProgram(s->program);
for(int i=0; i<UNIFORMS_MAX; i++) {
struct uniform *u = s->uniforms[i];
if(u == NULL) {
continue;
}
switch(u->datatype) {
case TYPE_VEC_1F: {
glUniform1f(u->id, *((GLfloat *) u->data));
break;
}
case TYPE_VEC_2F: {
float *v = u->data;
glUniform2f(u->id, v[0], v[1]);
break;
}
case TYPE_VEC_3F: {
glUniform3f(u->id, xyz(((float *) u->data)));
break;
}
case TYPE_VEC_4F: {
glUniform4f(u->id, xyzw(((float *) u->data)));
break;
}
case TYPE_MAT_4F: {
glUniformMatrix4fv(u->id, 1, GL_FALSE, (float *) u->data);
break;
}
case TYPE_VEC_1I: {
glUniform1i(u->id, *((GLint*)u->data));
break;
}
default:
shader_debug("Unknown datatype for uniform \"%s\"\n", u->name);
break;
}
}
}
void Triangulator::triangulateFromVp(cv::Mat &vp, cv::Mat &xyz){
// Solve for xyzw using determinant tensor
cv::Mat C = determinantTensor;
std::vector<cv::Mat> xyzw(4);
for(unsigned int i=0; i<4; i++){
// xyzw[i].create(vp.size(), CV_32F);
xyzw[i] = C.at<float>(cv::Vec4i(i,0,1,1)) - C.at<float>(cv::Vec4i(i,2,1,1))*uc - C.at<float>(cv::Vec4i(i,0,2,1))*vc -
C.at<float>(cv::Vec4i(i,0,1,2))*vp + C.at<float>(cv::Vec4i(i,2,1,2))*vp.mul(uc) + C.at<float>(cv::Vec4i(i,0,2,2))*vp.mul(vc);
}
// Convert to non homogenous coordinates
for(unsigned int i=0; i<3; i++)
xyzw[i] /= xyzw[3];
// Merge
cv::merge(std::vector<cv::Mat>(xyzw.begin(), xyzw.begin()+3), xyz);
}
Quat::Quat(const Vec3f &V1,const Vec3f &V2)
{
Quat q;
// normer les 2 vecteurs et sortir si un vecteur est trop petit et qu'on a pas r�ussi � normer
Float n1=V1.GetNorm();
Float n2=V2.GetNorm();
if ((n1<Float_Eps) || (n2<Float_Eps))
{
// Error Quat !!!
xyzw().SetDefault();
Normalize();
return;
}
Vec3f v1=V1* (1.f/n1);
Vec3f v2=V2* (1.f/n2);
if (v1*v2 >= 0.f)
{
// Angle < 90
Vec3f v3=v1+v2;
v3.Normalize();
v = v1^v3;
Float Sin2 = v*v;
//Should be Float_Eps*Float_Eps?
if (Sin2 < Float_Eps)
{
// Pas de rotation
xyzw().SetDefault();
Normalize();
return;
}
Float tempf=1.f-Sin2;
if (tempf<0.f) tempf=0.f;
if (tempf>1.f) tempf=1.f;
w = Sqrt(tempf);
Normalize();
return;
}
// Angle > 90
// On travaille avec le vecteur oppos�.
Vec3f v3=v2-v1;
v3.CNormalize();
v = v1^v3;
Float Sin2 = v*v;
if (Sin2 < Float_Eps)
{
// Rotation 180.
Float x = Abs(v1.x);
Float y = Abs(v1.y);
Float z = Abs(v1.z);
if ((v1.y > -0.707106f) && (v1.y < 0.707106f))
// if ((y < x) && (y < z))
{
v3.x = v1.z;
v3.y = v1.y;
v3.z = v1.x;
}
else
{
v3.x = v1.z;
v3.y = v1.x;
v3.z = v1.y;
}
v = v1^v3;
v.CNormalize();
w=0.f;
Normalize();
return;
}
w = Sqrt(Sin2);
if (w)
v *= Sqrt(1.f-Sin2) / w;
Normalize();
}
