/** * 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]; }