bool Mesh::compareArea(vector<Vertex> vec1, vector<Vertex> vec2){
int a1,a2;
vector<int> ia1 = longerSegment(vec1[0],vec1[1],vec1[2],0);
vector<int> ia2 = longerSegment(vec2[0],vec2[1],vec2[2],0);
int edgea1 = outTriplet(ia1,0,2);
int edgea2 = outTriplet(ia2,0,2);
a1 = vectorNorm(vec1[ia1[0]],vec1[edgea1])*vectorNorm(vec1[ia1[1]],vec1[edgea1])/2;
a2 = vectorNorm(vec2[ia2[0]],vec2[edgea2])*vectorNorm(vec2[ia2[1]],vec2[edgea2])/2;
if (a2>=a1) return true;
else return false;
}
void operator() ( const std::vector< double > &stateVector,
std::vector< double > &dXdt,
const double currentTime )
{
// calculate the gravitational accelerations first
std::vector< double > positionVector = { stateVector[ xPositionIndex ],
stateVector[ yPositionIndex ],
stateVector[ zPositionIndex ] };
double radialDistance = vectorNorm( positionVector );
double radialDistanceCube = radialDistance * radialDistance * radialDistance;
double Ux = -1.0 * gravParameter * positionVector[ xPositionIndex ] / radialDistanceCube;
double Uy = -1.0 * gravParameter * positionVector[ yPositionIndex ] / radialDistanceCube;
double Uz = -1.0 * gravParameter * positionVector[ zPositionIndex ] / radialDistanceCube;
// now calculate the derivatives
dXdt[ xPositionIndex ] = stateVector[ xVelocityIndex ];
dXdt[ yPositionIndex ] = stateVector[ yVelocityIndex ];
dXdt[ zPositionIndex ] = stateVector[ zVelocityIndex ];
dXdt[ xVelocityIndex ] = Ux;
dXdt[ yVelocityIndex ] = Uy;
dXdt[ zVelocityIndex ] = Uz;
}
int main ()
{
// set of complex vectors
vector<CVector> s({
CVector({
C(1, 0),
C(-1, 0),
C(1, 0)
}),
CVector({
C(1, 0),
C(0, 0),
C(1, 0)
}),
CVector({
C(1, 0),
C(1, 0),
C(2, 0)
})
});
vector<CVector> s_basis = orthoNormBasis(s);
printVectors(s_basis);
// check inner products (should be zero)
for (unsigned int i = 0; i < s_basis.size() - 1; i++)
{
std::cout << "INNER PRODUCT: " <<
vectorInnerProd(
s_basis[i],
s_basis[i + 1]
) << std::endl;
}
// check norms
for (auto x : s_basis)
std::cout << "NORM: " << vectorNorm(x) << std::endl;
}
Real Block::norm(rflag tp)const{
try{
checkUni10TypeError(tp);
return vectorNorm(m_elem, elemNum(), 1, ongpu);
}
catch(const std::exception& e){
propogate_exception(e, "In function Matrix::norm(uni10::rflag ):");
return 0;
}
}
Matrix& Matrix::normalize(cflag tp){
try{
checkUni10TypeError(tp);
Real norm = vectorNorm(cm_elem, elemNum(), 1, ongpu);
vectorScal((1./norm), cm_elem, elemNum(), ongpu);
}
catch(const std::exception& e){
propogate_exception(e, "In function Matrix::normalize(uni10::cflag ):");
}
return *this;
}
vector<int> Mesh::longerSegment(Vertex v0, Vertex v1, Vertex v2, int first){
vector<int> ind;
int max = vectorNorm(v0,v1);
if(vectorNorm(v1,v2)>max) {
ind.push_back(first + 1);
ind.push_back(first + 2);
ind.push_back(first); //no concerned by longest segment
}
else if(vectorNorm(v0,v2)>max) {
ind.push_back(first);
ind.push_back(first + 2);
ind.push_back(first + 1);
}
else {
ind.push_back(first);
ind.push_back(first + 1);
ind.push_back(first + 2);
}
return ind;
}
void Detumbling(MatrixMath_s* commDipole, MatrixMath_s bMeas, MatrixMath_s bDeriv, double ctrlgains_bdot) {
double temp_1e9 = pow(10, -9);
double temp = ctrlgains_bdot * temp_1e9;
double r = 0;
multiplyScalar(temp * (-1), &bDeriv);
multiplyScalar(temp_1e9, &bMeas);
r = pow(vectorNorm(&bMeas), 2);
multiplyScalar(1 / r, &bDeriv);
(*commDipole) = bDeriv;
}
Real Block::norm(cflag tp)const{
try{
checkUni10TypeError(tp);
if(typeID() == 0){
std::ostringstream err;
err<<"This matirx is empty" << std::endl << "In the file Block.cpp, line(" << __LINE__ << ")";
throw std::runtime_error(exception_msg(err.str()));
}
return vectorNorm(cm_elem, elemNum(), 1, ongpu);
}
catch(const std::exception& e){
propogate_exception(e, "In function Matrix::norm(uni10::cflag ):");
}
return 0;
}
bool scene::occluded(float *vectA, float *vectB)
{
bool result;
float *tempA;
float dist;
tempA = new float[3];
vectorSub(tempA, vectB, vectA);
dist = vectorMod(tempA);
vectorNorm(tempA, tempA);
if (beam(vectA, tempA, dist) != -1)
result = true;
else
result = false;
delete []tempA;
return result;
}
vector<double> GradientDescent::extremum(const vector<double> &start_point, const vector<vector<double>> &c,
const vector<double> &d, EType type)
{
vector<double> gr; // gradient
vector<double> res = start_point;
Matrix mat; // matrix of active constraints
bool found = false;
int iterations = 0;
while (!found)
{
gr = gradient(res);
mat = getActiveConstraints(c, d, res); // constraints, where sum = di
if (mat.size().first != 0) // there are some active constraints
{
//project gradient onto this area
mat = mat.transpose() * (mat * mat.transpose()).inverse() * mat;
mat = Matrix::E(mat.size().first) - mat;
mat = Matrix(gr) * mat;
}
else
mat = gr; // use gradient
for (uint i = 0; i < res.size(); i++) // go to the direction of projection of gradient(antigradient if miminization)
if (type == EType::Maximun)
res[i] += step*mat.at(0, i);
else
res[i] -= step*mat.at(0, i);
found = vectorNorm(gr) < stoppingE || ++iterations > max_iterations;
}
return res;
}
// search maximum or minimum of func
vector<double> GradientDescent::extremum(const vector<double> &start_point, EType type)
{
vector<double> gr; // gradient
vector<double> res = start_point;
bool found = false;
int iterations = 0;
while (!found)
{
gr = gradient(res);
for (uint i = 0; i < res.size(); i++) // go to the direction of gradient(antigradient if miminization)
if (type == EType::Maximun)
res[i] += step*gr[i];
else
res[i] -= step*gr[i];
found = vectorNorm(gr) < stoppingE || ++iterations > max_iterations;
}
return res;
}
void QRfactorization(long n, complex **A, complex **Q, complex **R)
{
/* QR factorization based on Householder transformations */
long i,j,k,m;
double nrm;
complex z,z1,z2;
complex *vj;
/* Init. Q = eye(n) (identity matrix) */
for (i=0; i<n; i++){
Q[i][i].re = 1.0;
Q[i][i].im = 0.0;
}
/* Init. R = A */
for(j=0; j<n; j++){
for (i=0; i<n; i++){
R[i][j].re = A[i][j].re;
R[i][j].im = A[i][j].im;
CheckValue(FUNCTION_NAME, "A[i][j].re","", A[i][j].re, -INFTY, INFTY);
CheckValue(FUNCTION_NAME, "A[i][j].im","", A[i][j].im, -INFTY, INFTY);
}
}
/* Allocate auxiliary variables*/
vj = (complex *)Mem(MEM_ALLOC, n, sizeof(complex));
/* printf("begin calc, n = %d\n", n); */
/* ------------------------------------------------------------*/
for (j=0; j<n; j++){ /* Main loop */
/* R(j:end, j) */
for (i=j; i<n; i++){
vj[i-j].re = R[i][j].re;
vj[i-j].im = R[i][j].im;
}
nrm = vectorNorm(n-j, vj);
/* v(1) = v(1) + sign(R(j,j)) * norm(v) */
vj[0].re = vj[0].re + R[j][j].re / c_norm(R[j][j]) * nrm;
vj[0].im = vj[0].im + R[j][j].im / c_norm(R[j][j]) * nrm;
/* Update norm */
nrm = vectorNorm(n-j, vj);
/* v = v./norm(v) */
for (i=0; i<n-j; i++){
vj[i].re = vj[i].re / nrm;
vj[i].im = vj[i].im / nrm;
}
/* Update */
/* R(j:end, :) = R(j:end,:) - 2 * vj * vj' * R(j:end,:), : */
/* Q(:,j:end) = Q(:,j:end) - 2 * Q(:,j:end) * vj * vj^T */
for (k=0; k<n; k++){
/* (v * v' * A)_ik = v_i SUM_m Conj(v_m) A_mk */
z.re = 0.0;
z.im = 0.0;
for (m=j; m<n; m++){
z1 = c_con(vj[m-j]);
z1 = c_mul(z1, R[m][k]);
z = c_add(z,z1);
}
for (i=j; i<n; i++){
z2 = c_mul(vj[i-j],z);
/* Update R(i,k) */
R[i][k].re = R[i][k].re - 2.0 * z2.re;
R[i][k].im = R[i][k].im - 2.0 * z2.im;
CheckValue(FUNCTION_NAME, "R[i][k].re","", R[i][k].re, -INFTY, INFTY);
CheckValue(FUNCTION_NAME, "R[i][k].im","", R[i][k].im, -INFTY, INFTY);
}
/* (A * v * v^')_ki = v_i * SUM_m Conj(v_m) A_km */
z.re = 0.0;
z.im = 0.0;
for (m=j; m<n; m++){
z1 = vj[m-j];
z1 = c_mul(z1, Q[k][m]);
z = c_add(z,z1);
}
for (i=j; i<n; i++){
z1 = c_con(vj[i-j]);
z2 = c_mul(z1,z);
/* Update Q(k,i)*/
Q[k][i].re = Q[k][i].re - 2.0 * z2.re;
Q[k][i].im = Q[k][i].im - 2.0 * z2.im;
CheckValue(FUNCTION_NAME, "Q[k][i].re","", Q[k][i].re, -INFTY, INFTY);
CheckValue(FUNCTION_NAME, "Q[k][i].im","", Q[k][i].im, -INFTY, INFTY);
}
}
} /* End of main loop (j) */
/* -------------------------------------------------------------*/
/* Free auxiliary variables */
Mem(MEM_FREE, vj);
return;
}
/**
* Solves a linear least squares problem to obtain a N degree polynomial that fits
* the specified input data as nearly as possible.
*
* Returns true if a solution is found, false otherwise.
*
* The input consists of two vectors of data points X and Y with indices 0..m-1
* along with a weight vector W of the same size.
*
* The output is a vector B with indices 0..n that describes a polynomial
* that fits the data, such the sum of W[i] * W[i] * abs(Y[i] - (B[0] + B[1] X[i]
* + B[2] X[i]^2 ... B[n] X[i]^n)) for all i between 0 and m-1 is minimized.
*
* Accordingly, the weight vector W should be initialized by the caller with the
* reciprocal square root of the variance of the error in each input data point.
* In other words, an ideal choice for W would be W[i] = 1 / var(Y[i]) = 1 / stddev(Y[i]).
* The weights express the relative importance of each data point. If the weights are
* all 1, then the data points are considered to be of equal importance when fitting
* the polynomial. It is a good idea to choose weights that diminish the importance
* of data points that may have higher than usual error margins.
*
* Errors among data points are assumed to be independent. W is represented here
* as a vector although in the literature it is typically taken to be a diagonal matrix.
*
* That is to say, the function that generated the input data can be approximated
* by y(x) ~= B[0] + B[1] x + B[2] x^2 + ... + B[n] x^n.
*
* The coefficient of determination (R^2) is also returned to describe the goodness
* of fit of the model for the given data. It is a value between 0 and 1, where 1
* indicates perfect correspondence.
*
* This function first expands the X vector to a m by n matrix A such that
* A[i][0] = 1, A[i][1] = X[i], A[i][2] = X[i]^2, ..., A[i][n] = X[i]^n, then
* multiplies it by w[i]./
*
* Then it calculates the QR decomposition of A yielding an m by m orthonormal matrix Q
* and an m by n upper triangular matrix R. Because R is upper triangular (lower
* part is all zeroes), we can simplify the decomposition into an m by n matrix
* Q1 and a n by n matrix R1 such that A = Q1 R1.
*
* Finally we solve the system of linear equations given by R1 B = (Qtranspose W Y)
* to find B.
*
* For efficiency, we lay out A and Q column-wise in memory because we frequently
* operate on the column vectors. Conversely, we lay out R row-wise.
*
* http://en.wikipedia.org/wiki/Numerical_methods_for_linear_least_squares
* http://en.wikipedia.org/wiki/Gram-Schmidt
*/
static bool solveLeastSquares(const float* x, const float* y,
const float* w, uint32_t m, uint32_t n, float* outB, float* outDet) {
#if DEBUG_STRATEGY
ALOGD("solveLeastSquares: m=%d, n=%d, x=%s, y=%s, w=%s", int(m), int(n),
vectorToString(x, m).c_str(), vectorToString(y, m).c_str(),
vectorToString(w, m).c_str());
#endif
// Expand the X vector to a matrix A, pre-multiplied by the weights.
float a[n][m]; // column-major order
for (uint32_t h = 0; h < m; h++) {
a[0][h] = w[h];
for (uint32_t i = 1; i < n; i++) {
a[i][h] = a[i - 1][h] * x[h];
}
}
#if DEBUG_STRATEGY
ALOGD(" - a=%s", matrixToString(&a[0][0], m, n, false /*rowMajor*/).c_str());
#endif
// Apply the Gram-Schmidt process to A to obtain its QR decomposition.
float q[n][m]; // orthonormal basis, column-major order
float r[n][n]; // upper triangular matrix, row-major order
for (uint32_t j = 0; j < n; j++) {
for (uint32_t h = 0; h < m; h++) {
q[j][h] = a[j][h];
}
for (uint32_t i = 0; i < j; i++) {
float dot = vectorDot(&q[j][0], &q[i][0], m);
for (uint32_t h = 0; h < m; h++) {
q[j][h] -= dot * q[i][h];
}
}
float norm = vectorNorm(&q[j][0], m);
if (norm < 0.000001f) {
// vectors are linearly dependent or zero so no solution
#if DEBUG_STRATEGY
ALOGD(" - no solution, norm=%f", norm);
#endif
return false;
}
float invNorm = 1.0f / norm;
for (uint32_t h = 0; h < m; h++) {
q[j][h] *= invNorm;
}
for (uint32_t i = 0; i < n; i++) {
r[j][i] = i < j ? 0 : vectorDot(&q[j][0], &a[i][0], m);
}
}
#if DEBUG_STRATEGY
ALOGD(" - q=%s", matrixToString(&q[0][0], m, n, false /*rowMajor*/).c_str());
ALOGD(" - r=%s", matrixToString(&r[0][0], n, n, true /*rowMajor*/).c_str());
// calculate QR, if we factored A correctly then QR should equal A
float qr[n][m];
for (uint32_t h = 0; h < m; h++) {
for (uint32_t i = 0; i < n; i++) {
qr[i][h] = 0;
for (uint32_t j = 0; j < n; j++) {
qr[i][h] += q[j][h] * r[j][i];
}
}
}
ALOGD(" - qr=%s", matrixToString(&qr[0][0], m, n, false /*rowMajor*/).c_str());
#endif
// Solve R B = Qt W Y to find B. This is easy because R is upper triangular.
// We just work from bottom-right to top-left calculating B's coefficients.
float wy[m];
for (uint32_t h = 0; h < m; h++) {
wy[h] = y[h] * w[h];
}
for (uint32_t i = n; i != 0; ) {
i--;
outB[i] = vectorDot(&q[i][0], wy, m);
for (uint32_t j = n - 1; j > i; j--) {
outB[i] -= r[i][j] * outB[j];
}
outB[i] /= r[i][i];
}
#if DEBUG_STRATEGY
ALOGD(" - b=%s", vectorToString(outB, n).c_str());
#endif
// Calculate the coefficient of determination as 1 - (SSerr / SStot) where
// SSerr is the residual sum of squares (variance of the error),
// and SStot is the total sum of squares (variance of the data) where each
// has been weighted.
float ymean = 0;
for (uint32_t h = 0; h < m; h++) {
ymean += y[h];
}
ymean /= m;
float sserr = 0;
float sstot = 0;
for (uint32_t h = 0; h < m; h++) {
float err = y[h] - outB[0];
float term = 1;
for (uint32_t i = 1; i < n; i++) {
term *= x[h];
err -= term * outB[i];
}
sserr += w[h] * w[h] * err * err;
float var = y[h] - ymean;
sstot += w[h] * w[h] * var * var;
}
*outDet = sstot > 0.000001f ? 1.0f - (sserr / sstot) : 1;
#if DEBUG_STRATEGY
ALOGD(" - sserr=%f", sserr);
ALOGD(" - sstot=%f", sstot);
ALOGD(" - det=%f", *outDet);
#endif
return true;
}
/*!
* This routine computes the trajectory for a single regolith ejected from the surface of an
* asteroid by first computing the appropriate initial conditions (IC) and then integrating the
* equations of motion using those ICs.
*
*/
void calculateRegolithTrajectory( const double alpha,
const double beta,
const double gamma,
const double gravitationalParameter,
const double density,
const std::vector< double > W,
const double Wmagnitude,
const double aXValue,
const double aYValue,
const double aZValue,
const double coneAngleAzimuth,
const double coneAngleDeclination,
const double velocityMagnitudeFactor,
const double integrationStepSize,
const double startTime,
const double endTime,
const double dataSaveIntervals,
std::ostringstream &filePath )
{
// get the initial position coordinates for the particle on the surface
std::vector< double > aVector { aXValue, aYValue, aZValue };
std::vector< double > positionUnitVector = naos::normalize( aVector );
double xTempTerm
= ( positionUnitVector[ xPositionIndex ] * positionUnitVector[ xPositionIndex ] )
/ ( alpha * alpha );
double yTempTerm
= ( positionUnitVector[ yPositionIndex ] * positionUnitVector[ yPositionIndex ] )
/ ( beta * beta );
double zTempTerm
= ( positionUnitVector[ zPositionIndex ] * positionUnitVector[ zPositionIndex ] )
/ ( gamma * gamma );
double positionMagnitude = std::sqrt( 1.0 / ( xTempTerm + yTempTerm + zTempTerm ) );
std::vector< double > regolithPositionVector( 3 );
regolithPositionVector[ xPositionIndex ]
= positionMagnitude * positionUnitVector[ xPositionIndex ];
regolithPositionVector[ yPositionIndex ]
= positionMagnitude * positionUnitVector[ yPositionIndex ];
regolithPositionVector[ zPositionIndex ]
= positionMagnitude * positionUnitVector[ zPositionIndex ];
// get the normal vector for the point on the surface from where the particle is launched
// this normal vector is not in the same direction as the position vector unless the central
// body is a sphere.
std::vector< double > regolithNormalVector
= { regolithPositionVector[ xPositionIndex ] / ( alpha * alpha ),
regolithPositionVector[ yPositionIndex ] / ( beta * beta ),
regolithPositionVector[ zPositionIndex ] / ( gamma * gamma ) };
// get the unit normal vector
std::vector< double > regolithNormalUnitVector = naos::normalize( regolithNormalVector );
// get the velocity vector
positionMagnitude = vectorNorm( regolithPositionVector );
// velocity magnitude calculated using simple point mass potential formula
const double velocityMagnitude = velocityMagnitudeFactor
* std::sqrt( 2.0 * gravitationalParameter / positionMagnitude );
std::vector< double > regolithVelocityVector( 3 );
// computeRegolithVelocityVector( regolithPositionVector,
// velocityMagnitude,
// coneAngleAzimuth,
// coneAngleDeclination,
// regolithNormalUnitVector,
// regolithVelocityVector );
computeRegolithVelocityVector2( regolithPositionVector,
velocityMagnitude,
coneAngleAzimuth,
coneAngleDeclination,
regolithNormalUnitVector,
regolithVelocityVector );
naos::Vector6 initialVector { regolithPositionVector[ xPositionIndex ],
regolithPositionVector[ yPositionIndex ],
regolithPositionVector[ zPositionIndex ],
regolithVelocityVector[ 0 ],
regolithVelocityVector[ 1 ],
regolithVelocityVector[ 2 ] };
// calculate the trajectory of the regolith
singleRegolithTrajectoryCalculator( alpha,
beta,
gamma,
gravitationalParameter,
W,
initialVector,
integrationStepSize,
startTime,
endTime,
filePath,
dataSaveIntervals );
}
