// *****************************************************************************
Transform dg::LookAt(const Vector &rkPos, const Vector &rkLook, const Vector &rkUp)
{
Real afM[4][4];
// initialize fourth column of viewing matrix
afM[0][3] = rkPos.x();
afM[1][3] = rkPos.y();
afM[2][3] = rkPos.z();
afM[3][3] = 1;
// initialize first three columns of viewing matrix
Vector kDir = Normalize(rkLook - rkPos);
Vector kRight = Cross(kDir, Normalize(rkUp));
Vector kNewUp = Cross(kRight, kDir);
afM[0][0] = kRight.x();
afM[1][0] = kRight.y();
afM[2][0] = kRight.z();
afM[3][0] = 0.;
afM[0][1] = kNewUp.x();
afM[1][1] = kNewUp.y();
afM[2][1] = kNewUp.z();
afM[3][1] = 0.;
afM[0][2] = kDir.x();
afM[1][2] = kDir.y();
afM[2][2] = kDir.z();
afM[3][2] = 0.;
// camera to world
Matrix *pkMatrix = new Matrix(afM);
// world to camera
Matrix *pkInverse = new Matrix( pkMatrix->inverse() );
return Transform(pkInverse, pkMatrix, true);
}
bool geometry::clockwise(const Point& a, const Point& b, const Point& c)
{
Vector ab = b-a;
Vector bc = c-b;
return ab.x()*bc.y()-ab.y()*bc.x()<0;
}
bool geometry::pointsOnLine(const Point& a, const Point& b, const Point& c)
{
Vector ab = b-a;
Vector bc = c-b;
return ab.x()*bc.y()-ab.y()*bc.x()==0;
}
void LookupGrid::expandGrid(Vector &v)
{
if(v.x() > xhi) xhi=v.x();
if(v.x() < xlo) xlo=v.x();
if(v.z() > zhi) zhi=v.z();
if(v.z() < zlo) zlo=v.z();
}
// Technique borrowed from http://slabode.exofire.net/circle_draw.shtml
void draw_circle( float radius, unsigned int nVerteces )
{
const float ROTATION = (2 * 3.145) / nVerteces; // Change in theta.
const float TANGENTAL_FACTOR = std::tan( ROTATION );
const float RADIAL_FACTOR = std::cos( ROTATION );
Vector<float,2> rv; // Radial vector.
rv.x( radius ); rv.y( 0.0f );
std::vector< GLfloat > verteces;
verteces.reserve( nVerteces );
verteces.insert( verteces.end(), rv.begin(), rv.end() );
// Since a circle can be drawn using GL_POLYGON, there is no need to draw
// the last vertex which is the same as the first. Therefore, nVerteces-1
// iterations is fine.
for( unsigned int i=0; i < nVerteces-1; i++ )
{
Vector<float,2> tv; // Tangential vector.
tv.x( -rv.y() ); tv.y( rv.x() );
rv += tv * TANGENTAL_FACTOR;
rv *= RADIAL_FACTOR;
verteces.insert( verteces.end(), rv.begin(), rv.end() );
}
using namespace glpp;
enableClientState( GL_VERTEX_ARRAY );
vertexPointer( 2, 0, verteces.data() );
drawArrays( GL_POLYGON, 0, nVerteces );
disableClientState( GL_VERTEX_ARRAY );
}
Vector Vector::operator ^(Vector v) const
//Producto vectorial
{
Vector pv= Vector( y()*v.z()-z()*v.y(), z()*v.x()-x()*v.z(), x()*v.y()-y()*v.x());
for(int i=0;i<3;i++) if(fabs(pv.elem(i))<EPSILON) pv.setelem(i,0);
return pv;
}
inline Vector<T, Size> cross (const Vector<T, Size>& a, const Vector<T, Size>& b)
{
DE_STATIC_ASSERT(Size == 3);
return Vector<T, Size>(
a.y() * b.z() - b.y() * a.z(),
a.z() * b.x() - b.z() * a.x(),
a.x() * b.y() - b.x() * a.y());
}
/**
* Constructs a Ray out of the compontent Vectors.
*
* @param U The direction that the ray is traveling
* @param L The origin of the Ray
*/
Ray::Ray(const Vector& L, const Vector& U, const Surface* source):
_L(L),
_U(U),
_iU(1.0 / U.x(), 1.0 / U.y(), 1.0 / U.z()),
_iL(1.0 / L.x(), 1.0 / L.y(), 1.0 / L.z()),
_positive({U.x() > 0.0, U.y() > 0.0, U.z() > 0.0}),
_nonzero ({U.x() != 0.0, U.y() != 0.0, U.z() != 0.0}),
_source(source) { }
void ARCBuilder::add_line_segment(Vector p1, Vector p2, Vector extrusion,
Double thickness)
{
*out << "<line>"
<< "<point x=\"" << p1.x() << "\" y=\"" << p1.y() << "\" />"
<< "<point x=\"" << p2.x() << "\" y=\"" << p2.y() << "\" />"
<< "</line>";
}
void ARCBuilder::add_ellipse(Vector center, Vector a, Vector b)
{
*out << "<ellipse>"
<< "<point x=\"" << center.x() << "\" y=\"" << center.y() << "\"/>"
<< "<point x=\"" << a.x() << "\" y=\"" << a.y() << "\"/>"
<< "<point x=\"" << b.x() << "\" y=\"" << b.y() << "\"/>"
<< "</ellipse>";
}
HDINLINE void operator()(Cursor& cursor, Vector& vector, const float3_X & fieldPos)
{
if (speciesParticleShape::ParticleShape::support % 2 == 0)
{
//even support
/* for any direction
* if fieldPos == 0.5 and vector<0.5
* shift curser+(-1) and new_vector=old_vector-(-1)
*
* (vector.x() < fieldPos.x()) is equal ((fieldPos == 0.5) && (vector<0.5))
*/
float3_X coordinate_shift(
-float_X(vector.x() < fieldPos.x()),
-float_X(vector.y() < fieldPos.y()),
-float_X(vector.z() < fieldPos.z())
);
cursor = cursor(
PMacc::math::Int < 3 > (
coordinate_shift.x(),
coordinate_shift.y(),
coordinate_shift.z()
));
//same as: vector = vector - fieldPos - coordinate_shift;
vector -= (fieldPos + coordinate_shift);
}
else
{
//odd support
/* for any direction
* if fieldPos < 0.5 and vector> 0.5
* shift curser+1 and new_vector=old_vector-1
*/
float3_X coordinate_shift(
float_X(vector.x() > float_X(0.5) && fieldPos.x() != float_X(0.5)),
float_X(vector.y() > float_X(0.5) && fieldPos.y() != float_X(0.5)),
float_X(vector.z() > float_X(0.5) && fieldPos.z() != float_X(0.5))
);
cursor = cursor(
PMacc::math::Int < 3 > (
coordinate_shift.x(),
coordinate_shift.y(),
coordinate_shift.z()
));
//same as: vector = vector - fieldPos - coordinate_shift;
vector -= (fieldPos + coordinate_shift);
}
}
Plane::Plane(const Vector& nor, const Point& p) {
Vector n = nor.normalized();
float d = -(p.x()*n.x() + p.y()*n.y() + p.z()*n.z());
m_equation[0] = n.x();
m_equation[1] = n.y();
m_equation[2] = n.z();
m_equation[3] = d;
}
Vector transformVec(const Matrix& left, const Vector& right)
{
double x = (right.x() * left.x[0][0] + right.y() * left.x[0][1] + right.z() * left.x[0][2]);
double y = (right.x() * left.x[1][0] + right.y() * left.x[1][1] + right.z() * left.x[1][2]);
double z = (right.x() * left.x[2][0] + right.y() * left.x[2][1] + right.z() * left.x[2][2]);
Vector ret(x, y, z);
return ret;
}
//******************************************************************************
Matrix Matrix::buildRotation( const Vector & rkFrom, const Vector & rkTo )
{
// compute dot product between From and To
Real fCosAngle = rkFrom.dot( rkTo );
Real fEpsilon = 10e-4;
if ( Abs( fCosAngle - 1 ) < fEpsilon ) /// dot product close to 1.0
{
// rotate by an angle of zero with an arbitrary direction
return buildRotation( 0.0, 1 ); // use y axis
}
else
if ( Abs( fCosAngle + 1.0 ) < fEpsilon ) // dot product close to 0.0
{
// find an orthogonal direction to create axis
Vector kOrtho;
if ( Abs( rkFrom.x() ) < Abs( rkFrom.y() ) )
{
if ( Abs( rkFrom.x() ) < Abs( rkFrom.z() ) )
{
kOrtho = Vector( 1.0, 0.0, 0.0 ); // use x axis.
}
else
{
kOrtho = Vector( 0.0, 0.0, 1.0 ); // use z axis
}
}
else
if ( Abs( rkFrom.y() ) < Abs( rkFrom.z() ) )
{
kOrtho = Vector( 0.0, 1.0, 0.0 ); // use y axis
}
else
{
kOrtho = Vector( 0.0, 0.0, 1.0 ); // use z axis
}
// find orthogonal axis to use for rotation
Vector kAxis( rkFrom.cross( kOrtho ) ); // Axis = From x Ortho (cross prod)
kAxis.normalize();
Real fAngle = ArcCosine( fCosAngle ); // find rotation angle
// create new rotation matrix about new axis
return buildRotation( fAngle, kAxis );
}
else
{
// find orthogonal axis to use for rotation
Vector kAxis( rkFrom.cross( rkTo ) ); // Axis = From x To (cross prod)
Real fAngle = ArcCosine( fCosAngle ); // find rotation angle
// create new rotation matrix about new axis
return buildRotation( fAngle, kAxis );
}
}
void ChainTask::model_update(){
Vector p = pose.p;
chi_f_baker(0)=atan2(p.y(),p.x());
chi_f_baker(1)=sqrt(p.x()*p.x()+p.y()*p.y());
chi_f_baker(2)=p.z();
Frame baker_pose;
fksolver_baker->JntToCart(chi_f_baker,baker_pose);
Rotation rot = (baker_pose.Inverse()*pose).M.Inverse();
if(new_rotation)
derive_angles(Frame(rot));
else
RPY_angles(rot);
geometry_msgs::PoseStamped pp;
// stupid hack for rviz (running locally, wtf?!)
pp.header.stamp = ros::Time::now() - ros::Duration(0.1);
pp.header.frame_id = "/pancake";
tf::PoseKDLToMsg(baker_pose, pp.pose);
ros_chain_pose_port.write(pp);
tf::PoseKDLToMsg(pose, pp.pose);
ros_o1o2_pose_port.write(pp);
/*
Frame spatula_pose;
fksolver_spatula->JntToCart(chi_f_spatula,spatula_pose);
if(baker_pose*spatula_pose.Inverse()!=pose)
log(Warning)<<"model update failed for some reason :("<<endlog();
*/
//reference frame is baker, reference point is end of baker chain
jacsolver_baker->JntToJac(chi_f_baker,Jf_baker);
//reference frame and reference point should be baker root
changeRefPoint(Jf_baker,-baker_pose.p,Jf_baker);
changeRefFrame(Jf_spatula,Frame(pose.M, pose.p),Jf_spatula);
for(unsigned int i=0;i<NC;i++)
if(i<N_CHIF_BAKER)
Jf_total.setColumn(i,Jf_baker.getColumn(i));
else
Jf_total.setColumn(i,Jf_spatula.getColumn(i-N_CHIF_BAKER));
// concatenate the two chi_f
for(unsigned int i=0; i < 3; i++)
chi_f(i) = chi_f_baker(i);
for(unsigned int i=3; i < 6; i++)
chi_f(i) = chi_f_spatula(i-3);
}
int Plane::intersect(const Vector &v1, const Vector &v2, Vector *vx) const
{
// based on Graphics Gems III, partition3d
int sign1, sign2;
double c1 = classify(v1);
if (c1 < -TOLERANCE)
sign1 = -1;
else if (c1 > TOLERANCE)
sign1 = 1;
else // no intersection; cases 1, 6, 7
return 0;
double c2 = classify(v2);
if (c2 < -TOLERANCE)
sign2 = -1;
else if (c2 > TOLERANCE)
sign2 = 1;
else { // intersection at v2; cases 8, 9
*vx = v2;
return sign1;
}
if (sign1 == sign2) // no intersection; cases 2, 3
return 0;
// intersection; cases 4, 5
/*
Vector vd = v2 - v1;
double tt = - (double(_abc * v1) + _d) / double(_abc * vd);
*vx = v1 + (vd * tt);
*/
double vd_x = (double)v2.x() - (double)v1.x();
double vd_y = (double)v2.y() - (double)v1.y();
double vd_z = (double)v2.z() - (double)v1.z();
double abc_v1_x = (double)_a * (double)v1.x();
double abc_v1_y = (double)_b * (double)v1.y();
double abc_v1_z = (double)_c * (double)v1.z();
double abc_v1 = abc_v1_x + abc_v1_y + abc_v1_z;
double abc_vd_x = (double)_a * vd_x;
double abc_vd_y = (double)_b * vd_y;
double abc_vd_z = (double)_c * vd_z;
double abc_vd = abc_vd_x + abc_vd_y + abc_vd_z;
double ttt = - (((double)_d + abc_v1) / abc_vd);
double vd_ttt_x = vd_x * ttt;
double vd_ttt_y = vd_y * ttt;
double vd_ttt_z = vd_z * ttt;
double vx_x = (double)v1.x() + vd_ttt_x;
double vx_y = (double)v1.y() + vd_ttt_y;
double vx_z = (double)v1.z() + vd_ttt_z;
*vx = Vector((float)vx_x, (float)vx_y, (float)vx_z);
return sign1;
}
//******************************************************************************
Matrix::Matrix(const Vector &rkW, const Vector &rkV, const Vector &rkU)
{
m_afV[0][0] = rkW.x(); m_afV[0][1] = rkW.y(); m_afV[0][2] = rkW.z();
m_afV[1][0] = rkV.x(); m_afV[1][1] = rkV.y(); m_afV[1][2] = rkV.z();
m_afV[2][0] = rkU.x(); m_afV[2][1] = rkU.y(); m_afV[2][2] = rkU.z();
m_afV[0][3] = m_afV[1][3] = m_afV[2][3] = 0;
m_afV[3][0] = m_afV[3][1] = m_afV[3][2] = 0;
m_afV[3][3] = 1.0;
}
void DrawObject::setScale(const Vector &scale) {
scaleTransform_->setMatrix(osg::Matrix::scale(
scale.x(), scale.y(), scale.z()));
posTransform_->setPivotPoint(osg::Vec3(
pivot_.x()*scale.x(), pivot_.y()*scale.y(), pivot_.z()*scale.z()));
scaledSize_ = Vector(
scale.x() * geometrySize_.x(),
scale.y() * geometrySize_.y(),
scale.z() * geometrySize_.z());
}
// *****************************************************************************
Transform dg::Rotate(Real fDegrees, const Vector &rkAxis)
{
Vector kA = Normalize(rkAxis);
Real fSin = Sine(Radians(fDegrees));
Real fCos = Cosine(Radians(fDegrees));
Real afM[4][4];
afM[0][0] = kA.x() * kA.x() + (1.f - kA.x() * kA.x()) * fCos;
afM[0][1] = kA.x() * kA.y() * (1.f - fCos) - kA.z() * fSin;
afM[0][2] = kA.x() * kA.z() * (1.f - fCos) + kA.y() * fSin;
afM[0][3] = 0;
afM[1][0] = kA.x() * kA.y() * (1.f - fCos) + kA.z() * fSin;
afM[1][1] = kA.y() * kA.y() + (1.f - kA.y() * kA.y()) * fCos;
afM[1][2] = kA.y() * kA.z() * (1.f - fCos) - kA.x() * fSin;
afM[1][3] = 0;
afM[2][0] = kA.x() * kA.z() * (1.f - fCos) - kA.y() * fSin;
afM[2][1] = kA.y() * kA.z() * (1.f - fCos) + kA.x() * fSin;
afM[2][2] = kA.z() * kA.z() + (1.f - kA.z() * kA.z()) * fCos;
afM[2][3] = 0;
afM[3][0] = 0;
afM[3][1] = 0;
afM[3][2] = 0;
afM[3][3] = 1;
Matrix *pkMatrix = new Matrix(afM);
Matrix *pkInverse = new Matrix( pkMatrix->transpose() );
return Transform(pkMatrix, pkInverse, true);
}
Vector Vector::crossProduct(Vector const& vector)
{
/* Vectors are parallel, thats undefined. */
assert(!isParallelWith(vector));
double resultX = y()*vector.z() - z()*vector.y(),
resultY = z()*vector.x() - x()*vector.z(),
resultZ = x()*vector.y() - y()*vector.x();
return Vector(resultX, resultY, resultZ);
}
bool Line::intersectionCoefficient(const Line& line, real& coefficient) const
{
Vector u = _direction;
Vector v = line._direction;
Vector w = _startPoint-line._startPoint;
real denominator = v.x()*u.y()-v.y()*u.x();
if (denominator==0) {
return false;
}
coefficient= (v.y()*w.x()-v.x()*w.y())/denominator;
return true;
}
__device__ __host__ d_Ray(Vector L, Vector U, int32_t src) :
L(L), U(U),
iL(1.0 / L.x(), 1.0 / L.y(), 1.0 / L.z()),
iU(1.0 / U.x(), 1.0 / U.y(), 1.0 / U.z()),
positive(), nonzero(), src(src) {
positive[0] = U.x() > 0;
positive[1] = U.y() > 0;
positive[2] = U.z() > 0;
nonzero [0] = L.x() != 0;
nonzero [1] = L.y() != 0;
nonzero [2] = L.z() != 0;
}
void BoundingBox::vminmax(
const Vector &v1, const Vector &v2,
Vector *vmin, Vector *vmax)
{
float xmin, ymin, zmin;
float xmax, ymax, zmax;
fminmax(v1.x(), v2.x(), &xmin, &xmax);
fminmax(v1.y(), v2.y(), &ymin, &ymax);
fminmax(v1.z(), v2.z(), &zmin, &zmax);
*vmin = Vector(xmin, ymin, zmin);
*vmax = Vector(xmax, ymax, zmax);
}
void TestVector::testNormalized2()
{
Vector v(1,1,1);
Vector norm = v.normalized();
Vector min(0.576,0.576,0.576);
Vector max(0.578,0.578,0.578);
QVERIFY((norm.x() > min.x()) && (norm.x() < max.x()));
QVERIFY((norm.y() > min.y()) && (norm.y() < max.y()));
QVERIFY((norm.z() > min.z()) && (norm.z() < max.z()));
}
//******************************************************************************
Matrix Matrix::buildTransform( const Vector & rkT, const Matrix & rkR, const Vector & rkS )
{
Matrix kT(
rkS.x() * rkR.m_afV[ 0 ] [ 0 ], rkS.y() * rkR.m_afV[ 0 ] [ 1 ],
rkS.z() * rkR.m_afV[ 0 ] [ 2 ], 0.0f,
rkS.x() * rkR.m_afV[ 1 ] [ 0 ], rkS.y() * rkR.m_afV[ 1 ] [ 1 ],
rkS.z() * rkR.m_afV[ 1 ] [ 2 ], 0.0f,
rkS.x() * rkR.m_afV[ 2 ] [ 0 ], rkS.y() * rkR.m_afV[ 2 ] [ 1 ],
rkS.z() * rkR.m_afV[ 2 ] [ 2 ], 0.0f,
rkT.x(), rkT.y(), rkT.z(), 1.0f );
return kT;
}
void inv(Field<tensor>& tf, const UList<tensor>& tf1)
{
if (tf.empty())
{
return;
}
scalar scale = magSqr(tf1[0]);
Vector<bool> removeCmpts
(
magSqr(tf1[0].xx())/scale < SMALL,
magSqr(tf1[0].yy())/scale < SMALL,
magSqr(tf1[0].zz())/scale < SMALL
);
if (removeCmpts.x() || removeCmpts.y() || removeCmpts.z())
{
tensorField tf1Plus(tf1);
if (removeCmpts.x())
{
tf1Plus += tensor(1,0,0,0,0,0,0,0,0);
}
if (removeCmpts.y())
{
tf1Plus += tensor(0,0,0,0,1,0,0,0,0);
}
if (removeCmpts.z())
{
tf1Plus += tensor(0,0,0,0,0,0,0,0,1);
}
TFOR_ALL_F_OP_FUNC_F(tensor, tf, =, inv, tensor, tf1Plus)
if (removeCmpts.x())
{
tf -= tensor(1,0,0,0,0,0,0,0,0);
}
if (removeCmpts.y())
{
tf -= tensor(0,0,0,0,1,0,0,0,0);
}
if (removeCmpts.z())
{
tf -= tensor(0,0,0,0,0,0,0,0,1);
}
}
Vector operator*(const Matrix& left, const Vector& right)
{
double temp;
double x = (right.x() * left.x[0][0] + right.y() * left.x[0][1] + right.z() * left.x[0][2] + left.x[0][3]);
double y = (right.x() * left.x[1][0] + right.y() * left.x[1][1] + right.z() * left.x[1][2] + left.x[1][3]);
double z = (right.x() * left.x[2][0] + right.y() * left.x[2][1] + right.z() * left.x[2][2] + left.x[2][3]);
temp = right.x() * left.x[3][0] + right.y() * left.x[3][1] + right.z() * left.x[3][2] + left.x[3][3];
Vector ret = Vector(x,y,z);
ret /= temp;
return ret;
}
Plane::Plane(const Point& p1, const Point& p2, const Point& p3) {
Vector u(p2, p1);
u.normalize();
Vector v(p3, p1);
v.normalize();
Vector n = u ^ v;
float d = -(p1.x()*n.x() + p1.y()*n.y() + p1.z()*n.z());
m_equation[0] = n.x();
m_equation[1] = n.y();
m_equation[2] = n.z();
m_equation[3] = d;
}
void promptInitialPosition(Vector& initialPosition)
{
BOOL validPosition = FALSE;
do
{
STRING initialPositionString;
cout << "Entrez la position initiale comme suit X0, Y0, Z0: ";
getline(cin, initialPositionString);
CHAR* nextToken = NULL;
INT tokenCount = 0;
CHAR* token = strtok_s((CHAR*) initialPositionString.c_str(), ",", &nextToken);
while (token != NULL && tokenCount < 3)
{
initialPosition[tokenCount] = (FLOAT) atof(token);
tokenCount++;
token = strtok_s(NULL, ",", &nextToken);
}
validPosition = TRUE;
if ( tokenCount != 3 )
{
cout << "Nombre de composants invalide" << endl;
validPosition = FALSE;
}
if ( initialPosition.x() < 0.0f || initialPosition.x() > Tp1Constants::PLAYER_ZONE_WIDTH )
{
cout << "Valeur en X invalide" << endl;
validPosition = FALSE;
}
if ( initialPosition.y() < 0.0f || initialPosition.y() > Tp1Constants::PLAYER_ZONE_LENGTH )
{
cout << "Valeur en Y invalide" << endl;
validPosition = FALSE;
}
if ( initialPosition.z() < 0.0f || initialPosition.z() > Tp1Constants::COURT_REACH )
{
cout << "Valeur en Z invalide" << endl;
validPosition = FALSE;
}
if ( !validPosition )
cout << "Position initiale invalide, veuillez recommencer" << endl << endl;
} while( !validPosition );
}
// *****************************************************************************
Transform dg::Translate(const Vector &rkDelta)
{
Matrix *pkMatrix, *pkInverse;
pkMatrix = new Matrix(1, 0, 0, rkDelta.x(),
0, 1, 0, rkDelta.y(),
0, 0, 1, rkDelta.z(),
0, 0, 0, 1);
pkInverse = new Matrix(1, 0, 0, -rkDelta.x(),
0, 1, 0, -rkDelta.y(),
0, 0, 1, -rkDelta.z(),
0, 0, 0, 1);
return Transform(pkMatrix, pkInverse, true);
}
