/*Cutting of the line. Getting visible part of this line*/
bool Graphics::cutOffLine(QVector3D *p_point1, QVector3D *p_point2)
{
QVector3D pointFrom3D = *p_point1;
QVector3D pointTo3D = *p_point2;
qreal denominator = scalarProduct(eye, (pointTo3D - basePoint));
if(denominator != 0)
{
/*Counting intersection line and plane*/
qreal t = (scalarProduct(eye, (basePoint - pointFrom3D)) / scalarProduct(eye, (pointTo3D - basePoint)));
/*The point of intersection is beŠµwen point1 and point2*/
if(t >= 0 && t <= 1)
{
if(isVisible(pointFrom3D))
{
*p_point2 = QVector3D(pointFrom3D + t * (pointTo3D - pointFrom3D));
}
else if(isVisible(pointTo3D))
{
*p_point1 = QVector3D(pointFrom3D + t * (pointTo3D - pointFrom3D));
}
}
/*Both points are situated by the one side of plane*/
else if(t < 0 || t > 1)
{
if(!isVisible(pointFrom3D))
{
return false;
}
}
}
return true;
}
void doOneBeta(const EngineType& engine,
std::vector<size_t>& ne,
OpLibFactoryType& opLibFactory,
size_t site,
size_t sigma,
RealType beta)
{
RealType sum = 0;
RealType sum2 = 0;
RealType denominator = 0;
FreeFermions::Combinations combinations(engine.size(),ne[0]);
size_t factorToMakeItSmaller = 1;
for (size_t i = 0; i<combinations.size(); ++i) {
OpDiagonalFactoryType opDiagonalFactory(engine);
EtoTheBetaHType ebh(beta,engine,0);
DiagonalOperatorType& eibOp = opDiagonalFactory(ebh);
std::vector<size_t> vTmp(engine.size(),0);
for (size_t j=0;j<combinations(i).size();++j) vTmp[combinations(i)[j]]=1;
std::vector<std::vector<size_t> > occupations(1,vTmp);
HilbertStateType thisState(engine,occupations);
HilbertStateType phi = thisState;
eibOp.applyTo(phi);
denominator += scalarProduct(thisState,phi)*factorToMakeItSmaller;
LibraryOperatorType& myOp2 = opLibFactory(LibraryOperatorType::N,site,sigma);
myOp2.applyTo(phi);
sum += scalarProduct(thisState,phi)*factorToMakeItSmaller;
myOp2.applyTo(phi);
sum2 += scalarProduct(thisState,phi)*factorToMakeItSmaller;
}
std::cout<<beta<<" "<<sum<<" "<<denominator<<" "<<sum/denominator<<" "<<sum2/denominator<<"\n";
}
MinimalBoundingSphere::MinimalBoundingSphere(MathLib::Point3d const& p,
MathLib::Point3d const& q, MathLib::Point3d const& r)
{
MathLib::Vector3 const a(p,r);
MathLib::Vector3 const b(p,q);
MathLib::Vector3 const cross_ab(crossProduct(a,b));
if (cross_ab.getLength() > 0)
{
double const denom = 2.0 * scalarProduct(cross_ab,cross_ab);
MathLib::Vector3 const o = (scalarProduct(b,b) * crossProduct(cross_ab, a)
+ scalarProduct(a,a) * crossProduct(b, cross_ab))
* (1.0 / denom);
_radius = o.getLength() + std::numeric_limits<double>::epsilon();
_center = MathLib::Vector3(p) + o;
}
else
{
MinimalBoundingSphere two_pnts_sphere;
if (a.getLength() > b.getLength())
two_pnts_sphere = MinimalBoundingSphere(p,r);
else
two_pnts_sphere = MinimalBoundingSphere(p,q);
_radius = two_pnts_sphere.getRadius();
_center = two_pnts_sphere.getCenter();
}
}
Vertex getSpecularComp(Vertex _3DPoint, Vector V) {
Vertex specularComp;
Vector R = getR(_3DPoint);
specularComp.x = pow(scalarProduct(V, R), n) * Ks * Il.R;
specularComp.y = pow(scalarProduct(V, R), n) * Ks * Il.G;
specularComp.z = pow(scalarProduct(V, R), n) * Ks * Il.B;
return specularComp;
}
Vertex getDiffuseCompTexture(Vertex _3DPoint, int x, int y) {
Vector L = getL(_3DPoint);
Vector N = _3DPoint.normal;
Vertex diffuse_comp;
diffuse_comp.x = scalarProduct(N, L) * Il.R * textureR[y][x] * Kd;
diffuse_comp.y = scalarProduct(N, L) * Il.G * textureG[y][x] * Kd;
diffuse_comp.z = scalarProduct(N, L) * Il.B * textureB[y][x] * Kd;
return diffuse_comp;
}
Vertex getDiffuseComp(Vertex _3DPoint) {
Vector L = getL(_3DPoint);
Vector N = _3DPoint.normal;
Vertex diffuse_comp;
diffuse_comp.x = scalarProduct(N, L) * Il.R * Od.x * Kd;
diffuse_comp.y = scalarProduct(N, L) * Il.G * Od.y * Kd;
diffuse_comp.z = scalarProduct(N, L) * Il.B * Od.z * Kd;
return diffuse_comp;
}
Vector getR(Vertex _3DPoint) {
Vector L = getL(_3DPoint);
Vector N = _3DPoint.normal;
Vector R;
R.x = (2 * N.x * scalarProduct(N, L)) - L.x;
R.y = (2 * N.y * scalarProduct(N, L)) - L.y;
R.z = (2 * N.z * scalarProduct(N, L)) - L.z;
normalize(R);
return R;
}
Real
OneDFSIFunctionSolverDefinedCompatibility::evaluateRHS ( const Real& eigenvalue, const container2D_Type& eigenvector,
const container2D_Type& deltaEigenvector, const Real& timeStep )
{
Real cfl = computeCFL ( eigenvalue, timeStep );
container2D_Type U_interpolated;
Real Qvisco_interpolated;
U_interpolated[0] = ( 1 - cfl ) * M_bcU[0] + cfl * (* (*M_solutionPtr) ["A"]) ( M_bcInternalNode );
U_interpolated[1] = ( 1 - cfl ) * M_bcU[1] + cfl * (* (*M_solutionPtr) ["Q"]) ( M_bcInternalNode );
// The second condition detects if there is a viscoelastic flow on the bondary.
if ( M_fluxPtr->physics()->data()->viscoelasticWall() && (* (*M_solutionPtr) ["Q_visc"]) (M_bcNode) > 1e-10 )
{
Qvisco_interpolated = ( 1 - cfl ) * (* (*M_solutionPtr) ["Q_visc"]) (M_bcNode) + cfl * (* (*M_solutionPtr) ["Q_visc"]) ( M_bcInternalNode );
}
else
{
Qvisco_interpolated = 0;
}
container2D_Type U;
container2D_Type bcNodes;
bcNodes[0] = M_bcNode;
bcNodes[1] = M_bcInternalNode;
#ifdef OLD_COMPATIBILITY
U[0] = U_interpolated[0] - timeStep * M_sourcePtr->interpolatedNonConservativeSource ( U_interpolated[0], U_interpolated[1], 0, bcNodes, cfl );
U[1] = U_interpolated[1] - timeStep * M_sourcePtr->interpolatedNonConservativeSource ( U_interpolated[0], U_interpolated[1], 1, bcNodes, cfl );
#else
container2D_Type U0_interpolated;
U0_interpolated[0] = ( 1 - cfl ) * M_fluxPtr->physics()->data()->area0 (bcNodes[0]) + cfl * M_fluxPtr->physics()->data()->area0 (bcNodes[1]);
U0_interpolated[1] = 0;
U[0] = U_interpolated[0]
- U0_interpolated[0] - timeStep * ( M_sourcePtr->interpolatedNonConservativeSource ( U_interpolated[0], U_interpolated[1], 0, bcNodes, cfl ) -
M_sourcePtr->interpolatedNonConservativeSource ( U0_interpolated[0], U0_interpolated[1], 0, bcNodes, cfl ) );
U[1] = (U_interpolated[1] - Qvisco_interpolated) // We consider just the elastic component
- U0_interpolated[1] - timeStep * ( M_sourcePtr->interpolatedNonConservativeSource ( U_interpolated[0], U_interpolated[1], 1, bcNodes, cfl ) -
M_sourcePtr->interpolatedNonConservativeSource ( U0_interpolated[0], U0_interpolated[1], 1, bcNodes, cfl ) );
U_interpolated[0] -= U0_interpolated[0];
U_interpolated[1] -= U0_interpolated[1];
// Adding Z_0
U[0] += M_fluxPtr->physics()->data()->area0 (bcNodes[0]);
U[1] += 0;
#endif
return scalarProduct ( eigenvector, U ) + timeStep * eigenvalue * scalarProduct ( deltaEigenvector, U_interpolated );
}
void SocSystem_Analytical::getConstraints(arr& cdir,arr& coff,uint t,const arr& qt){
cdir.clear();
coff.clear();
#ifndef USE_TRUNCATION
return;
#endif
uint i,M=obstacles.d0;
arr d;
#if 0 //direct and clean way to do it -- but depends simple scenario
cdir.resize(M,x.N);
coff.resize(M);
for(i=0;i<M;i++){
cdir[i] = qt-obstacles[i];
coff(i) = scalarProduct(cdir[i],obstacles[i]);
}
#elif 1 //assume that the task vector is a list of scalars, each constrained >0
arr J,y;
for(i=0;i<M;i++){
real haty = norm(x-obstacles[i]);
if(haty>.5) continue; //that's good enough -> don't add the constraint
J = (x-obstacles[i])/norm(x-obstacles[i]);
coff.append(-haty + scalarProduct(J,x));
cdir.append(J);
}
cdir.reshape(coff.N,x.N);
coff.reshape(coff.N);
#else //messy: try to combine all constraints into a single scalar, doesn't really work...
//first compute squared collision meassure...
arr J(1,qt.N),phiHatQ(1);
J.setZero();
phiHatQ.setZero();
for(i=0;i<obstacles.d0;i++){
real margin = .25;
real d = 1.-norm(x-obstacles[i])/margin;
//if(d<0) continue;
//phiHatQ(0) += d*d;
//J += (2.*d/margin)*(obstacles[i]-x)/norm(x-obstacles[i]);
phiHatQ(0) += d;
J += (1./margin)*(obstacles[i]-x)/norm(x-obstacles[i]);
}
//...then add a single constraint
if(phiHatQ(0)>0.){ //potential violation, else discard
cdir.append(-J);
coff.append(phiHatQ-scalarProduct(J,x)-1.);
cdir.reshape(1,x.N);
coff.reshape(1);
}
#endif
}
void VisualBird::invokeController()
{
//in case vicon lost track of model
if (sensor == "vicon")
{
if (eye->getCurrentVel().x == 0 && eye->getCurrentVel().y == 0 && eye->getCurrentVel().z == 0)
{
dummy->directDrive(0,0,0,eye->getPsi());
}
else
{
if (hover)
{
eye->updateIntError(targetPoint);
dummy->pidHoverController(vectorMinus(eye->getCurrentPoint(),targetPoint),eye->getCurrentVel(),eye->getIntError(),eye->getPsi());
}
else
{
eye->updateIntError(targetPoint);
pcl::PointXYZ acc_des = scalarProduct(1000,nav->getAccDes());
dummy->navController(acc_des,vectorMinus(eye->getCurrentPoint(),targetPoint),eye->getCurrentVel(),eye->getIntError(),eye->getPsi());
//dummy->directDrive(acc_des.x,acc_des.y,acc_des.z,eye->getPsi());
//eye->calculatePositionVelocityError();
//dummy->pidPathController(eye->getE_p(),eye->getE_v(),eye->getAcc_t(),eye->getPsi());
}
}
}
}
void BirdEye::calculatePositionVelocityError()
{
std::vector<int> pointIdxNKNSearch(1);
std::vector<float> pointNKNSquaredDistance(1);
if (kdtree.nearestKSearch(currentPoint,1,pointIdxNKNSearch,pointNKNSquaredDistance)>0)
{
for (size_t i = 0; i < pointIdxNKNSearch.size (); ++i)
{
pcl::PointXYZ pathPoint,pathPoint_vel,pathPoint_acc,pathTangent,pathNormal,pathBinormal;
pathPoint = cloud->points[ pointIdxNKNSearch[i] ];
pathPoint_vel = cloud_vel->points[ pointIdxNKNSearch[i] ];
pathPoint_acc = cloud_acc->points[ pointIdxNKNSearch[i] ];
pathTangent = normalize(pathPoint_vel);
//pathNormal = normalize(pathPoint_acc);
//pathBinormal = crossProduct(pathTangent,pathNormal);
e_p = vectorMinus(currentPoint,pathPoint);
e_p = vectorMinus(e_p,scalarProduct( dotProduct(e_p,pathTangent),pathTangent) );
ROS_DEBUG("current point: %.3f,%.3f,%.3f",currentPoint.x,currentPoint.y,currentPoint.z);
ROS_DEBUG("path point: %.3f,%.3f,%.3f",pathPoint.x,pathPoint.y,pathPoint.z);
e_v = vectorMinus(currentVel,pathPoint_vel);
acc_t = pathPoint_acc;
ROS_DEBUG("error in position: %.3f,%.3f,%.3f",e_p.x,e_p.y,e_p.z);
}
}
}
FieldType doOneOmegaOneSitePair(SizeType site1,
SizeType threadNum,
const HilbertStateType& phiKet0,
const HilbertStateType& phiKet1)
{
FieldType tmpC = 0.0;
SizeType sigma0 = 0;
SizeType sigma1 = 0;
for (SizeType dynType = 0; dynType < 2; ++dynType) {
RealType sign = observable_.signForWeight(sigma0, sigma1, dynType);
const HilbertStateType& phiKet = (dynType == 0) ? phiKet0 : phiKet1;
HilbertStateType phiBra = params_.gs;
observable_.applyLeftOperator(phiBra,
site1,
sigma1,
dynType,
threadNum);
tmpC += sign*scalarProduct(phiBra, phiKet);
}
return tmpC;
}
void VisualBird::invokeController()
{
//in case vicon lost track of model
if (sensor == "vicon")
{
if (eye->getCurrentVel().x == 0 && eye->getCurrentVel().y == 0 && eye->getCurrentVel().z == 0)
{
dummy->directDrive(0,0,0,eye->getPsi());
}
else
{
if (hover)
{
eye->updateIntError(targetPoint);
dummy->pidHoverController(vectorMinus(eye->getCurrentPoint(),targetPoint),eye->getCurrentVel(),eye->getIntError(),eye->getPsi());
}
else
{
eye->updateIntError(targetPoint);
pcl::PointXYZ acc_des = scalarProduct(1000., nav->getAccDes());
vel_ref.push_back(nav->getVelDes());
acc_ref.push_back(nav->getAccDes());
//ROS_INFO("DESIRED ACC %.3f %.3f %.3f", acc_des.x/1000, acc_des.y/1000, acc_des.z/1000);
dummy->navController(acc_des,vectorMinus(eye->getCurrentPoint(),targetPoint),eye->getCurrentVel(),eye->getIntError(),eye->getPsi());
}
}
}
}
void VisualBird::drive()
{
//update flight status
eye->updateFlightStatus();
ROS_DEBUG("%.3f %.3f %.3f", eye->getTargetVel().x, eye->getTargetVel().y, eye->getTargetVel().z);
//check whether arrived at start point
checkStartPoint();
//check landing condition
land();
if (norm(vectorMinus(eye->getCurrentPoint(), shiftOrigin)) < 150 && !landing && !circleStart)
{
ROS_INFO("Start virtual target");
targetCircleCenter = eye->getCurrentPoint();
targetCircleCenter.y += radius + 50;
currTheta = 0;
circleStart = true;
}
if (circleStart && !landing)
{
targetPoint.x = targetCircleCenter.x + radius * sin(currTheta);
targetPoint.y = targetCircleCenter.y - radius * cos(currTheta);
targetPosLog.push_back(targetPoint);
targetVel.x = radius * omega * cos(currTheta)/1000.;
targetVel.y = radius * omega * sin(currTheta)/1000.;
targetVel.z = 0;
targetVelLog.push_back(targetVel);
targetAcc.x = - radius * omega * omega * sin(currTheta)/1000.;
targetAcc.y = radius * omega * omega * cos(currTheta)/1000.;
targetAcc.z = 0;
targetAccLog.push_back(targetAcc);
currTheta += omega/freq;
//ROS_INFO("Virtual target at %.3f %.3f %.3f", targetPoint.x, targetPoint.y, targetPoint.z);
}
if (!hover)
nav->updatePVA(scalarProduct(0.001,vectorMinus(eye->getCurrentPoint(),shiftOrigin)),scalarProduct(0.001,vectorMinus(targetPoint,shiftOrigin)),scalarProduct(0.001,eye->getCurrentVel()),targetVel,targetAcc);
invokeController();
visualTarget.push_back(targetPoint);
geometry_msgs::PointStamped msg;
msg.header.frame_id = "world";
msg.point.x = targetPoint.x/1000;
msg.point.y = targetPoint.y/1000;
msg.point.z = targetPoint.z/1000;
msg.header.stamp = ros::Time::now();
target_pub.publish(msg);
double secs =ros::Time::now().toSec();
timeLog.push_back(secs);
counter ++;
}
FieldType calcSuperDensity(size_t site,
size_t site2,
const HilbertStateType& gs,
const EngineType& engine)
{
HilbertStateType savedVector = gs;
FieldType savedValue = 0;
FieldType sum = 0;
OpNormalFactoryType opNormalFactory(engine);
OpLibFactoryType opLibFactory(engine);
for (size_t sigma = 0;sigma<2;sigma++) {
HilbertStateType phi = gs;
LibraryOperatorType& myOp = opLibFactory(
LibraryOperatorType::N,site,1-sigma);
myOp.applyTo(phi);
OperatorType& myOp2 = opNormalFactory(OperatorType::CREATION,
site,sigma);
myOp2.applyTo(phi);
for (size_t sigma2 = 0;sigma2 < 2;sigma2++) {
HilbertStateType phi3 = phi;
LibraryOperatorType& myOp3 = opLibFactory(
LibraryOperatorType::NBAR,site2,1-sigma2);
myOp3.applyTo(phi3);
OperatorType& myOp4 = opNormalFactory(
OperatorType::DESTRUCTION,site2,sigma2);
myOp4.applyTo(phi3);
if (sigma ==0 && sigma2 ==0) savedVector = phi3;
sum += scalarProduct(phi3,phi3);
if (sigma ==1 && sigma2 ==1) {
savedValue = scalarProduct(phi3,savedVector);
}
}
}
sum += 2*real(savedValue);
//std::cerr<<"#sum2="<<scalarProduct(tmpV,tmpV)<<"\n";
return sum;
}
MinimalBoundingSphere::MinimalBoundingSphere(MathLib::Point3d const& p,
MathLib::Point3d const& q,
MathLib::Point3d const& r,
MathLib::Point3d const& s)
{
MathLib::Vector3 const a(p, q);
MathLib::Vector3 const b(p, r);
MathLib::Vector3 const c(p, s);
if (!GeoLib::isCoplanar(p, q, r, s))
{
double const denom = 2.0 * GeoLib::scalarTriple(a,b,c);
MathLib::Vector3 const o = (scalarProduct(c,c) * crossProduct(a,b)
+ scalarProduct(b,b) * crossProduct(c,a)
+ scalarProduct(a,a) * crossProduct(b,c))
* (1.0 / denom);
_radius = o.getLength() + std::numeric_limits<double>::epsilon();
_center = MathLib::Vector3(p) + o;
}
else
{
MinimalBoundingSphere const pqr(p, q , r);
MinimalBoundingSphere const pqs(p, q , s);
MinimalBoundingSphere const prs(p, r , s);
MinimalBoundingSphere const qrs(q, r , s);
_radius = pqr.getRadius();
_center = pqr.getCenter();
if (_radius < pqs.getRadius())
{
_radius = pqs.getRadius();
_center = pqs.getCenter();
}
if (_radius < prs.getRadius())
{
_radius = prs.getRadius();
_center = prs.getCenter();
}
if (_radius < qrs.getRadius())
{
_radius = qrs.getRadius();
_center = qrs.getCenter();
}
}
}
/*Checking if the point is behind of the projection plane*/
bool Graphics::isVisible(QVector3D point)
{
qreal angle = scalarProduct(eyePos - basePoint, point - basePoint);
if(angle < 0)
{
return true;
}
return false;
}
/**
* @fn executed by the slaves processes
*
* @param inMyRank the slave Id
* @param inLabel the master-slaves shared label to communicate
*
* @brief The slave :
* -Receives the vector
* -Then, it's waiting for matrix's rows
* -if is not the END label then
* -computes the scalar product
* -sends the results to the master
* -else
* -Game Over
*
*/
void slave(int inMyRank, int inLabel) {
int i;
int product;
int vectorSize;
MPI_Status status;
int * vector;
int * row;
//Beginning
printf("Slave#%d, I'm waiting for the vector\n", inMyRank);
/* -- Receive the vector -- */
//Receive the vector and its size
MPI_Bcast(&vectorSize, 1, MPI_INT, 0, MPI_COMM_WORLD);
vector = initVector(vectorSize);
row = initVector(vectorSize);
MPI_Bcast(vector, vectorSize, MPI_INT, 0, MPI_COMM_WORLD);
printf("Slave#%d, I received the vector : \n", inMyRank);
printVector(vector, vectorSize);
/* -- waiting for rows -- */
//Receive the first row
MPI_Recv(row, vectorSize, MPI_INT, 0, inLabel, MPI_COMM_WORLD, &status);
printf("Slave#%d, I received the row : \n", inMyRank);
printVector(row, vectorSize);
//if is not the END label
while (status.MPI_TAG != END) {
//compute the scalar product
product = scalarProduct(vector, row, vectorSize);
printf("Slave#%d, The scalar product is : %d\n", inMyRank, product);
//Send the computed result
MPI_Send(&product, 1, MPI_INT, 0, inLabel, MPI_COMM_WORLD);
//Get the next row
printf("Slave#%d, I'm waiting for a new line\n", inMyRank);
MPI_Recv(row, vectorSize, MPI_INT, 0, MPI_ANY_TAG, MPI_COMM_WORLD,
&status);
printf("Slave#%d, I received the row : \n", inMyRank);
printVector(row, vectorSize);
}
//Game Over
printf("Slave#%d, I'm done. Bye\n", inMyRank);
//Cleaning
free(vector);
free(row);
}
const Vector<T> operator* (const Vector<T>& lhs, const T& rhs)
{
Vector<T> scalarProduct(lhs);
for(int i = 0; i < lhs.terms; i++)
{
scalarProduct.data[i] *= rhs;
}
return scalarProduct;
}
// ===================================================
// Methods
// ===================================================
Real
OneDFSIFunctionSolverDefinedAbsorbing::operator() ( const Real& /*time*/, const Real& timeStep )
{
updateBCVariables();
computeEigenValuesVectors();
Real W_out (0.), W_out_old (0.);
switch ( M_bcType )
{
case OneDFSI::W1:
W_out = M_bcW[1] + evaluateRHS ( M_eigenvalues[1], M_leftEigenvector2, M_deltaLeftEigenvector2, timeStep ) - scalarProduct ( M_leftEigenvector2, M_bcU );
W_out_old = -M_bcW[0] + scalarProduct ( M_leftEigenvector1, M_bcU );
break;
case OneDFSI::W2:
W_out = M_bcW[0] + evaluateRHS ( M_eigenvalues[0], M_leftEigenvector1, M_deltaLeftEigenvector1, timeStep ) - scalarProduct ( M_leftEigenvector1, M_bcU );
W_out_old = -M_bcW[1] + scalarProduct ( M_leftEigenvector2, M_bcU );
break;
default:
std::cout << "Warning: bcType \"" << M_bcType << "\"not available!" << std::endl;
break;
}
Real a1, a2, a11, a22, b1, b2, c1, c2;
a1 = M_fluxPtr->physics()->pressure ( M_bcU[0], timeStep, M_bcNode ) - this->venousPressure(); // pressure at previous time step
a2 = M_bcU[1]; // flux at previous time step
b1 = M_fluxPtr->physics()->dPdW ( M_bcW[0], M_bcW[1], 0, M_bcNode ); // dP / dW1
b2 = M_bcU[0] / 2; // dQ / dW1
c1 = M_fluxPtr->physics()->dPdW ( M_bcW[0], M_bcW[1], 1, M_bcNode ); // dP / dW2
c2 = b2; // dQ / dW2
a11 = a1 - b1 * M_bcW[0] - c1 * M_bcW[1];
a22 = a2 - b2 * M_bcW[0] - c2 * M_bcW[1];
Real resistance (b1 / b2);
this->resistance ( resistance );
return W_out_old + W_out * (b2 * resistance - b1) / (c1 - c2 * resistance) + (a22 * resistance - a11) / (c1 - c2 * resistance);
}
// Gets point's projection's parameter on the line,
// parametrized by points zero and one.
double Vector::getLineParameter(const Vector& zero, const Vector& one) const
{
// pr_this = zero + (one - zero) * t
// From Creature::project:
// pr_this = zero + (one - zero) * (this - zero, one - zero) / |one - zero|
// t = (this - zero, one - zero) / |one - zero|
double dist = zero.getDistance(one);
if (DoubleComparison::areEqual(dist, 0))
{
return 0;
}
return scalarProduct(*this - zero, one - zero) / dist;
}
pcl::PointXYZ BirdEye::differentiate(const std::vector<pcl::PointXYZ>& x)
{
pcl::PointXYZ dx;
if (lostVicon)
{
dx = zeroVector;
}
else
{
if (freezeCounter!=0)
{
if (freezeCounter>averageStep)
dx = scalarProduct(freq/(freezeCounter+1), vectorMinus(x.back(),x[x.size()-1-freezeCounter-1]));
else
dx = scalarProduct(freq/averageStep, vectorMinus(x.back(),x[x.size()-1-averageStep]));
freezeCounter = 0;
}
else
{
if (x.size()>(unsigned)(1+averageStep))
{
if (!checkZeroVector(x[x.size()-1-averageStep]))
dx = scalarProduct(freq/averageStep, vectorMinus(x.back(),x[x.size()-1-averageStep]));
else
{
int tempCounter = 1;
while (checkZeroVector(x[x.size()-1-averageStep-tempCounter]))
tempCounter++;
dx = scalarProduct(freq/(averageStep+tempCounter), vectorMinus(x.back(),x[x.size()-1-averageStep-tempCounter]));
}
}
else
{
dx = scalarProduct(freq/x.size(),vectorMinus(x.back(),x.front()));
}
}
}
return dx;
}
void generateConditionedRandomProjection(arr& M, uint n, double condition) {
uint i,j;
//let M be a ortho-normal matrix (=random rotation matrix)
M.resize(n,n);
rndUniform(M,-1.,1.,false);
//orthogonalize
for(i=0; i<n; i++) {
for(j=0; j<i; j++) M[i]()-=scalarProduct(M[i],M[j])*M[j];
M[i]()/=length(M[i]);
}
//we condition each column of M with powers of the condition
for(i=0; i<n; i++) M[i]() *= pow(condition, double(i) / (2.*double(n - 1)));
}
void VisualBird::drive()
{
//update flight status
eye->updateFlightStatus();
ROS_DEBUG("%.3f %.3f %.3f", eye->getTargetVel().x, eye->getTargetVel().y, eye->getTargetVel().z);
nav->updatePVA(scalarProduct(0.001,vectorMinus(eye->getCurrentPoint(),shiftOrigin)),eye->getTargetPos(),scalarProduct(0.001,eye->getCurrentVel()),eye->getTargetVel(),0);
//ROS_DEBUG("%.3f %.3f %.3f",vectorMinus(eye->getCurrentPoint(),shiftOrigin).x,vectorMinus(eye->getCurrentPoint(),shiftOrigin).y,vectorMinus(eye->getCurrentPoint(),shiftOrigin).z);
//check whether arrived at start point
//checkStartPoint();
//hover to points
//hoverFlight();
//check landing condition
land();
//check out range condition
//safeOutRange();
//ROS_INFO("DISTANCE %f",norm(vectorMinus(eye->getCurrentPoint(), shiftOrigin)));
//ROS_INFO("Target %f %f %f", targetPoint.x, targetPoint.y, targetPoint.z);
if (norm(vectorMinus(eye->getCurrentPoint(), shiftOrigin)) < 150 && !landing && visual_feedback && !start_visual)
{
ROS_INFO("VISUAL GUIDANCE Initiated");
start_visual = true;
double secs =ros::Time::now().toSec();
timeTable.push_back(secs);
}
if (start_visual && !visualBuffer.empty() && !landing && !hover)
{
ROS_INFO("VISUAL GUIDANCE In Effect");
double secs =ros::Time::now().toSec();
timeTable.push_back(secs);
switchHover();
//Notice that here switchHover is not used because we want to have z stabiled with integral feedback
}
//call hover controller or path controller according to (bool hover)
invokeController();
visualTarget.push_back(targetPoint);
double secs =ros::Time::now().toSec();
timeLog.push_back(secs);
counter ++;
}
/**
* @brief Get the forth and fiveth centroids for ElectroShape method
* @author Javier Klett <*****@*****.**>
*
* @param xs Vector of X coordinates
* @param ys Vector of Y coordinates
* @param zs Vector of Z coordinates
* @param qs Vector of partial charges
* @param n_atoms Number of atoms of the molecule
* @param mycentroids Resulting centroids
* @return 0 on success
*
*/
int getCentroid4_5(float * xs, float * ys, float * zs, float * qs, int n_atoms, double ***mycentroids){
double A[DIMENSION-1],B[DIMENSION-1],Cvec[DIMENSION-1];
double moduleA, moduleCrossAB, crossAB[DIMENSION-1],K;
int i;
double tmp[4];
double qMin=0, qMax=0;
double **centroids = NULL;
centroids = *mycentroids;
for (i =0;i < DIMENSION-1;i++){
A[i] = centroids[1][i]-centroids[0][i];
B[i] = centroids[2][i]-centroids[0][i];
}
moduleA = sqrt((centroids[1][0]-centroids[0][0])*(centroids[1][0]-centroids[0][0])+(centroids[1][1]-centroids[0][1])*(centroids[1][1]-centroids[0][1])+(centroids[1][2]-centroids[0][2])*(centroids[1][2]-centroids[0][2])+(centroids[1][3]-centroids[0][3])*(centroids[1][3]-centroids[0][3]));
crossProduct(A,B,crossAB);
moduleCrossAB = scalarProduct(crossAB,crossAB);
K = moduleA/(2*moduleCrossAB);
tmp[0] = K*crossAB[0];
tmp[1] = K*crossAB[1];
tmp[2] = K*crossAB[2];
for (i=0; i < n_atoms; i++){
if (SCALING_CHARGE*qs[i] > qMax){
qMax = SCALING_CHARGE*qs[i];
}else if(SCALING_CHARGE*qs[i] < qMin){
qMin = SCALING_CHARGE*qs[i];
}
}
centroids[3][0] = centroids[0][0] + tmp[0];
centroids[3][1] = centroids[0][1] + tmp[1];
centroids[3][2] = centroids[0][2] + tmp[2];
centroids[3][3] = qMax;
centroids[4][0] = centroids[0][0] + tmp[0];
centroids[4][1] = centroids[0][1] + tmp[1];
centroids[4][2] = centroids[0][2] + tmp[2];
centroids[4][3] = qMin;
return 0;
}
pcl::PointXYZ VisualBird::rescaleVisual(pcl::PointXYZ input)
{
pcl::PointXYZ output,temp;
//drifterror = scalarProduct(120./240*(eye->getCurrentPoint().z-eye->getOriginPoint().z)/300.,input);
temp = scalarProduct(140./240*(eye->getCurrentPoint().z-eye->getOriginPoint().z)/300,input);
output.x = temp.x * cos(M_PI*3/4) - temp.y * sin(M_PI*3/4);
output.y = temp.x * sin(M_PI*3/4) + temp.y * cos(M_PI*3/4);
temp.x = eye->getCurrentPoint().x + output.x * cos(eye->getPsi()) - output.y * sin(eye->getPsi());
temp.y = eye->getCurrentPoint().y + output.x * sin(eye->getPsi()) + output.y * cos(eye->getPsi());
temp.z = targetBound.z;
return temp;
}
unsigned MsrMatrix::solve(double *result) const {
double alpha = 1;
double rho = 1;
double omega = 1;
double *buffer = new double[size * 6];
double *r = buffer;
double *rcap = buffer + size;
double *v = buffer + size * 2;
double *p = buffer + size * 3;
double *s = buffer + size * 4;
double *t = buffer + size * 5;;
unsigned i, iter;
double beta, rhoold;
const double bnorm2 = scalarProduct(rightCol, rightCol);
const double accuracy2 = accuracy * accuracy;
applyToVector(result, r);
memset (v, 0, size * sizeof(double));
memset (p, 0, size * sizeof(double));
for (i = 0; i < size; ++i) {
r[i] = rightCol[i] - r[i];
rcap[i] = r[i];
}
for (iter = 1; iter < max_iter; ++iter) {
rhoold = rho;
rho = scalarProduct(rcap, r);
beta = (rho * alpha) / (rhoold * omega);
for (i = 0; i < size; ++i) {
p[i] = r[i] + beta * (p[i] - omega * v[i]);
}
applyToVector(p, v);
alpha = rho / scalarProduct(rcap, v);
for (i = 0; i < size; ++i) {
s[i] = r[i] - alpha * v[i];
}
applyToVector(s, t);
omega = scalarProduct(t, s) / scalarProduct(t, t);
for (i = 0; i < size; ++i) {
result[i] += (alpha * p[i] + omega * s[i]);
}
if (scalarProduct(r, r) < accuracy2 * bnorm2) {
delete[] buffer;
return iter;
}
for (i = 0; i < size; ++i) {
r[i] = s[i] - omega * t[i];
}
}
delete[] buffer;
throw std::runtime_error("Maximum number of iterations reached.");
}
pcl::PointXYZ VisualBird::rescaleVisual(pcl::PointXYZ input)
{
pcl::PointXYZ output,temp;
//drifterror = scalarProduct(120./240*(eye->getCurrentPoint().z-eye->getOriginPoint().z)/300.,input);
temp = scalarProduct(140./120*(eye->getCurrentPoint().z-eye->getOriginPoint().z)/300,input);
ROS_DEBUG("OUTPUT %f %f", temp.x, temp.y);
output.x = temp.x * cos(M_PI*3/4) - temp.y * sin(M_PI*3/4);
output.y = temp.x * sin(M_PI*3/4) + temp.y * cos(M_PI*3/4);
temp.x = output.x * cos(eye->getPsi()) - output.y * sin(eye->getPsi());
temp.y = output.x * sin(eye->getPsi()) + output.y * cos(eye->getPsi());
temp.z = 0;
return temp;
}
void TruncateGaussian(arr& a,arr& A,const arr& c,real d){
//cout <<"A=" <<A <<" a=" <<a <<" c=" <<c <<" d=" <<d <<endl;
//-- linear transform to make (a,A) to a standard Gaussian
arr M;
lapack_cholesky(M,A);
//cout <<"M=" <<M <<endl <<~M*M <<endl <<"A=" <<A <<endl;
//-- transform and rescale the constraint
real z=scalarProduct(c,a)-d;
//cout <<"a=" <<a <<"\nc*a=" <<scalarProduct(c,a) <<"\nd=" <<d <<endl;
arr v;
v=M*c;
real norm_v=norm(v);
CHECK(norm_v>1e-10,"no gradient for Gaussian trunctation!");
z/=norm_v;
v/=norm_v;
//cout <<"c=" <<c <<" v=" <<v <<endl;
//-- build rotation matrix for constraint to be along the x-axis
uint n=a.N;
arr e_1(n);
e_1.setZero(); e_1(0)=1.;
arr R;
rotationFromAtoB(R,e_1,v);
//cout <<"R=" <<R <<~R <<inverse(R) <<v <<endl <<R*e_1 <<endl;
//-- get mean and variance along x-axis
real mean,var;
TruncatedStandardGaussian(mean,var,-z);
arr B(n,n),b(n);
b.setZero(); b(0)=mean;
B.setId(n); B(0,0)=var;
//-- transform back
A = ~M*R*B*~R*M;
a = ~M*R*b + a;
checkNan(a);
checkNan(A);
}
void TruncateGaussianBySampling(arr& a,arr& A,const arr& c,real d,uint N,arr *data){
uint i,j,n=a.N;
//-- generate samples from the Gaussian
arr M,x(n),X;
lapack_cholesky(M,A);
for(i=0;i<N;i++){
for(j=0;j<n;j++) x(j)=rnd.gauss();
x = ~M*x;
x += a;
if(scalarProduct(c,x)-d>=0.) X.append(x);
}
if(!X.N){
cout <<"TruncateGaussianBySampling: no samples survived!" <<endl;
if(data) (*data).clear();
return;
}
X.reshape(X.N/n,n);
gaussFromData(a,A,X);
if(data) *data = X;
}
