// Möller–Trumbore intersection algorithm
bool PnPProblem::intersect_MollerTrumbore(Ray &Ray, Triangle &Triangle, double *out)
{
const double EPSILON = 0.000001;
cv::Point3f e1, e2;
cv::Point3f P, Q, T;
double det, inv_det, u, v;
double t;
cv::Point3f V1 = Triangle.getV0(); // Triangle vertices
cv::Point3f V2 = Triangle.getV1();
cv::Point3f V3 = Triangle.getV2();
cv::Point3f O = Ray.getP0(); // Ray origin
cv::Point3f D = Ray.getP1(); // Ray direction
//Find vectors for two edges sharing V1
e1 = SUB(V2, V1);
e2 = SUB(V3, V1);
// Begin calculation determinant - also used to calculate U parameter
P = CROSS(D, e2);
// If determinant is near zero, ray lie in plane of triangle
det = DOT(e1, P);
//NOT CULLING
if(det > -EPSILON && det < EPSILON) return false;
inv_det = 1.f / det;
//calculate distance from V1 to ray origin
T = SUB(O, V1);
//Calculate u parameter and test bound
u = DOT(T, P) * inv_det;
//The intersection lies outside of the triangle
if(u < 0.f || u > 1.f) return false;
//Prepare to test v parameter
Q = CROSS(T, e1);
//Calculate V parameter and test bound
v = DOT(D, Q) * inv_det;
//The intersection lies outside of the triangle
if(v < 0.f || u + v > 1.f) return false;
t = DOT(e2, Q) * inv_det;
if(t > EPSILON) { //ray intersection
*out = t;
return true;
}
// No hit, no win
return false;
}
static void
findstats(Pos p, Ori o)
{
/* Recalculate cross assert and score total at 'p'
*/
Pos left, right;
Word lword, rword;
Node n;
Edge e;
int s;
lword.n = rword.n = 0;
if(EDGE(p))
return;
/* find word to the left */
s = 0;
for(left=PREV(p,o); HASLETTER(left); left = PREV(left,o))
;
left = NEXT(left,o);
while (HASLETTER(left)) {
lword.c[lword.n++] = LETTER(left);
s += SCORE(left);
left = NEXT(left,o);
}
/* find word to the right */
for(right=NEXT(p,o); HASLETTER(right); right = NEXT(right,o)) {
rword.c[rword.n++] = LETTER(right);
s += SCORE(right);
}
if(DBG) {
wordprint(&lword);
print("X");
wordprint(&rword);
print(" [%d] ", s);
}
SIDE(p,o) = s;
ISANCHOR(p) = true;
/* calculate cross asserts */
CROSS(p,o) = 0;
n = traverse(root, &lword, 0);
assert(n>=0);
if(n>0)
do {
e = dict[n++];
if ( (rword.n && isword(NODE(e), &rword)) ||
(!rword.n && TERM(e)) ) {
CROSS(p,o) |= 1 << LET(e);
DPRINT("%c, ", LET(e)+'a');
}
} while (!(LAST(e)));
DPRINT("\n");
}
/* and one CROSS has been moved out from the if-else if-else */
int intersect_triangle3(double orig[3], double dir[3],
double vert0[3], double vert1[3], double vert2[3],
double *t, double *u, double *v)
{
double edge1[3], edge2[3], tvec[3], pvec[3], qvec[3];
double det,inv_det;
/* find vectors for two edges sharing vert0 */
SUB(edge1, vert1, vert0);
SUB(edge2, vert2, vert0);
/* begin calculating determinant - also used to calculate U parameter */
CROSS(pvec, dir, edge2);
/* if determinant is near zero, ray lies in plane of triangle */
det = DOT(edge1, pvec);
/* calculate distance from vert0 to ray origin */
SUB(tvec, orig, vert0);
inv_det = 1.0 / det;
CROSS(qvec, tvec, edge1);
if (det > EPSILON)
{
*u = DOT(tvec, pvec);
if (*u < 0.0 || *u > det)
return 0;
/* calculate V parameter and test bounds */
*v = DOT(dir, qvec);
if (*v < 0.0 || *u + *v > det)
return 0;
}
else if(det < -EPSILON)
{
/* calculate U parameter and test bounds */
*u = DOT(tvec, pvec);
if (*u > 0.0 || *u < det)
return 0;
/* calculate V parameter and test bounds */
*v = DOT(dir, qvec) ;
if (*v > 0.0 || *u + *v < det)
return 0;
}
else return 0; /* ray is parallell to the plane of the triangle */
*t = DOT(edge2, qvec) * inv_det;
(*u) *= inv_det;
(*v) *= inv_det;
return 1;
}
/* and one CROSS has been moved out from the if-else if-else */
int intersect_triangle3(const RAYTRI *rt)
{
double u, v;
double edge1[3], edge2[3], tvec[3], pvec[3], qvec[3];
double det,inv_det;
/* find vectors for two edges sharing rt->v0 */
SUB(edge1, rt->v1, rt->v0);
SUB(edge2, rt->v2, rt->v0);
/* begin calculating determinant - also used to calculate U parameter */
CROSS(pvec, rt->dir, edge2);
/* if determinant is near zero, ray lies in plane of triangle */
det = DOT(edge1, pvec);
/* calculate distance from rt->v0 to ray origin */
SUB(tvec, rt->org, rt->v0);
inv_det = 1.0 / det;
CROSS(qvec, tvec, edge1);
if (det > EPSILON)
{
u = DOT(tvec, pvec);
if (u < 0.0 || u > det)
return 0;
/* calculate V parameter and test bounds */
v = DOT(rt->dir, qvec);
if (v < 0.0 || u + v > det)
return 0;
}
else if(det < -EPSILON)
{
/* calculate U parameter and test bounds */
u = DOT(tvec, pvec);
if (u > 0.0 || u < det)
return 0;
/* calculate V parameter and test bounds */
v = DOT(rt->dir, qvec) ;
if (v > 0.0 || u + v < det)
return 0;
}
else return 0; /* ray is parallell to the plane of the triangle */
double t = DOT(edge2, qvec) * inv_det;
if(t<0.0 || t>1.0) return 0;
return 1;
}
long point_triangle_intersection(Point3 p, Triangle3 t)
{
long sign12,sign23,sign31;
Point3 vect12,vect23,vect31,vect1h,vect2h,vect3h;
Point3 cross12_1p,cross23_2p,cross31_3p;
/* First, a quick bounding-box test: */
/* If P is outside triangle bbox, there cannot be an intersection. */
if (p.x > MAX3(t.v1.x, t.v2.x, t.v3.x)) return(OUTSIDE);
if (p.y > MAX3(t.v1.y, t.v2.y, t.v3.y)) return(OUTSIDE);
if (p.z > MAX3(t.v1.z, t.v2.z, t.v3.z)) return(OUTSIDE);
if (p.x < MIN3(t.v1.x, t.v2.x, t.v3.x)) return(OUTSIDE);
if (p.y < MIN3(t.v1.y, t.v2.y, t.v3.y)) return(OUTSIDE);
if (p.z < MIN3(t.v1.z, t.v2.z, t.v3.z)) return(OUTSIDE);
/* For each triangle side, make a vector out of it by subtracting vertexes; */
/* make another vector from one vertex to point P. */
/* The crossproduct of these two vectors is orthogonal to both and the */
/* signs of its X,Y,Z components indicate whether P was to the inside or */
/* to the outside of this triangle side. */
SUB(t.v1, t.v2, vect12)
SUB(t.v1, p, vect1h);
CROSS(vect12, vect1h, cross12_1p)
sign12 = SIGN3(cross12_1p); /* Extract X,Y,Z signs as 0..7 or 0...63 integer */
SUB(t.v2, t.v3, vect23)
SUB(t.v2, p, vect2h);
CROSS(vect23, vect2h, cross23_2p)
sign23 = SIGN3(cross23_2p);
SUB(t.v3, t.v1, vect31)
SUB(t.v3, p, vect3h);
CROSS(vect31, vect3h, cross31_3p)
sign31 = SIGN3(cross31_3p);
/* If all three crossproduct vectors agree in their component signs, */
/* then the point must be inside all three. */
/* P cannot be OUTSIDE all three sides simultaneously. */
/* this is the old test; with the revised SIGN3() macro, the test
* needs to be revised. */
#ifdef OLD_TEST
if ((sign12 == sign23) && (sign23 == sign31))
return(INSIDE);
else
return(OUTSIDE);
#else
return ((sign12 & sign23 & sign31) == 0) ? OUTSIDE : INSIDE;
#endif
}
void Camera::setFaceToOrigin() {
// logger.prs(theta);
// logger.prl(gamma);
double rad = position.mag();
position.x = rad * cos(D2R(theta)) * cos(D2R(gamma));
position.y = rad * sin(D2R(theta)) * cos(D2R(gamma));
position.z = rad * sin(D2R(gamma));
forward = -position;
up = CROSS(CROSS(forward, Vector(0, 0, 1)), forward);
}
hacd::HaI32 tri_tri_overlap_test_3d(hacd::HaF32 p1[3], hacd::HaF32 q1[3], hacd::HaF32 r1[3],
hacd::HaF32 p2[3], hacd::HaF32 q2[3], hacd::HaF32 r2[3])
{
hacd::HaF32 dp1, dq1, dr1, dp2, dq2, dr2;
hacd::HaF32 v1[3], v2[3];
hacd::HaF32 N1[3], N2[3];
/* Compute distance signs of p1, q1 and r1 to the plane of
triangle(p2,q2,r2) */
SUB(v1,p2,r2)
SUB(v2,q2,r2)
CROSS(N2,v1,v2)
SUB(v1,p1,r2)
dp1 = DOT(v1,N2);
SUB(v1,q1,r2)
dq1 = DOT(v1,N2);
SUB(v1,r1,r2)
dr1 = DOT(v1,N2);
if (((dp1 * dq1) > 0.0f) && ((dp1 * dr1) > 0.0f)) return 0;
/* Compute distance signs of p2, q2 and r2 to the plane of
triangle(p1,q1,r1) */
SUB(v1,q1,p1)
SUB(v2,r1,p1)
CROSS(N1,v1,v2)
SUB(v1,p2,r1)
dp2 = DOT(v1,N1);
SUB(v1,q2,r1)
dq2 = DOT(v1,N1);
SUB(v1,r2,r1)
dr2 = DOT(v1,N1);
if (((dp2 * dq2) > 0.0f) && ((dp2 * dr2) > 0.0f)) return 0;
/* Permutation in a canonical form of T1's vertices */
if (dp1 > 0.0f) {
if (dq1 > 0.0f) TRI_TRI_3D(r1,p1,q1,p2,r2,q2,dp2,dr2,dq2)
else if (dr1 > 0.0f) TRI_TRI_3D(q1,r1,p1,p2,r2,q2,dp2,dr2,dq2)
else TRI_TRI_3D(p1,q1,r1,p2,q2,r2,dp2,dq2,dr2)
} else if (dp1 < 0.0f) {
void PenaltyMarkPercept::draw() const
{
DECLARE_DEBUG_DRAWING("representation:PenaltyMarkPercept:image", "drawingOnImage");
DECLARE_DEBUG_DRAWING("representation:PenaltyMarkPercept:field", "drawingOnField");
if(Blackboard::getInstance().exists("FrameInfo"))
{
const FrameInfo& frameInfo = static_cast<const FrameInfo&>(Blackboard::getInstance()["FrameInfo"]);
if(timeLastSeen == frameInfo.time)
{
CROSS("representation:PenaltyMarkPercept:image", position.x(), position.y(), 5, 5, Drawings::solidPen, ColorRGBA::blue);
CROSS("representation:PenaltyMarkPercept:field", positionOnField.x(), positionOnField.y(), 40, 40, Drawings::solidPen, ColorRGBA::blue);
}
}
}
int
main(int argc,char *argv[])
{
/* rotates input vector by specified angle around given rotation axis */
VECTOR r,n,nxr,rp;
double phi,cphi,sphi,ndotr;
if (argc != 8) {
(void) fprintf(stderr,"Usage: %s x y z phi rx ry rz\n",argv[0]);
return 1;
}
SET_VEC(r,atof(argv[1]),atof(argv[2]),atof(argv[3]));
assert(MAG(r) > 0.0);
phi = atof(argv[4]);
SET_VEC(n,atof(argv[5]),atof(argv[6]),atof(argv[7]));
assert(MAG(n) > 0.0);
ndotr = DOT(n,r);
CROSS(n,r,nxr);
cphi = cos(phi);
sphi = sin(phi);
SCALE_VEC(r,cphi);
SCALE_VEC(n,ndotr*(1.0-cphi));
SCALE_VEC(nxr,sphi);
ADD_VEC(r,n,rp);
ADD_VEC(rp,nxr,rp);
(void) printf("%g %g %g\n",rp[X],rp[Y],rp[Z]);
return 0;
}
/**
* @name angle_change
*
* Return the change in angle (degrees) of the line segments between
* points one and two, and two and three.
*/
int Wordrec::angle_change(EDGEPT *point1, EDGEPT *point2, EDGEPT *point3) {
VECTOR vector1;
VECTOR vector2;
int angle;
/* Compute angle */
vector1.x = point2->pos.x - point1->pos.x;
vector1.y = point2->pos.y - point1->pos.y;
vector2.x = point3->pos.x - point2->pos.x;
vector2.y = point3->pos.y - point2->pos.y;
/* Use cross product */
float length = std::sqrt(static_cast<float>(LENGTH(vector1)) * LENGTH(vector2));
if (static_cast<int>(length) == 0)
return (0);
angle = static_cast<int>(floor(asin(CROSS (vector1, vector2) /
length) / M_PI * 180.0 + 0.5));
/* Use dot product */
if (SCALAR (vector1, vector2) < 0)
angle = 180 - angle;
/* Adjust angle */
if (angle > 180)
angle -= 360;
if (angle <= -180)
angle += 360;
return (angle);
}
void MotionRequest::draw() const
{
DECLARE_DEBUG_DRAWING("representation:MotionRequest", "drawingOnField"); // drawing of a request walk vector
if(motion == walk)
{
switch(walkRequest.mode)
{
case WalkRequest::targetMode:
{
LINE("representation:MotionRequest", 0, 0, walkRequest.target.translation.x, walkRequest.target.translation.y, 0, Drawings::ps_solid, ColorRGBA(0xcd, 0, 0));
CROSS("representation:MotionRequest", walkRequest.target.translation.x, walkRequest.target.translation.y, 50, 0, Drawings::ps_solid, ColorRGBA(0xcd, 0, 0));
Vector2<> rotation(500.f, 0.f);
rotation.rotate(walkRequest.target.rotation);
ARROW("representation:MotionRequest", walkRequest.target.translation.x, walkRequest.target.translation.y, walkRequest.target.translation.x + rotation.x, walkRequest.target.translation.y + rotation.y, 0, Drawings::ps_solid, ColorRGBA(0xcd, 0, 0, 127));
}
break;
case WalkRequest::speedMode:
case WalkRequest::percentageSpeedMode:
{
Vector2<> translation = walkRequest.mode == WalkRequest::speedMode ? walkRequest.speed.translation * 10.f : walkRequest.speed.translation * 1000.f;
ARROW("representation:MotionRequest", 0, 0, translation.x, translation.y, 0, Drawings::ps_solid, ColorRGBA(0xcd, 0, 0));
if(walkRequest.target.rotation != 0.0f)
{
translation.x = translation.abs();
translation.y = 0;
translation.rotate(walkRequest.speed.rotation);
ARROW("representation:MotionRequest", 0, 0, translation.x, translation.y, 0, Drawings::ps_solid, ColorRGBA(0xcd, 0, 0, 127));
}
}
break;
default:
break;
}
}
}
void AseFile::ComputeNormals(zASE_Object &obj)
{
if(obj.pFaceNormals==NULL && obj.numOfFaces>0)obj.pFaceNormals = new vec[obj.numOfFaces];
if(obj.pFaceNormals==NULL)return;
if(obj.pNormals==NULL && obj.numOfVerts>0)obj.pNormals = new vec[obj.numOfVerts];
if(obj.pNormals==NULL)return;
for(int j=0; j<obj.numOfFaces; j++)
{
vec a,b;
a = obj.pVerts[obj.pFaces[j].index[1]] - obj.pVerts[obj.pFaces[j].index[0]];
b = obj.pVerts[obj.pFaces[j].index[2]] - obj.pVerts[obj.pFaces[j].index[0]];
obj.pFaceNormals[j] = CROSS( a, b);
obj.pFaceNormals[j].Normalize();
}
int count;
for( j=0; j<obj.numOfVerts; j++)
{
count=0;
obj.pNormals[j].clear();
for(int k=0; k<obj.numOfFaces; k++)
{
if( obj.pFaces[k].index[0]==j || obj.pFaces[k].index[1]==j || obj.pFaces[k].index[2]==j )
{
obj.pNormals[j] += obj.pFaceNormals[k];
count++;
}
}
obj.pNormals[j] *= 1.f/(float)count;
obj.pNormals[j].Normalize();
}
}
///
// PointTriangleIntersection()
//
// Test if 3D point is inside 3D triangle
static
int PointTriangleIntersection(const vector3& p, const TRI& t)
{
int sign12,sign23,sign31;
vector3 vect12,vect23,vect31,vect1h,vect2h,vect3h;
vector3 cross12_1p,cross23_2p,cross31_3p;
///
// First, a quick bounding-box test:
// If P is outside triangle bbox, there cannot be an intersection.
//
if (p.x() > MAX3(t.m_P[0].x(), t.m_P[1].x(), t.m_P[2].x())) return(OUTSIDE);
if (p.y() > MAX3(t.m_P[0].y(), t.m_P[1].y(), t.m_P[2].y())) return(OUTSIDE);
if (p.z() > MAX3(t.m_P[0].z(), t.m_P[1].z(), t.m_P[2].z())) return(OUTSIDE);
if (p.x() < MIN3(t.m_P[0].x(), t.m_P[1].x(), t.m_P[2].x())) return(OUTSIDE);
if (p.y() < MIN3(t.m_P[0].y(), t.m_P[1].y(), t.m_P[2].y())) return(OUTSIDE);
if (p.z() < MIN3(t.m_P[0].z(), t.m_P[1].z(), t.m_P[2].z())) return(OUTSIDE);
///
// For each triangle side, make a vector out of it by subtracting vertexes;
// make another vector from one vertex to point P.
// The crossproduct of these two vectors is orthogonal to both and the
// signs of its X,Y,Z components indicate whether P was to the inside or
// to the outside of this triangle side.
//
SUB(t.m_P[0], t.m_P[1], vect12);
SUB(t.m_P[0], p, vect1h);
CROSS(vect12, vect1h, cross12_1p)
sign12 = SIGN3(cross12_1p); /* Extract X,Y,Z signs as 0..7 or 0...63 integer */
SUB(t.m_P[1], t.m_P[2], vect23)
SUB(t.m_P[1], p, vect2h);
CROSS(vect23, vect2h, cross23_2p)
sign23 = SIGN3(cross23_2p);
SUB(t.m_P[2], t.m_P[0], vect31)
SUB(t.m_P[2], p, vect3h);
CROSS(vect31, vect3h, cross31_3p)
sign31 = SIGN3(cross31_3p);
///
// If all three crossproduct vectors agree in their component signs, /
// then the point must be inside all three.
// P cannot be OUTSIDE all three sides simultaneously.
//
return (((sign12 & sign23 & sign31) == 0) ? OUTSIDE : INSIDE);
}
int
intersect_triangle(const float orig[3], const float dir[3],
const float vert0[3], const float vert1[3], const float vert2[3],
float *t, float *u, float *v)
{
float edge1[3], edge2[3], tvec[3], pvec[3], qvec[3];
float det,inv_det;
/* find vectors for two edges sharing vert0 */
SUB(edge1, vert1, vert0);
SUB(edge2, vert2, vert0);
/* begin calculating determinant - also used to calculate U parameter */
CROSS(pvec, dir, edge2);
/* if determinant is near zero, ray lies in plane of triangle */
det = DOT(edge1, pvec);
if (det > -EPSILON && det < EPSILON)
return 0;
inv_det = 1.0f / det;
/* calculate distance from vert0 to ray origin */
SUB(tvec, orig, vert0);
/* calculate U parameter and test bounds */
*u = DOT(tvec, pvec) * inv_det;
if (*u < 0.0 || *u > 1.0)
return 0;
/* prepare to test V parameter */
CROSS(qvec, tvec, edge1);
/* calculate V parameter and test bounds */
*v = DOT(dir, qvec) * inv_det;
if (*v < 0.0 || *u + *v > 1.0)
return 0;
/* calculate t, ray intersects triangle */
*t = DOT(edge2, qvec) * inv_det;
return 1;
}
void
bp_camera_set_look_at (camera_t *camera, vector_t look_at)
{
float dir_length, up_length, right_length;
vector_t dir; /* direction vector not normalized */
dir_length = MAG (camera->direction);
up_length = MAG (camera->up);
right_length = MAG (camera->right);
SUB (dir, look_at, camera->location);
CROSS (camera->right, dir, camera->sky);
VRESIZE (camera->right, right_length);
CROSS (camera->up, camera->right, dir);
VRESIZE (camera->up, up_length);
VSET_SIZE (camera->direction, dir, dir_length);
}
static void tri_normal(tri * trn, vector * pnt, ray * incident, vector * N) {
CROSS((*N), trn->edge1, trn->edge2);
VNorm(N);
if (VDot(N, &(incident->d)) > 0.0) {
N->x=-N->x;
N->y=-N->y;
N->z=-N->z;
}
}
// Computes the min and max cross product of the outline points with the
// given vec and returns the results in min_xp and max_xp. Geometrically
// this is the left and right edge of the outline perpendicular to the
// given direction, but to get the distance units correct, you would
// have to divide by the modulus of vec.
void TESSLINE::MinMaxCrossProduct(const TPOINT vec,
int* min_xp, int* max_xp) const {
*min_xp = MAX_INT32;
*max_xp = MIN_INT32;
EDGEPT* this_edge = loop;
do {
if (!this_edge->IsHidden() || !this_edge->prev->IsHidden()) {
int product = CROSS(this_edge->pos, vec);
UpdateRange(product, min_xp, max_xp);
}
this_edge = this_edge->next;
} while (this_edge != loop);
}
void shNgin3dMove(float frontSpeed, float rightSpeed, float upSpeed, unsigned long dt)
{
// cashed version of conversion of latitude and longitude to points already exist in _Ngin3d_cameraTarget and _Ngin3d_cameraUp
float targetDir[3] = {_Ngin3d_cameraTarget[0]-_Ngin3d_cameraPosition[0], _Ngin3d_cameraTarget[1]-_Ngin3d_cameraPosition[1], _Ngin3d_cameraTarget[2]-_Ngin3d_cameraPosition[2]};
float rightDir[3] = {CROSS(targetDir, _Ngin3d_cameraUp)}; // targetDir and _Ngin3d_cameraUp should already be normalized
float upDir[3] = {0, 1, 0};
if (g_Ngin3d_uprightMode)
{
if (_Ngin3d_cameraUp[1] < 0)
upDir[1] = -1;
}
else
{
upDir[0] = _Ngin3d_cameraUp[0];
upDir[1] = _Ngin3d_cameraUp[1];
upDir[2] = _Ngin3d_cameraUp[2];
}
float speed[3] = {frontSpeed*targetDir[0]+upSpeed*upDir[0]+rightSpeed*rightDir[0],
frontSpeed*targetDir[1]+upSpeed*upDir[1]+rightSpeed*rightDir[1],
frontSpeed*targetDir[2]+upSpeed*upDir[2]+rightSpeed*rightDir[2]};
float gravity[3] = {g_Ngin3d_settings[NGIN3D_GRAVITY_X], g_Ngin3d_settings[NGIN3D_GRAVITY_Y], g_Ngin3d_settings[NGIN3D_GRAVITY_Z]};
float normGravity = NORM(gravity);
if (g_Ngin3d_uprightMode && !STATE_IN_AIR && normGravity > EPSILON)
{
float speedInDirectionOfGravity = DOT(speed, gravity)/normGravity/normGravity;
speed[0] -= gravity[0]*speedInDirectionOfGravity;
speed[1] -= gravity[1]*speedInDirectionOfGravity;
speed[2] -= gravity[2]*speedInDirectionOfGravity;
}
shNgin3dCollisionResult cr = g_Ngin3d_player.shNgin3dPOUpdate(dt, speed);
if (cr == NGIN3D_COLLISION_WITH_GROUND)
{
STATE_IN_AIR = false;
}
float cameraDisplacement[3] = {0, 0, 0};
if (g_Ngin3d_uprightMode)
{
if (STATE_CROUCHED)
cameraDisplacement[1] = g_Ngin3d_settings[NGIN3D_CROUCHED_HEIGHT];
else
cameraDisplacement[1] = g_Ngin3d_settings[NGIN3D_HEIGHT];
}
_Ngin3d_cameraPosition[0] = g_Ngin3d_player.position[0]+cameraDisplacement[0];
_Ngin3d_cameraPosition[1] = g_Ngin3d_player.position[1]+cameraDisplacement[1];
_Ngin3d_cameraPosition[2] = g_Ngin3d_player.position[2]+cameraDisplacement[2];
_Ngin3d_cameraTarget[0] = _Ngin3d_cameraPosition[0]+targetDir[0];
_Ngin3d_cameraTarget[1] = _Ngin3d_cameraPosition[1]+targetDir[1];
_Ngin3d_cameraTarget[2] = _Ngin3d_cameraPosition[2]+targetDir[2];
(*_Ngin3d_cameraPositionFunction)(_Ngin3d_cameraPosition);
(*_Ngin3d_cameraTargetFunction)(_Ngin3d_cameraTarget);
}
/**********************************************************************
* divide_blobs
*
* Create two blobs by grouping the outlines in the appropriate blob.
* The outlines that are beyond the location point are moved to the
* other blob. The ones whose x location is less than that point are
* retained in the original blob.
**********************************************************************/
void divide_blobs(TBLOB *blob, TBLOB *other_blob, bool italic_blob,
const TPOINT& location) {
TPOINT vertical = italic_blob ? kDivisibleVerticalItalic
: kDivisibleVerticalUpright;
TESSLINE *outline1 = NULL;
TESSLINE *outline2 = NULL;
TESSLINE *outline = blob->outlines;
blob->outlines = NULL;
int location_prod = CROSS(location, vertical);
while (outline != NULL) {
TPOINT mid_pt = {(outline->topleft.x + outline->botright.x) / 2,
(outline->topleft.y + outline->botright.y) / 2};
int mid_prod = CROSS(mid_pt, vertical);
if (mid_prod < location_prod) {
// Outline is in left blob.
if (outline1)
outline1->next = outline;
else
blob->outlines = outline;
outline1 = outline;
} else {
// Outline is in right blob.
if (outline2)
outline2->next = outline;
else
other_blob->outlines = outline;
outline2 = outline;
}
outline = outline->next;
}
if (outline1)
outline1->next = NULL;
if (outline2)
outline2->next = NULL;
}
/**********************************************************************
* divisible_blob
*
* Returns true if the blob contains multiple outlines than can be
* separated using divide_blobs. Sets the location to be used in the
* call to divide_blobs.
**********************************************************************/
bool divisible_blob(TBLOB *blob, bool italic_blob, TPOINT* location) {
if (blob->outlines == NULL || blob->outlines->next == NULL)
return false; // Need at least 2 outlines for it to be possible.
int max_gap = 0;
TPOINT vertical = italic_blob ? kDivisibleVerticalItalic
: kDivisibleVerticalUpright;
for (TESSLINE* outline1 = blob->outlines; outline1 != NULL;
outline1 = outline1->next) {
if (outline1->is_hole)
continue; // Holes do not count as separable.
TPOINT mid_pt1 = {(outline1->topleft.x + outline1->botright.x) / 2,
(outline1->topleft.y + outline1->botright.y) / 2};
int mid_prod1 = CROSS(mid_pt1, vertical);
int min_prod1, max_prod1;
outline1->MinMaxCrossProduct(vertical, &min_prod1, &max_prod1);
for (TESSLINE* outline2 = outline1->next; outline2 != NULL;
outline2 = outline2->next) {
if (outline2->is_hole)
continue; // Holes do not count as separable.
TPOINT mid_pt2 = { (outline2->topleft.x + outline2->botright.x) / 2,
(outline2->topleft.y + outline2->botright.y) / 2};
int mid_prod2 = CROSS(mid_pt2, vertical);
int min_prod2, max_prod2;
outline2->MinMaxCrossProduct(vertical, &min_prod2, &max_prod2);
int mid_gap = abs(mid_prod2 - mid_prod1);
int overlap = MIN(max_prod1, max_prod2) - MAX(min_prod1, min_prod2);
if (mid_gap - overlap / 2 > max_gap) {
max_gap = mid_gap - overlap / 2;
*location = mid_pt1;
*location += mid_pt2;
*location /= 2;
}
}
}
// Use the y component of the vertical vector as an approximation to its
// length.
return max_gap > vertical.y;
}
static void stri_normal(stri * trn, vector * pnt, ray * incident, vector * N) {
flt U, V, W, lensqr;
vector P, tmp, norm;
CROSS(norm, trn->edge1, trn->edge2);
lensqr = DOT(norm, norm);
VSUB((*pnt), trn->v0, P);
CROSS(tmp, P, trn->edge2);
U = DOT(tmp, norm) / lensqr;
CROSS(tmp, trn->edge1, P);
V = DOT(tmp, norm) / lensqr;
W = 1.0 - (U + V);
N->x = W*trn->n0.x + U*trn->n1.x + V*trn->n2.x;
N->y = W*trn->n0.y + U*trn->n1.y + V*trn->n2.y;
N->z = W*trn->n0.z + U*trn->n1.z + V*trn->n2.z;
VNorm(N);
}
/*---------------------------------------------------------------------------*/
int testDegenerateTriangle(trian t){
double v1[3],v2[3],v3[3],s1[3],s2[3],pv[3];
STORE(v1,t.v0);
STORE(v2,t.v1);
STORE(v3,t.v2);
SUB(s1,v2,v1);
SUB(s2,v3,v1);
CROSS(pv,s1,s2);
if((SIGNO(pv[0] == LIM)) &&(SIGNO(pv[1] == LIM)) &&(SIGNO(pv[2] == LIM)))
return 0;
return 1;
}
static void
adj_ang_mom(SSDATA *d,int n,PROPERTIES *p,VECTOR v)
{
VECTOR u,w;
int i;
invert(p->inertia);
SUB_VEC(v,p->ang_mom,v);
Transform(p->inertia,v,u);
SCALE_VEC(u,p->total_mass);
for (i=0;i<n;i++) {
CROSS(u,d[i].pos,w);
ADD_VEC(d[i].vel,w,d[i].vel);
}
}
static void tri_intersect(tri * trn, ray * ry) {
vector tvec, pvec, qvec;
flt det, inv_det, t, u, v;
/* begin calculating determinant - also used to calculate U parameter */
CROSS(pvec, ry->d, trn->edge2);
/* if determinant is near zero, ray lies in plane of triangle */
det = DOT(trn->edge1, pvec);
if (det > -EPSILON && det < EPSILON)
return;
inv_det = 1.0 / det;
/* calculate distance from vert0 to ray origin */
SUB(tvec, ry->o, trn->v0);
/* calculate U parameter and test bounds */
u = DOT(tvec, pvec) * inv_det;
if (u < 0.0 || u > 1.0)
return;
/* prepare to test V parameter */
CROSS(qvec, tvec, trn->edge1);
/* calculate V parameter and test bounds */
v = DOT(ry->d, qvec) * inv_det;
if (v < 0.0 || u + v > 1.0)
return;
/* calculate t, ray intersects triangle */
t = DOT(trn->edge2, qvec) * inv_det;
add_intersection(t,(object *) trn, ry);
}
void incr_facet(
REAL n1, REAL n2, REAL n3,
REAL i1, REAL i2, REAL i3,
REAL j1, REAL j2, REAL j3,
REAL k1, REAL k2, REAL k3){
REAL x[3] = {i1-j1,i2-j2,i3-j3};
REAL y[3] = {i1-k1,i2-k2,i3-k3};
REAL zz[3]; CROSS(x,y,zz);
REAL A = sqrt(NORM2(zz))/2;
REAL incr = 1./3. * (i1*n1+i2*n2+i3*n3)*A;
// REAL incr = (-1*k1*j2*i3 + j1*k2*i3 + k1*i2*j3 - i1*k2*j3 - j1*i2*k3 + i1*j2*k3)/6.;
vol+=incr;
}
void AseFile::ComputeTangentBinormal()
{
for(int i=0; i<this->objects.size(); i++)
{
if(!objects[i].numOfTexVerts || !objects[i].numOfFaces)continue;
vec* faceTangent;
zASE_Object *obj;
obj = &objects[i];
faceTangent = new vec[obj->numOfFaces]; if(faceTangent==NULL)return;
obj->pTangent = new vec[obj->numOfVerts]; if(obj->pTangent==NULL)return;
obj->pBinormal = new vec[obj->numOfVerts]; if(obj->pBinormal==NULL)return;
for(int j=0; j<obj->numOfFaces; j++)
{
tangent( obj->pVerts[obj->pFaces[j].index[0]],
obj->pVerts[obj->pFaces[j].index[1]],
obj->pVerts[obj->pFaces[j].index[2]],
obj->pTexVerts[obj->pFaceCoord[j].index[0]],
obj->pTexVerts[obj->pFaceCoord[j].index[1]],
obj->pTexVerts[obj->pFaceCoord[j].index[2]],
faceTangent[j] );
}
for(j=0; j<obj->numOfVerts; j++) // Go through all of the vertices
{
obj->pTangent[j].clear();
for(int k=0; k<obj->numOfFaces; k++) // Go through all of the triangles
{ // Check if the vertex is shared by another face
if( obj->pFaces[k].index[0]==j ||
obj->pFaces[k].index[1]==j ||
obj->pFaces[k].index[2]==j )
{
obj->pTangent[j] += faceTangent[k];
}
}
obj->pTangent[j].Normalize();
obj->pBinormal[j] = Normalize( CROSS( obj->pNormals[j], obj->pTangent[j] ));
}
if(faceTangent!=NULL)delete [] faceTangent;
}
}
int
gemIntegrate_bar(gemQuilt *quilt, /* (in) the quilt to integrate upon */
int geomFlag, /* (in) 0 - data ref, 1 - geom based */
int eIndex, /* (in) element index (bias 1) */
int rank, /* (in) data depth */
double res_bar[], /* (in) d(objective)/d(result)
(rank in length) */
double dat_bar[]) /* (both) d(objective)/d(data)
(rank*npts in len) */
{
int i, in[3];
double x1[3], x2[3], x3[3], area;
/* element indices */
in[0] = quilt->elems[eIndex-1].gIndices[0] - 1;
in[1] = quilt->elems[eIndex-1].gIndices[1] - 1;
in[2] = quilt->elems[eIndex-1].gIndices[2] - 1;
x1[0] = quilt->points[in[1]].xyz[0] - quilt->points[in[0]].xyz[0];
x2[0] = quilt->points[in[2]].xyz[0] - quilt->points[in[0]].xyz[0];
x1[1] = quilt->points[in[1]].xyz[1] - quilt->points[in[0]].xyz[1];
x2[1] = quilt->points[in[2]].xyz[1] - quilt->points[in[0]].xyz[1];
x1[2] = quilt->points[in[1]].xyz[2] - quilt->points[in[0]].xyz[2];
x2[2] = quilt->points[in[2]].xyz[2] - quilt->points[in[0]].xyz[2];
CROSS(x3, x1, x2);
area = sqrt(DOT(x3, x3))/2.0; /* 1/2 for area */
if (geomFlag == 0) {
in[0] = quilt->elems[eIndex-1].dIndices[0] - 1;
in[1] = quilt->elems[eIndex-1].dIndices[1] - 1;
in[2] = quilt->elems[eIndex-1].dIndices[2] - 1;
}
for (i = 0; i < rank; i++) {
/* result[i] = area*(data[rank*in[0]+i] + data[rank*in[1]+i] +
data[rank*in[2]+i])/3.0; */
dat_bar[rank*in[0]+i] += area*res_bar[i]/3.0;
dat_bar[rank*in[1]+i] += area*res_bar[i]/3.0;
dat_bar[rank*in[2]+i] += area*res_bar[i]/3.0;
}
return GEM_SUCCESS;
}
int
gemIntegration(gemQuilt *quilt, /* (in) the quilt to integrate upon */
int geomFlag, /* (in) 0 - data ref, 1 - geom based */
int eIndex, /* (in) element index (bias 1) */
int rank, /* (in) data depth */
double data[], /* (in) values (rank*npts in length) */
double result[]) /* (out) integrated result - (rank) */
{
int i, in[3];
double x1[3], x2[3], x3[3], area;
/* element indices */
in[0] = quilt->elems[eIndex-1].gIndices[0] - 1;
in[1] = quilt->elems[eIndex-1].gIndices[1] - 1;
in[2] = quilt->elems[eIndex-1].gIndices[2] - 1;
x1[0] = quilt->points[in[1]].xyz[0] - quilt->points[in[0]].xyz[0];
x2[0] = quilt->points[in[2]].xyz[0] - quilt->points[in[0]].xyz[0];
x1[1] = quilt->points[in[1]].xyz[1] - quilt->points[in[0]].xyz[1];
x2[1] = quilt->points[in[2]].xyz[1] - quilt->points[in[0]].xyz[1];
x1[2] = quilt->points[in[1]].xyz[2] - quilt->points[in[0]].xyz[2];
x2[2] = quilt->points[in[2]].xyz[2] - quilt->points[in[0]].xyz[2];
CROSS(x3, x1, x2);
area = sqrt(DOT(x3, x3))/2.0; /* 1/2 for area */
if (geomFlag == 0) {
in[0] = quilt->elems[eIndex-1].dIndices[0] - 1;
in[1] = quilt->elems[eIndex-1].dIndices[1] - 1;
in[2] = quilt->elems[eIndex-1].dIndices[2] - 1;
}
for (i = 0; i < rank; i++)
result[i] = area*(data[rank*in[0]+i] + data[rank*in[1]+i] +
data[rank*in[2]+i])/3.0;
return GEM_SUCCESS;
}
bool intersect_triangle(Point3F orig, Point3F dir,
Point3F vert0, Point3F vert1, Point3F vert2,
F32& t, F32& u, F32& v)
{
Point3F edge1, edge2, tvec, pvec, qvec;
F32 det,inv_det;
/* find vectors for two edges sharing vert0 */
edge1.x = vert1.x - vert0.x;
edge1.y = vert1.y - vert0.y;
edge1.z = vert1.z - vert0.z;
edge2.x = vert2.x - vert0.x;
edge2.y = vert2.y - vert0.y;
edge2.z = vert2.z - vert0.z;
/* begin calculating determinant - also used to calculate U parameter */
//CROSS(pvec, dir, edge2);
mCross(dir, edge2, &pvec);
/* if determinant is near zero, ray lies in plane of triangle */
//det = DOT(edge1, pvec);
det = mDot(edge1, pvec);
#ifdef TEST_CULL /* define TEST_CULL if culling is desired */
if (det < EPSILON)
return 0;
/* calculate distance from vert0 to ray origin */
SUB(tvec, orig, vert0);
/* calculate U parameter and test bounds */
*u = DOT(tvec, pvec);
if (*u < 0.0 || *u > det)
return 0;
/* prepare to test V parameter */
CROSS(qvec, tvec, edge1);
/* calculate V parameter and test bounds */
*v = DOT(dir, qvec);
if (*v < 0.0 || *u + *v > det)
return 0;
/* calculate t, scale parameters, ray intersects triangle */
*t = DOT(edge2, qvec);
inv_det = 1.0 / det;
*t *= inv_det;
*u *= inv_det;
*v *= inv_det;
#else /* the non-culling branch */
if (det > -EPSILON && det < EPSILON)
return false;
inv_det = 1.0 / det;
/* calculate distance from vert0 to ray origin */
//SUB(tvec, orig, vert0);
tvec.x = orig.x - vert0.x;
tvec.y = orig.y - vert0.y;
tvec.z = orig.z - vert0.z;
/* calculate U parameter and test bounds */
// *u = DOT(tvec, pvec) * inv_det;
u = mDot(tvec, pvec) * inv_det;
if (u < 0.0 || u > 1.0)
return false;
/* prepare to test V parameter */
//CROSS(qvec, tvec, edge1);
mCross(tvec, edge1, &qvec);
/* calculate V parameter and test bounds */
// *v = DOT(dir, qvec) * inv_det;
v = mDot(dir, qvec) * inv_det;
if (v < 0.0 || u + v > 1.0)
return false;
/* calculate t, ray intersects triangle */
// *t = DOT(edge2, qvec) * inv_det;
t = mDot(edge2, qvec) * inv_det;
#endif
return true;
}
void ArmContactModelProvider::update(ArmContactModel& model)
{
MODIFY("module:ArmContactModelProvider:parameters", p);
DECLARE_PLOT("module:ArmContactModelProvider:errorLeftX");
DECLARE_PLOT("module:ArmContactModelProvider:errorRightX");
DECLARE_PLOT("module:ArmContactModelProvider:errorLeftY");
DECLARE_PLOT("module:ArmContactModelProvider:errorRightY");
DECLARE_PLOT("module:ArmContactModelProvider:errorDurationLeft");
DECLARE_PLOT("module:ArmContactModelProvider:errorDurationRight");
DECLARE_PLOT("module:ArmContactModelProvider:errorYThreshold");
DECLARE_PLOT("module:ArmContactModelProvider:errorXThreshold");
DECLARE_PLOT("module:ArmContactModelProvider:contactLeft");
DECLARE_PLOT("module:ArmContactModelProvider:contactRight");
DECLARE_DEBUG_DRAWING("module:ArmContactModelProvider:armContact", "drawingOnField");
CIRCLE("module:ArmContactModelProvider:armContact", 0, 0, 200, 30, Drawings::ps_solid, ColorClasses::blue, Drawings::ps_null, ColorClasses::blue);
CIRCLE("module:ArmContactModelProvider:armContact", 0, 0, 400, 30, Drawings::ps_solid, ColorClasses::blue, Drawings::ps_null, ColorClasses::blue);
CIRCLE("module:ArmContactModelProvider:armContact", 0, 0, 600, 30, Drawings::ps_solid, ColorClasses::blue, Drawings::ps_null, ColorClasses::blue);
CIRCLE("module:ArmContactModelProvider:armContact", 0, 0, 800, 30, Drawings::ps_solid, ColorClasses::blue, Drawings::ps_null, ColorClasses::blue);
CIRCLE("module:ArmContactModelProvider:armContact", 0, 0, 1000, 30, Drawings::ps_solid, ColorClasses::blue, Drawings::ps_null, ColorClasses::blue);
CIRCLE("module:ArmContactModelProvider:armContact", 0, 0, 1200, 30, Drawings::ps_solid, ColorClasses::blue, Drawings::ps_null, ColorClasses::blue);
/* Buffer arm angles */
struct ArmAngles angles;
angles.leftX = theJointRequest.angles[JointData::LShoulderPitch];
angles.leftY = theJointRequest.angles[JointData::LShoulderRoll];
angles.rightX = theJointRequest.angles[JointData::RShoulderPitch];
angles.rightY = theJointRequest.angles[JointData::RShoulderRoll];
angleBuffer.add(angles);
/* Reset in case of a fall or penalty */
if(theFallDownState.state == FallDownState::onGround || theRobotInfo.penalty != PENALTY_NONE)
{
leftErrorBuffer.init();
rightErrorBuffer.init();
}
Pose2D odometryOffset = theOdometryData - lastOdometry;
lastOdometry = theOdometryData;
const Vector3<>& leftHandPos3D = theRobotModel.limbs[MassCalibration::foreArmLeft].translation;
Vector2<> leftHandPos(leftHandPos3D.x, leftHandPos3D.y);
Vector2<> leftHandSpeed = (odometryOffset + Pose2D(leftHandPos) - Pose2D(lastLeftHandPos)).translation / theFrameInfo.cycleTime;
float leftFactor = std::max(0.f, 1.f - leftHandSpeed.abs() / p.speedBasedErrorReduction);
lastLeftHandPos = leftHandPos;
const Vector3<>& rightHandPos3D = theRobotModel.limbs[MassCalibration::foreArmRight].translation;
Vector2<> rightHandPos(rightHandPos3D.x, rightHandPos3D.y);
Vector2<> rightHandSpeed = (odometryOffset + Pose2D(rightHandPos) - Pose2D(lastRightHandPos)).translation / theFrameInfo.cycleTime;
float rightFactor = std::max(0.f, 1.f - rightHandSpeed.abs() / p.speedBasedErrorReduction);
lastRightHandPos = rightHandPos;
/* Check for arm contact */
// motion types to take into account: stand, walk (if the robot is upright)
if((theMotionInfo.motion == MotionInfo::stand || theMotionInfo.motion == MotionInfo::walk) &&
(theFallDownState.state == FallDownState::upright || theFallDownState.state == FallDownState::staggering) &&
(theGameInfo.state == STATE_PLAYING || theGameInfo.state == STATE_READY) &&
(theRobotInfo.penalty == PENALTY_NONE)) // TICKET 897: ArmContact only if robot is not penalized
{
checkArm(LEFT, leftFactor);
checkArm(RIGHT, rightFactor);
//left and right are projections of the 3 dimensional shoulder-joint vector
//onto the x-y plane.
Vector2f left = leftErrorBuffer.getAverageFloat();
Vector2f right = rightErrorBuffer.getAverageFloat();
//Determine if we are being pushed or not
bool leftX = fabs(left.x) > fromDegrees(p.errorXThreshold);
bool leftY = fabs(left.y) > fromDegrees(p.errorYThreshold);
bool rightX = fabs(right.x)> fromDegrees(p.errorXThreshold);
bool rightY = fabs(right.y)> fromDegrees(p.errorYThreshold);
// update the model
model.contactLeft = leftX || leftY;
model.contactRight = rightX || rightY;
// The duration of the contact is counted upwards as long as the error
//remains. Otherwise it is reseted to 0.
model.durationLeft = model.contactLeft ? model.durationLeft + 1 : 0;
model.durationRight = model.contactRight ? model.durationRight + 1 : 0;
model.contactLeft &= model.durationLeft < p.malfunctionThreshold;
model.contactRight &= model.durationRight < p.malfunctionThreshold;
if(model.contactLeft)
{
model.timeOfLastContactLeft = theFrameInfo.time;
}
if(model.contactRight)
{
model.timeOfLastContactRight = theFrameInfo.time;
}
model.pushDirectionLeft = getDirection(LEFT, leftX, leftY, left);
model.pushDirectionRight = getDirection(RIGHT, rightX, rightY, right);
model.lastPushDirectionLeft = model.pushDirectionLeft != ArmContactModel::NONE ? model.pushDirectionLeft : model.lastPushDirectionLeft;
model.lastPushDirectionRight = model.pushDirectionRight != ArmContactModel::NONE ? model.pushDirectionRight : model.lastPushDirectionRight;
PLOT("module:ArmContactModelProvider:errorLeftX", toDegrees(left.x));
PLOT("module:ArmContactModelProvider:errorRightX", toDegrees(right.x));
PLOT("module:ArmContactModelProvider:errorLeftY", toDegrees(left.y));
PLOT("module:ArmContactModelProvider:errorRightY", toDegrees(right.y));
PLOT("module:ArmContactModelProvider:errorDurationLeft", model.durationLeft);
PLOT("module:ArmContactModelProvider:errorDurationRight", model.durationRight);
PLOT("module:ArmContactModelProvider:errorYThreshold", toDegrees(p.errorYThreshold));
PLOT("module:ArmContactModelProvider:errorXThreshold", toDegrees(p.errorXThreshold));
PLOT("module:ArmContactModelProvider:contactLeft", model.contactLeft ? 10.0 : 0.0);
PLOT("module:ArmContactModelProvider:contactRight", model.contactRight ? 10.0 : 0.0);
ARROW("module:ArmContactModelProvider:armContact", 0, 0, -(toDegrees(left.y) * SCALE), toDegrees(left.x) * SCALE, 20, Drawings::ps_solid, ColorClasses::green);
ARROW("module:ArmContactModelProvider:armContact", 0, 0, toDegrees(right.y) * SCALE, toDegrees(right.x) * SCALE, 20, Drawings::ps_solid, ColorClasses::red);
COMPLEX_DRAWING("module:ArmContactModelProvider:armContact",
{
DRAWTEXT("module:ArmContactModelProvider:armContact", -2300, 1300, 20, ColorClasses::black, "LEFT");
DRAWTEXT("module:ArmContactModelProvider:armContact", -2300, 1100, 20, ColorClasses::black, "ErrorX: " << toDegrees(left.x));
DRAWTEXT("module:ArmContactModelProvider:armContact", -2300, 900, 20, ColorClasses::black, "ErrorY: " << toDegrees(left.y));
DRAWTEXT("module:ArmContactModelProvider:armContact", -2300, 500, 20, ColorClasses::black, ArmContactModel::getName(model.pushDirectionLeft));
DRAWTEXT("module:ArmContactModelProvider:armContact", -2300, 300, 20, ColorClasses::black, "Time: " << model.timeOfLastContactLeft);
DRAWTEXT("module:ArmContactModelProvider:armContact", 1300, 1300, 20, ColorClasses::black, "RIGHT");
DRAWTEXT("module:ArmContactModelProvider:armContact", 1300, 1100, 20, ColorClasses::black, "ErrorX: " << toDegrees(right.x));
DRAWTEXT("module:ArmContactModelProvider:armContact", 1300, 900, 20, ColorClasses::black, "ErrorY: " << toDegrees(right.y));
DRAWTEXT("module:ArmContactModelProvider:armContact", 1300, 500, 20, ColorClasses::black, ArmContactModel::getName(model.pushDirectionRight));
DRAWTEXT("module:ArmContactModelProvider:armContact", 1300, 300, 20, ColorClasses::black, "Time: " << model.timeOfLastContactRight);
if (model.contactLeft)
{
CROSS("module:ArmContactModelProvider:armContact", -2000, 0, 100, 20, Drawings::ps_solid, ColorClasses::red);
}
if (model.contactRight)
{
CROSS("module:ArmContactModelProvider:armContact", 2000, 0, 100, 20, Drawings::ps_solid, ColorClasses::red);
}
});