const char* sequence(const char* src) {
const char* rslt = src;
(rslt = mx1(rslt)) && (rslt = mx2(rslt)) &&
(rslt = mx3(rslt)) && (rslt = mx4(rslt)) &&
(rslt = mx5(rslt)) && (rslt = mx6(rslt));
return rslt;
}
const char* alternatives(const char* src) {
const char* rslt = src;
(rslt = mx1(rslt)) || (rslt = mx2(rslt)) ||
(rslt = mx3(rslt)) || (rslt = mx4(rslt)) ||
(rslt = mx5(rslt)) || (rslt = mx6(rslt)) ||
(rslt = mx7(rslt)) || (rslt = mx8(rslt));
return rslt;
}
void sphere::contactSphere(const sphere& s, vec3& v, float& f) const
{
vec3 mn1(c - r), mx1(c + r);
vec3 mn2(s.c - s.r), mx2(s.c + s.r);
if(mn2 < mn1) mn1 = mn2;
if(mx2 > mx1) mx1 = mx2;
v = vec3((mx1 + mn1) / 2.0f);
mn1 = (mx1 - mn1) / 2.0f;
f = max(mn1.z, max(mn1.x, mn1.y));
}
MainWindow::MainWindow(QWidget* _parent)
:QMainWindow(_parent),
m_settings(new Settings()),
m_ui(new Ui::MainWindow()),
m_copterCtrl(),
m_tcpServer(),
m_tcpConnection(),
m_accelerometerInputFd(-1),
m_accelerometerInputNotifier(0),
m_lastTiltX(0),
m_lastTiltY(0)
{
m_ui->setupUi(this);
const auto s_ctrl_path = m_settings->getControlPath();
QSharedPointer<CopterMotor> mx1(new CopterMotor(m_settings, s_ctrl_path+"ehrpwm.0/pwm/ehrpwm.0:0/duty_percent", m_ui->motor_x1));
QSharedPointer<CopterMotor> mx2(new CopterMotor(m_settings, s_ctrl_path+"ehrpwm.0/pwm/ehrpwm.0:1/duty_percent", m_ui->motor_x2));
QSharedPointer<CopterMotor> my1(new CopterMotor(m_settings, s_ctrl_path+"ehrpwm.1/pwm/ehrpwm.1:0/duty_percent", m_ui->motor_y1));
QSharedPointer<CopterMotor> my2(new CopterMotor(m_settings, s_ctrl_path+"ehrpwm.1/pwm/ehrpwm.1:1/duty_percent", m_ui->motor_y2));
QSharedPointer<CopterAxis> m_axisX(new CopterAxis(mx1, mx2));
QSharedPointer<CopterAxis> m_axisY(new CopterAxis(my1, my2));
m_copterCtrl = new CopterCtrl(m_settings, m_axisX, m_axisY, m_ui->motor_all);
m_tcpServer.listen(QHostAddress::Any, m_settings->getTcpPort());
connect(&m_tcpServer, SIGNAL(newConnection()), this, SLOT(onConnection()));
auto const s_accel_input_path = m_settings->getAccelInputPath();
m_accelerometerInputFd = ::open(s_accel_input_path.toLatin1().data(), O_SYNC, O_RDONLY);
if (m_accelerometerInputFd == -1)
qDebug() << "Cannot open accelerometer input file " << s_accel_input_path << ", reason: " << errno;
m_accelerometerInputNotifier = new QSocketNotifier(m_accelerometerInputFd, QSocketNotifier::Read, this);
connect(m_accelerometerInputNotifier, SIGNAL(activated(int)), this, SLOT(onAccelerometerRead()));
m_accelerometerInputNotifier->setEnabled(true);
m_copterCtrl->adjustPower(0);
showFullScreen();
showMaximized();
}
int
main()
{
JMatrix mx1(2,2),mx2(2,2); // constructor
cout << "mx1 is " << mx1.GetRowCount() << " x " << mx1.GetColCount() << endl;
cout << "mx1 should be 0 0, 0 0" << endl;
cout << "mx1 = " << mx1 << endl;
mx1 = JIdentityMatrix(2);
mx2.SetElement(1,1, 0.0);
mx2.SetElement(1,2, 1.0);
mx2.SetElement(2,1, 1.0);
mx2.SetElement(2,2, 0.0);
cout << "mx1 should be 1 0, 0 1" << endl;
cout << "mx1 = " << mx1 << endl;
cout << "mx2 should be 0 1, 1 0" << endl;
cout << "mx2 = " << mx2 << endl;
JWaitForReturn();
JMatrix mx3 = -mx2; // copy constructor
cout << "mx3 should be 0 -1, -1 0" << endl;
cout << "mx3 = " << mx3 << endl;
mx3 += mx1;
JMatrix mx4 = mx1 - mx2;
cout << "mx4 should be 1 -1, -1 1" << endl;
cout << "mx4 = " << mx4 << endl;
cout << "mx3 should equal mx4" << endl;
cout << "mx3 equals mx4? " << (mx3 == mx4) << endl;
mx4 /= 0.5;
cout << "mx4 should be 2 -2, -2 2" << endl;
cout << "mx4 = " << mx4 << endl;
mx4.SetAllElements(1.0);
mx4.SetElement(1,2, 0.0);
cout << "mx4 should be 1 0, 1 1" << endl;
cout << "mx4 = " << mx4 << endl;
JMatrix mx5 = mx4.Transpose();
cout << "mx5 should be 1 1, 0 1" << endl;
cout << "mx5 = " << mx5 << endl;
cout << "mx4 * mx5 should be 1 1, 1 2" << endl;
cout << "mx4 * mx5 = " << mx4 * mx5 << endl;
JWaitForReturn();
JVector v1 = mx4.GetColVector(2);
JVector v2 = mx4.GetRowVector(1);
cout << "v1 should be 0 1" << endl;
cout << "v1 = " << v1 << endl;
cout << "v1 dot v2 should be 0" << endl;
cout << "v1 dot v2 = " << JDotProduct(v1, v2) << endl;
cout << "mx4 * v1 should be 0, 1" << endl;
cout << "mx4 * v1 = " << mx4 * v1 << endl;
cout << "(v1)t * mx4 should be 1 1" << endl;
cout << "(v1)t * mx4 = " << v1.Transpose() * mx4 << endl;
cout << "Enter new mx5: ";
cin >> mx5;
JInputFinished();
cout << "mx5 = " << mx5 << endl;
JWaitForReturn();
JMatrix mx6(3,3), mx7(3,3);
mx6.SetElement(1,1, 1.0);
mx6.SetElement(2,2, 2.0);
mx6.SetElement(3,3, 3.0);
cout << "det(mx6) should be 6" << endl;
cout << "det(mx6) = " << mx6.Determinant() << endl;
assert( mx6.Invert(&mx7) );
cout << "mx7 should be 1 0 0, 0 0.5 0, 0 0 0.333333" << endl;
cout << "mx7 = " << mx7 << endl;
mx6.SetElement(1,1, 1.0);
mx6.SetElement(1,2, 2.0);
mx6.SetElement(1,3, 3.0);
mx6.SetElement(2,1, 4.0);
mx6.SetElement(2,2, 5.0);
mx6.SetElement(2,3, 6.0);
mx6.SetElement(3,1, 2.0);
mx6.SetElement(3,2, 3.0);
mx6.SetElement(3,3, 4.0);
cout << endl;
cout << "mx6 should be 1 2 3, 4 5 6, 2 3 4" << endl;
cout << "mx6 = " << mx6 << endl;
cout << "det(mx6) should be 0" << endl;
cout << "det(mx6) = " << mx6.Determinant() << endl;
assert( !mx6.Invert(&mx7) );
cout << "mx6 was not invertible (correct)" << endl;
mx6.SetElement(1,1, 1.0);
mx6.SetElement(1,2, 0.5);
mx6.SetElement(1,3, -2.0);
mx6.SetElement(2,1, 1.0/3.0);
mx6.SetElement(2,2, 5.0);
mx6.SetElement(2,3, -7.5);
mx6.SetElement(3,1, 0.0);
mx6.SetElement(3,2, 1.0);
mx6.SetElement(3,3, -3.0);
cout << endl;
assert( mx6.Invert(&mx7) );
cout << "det(mx6) should be 1/det(mx7) should be -7.666667" << endl;
cout << mx6.Determinant() << " ?= " << 1.0/mx7.Determinant() << endl;
cout << "A*A-1 should get 1 0 0, 0 1 0, 0 0 1" << endl;
cout << "A*A-1 = " << mx6 * mx7 << endl;
cout << "A-1*A = " << mx7 * mx6 << endl;
mx6.SetElement(1,1, 2.0);
mx6.SetElement(1,2, -1.0);
mx6.SetElement(1,3, 1.0);
mx6.SetElement(2,1, 1.0);
mx6.SetElement(2,2, 3.0);
mx6.SetElement(2,3, 2.0);
mx6.SetElement(3,1, 5.0);
mx6.SetElement(3,2, 0.0);
mx6.SetElement(3,3, -2.0);
JVector b(3, 3.0, 13.0, -1.0);
JVector x(3);
cout << endl;
cout << "Solution should be: 1 2 3" << endl;
if (JGaussianElimination(mx6, b, &x))
{
cout << "Solution is: " << x << endl;
}
else
{
cout << "ERROR: mx6 should be invertible!" << endl;
}
mx6.SetElement(3,1, 3.0);
mx6.SetElement(3,2, 2.0);
mx6.SetElement(3,3, 3.0);
cout << endl;
cout << "Solution should not exist" << endl;
if (!JGaussianElimination(mx6, b, &x))
{
cout << "Solution does not exist" << endl;
}
else
{
cout << "Solution exists: " << x << endl;
cout << "(because of round off error)" << endl;
}
return 0;
}
const char* alternatives(const char* src) {
const char* rslt;
(rslt = mx1(src)) || (rslt = mx2(src)) || (rslt = mx3(src)) ||
(rslt = mx4(src)) || (rslt = mx5(src)) || (rslt = mx6(src));
return rslt;
}
const char* sequence(const char* src) {
const char* rslt = src;
if (!(rslt = mx1(rslt))) return 0;
return sequence<mx2, mxs...>(rslt);
}
const char* sequence(const char* src) {
const char* rslt = src;
if (!(rslt = mx1(rslt))) return 0;
return rslt;
}
const char* alternatives(const char* src) {
const char* rslt;
if ((rslt = mx1(src))) return rslt;
return alternatives<mx2, mxs...>(src);
}
