HiDac/vector.cpp at master · mrerrormessage/HiDac · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
#include "constants.h"


void v2fCopy( v2f src, v2f dst ){
  dst[0] = src[0];
  dst[1] = src[1];
  return;
}

float v2fDot( v2f v1, v2f v2){
  return (v1[0] * v2[0] + v1[1] * v2[1]);
}

float v2fLen( v2f v ) {

  float aSquared = pow( v[0], 2);
  float bSquared = pow( v[1], 2);

  if( aSquared + bSquared <= 0.0){
    return 0.0;
  }

  return sqrt(aSquared + bSquared);

}


void v2fNormalize( v2f v, v2f res ){
  float len = v2fLen( v );
  if( len == 0.0){
    v2fCopy(v, res);
    return;
  }
  res[0] = v[0] / len;
  res[1] = v[1] / len;
  return;
}

float v2fCross( v2f v1, v2f v2){

  return (v1[0] * v2[1]) - (v1[1] * v2[0]);

}

//gets the tangent vector in the
void v2fTangent( v2f v, v2f ret ){
  ret[0] = v[1];
  ret[1] = -v[0];
}

void v2fNegate( v2f v){
  v[0] = -v[0];
  v[1] = -v[1];
}

void v2fSub( v2f v1, v2f v2, v2f res ){
  res[0] = v1[0] - v2[0];
  res[1] = v1[1] - v2[1];
}

void v2fAdd( v2f v1, v2f v2, v2f res ){
  res[0] = v1[0] + v2[0];
  res[1] = v1[1] + v2[1];
}

void v2fAdd( v2f v1, v2f v2, float t, v2f res){
  res[0] = v1[0] + (v2[0] * t);
  res[1] = v1[1] + (v2[1] * t);

}

void v2fMult( v2f v, float f, v2f res ){
  res[0] = f * v[0];
  res[1] = f * v[1];
  if( f == 0.0 ){
    res[0] = 0.0;
    res[1] = 0.0;
  }
}

void v2fFromJson( Json::Value v, v2f res){
  res[0] = v[0u].asDouble();
  res[1] = v[1u].asDouble();
}

void v2fPrint( v2f p ){
  std::cout << "<" << p[0] << "," << p[1] << ">";
  return;
}

void v2fPrint( const char * label, v2f p){
  std::cout << label << "<" << p[0] << "," << p[1] << ">" << "\n";
}

/*procedure taken from geometric tools for computer graphics*/
float ptToLineDist( v2f pos, v2f start, v2f dir, float len ){

  v2fNormalize(dir, dir);
  v2f diff, end;
  v2fSub( pos, start, diff );
  float t = v2fDot(dir, diff);
  if (t >= len ){

    v2fAdd( start, dir, len, end );
    v2fSub( pos, end, diff );
    return v2fLen( diff);
  }
  if (t <= 0.0 ){
    v2fSub( pos, start, diff);
    return v2fLen( diff);
  }


  v2fAdd( start, dir, t, end );
  v2fSub( pos, end, diff );
  return v2fLen( diff);

}

void v2fEpsilon( v2f v){
  if( v[0] <= MY_EPSILON && v[0] >= -MY_EPSILON )
    v[0] = 0.0;
  if( v[1] <= MY_EPSILON && v[1] >= -MY_EPSILON )
    v[1] = 0.0;
  return;
}