CMatrix::MatrixError_t CMatrix::init (int iNumRows, int iNumCols)
{
int i;
assert (iNumRows >= 0);
assert (iNumCols >= 0);
if (iNumRows == m_aiMatrixDimensions[kRow] && iNumCols == m_aiMatrixDimensions[kCol])
{
setZero();
return kMatrixNoError;
}
else
reset();
m_ppfMatrix = new float* [iNumRows];
if (!m_ppfMatrix)
return kMatrixMemAllocError;
for (i = 0; i < iNumRows; i++)
{
m_ppfMatrix[i] = new float [iNumCols];
if (!m_ppfMatrix[i])
return kMatrixMemAllocError;
}
m_aiMatrixDimensions[kRow] = iNumRows;
m_aiMatrixDimensions[kCol] = iNumCols;
setZero();
return kMatrixNoError;
}
bool GainClient::disablePositionForceControlGains(double transition_duration, bool wait_for_success)
{
sl_controller_msgs::CartesianGains position_gains;
setZero(position_gains);
sl_controller_msgs::CartesianGains force_gains;
setZero(force_gains);
return setArmPositionForceControlGains(position_gains, force_gains, transition_duration, wait_for_success);
}
void initializeA(float A[][M][maxDegree+1], float Acopy[][M][maxDegree+1]) {
int i, n, m;
float temp;
for(n = 0; n < N; ++n) {
for(m = 0; m < M; ++m) {
setZero(A[n][m]);
setZero(Acopy[n][m]);
}
}
int choice;
printf("If you would like to manually enter a matrix of polynomials, press 0.\nIf you would like have one generated randomly, press 1: ");
scanf("%d", &choice);
while (choice != 0 && choice != 1) {
printf("\nPlease enter 0 (manual) or 1 (random): ");
scanf("%d", &choice);
}
if (choice == 1) {
float negative;
for(n = 0; n < N; ++n) {
for(m = 0; m < M; ++m) {
for(i = 0; i <= maxEntry; ++i) {
// generate a number that is either -1 or +1
negative = (float) (rand()%2);
negative = negative - 0.5;
negative *= 2;
// generate a random coefficient
temp = (float) (rand()%10);
temp *= negative;
A[n][m][i] = temp;
Acopy[n][m][i] = temp;
}
}
}
}
else {
printf("Okay, please initialize the matrix \"A\".\n");
for(n = 0; n < N; ++n) {
for(m = 0; m < M; ++m) {
for(i = 0; i <= maxEntry; ++i) {
printf("Enter the coefficient of x^%d in A[%d][%d]: ", i, n, m);
scanf("%f", &temp);
A[n][m][i] = temp;
Acopy[n][m][i] = temp;
}
}
}
}
}
void Decimal::setReal(double value)
{
setZero();
int dec;
int sign;
char digitText[DOUBLE_SIG_DIGITS+2];
if (!safe_ecvt(sizeof(digitText), digitText, value, DOUBLE_SIG_DIGITS, &dec, &sign))
return;
int len = DOUBLE_SIG_DIGITS;
int hi = zeroDigit - 1 + dec;
int lo = hi - (len -1);
const char * finger = digitText;
//Number too big - should it create a maximum value? or truncate as it currently does.
if (hi >= maxDigits) // Most of this work is dealing with out of range cases
{
if (lo >= maxDigits)
return;
finger += (hi - (maxDigits-1));
hi = maxDigits-1;
}
if (lo < 0)
{
if (hi < 0)
return;
lo = 0;
}
msb = hi;
lsb = lo;
for ( int i = hi; i >= lo; i-- )
{
byte next = *finger++ - '0';
if (next < 10)
digits[i] = next;
else
{
//infinity????
setZero();
return;
}
}
if (sign)
negative = true;
}
Decimal & Decimal::round(int places)
{
//out of range - either 0 or overflow
if (places < -maxPrecision)
{
setZero();
return *this;
}
if (zeroDigit - places <= lsb)
return *this;
lsb = zeroDigit - places;
if (lsb > msb)
{
digits[lsb] = 0;
if ((lsb == msb+1) && digits[msb] >= 5)
digits[lsb]++;
msb = lsb;
return *this;
}
if (digits[lsb-1] < 5)
return *this;
return incLSD();
}
aol::Vector<_DataType>::Vector ( aol::Vector<_DataType> const&Vec, CopyFlag copyFlag )
: Obj ( Vec ), _size ( Vec._size ), _sizeReserved ( Vec._sizeReserved ),
overflowHandling ( Vec.overflowHandling ), overflowMin ( Vec.overflowMin ), overflowMax ( Vec.overflowMax ) {
switch ( copyFlag ) {
case DEEP_COPY:
_pData = static_cast<DataType*> ( aol::MemoryManager::allocateAtLeast ( _sizeReserved, sizeof ( DataType ) ) );
_deleteFlag = true;
memcpy ( _pData, Vec._pData, _size*sizeof ( DataType ) );
break;
case FLAT_COPY:
_pData = Vec._pData;
_deleteFlag = false;
// _size = Vec._size; // this happens in the initialization list.
// _sizeReserved = Vec._sizeReserved;
break;
case STRUCT_COPY:
_pData = static_cast<DataType*> ( aol::MemoryManager::allocateAtLeast ( _sizeReserved, sizeof ( DataType ) ) );
_deleteFlag = true;
setZero(); // same as in standard constructor: by convention, vectors are initialized with zero.
break;
case STRUCT_COPY_UNINIT:
_pData = static_cast<DataType*> ( aol::MemoryManager::allocateAtLeast ( _sizeReserved, sizeof ( DataType ) ) );
_deleteFlag = true;
// do not set to zero
break;
default:
throw aol::Exception ( "Invalid CopyFlag specified", __FILE__, __LINE__ );
break;
};
}
bool runRalfFunction(std::string name, scalarFnType fun, CUmodule* hModule, CUdeviceptr d_data,
DataStruct *h_data,DataStruct* h_data_reference, unsigned int memSize)
{
const unsigned inputNr = 10;
const float scalarInputs[4][inputNr] = {{ 0.f, 3.f, 2.f, 8.f, 10.2f, -1.f, 0.f, 1000.23f, 0.02f, -0.02f },
{ 1.f, 2.f, 4.f, 6.f, -14.13f, -13.f, 0.f, 0.02f, 420.001f, -420.001f },
{ 2.f, 1.f, 6.f, 4.f, 999.f, -5.f, 0.f, 420.001f, 0.01f, 0.01f },
{ 3.f, 0.f, 8.f, 2.f, 0.f, -420.001f, 0.f, 0.01f, 1000.23f, 0.01f }};
std::cout << "====================== " << name << "===============================\n";
for (unsigned i=0; i<inputNr; ++i) {
for (unsigned j=0; j<inputNr; ++j)
for (unsigned k=0; k<4; ++k)
{
setZero(h_data,h_data_reference);
h_data->fa[0] = h_data_reference->fa[0] = scalarInputs[k][i];
h_data->fa[1] = h_data_reference->fa[1] = scalarInputs[k][j];
//run device function
loadAndRunTestFunction(hModule, name, d_data, h_data, memSize);
fun(h_data_reference);
if(!compareData(h_data, h_data_reference)) //compare Data
{
std::cout << "\n Error in Ralf: fa0=" << scalarInputs[k][i]
<< ", fa1=" << scalarInputs[k][j] << " (" << name << ")\n";
return false;
}
}
}
std::cout << " => Test passed!!!\n";
return true;
}
// set the identity matrix of dimension n
void Matrix::setIdentityMatrix( int n )
{
assert( n > 0 );
setDimensions( n, n );
setZero();
for ( int i = 0; i < n; i++ ) setElement( i, i, 1.0 );
}
aol::Vector<_DataType>::Vector ( const qc::GridStructure &Grid ) : _size ( Grid.getNumberOfNodes() ), _sizeReserved ( _size ), _deleteFlag ( true ), overflowHandling ( aol::CLIP ), overflowMin ( 0 ), overflowMax ( aol::OverflowTrait<DataType>::max ) {
_pData = static_cast<DataType*> ( aol::MemoryManager::allocateAtLeast ( _sizeReserved, sizeof ( DataType ) ) );
if ( !_pData )
throw OutOfMemoryException ( "Vector<DataType,Realtype>::Vector: Could not allocate memory for Vector.", __FILE__, __LINE__ );
setZero ();
}
//Add with carry
void adc(CPU *c, OP_CODE_INFO *o){
int8_t carry = getFlag(c,C);
int8_t accum = getRegByte(c,ACCUM);
int8_t operand = o->operand;
int16_t sum = (0x00FF&carry) + (0x00FF&accum) + (0x00FF&operand);
int8_t sumByte = sum & 0x00FF;
setZero(c,sumByte);
if(getFlag(c,D)){ //in decimal mode
//if lower 4 bits of operands plus
//the carry in are larger than 9,
//then we need to apply conversions
//to remain in binary coded decimal format.
if((accum & 0xF) + (operand & 0xF)
+ carry > 9){
sum += 6;
}
setSign(c,sum&0xFF);
setOverflow(c,accum,operand,sum&0xFF);
//if the higher bits aren't in
//BCD format we need to add 96 to convert.
//Black magic from http://nesdev.com/6502.txt
sum += sum > 0x99 ? 96 : 0;
setCarryBCD(c, sum);
} else {
setSign(c,sumByte);
setOverflow(c,accum,operand,sumByte);
setCarry(c,sum);
}
setRegByte(c,ACCUM,sum&0xFF);
}
static void ec_double(const uint32_t *px, const uint32_t *py, uint32_t *Dx, uint32_t *Dy){
uint32_t tempA[8];
uint32_t tempB[8];
uint32_t tempC[8];
uint32_t tempD[16];
if(isZero(px) && isZero(py)){
copy(px, Dx,arrayLength);
copy(py, Dy,arrayLength);
return;
}
fieldMult(px, px, tempD, arrayLength);
fieldModP(tempA, tempD);
setZero(tempB, 8);
tempB[0] = 0x00000001;
fieldSub(tempA, tempB, ecc_prime_m, tempC); //tempC = (qx^2-1)
tempB[0] = 0x00000003;
fieldMult(tempC, tempB, tempD, arrayLength);
fieldModP(tempA, tempD);//tempA = 3*(qx^2-1)
fieldAdd(py, py, ecc_prime_r, tempB); //tempB = 2*qy
fieldInv(tempB, ecc_prime_m, ecc_prime_r, tempC); //tempC = 1/(2*qy)
fieldMult(tempA, tempC, tempD, arrayLength); //tempB = lambda = (3*(qx^2-1))/(2*qy)
fieldModP(tempB, tempD);
fieldMult(tempB, tempB, tempD, arrayLength); //tempC = lambda^2
fieldModP(tempC, tempD);
fieldSub(tempC, px, ecc_prime_m, tempA); //lambda^2 - Px
fieldSub(tempA, px, ecc_prime_m, Dx); //lambda^2 - Px - Qx
fieldSub(px, Dx, ecc_prime_m, tempA); //tempA = qx-dx
fieldMult(tempB, tempA, tempD, arrayLength); //tempC = lambda * (qx-dx)
fieldModP(tempC, tempD);
fieldSub(tempC, py, ecc_prime_m, Dy); //Dy = lambda * (qx-dx) - px
}
void TMatrix3::setIdentity()
{
setZero();
v_[0][0].coeff(0) = 1;
v_[1][1].coeff(0) = 1;
v_[2][2].coeff(0) = 1;
}
// set the diagonal matrix
void Matrix::setDiagonalMatrix( const vector< double >& diag )
{
int n = diag.size();
setDimensions( n, n );
setZero();
for ( int i = 0; i < n; i++ ) setElement( i, i, diag[ i ] );
}
void and(CPU *c, OP_CODE_INFO *o){
int8_t accum = getRegByte(c,ACCUM);
int8_t operand = o->operand;
int8_t res = accum & operand;
setSign(c,res);
setZero(c,res);
setRegByte(c,ACCUM,res);
}
Point Manifold::getZero() const
{
mnf_assert(isValid() || seeMessageAbove());
lock();
Eigen::VectorXd id(representationDim_);
setZero(id);
return Point(*this, id);
}
void initializeA(float A[][M][maxDegree+1], float Acopy[][M][maxDegree+1]) {
int i, n, m;
for(n = 0; n < N; ++n) {
for(m = 0; m < M; ++m) {
setZero(A[n][m]);
setZero(Acopy[n][m]);
}
}
// A[0][0][0] = 1;
// A[0][0][1] = 0;
// A[0][0][2] = 1;
// A[0][1][0] = 0;
// A[0][1][1] = -4;
// A[0][1][2] = 3;
// A[1][0][0] = 0;
// A[1][0][1] = 8;
// A[1][0][2] = 0;
// A[1][1][0] = 0;
// A[1][1][1] = 0;
// A[1][1][2] = -1;
float temp;
float negative;
// printf("Initialize the matrix \"A\".\n");
for(n = 0; n < N; ++n) {
for(m = 0; m < M; ++m) {
for(i = 0; i <= maxEntry; ++i) {
// printf("Enter the coefficient of x^%d in A[%d][%d]: ", i, n, m);
// scanf("%f", &temp);
//after this, negative will be either +1 or -1
negative = (float) (rand()%2);
negative = negative - 0.5;
negative *= 2;
temp = (float) (rand()%10);
temp *= negative;
A[n][m][i] = temp;
Acopy[n][m][i] = temp;
}
}
}
}
void ossimMatrix4x4::setIdentity()
{
setZero();
theData[0][0] = 1.0;
theData[1][1] = 1.0;
theData[2][2] = 1.0;
theData[3][3] = 1.0;
}
//finite Field multiplication
//32bit * 32bit = 64bit
static int fieldMult(const uint32_t *x, const uint32_t *y, uint32_t *result, uint8_t length){
uint32_t temp[length * 2];
setZero(temp, length * 2);
setZero(result, length * 2);
uint8_t k, n;
uint64_t l;
for (k = 0; k < length; k++){
for (n = 0; n < length; n++){
l = (uint64_t)x[n]*(uint64_t)y[k];
temp[n+k] = l&0xFFFFFFFF;
temp[n+k+1] = l>>32;
add(&temp[n+k], &result[n+k], &result[n+k], (length * 2) - (n + k));
setZero(temp, length * 2);
}
}
return 0;
}
static int fieldAdd(const uint32_t *x, const uint32_t *y, const uint32_t *reducer, uint32_t *result){
if(add(x, y, result, arrayLength)){ //add prime if carry is still set!
uint32_t tempas[8];
setZero(tempas, 8);
add(result, reducer, tempas, arrayLength);
copy(tempas, result, arrayLength);
}
return 0;
}
static int fieldSub(const uint32_t *x, const uint32_t *y, const uint32_t *modulus, uint32_t *result){
if(sub(x, y, result, arrayLength)){ //add modulus if carry is set
uint32_t tempas[8];
setZero(tempas, 8);
add(result, modulus, tempas, arrayLength);
copy(tempas, result, arrayLength);
}
return 0;
}
Decimal & Decimal::multiply(const Decimal & other)
{
int low1, high1, low2, high2, lowt, hight;
clip(low1, high1);
other.clip(low2, high2);
lowt = low1+low2-zeroDigit;
if (lowt < 0) lowt = 0;
hight = high1 + high2 - zeroDigit;
if (hight >= maxDigits) hight = maxDigits-1;
else if (hight < 0)
{
if (hight < -1)
{
setZero();
return *this;
}
hight = 0;
}
unsigned temp[maxDigits*2];
_clear(temp);
// memset(temp+low1+low2, 0, (high1+high2-low1-low2+2)*sizeof(unsigned)); // only need to clear part of the target we're adding to.
//More: could copy across 1st time round - might be worth it.
const byte * digits1 = digits;
const byte * digits2 = other.digits;
for (int i = low1; i <= high1; i++)
{
byte next = digits1[i];
if (next)
{
for (int j=low2; j <= high2; j++)
temp[i+j] += next * digits2[j];
}
}
//Now copy the results, taking cary of the carries
unsigned carry = 0;
int j;
for (j = low1+low2 - zeroDigit; j < lowt; j++)
carry = (temp[j+zeroDigit]+carry)/10;
for (j = lowt; j <= hight; j++)
{
div_t next = div(temp[j+zeroDigit]+carry, 10);
digits[j] = next.rem;
carry = next.quot;
}
if ((hight < maxDigits-1) && (carry != 0))
digits[++hight] = carry % 10;
lsb = lowt;
msb = hight;
negative ^= other.negative;
return *this;
}
int main(int argc, char **argv)
{
Show( Array);
setZero( Array );
Show( Array);
return 0;
}
Mat4& Mat4::setIdentity()
{
setZero();
m_arrData[0][0] = 1;
m_arrData[1][1] = 1;
m_arrData[2][2] = 1;
m_arrData[3][3] = 1;
return *this;
}
static void ec_add(const uint32_t *px, const uint32_t *py, const uint32_t *qx, const uint32_t *qy, uint32_t *Sx, uint32_t *Sy){
uint32_t tempA[8];
uint32_t tempB[8];
uint32_t tempC[8];
uint32_t tempD[16];
if(isZero(px) && isZero(py)){
copy(qx, Sx,arrayLength);
copy(qy, Sy,arrayLength);
return;
} else if(isZero(qx) && isZero(qy)) {
copy(px, Sx,arrayLength);
copy(py, Sy,arrayLength);
return;
}
if(isSame(px, qx, arrayLength)){
if(!isSame(py, qy, arrayLength)){
setZero(Sx, 8);
setZero(Sy, 8);
return;
} else {
ec_double(px, py, Sx, Sy);
return;
}
}
fieldSub(py, qy, ecc_prime_m, tempA);
fieldSub(px, qx, ecc_prime_m, tempB);
fieldInv(tempB, ecc_prime_m, ecc_prime_r, tempB);
fieldMult(tempA, tempB, tempD, arrayLength);
fieldModP(tempC, tempD); //tempC = lambda
fieldMult(tempC, tempC, tempD, arrayLength); //tempA = lambda^2
fieldModP(tempA, tempD);
fieldSub(tempA, px, ecc_prime_m, tempB); //lambda^2 - Px
fieldSub(tempB, qx, ecc_prime_m, Sx); //lambda^2 - Px - Qx
fieldSub(qx, Sx, ecc_prime_m, tempB);
fieldMult(tempC, tempB, tempD, arrayLength);
fieldModP(tempC, tempD);
fieldSub(tempC, qy, ecc_prime_m, Sy);
}
//initializes P to identity
void initializeP(float P[][N][maxDegree+1]) {
int i, j;
for(i = 0; i < N; ++i) {
for(j = 0; j < N; ++j) {
setZero(P[i][j]);
if (i == j) {
P[i][j][0] = 1;
}
}
}
}
// initializes Q to identity
void initializeQ(float Q[][M][maxDegree+1]) {
int i, j;
for(i = 0; i < M; ++i) {
for(j = 0; j < M; ++j) {
setZero(Q[i][j]);
if (i == j) {
Q[i][j][0] = 1;
}
}
}
}
void aol::Vector<_DataType>::reallocate ( const int Length ) {
if ( ( _deleteFlag == false ) && ( _pData != NULL ) )
throw Exception ( "aol::Vector<DataType>::reallocate may not be called on FLAT_COPY vectors!\n", __FILE__, __LINE__ );
if ( Length > _sizeReserved ) {
reserve ( Length );
_sizeReserved = _size = Length;
} else {
_size = Length;
}
setZero();
}
// sets q equal to the polynomial s.t. a = bq + r
void eucDiv(float a[], float b[], float q[]) {
if (equalsZero(a) == 1) {
printf("You are calling eucDiv with a equal to the zero vector\n");
return;
}
if (equalsZero(b) == 1) {
printf("You are calling a division by 0\n");
return;
}
// printf("a in eucdiv call: ");
// printPoly(a);
// printf("b in eucdiv call: ");
// printPoly(b);
int i;
// printf("\narg1 of eucdiv: ");
// printPoly(a);
// printf("\narg2 of eucdiv: ");
// printPoly(b);
int degA = degree(a);
int degB = degree(b);
//printf("\ndeg arg1: %d, deg arg2: %d\n", degA, degB);
int d = degA - degB;
if (d < 0) {
printf("This is a nonsensical call of eucDiv\n");
return;
}
float tempQ;
setZero(q);
// creates tempA as a clone of a
float tempA[maxDegree+1];
setEquals(tempA, a);
float tempB[maxDegree+1];
setEquals(tempB, b);
// float tempPoly[maxDegree+1];
// algorithm for euclidean division
for (i = d; i >= 0; --i) {
tempQ = tempA[degA + i - d] / tempB[degB];
q[i] = tempQ;
if (tempQ != 0) {
scale(tempB, tempQ);
polyTimesXn(tempB, i);
subtract(tempA, tempB);
polyTimesXn(tempB, -i);
scale(tempB, 1.0/tempQ);
}
}
}
returnValue EvaluationPoint::init( const Function &f ,
uint nx_, uint na_, uint np_,
uint nu_, uint nw_, uint nd_,
uint N_ ) {
uint run1;
deleteAll();
nx = acadoMax( nx_, f.getNX () );
na = acadoMax( na_, f.getNXA() );
np = acadoMax( np_, f.getNP () );
nu = acadoMax( nu_, f.getNU () );
nw = acadoMax( nw_, f.getNW () );
nd = acadoMax( nd_, f.getNDX() );
N = acadoMax( N_ , f.getNumberOfVariables()+1 );
if( N != 0 ) z = new double[N];
else z = 0 ;
setZero( );
idx = new int*[7 ];
idx[0] = new int [1 ];
idx[1] = new int [nx];
idx[2] = new int [na];
idx[3] = new int [np];
idx[4] = new int [nu];
idx[5] = new int [nw];
idx[6] = new int [nd];
idx[0][0] = f.index( VT_TIME, 0 );
for( run1 = 0; run1 < nx; run1++ )
idx[1][run1] = f.index( VT_DIFFERENTIAL_STATE, run1 );
for( run1 = 0; run1 < na; run1++ )
idx[2][run1] = f.index( VT_ALGEBRAIC_STATE, run1 );
for( run1 = 0; run1 < np; run1++ )
idx[3][run1] = f.index( VT_PARAMETER, run1 );
for( run1 = 0; run1 < nu; run1++ )
idx[4][run1] = f.index( VT_CONTROL, run1 );
for( run1 = 0; run1 < nw; run1++ )
idx[5][run1] = f.index( VT_DISTURBANCE, run1 );
for( run1 = 0; run1 < nd; run1++ )
idx[6][run1] = f.index( VT_DDIFFERENTIAL_STATE, run1 );
return SUCCESSFUL_RETURN;
}
//Arithmetic shift left
void asl(CPU *c, OP_CODE_INFO *o){
int8_t operand = o->operand;
int16_t res = (0x00FF&operand) << 1;
int8_t resByte = res & 0x00FF;
setCarry(c,res);
setSign(c,resByte);
setZero(c,resByte);
if(o->mode == modeAccumulator){
setRegByte(c,ACCUM,res);
} else {
write(c,o->address,res);
}
}
