VECTOR4D VECTOR4D::GetRotatedZ(double angle) const
{
VECTOR3D v3d(x, y, z);
v3d.RotateZ(angle);
return VECTOR4D(v3d.x, v3d.y, v3d.z, w);
}
VECTOR4D VECTOR4D::GetRotatedAxis(double angle, const VECTOR3D & axis) const
{
VECTOR3D v3d(x, y, z);
v3d.RotateAxis(angle, axis);
return VECTOR4D(v3d.x, v3d.y, v3d.z, w);
}
TEST(Vector3, Addition) {
float fv[3] = { 1.0f, 2.0f, 3.0f };
FasTC::Vec3f v3f (fv);
double dv[3] = { 4.3, -10.2, 0.0f };
FasTC::Vec3d v3d (dv);
EXPECT_NEAR((v3f + v3d).X(), 5.3, kEpsilon);
EXPECT_NEAR((v3f + v3d).Y(), -8.2, kEpsilon);
EXPECT_NEAR((v3f + v3d).Z(), 3.0, kEpsilon);
}
TEST(Vector4, Addition) {
float fv[4] = { 1.0f, 2.0f, 3.0f, 4.0f };
FasTC::Vec4f v4f (fv);
double dv[4] = { 4.3, -10.2, 0.0f, -22.0f };
FasTC::Vec4d v3d (dv);
EXPECT_NEAR((v4f + v3d).X(), 5.3, kEpsilon);
EXPECT_NEAR((v4f + v3d).Y(), -8.2, kEpsilon);
EXPECT_NEAR((v4f + v3d).Z(), 3.0, kEpsilon);
EXPECT_NEAR((v4f + v3d).W(), -18.0, kEpsilon);
}
TEST(VectorBase, CastVector) {
FasTC::VectorBase<float, 3> v3f;
v3f[0] = 1000000.0f;
v3f[1] = -2.0f;
v3f[2] = -1.1f;
FasTC::VectorBase<double, 3> v3d = v3f;
FasTC::VectorBase<int, 3> v3i = v3f;
for(int i = 0; i < 3; i++) {
EXPECT_EQ(v3d(i), static_cast<double>(v3f(i)));
EXPECT_EQ(v3i(i), static_cast<int>(v3f(i)));
}
}
TEST(MatrixBase, CastVector) {
srand(static_cast<unsigned>(time(NULL)));
FasTC::MatrixBase<float, 3, 2> v3f;
for(int i = 0; i < 6; i++) {
v3f[i] = static_cast<float>(rand());
}
FasTC::MatrixBase<double, 3, 2> v3d = v3f;
FasTC::MatrixBase<int, 3, 2> v3i = v3f;
for(int i = 0; i < 3*2; i++) {
EXPECT_EQ(v3d(i), static_cast<double>(v3f(i)));
EXPECT_EQ(v3i(i), static_cast<int>(v3f(i)));
}
}
bool CLevelGraph::create_straight_path(u32 start_vertex_id, const Fvector2 &start_point, const Fvector2 &finish_point, xr_vector<Fvector> &tpaOutputPoints, xr_vector<u32> &tpaOutputNodes, bool bAddFirstPoint, bool bClearPath) const
{
if (!valid_vertex_position(v3d(finish_point)))
return (false);
u32 cur_vertex_id = start_vertex_id, prev_vertex_id = start_vertex_id;
Fbox2 box;
Fvector2 identity, start, dest, dir;
identity.x = identity.y = header().cell_size()*.5f;
start = start_point;
dest = finish_point;
dir.sub (dest,start);
u32 dest_xz = vertex_position(v3d(dest)).xz();
Fvector2 temp;
Fvector pos3d;
unpack_xz (vertex(start_vertex_id),temp.x,temp.y);
if (bClearPath) {
tpaOutputPoints.clear ();
tpaOutputNodes.clear ();
}
if (bAddFirstPoint) {
pos3d = v3d(start_point);
pos3d.y = vertex_plane_y(start_vertex_id,start_point.x,start_point.y);
tpaOutputPoints.push_back(pos3d);
tpaOutputNodes.push_back(start_vertex_id);
}
float cur_sqr = _sqr(temp.x - dest.x) + _sqr(temp.y - dest.y);
for (;;) {
const_iterator I,E;
begin (cur_vertex_id,I,E);
bool found = false;
for ( ; I != E; ++I) {
u32 next_vertex_id = value(cur_vertex_id,I);
if ((next_vertex_id == prev_vertex_id) || !valid_vertex_id(next_vertex_id))
continue;
CVertex *v = vertex(next_vertex_id);
unpack_xz (v,temp.x,temp.y);
box.min = box.max = temp;
box.grow (identity);
if (box.pick_exact(start,dir)) {
Fvector2 temp;
temp.add (box.min,box.max);
temp.mul (.5f);
float dist = _sqr(temp.x - dest.x) + _sqr(temp.y - dest.y);
if (dist > cur_sqr)
continue;
Fvector2 next1, next2;
#ifdef DEBUG
next1 = next2 = Fvector2().set(0.f,0.f);
#endif
Fvector tIntersectPoint;
switch (I) {
case 0 : {
next1 = box.max;
next2.set (box.max.x,box.min.y);
break;
}
case 1 : {
next1 = box.min;
next2.set (box.max.x,box.min.y);
break;
}
case 2 : {
next1 = box.min;
next2.set (box.min.x,box.max.y);
break;
}
case 3 : {
next1 = box.max;
next2.set (box.min.x,box.max.y);
break;
}
default : NODEFAULT;
}
VERIFY (_valid(next1));
VERIFY (_valid(next2));
u32 dwIntersect = intersect(start_point.x,start_point.y,finish_point.x,finish_point.y,next1.x,next1.y,next2.x,next2.y,&tIntersectPoint.x,&tIntersectPoint.z);
if (!dwIntersect)
continue;
tIntersectPoint.y = vertex_plane_y(vertex(cur_vertex_id),tIntersectPoint.x,tIntersectPoint.z);
tpaOutputPoints.push_back(tIntersectPoint);
tpaOutputNodes.push_back(cur_vertex_id);
if (dest_xz == v->position().xz())
return (true);
cur_sqr = dist;
found = true;
prev_vertex_id = cur_vertex_id;
cur_vertex_id = next_vertex_id;
break;
}
}
if (!found)
return (false);
}
}
float DistanceSensor::calcDistance(tTrack *track, tCarElt* car)
{
float distance = 200.0f;
v2t<float> rotatedDirection = v2t<float>(this->m_direction.x * cos(car->pub.DynGCg.pos.az) - this->m_direction.y * sin(car->pub.DynGCg.pos.az), this->m_direction.x * sin(car->pub.DynGCg.pos.az) + this->m_direction.y * cos(car->pub.DynGCg.pos.az));
v2t<float> collisionPoint;
v2t<float> sensorStart = v2t<float>(car->pub.DynGCg.pos.x, car->pub.DynGCg.pos.y);
v2t<float> sensorEnd = v2t<float>(car->pub.DynGCg.pos.x, car->pub.DynGCg.pos.y) + rotatedDirection;
v2t<float> borderStart;
v2t<float> borderEnd;
float denominator;
car->pub.posMat;
printf_s("Rotated direction: %f %f\n", rotatedDirection.x, rotatedDirection.y);
for(int i = 0; i < track->nseg; i++)
{
//Left side
borderStart = v2t<float>(track->seg[i].vertex[TR_SL].x, track->seg[i].vertex[TR_SL].y);
borderEnd = v2t<float>(track->seg[i].vertex[TR_EL].x, track->seg[i].vertex[TR_EL].y);
denominator = (sensorStart.x - sensorEnd.x) * (borderStart.y - borderEnd.y) - (sensorStart.y - sensorEnd.y) * (borderStart.x - borderEnd.x);
if(denominator != 0)
{
collisionPoint.x = ((sensorStart.x * sensorEnd.y - sensorStart.y * sensorEnd.x) * (borderStart.x - borderEnd.x) - (borderStart.x * borderEnd.y - borderStart.y * borderEnd.x) * (sensorStart.x - sensorEnd.x)) / denominator;
collisionPoint.y = ((sensorStart.x * sensorEnd.y - sensorStart.y * sensorEnd.x) * (borderStart.y - borderEnd.y) - (borderStart.x * borderEnd.y - borderStart.y * borderEnd.x) * (sensorStart.y - sensorEnd.y)) / denominator;
//Check if the collision point is negative to the direction
if(collisionPoint.x / this->m_direction.x > 0.0f)
{
//Check if the collision point is within the vector
if(collisionPoint.x - borderStart.x > 0.0f && collisionPoint.x - borderStart.x < borderEnd.x && collisionPoint.y - borderStart.y > 0.0f && collisionPoint.y - borderStart.y < borderEnd.y)
{
//Update the shortest distance
distance = min(v3d(collisionPoint.x - borderStart.x, collisionPoint.y - borderStart.y, 0.0f).len(), distance);
}
}
}
//Right side
borderStart = v2t<float>(track->seg[i].vertex[TR_SR].x, track->seg[i].vertex[TR_SR].y);
borderEnd = v2t<float>(track->seg[i].vertex[TR_ER].x, track->seg[i].vertex[TR_ER].y);
denominator = (sensorStart.x - sensorEnd.x) * (borderStart.y - borderEnd.y) - (sensorStart.y - sensorEnd.y) * (borderStart.x - borderEnd.x);
if(denominator != 0)
{
collisionPoint.x = ((sensorStart.x * sensorEnd.y - sensorStart.y * sensorEnd.x) * (borderStart.x - borderEnd.x) - (borderStart.x * borderEnd.y - borderStart.y * borderEnd.x) * (sensorStart.x - sensorEnd.x)) / denominator;
collisionPoint.y = ((sensorStart.x * sensorEnd.y - sensorStart.y * sensorEnd.x) * (borderStart.y - borderEnd.y) - (borderStart.x * borderEnd.y - borderStart.y * borderEnd.x) * (sensorStart.y - sensorEnd.y)) / denominator;
//Check if the collision point is negative to the direction
if(collisionPoint.x / this->m_direction.x > 0.0f)
{
//Check if the collision point is within the vector
if(collisionPoint.x > borderStart.x && collisionPoint.x < borderEnd.x && collisionPoint.y > borderStart.y && collisionPoint.y < borderEnd.y)
{
//Update the shortest distance
distance = min(v3d(collisionPoint.x - borderStart.x, collisionPoint.y - borderStart.y, 0.0f).len(), distance);
}
}
}
}
return distance;
}
