{
#if OPERATION_sec_div_qr
/* Needs (nn + dn + 1) + mpn_sec_pi1_div_qr's needs of (2nn' - dn + 1) for a
total of 3nn + 4 limbs at tp. Note that mpn_sec_pi1_div_qr's nn is one
greater than ours, therefore +4 and not just +2. */
return 3 * nn + 4;
#endif
#if OPERATION_sec_div_r
/* Needs (nn + dn + 1) + mpn_sec_pi1_div_r's needs of (dn + 1) for a total of
nn + 2dn + 2 limbs at tp. */
return nn + 2 * dn + 2;
#endif
}
RETTYPE
FNAME (Q(mp_ptr qp)
mp_ptr np, mp_size_t nn,
mp_srcptr dp, mp_size_t dn,
mp_ptr tp)
{
mp_limb_t d1, d0;
unsigned int cnt;
mp_limb_t inv32;
ASSERT (dn >= 1);
ASSERT (nn >= dn);
ASSERT (dp[dn - 1] != 0);
d1 = dp[dn - 1];
count_leading_zeros (cnt, d1);
//BOOL OTsender(int nSndVals, int nOTs, int startpos, CSocket& sock, CBitVector& U, AES_KEY* vKeySeeds, CBitVector* values, BYTE* seed)
BOOL IKNPOTExtSnd::sender_routine(uint32_t id, uint64_t myNumOTs) {
uint64_t myStartPos = id * myNumOTs;
uint64_t wd_size_bits = m_nBlockSizeBits;
uint64_t processedOTBlocks = min((uint64_t) NUMOTBLOCKS, ceil_divide(myNumOTs, wd_size_bits));
uint64_t OTsPerIteration = processedOTBlocks * wd_size_bits;
channel* chan = new channel(id, m_cRcvThread, m_cSndThread);
uint64_t tmpctr, tmpotlen;
uint64_t** rndmat;
myNumOTs = min(myNumOTs + myStartPos, m_nOTs) - myStartPos;
uint64_t lim = myStartPos + myNumOTs;
// The vector with the received bits
#ifdef GENERATE_T_EXPLICITELY
CBitVector vRcv(2 * m_nBaseOTs * OTsPerIteration);
#else
CBitVector vRcv(m_nBaseOTs * OTsPerIteration);
#endif
// Holds the reply that is sent back to the receiver
uint32_t numsndvals = 2;
CBitVector* vSnd;
CBitVector* seedbuf = new CBitVector[m_nSndVals];
for (uint32_t u = 0; u < m_nSndVals; u++)
seedbuf[u].Create(OTsPerIteration * m_cCrypt->get_aes_key_bytes() * 8);
#ifdef ZDEBUG
cout << "seedbuf size = " <<OTsPerIteration * AES_KEY_BITS << endl;
#endif
vSnd = new CBitVector[numsndvals];
for (uint32_t i = 0; i < numsndvals; i++) {
vSnd[i].Create(OTsPerIteration * m_nBitLength);
}
// Contains the parts of the V matrix
CBitVector Q(wd_size_bits * OTsPerIteration);
uint64_t otid = myStartPos;
uint8_t *rcvbuftmpptr, *rcvbufptr;
#ifdef OTTiming
double totalMtxTime = 0, totalTnsTime = 0, totalHshTime = 0, totalRcvTime = 0, totalSndTime = 0, totalUnMaskTime=0;
timeval tempStart, tempEnd;
#endif
if(m_eSndOTFlav == Snd_GC_OT) {
uint8_t* rnd_seed = (uint8_t*) malloc(m_nSymSecParam);
m_cCrypt->gen_rnd(rnd_seed, m_nSymSecParam);
chan->send(rnd_seed, m_nSymSecParam);
initRndMatrix(&rndmat, m_nBitLength, m_nBaseOTs);
fillRndMatrix(rnd_seed, rndmat, m_nBitLength, m_nBaseOTs, m_cCrypt);
free(rnd_seed);
}
while (otid < lim) //do while there are still transfers missing
{
processedOTBlocks = min((uint64_t) NUMOTBLOCKS, ceil_divide(lim - otid, wd_size_bits));
OTsPerIteration = processedOTBlocks * wd_size_bits;
#ifdef ZDEBUG
cout << "Processing block " << nProgress << " with length: " << OTsPerIteration << ", and limit: " << lim << endl;
#endif
#ifdef OTTiming
gettimeofday(&tempStart, NULL);
#endif
//rcvbufptr = chan->blocking_receive_id_len(&rcvbuftmpptr, &tmpctr, &tmpotlen);
//vRcv.AttachBuf(rcvbuftmpptr, bits_in_bytes(m_nBaseOTs * OTsPerIteration));
ReceiveMasks(vRcv, chan, OTsPerIteration);
#ifdef OTTiming
gettimeofday(&tempEnd, NULL);
totalRcvTime += getMillies(tempStart, tempEnd);
gettimeofday(&tempStart, NULL);
#endif
BuildQMatrix(Q, otid, processedOTBlocks);
#ifdef OTTiming
gettimeofday(&tempEnd, NULL);
totalMtxTime += getMillies(tempStart, tempEnd);
gettimeofday(&tempStart, NULL);
#endif
UnMaskBaseOTs(Q, vRcv, processedOTBlocks);
#ifdef OTTiming
gettimeofday(&tempEnd, NULL);
totalUnMaskTime += getMillies(tempStart, tempEnd);
gettimeofday(&tempStart, NULL);
#endif
Q.Transpose(wd_size_bits, OTsPerIteration);
#ifdef OTTiming
gettimeofday(&tempEnd, NULL);
totalTnsTime += getMillies(tempStart, tempEnd);
gettimeofday(&tempStart, NULL);
#endif
HashValues(Q, seedbuf, vSnd, otid, min(lim - otid, OTsPerIteration), rndmat);
#ifdef OTTiming
gettimeofday(&tempEnd, NULL);
totalHshTime += getMillies(tempStart, tempEnd);
gettimeofday(&tempStart, NULL);
#endif
MaskAndSend(vSnd, otid, min(lim - otid, OTsPerIteration), chan);
#ifdef OTTiming
gettimeofday(&tempEnd, NULL);
totalSndTime += getMillies(tempStart, tempEnd);
#endif
otid += min(lim - otid, OTsPerIteration);
Q.Reset();
//free(rcvbufptr);
}
//vRcv.delCBitVector();
chan->synchronize_end();
Q.delCBitVector();
for (uint32_t u = 0; u < m_nSndVals; u++)
seedbuf[u].delCBitVector();
for (uint32_t i = 0; i < numsndvals; i++)
vSnd[i].delCBitVector();
if (numsndvals > 0)
free(vSnd);
if(m_eSndOTFlav==Snd_GC_OT)
freeRndMatrix(rndmat, m_nBaseOTs);
#ifdef OTTiming
cout << "Sender time benchmark for performing " << myNumOTs << " OTs on " << m_nBitLength << " bit strings" << endl;
cout << "Time needed for: " << endl;
cout << "\t Matrix Generation:\t" << totalMtxTime << " ms" << endl;
cout << "\t Unmasking values:\t" << totalUnMaskTime << " ms" << endl;
cout << "\t Sending Matrix:\t" << totalSndTime << " ms" << endl;
cout << "\t Transposing Matrix:\t" << totalTnsTime << " ms" << endl;
cout << "\t Hashing Matrix:\t" << totalHshTime << " ms" << endl;
cout << "\t Receiving Values:\t" << totalRcvTime << " ms" << endl;
#endif
delete chan;
return TRUE;
}
// Initialization process
void EkfNode::init() {
/**************************************************************************
* Initialize firstRun, time, and frame names
**************************************************************************/
// Set first run to true for encoders. Once a message is received, this will
// be set to false.
firstRunEnc_ = true;
firstRunSys_ = true;
// Set Times
ros::Time currTime = ros::Time::now();
lastEncTime_ = currTime;
lastSysTime_ = currTime;
// Use the ROS parameter server to initilize parameters
if(!private_nh_.getParam("base_frame", base_frame_))
base_frame_ = "base_link";
if(!private_nh_.getParam("odom_frame", base_frame_))
odom_frame_ = "odom";
if(!private_nh_.getParam("map_frame", map_frame_))
map_frame_ = "map";
/**************************************************************************
* Initialize state for ekf_odom_ and ekf_map_
**************************************************************************/
// Create temp array to initialize state
double state_temp[] = {0, 0, 0, 0, 0, 0};
// And a std::vector, which will be used to initialize an Eigen Matrix
std::vector<double> state (state_temp, state_temp + sizeof(state_temp) / sizeof(double));
// Check the parameter server and initialize state
if(!private_nh_.getParam("state", state)) {
ROS_WARN_STREAM("No state found. Using default.");
}
// Check to see if the size is not equal to 6
if (state.size() != 6) {
ROS_WARN_STREAM("state isn't 6 elements long!");
}
// And initialize the Matrix
typedef Matrix<double, 6, 1> Vector6d;
Vector6d stateMat(state.data());
std::cout << "state_map = " << std::endl;
std::cout << stateMat << std::endl;
ekf_map_.initState(stateMat);
// Odom is always initialized at all zeros.
stateMat << 0, 0, 0, 0, 0, 0;
ekf_odom_.initState(stateMat);
std::cout << "state_odom = " << std::endl;
std::cout << stateMat << std::endl;
/**************************************************************************
* Initialize covariance for ekf_odom_ and ekf_map_
**************************************************************************/
// Create temp array to initialize covariance
double cov_temp[] = {0.01, 0, 0, 0, 0, 0,
0, 0.01, 0, 0, 0, 0,
0, 0, 0.01, 0, 0, 0,
0, 0, 0, 0.01, 0, 0,
0, 0, 0, 0, 0.01, 0,
0, 0, 0, 0, 0, 0.01};
// And a std::vector, which will be used to initialize an Eigen Matrix
std::vector<double> cov (cov_temp, cov_temp + sizeof(cov_temp) / sizeof(double));
// Check the parameter server and initialize cov
if(!private_nh_.getParam("covariance", cov)) {
ROS_WARN_STREAM("No covariance found. Using default.");
}
// Check to see if the size is not equal to 36
if (cov.size() != 36) {
ROS_WARN_STREAM("cov isn't 36 elements long!");
}
// And initialize the Matrix
typedef Matrix<double, 6, 6, RowMajor> Matrix66;
Matrix66 covMat(cov.data());
std::cout << "covariance = " << std::endl;
std::cout << covMat << std::endl;
ekf_map_.initCov(covMat);
// Initialize odom covariance the same as the map covariance (this isn't
// correct. But, since it is all an estimate anyway, it should be fine.
ekf_odom_.initCov(covMat);
/**************************************************************************
* Initialize Q for ekf_odom_ and ekf_map_
**************************************************************************/
// Create temp array to initialize Q
double Q_temp[] = {0.01, 0, 0, 0, 0, 0,
0, 0.01, 0, 0, 0, 0,
0, 0, 0.01, 0, 0, 0,
0, 0, 0, 0.01, 0, 0,
0, 0, 0, 0, 0.01, 0,
0, 0, 0, 0, 0, 0.01};
// And a std::vector, which will be used to initialize an Eigen Matrix
std::vector<double> Q (Q_temp, Q_temp + sizeof(Q_temp) / sizeof(double));
// Check the parameter server and initialize Q
if(!private_nh_.getParam("Q", Q)) {
ROS_WARN_STREAM("No Q found. Using default.");
}
// Check to see if the size is not equal to 36
if (Q.size() != 36) {
ROS_WARN_STREAM("Q isn't 36 elements long!");
}
// And initialize the Matrix
Matrix66 QMat(Q.data());
std::cout << "Q = " << std::endl;
std::cout << QMat << std::endl;
ekf_map_.initSystem(QMat);
ekf_odom_.initSystem(QMat);
/**************************************************************************
* Initialize Decawave for ekf_map_ (not used in ekf_odom_)
**************************************************************************/
// Create temp array to initialize R for DecaWave
double RDW_temp[] = {0.01, 0, 0, 0,
0, 0.01, 0, 0,
0, 0, 0.01, 0,
0, 0, 0, 0.01};
// And a std::vector, which will be used to initialize an Eigen Matrix
std::vector<double> RDW (RDW_temp, RDW_temp + sizeof(RDW_temp) / sizeof(double));
// Check the parameter server and initialize RDW
if(!private_nh_.getParam("DW_R", RDW)) {
ROS_WARN_STREAM("No DW_R found. Using default.");
}
// Check to see if the size is not equal to 16
if (RDW.size() != 16) {
ROS_WARN_STREAM("DW_R isn't 16 elements long!");
}
// And initialize the Matrix
typedef Matrix<double, 4, 4, RowMajor> Matrix44;
Matrix44 RDWMat(RDW.data());
std::cout << "RDecaWave = " << std::endl;
std::cout << RDWMat << std::endl;
// Create temp array to initialize beacon locations for DecaWave
double DWBL_temp[] = {0, 0,
5, 0,
5, 15,
0, 15};
// And a std::vector, which will be used to initialize an Eigen Matrix
std::vector<double> DWBL (DWBL_temp, DWBL_temp + sizeof(DWBL_temp) / sizeof(double));
// Check the parameter server and initialize DWBL
if(!private_nh_.getParam("DW_Beacon_Loc", DWBL)) {
ROS_WARN_STREAM("No DW_Beacon_Loc found. Using default.");
}
// Check to see if the size is not equal to 8
if (DWBL.size() != 8) {
ROS_WARN_STREAM("DW_Beacon_Loc isn't 8 elements long!");
}
// And initialize the Matrix
typedef Matrix<double, 4, 2, RowMajor> Matrix42;
Matrix42 DWBLMat(DWBL.data());
std::cout << "DW_Beacon_Loc = " << std::endl;
std::cout << DWBLMat << std::endl;
MatrixXd DecaWaveBeaconLoc(4,2);
MatrixXd DecaWaveOffset(2,1);
double DW_offset_x;
double DW_offset_y;
if(!private_nh_.getParam("DW_offset_x", DW_offset_x))
DW_offset_x = 0.0;
if(!private_nh_.getParam("DW_offset_y", DW_offset_y))
DW_offset_y = 0.0;
DecaWaveOffset << DW_offset_x, DW_offset_y;
std::cout << "DecaWaveOffset = " << std::endl;
std::cout << DecaWaveOffset << std::endl;
ekf_map_.initDecaWave(RDWMat, DWBLMat, DecaWaveOffset);
// Decawave is not used in odom, so no need to initialize
/**************************************************************************
* Initialize Encoders for ekf_odom_ and ekf_map_
**************************************************************************/
// Create temp array to initialize R for DecaWave
double REnc_temp[] = {0.01, 0,
0, 0.01};
// And a std::vector, which will be used to initialize an Eigen Matrix
std::vector<double> REnc (REnc_temp, REnc_temp + sizeof(REnc_temp) / sizeof(double));
// Check the parameter server and initialize RDW
if(!private_nh_.getParam("Enc_R", REnc)) {
ROS_WARN_STREAM("No Enc_R found. Using default.");
}
// Check to see if the size is not equal to 4
if (REnc.size() != 4) {
ROS_WARN_STREAM("Enc_R isn't 4 elements long!");
}
// And initialize the Matrix
typedef Matrix<double, 2, 2, RowMajor> Matrix22;
Matrix22 REncMat(REnc.data());
std::cout << "R_Enc = " << std::endl;
std::cout << REncMat << std::endl;
double b, tpmRight, tpmLeft;
if(!private_nh_.getParam("track_width", b))
b = 1;
if(!private_nh_.getParam("tpm_right", tpmRight))
tpmRight = 1;
if(!private_nh_.getParam("tpm_left", tpmLeft))
tpmLeft = 1;
ekf_map_.initEnc(REncMat, b, tpmRight, tpmLeft);
ekf_odom_.initEnc(REncMat, b, tpmRight, tpmLeft);
std::cout << "track_width = " << b << std::endl;
std::cout << "tpm_right = " << tpmRight << std::endl;
std::cout << "tpm_left = " << tpmLeft << std::endl;
/**************************************************************************
* Initialize IMU for ekf_odom_ and ekf_map_
**************************************************************************/
double RIMU;
if(!private_nh_.getParam("R_IMU", RIMU))
RIMU = 0.01;
ekf_map_.initIMU(RIMU);
ekf_odom_.initIMU(RIMU);
std::cout << "R_IMU = " << RIMU << std::endl;
// Publish the initialized state and exit initialization.
publishState(currTime);
ROS_INFO_STREAM("Finished with initialization.");
}
static void
negative_s2_influence(double t, double s2, double *A1, double *A3)
{
*A1 = g_blend(1-t, Q(s2));
*A3 = h_blend(t-1, Q(s2));
}
void meow() {
evil<int> Q(0); // expected-note {{in instantiation of member function}}
}
extern "C" magma_int_t
magma_dlaex1(
magma_int_t n, double* d, double* q, magma_int_t ldq,
magma_int_t* indxq, double rho, magma_int_t cutpnt,
double* work, magma_int_t* iwork, magmaDouble_ptr dwork,
magma_range_t range, double vl, double vu,
magma_int_t il, magma_int_t iu,
magma_queue_t queue,
magma_int_t* info)
{
/*
-- clMAGMA (version 1.3.0) --
Univ. of Tennessee, Knoxville
Univ. of California, Berkeley
Univ. of Colorado, Denver
@date November 2014
.. Scalar Arguments ..
CHARACTER RANGE
INTEGER IL, IU, CUTPNT, INFO, LDQ, N
DOUBLE PRECISION RHO, VL, VU
..
.. Array Arguments ..
INTEGER INDXQ( * ), iwork[* )
DOUBLE PRECISION D( * ), Q( LDQ, * ), WORK( * ), DWORK( * )
..
Purpose
=======
DLAEX1 computes the updated eigensystem of a diagonal
matrix after modification by a rank-one symmetric matrix.
T = Q(in) ( D(in) + RHO * Z*Z' ) Q'(in) = Q(out) * D(out) * Q'(out)
where Z = Q'u, u is a vector of length N with ones in the
CUTPNT and CUTPNT + 1 th elements and zeros elsewhere.
The eigenvectors of the original matrix are stored in Q, and the
eigenvalues are in D. The algorithm consists of three stages:
The first stage consists of deflating the size of the problem
when there are multiple eigenvalues or if there is a zero in
the Z vector. For each such occurence the dimension of the
secular equation problem is reduced by one. This stage is
performed by the routine DLAED2.
The second stage consists of calculating the updated
eigenvalues. This is done by finding the roots of the secular
equation via the routine DLAED4 (as called by DLAED3).
This routine also calculates the eigenvectors of the current
problem.
The final stage consists of computing the updated eigenvectors
directly using the updated eigenvalues. The eigenvectors for
the current problem are multiplied with the eigenvectors from
the overall problem.
Arguments
=========
N (input) INTEGER
The dimension of the symmetric tridiagonal matrix. N >= 0.
D (input/output) DOUBLE PRECISION array, dimension (N)
On entry, the eigenvalues of the rank-1-perturbed matrix.
On exit, the eigenvalues of the repaired matrix.
Q (input/output) DOUBLE PRECISION array, dimension (LDQ,N)
On entry, the eigenvectors of the rank-1-perturbed matrix.
On exit, the eigenvectors of the repaired tridiagonal matrix.
LDQ (input) INTEGER
The leading dimension of the array Q. LDQ >= max(1,N).
INDXQ (input/output) INTEGER array, dimension (N)
On entry, the permutation which separately sorts the two
subproblems in D into ascending order.
On exit, the permutation which will reintegrate the
subproblems back into sorted order,
i.e. D( INDXQ( I = 1, N ) ) will be in ascending order.
RHO (input) DOUBLE PRECISION
The subdiagonal entry used to create the rank-1 modification.
CUTPNT (input) INTEGER
The location of the last eigenvalue in the leading sub-matrix.
min(1,N) <= CUTPNT <= N/2.
WORK (workspace) DOUBLE PRECISION array, dimension (4*N + N**2)
IWORK (workspace) INTEGER array, dimension (4*N)
DWORK (device workspace) DOUBLE PRECISION array, dimension (3*N*N/2+3*N)
RANGE (input) CHARACTER*1
= 'A': all eigenvalues will be found.
= 'V': all eigenvalues in the half-open interval (VL,VU]
will be found.
= 'I': the IL-th through IU-th eigenvalues will be found.
VL (input) DOUBLE PRECISION
VU (input) DOUBLE PRECISION
if RANGE='V', the lower and upper bounds of the interval to
be searched for eigenvalues. VL < VU.
Not referenced if RANGE = 'A' or 'I'.
IL (input) INTEGER
IU (input) INTEGER
if RANGE='I', the indices (in ascending order) of the
smallest and largest eigenvalues to be returned.
1 <= IL <= IU <= N, if N > 0; IL = 1 and IU = 0 if N = 0.
Not referenced if RANGE = 'A' or 'V'.
INFO (output) INTEGER
= 0: successful exit.
< 0: if INFO = -i, the i-th argument had an illegal value.
> 0: if INFO = 1, an eigenvalue did not converge
Further Details
===============
Based on contributions by
Jeff Rutter, Computer Science Division, University of California
at Berkeley, USA
Modified by Francoise Tisseur, University of Tennessee.
===================================================================== */
magma_int_t coltyp, i, idlmda;
magma_int_t indx, indxc, indxp;
magma_int_t iq2, is, iw, iz, k, tmp;
magma_int_t ione = 1;
// Test the input parameters.
*info = 0;
if( n < 0 )
*info = -1;
else if( ldq < max(1, n) )
*info = -4;
else if( min( 1, n/2 ) > cutpnt || n/2 < cutpnt )
*info = -7;
if( *info != 0 ) {
magma_xerbla( __func__, -*info );
return MAGMA_ERR_ILLEGAL_VALUE;
}
// Quick return if possible
if( n == 0 )
return MAGMA_SUCCESS;
// The following values are integer pointers which indicate
// the portion of the workspace
// used by a particular array in DLAED2 and DLAED3.
iz = 0;
idlmda = iz + n;
iw = idlmda + n;
iq2 = iw + n;
indx = 0;
indxc = indx + n;
coltyp = indxc + n;
indxp = coltyp + n;
// Form the z-vector which consists of the last row of Q_1 and the
// first row of Q_2.
blasf77_dcopy( &cutpnt, Q(cutpnt-1, 0), &ldq, &work[iz], &ione);
tmp = n-cutpnt;
blasf77_dcopy( &tmp, Q(cutpnt, cutpnt), &ldq, &work[iz+cutpnt], &ione);
// Deflate eigenvalues.
magma_dlaed2(&k, &n, &cutpnt, d, q, &ldq, indxq, &rho, &work[iz],
&work[idlmda], &work[iw], &work[iq2],
&iwork[indx], &iwork[indxc], &iwork[indxp],
&iwork[coltyp], info);
if( *info != 0 )
return MAGMA_SUCCESS;
// Solve Secular Equation.
if( k != 0 ) {
is = (iwork[coltyp]+iwork[coltyp+1])*cutpnt + (iwork[coltyp+1]+iwork[coltyp+2])*(n-cutpnt) + iq2;
magma_dlaex3(k, n, cutpnt, d, q, ldq, rho,
&work[idlmda], &work[iq2], &iwork[indxc],
&iwork[coltyp], &work[iw], &work[is],
indxq, dwork, range, vl, vu, il, iu, queue, info );
if( *info != 0 )
return MAGMA_SUCCESS;
}
else {
for (i = 0; i<n; ++i)
indxq[i] = i+1;
}
return MAGMA_SUCCESS;
} /* magma_dlaex1 */
void Foam::sixDoFRigidBodyMotion::applyConstraints(scalar deltaT)
{
if (constraints_.empty())
{
return;
}
if (Pstream::master())
{
label iteration = 0;
bool allConverged = true;
// constraint force accumulator
vector cFA = vector::zero;
// constraint moment accumulator
vector cMA = vector::zero;
do
{
allConverged = true;
forAll(constraints_, cI)
{
if (sixDoFRigidBodyMotionConstraint::debug)
{
Info<< "Constraint " << constraintNames_[cI] << ": ";
}
// constraint position
point cP = vector::zero;
// constraint force
vector cF = vector::zero;
// constraint moment
vector cM = vector::zero;
bool constraintConverged = constraints_[cI].constrain
(
*this,
cFA,
cMA,
deltaT,
cP,
cF,
cM
);
allConverged = allConverged && constraintConverged;
// Accumulate constraint force
cFA += cF;
// Accumulate constraint moment
cMA += cM + ((cP - centreOfMass()) ^ cF);
}
} while(++iteration < maxConstraintIterations_ && !allConverged);
if (iteration >= maxConstraintIterations_)
{
FatalErrorIn
(
"Foam::sixDoFRigidBodyMotion::applyConstraints"
"(scalar deltaT)"
)
<< nl << "Maximum number of sixDoFRigidBodyMotion constraint "
<< "iterations ("
<< maxConstraintIterations_
<< ") exceeded." << nl
<< exit(FatalError);
}
Info<< "sixDoFRigidBodyMotion constraints converged in "
<< iteration << " iterations" << endl;
if (report_)
{
Info<< "Constraint force: " << cFA << nl
<< "Constraint moment: " << cMA
<< endl;
}
// Add the constraint forces and moments to the motion state variables
a() += cFA/mass_;
// The moment of constraint forces has already been added
// during accumulation. Moments are returned in global axes,
// transforming to body local
tau() += Q().T() & cMA;
}
}
//
// inverse approximation
//
#include <stdio.h>
#include <stdlib.h>
#include "liquidfpm.internal.h"
#define DEBUG_INV_NEWTON 0
#define Q(name) LIQUIDFPM_CONCAT(q32,name)
// computes x = inv(d) = 1/d using iterative Newtonian method:
// x[k+1] = x[k] + x[k]*(1 - d*x[k])
Q(_t) Q(_inv_newton)( Q(_t) _x, unsigned int _n )
{
int negate = (_x < 0);
_x = Q(_abs)(_x);
// initial guess: x0 = 2^-floor(log2(|_x|))
int b = liquidfpm_msb_index(_x) - 1; // base index
int s = (int)Q(_fracbits) - b - 1; // shift amount
Q(_t) x0 = s>0 ? Q(_one)<<s : Q(_one)>>(-s);
Q(_t) x1=0;
Q(_t) y0=0;
Q(_t) d=_x;
Q(_t) dx0=0;
Q(_t) x0y0=0;
void SOGP::train(const VectorXd &state, const VectorXd &output) {
//check if we have initialised the system
if (!this->initialized) {
throw OTLException("SOGP not yet initialised");
}
double kstar = this->kernel->eval(state);
//change the output format if this is a classification problem
VectorXd mod_output;
if (this->problem_type == SOGP::CLASSIFICATION) {
mod_output = VectorXd::Zero(this->output_dim);
for (unsigned int i=0; i<this->output_dim; i++) {
mod_output(i) = -1;
}
mod_output(output(0)) = 1;
} else {
mod_output = output;
}
//we are just starting.
if (this->current_size == 0) {
this->alpha.block(0,0,1, this->output_dim) = (mod_output.array() / (kstar + this->noise)).transpose();
this->C.block(0,0,1,1) = VectorXd::Ones(1)*-1/(kstar + this->noise);
this->Q.block(0,0,1,1) = VectorXd::Ones(1)*1/(kstar);
this->basis_vectors.push_back(state);
this->current_size++;
return;
}
//Test if this is a "novel" state
VectorXd k;
this->kernel->eval(state, this->basis_vectors, k);
//cache Ck
VectorXd Ck = this->C.block(0,0, this->current_size, this->current_size)*k;
VectorXd m = k.transpose()*this->alpha.block(0,0,this->current_size, this->output_dim);
double s2 = kstar + (k.dot(Ck));
if (s2 < 1e-12) {
//std::cout << "s2: " << s2 << std::endl;
s2 = 1e-12;
}
double r = 0.0;
VectorXd q;
if (this->problem_type == SOGP::REGRESSION) {
r = -1.0/(s2 + this->noise);
q = (mod_output - m)*(-r);
} else if (this->problem_type == SOGP::CLASSIFICATION) {
double sx2 = this->noise + s2;
double sx = sqrt(sx2);
VectorXd z = VectorXd(this->output_dim);
VectorXd Erfz = VectorXd(this->output_dim);
for (unsigned int i=0; i<this->output_dim; i++) {
z(i) = mod_output(i) * m(i) / sx;
Erfz(i) = stdnormcdf(z(i));
//dErfz(i) = 1.0/sqrt(2*M_PI)*exp(-(z(i)*z(i))/2.0);
//dErfz2(i) = dErfz(i)*(-z(i));
}
/*
TO CONNTINUE
Erfz = Erf(z);
dErfz = 1.0/sqrt(2*pi)*exp(-(z.^2)/2);
dErfz2 = dErfz.*(-z);
q = y/sx * (dErfz/Erfz);
r = (1/sx2)*(dErfz2/dErfz - (dErfz/Erfz)^2);
*/
} else {
throw OTL::OTLException("Whoops! My problem type is wrong. How did this happen?");
}
VectorXd ehat = this->Q.block(0,0, this->current_size, this->current_size)*k;
double gamma = kstar - k.dot(ehat);
double eta = 1.0/(1.0 + gamma*r);
if (gamma < 1e-12) {
gamma = 0.0;
}
if (gamma >= this->epsilon*kstar) {
//perform a full update
VectorXd s = Ck;
s.conservativeResize(this->current_size + 1);
s(this->current_size) = 1;
//update Q (inverse of C)
ehat.conservativeResize(this->current_size+1);
ehat(this->current_size) = -1;
MatrixXd diffQ = Q.block(0,0,this->current_size+1, this->current_size+1)
+ (ehat*ehat.transpose())*(1.0/gamma);
Q.block(0,0,this->current_size+1, this->current_size+1) = diffQ;
//update alpha
MatrixXd diffAlpha = alpha.block(0,0, this->current_size+1, this->output_dim)
+ (s*q.transpose());
alpha.block(0,0, this->current_size+1, this->output_dim) = diffAlpha;
//update C
MatrixXd diffC = C.block(0,0, this->current_size+1, this->current_size+1)
+ r*(s*s.transpose());
C.block(0,0, this->current_size+1, this->current_size+1) = diffC;
//add to basis vectors
this->basis_vectors.push_back(state);
//increment current size
this->current_size++;
} else {
//perform a sparse update
VectorXd s = Ck + ehat;
//update alpha
MatrixXd diffAlpha = alpha.block(0,0, this->current_size, this->output_dim)
+ s*((q*eta).transpose());
alpha.block(0,0, this->current_size, this->output_dim) = diffAlpha;
//update C
MatrixXd diffC = C.block(0,0, this->current_size, this->current_size) +
r*eta*(s*s.transpose());
C.block(0,0, this->current_size, this->current_size) = diffC;
}
//check if we need to reduce size
if (this->basis_vectors.size() > this->capacity) {
//std::cout << "Reduction!" << std::endl;
double min_val = (alpha.row(0)).squaredNorm()/(Q(0,0) + C(0,0));
unsigned int min_index = 0;
for (unsigned int i=1; i<this->basis_vectors.size(); i++) {
double scorei = (alpha.row(i)).squaredNorm()/(Q(i,i) + C(i,i));
if (scorei < min_val) {
min_val = scorei;
min_index = i;
}
}
this->reduceBasisVectorSet(min_index);
}
return;
}
void EN
( const DistSparseMatrix<Real>& A,
const DistMultiVec<Real>& b,
Real lambda1,
Real lambda2,
DistMultiVec<Real>& x,
const qp::affine::Ctrl<Real>& ctrl )
{
EL_DEBUG_CSE
const Int m = A.Height();
const Int n = A.Width();
const Grid& grid = A.Grid();
DistSparseMatrix<Real> Q(grid), AHat(grid), G(grid);
DistMultiVec<Real> c(grid), h(grid);
// Q := | 2*lambda_2 0 0 |
// | 0 2*lambda_2 0 |
// | 0 0 2 |
// ================================
Zeros( Q, 2*n+m, 2*n+m );
Q.Reserve( Q.LocalHeight() );
for( Int iLoc=0; iLoc<Q.LocalHeight(); ++iLoc )
{
const Int i = Q.GlobalRow(iLoc);
if( i < 2*n )
Q.QueueLocalUpdate( iLoc, i, 2*lambda2 );
else
Q.QueueLocalUpdate( iLoc, i, Real(2) );
}
Q.ProcessLocalQueues();
// c := lambda_1*[1;1;0]
// =====================
Zeros( c, 2*n+m, 1 );
for( Int iLoc=0; iLoc<c.LocalHeight(); ++iLoc )
if( c.GlobalRow(iLoc) < 2*n )
c.SetLocal( iLoc, 0, lambda1 );
// \hat A := [A, -A, I]
// ====================
// NOTE: Since A and \hat A are the same height and each distributed within
// columns, it is possible to form \hat A from A without communication
const Int numLocalEntriesA = A.NumLocalEntries();
Zeros( AHat, m, 2*n+m );
AHat.Reserve( 2*numLocalEntriesA+AHat.LocalHeight() );
for( Int e=0; e<numLocalEntriesA; ++e )
{
AHat.QueueUpdate( A.Row(e), A.Col(e), A.Value(e) );
AHat.QueueUpdate( A.Row(e), A.Col(e)+n, -A.Value(e) );
}
for( Int iLoc=0; iLoc<AHat.LocalHeight(); ++iLoc )
{
const Int i = AHat.GlobalRow(iLoc);
AHat.QueueLocalUpdate( iLoc, i+2*n, Real(1) );
}
AHat.ProcessLocalQueues();
// G := | -I 0 0 |
// | 0 -I 0 |
// ================
Zeros( G, 2*n, 2*n+m );
G.Reserve( G.LocalHeight() );
for( Int iLoc=0; iLoc<G.LocalHeight(); ++iLoc )
{
const Int i = G.GlobalRow(iLoc);
G.QueueLocalUpdate( iLoc, i, Real(-1) );
}
G.ProcessLocalQueues();
// h := 0
// ======
Zeros( h, 2*n, 1 );
// Solve the affine QP
// ===================
DistMultiVec<Real> xHat(grid), y(grid), z(grid), s(grid);
QP( Q, AHat, G, b, c, h, xHat, y, z, s, ctrl );
// x := u - v
// ==========
Zeros( x, n, 1 );
Int numRemoteUpdates = 0;
for( Int iLoc=0; iLoc<xHat.LocalHeight(); ++iLoc )
if( xHat.GlobalRow(iLoc) < 2*n )
++numRemoteUpdates;
else
break;
x.Reserve( numRemoteUpdates );
for( Int iLoc=0; iLoc<xHat.LocalHeight(); ++iLoc )
{
const Int i = xHat.GlobalRow(iLoc);
if( i < n )
x.QueueUpdate( i, 0, xHat.GetLocal(iLoc,0) );
else if( i < 2*n )
x.QueueUpdate( i-n, 0, -xHat.GetLocal(iLoc,0) );
else
break;
}
x.ProcessQueues();
}
void EN
( const Matrix<Real>& A,
const Matrix<Real>& b,
Real lambda1,
Real lambda2,
Matrix<Real>& x,
const qp::affine::Ctrl<Real>& ctrl )
{
EL_DEBUG_CSE
const Int m = A.Height();
const Int n = A.Width();
const Range<Int> uInd(0,n), vInd(n,2*n), rInd(2*n,2*n+m);
Matrix<Real> Q, c, AHat, G, h;
// Q := | 2*lambda_2 0 0 |
// | 0 2*lambda_2 0 |
// | 0 0 2 |
// ================================
Zeros( Q, 2*n+m, 2*n+m );
auto QTL = Q( IR(0,2*n), IR(0,2*n) );
FillDiagonal( QTL, 2*lambda2 );
auto Qrr = Q( rInd, rInd );
FillDiagonal( Qrr, Real(1) );
// c := lambda_1*[1;1;0]
// =====================
Zeros( c, 2*n+m, 1 );
auto cuv = c( IR(0,2*n), ALL );
Fill( cuv, lambda1 );
// \hat A := [A, -A, I]
// ====================
Zeros( AHat, m, 2*n+m );
auto AHatu = AHat( ALL, uInd );
auto AHatv = AHat( ALL, vInd );
auto AHatr = AHat( ALL, rInd );
AHatu = A;
AHatv -= A;
FillDiagonal( AHatr, Real(1) );
// G := | -I 0 0 |
// | 0 -I 0 |
// ================
Zeros( G, 2*n, 2*n+m );
FillDiagonal( G, Real(-1) );
// h := 0
// ======
Zeros( h, 2*n, 1 );
// Solve the affine QP
// ===================
Matrix<Real> xHat, y, z, s;
QP( Q, AHat, G, b, c, h, xHat, y, z, s, ctrl );
// x := u - v
// ==========
x = xHat( uInd, ALL );
x -= xHat( vInd, ALL );
}
void EN
( const AbstractDistMatrix<Real>& A,
const AbstractDistMatrix<Real>& b,
Real lambda1,
Real lambda2,
AbstractDistMatrix<Real>& xPre,
const qp::affine::Ctrl<Real>& ctrl )
{
EL_DEBUG_CSE
DistMatrixWriteProxy<Real,Real,MC,MR> xProx( xPre );
auto& x = xProx.Get();
const Int m = A.Height();
const Int n = A.Width();
const Grid& g = A.Grid();
const Range<Int> uInd(0,n), vInd(n,2*n), rInd(2*n,2*n+m);
DistMatrix<Real> Q(g), c(g), AHat(g), G(g), h(g);
// Q := | 2*lambda_2 0 0 |
// | 0 2*lambda_2 0 |
// | 0 0 2 |
// ================================
Zeros( Q, 2*n+m, 2*n+m );
auto QTL = Q( IR(0,2*n), IR(0,2*n) );
FillDiagonal( QTL, 2*lambda2 );
auto Qrr = Q( rInd, rInd );
FillDiagonal( Qrr, Real(1) );
// c := lambda_1*[1;1;0]
// =====================
Zeros( c, 2*n+m, 1 );
auto cuv = c( IR(0,2*n), ALL );
Fill( cuv, lambda1 );
// \hat A := [A, -A, I]
// ====================
Zeros( AHat, m, 2*n+m );
auto AHatu = AHat( ALL, uInd );
auto AHatv = AHat( ALL, vInd );
auto AHatr = AHat( ALL, rInd );
AHatu = A;
AHatv -= A;
FillDiagonal( AHatr, Real(1) );
// G := | -I 0 0 |
// | 0 -I 0 |
// ================
Zeros( G, 2*n, 2*n+m );
FillDiagonal( G, Real(-1) );
// h := 0
// ======
Zeros( h, 2*n, 1 );
// Solve the affine QP
// ===================
DistMatrix<Real> xHat(g), y(g), z(g), s(g);
QP( Q, AHat, G, b, c, h, xHat, y, z, s, ctrl );
// x := u - v
// ==========
x = xHat( uInd, ALL );
x -= xHat( vInd, ALL );
}
static RBuffer* create(RBin* bin, const ut8 *code, int codelen, const ut8 *data, int datalen) {
ut64 filesize, codeva, datava;
ut32 ncmds, magiclen, headerlen;
ut64 p_codefsz=0, p_codeva=0, p_codesz=0, p_codepa=0;
ut64 p_datafsz=0, p_datava=0, p_datasz=0, p_datapa=0;
ut64 p_cmdsize=0, p_entry=0, p_tmp=0;
ut64 baddr = 0x100001000LL;
// TODO: baddr must be overriden with -b
RBuffer *buf = r_buf_new ();
#define B(x,y) r_buf_append_bytes(buf,(const ut8*)x,y)
#define D(x) r_buf_append_ut32(buf,x)
#define Q(x) r_buf_append_ut64(buf,x)
#define Z(x) r_buf_append_nbytes(buf,x)
#define W(x,y,z) r_buf_write_at(buf,x,(const ut8*)y,z)
#define WZ(x,y) p_tmp=buf->length;Z(x);W(p_tmp,y,strlen(y))
/* MACH0 HEADER */
// 32bit B ("\xce\xfa\xed\xfe", 4); // header
B ("\xcf\xfa\xed\xfe", 4); // header
D (7 | 0x01000000); // cpu type (x86) | ABI64
//D (3); // subtype (i386-all)
D(0x80000003); // unknown subtype issue
D (2); // filetype (executable)
ncmds = (data && datalen>0)? 3: 2;
/* COMMANDS */
D (ncmds); // ncmds
p_cmdsize = buf->length;
D (-1); // headsize // cmdsize?
D (0);//0x85); // flags
D (0); // reserved -- only found in x86-64
magiclen = buf->length;
/* TEXT SEGMENT */
D (0x19); // cmd.LC_SEGMENT_64
//D (124+16+8); // sizeof (cmd)
D (124+28); // sizeof (cmd)
WZ (16, "__TEXT");
Q (baddr); // vmaddr
Q (0x1000); // vmsize XXX
Q (0); // fileoff
p_codefsz = buf->length;
Q (-1); // filesize
D (7); // maxprot
D (5); // initprot
D (1); // nsects
D (0); // flags
// define section
WZ (16, "__text");
WZ (16, "__TEXT");
p_codeva = buf->length; // virtual address
Q (-1);
p_codesz = buf->length; // size of code (end-start)
Q (-1);
p_codepa = buf->length; // code - baddr
D (-1); // offset, _start-0x1000);
D (2); // align
D (0); // reloff
D (0); // nrelocs
D (0); // flags
D (0); // reserved1
D (0); // reserved2
D (0); // reserved3
if (data && datalen>0) {
/* DATA SEGMENT */
D (0x19); // cmd.LC_SEGMENT_64
D (124+28); // sizeof (cmd)
p_tmp = buf->length;
Z (16);
W (p_tmp, "__TEXT", 6); // segment name
//XXX must be vmaddr+baddr
Q (0x2000); // vmaddr
//XXX must be vmaddr+baddr
Q (0x1000); // vmsize
Q (0); // fileoff
p_datafsz = buf->length;
Q (-1); // filesize
D (6); // maxprot
D (6); // initprot
D (1); // nsects
D (0); // flags
WZ (16, "__data");
WZ (16, "__DATA");
p_datava = buf->length;
Q (-1);
p_datasz = buf->length;
Q (-1);
p_datapa = buf->length;
D (-1); //_start-0x1000);
D (2); // align
D (0); // reloff
D (0); // nrelocs
D (0); // flags
D (0); // reserved1
D (0); // reserved2
D (0); // reserved3
}
#define STATESIZE (21*sizeof (ut64))
/* THREAD STATE */
D (5); // LC_UNIXTHREAD
D (184); // sizeof (cmd)
D (4); // 1=i386, 4=x86_64
D (42); // thread-state-count
p_entry = buf->length + (16*sizeof (ut64));
Z (STATESIZE);
headerlen = buf->length - magiclen;
codeva = buf->length + baddr;
datava = buf->length + codelen + baddr;
W (p_entry, &codeva, 8); // set PC
/* fill header variables */
W (p_cmdsize, &headerlen, 4);
filesize = magiclen + headerlen + codelen + datalen;
// TEXT SEGMENT //
W (p_codefsz, &filesize, 8);
W (p_codeva, &codeva, 8);
{
ut64 clen = codelen;
W (p_codesz, &clen, 8);
}
p_tmp = codeva - baddr;
W (p_codepa, &p_tmp, 8);
B (code, codelen);
if (data && datalen>0) {
/* append data */
W (p_datafsz, &filesize, 8);
W (p_datava, &datava, 8);
W (p_datasz, &datalen, 8);
p_tmp = datava - baddr;
W (p_datapa, &p_tmp, 8);
B (data, datalen);
}
return buf;
}
/* FIXME: use lldiv once it's fixed to compute quot,rem together */
struct tm *__time_to_tm(time_t t, struct tm *tm)
{
/* months are march-based */
static const int days_thru_month[] = {31,61,92,122,153,184,214,245,275,306,337,366};
long long bigday;
unsigned int day, year4, year100;
int year, year400;
int month;
int leap;
int hour, min, sec;
int wday, mday, yday;
/* start from 2000-03-01 (multiple of 400 years) */
t += -946684800 - 86400*(31+29);
bigday = Q(t, 86400);
sec = t-bigday*86400;
hour = sec/3600;
sec -= hour*3600;
min = sec/60;
sec -= min*60;
/* 2000-03-01 was a wednesday */
wday = (3+bigday)%7;
if (wday < 0) wday += 7;
t = -946684800LL - 86400*(31+29) + 9000000;
year400 = Q(bigday, DAYS_PER_400Y);
day = bigday-year400*DAYS_PER_400Y;
year100 = day/DAYS_PER_100Y;
if (year100 == 4) year100--;
day -= year100*DAYS_PER_100Y;
year4 = day/DAYS_PER_4Y;
if (year4 == 25) year4--;
day -= year4*DAYS_PER_4Y;
year = day/365;
if (year == 4) year--;
day -= year*365;
leap = !year && (year4 || !year100);
yday = day + 31+28 + leap;
if (yday >= 365+leap) yday -= 365+leap;
year += 4*year4 + 100*year100 + 400*year400 + 2000-1900;
for (month=0; days_thru_month[month] <= day; month++);
if (month) day -= days_thru_month[month-1];
month += 2;
if (month >= 12) {
month -= 12;
year++;
}
mday = day+1;
tm->tm_sec = sec;
tm->tm_min = min;
tm->tm_hour= hour;
tm->tm_mday= mday;
tm->tm_mon = month;
tm->tm_year= year;
tm->tm_wday= wday;
tm->tm_yday= yday;
tm->__tm_zone = 0;
tm->__tm_gmtoff = 0;
return tm;
}
QuestManager::QuestManager() {
Q("Test Quest", A1Q1_Test, a1q1);
}
int HRP2ColorDetectionProcess::RealizeTheProcess()
{
if (!m_Computing)
return 0;
unsigned int lw,lh;
double x[m_NbOfCameras],y[m_NbOfCameras];
int x1[2]={-1,-1},x2[2]={-1,-1};
double a3DPt[3];
VNL::Vector<double> Q(4);
VNL::Vector<double> QHead(3);
if (m_FirstTime)
{
ReadRobotMatrix();
m_FirstTime = false;
}
for(int i=0;i<m_NbOfCameras;i++)
{
m_ImageInputRGB[i]->GetSize(lw,lh);
/* VW::ImageConversions::RGBBuffer_to_VWImage((char *)m_InputImage[i].Image,
m_ImageInputRGB[i],lw,lh);*/
m_ColorDetector[i].FilterOnHistogram((unsigned char *)m_InputImage[i].Image,m_ImageResult[i]);
{
char Buffer[1024];
sprintf(Buffer,"IntermediateImageResult_%03d.ppm",i);
m_ImageResult[i]->WriteImage(Buffer);
}
m_ColorDetector[i].ComputeCoG(m_ImageResult[i],x[i],y[i]);
if ((i==1) && (x[0]>=0.0) && (x[1]>=0.0)
&& (y[0]>=0.0) && (y[1]>=0.0))
{
x1[0] = (int) x[0]; x1[1] = (int)y[0];
x2[0] = (int) x[1]; x2[1] = (int)y[1];
m_TM.Triangulation(*m_PI[0],*m_PI[1],x1,x2,a3DPt);
for(int j=0;j<3;j++)
Q[j] = a3DPt[j];
Q[3] = 1.0;
QHead = m_HtO * Q;
QHead *= 0.001;
ODEBUG( "Triangulation: "
<< a3DPt[0] << " "
<< a3DPt[1] << " "
<< a3DPt[2]);
ODEBUG3("QHead :" << QHead <<endl );
#if 0
if (!CORBA::is_nil(m_VisualServoing))
{
try
{
// m_VisualServoing->SendTarget2DPosition(x[i],y[i]);
CORBA::Double x_left = x[0];
CORBA::Double y_left = y[0];
CORBA::Double X = QHead[0];
CORBA::Double Y = QHead[1];
CORBA::Double Z = QHead[2];
m_VisualServoing->SendTarget3DPosition(x_left,y_left,X,Y,Z);
ODEBUG("Send " << x << " " << y << " to visual servoing");
}
catch(...)
{
ODEBUG("Sorry the data was not send to the visual servoing plugin" );
if (m_NbOfProcessWithoutTrialConnection>m_IntervalBetweenConnectionTrials)
{
CORBA::release(m_VisualServoing);
TryConnectionToVisualServoing();
m_NbOfProcessWithoutTrialConnection=0;
}
else
m_NbOfProcessWithoutTrialConnection++;
}
}
else
{
if (m_NbOfProcessWithoutTrialConnection>m_IntervalBetweenConnectionTrials)
{
TryConnectionToVisualServoing();
m_NbOfProcessWithoutTrialConnection=0;
}
else
m_NbOfProcessWithoutTrialConnection++;
}
#endif
}
/*
if ((x==-1) && (y==-1))
{
epbm_save("ImageInput.epbm",&m_InputImage[i],0);
m_ImageResult[i]->WriteImage("ImageIntermediate.pgm");
exit(0);
//m_ImageInputRGB[0]->WriteImage("ImageInput.ppm");
}
ODEBUG3("Center ( " << i << " ) :"<< x << " " << y ); */
}
return 0;
}
int main( ){
USING_NAMESPACE_ACADO
// INTRODUCE THE VARIABLES:
// -------------------------
DifferentialState x,y,z,vx,vy,vz,phi,theta,psi,p,q,r,u1,u2,u3,u4;
// x, y, z : position
// vx, vy, vz : linear velocity
// phi, theta, psi : orientation (Yaw-Pitch-Roll = Euler(3,2,1))
// p, q, r : angular velocity
// u1, u2, u3, u4 : velocity of the propellers
Control vu1,vu2,vu3,vu4;
// vu1, vu2, vu3, vu4 : derivative of u1, u2, u3, u4
DifferentialEquation f;
// Quad constants
const double c = 0.00001;
const double Cf = 0.00065;
const double d = 0.250;
const double Jx = 0.018;
const double Jy = 0.018;
const double Jz = 0.026;
const double Im = 0.0001;
const double m = 0.9;
const double g = 9.81;
const double Cx = 0.1;
// Minimization Weights
double coeffX = .00001;
double coeffU = coeffX*0.0001;//0.000000000000001;
double coeffX2 = coeffX * 100.;
double coeffX3 = coeffX * 0.00001;
double coeffO = -coeffX * 0.1;
// final position (used in the cost function)
double xf = 0., yf = 0., zf = 20.;
//
double T = 8.; //length (in second) of the trajectory predicted in the MPC
int nb_nodes = 20.; //number of nodes used in the Optimal Control Problem
// 20 nodes means that the algorithm will discretized the trajectory equally into 20 pieces
// If you increase the number of node, the solution will be more precise but calculation will take longer (~nb_nodes^2)
// In ACADO, the commands are piecewise constant functions, constant between each node.
double tmpc = 0.2; //time (in second) between two activation of the MPC algorithm
// DEFINE A OPTIMAL CONTROL PROBLEM
// -------------------------------
OCP ocp( 0.0, T, nb_nodes );
// DEFINE THE COST FUNCTION
// -------------------------------
Function h, hf;
h << x;
h << y;
h << z;
h << vu1;
h << vu2;
h << vu3;
h << vu4;
h << p;
h << q;
h << r;
hf << x;
hf << y;
hf << z;
DMatrix Q(10,10), Qf(3,3);
Q(0,0) = coeffX;
Q(1,1) = coeffX;
Q(2,2) = coeffX;
Q(3,3) = coeffU;
Q(4,4) = coeffU;
Q(5,5) = coeffU;
Q(6,6) = coeffU;
Q(7,7) = coeffX2;
Q(8,8) = coeffX2;
Q(9,9) = coeffX2;
Qf(0,0) = 1.;
Qf(1,1) = 1.;
Qf(2,2) = 1.;
DVector reff(3), ref(10);
ref(0) = xf;
ref(1) = yf;
ref(2) = zf;
ref(3) = 58.;
ref(4) = 58.;
ref(5) = 58.;
ref(6) = 58.;
ref(7) = 0.;
ref(8) = 0.;
ref(9) = 0.;
reff(0) = xf;
reff(1) = yf;
reff(2) = zf;
// The cost function is define as : integrale from 0 to T { transpose(h(x,u,t)-ref)*Q*(h(x,u,t)-ref) } + transpose(hf(x,u,T))*Qf*hf(x,u,T)
// == integrale cost (also called running cost or Lagrange Term) + final cost (Mayer Term)
ocp.minimizeLSQ ( Q, h, ref);
// ocp.minimizeLSQEndTerm(Qf, hf, reff);
// When doing MPC, you need terms in the cost function to stabilised the system => p, q, r and vu1, vu2, vu3, vu4. You can check that if you reduce their weights "coeffX2" or "coeffU" too low, the optimization will crashed.
// DEFINE THE DYNAMIC EQUATION OF THE SYSTEM:
// ----------------------------------
f << dot(x) == vx;
f << dot(y) == vy;
f << dot(z) == vz;
f << dot(vx) == Cf*(u1*u1+u2*u2+u3*u3+u4*u4)*sin(theta)/m;
f << dot(vy) == -Cf*(u1*u1+u2*u2+u3*u3+u4*u4)*sin(psi)*cos(theta)/m;
f << dot(vz) == Cf*(u1*u1+u2*u2+u3*u3+u4*u4)*cos(psi)*cos(theta)/m - g;
f << dot(phi) == -cos(phi)*tan(theta)*p+sin(phi)*tan(theta)*q+r;
f << dot(theta) == sin(phi)*p+cos(phi)*q;
f << dot(psi) == cos(phi)/cos(theta)*p-sin(phi)/cos(theta)*q;
f << dot(p) == (d*Cf*(u1*u1-u2*u2)+(Jy-Jz)*q*r)/Jx;
f << dot(q) == (d*Cf*(u4*u4-u3*u3)+(Jz-Jx)*p*r)/Jy;
f << dot(r) == (c*(u1*u1+u2*u2-u3*u3-u4*u4)+(Jx-Jy)*p*q)/Jz;
f << dot(u1) == vu1;
f << dot(u2) == vu2;
f << dot(u3) == vu3;
f << dot(u4) == vu4;
// ---------------------------- DEFINE CONTRAINTES --------------------------------- //
//Dynamic
ocp.subjectTo( f );
//State constraints
//Constraints on the velocity of each propeller
ocp.subjectTo( 16 <= u1 <= 95 );
ocp.subjectTo( 16 <= u2 <= 95 );
ocp.subjectTo( 16 <= u3 <= 95 );
ocp.subjectTo( 16 <= u4 <= 95 );
//Command constraints
//Constraints on the acceleration of each propeller
ocp.subjectTo( -100 <= vu1 <= 100 );
ocp.subjectTo( -100 <= vu2 <= 100 );
ocp.subjectTo( -100 <= vu3 <= 100 );
ocp.subjectTo( -100 <= vu4 <= 100 );
#if AVOID_SINGULARITIES == 1
//Constraint to avoid singularity
// In this example I used Yaw-Pitch-Roll convention to describe orientation of the quadrotor
// when using Euler Angles representation, you always have a singularity, and we need to
// avoid it otherwise the algorithm will crashed.
ocp.subjectTo( -1. <= theta <= 1.);
#endif
//Example of Eliptic obstacle constraints (here, cylinders with eliptic basis)
ocp.subjectTo( 16 <= ((x+3)*(x+3)+2*(z-5)*(z-5)) );
ocp.subjectTo( 16 <= ((x-3)*(x-3)+2*(z-9)*(z-9)) );
ocp.subjectTo( 16 <= ((x+3)*(x+3)+2*(z-15)*(z-15)) );
// SETTING UP THE (SIMULATED) PROCESS:
// -----------------------------------
OutputFcn identity;
// The next line define the equation used to simulate the system :
// here "f" is used (=the same as the one used for the ocp) so the trajectory simulated
// from the commands will be exactly the same as the one calculated by the MPC.
// If for instance you want to test robusness, you can define an other dynamic equation "f2"
// with changed parameters and put it here.
DynamicSystem dynamicSystem( f,identity );
Process process( dynamicSystem,INT_RK45 );
// SETTING UP THE MPC CONTROLLER:
// ------------------------------
RealTimeAlgorithm alg( ocp, tmpc );
//Usually, you do only one step of the optimisation algorithm (~Gauss-Newton here)
//at each activation of the MPC, that way the delay between getting the state and
//sending a command is as quick as possible.
alg.set( MAX_NUM_ITERATIONS, 1 );
StaticReferenceTrajectory zeroReference;
Controller controller( alg,zeroReference );
// SET AN INITIAL GUESS FOR THE FIRST MPC LOOP (NEXT LOOPS WILL USE AS INITIAL GUESS THE SOLUTION FOUND AT THE PREVIOUS MPC LOOP)
Grid timeGrid(0.0,T,nb_nodes+1);
VariablesGrid x_init(16, timeGrid);
// init with static
for (int i = 0 ; i<nb_nodes+1 ; i++ ) {
x_init(i,0) = 0.;
x_init(i,1) = 0.;
x_init(i,2) = 0.;
x_init(i,3) = 0.;
x_init(i,4) = 0.;
x_init(i,5) = 0.;
x_init(i,6) = 0.;
x_init(i,7) = 0.;
x_init(i,8) = 0.;
x_init(i,9) = 0.;
x_init(i,10) = 0.;
x_init(i,11) = 0.;
x_init(i,12) = 58.; //58. is the propeller rotation speed so the total thrust balance the weight of the quad
x_init(i,13) = 58.;
x_init(i,14) = 58.;
x_init(i,15) = 58.;
}
alg.initializeDifferentialStates(x_init);
// SET OPTION AND PLOTS WINDOW
// ---------------------------
// Linesearch is an algorithm which will try several points along the descent direction to choose a better step length.
// It looks like activating this option produice more stable trajectories.198
alg.set( GLOBALIZATION_STRATEGY, GS_LINESEARCH );
alg.set(INTEGRATOR_TYPE, INT_RK45);
// You can uncomment those lines to see how the predicted trajectory involve along time
// (but be carefull because you will have 1 ploting window per MPC loop)
// GnuplotWindow window1(PLOT_AT_EACH_ITERATION);
// window1.addSubplot( z,"DifferentialState z" );
// window1.addSubplot( x,"DifferentialState x" );
// window1.addSubplot( theta,"DifferentialState theta" );
// window1.addSubplot( 16./((x+3)*(x+3)+4*(z-5)*(z-5)),"Dist obs1" );
// window1.addSubplot( 16./((x-3)*(x-3)+4*(z-9)*(z-9)),"Dist obs2" );
// window1.addSubplot( 16./((x+2)*(x+2)+4*(z-15)*(z-15)),"Dist obs3" );
// alg<<window1;
// SETTING UP THE SIMULATION ENVIRONMENT, RUN THE EXAMPLE...
// ----------------------------------------------------------
// The first argument is the starting time, the second the end time.
SimulationEnvironment sim( 0.0,10.,process,controller );
//Setting the state of the sytem at the beginning of the simulation.
DVector x0(16);
x0.setZero();
x0(0) = 0.;
x0(12) = 58.;
x0(13) = 58.;
x0(14) = 58.;
x0(15) = 58.;
t = clock();
if (sim.init( x0 ) != SUCCESSFUL_RETURN)
exit( EXIT_FAILURE );
if (sim.run( ) != SUCCESSFUL_RETURN)
exit( EXIT_FAILURE );
t = clock() - t;
std::cout << "total time : " << (((float)t)/CLOCKS_PER_SEC)<<std::endl;
// ...SAVE THE RESULTS IN FILES
// ----------------------------------------------------------
std::ofstream file;
file.open("/tmp/log_state.txt",std::ios::out);
std::ofstream file2;
file2.open("/tmp/log_control.txt",std::ios::out);
VariablesGrid sampledProcessOutput;
sim.getSampledProcessOutput( sampledProcessOutput );
sampledProcessOutput.print(file);
VariablesGrid feedbackControl;
sim.getFeedbackControl( feedbackControl );
feedbackControl.print(file2);
// ...AND PLOT THE RESULTS
// ----------------------------------------------------------
GnuplotWindow window;
window.addSubplot( sampledProcessOutput(0), "x " );
window.addSubplot( sampledProcessOutput(1), "y " );
window.addSubplot( sampledProcessOutput(2), "z " );
window.addSubplot( sampledProcessOutput(6),"phi" );
window.addSubplot( sampledProcessOutput(7),"theta" );
window.addSubplot( sampledProcessOutput(8),"psi" );
window.plot( );
graphics::corbaServer::ClientCpp client = graphics::corbaServer::ClientCpp();
client.createWindow("window");
return 0;
}
void
MergeWidget::onBrowseChapters() {
auto fileName = getOpenFileName(QY("Select chapter file"), QY("XML files") + Q(" (*.xml)"), ui->chapters);
if (!fileName.isEmpty())
m_config.m_chapters = fileName;
}
hkDemo::Result MirroredMotionDemo::stepDemo()
{
// Check user input
{
if (m_env->m_gamePad->wasButtonPressed( HKG_PAD_BUTTON_1 ))
{
for ( int i = 0; i < 2; i++ )
{
if (( m_control[i]->getEaseStatus() == hkaDefaultAnimationControl::EASING_IN ) ||
( m_control[i]->getEaseStatus() == hkaDefaultAnimationControl::EASED_IN ))
{
m_control[i]->easeOut( .5f );
}
else
{
m_control[i]->easeIn( .5f );
}
}
}
if (m_env->m_gamePad->wasButtonPressed( HKG_PAD_BUTTON_2 ))
{
m_wireframe = !m_wireframe;
setGraphicsState( HKG_ENABLED_WIREFRAME, m_wireframe );
}
if (m_env->m_gamePad->wasButtonPressed( HKG_PAD_BUTTON_3 ))
{
m_drawSkin = !m_drawSkin;
}
if ( m_env->m_gamePad->wasButtonPressed( HKG_PAD_BUTTON_0 ) )
{
m_useExtractedMotion = !m_useExtractedMotion;
}
if ( m_env->m_gamePad->wasButtonPressed( HKG_PAD_BUTTON_L1 ) )
{
m_accumulatedMotion[0].setIdentity();
m_accumulatedMotion[1].setIdentity();
}
if ( m_env->m_gamePad->wasButtonPressed( HKG_PAD_BUTTON_R1 ) )
{
m_paused = !m_paused;
if ( m_paused )
{
m_timestep = 0;
}
else
{
m_timestep = 1.0f / 60.0f;
}
}
}
const int boneCount = m_skeleton->m_numBones;
for ( int j = 0; j < 2; j++ )
{
// Grab accumulated motion
{
hkQsTransform deltaMotion;
deltaMotion.setIdentity();
m_skeletonInstance[j]->getDeltaReferenceFrame( m_timestep, deltaMotion );
m_accumulatedMotion[j].setMulEq( deltaMotion );
}
// Advance the active animations
m_skeletonInstance[j]->stepDeltaTime( m_timestep );
// Sample the active animations and combine into a single pose
hkaPose pose (m_skeleton);
m_skeletonInstance[j]->sampleAndCombineAnimations( pose.accessUnsyncedPoseLocalSpace().begin(), pose.getFloatSlotValues().begin() );
hkReal separation = m_separation * hkReal( 2*j-1 );
hkQsTransform Q( hkQsTransform::IDENTITY );
Q.m_translation.set( separation, 0, 0 );
if ( m_useExtractedMotion )
{
Q.setMulEq( m_accumulatedMotion[j] );
}
pose.syncModelSpace();
AnimationUtils::drawPose( pose, Q );
// Test if the skin is to be drawn
if ( !m_drawSkin )
{
Q.m_translation( 2 ) -= 2.0f * m_separation;
}
// Construct the composite world transform
hkLocalArray<hkTransform> compositeWorldInverse( boneCount );
compositeWorldInverse.setSize( boneCount );
const hkArray<hkQsTransform>& poseModelSpace = pose.getSyncedPoseModelSpace();
// Skin the meshes
for (int i=0; i < m_numSkinBindings[j]; i++)
{
// assumes either a straight map (null map) or a single one (1 palette)
hkInt16* usedBones = m_skinBindings[j][i]->m_mappings? m_skinBindings[j][i]->m_mappings[0].m_mapping : HK_NULL;
int numUsedBones = usedBones? m_skinBindings[j][i]->m_mappings[0].m_numMapping : boneCount;
// Multiply through by the bind pose inverse world inverse matrices
for (int p=0; p < numUsedBones; p++)
{
int boneIndex = usedBones? usedBones[p] : p;
hkTransform tWorld; poseModelSpace[ boneIndex ].copyToTransform(tWorld);
compositeWorldInverse[p].setMul( tWorld, m_skinBindings[j][i]->m_boneFromSkinMeshTransforms[ boneIndex ] );
}
AnimationUtils::skinMesh( *m_skinBindings[j][i]->m_mesh, hkTransform( Q.m_rotation, hkVector4( Q.m_translation(0), Q.m_translation(1), Q.m_translation(2), 1 ) ), compositeWorldInverse.begin(), *m_env->m_sceneConverter );
}
// Move the attachments
{
HK_ALIGN(float f[16], 16);
for (int a=0; a < m_numAttachments[j]; a++)
{
if (m_attachmentObjects[j][a])
{
hkaBoneAttachment* ba = m_attachments[j][a];
hkQsTransform modelFromBone = pose.getBoneModelSpace( ba->m_boneIndex );
hkQsTransform worldFromBoneQs;
worldFromBoneQs.setMul( Q, modelFromBone );
worldFromBoneQs.get4x4ColumnMajor(f);
hkMatrix4 worldFromBone; worldFromBone.set4x4ColumnMajor(f);
hkMatrix4 worldFromAttachment; worldFromAttachment.setMul(worldFromBone, ba->m_boneFromAttachment);
m_env->m_sceneConverter->updateAttachment(m_attachmentObjects[j][a], worldFromAttachment);
}
}
}
}
return hkDemo::DEMO_OK;
}
void
MergeWidget::onBrowseGlobalTags() {
auto fileName = getOpenFileName(QY("Select tags file"), QY("XML files") + Q(" (*.xml)"), ui->globalTags);
if (!fileName.isEmpty())
m_config.m_globalTags = fileName;
}
static void
negative_s1_influence(double t, double s1, double *A0, double *A2)
{
*A0 = h_blend(-t, Q(s1));
*A2 = g_blend(t, Q(s1));
}
void
MergeWidget::onBrowseSegmentInfo() {
auto fileName = getOpenFileName(QY("Select segment info file"), QY("XML files") + Q(" (*.xml)"), ui->segmentInfo);
if (!fileName.isEmpty())
m_config.m_segmentInfo = fileName;
}
// first test
static int val = START_VAL;
struct Q {
int q;
Q() { q = val; val++; };
void qr() const;
};
struct C : Q {
int c;
C(int in) { c = in; };
C( Q const & q_in ) : Q(q_in) {};
void xyz() const;
};
void func( C const & cr = C(Q()) );
void func( C const & cr )
{
cr.xyz();
}
void C::xyz() const
{
qr();
}
void Q::qr() const
{
if( q != START_VAL ) fail(__LINE__);
}
void
MergeWidget::onSplitModeChanged(int newMode) {
auto splitMode = static_cast<MuxConfig::SplitMode>(newMode);
m_config.m_splitMode = splitMode;
if (MuxConfig::DoNotSplit == splitMode) {
Util::enableWidgets(m_splitControls, false);
return;
}
Util::enableWidgets(m_splitControls, true);
auto tooltip = QStringList{};
auto entries = QStringList{};
auto label = QString{};
if (MuxConfig::SplitAfterSize == splitMode) {
label = QY("Size:");
tooltip << QY("The size after which a new output file is started.")
<< QY("The letters 'G', 'M' and 'K' can be used to indicate giga/mega/kilo bytes respectively.")
<< QY("All units are based on 1024 (G = 1024^3, M = 1024^2, K = 1024).");
entries << Q("")
<< Q("350M")
<< Q("650M")
<< Q("700M")
<< Q("703M")
<< Q("800M")
<< Q("1000M")
<< Q("4483M")
<< Q("8142M");
} else if (MuxConfig::SplitAfterDuration == splitMode) {
label = QY("Duration:");
tooltip << QY("The duration after which a new output file is started.")
<< QY("The time can be given either in the form HH:MM:SS.nnnnnnnnn or as the number of seconds followed by 's'.")
<< QY("You may omit the number of hours 'HH' and the number of nanoseconds 'nnnnnnnnn'.")
<< QY("If given then you may use up to nine digits after the decimal point.")
<< QY("Examples: 01:00:00 (after one hour) or 1800s (after 1800 seconds).");
entries << Q("")
<< Q("01:00:00")
<< Q("1800s");
} else if (MuxConfig::SplitAfterTimecodes == splitMode) {
label = QY("Timecodes:");
tooltip << QY("The timecodes after which a new output file is started.")
<< QY("The timecodes refer to the whole stream and not to each individual output file.")
<< QY("The timecodes can be given either in the form HH:MM:SS.nnnnnnnnn or as the number of seconds followed by 's'.")
<< QY("You may omit the number of hours 'HH'.")
<< QY("You can specify up to nine digits for the number of nanoseconds 'nnnnnnnnn' or none at all.")
<< QY("If given then you may use up to nine digits after the decimal point.")
<< QY("If two or more timecodes are used then you have to separate them with commas.")
<< QY("The formats can be mixed, too.")
<< QY("Examples: 01:00:00,01:30:00 (after one hour and after one hour and thirty minutes) or 1800s,3000s,00:10:00 (after three, five and ten minutes).");
} else if (MuxConfig::SplitByParts == splitMode) {
label = QY("Parts:");
tooltip << QY("A comma-separated list of timecode ranges of content to keep.")
<< QY("Each range consists of a start and end timecode with a '-' in the middle, e.g. '00:01:15-00:03:20'.")
<< QY("If a start timecode is left out then the previous range's end timecode is used, or the start of the file if there was no previous range.")
<< QY("The timecodes can be given either in the form HH:MM:SS.nnnnnnnnn or as the number of seconds followed by 's'.")
<< QY("If a range's start timecode is prefixed with '+' then its content will be written to the same file as the previous range. Otherwise a new file will be created for this range.");
} else if (MuxConfig::SplitByPartsFrames == splitMode) {
label = QY("Parts:");
tooltip << QY("A comma-separated list of frame/field number ranges of content to keep.")
<< QY("Each range consists of a start and end frame/field number with a '-' in the middle, e.g. '157-238'.")
<< QY("The numbering starts at 1.")
<< QY("This mode considers only the first video track that is output.")
<< QY("If no video track is output no splitting will occur.")
<< QY("The numbers given with this argument are interpreted based on the number of Matroska blocks that are output.")
<< QY("A single Matroska block contains either a full frame (for progressive content) or a single field (for interlaced content).")
<< QY("mkvmerge does not distinguish between those two and simply counts the number of blocks.")
<< QY("If a start number is left out then the previous range's end number is used, or the start of the file if there was no previous range.")
<< QY("If a range's start number is prefixed with '+' then its content will be written to the same file as the previous range. Otherwise a new file will be created for this range.");
} else if (MuxConfig::SplitByFrames == splitMode) {
label = QY("Frames/fields:");
tooltip << QY("A comma-separated list of frame/field numbers after which to split.")
<< QY("The numbering starts at 1.")
<< QY("This mode considers only the first video track that is output.")
<< QY("If no video track is output no splitting will occur.")
<< QY("The numbers given with this argument are interpreted based on the number of Matroska blocks that are output.")
<< QY("A single Matroska block contains either a full frame (for progressive content) or a single field (for interlaced content).")
<< QY("mkvmerge does not distinguish between those two and simply counts the number of blocks.");
} else if (MuxConfig::SplitAfterChapters == splitMode) {
label = QY("Chapter numbers:");
tooltip << QY("Either the word 'all' which selects all chapters or a comma-separated list of chapter numbers before which to split.")
<< QY("The numbering starts at 1.")
<< QY("Splitting will occur right before the first key frame whose timecode is equal to or bigger than the start timecode for the chapters whose numbers are listed.")
<< QY("A chapter starting at 0s is never considered for splitting and discarded silently.")
<< QY("This mode only considers the top-most level of chapters across all edition entries.");
}
auto options = ui->splitOptions->currentText();
ui->splitOptionsLabel->setText(label);
ui->splitOptions->clear();
ui->splitOptions->addItems(entries);
ui->splitOptions->setCurrentText(options);
ui->splitOptions->setToolTip(tooltip.join(Q(" ")));
}
I rk(I f,A r,A a,A w)
{
A z=0,*p=0;
if(w)ND2 else ND1;
{
XA;
C *pp=0,*ap,*wp;
I wt=0,wr=0,wn=0,*wd=0;
I n=0,t=0,i,j,k,d[9],rw,ra,ri,ir,iw=0,ia=0,ii=0,
e=!w&&f==MP(9),h=QP(f)&&f!=MP(71)&&!e;
Q(!QA(r),9);
Q(r->t,6);
Q(r->n<1||r->n>3,8);
ar-=ra=raw(ar,*r->p);
if(!w) mv(d,ad,ra),ir=tr(ra,ad),ad+=ra;
else
{
wt=w->t,wr=w->r,wd=w->d;
wr-=rw=raw(wr,r->p[r->n>1]),ri=r->n>2?r->p[2]:9;
Q(ri<0,9);
if(ri>ra)ri=ra;
if(ri>rw)ri=rw;
mv(d,ad,ra-=ri);
ia=tr(ra,ad),mv(d+ra,wd,rw),iw=tr(rw-=ri,wd);
Q(cm(ad+=ra,wd+=rw,ri),11);
ii=tr(ri,ad),ra+=rw+ri,ir=ia*iw*ii,wn=tr(wr,wd+=ri),ad+=ri;
if(h&&ir>iw&&(f==MP(21)||f==MP(25)||f==MP(26)||f==MP(32)||f==MP(33)))
h=0;
}
an=tr(ar,ad);
if(h)
{
g=0;
aw_c[0]=a->c;
aw_c[1]=w&&w->c;
r=(A)fa(f,gC(at,ar,an,ad,a->n?a->p:0),w?gC(wt,wr,wn,wd,w->n?w->p:0):0);
aw_c[0]=aw_c[1]=1;
if(!r)R 0;
r=un(&r);
mv(d+ra,r->d,j=r->r);
if((j+=ra)>MAXR)R q=13,(I)r;
n=r->n;t=r->t;
if(ir<2) R mv(r->d,d,r->r=j),r->n*=ir,(I)r;
dc(r);
if(g==(I (*)())rsh) R rsh(w?w:a,j,d);
if(!g){h=0;}
else
{
if(at=atOnExit,w) wt=wtOnExit;
if(at!=a->t&&!(a=at?ep_cf(1):ci(1))) R 0;
if(w&&wt!=w->t&&!(w=wt?ep_cf(2):ci(2))) R 0;
OF(i,ir,n);
W(ga(t,j,i,d));
pp=(C*)z->p;
}
}
if(!h)
{
W(ga(Et,ra,ir,d));
*--Y=zr(z),p=(A*)z->p;
}
if(!w)
{
for(ap=(C*)a->p;ir--;ap+=Tt(at,an))
if(h) (*(I(*)(C*,C*,I))g)(pp,ap,an),pp+=Tt(t,n);
else a=gc(at,ar,an,ad,(I*)ap),*p++=e?a:(A)fa(f,(I)a,0);
}
else
{
for(i=0;i<ia;++i)for(j=0;j<iw;++j)
for(k=0;k<ii;++k){
ap=(C*)a->p+Tt(at,(i*ii+k)*an);
wp=(C*)w->p+Tt(wt,(j*ii+k)*wn);
if(h)
{
(*(I(*)(C*,C*,C*,I))g)(pp,ap,wp,n),pp+=Tt(t,n);
if(q==1)*--Y=(I)z,aplus_err(q,(A)Y[1]),++Y;
}
else
{
*p++=(A)fa(f,(I)gc(at,ar,an,ad,(I*)ap),(I)gc(wt,wr,wn,wd,(I*)wp));
}
}
}
if(h)R(I)z;
if(!e)z=(A)dis(r=z),dc(r);
R ++Y,(I)z;
}
}
static void beg_const(void)
{
QI = Q(type_I);
QT = Q(type_T);
QF = Q(type_F);
QY = Q(type_Y);
Qvoid = Q(type_void);
Qcons = Q(type_cons);
Qnull = Q(type_null);
Qeval = Q(type_eval);
Qlater = Q(type_later);
Qput = Q(type_put);
Qnl = Q(type_nl);
Qfput = Q(type_fput);
Qfnl = Q(type_fnl);
Qtuple = Q(type_tuple);
Qparse_file = Q(type_parse_file);
Qevaluate = Q(type_evaluate);
init_signal();
}
int main() {
int N = random();
#define P(n,args) p(#n #args, __builtin_##n args)
#define Q(n,args) q(#n #args, __builtin_##n args)
#define R(n,args) r(#n #args, __builtin_##n args)
#define V(n,args) p(#n #args, (__builtin_##n args, 0))
P(types_compatible_p, (int, float));
P(choose_expr, (0, 10, 20));
P(constant_p, (sizeof(10)));
P(expect, (N == 12, 0));
V(prefetch, (&N));
V(prefetch, (&N, 1));
V(prefetch, (&N, 1, 0));
// Numeric Constants
Q(huge_val, ());
Q(huge_valf, ());
Q(huge_vall, ());
Q(inf, ());
Q(inff, ());
Q(infl, ());
P(fpclassify, (0, 1, 2, 3, 4, 1.0));
P(fpclassify, (0, 1, 2, 3, 4, 1.0f));
P(fpclassify, (0, 1, 2, 3, 4, 1.0l));
Q(nan, (""));
Q(nanf, (""));
Q(nanl, (""));
Q(nans, (""));
Q(nan, ("10"));
Q(nanf, ("10"));
Q(nanl, ("10"));
Q(nans, ("10"));
P(isgreater, (1., 2.));
P(isgreaterequal, (1., 2.));
P(isless, (1., 2.));
P(islessequal, (1., 2.));
P(islessgreater, (1., 2.));
P(isunordered, (1., 2.));
P(isinf, (1.));
P(isinf_sign, (1.));
P(isnan, (1.));
// Bitwise & Numeric Functions
P(abs, (N));
P(clz, (N));
P(clzl, (N));
P(clzll, (N));
P(ctz, (N));
P(ctzl, (N));
P(ctzll, (N));
P(ffs, (N));
P(ffsl, (N));
P(ffsll, (N));
P(parity, (N));
P(parityl, (N));
P(parityll, (N));
P(popcount, (N));
P(popcountl, (N));
P(popcountll, (N));
Q(powi, (1.2f, N));
Q(powif, (1.2f, N));
Q(powil, (1.2f, N));
// Lib functions
int a, b, n = random(); // Avoid optimizing out.
char s0[10], s1[] = "Hello";
V(strcat, (s0, s1));
V(strcmp, (s0, s1));
V(strncat, (s0, s1, n));
V(strchr, (s0, s1[0]));
V(strrchr, (s0, s1[0]));
V(strcpy, (s0, s1));
V(strncpy, (s0, s1, n));
// Object size checking
V(__memset_chk, (s0, 0, sizeof s0, n));
V(__memcpy_chk, (s0, s1, sizeof s0, n));
V(__memmove_chk, (s0, s1, sizeof s0, n));
V(__mempcpy_chk, (s0, s1, sizeof s0, n));
V(__strncpy_chk, (s0, s1, sizeof s0, n));
V(__strcpy_chk, (s0, s1, n));
s0[0] = 0;
V(__strcat_chk, (s0, s1, n));
P(object_size, (s0, 0));
P(object_size, (s0, 1));
P(object_size, (s0, 2));
P(object_size, (s0, 3));
// Whatever
P(bswap16, (N));
P(bswap32, (N));
P(bswap64, (N));
// CHECK: @llvm.bitreverse.i8
// CHECK: @llvm.bitreverse.i16
// CHECK: @llvm.bitreverse.i32
// CHECK: @llvm.bitreverse.i64
P(bitreverse8, (N));
P(bitreverse16, (N));
P(bitreverse32, (N));
P(bitreverse64, (N));
// FIXME
// V(clear_cache, (&N, &N+1));
V(trap, ());
R(extract_return_addr, (&N));
P(signbit, (1.0));
return 0;
}
/* Resolve standard names. */
static value standard(value name)
{
cur_name = str_data(name);
/* Resolve numeric literals. */
{
value def = Qnum_str0(cur_name);
if (def) return def;
}
/* Resolve other names. */
if (match("put")) return hold(Qput);
if (match("nl")) return hold(Qnl);
if (match("say")) return Q(type_say);
if (match("fput")) return hold(Qfput);
if (match("fnl")) return hold(Qfnl);
if (match("fsay")) return Q(type_fsay);
if (match("fflush")) return Q(type_fflush);
if (match("+")) return Q(type_add);
if (match("-")) return Q(type_sub);
if (match("*")) return Q(type_mul);
if (match("/")) return Q(type_div);
if (match("^")) return Q(type_pow);
if (match("xor")) return Q(type_xor);
if (match("round")) return Q(type_round);
if (match("trunc")) return Q(type_trunc);
if (match("abs")) return Q(type_abs);
if (match("sqrt")) return Q(type_sqrt);
if (match("exp")) return Q(type_exp);
if (match("log")) return Q(type_log);
if (match("sin")) return Q(type_sin);
if (match("cos")) return Q(type_cos);
if (match("pi")) return Qnum(num_pi());
if (match("lt")) return Q(type_lt);
if (match("le")) return Q(type_le);
if (match("eq")) return Q(type_eq);
if (match("ne")) return Q(type_ne);
if (match("ge")) return Q(type_ge);
if (match("gt")) return Q(type_gt);
if (match("I")) return hold(QI);
if (match("T")) return hold(QT);
if (match("F")) return hold(QF);
if (match("@")) return hold(QY);
if (match("void")) return hold(Qvoid);
if (match("cons")) return hold(Qcons);
if (match("null")) return hold(Qnull);
if (match("eval")) return hold(Qeval);
if (match("once")) return Q(type_once);
if (match("later")) return hold(Qlater);
if (match("is_defined")) return Q(type_is_defined);
if (match("is_void")) return Q(type_is_void);
if (match("is_good")) return Q(type_is_good);
if (match("is_bool")) return Q(type_is_bool);
if (match("is_list")) return Q(type_is_list);
if (match(".")) return Q(type_concat);
if (match("length")) return Q(type_length);
if (match("slice")) return Q(type_slice);
if (match("search")) return Q(type_search);
if (match("str_num")) return Q(type_str_num);
if (match("ord")) return Q(type_ord);
if (match("chr")) return Q(type_chr);
if (match("char_width")) return Q(type_char_width);
if (match("dirname")) return Q(type_dirname);
if (match("basename")) return Q(type_basename);
if (match("length_common")) return Q(type_length_common);
if (match("is_str")) return Q(type_is_str);
if (match("num_str")) return Q(type_num_str);
if (match("is_num")) return Q(type_is_num);
if (match("is_tuple")) return Q(type_is_tuple);
if (match("arity")) return Q(type_arity);
if (match("split_tuple")) return Q(type_split_tuple);
if (match("join_tuple")) return Q(type_join_tuple);
if (match("stdin")) return Qfile(stdin);
if (match("stdout")) return Qfile(stdout);
if (match("stderr")) return Qfile(stderr);
if (match("fopen")) return Q(type_fopen);
if (match("fclose")) return Q(type_fclose);
if (match("fgetc")) return Q(type_fgetc);
if (match("fget")) return Q(type_fget);
if (match("flook")) return Q(type_flook);
if (match("remove")) return Q(type_remove);
if (match("is_newer")) return Q(type_is_newer);
if (match("is_file")) return Q(type_is_file);
if (match("is_dir")) return Q(type_is_dir);
if (match("flock_ex")) return Q(type_flock_ex);
if (match("flock_sh")) return Q(type_flock_sh);
if (match("flock_un")) return Q(type_flock_un);
if (match("readlink")) return Q(type_readlink);
if (match("mkdir")) return Q(type_mkdir);
if (match("rmdir")) return Q(type_rmdir);
if (match("ftruncate")) return Q(type_ftruncate);
if (match("fseek_set")) return Q(type_fseek_set);
if (match("fseek_cur")) return Q(type_fseek_cur);
if (match("fseek_end")) return Q(type_fseek_end);
if (match("ftell")) return Q(type_ftell);
if (match("fread")) return Q(type_fread);
if (match("mkfile")) return Q(type_mkfile);
if (match("dir_names")) return Q(type_dir_names);
if (match("mod_time")) return Q(type_mod_time);
if (match("file_size")) return Q(type_file_size);
if (match("symlink")) return Q(type_symlink);
if (match("rename")) return Q(type_rename);
if (match("time")) return Q(type_time);
if (match("localtime")) return Q(type_localtime);
if (match("die")) return Q(type_die);
if (match("argv")) return Q(type_argv);
if (match("sleep")) return Q(type_sleep);
if (match("usleep")) return Q(type_usleep);
if (match("run_process")) return Q(type_run_process);
if (match("spawn")) return Q(type_spawn);
if (match("exec")) return Q(type_exec);
if (match("fexl_benchmark")) return Q(type_fexl_benchmark);
if (match("seed_rand")) return Q(type_seed_rand);
if (match("rand")) return Q(type_rand);
if (match("parse")) return Q(type_parse);
if (match("parse_file")) return hold(Qparse_file);
if (match("evaluate")) return hold(Qevaluate);
if (match("resolve")) return Q(type_resolve);
if (match("buf_new")) return Q(type_buf_new);
if (match("buf_put")) return Q(type_buf_put);
if (match("buf_get")) return Q(type_buf_get);
if (match("readstr")) return Q(type_readstr);
if (match("sgetc")) return Q(type_sgetc);
if (match("sget")) return Q(type_sget);
if (match("slook")) return Q(type_slook);
if (match("var_new")) return Q(type_var_new);
if (match("var_get")) return Q(type_var_get);
if (match("var_put")) return Q(type_var_put);
if (match("is_var")) return Q(type_is_var);
if (match("limit_time")) return Q(type_limit_time);
if (match("limit_memory")) return Q(type_limit_memory);
if (match("limit_stack")) return Q(type_limit_stack);
if (match("unpack")) return Q(type_unpack);
if (match("pack")) return Q(type_pack);
if (match("random_bytes")) return Q(type_random_bytes);
if (match("random_nonce")) return Q(type_random_nonce);
if (match("random_secret_key")) return Q(type_random_secret_key);
if (match("crypto_box_public")) return Q(type_crypto_box_public);
if (match("crypto_box_prepare")) return Q(type_crypto_box_prepare);
if (match("crypto_box_seal")) return Q(type_crypto_box_seal);
if (match("crypto_box_open")) return Q(type_crypto_box_open);
if (match("crypto_sign_public")) return Q(type_crypto_sign_public);
if (match("crypto_sign_seal")) return Q(type_crypto_sign_seal);
if (match("crypto_sign_open")) return Q(type_crypto_sign_open);
if (match("crypto_hash")) return Q(type_crypto_hash);
if (match("set_alarm")) return Q(type_set_alarm);
if (match("start_server")) return Q(type_start_server);
if (match("kill")) return Q(type_kill);
if (match("connect")) return Q(type_connect);
if (match("receive_keystrokes")) return Q(type_receive_keystrokes);
return 0;
}
Rational Rational::TRANS_Z_Q(const BigInt &a) {
Natural N("1");
Rational Q(a, N);
return Q;
};
* You should have received a copy of the GNU General Public License
* along with liquid. If not, see <http://www.gnu.org/licenses/>.
*/
//
// qtype_div.port.c
//
// Portable qtype_t fixed-point division implementation
//
#include "liquidfpm.internal.h"
#define Q(name) LIQUIDFPM_CONCAT(q32,name)
//
Q(_t) Q(_div)(Q(_t) _x, Q(_t) _y)
{
#if 0
// define data types of double precision
// (e.g. int64_t for 32-bit fixed-point)
Q(_at) a = _x;
Q(_at) b = _y;
// pre-shift numerator by qtype_fracbits
a <<= Q(_fracbits);
return (Q(_t))(a / b);
#else
// compute division by pre-shifting numerator using
// high-order accumulator, then dividing by denominator