void Camera::matrixByAxisCamera(QMatrix4x4& result, const QVector3D& pos,
const QVector3D& vRightNormalized,
const QVector3D& vUpNormalized) const
{
QVector3D right = vRightNormalized;
QVector3D up = vUpNormalized;
QVector3D dir = QVector3D::crossProduct(up, right);
result.setToIdentity();
result.setRow(0, right);
result.setRow(1, dir);
result.setRow(2, up);
result.translate( -pos );
}
QVector4D Cylinder::surfaceNormal(const QVector4D &p, const Ray &ray)
{
QVector4D lp = m_worldToObject * p;
QMatrix4x4 a = m_worldToObject.transposed();
a.setRow(3, QVector4D(0.0, 0.0, 0.0, 1.0));
if (m_hasCaps && fabs(fabs(lp.y()) - m_length * 0.5) < MathUtils::dEpsilon) {
// Return cap normal
QVector4D n;
if (lp.y() < 0.0)
n = QVector4D(0.0, -1.0, 0.0, 0.0);
else
n = QVector4D(0.0, 1.0, 0.0, 0.0);
return (a * n).normalized();
} else {
// calculate cylinder normal
QVector4D o = QVector4D(0.0, lp.y(), 0.0, 1.0);
QVector4D n = lp - o;
n = a * n;
QVector4D rd = -ray.direction();
if (QVector4D::dotProduct(n, rd) < 0)
return -n.normalized();
else
return n.normalized();
}
}
QVector4D Cone::surfaceNormal(const QVector4D &p, const Ray &ray)
{
QVector4D objectP = m_worldToObject * p;
QVector4D n = fabs(objectP.y()) < MathUtils::dEpsilon ? QVector4D(0.0, -1.0, 0.0, 0.0) : // We're on the base disc
QVector4D(objectP.x(), 1.0 - objectP.y(), objectP.z(), 0.0);
QMatrix4x4 a = m_worldToObject.transposed();
a.setRow(3, QVector4D(0.0, 0.0, 0.0, 1.0));
n = a * n;
return n.normalized();
}
/**
* Get polygon oriented bounding box
**/
void Polygon3D::getLoopOBB(Loop3D &pin, QVector3D &size, QMatrix4x4 &xformMat){
float alpha = 0.0f;
float deltaAlpha = 0.05*3.14159265359f;
float bestAlpha;
Loop3D rotLoop;
QMatrix4x4 rotMat;
QVector3D minPt, maxPt;
QVector3D origMidPt;
QVector3D boxSz;
QVector3D bestBoxSz;
float curArea;
float minArea = FLT_MAX;
rotLoop = pin;
Polygon3D::getLoopAABB(rotLoop, minPt, maxPt);
origMidPt = 0.5f*(minPt + maxPt);
//while(alpha < 0.5f*_PI){
int cSz = pin.size();
QVector3D difVec;
for(int i=0; i<pin.size(); ++i){
difVec = (pin.at((i+1)%cSz) - pin.at(i)).normalized();
alpha = atan2(difVec.x(), difVec.y());
rotMat.setToIdentity();
rotMat.rotate(57.2957795f*(alpha), 0.0f, 0.0f, 1.0f);//57.2957795 rad2degree
transformLoop(pin, rotLoop, rotMat);
boxSz = Polygon3D::getLoopAABB(rotLoop, minPt, maxPt);
curArea = boxSz.x() * boxSz.y();
if(curArea < minArea){
minArea = curArea;
bestAlpha = alpha;
bestBoxSz = boxSz;
}
//alpha += deltaAlpha;
}
xformMat.setToIdentity();
xformMat.rotate(57.2957795f*(bestAlpha), 0.0f, 0.0f, 1.0f);//57.2957795 rad2degree
xformMat.setRow(3, QVector4D(origMidPt.x(), origMidPt.y(), origMidPt.z(), 1.0f));
size = bestBoxSz;
}//
void PSMoveForm::parseMoveData(MoveData d)
{
setButtons(d.buttons);
ui->tempLcdNumber->display(d.temperature);
if (d.battery <= 5) {
for (int i = 0; i < 5; i++) {
mBatteryCheckBoxes[i].setChecked(i < d.battery);
}
mBatteryLayout->setCurrentIndex(0);
} else {
mBatteryLayout->setCurrentIndex(1);
}
//emit setRumble(d.buttons.trigger);
/*const QVector3D acc = d.accelerometer.normalized() * 128 + QVector3D(128, 128, 128);
const QColor rgb(acc.x(), acc.y(), acc.z());
static int w = 0;
if (w++ == 10) {
w = 0;
emit setRgb(rgb);
}*/
//QVector3D normalisedAcc = d.accelerometer / 32768;
//QVector3D normalisedMag = d.mag / 32768;
//normalisedAcc.normalize();
//normalisedMag = QVector3D::crossProduct(normalisedMag.normalized(), normalisedAcc).normalized();
//QVector3D north = QVector3D::crossProduct(normalisedMag, normalisedAcc).normalized();
bool movePressedB = d.buttons.buttonsPressed.contains(MoveButtons::Move);
if (!mPrevMovePressed && movePressedB) {
//mOne = normalisedAcc;
//mTwo = normalisedMag;
//mThree = north;
qDebug() << "acc:" << d.accelerometer;
qDebug() << "mag:" << d.mag;
emit movePressed();
}
mPrevMovePressed = movePressedB;
/*float one = std::acos(QVector3D::dotProduct(normalisedAcc, mOne));
float two = std::acos(QVector3D::dotProduct(normalisedMag, mTwo));
float three = std::acos(QVector3D::dotProduct(north, mThree));*/
//qDebug() << QString::number(one, 'g', 4) << QString::number(two, 'g', 4) << QString::number(three, 'g', 4);
/*ui->accXProgressBar->setValue(one * 180 / M_PI);
ui->accYProgressBar->setValue(two * 180 / M_PI);
ui->accZProgressBar->setValue(three * 180 / M_PI);*/
if (ui->accGroupBox->isChecked()) {
ui->accXProgressBar->setValue(d.accelerometer.x());
ui->accYProgressBar->setValue(d.accelerometer.y());
ui->accZProgressBar->setValue(d.accelerometer.z());
}
if (ui->gyroGroupBox->isChecked()) {
ui->gyroXProgressBar->setValue(d.gyro.x());
ui->gyroYProgressBar->setValue(d.gyro.y());
ui->gyroZProgressBar->setValue(d.gyro.z());
}
if (ui->magGroupBox->isChecked()) {
ui->magXProgressBar->setValue(d.mag.x());
ui->magYProgressBar->setValue(d.mag.y());
ui->magZProgressBar->setValue(d.mag.z());
}
bool triPressed = d.buttons.buttonsPressed.contains(MoveButtons::Triangle);
if (triPressed && !mPrevTriPressed) {
emit setTopRightCorner();
}
mPrevTriPressed = triPressed;
bool squPressed = d.buttons.buttonsPressed.contains(MoveButtons::Square);
if (squPressed && !mPrevSquPressed) {
emit setBottomLeftCorner();
}
mPrevSquPressed = squPressed;
#if 0
QVector3D acc = d.accelerometer;
QVector3D mag = d.mag;
acc.normalize();
mag.normalize();
QMatrix4x4 m;
m.setRow(2, acc);
QVector3D east = QVector3D::crossProduct(acc, -mag);
m.setRow(0, east);
QVector3D north = QVector3D::crossProduct(acc, -mag);
m.setRow(1, north);
//qDebug() << mag;
// QVector3D gyro = d.gyro / 32768;
// acc.setX(mAccFilters[0]->step(acc.x()));
// acc.setY(mAccFilters[1]->step(acc.y()));
// acc.setZ(mAccFilters[2]->step(acc.z()));
// mag.setX(mMagFilters[0]->step(mag.x()));
// mag.setY(mMagFilters[1]->step(mag.y()));
// mag.setZ(mMagFilters[2]->step(mag.z()));
QVector3D newY = acc;
QVector3D newZ = mag;
newZ.setZ(-newZ.z());
QVector3D newX = QVector3D::crossProduct(newZ, newY).normalized();
QMatrix4x4 newMat;
newMat.setRow(0, newX);
newMat.setRow(1, newY);
newMat.setRow(2, newZ);
newMat.setRow(3, QVector4D(0, 0, 0, 1));
float qq1 = q0;
float qq2 = q1;
float qq3 = q2;
float qq4 = q3;
QQuaternion quat(qq1, qq2, qq3, qq4);
QVector3D vec(1, 1, 1);
emit setMatrix(newMat);
//emit setVector(quat.rotatedVector(vec));
#endif
}
