/**
* Berechnet die neue Kamerarichtung mithilfe des Quaternions aus der HMD.
*/
void updateCameraPosition (void)
{
if (1) {
int i;
Vector3d axis={0.0,0.0,0.0};
Vector3d vector={0.0,0.0,0.0};
double angle=0.0;
G_center[0]=0.0;
G_center[1]=0.0;
G_center[2] = -1.0;
G_up[0] = 0.0;
G_up[1] = 1.0;
G_up[2] = 0.0;
/* Yaw */
for (i=0;i<3;i++) axis[i]=G_up[i];
for (i=0;i<3;i++) vector[i]=G_center[i];
angle = -g_rotationAngleY;
rotateVectorVector3d (&vector, axis, angle);
for (i=0;i<3;i++) G_center[i]=vector[i];
/* Pitch */
crossProduct3D (axis, G_up, G_center);
angle = g_rotationAngleX;
for (i=0;i<3;i++) vector[i]=G_up[i];
rotateVectorVector3d(&vector, axis, angle);
for (i=0;i<3;i++) G_up[i]=vector[i];
for (i=0;i<3;i++) vector[i]=G_center[i];
rotateVectorVector3d(&vector, axis, angle);
for (i=0;i<3;i++) G_center[i]=vector[i];
/* Roll */
for (i=0;i<3;i++) axis[i] = G_center[i]-G_eye[i];
for (i=0;i<3;i++) vector[i] = G_up[i];
angle = -g_rotationAngleZ;
rotateVectorVector3d(&vector, axis, angle);
for (i=0;i<3;i++) G_up[i]=vector[i];
} else {
G_center[0] = 0.0;
G_center[1] = 0.0;
G_center[2] = -1.0;
G_up[0] = 0.0;
G_up[1] = 1.0;
G_up[2] = 0.0;
cameraCalcYprToQuaternion ();
}
}
bool
ObjectModelPlaneFromLines::computeModelCoefficients (const std::vector<int> &sample,
Eigen::VectorXf &modelCoefficients){
line line1,line2;
//Create line1 from sample[0] and sample[1]
line1.pbase = (*inputPointCloud->getPointCloud())[sample[0]];
line1.director.resize(3);
line1.director[0]=(*inputPointCloud->getPointCloud())[sample[1]].getX() - (*inputPointCloud->getPointCloud())[sample[0]].getX();
line1.director[1]=(*inputPointCloud->getPointCloud())[sample[1]].getY() - (*inputPointCloud->getPointCloud())[sample[0]].getY();
line1.director[2]=(*inputPointCloud->getPointCloud())[sample[1]].getZ() - (*inputPointCloud->getPointCloud())[sample[0]].getZ();
//Create line1 from sample[2] and sample[3]
line2.pbase = (*inputPointCloud->getPointCloud())[sample[2]];
line2.director.resize(3);
line2.director[0]=(*inputPointCloud->getPointCloud())[sample[3]].getX() - (*inputPointCloud->getPointCloud())[sample[2]].getX();
line2.director[1]=(*inputPointCloud->getPointCloud())[sample[3]].getY() - (*inputPointCloud->getPointCloud())[sample[2]].getY();
line2.director[2]=(*inputPointCloud->getPointCloud())[sample[3]].getZ() - (*inputPointCloud->getPointCloud())[sample[2]].getZ();
//Compute the model co-efficient of the lines from these lines
std::vector<double> normal;
normal.resize(3);
crossProduct3D(line1.director,line2.director,normal);
modelCoefficients.resize(4);
modelCoefficients[0] = normal[0];
modelCoefficients[1] = normal[1];
modelCoefficients[2] = normal[2];
modelCoefficients[3]= -modelCoefficients[0] * line1.pbase.getX() -
modelCoefficients[1] * line1.pbase.getY() -
modelCoefficients[2] * line1.pbase.getZ();
if (abs(modelCoefficients[0])<geometryEpsilon&&abs(modelCoefficients[1])<geometryEpsilon&&abs(modelCoefficients[2])<geometryEpsilon) {
//Lines are parallel
if (contains(line1, line2.pbase))
return false;
//Use a line's director vector and both pBase's difference to create the plane.
std::vector<double> baseDifference;
baseDifference.resize(3);
baseDifference[0]=line1.pbase.getX()-line2.pbase.getX();
baseDifference[1]=line1.pbase.getY()-line2.pbase.getY();
baseDifference[2]=line1.pbase.getZ()-line2.pbase.getZ();
crossProduct3D(line1.director,baseDifference,normal);
modelCoefficients[0] = normal[0];
modelCoefficients[1] = normal[1];
modelCoefficients[2] = normal[2];
modelCoefficients[3]=-modelCoefficients[0] * line1.pbase.getX() -modelCoefficients[1] * line1.pbase.getY() -modelCoefficients[2] * line1.pbase.getZ();
} else {
double x = modelCoefficients[0]*line2.pbase.getX()+
modelCoefficients[1]*line2.pbase.getY()+
modelCoefficients[2]*line2.pbase.getZ()+modelCoefficients[3];
if (abs(x)>=geometryEpsilon) {
cout<<"Lines do not intersect"<<endl;
return false;
}
}
return true;
}
/**
* Erstellt aus den Drehungen Yaw, Pitch, Roll ein (mehrere) Quaternionen
* und wendet diese passend auf den Up- und Center-Vector an.
*/
void cameraCalcYprToQuaternion (void)
{
Quaternion qr = malloc (sizeof(*qr));
Quaternion qp = malloc (sizeof(*qp));
Quaternion qy = malloc (sizeof(*qy));
Quaternion qres = malloc (sizeof(*qres));
Quaternion p = malloc (sizeof (*p));
Vector3d upTmp = {0.0,0.0,0.0};
Vector3d centerTmp = {0.0,0.0,0.0};
Vector3d axis = {0.0,0.0,0.0};
int i;
/* Up- und Centervector zwischenspeichern */
for (i=0;i<3;i++) upTmp[i] = G_up[i];
for (i=0;i<3;i++) centerTmp[i] = G_center[i];
printf ("logic up: %f, %f, %f\n", G_up[0],G_up[1],G_up[2]);
/* Up-Vector */
/* Axis für qr (roll) */
for (i=0;i<3;i++) axis[i] = upTmp[i];
/* qr erstellen */
qr->s = cos ((g_rotationAngleZ/2.0) * PI / 180.0);
printf ("qr->s = %f\n", qr->s);
for (i=0;i<3;i++) qr->v[i] = sin ((g_rotationAngleZ/2.0) * PI / 180.0) * axis[i];
/* Axis für qp (pitch) */
crossProduct3D (axis, upTmp, centerTmp);
printVector (axis);
printf("g_rotationAngle... %f\n", g_rotationAngleX);
/* qp erstellen */
qp->s = cos ((g_rotationAngleX/2.0) * PI / 180.0);
printf ("pq->s : %f\n", cos ((g_rotationAngleX/2.0) * PI / 180.0));
for (i=0;i<3;i++) qp->v[i] = sin ((g_rotationAngleX/2.0) * PI / 180.0) * axis[i];
printVector (qp->v);
qr = normQuaternion (qr);
qp = normQuaternion (qp);
/* qr * qp */
qres = multQuaterionQuaterion (qr, qp);
/* p erstellen */
p->s = 0.0;
for (i=0;i<3;i++) p->v[i] = upTmp[i];
qres = normQuaternion (qres);
p = normQuaternion (p);
/* q * p * q* */
qres = multQuaterionQuaterion (multQuaterionQuaterion (qres, p), inverseQuaternion (qres));
qres = normQuaternion (qres);
/* up sichern */
for (i=0;i<3;i++) G_up[i] = qres->v[i];
printf ("logic up: %f, %f, %f\n", G_up[0],G_up[1],G_up[2]);
/*exit(1);*/
/* Center-Vector */
/* Axis für qp (pitch) */
crossProduct3D (axis, upTmp, centerTmp);
/* qp erstellen */
qp->s = cos ((g_rotationAngleX/2.0) * PI / 180.0);
for (i=0;i<3;i++) qp->v[i] = sin ((g_rotationAngleX/2.0) * PI / 180.0) * axis[i];
/* qy erstellen */
qy->s = cos ((g_rotationAngleY/2.0) * PI / 180.0);
for (i=0;i<3;i++) qy->v[i] = sin ((g_rotationAngleY/2.0) * PI / 180.0) * upTmp[i];
qy = normQuaternion (qr);
qp = normQuaternion (qp);
/* qp * qy */
qres = multQuaterionQuaterion (qp, qy);
qres = normQuaternion (qres);
/* p erstellen */
p->s = 0.0;
for (i=0;i<3;i++) p->v[i] = centerTmp[i];
qres = normQuaternion (qres);
p = normQuaternion (p);
/* q * p * p* */
qres = multQuaterionQuaterion (multQuaterionQuaterion (qres, p), inverseQuaternion (qres));
qres = normQuaternion (qres);
/* center sichern */
for (i=0;i<3;i++) G_center[i] = qres->v[i];
}
