void AtomicModel::write(std::ostream& out) const
{
out << "<model name=\"" << getName().c_str() << "\" "
<< "type=\"atomic\" ";
if (not conditions().empty()) {
out << "conditions=\"";
std::vector < std::string >::const_iterator it =
conditions().begin();
while (it != conditions().end()) {
out << it->c_str();
++it;
if (it != conditions().end()) {
out << ",";
}
}
out << "\" ";
}
out << "dynamics=\"" << dynamics().c_str() << "\" ";
if (not observables().empty()) {
out << "observables=\"" << observables().c_str() << "\" ";
}
writeGraphics(out);
out << ">\n";
writePort(out);
out << "</model>\n";
}
void AtomicModel::updateDynamics(const std::string& oldname,
const std::string& newname)
{
if (dynamics() == oldname) {
setDynamics(newname);
}
}
void AtomicModel::purgeDynamics(const std::set < std::string >&
dynamicslist)
{
if (dynamicslist.find(dynamics()) == dynamicslist.end()) {
setDynamics("");
}
}
void SimObjBase::setRotation(const Rotation &r)
{
if (dynamics()) { return; }
const dReal *q = r.q();
setQ(q);
}
ExecuterWallBall(const vd::ExecutiveInit& init,
const vd::InitEventList& events)
: vd::Executive(init, events),mWallNumbers(0),mBallNumbers(0)
{
mView = "view1";
vp::Dynamic dyn("dyn_ball");
dyn.setPackage("vle.extension.mas");
dyn.setLibrary("BallG");
dynamics().add(dyn);
vp::Dynamic dynw("dyn_wall");
dynw.setPackage("vle.extension.mas");
dynw.setLibrary("WallG");
dynamics().add(dynw);
}
vec EnsembleCCE::cluster_evolution(int cce_order, int index)
{
vector<cSPIN> spin_list = _my_clusters.getCluster(cce_order, index);
Hamiltonian hami0 = create_spin_hamiltonian(_center_spin, _state_pair.first, spin_list);
Hamiltonian hami1 = create_spin_hamiltonian(_center_spin, _state_pair.second, spin_list);
vector<QuantumOperator> left_hm_list = riffle((QuantumOperator) hami0, (QuantumOperator) hami1, _pulse_num);
vector<QuantumOperator> right_hm_list;
if (_pulse_num % 2 == 0)
right_hm_list= riffle((QuantumOperator) hami1, (QuantumOperator) hami0, _pulse_num);
else
right_hm_list = riffle((QuantumOperator) hami0, (QuantumOperator) hami1, _pulse_num);
vector<double> time_segment = Pulse_Interval(_pulse_name, _pulse_num);
DensityOperator ds = create_spin_density_state(spin_list);
PiecewiseFullMatrixMatrixEvolution kernel(left_hm_list, right_hm_list, time_segment, ds);
kernel.setTimeSequence( _t0, _t1, _nTime);
ClusterCoherenceEvolution dynamics(&kernel);
dynamics.run();
return calc_observables(&kernel);
}
ConsistentVector Simulable::rk4(double& time, double dt,
const ConsistentVector& state,
const ConsistentVector& control) const {
// Formula from: http://mathworld.wolfram.com/Runge-KuttaMethod.html
checkStateSize(state);
checkControlSize(control);
const ConsistentVector& k1 = dynamics(time, state, control);
const ConsistentVector& k2 = dynamics(time + 0.5*dt, state + 0.5*dt*k1, control);
const ConsistentVector& k3 = dynamics(time + 0.5*dt, state + 0.5*dt*k2, control);
const ConsistentVector& k4 = dynamics(time + dt, state + dt*k3, control);
time += dt;
ConsistentVector result = state + RK4_INTEGRATION_CONSTANT*dt*(k1 + 2.0*(k2 + k3) + k4);
return result;
}
void update(float h) {
math::matrix2x1<float> yn1 = state + (3.0f/2.0f) * h * dynamics(state) - 0.5f * h * dynamics(state_1);
state_1 = state;
state = yn1;
}
/**
* @brief Set angular velocity of the entity
* @param x_ X element of angular velocity [rad/s]
* @param y_ Y element of angular velocity [rad/s]
* @param z_ Z element of angular velocity [rad/s]
*/
void SimObjBase::setAngularVelocity(double x_,double y_,double z_)
{
// Set is not permitted in dynamics on mode
if (!dynamics()) { return; }
avx(x_);
avy(y_);
avz(z_);
}
// Changed from setVelocity: by inamura on 2013-12-30
void SimObjBase::setLinearVelocity(double vx_,double vy_,double vz_)
{
// Set is not permitted in dynamics on mode
if (!dynamics()) { return; }
vx(vx_);
vy(vy_);
vz(vz_);
}
void BlockLocalPositionEstimator::predict()
{
// if can't update anything, don't propagate
// state or covariance
if (!_validXY && !_validZ) { return; }
if (integrate) {
matrix::Quaternion<float> q(&_sub_att.get().q[0]);
_eul = matrix::Euler<float>(q);
_R_att = matrix::Dcm<float>(q);
Vector3f a(_sub_sensor.get().accelerometer_m_s2);
// note, bias is removed in dynamics function
_u = _R_att * a;
_u(U_az) += 9.81f; // add g
} else {
_u = Vector3f(0, 0, 0);
}
// update state space based on new states
updateSSStates();
// continuous time kalman filter prediction
// integrate runge kutta 4th order
// TODO move rk4 algorithm to matrixlib
// https://en.wikipedia.org/wiki/Runge%E2%80%93Kutta_methods
float h = getDt();
Vector<float, n_x> k1, k2, k3, k4;
k1 = dynamics(0, _x, _u);
k2 = dynamics(h / 2, _x + k1 * h / 2, _u);
k3 = dynamics(h / 2, _x + k2 * h / 2, _u);
k4 = dynamics(h, _x + k3 * h, _u);
Vector<float, n_x> dx = (k1 + k2 * 2 + k3 * 2 + k4) * (h / 6);
// propagate
correctionLogic(dx);
_x += dx;
Matrix<float, n_x, n_x> dP = (_A * _P + _P * _A.transpose() +
_B * _R * _B.transpose() + _Q) * getDt();
covPropagationLogic(dP);
_P += dP;
_xLowPass.update(_x);
_aglLowPass.update(agl());
}
void SimObjBase::setAxisAndAngle(double ax, double ay, double az, double angle)
{
if (dynamics()) { return; }
Rotation r;
r.setAxisAndAngle(ax, ay, az, angle);
const dReal *q = r.q();
setQ(q);
}
void update(float h) {
math::matrix2x1<float> state_dot = dynamics();
state = state + h * (1901.0f/720.0f * state_dot - 1387.0f/360.0f * state_dot_1 + 109.0f/30.0f * state_dot_2 -
637.0f/360.0f * state_dot_3 + 251.0f/720.0f * state_dot_4);
state_dot_4 = state_dot_3;
state_dot_3 = state_dot_2;
state_dot_2 = state_dot_1;
state_dot_1 = state_dot;
}
static void optBK(double *B_, double *Ko_,
double *x, double *u)
{
#define B(i,j) B_[ ((i)-1) + ((j)-1)*(NS) ] // columnwise
#define Ko(i,j) Ko_[ ((i)-1)*NS + ((j)-1) ] // rowwise
#define P(i,j) (( (j<=i) ? P_[i-1][j-1] : P_[j-1][i-1] )) // low tri
double *P__ = x;
// pointers to 'input structure'
double *alf = u, *mu = alf+NS, *wt = mu+NI;
double *P_[NS]; // array of pointers for lower triangular P
int i, j, k;
double ko;
j = 0; // set up lower triangular structure for P
for (i=0; i<NS; i++) {
P_[i] = P__ + j;
j += i+1;
}
// get B of linearization about (alf(t),mu(t))
/*
dynamics(x,u,wt,ders, dx,y, fxu_x_,fxu_u_, q,q_fxu_x_x_,q_fxu_x_u_,q_fxu_u_u_);
ders: dx(1),y(2), A(4),B(8), Q(16), S(32), R(64)
cost(x,u,wlt, ders, lxu, lxu_x_,lxu_u_, lxu_x_x_,lxu_x_u_,lxu_u_u_);
ders: lxu(1), a(2),b(4), Q(8), S(16), R(32)
*/
// get B(8)
dynamics(alf,mu,wt, 8, NULL,NULL, NULL,B_, NULL, NULL,NULL,NULL);
// Kopt = R^{-1} B^T P
// this calc requires R to be diagonal
for (i=1; i<=NI; i++) {
for (j=1; j<=NS; j++) {
ko = 0.0;
for (k=1; k<=NS; k++) {
ko += B(k,i)*P(k,j); // B^T P
}
Ko(i,j) = Rri[i-1]*ko; // R^{-1} B^T P
}
}
#undef P
#undef Ko
#undef B
}
boost::shared_ptr<Lattice>
TwoFactorModel::tree(const TimeGrid& grid,
const boost::shared_ptr<AdditionalResultCalculator>& additionalResultCalculator) const {
boost::shared_ptr<ShortRateDynamics> dyn = dynamics();
boost::shared_ptr<TrinomialTree> tree1(
new TrinomialTree(dyn->xProcess(), grid));
boost::shared_ptr<TrinomialTree> tree2(
new TrinomialTree(dyn->yProcess(), grid));
return boost::shared_ptr<Lattice>(
new TwoFactorModel::ShortRateTree(tree1, tree2, dyn));
}
/*!
* @brief It rotates for the specification of the relative angle.
* @param[in] x-axis rotation weather(i of quaternion complex part)
* @param[in] y-axis rotation weather(j of quaternion complex part)
* @param[in] z-axis rotation weather(k of quaternion complex part)
* @param[in] flag for ansolute / relational (1.0=absolute, else=relational)
*/
void SimObjBase::setAxisAndAngle(double ax, double ay, double az, double angle, double direct)
{
// The angle is used now at the relative angle specification.
if (dynamics()) { return; }
Rotation r;
if (direct != 1.0) r.setQuaternion(qw(), qx(), qy(), qz());
// alculate relational angle
r.setAxisAndAngle(ax, ay, az, angle, direct);
const dReal *q = r.q();
setQ(q);
}
//------------------------------------------------------------------------------
// updateTC() -- update time critical stuff here
//------------------------------------------------------------------------------
void System::updateTC(const LCreal dt0)
{
// We're nothing without an ownship ...
if (ownship == 0 && getOwnship() == 0) return;
// ---
// Delta time
// ---
// real or frozen?
LCreal dt = dt0;
if (isFrozen()) dt = 0.0;
// Delta time for methods that are running every fourth phase
LCreal dt4 = dt * 4.0f;
// ---
// Four phases per frame
// ---
Simulation* sim = ownship->getSimulation();
if (sim == 0) return;
switch (sim->phase()) {
case 0 : // Frame0 --- Dynamics method
dynamics(dt4);
break;
case 1 : // Frame1 --- Transmit method
transmit(dt4);
break;
case 2 : // Frame2 --- Receive method
receive(dt4);
break;
case 3 : // Frame3 --- Process method
process(dt4);
break;
}
// ---
// Last, update our base class
// and use 'dt' because if we're frozen then so are our subcomponents.
// ---
BaseClass::updateTC(dt);
}
void defbudba::iteration(const qint64& t)
{
qreal ptratex;
char state[5];
qreal ztnot;
qreal ytnot;
qreal xtnot;
qreal ct;
ptratex=expectedInflationRate(t);
notProd(ztnot,ytnot);
notCom(xtnot,ct,ptratex);
empAndOut(xtnot,ytnot);
detDiseq(xtnot,ytnot,state);
wageAndPrice(xtnot,ytnot,ztnot,state);
dynamics();
}
void AgentCore::algorithmCallback(const ros::TimerEvent &timer_event) {
consensus(); // also clears the received statistics container
control(); // also publishes virtual agent pose and path
guidance();
dynamics(); // also publishes agent pose and path
waitForSlotTDMA(slot_tdma_*agent_id_); // sync to the next transmission TDMA slot (agent dependent)
// publishes the last estimated statistics in the proper TDMA slot
formation_control::FormationStatisticsStamped msg;
msg.header.frame_id = agent_virtual_frame_;
msg.header.stamp = ros::Time::now();
msg.agent_id = agent_id_;
msg.stats = estimated_statistics_;
stats_publisher_.publish(msg);
std::stringstream s;
s << "Estimated statistics published.";
console(__func__, s, DEBUG);
}
void BlockLocalPositionEstimator::predict()
{
// get acceleration
matrix::Quatf q(&_sub_att.get().q[0]);
_eul = matrix::Euler<float>(q);
_R_att = matrix::Dcm<float>(q);
Vector3f a(_sub_sensor.get().accelerometer_m_s2);
// note, bias is removed in dynamics function
_u = _R_att * a;
_u(U_az) += 9.81f; // add g
// update state space based on new states
updateSSStates();
// continuous time kalman filter prediction
// integrate runge kutta 4th order
// TODO move rk4 algorithm to matrixlib
// https://en.wikipedia.org/wiki/Runge%E2%80%93Kutta_methods
float h = getDt();
Vector<float, n_x> k1, k2, k3, k4;
k1 = dynamics(0, _x, _u);
k2 = dynamics(h / 2, _x + k1 * h / 2, _u);
k3 = dynamics(h / 2, _x + k2 * h / 2, _u);
k4 = dynamics(h, _x + k3 * h, _u);
Vector<float, n_x> dx = (k1 + k2 * 2 + k3 * 2 + k4) * (h / 6);
// don't integrate position if no valid xy data
if (!(_estimatorInitialized & EST_XY)) {
dx(X_x) = 0;
dx(X_vx) = 0;
dx(X_y) = 0;
dx(X_vy) = 0;
}
// don't integrate z if no valid z data
if (!(_estimatorInitialized & EST_Z)) {
dx(X_z) = 0;
}
// don't integrate tz if no valid tz data
if (!(_estimatorInitialized & EST_TZ)) {
dx(X_tz) = 0;
}
// saturate bias
float bx = dx(X_bx) + _x(X_bx);
float by = dx(X_by) + _x(X_by);
float bz = dx(X_bz) + _x(X_bz);
if (std::abs(bx) > BIAS_MAX) {
bx = BIAS_MAX * bx / std::abs(bx);
dx(X_bx) = bx - _x(X_bx);
}
if (std::abs(by) > BIAS_MAX) {
by = BIAS_MAX * by / std::abs(by);
dx(X_by) = by - _x(X_by);
}
if (std::abs(bz) > BIAS_MAX) {
bz = BIAS_MAX * bz / std::abs(bz);
dx(X_bz) = bz - _x(X_bz);
}
// propagate
_x += dx;
Matrix<float, n_x, n_x> dP = (_A * _P + _P * _A.transpose() +
_B * _R * _B.transpose() + _Q) * getDt();
// covariance propagation logic
for (int i = 0; i < n_x; i++) {
if (_P(i, i) > P_MAX) {
// if diagonal element greater than max, stop propagating
dP(i, i) = 0;
for (int j = 0; j < n_x; j++) {
dP(i, j) = 0;
dP(j, i) = 0;
}
}
}
_P += dP;
_xLowPass.update(_x);
_aglLowPass.update(agl());
}
void mexFunction(
int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[]
)
{
double *x, *u, *wt = NULL, *dx, *y = NULL;
double
*fxu_x_ = NULL, *fxu_u_ = NULL,
*q = NULL, *q_fxu_x_x_ = NULL, *q_fxu_x_u_ = NULL, *q_fxu_u_u_ = NULL;
int ders, der, nx, nu, nwt, nq;
unsigned int m, n;
int i;
char errMsg[256];
/*
* things to check:
*
* nrhs > 1 always (need x and u)
* nrhs > 2 if NW > 0 (need wt)
* nrhs > 3 if nlhs > 4 (need q for 2nd derivs)
* sizes of x,u,wt,q are correct
*
*/
// printf("nrhs: %d, nlhs: %d\n",nrhs,nlhs);
// nrhs > 1 always (need x and u)
if (nrhs < 2) {
sprintf(errMsg, "%s_m requires x and u:\n [dx,y, fxu_x,fxu_u, q_fxu_x_x,q_fxu_x_u,q_fxu_u_u] = %s_m(x, u, wt, q);", sys_name, sys_name);
mexErrMsgTxt(errMsg);
}
// nrhs > 2 if NW > 0 (need wt)
if ( NW > 0 && nrhs < 3 ) {
sprintf(errMsg, "%s_m requires a wt input of size %d:\n [dx,y, fxu_x,fxu_u, q_fxu_x_x,q_fxu_x_u,q_fxu_u_u] = %s_m(x, u, wt, q);", sys_name, NW, sys_name);
mexErrMsgTxt(errMsg);
}
// nrhs > 3 if nlhs > 4 (need q for 2nd derivs)
if ( nlhs > 4 && nrhs < 4 ) {
sprintf(errMsg, "%s_m: 2nd derivs require q:\n [dx,y, fxu_x,fxu_u, q_fxu_x_x,q_fxu_x_u,q_fxu_u_u] = %s_m(x, u, wt, q);", sys_name, sys_name);
mexErrMsgTxt(errMsg);
}
// get input matrices/vectors
x = mxGetPr(XX);
u = mxGetPr(UU);
// *** ADD *** size check on x and u
if (nrhs > 2) {
nwt = mxGetNumberOfElements(WT);
if (nwt != NW) {
sprintf(errMsg, "%s_m: wt has %d elements but should have %d:\n [dx,y, fxu_x,fxu_u, q_fxu_x_x,q_fxu_x_u,q_fxu_u_u] = %s_m(x, u, wt, q);", sys_name, nwt, NW, sys_name);
mexErrMsgTxt(errMsg);
}
wt = mxGetPr(WT);
}
if (nrhs > 3) {
// *** ADD *** size check on q
q = mxGetPr(QQ);
}
// determine number of derivs wanted
ders = 1;
der = 1;
for (i=1; i<nlhs; i++) {
der *= 2;
ders += der;
}
// create dx vector output
DX = mxCreateDoubleMatrix((mwSize)NX, (mwSize)1, mxREAL);
dx = mxGetPr(DX);
if (nlhs > 1) {
YY = mxCreateDoubleMatrix((mwSize)NO, (mwSize)1, mxREAL);
y = mxGetPr(YY);
}
if (nlhs > 2) {
FXU_X = mxCreateDoubleMatrix((mwSize)NX, (mwSize)NX, mxREAL);
fxu_x_ = mxGetPr(FXU_X);
}
if (nlhs > 3) {
FXU_U = mxCreateDoubleMatrix((mwSize)NX, (mwSize)NU, mxREAL);
fxu_u_ = mxGetPr(FXU_U);
}
if (nlhs > 4) {
Q_FXU_X_X = mxCreateDoubleMatrix((mwSize)NX, (mwSize)NX, mxREAL);
q_fxu_x_x_ = mxGetPr(Q_FXU_X_X);
}
if (nlhs > 5) {
Q_FXU_X_U = mxCreateDoubleMatrix((mwSize)NX, (mwSize)NU, mxREAL);
q_fxu_x_u_ = mxGetPr(Q_FXU_X_U);
}
if (nlhs > 6) {
Q_FXU_U_U = mxCreateDoubleMatrix((mwSize)NU, (mwSize)NU, mxREAL);
q_fxu_u_u_ = mxGetPr(Q_FXU_U_U);
}
/* prontoTK spec:
dynamics(x,u,wt,ders, dx,y, fxu_x_,fxu_u_, q,q_fxu_x_x_,q_fxu_x_u_,q_fxu_u_u_);
ders: dx(1),y(2), A(4),B(8), Q(16), S(32), R(64)
cost(x,u,wlt,ders, lxu, lxu_x_,lxu_u_, lxu_x_x_,lxu_x_u_,lxu_u_u_);
ders: lxu(1), a(2),b(4), Q(8), S(16), R(32)
*/
dynamics(x,u,wt,ders,dx,y, fxu_x_,fxu_u_, q,q_fxu_x_x_,q_fxu_x_u_,q_fxu_u_u_);
#undef NX
#undef NU
}
void SimObjBase::setForce(double fx_, double fy_, double fz_)
{
if (!dynamics()) { return; }
fx(fx_); fy(fy_); fz(fz_);
}
void SimObjBase::setTorque(double x_, double y_, double z_)
{
if (!dynamics()) { return; }
tqx(x_); tqy(y_); tqz(z_);
}
void
ShapesDialog::onSubscribeButtonClicked()
{
dds::topic::qos::TopicQos topicQos = dp_.default_topic_qos()
<< dds::core::policy::Durability::Persistent()
<< dds::core::policy::DurabilityService(
dds::core::Duration(3600,0),
dds::core::policy::HistoryKind::KEEP_LAST,
100,
8192,
4196,
8192);
dds::sub::qos::SubscriberQos SQos = dp_.default_subscriber_qos() << gQos_;
dds::sub::Subscriber sub(dp_, SQos);
int d = mainWidget.sizeSlider->value();
QRect rect(0, 0, d, d);
QRect constr(0, 0, IS_WIDTH, IS_HEIGHT);
int x = static_cast<int>(constr.width() * ((float)rand() / RAND_MAX)*0.9F);
int y = static_cast<int>(constr.height() * ((float)rand() / RAND_MAX)*0.9F);
int sIdx = mainWidget.rShapeList->currentIndex();
QColor gray = QColor(0x99, 0x99, 0x99);
QBrush brush(gray, Qt::SolidPattern);
QPen pen(QColor(0xff,0xff,0xff), 1, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin);
std::vector<std::string> empty;
filterParams_ = empty;
std::string filterS;
if (filterDialog_->isEnabled())
{
QRect rect = filterDialog_->getFilterBounds();
std::string x0 = lexicalCast(rect.x());
std::string x1 = lexicalCast(rect.x() + rect.width() -d);
std::string y0 = lexicalCast(rect.y());
std::string y1 = lexicalCast(rect.y() + rect.height() -d);
filterParams_.push_back(x0);
filterParams_.push_back(x1);
filterParams_.push_back(y0);
filterParams_.push_back(y1);
filterS = "(x BETWEEN "
+ filterParams_[0]
+ " AND "
+ filterParams_[1]
+ ") AND (y BETWEEN "
+ filterParams_[2]
+ " AND "
+ filterParams_[3]
+ ")";
if (filterDialog_->filterOutside() == false)
{
filterS = "(x < "
+ filterParams_[0]
+ " ) OR ( x > "
+ filterParams_[1]
+ " ) OR (y < "
+ filterParams_[2]
+ ") OR ( y > "
+ filterParams_[3]
+ ")";
}
}
switch (sIdx)
{
case CIRCLE:
{
dds::topic::Topic<ShapeType> circle_(dp_, circleTopicName, topicQos);
dds::topic::ContentFilteredTopic<ShapeType> cfcircle_(dds::core::null);
dds::sub::DataReader<ShapeType> dr(sub, circle_, readerQos_.get_qos());
if (filterDialog_->isEnabled())
{
std::string tname = "CFCircle";
const dds::topic::Filter filter(filterS);
std::cout << filterS << std::endl;
dds::topic::ContentFilteredTopic<ShapeType> cfcircle_(circle_, "CFCircle", filter);
dds::sub::DataReader<ShapeType> dr2(sub, cfcircle_, readerQos_.get_qos());
dr = dr2;
}
for (int i = 0; i < CN; ++i)
{
std::string colorStr(colorString_[i]);
DDSShapeDynamics::ref_type
dynamics(new DDSShapeDynamics(x, y, dr, colorStr, i));
Shape::ref_type
circle(new Circle(rect, dynamics, pen, brush, true));
dynamics->setShape(circle);
shapesWidget->addShape(circle);
}
break;
}
case SQUARE:
{
dds::topic::Topic<ShapeType> square_(dp_, squareTopicName, topicQos);
dds::sub::LoanedSamples<ShapeType>::iterator si;
dds::topic::ContentFilteredTopic<ShapeType> cfsquare_(dds::core::null);
dds::sub::DataReader<ShapeType> dr(sub, square_, readerQos_.get_qos());
if (filterDialog_->isEnabled())
{
std::string tname = "CFSquare";
const dds::topic::Filter filter(filterS);
std::cout << filterS << std::endl;
dds::topic::ContentFilteredTopic<ShapeType> cfsquare_(square_, "CFSquare", filter);
dds::sub::DataReader<ShapeType> dr2(sub, cfsquare_, readerQos_.get_qos());
dr = dr2;
}
for (int i = 0; i < CN; ++i)
{
std::string colorStr(colorString_[i]);
DDSShapeDynamics::ref_type
dynamics(new DDSShapeDynamics(x, y, dr, colorStr, i));
Shape::ref_type
square(new Square(rect, dynamics, pen, brush, true));
dynamics->setShape(square);
shapesWidget->addShape(square);
}
break;
}
case TRIANGLE:
{
dds::topic::Topic<ShapeType> triangle_(dp_, triangleTopicName, topicQos);
dds::sub::LoanedSamples<ShapeType>::iterator si;
dds::topic::ContentFilteredTopic<ShapeType> cftriangle_(dds::core::null);
dds::sub::DataReader<ShapeType> dr(sub, triangle_, readerQos_.get_qos());
if (filterDialog_->isEnabled())
{
std::string tname = "CFTriangle";
const dds::topic::Filter filter(filterS);
std::cout << filterS << std::endl;
dds::topic::ContentFilteredTopic<ShapeType> cftriangle_(triangle_, "CFTriangle", filter);
dds::sub::DataReader<ShapeType> dr2(sub, cftriangle_, readerQos_.get_qos());
dr = dr2;
}
for (int i = 0; i < CN; ++i)
{
std::string colorStr(colorString_[i]);
DDSShapeDynamics::ref_type
dynamics(new DDSShapeDynamics(x, y, dr, colorStr, i));
Shape::ref_type
triangle(new Triangle(rect, dynamics, pen, brush, true));
dynamics->setShape(triangle);
shapesWidget->addShape(triangle);
}
break;
}
default:
break;
}
}
void
ShapesDialog::onPublishButtonClicked()
{
dds::topic::qos::TopicQos topicQos = dp_.default_topic_qos()
<< dds::core::policy::Durability::Persistent()
<< dds::core::policy::DurabilityService(dds::core::Duration(3600,0),
dds::core::policy::HistoryKind::KEEP_LAST,
100,
8192,
4196,
8192);
dds::pub::qos::PublisherQos PQos = dp_.default_publisher_qos()
<< gQos_;
dds::pub::Publisher pub(dp_, PQos);
int d = mainWidget.sizeSlider->value();
float speed = ((float)mainWidget.speedSlider->value()) / 9;
QRect rect(0, 0, d, d);
// TODO: This should be retrieved from the canvas...
QRect constr(0, 0, IS_WIDTH, IS_HEIGHT);
// QRect constr = this->geometry();
int x = constr.width() * ((float)rand() / RAND_MAX);
int y = constr.height() * ((float)rand() / RAND_MAX);
int cIdx = mainWidget.colorList->currentIndex();
int sIdx = mainWidget.wShapeList->currentIndex();
QBrush brush(color_[cIdx], Qt::SolidPattern);
QPen pen(QColor(0xff, 0xff, 0xff), 1, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin);
ShapeType shape;
shape.color = DDS::string_dup(colorString_[cIdx]);
shape.shapesize = rect.width();
shape.x = x;
shape.y = y;
switch (sIdx)
{
case CIRCLE:
{
dds::topic::Topic<ShapeType> circle_(dp_, circleTopicName, topicQos);
dds::pub::qos::DataWriterQos dwqos = circle_.qos();
dds::pub::DataWriter<ShapeType> dw(pub, circle_, writerQos_.get_qos());
BouncingShapeDynamics::ref_type dynamics(new BouncingShapeDynamics(x, y, rect, constr, PI/6, speed, shape, dw));
Shape::ref_type circle(new Circle(rect, dynamics, pen, brush));
shapesWidget->addShape(circle);
break;
}
case SQUARE:
{
dds::topic::Topic<ShapeType> square_(dp_, squareTopicName, topicQos);
dds::pub::qos::DataWriterQos dwqos = square_.qos();
dds::pub::DataWriter<ShapeType> dw(pub, square_, writerQos_.get_qos());
BouncingShapeDynamics::ref_type dynamics(new BouncingShapeDynamics(x, y, rect, constr, PI/6, speed, shape, dw));
Shape::ref_type square(new Square(rect, dynamics, pen, brush));
shapesWidget->addShape(square);
break;
}
case TRIANGLE:
{
dds::topic::Topic<ShapeType> triangle_(dp_, triangleTopicName, topicQos);
dds::pub::qos::DataWriterQos dwqos = triangle_.qos();
dds::pub::DataWriter<ShapeType> dw(pub, triangle_, writerQos_.get_qos());
BouncingShapeDynamics::ref_type dynamics(new BouncingShapeDynamics(x, y, rect, constr, PI/6, speed, shape, dw));
Shape::ref_type triangle(new Triangle(rect, dynamics, pen, brush));
shapesWidget->addShape(triangle);
break;
}
default:
break;
};
}
void update(float h) {
math::matrix2x1<float> state_dot = dynamics(state);
state = state + state_dot * h;
}
void SimObjBase::setPosition(double x_, double y_, double z_)
{
if (dynamics()) { return; }
x(x_); y(y_); z(z_);
}
static void mdlDerivatives(SimStruct *S)
{
real_T *dx = ssGetdX(S);
real_T *x = ssGetContStates(S);
InputRealPtrsType uPtrs = ssGetInputPortRealSignalPtrs(S,0);
// double *param0 = mxGetPr(ssGetSFcnParam(S,0));
int i, j, k;
double u[N_INPUTS];
// pointers to 'input structure'
double *alf = u, *mu = alf+NS, *wt = mu+NI;
// double a[NS], b[NI], Ko_b[NS], K_b[NS];
double *P__ = x;
double *dP__ = dx;
double dp;
double *P_[NS], *dP_[NS]; // array of pointers for lower triangular P, dP
double A_[NS2], A_BKo_[NS2];
double B_[NS*NI], Ko_[NI*NS];
double Q_[NS2];
/* index A & P with 1:NS (NOT 0:NS-1!) */
#define A(i,j) A_[((i)-1) + NS*((j)-1)]
#define Q(i,j) Q_[((i)-1) + NS*((j)-1)]
#define P(i,j) (( (j<=i) ? P_[i-1][j-1] : P_[j-1][i-1] )) // low tri
#define dP(i,j) ( dP_[i-1][j-1] ) // *** i <= j *** REQUIRED!
#define B(i,j) B_[ ((i)-1) + ((j)-1)*(NS) ] // columnwise
#define Ko(i,j) Ko_[ ((i)-1)*NS + ((j)-1) ] // rowwise
// set up lower triangular structure for P and dP
j = 0;
for (i=0; i<NS; i++) {
P_[i] = P__ + j;
dP_[i] = dP__ + j;
j += i+1;
}
/* copy inputs to u[] to ensure contiguity ... and easy access*/
for (i=0; i<N_INPUTS; i++)
u[i] = *uPtrs[i];
/* prontoTK spec:
dynamics(x,u,wt,ders, dx,y, fxu_x_,fxu_u_, q,q_fxu_x_x_,q_fxu_x_u_,q_fxu_u_u_);
ders: dx(1),y(2), A(4),B(8), Q(16), S(32), R(64)
cost(x,u,wlt,ders, lxu, lxu_x_,lxu_u_, lxu_x_x_,lxu_x_u_,lxu_u_u_);
ders: lxu(1), a(2),b(4), Q(8), S(16), R(32)
*/
// get A of linearization about (alf(t),mu(t))
// get A(4)
dynamics(alf,mu,wt, 4, NULL,NULL, A_,NULL, NULL, NULL,NULL,NULL);
/* set up Q matrix --- start with zero */
for (i=0; i<NS2; i++){
Q_[i] = 0.0;
}
/* add diagonal cost fcn terms */
for (i=1; i<=NS; i++){
Q(i,i) = Qr[i-1];
}
optBK(B_, Ko_, x, u);
/* only *lower triangle* of dP */
for (i=1; i<=NS; i++) {
for (j=1; j<=i; j++) {
dp = Q(i,j);
for (k=1; k<=NS; k++) {
dp += A(k,i)*P(k,j) + P(i,k)*A(k,j); // A^T P + P A
}
for (k=1; k<=NI; k++) {
dp -= Ko(k,i)*Rr[k-1]*Ko(k,j); // -P B R^-1 B^T P = -Ko^T R Ko
}
dP(i,j) /* = dP(j,i) */ = dp;
}
}
#undef B
#undef K
#undef Ko
#undef dP
#undef P
#undef A
}
void SimObjBase::setPosition(const Vector3d &v)
{
if (dynamics()) { return; }
x(v.x()); y(v.y()); z(v.z());
}