//Procedure that calculates the angle inbetween two vectors
float angleInbetweenVector (vector<float> vec1,vector<float> vec2)
{
float angleDeg;
if ((vectorLength(vec1)!=0 &&(vectorLength(vec2)!=0)))
{
float cosRad = (vecDotProduct(vec1,vec2)/(vectorLength(vec1)*vectorLength(vec2)));
angleDeg = acos (cosRad)*180/PI;
}else{
angleDeg=0;
}
return angleDeg;
}
DLLEX int vectorCompare(Vector *a, Vector *b) {
double l1, l2;
l1 = vectorLength(a); // Compute lengths of a and b
l2 = vectorLength(b);
if (l1 < l2) { // Compare the length of each and return the appropriate code
return -1;
} else if (l1 == l2) {
return 0;
} else {
return 1;
}
}
void Cursor::onProcess(const WidgetEvents & events)
{
if (!isVisible())
return;
sf::Vector2f pos = events.mousePosition - getPosition();
if (myCursorTransform.getPosition() != pos)
{
myCursorTransform.setPosition(pos);
invalidateVertices();
}
if (isRotatable())
{
sf::Vector2f deltaVec = myBackPosition - myCursorTransform.getPosition();
sf::Vector2f normVec = vectorLength(deltaVec) < 0.0001f ? sf::Vector2f(1, 0) : deltaVec / vectorLength(deltaVec);
myBackPosition = myCursorTransform.getPosition() + normVec * getRotationOffset();
float angle = radToDeg(angleBetween(myBackPosition, myCursorTransform.getPosition())) + getAngle();
myCursorTransform.setRotation(angle);
/*
if (myCursorTransform.getRotation() != angle)
{
myCursorTransform.setRotation(angle);
invalidateVertices();
}
*/
}
}
void Cell::loadCell (matvar_t *matVar, int i)
{
loadType (matVar, i);
loadLhs (matVar, i);
loadDetwindow (matVar, i);
loadI (matVar, i);
loadOffset (matVar, i);
// Type of structure 1 (last field called def)
if ( matVar->dims[0] == 1 && matVar->dims[1] == 1 )
{
setFlagStr(STR_DEF);
loadRhs (matVar, i);
loadDef (matVar, i);
}
// Type of structure 2 (last field called anchor)
else if ( vectorLength(matVar) > 1 )
{
setFlagStr(STR_ANCHOR);
loadRhs (matVar, i, false);
loadAnchor (matVar, i);
}
_IxDim = -1;
_Ix = NULL;
_IyDim = -1;
_Iy = NULL;
_scoreDim = -1;
_score = NULL;
}
C3DTVector quaternionToAxisAngle(_C3DTQuaternion q)
{
float lenOfVector = vectorLength(q);
C3DTVector r;
if(lenOfVector < EPSILON)
{
// Arbitrary vector, no rotation
r.cartesian.x = 1.0f;
r.cartesian.y = 0.0f;
r.cartesian.z = 0.0f;
r.cartesian.w = 0.0f;
}
else
{
float invLen = 1.0f / lenOfVector;
r.cartesian.x = q.cartesian.x * invLen;
r.cartesian.y = q.cartesian.y * invLen;
r.cartesian.z = q.cartesian.z * invLen;
r.cartesian.w = 2.0f * acosf(q.cartesian.w);
}
return r;
}
Vec2 normalizeVector(Vec2 * vec) {
Vec2 ret;
float length = vectorLength(vec);
ret.x = vec->x / length;
ret.y = vec->y / length;
return ret;
}
vector vectorCartesianToPolar(vector v)
{
double vAng = vectorAngle(v), vLength = vectorLength(v);
v.x = vLength;
v.y = vAng;
return v;
}
VectorType VectorType::normalizeVector()
{
//normalize the vector to length 1.
double normalizeScalar = 1.0 / vectorLength();
VectorType results = multiplyVector(normalizeScalar);
return results;
}
DLLEX double angle(Vector *a, Vector *b) {
double theta, dot, lenA, lenB;;
// We know that a.b = |a||b|cos (theta), so by computing the
// dot-product, and the magnitudes of a and b, we can solve for cos (theta)
// by rearranging to cos (theta) = a.b/(|a||b|)
// and use acos to find theta
dot = dotProduct(a, b);
lenA = vectorLength(a);
lenB = vectorLength(b);
theta = acos(dot/(lenA*lenB));
return theta * 180.0 / PI;
}
vector vectorAngleSet(vector v, double ang)
{
double vLength;
vLength = vectorLength(v);
v.x = cos(ang) * vLength;
v.y = sin(ang) * vLength;
return v;
}
size_t vectorLength(Type* t){
VectorType* vt = dyn_cast<VectorType>(t);
if(!vt){
return 0;
}
return vectorLength(vt);
}
float Spline::getInterpolatedLength() const
{
if (m_sfmlVertices.size() < 2)
return 0.f;
float total{ 0.f };
for (unsigned int v{ 0 }; v < m_sfmlVertices.size() - 1; ++v)
total += vectorLength(m_sfmlVertices[v + 1].position - m_sfmlVertices[v].position);
return total;
}
DLLEX Vector *vectorNormalize(Vector *v) {
double length;
Vector *ret;
length = vectorLength(v);
ret = vectorDiv(v, length);
return ret;
}
float Spline::getLength() const
{
if (m_vertices.size() < 2)
return 0.f;
float total{ 0.f };
for (unsigned int v{ 0 }; v < getLastVertexIndex(); ++v)
total += vectorLength(m_vertices[v + 1].position - m_vertices[v].position);
return total;
}
struct Vector vectorNormalize(struct Vector a)
{
struct Vector result;
double length = vectorLength(a);
if(length < EPSILON)
return vectorZero();
result.x = a.x/length;
result.y = a.y/length;
result.z = a.z/length;
return result;
}
//procedure that normalizes a give vector
vector<float> normVector (vector<float> vecToN)
{
vector<float> vecNorm;
if (vectorLength (vecToN)!=0)
{
vecNorm.push_back(vecToN[0]/vectorLength(vecToN));
vecNorm.push_back(vecToN[1]/vectorLength(vecToN));
vecNorm.push_back(vecToN[2]/vectorLength(vecToN));
}else{
vecNorm.push_back(0);
vecNorm.push_back(0);
vecNorm.push_back(0);
}
return vecNorm;
}
double VectorType::angleBetweenVectors(VectorType secondVector)
{
//cos(theta) = (u . v) / (|u|*|v|
double theta = acos(dotProduct(secondVector)
/
(vectorLength() * secondVector.vectorLength())
);
return theta;
}
void main() {
int i;
int start;
double c1[3] = {22.5, 12.8, 80.2};
double c2[3] = {81.0, 5.0, 56.1};
Vector *a = vectorNew(c1, 3);
Vector *b = vectorNew(c2, 3);
start = clock();
for (i=0; i<100000; i++) {
vectorAdd(a, b);
vectorSub(a, b);
vectorCompare(a, b);
angle(a, b);
vectorLength(a);
vectorLength(b);
dotProduct(a, b);
crossProduct(a, b);
}
printf("%f ", (float)(clock()-start)/CLOCKS_PER_SEC);
getchar();
}
// Gets a sphere that completely surrounds a bounding box
_C3DTSpheroid sphereFromBounds(_C3DTBounds b)
{
_C3DTSpheroid s;
// Center is the middle point between the 2 bounds coordinate
s.center.cartesian.x = (b.topRightFar.cartesian.x + b.bottomLeftNear.cartesian.x) / 2.0f;
s.center.cartesian.y = (b.topRightFar.cartesian.y + b.bottomLeftNear.cartesian.y) / 2.0f;
s.center.cartesian.z = (b.topRightFar.cartesian.z + b.bottomLeftNear.cartesian.z) / 2.0f;
s.radius = vectorLength( vectorSubtract(b.topRightFar, s.center));
return s;
}
/*
-----------------------------------------------------------------------------
Function:
Parameters:
Returns:
Notes:
-----------------------------------------------------------------------------
*/
PUBLIC float RadiusFromBounds( const vec3_t mins, const vec3_t maxs )
{
int i;
vec3_t corner;
float a, b;
for( i = 0; i < 3; ++i )
{
a = (float)fabs( mins[i] );
b = (float)fabs( maxs[i] );
corner[i] = a > b ? a : b;
}
return vectorLength( corner );
}
void Spline::smoothHandles()
{
for (unsigned int v{ 0 }; v < m_vertices.size() - 1; ++v)
{
const sf::Vector2f p1{ m_vertices[v].position };
const sf::Vector2f p2{ m_vertices[v + 1].position };
sf::Vector2f p0{ p1 };
sf::Vector2f p3{ p2 };
if (v > 0)
p0 = m_vertices[v - 1].position;
if (v < m_vertices.size() - 2)
p3 = m_vertices[v + 2].position;
const sf::Vector2f m0{ linearInterpolation(p0, p1, 0.5f) };
const sf::Vector2f m1{ linearInterpolation(p1, p2, 0.5f) };
const sf::Vector2f m2{ linearInterpolation(p2, p3, 0.5f) };
const float p01{ vectorLength(p1 - p0) };
const float p12{ vectorLength(p2 - p1) };
const float p23{ vectorLength(p3 - p2) };
float proportion0{ 0.f };
float proportion1{ 0.f };
if (p01 + p12 != 0.f)
proportion0 = p01 / (p01 + p12);
if (p12 + p23 != 0.f)
proportion1 = p12 / (p12 + p23);
const sf::Vector2f q0{ linearInterpolation(m0, m1, proportion0) };
const sf::Vector2f q1{ linearInterpolation(m1, m2, proportion1) };
m_vertices[v].frontHandle = m1 - q0;
m_vertices[v + 1].backHandle = m1 - q1;
}
m_vertices.front().backHandle = { 0.f, 0.f };
m_vertices.back().frontHandle = { 0.f, 0.f };
}
// Normalizing of a plane
inline C3DTPlane planeNormalize(C3DTPlane p)
{
float dist = vectorLength(p);
C3DTPlane r;
if (dist == 0.0f) {
return p;
}
r.cartesian.x = p.cartesian.x / dist;
r.cartesian.y = p.cartesian.y / dist;
r.cartesian.z = p.cartesian.z / dist;
r.cartesian.w = p.cartesian.w / dist;
return r;
}
void Cell::initializeAnchor (matvar_t *anchorStructure)
{
int length = vectorLength (anchorStructure);
anchor *a = new anchor [length];
assert (a != NULL);
setAnchorDim (length);
for (int i = 0; i < length; i++)
readNumber (anchorStructure, NULL, &(a[i].array), &(a[i].dim), i);
setAnchor (a);
delete[] a;
}
float projVector (vector<float> vec1,vector<float> vec2)
{
float proj;
float dot = vecDotProduct(vec1,vec2);
float lenght = vectorLength(vec2);
if (lenght!=0)
{
proj = dot/lenght;
}else{
proj=0;
}
return proj;
}
inline C3DTVector vectorNormalize( C3DTVector v ) {
float vecLength = vectorLength(v);
if(vecLength==0.0f){
return v;
}
C3DTVector r;
#ifdef USEVEC
vDSP_vsdiv( (float *)&v.flts, 1, &vecLength, (float *)&r.flts, 1, 4);
#else
r.cartesian.x = v.cartesian.x / vecLength;
r.cartesian.y = v.cartesian.y / vecLength;
r.cartesian.z = v.cartesian.z / vecLength;
#endif
return r;
}
void testVectorAlgebra(){
double v[]={4,11,8,10};
double vLength=0;
double v1[]={3,2,1,-2};
double v2[]={2,-1,4,1};
printf("\nMatrix\n");
vLength=vectorLength(v, 4);
printf("vector length: %f\n",vLength);
printf("vector addition\n");
vectorAddition(v1, v2, 4);
printVector(v1,4);
}
ObjectID::StringWrapper_t ObjectID::wrapStringToken(const std::string& token)
{
const size_t tokenLength = token.length() + 1; // +1 -> include null
size_t vectorLength(0);
if ((tokenLength % sizeof(MultipleCharWrapper_t)) == 0) // exact fit with null
{
vectorLength = tokenLength / sizeof(MultipleCharWrapper_t);
}
else // (tokenLength % sizeof (MultipleCharWrapper_t)) > 0; need to allocate one more to fit
{
vectorLength = (tokenLength / sizeof(MultipleCharWrapper_t)) + 1;
}
StringWrapper_t fastToken(vectorLength, MultipleCharWrapper_t(0));
memcpy(fastToken.data(), token.data(), token.length());
return fastToken;
}
/*----------------------------------------------------------------------------*/
static MATRIX buildRMatrix(MATRIX UB, MATRIX planeNormal,
tasQEPosition qe, int *errorCode){
MATRIX U1V, U2V, TV, TVINV, M;
*errorCode = 1;
U1V = tasReflectionToQC(qe,UB);
if(U1V == NULL){
*errorCode = UBNOMEMORY;
return NULL;
}
normalizeVector(U1V);
U2V = vectorCrossProduct(planeNormal,U1V);
if(U2V == NULL){
killVector(U1V);
*errorCode = UBNOMEMORY;
return NULL;
}
if(vectorLength(U2V) < .0001){
*errorCode = BADUBORQ;
killVector(U1V);
killVector(U2V);
return NULL;
}
TV = buildTVMatrix(U1V,U2V);
if(TV == NULL){
killVector(U1V);
killVector(U2V);
*errorCode = UBNOMEMORY;
return NULL;
}
TVINV = mat_inv(TV);
if(TVINV == NULL){
*errorCode = BADUBORQ;
}
killVector(U1V);
killVector(U2V);
mat_free(TV);
return TVINV;
}
void boatReadKeyboard(boat b)
{
b->extra->isAccel = 0;
b->extra->isTurning = 0;
if (key[b->extra->keyLayout[ACCEL_BUTTON]])
b->extra->isAccel += 1;
if (key[b->extra->keyLayout[BRAKE_BUTTON]])
b->extra->isAccel -= 1;
if (key[b->extra->keyLayout[TURN_RIGHT_BUTTON]])
b->extra->isTurning += 1;
if (key[b->extra->keyLayout[TURN_LEFT_BUTTON]])
b->extra->isTurning -= 1;
if (key[b->extra->keyLayout[ANCHOR_BUTTON]]) {
if (!b->extra->anchorButtonHeld && vectorLength(b->vel) <= b->extra->maxAnchorSpeed) {
b->extra->isAnchored = !b->extra->isAnchored;
b->extra->anchorButtonHeld = 1;
}
} else
b->extra->anchorButtonHeld = 0;
}
double * normalVector(double *a, int n)
{
int i;
double vLength=0;
double *b;
b = allocateDoubleVector(n);
vLength=vectorLength(a,n);
for(i=0;i<n;i++)
vLength=vLength+(a[i]*a[i]);
vLength=sqrt(vLength);
for(i=0;i<n;i++)
b[i]=a[i]/vLength;
return b;
}
