void N_VScale(real c, N_Vector x, N_Vector z)
{
integer N;
real *xd, *zd;
if (z == x) { /* BLAS usage: scale x <- cx */
VScaleBy(c, x);
return;
}
if (c == ONE) {
VCopy(x, z);
} else if (c == -ONE) {
VNeg(x, z);
} else {
N = x->length;
xd = x->data;
zd = z->data;
#ifndef _OPENMP
for (integer i=0; i < N; i++) *zd++ = c * (*xd++);
#else
#pragma omp parallel for
for (integer i=0; i < N; i++)
zd[i] = c * xd[i];
#endif
}
}
void CoastToAttitude(float* myState, float* attitudeTarget)
{
float intermediate[3];
if (VAngle(&myState[6], attitudeTarget) < (COASTING_VELOCITY * 1.5))
VCopy(attitudeTarget, intermediate);
else
VRotate(&myState[6], COASTING_VELOCITY, attitudeTarget, intermediate);
ZRSetAttitudeTarget(intermediate);
}
static void RotateTarget(float * myState, float * pos)
{
#define VLen(a) mathVecMagnitude((a), 3)
#define VAdd(a, b, result) mathVecAdd(result, (a), (b), 3)
#define VSub(a, b, result) Vfunc(2, (a), (b), (result), 0)
#define VUnit(a, result) Vfunc(3, (a), NULL, (result), 0)
#define VMult(a, b, result) Vfunc(4, (a), NULL, (result), (b))
#define VDot(a, b) Vfunc(5, (a), (b), NULL, 0)
#define VDist(a, b) Vfunc(6, (a), (b), NULL, 0)
#define VCopy(a, result) memcpy((result), (a), sizeof(float)*3)
#define VAngle(a, b) Vfunc(8, (a), (b), NULL, 0)
#define VPoint(a, b, result) Vfunc(9, (a), (b), (result), 0)
#define Deg2Rad(Deg) (Deg*PI/180.0)
#define Rad2Deg(Rad) (Rad*180.0/PI)
// do ZRSetAttitudeTarget(current_att + AngleForward)
#define AngleForward 30
float temp[3], target_att[3], current_att[3];
float current_theta, target_theta;
ZRSetPositionTarget(pos);
VCopy(myState+6,current_att);
current_theta = atan2(current_att[2],current_att[1]);
if (current_theta < 0)
current_theta = current_theta + 2*PI;
target_theta = current_theta + Deg2Rad(AngleForward);
temp[0] = -5.0*current_att[0];
temp[1] = cos(target_theta);
temp[2] = sin(target_theta);
VUnit(temp,target_att);
//printf("time: %2.0f, current_att: (%5.2f, %5.2f, %5.2f), target: (%5.2f, %5.2f, %5.2f), to_target: %5.1f",
//procvar[0],current_att[0],current_att[1],current_att[2],target_att[0],target_att[1],target_att[2],
//VAngle(current_att,target_att));
//printf(" rates: (%5.2f, %5.2f, %5.2f)\n", myState[9],myState[10],myState[11]);
//printf( " out-of-plane: %5.2f\n",asin(current_att[0])*180/3.14159);
ZRSetAttitudeTarget(target_att);
}
void N_VScale(float c, N_Vector x, N_Vector z)
{
int i, N;
float *xd, *zd;
if (z == x) { /* BLAS usage: scale x <- cx */
VScaleBy(c, x);
return;
}
if (c == ONE) {
VCopy(x, z);
} else if (c == -ONE) {
VNeg(x, z);
} else {
N = x->length;
xd = x->data;
zd = z->data;
for (i=0; i < N; ++i ) *zd++ = c * (*xd++);
}
}
int main(void)
{
//assign X component
vec1[0] = 2.0;
//assign Y component
vec1[1] = 2.0;
//assign Z component
vec1[2] = 2.0;
vec2[0] = 5.0;
vec2[1] = 5.0;
vec2[2] = 5.0;
VectorAdd(vec1,vec2,vec3);
printf("Vector addition: ");
int i;
for(i = 0; i < 3; i++)
{
printf("%g ", vec3[i]);
}
VectorSubtract(vec1,vec2,vec3);
printf("\nVector subtraction: ");
for(i = 0; i < sizeof(vec3)/sizeof(int); i++)
{
printf("%g ", vec3[i]);
}
DotProduct(vec1,vec2,cheez);
printf("\nDot product: %g", cheez);
Scaling(vec1, 2);
printf("\nScaling by 2: ");
for(i = 0; i < sizeof(vec1)/sizeof(int); i++)
{
printf("%g ", vec1[i]);
}
VCopy(vec1, vec3);
printf("\nCopying vec1 to vec3: ");
for(i = 0; i < sizeof(vec3)/sizeof(int); i++)
{
printf("%g ", vec3[i]);
}
VClear(vec3);
printf("\nClearing vec3: ");
for(i = 0; i < sizeof(vec3)/sizeof(int); i++)
{
printf("%g ", vec3[i]);
}
Inverse(vec2);
printf("\nInverse of vec2: ");
for(i = 0; i < sizeof(vec3)/sizeof(int); i++)
{
printf("%g ", vec2[i]);
}
Cross(vec1, vec2, vec3);
printf("\nCross of vec1 and vec2: ");
for(i = 0; i < sizeof(vec3)/sizeof(int); i++)
{
printf("%g ", vec3[i]);
}
Magnitude(vec2);
printf("\nMagnitude of vec2: ");
printf("%g ", Magnitude(vec2));
Normalize(vec2);
printf("\nNormalize of vec2: ");
for(i = 0; i < sizeof(vec3)/sizeof(int); i++)
{
printf("%g ", vec2[i]);
}
}
