bool Sphere::doesIntersect(const Ray& ray)
{
Ray translatedRay = ray;
translatedRay.origin -= origin;
// Simplifies to the quadratic formula
// a = 1
float b = 2 * dot(translatedRay.origin, translatedRay.direction);
float c = translatedRay.origin.length2() - squared(radius);
float discriminant = squared(b) - 4.0f * c;
if (discriminant < 0.0f)
return false;
discriminant = std::sqrt(discriminant);
float t1 = (-b + discriminant) / 2;
float t2 = (-b - discriminant) / 2;
if (t2 < t1)
{
float tmp = t2;
t2 = t1;
t1 = tmp;
}
if ((t1 < ray.maxDist && t1 > kRayMinDist) || (t2 < ray.maxDist && t2 > kRayMinDist))
{
return true;
}
else
{
return false;
}
}
void PtexTriangleFilter::buildKernel(PtexTriangleKernel& k, float u, float v,
float uw1, float vw1, float uw2, float vw2,
float width, float blur, Res faceRes)
{
const float sqrt3 = 1.7320508075688772f;
// compute ellipse coefficients, A*u^2 + B*u*v + C*v^2 == AC - B^2/4
float scaleAC = 0.25f * width*width;
float scaleB = -2.0f * scaleAC;
float A = (vw1*vw1 + vw2*vw2) * scaleAC;
float B = (uw1*vw1 + uw2*vw2) * scaleB;
float C = (uw1*uw1 + uw2*uw2) * scaleAC;
// convert to cartesian domain
float Ac = 0.75f * A;
float Bc = float(sqrt3/2) * (B-A);
float Cc = 0.25f * A - 0.5f * B + C;
// compute min blur for eccentricity clamping
const float maxEcc = 15.0f; // max eccentricity
const float eccRatio = (maxEcc*maxEcc + 1.0f) / (maxEcc*maxEcc - 1.0f);
float X = sqrtf(squared(Ac - Cc) + squared(Bc));
float b_e = 0.5f * (eccRatio * X - (Ac + Cc));
// compute min blur for texel clamping
// (ensure that ellipse is no smaller than a texel)
float b_t = squared(0.5f / (float)faceRes.u());
// add blur
float b_b = 0.25f * blur * blur;
float b = PtexUtils::max(b_b, PtexUtils::max(b_e, b_t));
Ac += b;
Cc += b;
// compute minor radius
float m = sqrtf(2.0f*(Ac*Cc - 0.25f*Bc*Bc) / (Ac + Cc + X));
// choose desired resolution
int reslog2 = PtexUtils::max(0, PtexUtils::calcResFromWidth(2.0f*m));
// convert back to triangular domain
A = float(4/3.0) * Ac;
B = float(2/sqrt3) * Bc + A;
C = -0.25f * A + 0.5f * B + Cc;
// scale by kernel width
float scale = PtexTriangleKernelWidth * PtexTriangleKernelWidth;
A *= scale;
B *= scale;
C *= scale;
// find u,v,w extents
float uw = PtexUtils::min(sqrtf(C), 1.0f);
float vw = PtexUtils::min(sqrtf(A), 1.0f);
float ww = PtexUtils::min(sqrtf(A-B+C), 1.0f);
// init kernel
float w = 1.0f - u - v;
k.set(Res((int8_t)reslog2, (int8_t)reslog2), u, v, u-uw, v-vw, w-ww, u+uw, v+vw, w+ww, A, B, C);
}
// given the extent of the altitude and the difference beween the collinear
// base intercept and the altitude's base intercept, we can use
// Pythagorean's theorem to compute the length of the collinear line.
byte Location::collinearHeight(byte i) {
// use i altitude height and base; i collinear base
return( d->Cb[i] >= d->Ab[i] ? // unsigned, so careful of order
squareRoot( squared(d->Ah[i]) + squared(d->Cb[i]-d->Ab[i]) ) :
squareRoot( squared(d->Ah[i]) + squared(d->Ab[i]-d->Cb[i]) )
);
}
// given two distances and a known side length, we can calculate the
// the altitude's distance along the side length of the altitude.
// from https://en.wikipedia.org/wiki/Heron%27s_formula#Algebraic_proof_using_the_Pythagorean_theorem
byte Location::altitudeBase(byte i) {
if( d->D[left(i)] < d->Ah[i] ) return(0);
// use left-i distance and ith height
return(
squareRoot( squared(d->D[left(i)]) - squared(d->Ah[i]) )
);
}
static double evalGaussianPdf(const Eigen::VectorXf &x, const Eigen::VectorXf &mean, const Eigen::VectorXf &std)
{
double normConst=1.0;
int k=x.rows();
float quadraticForm=0;
for (int d=0; d<k; d++)
{
quadraticForm+=squared(x[d]-mean[d])/squared(std[d]);
}
return diagGaussianNormalizingConstant(std)*expf(-0.5f*quadraticForm);
}
void CustomPhysicsEngine::ApplyCollisionResolutionTest(CollisionManifold* _manifold, Rigidbody* _rigidA, Rigidbody* _rigidB) {
//Get Mass
float massA = _rigidA->mass;
float massB = _rigidB->mass;
//Get Moment Of Inertia
float moiA = _rigidA->momentOfInertia;
float moiB = _rigidB->momentOfInertia;
//Get Inverse Mass
float invMassA = 1.0f / _rigidA->mass;
float invMassB = 1.0f / _rigidB->mass;
//Get Inverse Moment Of Inertia
float invMOIA = 1.0f / _rigidA->momentOfInertia;
float invMOIB = 1.0f / _rigidB->momentOfInertia;
vec3 relativePointA = _manifold->point - _rigidA->transform->position;
vec3 relativePointB = _manifold->point - _rigidB->transform->position;
vec3 rXn_A = glm::cross(relativePointA, _manifold->normal) / moiA;
vec3 rXn_B = glm::cross(relativePointB, _manifold->normal) / moiB;
vec3 velocityA = _rigidA->velocity + _rigidA->angularVelocity * relativePointA;
vec3 velocityB = _rigidB->velocity + _rigidB->angularVelocity * relativePointB;
vec3 relativeVelocity = velocityA - velocityB;
float rXnDn_A = glm::dot(rXn_A, _manifold->normal);
float rXnDn_B = glm::dot(rXn_B, _manifold->normal);
float numerator = -(1.0f + _manifold->restitution) * glm::dot(relativeVelocity, _manifold->normal);
float denominator = glm::dot(_manifold->normal, _manifold->normal * (invMassA + invMassB)) +
squared(rXnDn_A) / moiA +
squared(rXnDn_B) / moiB;
float j = numerator / denominator;
vec3 impulse = _manifold->normal * j;
if (!_rigidA->gameObject->isStatic) {
_rigidA->velocity += (impulse * invMassA);
_rigidA->angularVelocity += glm::cross(rXn_A, impulse) / moiA;
}
if (!_rigidB->gameObject->isStatic) {
_rigidB->velocity += (-impulse * invMassB);
_rigidB->angularVelocity += glm::cross(rXn_B, -impulse) / moiB;
}
}
// Arguments in Hertz.
double magnitudeForFrequency( double inFreq) {
// Cast to Double.
double _b0 = double(b0);
double _b1 = double(b1);
double _b2 = double(b2);
double _a1 = double(a1);
double _a2 = double(a2);
// Frequency on unit circle in z-plane.
double zReal = cos(M_PI * inFreq);
double zImaginary = sin(M_PI * inFreq);
// Zeros response.
double numeratorReal = (_b0 * (squared(zReal) - squared(zImaginary))) + (_b1 * zReal) + _b2;
double numeratorImaginary = (2.0 * _b0 * zReal * zImaginary) + (_b1 * zImaginary);
double numeratorMagnitude = sqrt(squared(numeratorReal) + squared(numeratorImaginary));
// Poles response.
double denominatorReal = squared(zReal) - squared(zImaginary) + (_a1 * zReal) + _a2;
double denominatorImaginary = (2.0 * zReal * zImaginary) + (_a1 * zImaginary);
double denominatorMagnitude = sqrt(squared(denominatorReal) + squared(denominatorImaginary));
// Total response.
double response = numeratorMagnitude / denominatorMagnitude;
return response;
}
/**************************************************************************************************************************************
PURPOSE: Calculate the slope of a line of best fit
HISTORY:
05/29/2012 (DLM) initial coding
**************************************************************************************************************************************/
double lobfM(double xs[], double ys[], unsigned int n) {
double sumX, sumY, sumXY, sumX2, temp;
unsigned int i;
sumX = sumY = sumXY = sumX2 = 0.0; // init values to 0
for (i = 0; i < n; i++) {
sumX += xs[i]; // sumX is the sum of all the x values
sumY += ys[i]; // sumY is the sum of all the y values
sumXY += (xs[i] * ys[i]); // sumXY is the sum of the product of all the x & y values (ie: x1 * y1 + x2 * y2 + ...... + xn * yn)
sumX2 += squared(xs[i]); // sumX2 is the sum of the square of all the x values (ie: x1^2 + x2^2 + ....... + xn^2)
}
temp = n + 0.0;
return ((sumX * sumY - temp * sumXY) / (squared(sumX) - n * sumX2));
}
/// Repelling force between two particles given their signed distance.
float kraft(float d) {
if (d < 0) return -kraft(-d);
if (d <= 2) {
return 0.25f * d;
}
return squared(1.0f / d);
}
static double diagGaussianNormalizingConstant(const Eigen::VectorXf &std)
{
//the norm. constant for a k-dimensional gaussian is given by 1/sqrt[(2pi)^k det(cov)]
//For diag. covariance, det(cov)=trace(cov)=squared(std.prod())
double det=squared(std.prod());
return 1/sqrt(pow(2*PI,std.rows())*det);
}
curve_G2 CompressedG2::to_libsnark_g2() const
{
auto x_coordinate = x.to_libsnark_fq2<curve_Fq2>();
// y = +/- sqrt(x^3 + b)
auto y_coordinate = ((x_coordinate.squared() * x_coordinate) + alt_bn128_twist_coeff_b).sqrt();
auto y_coordinate_neg = -y_coordinate;
if ((fq2_to_bigint(y_coordinate) > fq2_to_bigint(y_coordinate_neg)) != y_gt) {
y_coordinate = y_coordinate_neg;
}
curve_G2 r = curve_G2::one();
r.X = x_coordinate;
r.Y = y_coordinate;
r.Z = curve_Fq2::one();
assert(r.is_well_formed());
if (alt_bn128_modulus_r * r != curve_G2::zero()) {
throw std::runtime_error("point is not in G2");
}
return r;
}
int ConjGradientMod(float3N &X, float3Nx3N &A, float3N &B,int3 hack)
{
// obsolete!!!
// Solves for unknown X in equation AX=B
conjgrad_loopcount=0;
int n=B.count;
float3N q(n),d(n),tmp(n),r(n);
r = B - Mul(tmp,A,X); // just use B if X known to be zero
r[hack[0]] = r[hack[1]] = r[hack[2]] = float3(0,0,0);
d = r;
float s = dot(r,r);
float starget = s * squared(conjgrad_epsilon);
while( s>starget && conjgrad_loopcount++ < conjgrad_looplimit)
{
Mul(q,A,d); // q = A*d;
q[hack[0]] = q[hack[1]] = q[hack[2]] = float3(0,0,0);
float a = s/dot(d,q);
X = X + d*a;
if(conjgrad_loopcount%50==0)
{
r = B - Mul(tmp,A,X);
r[hack[0]] = r[hack[1]] = r[hack[2]] = float3(0,0,0);
}
else
{
r = r - q*a;
}
float s_prev = s;
s = dot(r,r);
d = r+d*(s/s_prev);
d[hack[0]] = d[hack[1]] = d[hack[2]] = float3(0,0,0);
}
conjgrad_lasterror = s;
return conjgrad_loopcount<conjgrad_looplimit; // true means we reached desired accuracy in given time - ie stable
}
int ConjGradientFiltered(float3N &X, const float3Nx3N &A, const float3N &B,const float3Nx3N &S)
{
// Solves for unknown X in equation AX=B
conjgrad_loopcount=0;
int n=B.count;
float3N q(n),d(n),tmp(n),r(n);
r = B - Mul(tmp,A,X); // just use B if X known to be zero
filter(r,S);
d = r;
float s = dot(r,r);
float starget = s * squared(conjgrad_epsilon);
while( s>starget && conjgrad_loopcount++ < conjgrad_looplimit)
{
Mul(q,A,d); // q = A*d;
filter(q,S);
float a = s/dot(d,q);
X = X + d*a;
if(conjgrad_loopcount%50==0)
{
r = B - Mul(tmp,A,X);
filter(r,S);
}
else
{
r = r - q*a;
}
float s_prev = s;
s = dot(r,r);
d = r+d*(s/s_prev);
filter(d,S);
}
conjgrad_lasterror = s;
return conjgrad_loopcount<conjgrad_looplimit; // true means we reached desired accuracy in given time - ie stable
}
bool Sphere::intersect(Intersection& intersection)
{
Ray translatedRay = intersection.ray;
translatedRay.origin -= origin;
// Simplifies to the quadratic formula
// a = 1
float b = 2 * dot(translatedRay.origin, translatedRay.direction);
float c = translatedRay.origin.length2() - squared(radius);
float discriminant = squared(b) - 4.0f * c;
if (discriminant < 0.0f)
return false;
discriminant = std::sqrt(discriminant);
float t1 = (-b + discriminant) / 2;
float t2 = (-b - discriminant) / 2;
if (t2 < t1)
{
float tmp = t2;
t2 = t1;
t1 = tmp;
}
if (t1 < intersection.dist && t1 > kRayMinDist)
{
intersection.dist = t1;
}
else if (t2 < intersection.dist && t2 > kRayMinDist)
{
intersection.dist = t2;
}
else
{
return false;
}
intersection.normal = translatedRay.calc(intersection.dist).normalized();
intersection.pShape = this;
intersection.pMaterial = pMaterial;
return true;
}
float Sphere::pdfSA(const Point& refPosition,
const Vector& refNormal,
const Point& surfPosition,
const Vector& surfNormal) const
{
Vector toCenter = origin - refPosition;
float dist2 = toCenter.length2();
if (dist2 > squared(radius) * 1.00001f)
{
// Point is on or in the sphere
Vector toSurf = refPosition - surfPosition;
return toSurf.length2() * surfaceAreaPDF() / std::fabs(dot(toSurf.normalized(), surfNormal));
}
float sinThetaMax2 = squared(radius) / dist2;
float cosThetaMax = std::sqrt(std::max(0.0f, 1.0f - sinThetaMax2));
return uniformConePdf(cosThetaMax);
}
float Shape::pdfSA(
const Point& refPosition,
const Vector& refNormal,
const Point& surfPosition,
const Vector& surfNormal) const
{
Vector incoming = surfPosition - refPosition;
float dist = incoming.normalize();
return squared(dist) * surfaceAreaPDF() / std::fabs(dot(surfNormal, incoming));
}
char OctreeNodeIntersectSphere( TOctreeNode * node, TSphereShape * sphere ) {
float r2 = sphere->radius * sphere->radius;
float dmin = 0;
if( sphere->position.x < node->min.x ) {
dmin += squared( sphere->position.x - node->min.x );
} else if( sphere->position.x > node->max.x ) {
dmin += squared( sphere->position.x - node->max.x );
}
if( sphere->position.y < node->min.y ) {
dmin += squared( sphere->position.y - node->min.y );
} else if( sphere->position.y > node->max.y ) {
dmin += squared( sphere->position.y - node->max.y );
}
if( sphere->position.z < node->min.z ) {
dmin += squared( sphere->position.z - node->min.z );
} else if( sphere->position.z > node->max.z ) {
dmin += squared( sphere->position.z - node->max.z );
}
char sphereInside = (sphere->position.x >= node->min.x) && (sphere->position.x <= node->max.x) &&
(sphere->position.y >= node->min.y) && (sphere->position.y <= node->max.y) &&
(sphere->position.z >= node->min.z) && (sphere->position.z <= node->max.z);
return dmin <= r2 || sphereInside;
}
bool Sphere::sampleSurface(const Point& refPosition,
const Vector& refNormal,
float u1, float u2, float u3,
Point& outPosition,
Vector& outNormal,
float& outPdf)
{
Vector toCenter = origin - refPosition;
float dist2 = toCenter.length2();
if (dist2 < squared(radius) * 1.00001f)
{
// Point is on or in the sphere
outNormal = uniformToSphere(u1, u2);
outPosition = origin + outNormal * radius;
Vector toSurf = refPosition - outPosition;
outPdf = toSurf.length2() * surfaceAreaPDF() / std::fabs(dot(toSurf.normalized(), outNormal));
return true;
}
// Outside the sphere, fit a cone around to sample more efficiently
float sinThetaMax2 = squared(radius) / dist2;
float cosThetaMax = std::sqrt(std::max(0.0f, 1 - sinThetaMax2));
Vector x, y, z;
makeCoordinateSpace(toCenter, x, y, z);
Vector localCone = uniformToCone(u1, u2, cosThetaMax);
Vector cone = transformFromLocalSpace(localCone, x, y, z);
// Make sure direction hits sphere
Ray ray(refPosition, cone);
Intersection isect(ray);
if (!intersect(isect))
isect.dist = dot(toCenter, cone);
outPosition = ray.calc(isect.dist);
outNormal = (outPosition - origin).normalized();
outPdf = uniformConePdf(cosThetaMax);
return true;
}
inline bool KDTreeNode::box_in_search_range(SearchRecord& sr) {
// does the bounding box, represented by minbox[*],maxbox[*]
// have any point which is within 'sr.ballsize' to 'sr.qv'??
int dim = sr.dim;
float dis2 =0.0;
float ballsize = sr.ballsize;
for (int i=0; i<dim; i++) {
dis2 += squared(dis_from_bnd(sr.qv[i], box[i].lower, box[i].upper));
if (dis2 > ballsize)
return(false);
}
return(true);
}
void dtHelper (double *src, double *dst, int *ptr, int step, int s1, int s2,
int d1, int d2, double a, double b)
{
if (d2 >= d1)
{
int d = (d1+d2) >> 1;
int s = s1;
for (int p = s1+1; p <= s2; p++)
{
if (src[s*step] - a*squared(d-s) - b*(d-s) <
src[p*step] - a*squared(d-p) - b*(d-p))
{
s = p;
}
}
dst[d*step] = src[s*step] - a*squared(d-s) - b*(d-s);
ptr[d*step] = s;
dtHelper (src, dst, ptr, step, s1, s, d1, d-1, a, b);
dtHelper (src, dst, ptr, step, s, s2, d+1, d2, a, b);
}
void KDTree::n_nearest_brute_force(std::vector<float>& qv, int nn, KDTreeResultVector& result) {
result.clear();
for (int i=0; i<N; i++) {
float dis = 0.0;
KDTreeResult e;
for (int j=0; j<dim; j++) {
dis += squared( the_data[i][j] - qv[j]);
}
e.dis = dis;
e.idx = i;
result.push_back(e);
}
sort(result.begin(), result.end() );
}
void KDTreeNode::search(SearchRecord& sr) {
// the core search routine.
// This uses true distance to bounding box as the
// criterion to search the secondary node.
//
// This results in somewhat fewer searches of the secondary nodes
// than 'search', which uses the vdiff vector, but as this
// takes more computational time, the overall performance may not
// be improved in actual run time.
if ((left == NULL) && (right == NULL)) {
// we are on a terminal node
if (sr.nn == 0) {
process_terminal_node_fixedball(sr);
}
else {
process_terminal_node(sr);
}
}
else {
KDTreeNode *ncloser, *nfarther;
float extra;
float qval = sr.qv[cut_dim];
// value of the wall boundary on the cut dimension.
if (qval < cut_val) {
ncloser = left;
nfarther = right;
extra = cut_val_right - qval;
}
else {
ncloser = right;
nfarther = left;
extra = qval - cut_val_left;
};
if (ncloser != NULL) ncloser->search(sr);
if ((nfarther != NULL) && (squared(extra) < sr.ballsize)) {
// first cut
if (nfarther->box_in_search_range(sr)) {
nfarther->search(sr);
}
}
}
}
QPixmap TTKCircleProgressWidget::generatePixmap() const
{
QPixmap pixmap(squared(rect()).size().toSize());
pixmap.fill(QColor(0,0,0,0));
QPainter painter(&pixmap);
painter.setRenderHint(QPainter::Antialiasing, true);
QRectF rect = pixmap.rect().adjusted(1,1,-1,-1);
qreal margin = rect.width()*(1.0 - m_outerRadius)/2.0;
rect.adjust(margin,margin,-margin,-margin);
qreal innerRadius = m_innerRadius*rect.width()/2.0;
painter.setBrush(QColor(225, 225, 225));
painter.setPen(QColor(225, 225, 225));
painter.drawPie(rect, 0, 360*16);
painter.setBrush(m_color);
painter.setPen(m_color);
if(m_maximum == 0)
{
int startAngle = -m_infiniteAnimationValue * 360 * 16;
int spanAngle = 0.15 * 360 * 16;
painter.drawPie(rect, startAngle, spanAngle);
}
else
{
int value = qMin(m_visibleValue, m_maximum);
int startAngle = 90 * 16;
int spanAngle = -qreal(value) * 360 * 16 / m_maximum;
painter.drawPie(rect, startAngle, spanAngle);
}
painter.setBrush(QColor(255,255,255));
painter.setPen(QColor(0,0,0, 60));
painter.drawEllipse(rect.center(), innerRadius, innerRadius);
painter.drawArc(rect, 0, 360*16);
return pixmap;
}
void RateMatrix::SetMatrix(
vector<double>& matrix,
double len
)
{
int i,j,k;
double expt[numStates];
vector<double>::iterator P;
// P(t)ij = SUM Cijk * exp{Root*t}
P=matrix.begin();
if (len<1e-6) {
for (i=0; i<numStates; i++) {
for (j=0; j<numStates; j++) {
if (i==j)
(*P)=1.0;
else
(*P)=0.0;
P++;
}
}
return;
}
for (k=1; k<numStates; k++) {
expt[k]=exp(len*Root[k]);
}
vector<unsigned int> squared (numStates, 0), power_1 (numStates, 0);
for (i=0; i<numStates; i++) { squared[i]=i*numStates_squared; power_1[i]=i*numStates; }
for (i=0; i<numStates; i++) {
for (j=0; j<numStates; j++) {
(*P)=Cijk.at(squared[i]+power_1[j]+0);
for (k=1; k<numStates; k++) {
(*P)+=Cijk.at(squared[i]+power_1[j]+k)*expt[k];
}
P++;
}
}
}
void ellipse_convert (double sigx,double sigy,double rho,double conrad,double *eigen1,double *eigen2,double *ang)
/* convert from one parameterization of an ellipse to another */
/* Kurt Feigl, from code by T. Herring */
{
/* INPUT */
/* sigx, sigy - Sigmas in the x and y directions. */
/* rho - Correlation coefficient between x and y */
/* OUTPUT (returned) */
/* eigen1 - the smaller eigenvalue */
/* eigen2 - the larger eigenvalue */
/* ang - Orientation of ellipse relative to X axis in radians */
/* - should be counter-clockwise from X axis */
/* LOCAL VARIABLES */
/* a,b,c,d,e - Constants used in getting eigenvalues */
/* conrad - Radius for the confidence interval */
double a,b,c,d,e;
/* confidence scaling */
/* confid - Confidence interval wanted (0-1) */
/* conrad = sqrt( -2.0 * log(1.0 - confid)); */
/* the formulas for this part may be found in Bomford, p. 719 */
a = squared(sigy*sigy - sigx*sigx);
b = 4. * squared(rho*sigx*sigy);
c = squared(sigx) + squared(sigy);
/* minimum eigenvector (semi-minor axis) */
*eigen1 = conrad * sqrt((c - sqrt(a + b))/2.);
/* maximu eigenvector (semi-major axis) */
*eigen2 = conrad * sqrt((c + sqrt(a + b))/2.);
d = 2. * rho * sigx * sigy;
e = squared(sigx) - squared(sigy);
*ang = atan2(d,e)/2.;
/* that is all */
}
int main( int argc, char *argv[] ) {
int **odd, n, temp, i, j ;
printf( "Please enter an odd integer from 3 - 15 for the size of a square array. " ) ;
scanf( "%d", &temp ) ;
if( ( temp < 3 ) || ( temp > 15 ) || ( ( temp % 2 ) == 0 ) ) {
printf( "You must enter a number that fulfills the requirements." ) ;
return 0 ;
}
printf( "Please enter a number for the intial value." ) ;
scanf( "%d", &n ) ;
if( ( temp + n ) * ( temp + n ) > 1000 ) {
printf( "The values you entered are too big." ) ;
return 0 ;
}
odd = ( int ** ) malloc( sizeof( int ) * temp ) ;
for( i = 0 ; i < temp ; i++ ) {
odd[ i ] = ( int * ) malloc( sizeof( int ) * temp ) ;
}
squared( odd, n, temp ) ;
for( i = 0 ; i < temp ; i++ ) {
for( j = 0 ; j < temp ; j++ ) {
printf( " %d", odd[ i ][ j ] ) ;
}
printf( "\n" ) ;
}
return 0 ;
}
double hessnorm(double x, double mu, double sig, int ms1, int ms2, int logsc) {
double hessian=0.0;
/* hess of mu and mu */
if(ms1==1 && ms2==1) hessian=dnorm(x,mu,sig,0)*(squared((x-mu)/(squared(sig))) - 1/squared(sig));
/* hess of mu and sig */
if((ms1==1 && ms2==2)||(ms1==2 && ms2==1)) hessian=dnorm(x,mu,sig,0)*
((((squared(x-mu)/(squared(sig)*sig))-(1/sig)))
*((x-mu)/(squared(sig)))
+((2*(mu-x))/(squared(sig)*sig)) );
/* hess of sig and sig */
if(ms1==2 && ms2==2) hessian=dnorm(x,mu,sig,0)*((squared(((squared(x-mu))/(squared(sig)*sig))-(1/sig)))
+((1/squared(sig))-( (3*squared(x-mu))/(squared(squared(sig))) )) );
return(hessian);
}
double rms(std::vector<double>&& v)
{
return std::sqrt(mean(squared(std::move(v))));
}
double rms(const std::vector<double>& v)
{
return std::sqrt(mean(squared(v)));
}
void getInclination()
{
int w = 0;
float tmpf = 0.0;
int currentTime, signRzGyro;
currentTime = millis();
interval = currentTime - lastTime;
lastTime = currentTime;
//if (firstSample || Float.isNaN(RwEst[0])) { // NaN用来等待检查从arduino过来的数据
if (firstSample)
{ // NaN用来等待检查从arduino过来的数据
for (w=0;w<=2;w++)
{
RwEst[w] = RwAcc[w]; // 初始化加速度传感器读数
}
}
else
{
// 对RwGyro进行评估
if (abs(RwEst[2]) < 0.1)
{
// Rz值非常的小，它的作用是作为Axz与Ayz的计算参照值，防止放大的波动产生错误的结果。
// 这种情况下就跳过当前的陀螺仪数据，使用以前的。
for (w=0;w<=2;w++)
{
RwGyro[w] = RwEst[w];
}
}
else
{
// ZX/ZY平面和Z轴R的投影之间的角度，基于最近一次的RwEst值
for (w=0;w<=1;w++)
{
tmpf = Gyro[w]; // 获取当前陀螺仪的deg/s
tmpf *= interval / 1000.0f; // 得到角度变化值
Awz[w] = atan2(RwEst[w], RwEst[2]) * 180 / PI; // 得到角度并转换为度
Awz[w] += tmpf; // 根据陀螺仪的运动得到更新后的角度
}
// 判断RzGyro是多少，主要看Axz的弧度是多少
// 当Axz在-90 ..90 => cos(Awz) >= 0这个范围内的时候RzGyro是准确的
signRzGyro = ( cos(Awz[0] * PI / 180) >=0 ) ? 1 : -1;
// 从Awz的角度值反向计算RwGyro的公式请查看网页 http://starlino.com/imu_guide.html
for (w=0;w<=1;w++)
{
RwGyro[0] = sin(Awz[0] * PI / 180);
RwGyro[0] /= sqrt( 1 + squared(cos(Awz[0] * PI / 180)) * squared(tan(Awz[1] * PI / 180)) );
RwGyro[1] = sin(Awz[1] * PI / 180);
RwGyro[1] /= sqrt( 1 + squared(cos(Awz[1] * PI / 180)) * squared(tan(Awz[0] * PI / 180)) );
}
RwGyro[2] = signRzGyro * sqrt(1 - squared(RwGyro[0]) - squared(RwGyro[1]));
}
// 把陀螺仪与加速度传感器的值进行结合
for (w=0;w<=2;w++) RwEst[w] = (RwAcc[w] + wGyro * RwGyro[w]) / (1 + wGyro);
}
firstSample = false;
SerialUSB.print(RwEst[0]);//x
SerialUSB.print(" ");
SerialUSB.print(RwEst[1]); //y
SerialUSB.print(" ");
SerialUSB.println(RwEst[2]);//z
delay(50);
}
