//------------------------------------------------------------------------------
void Missile::frameMove(float dt)
{
Projectile::frameMove(dt);
updateTarget(dt);
Vector cur_dir = getGlobalLinearVel();
cur_dir.normalize();
Vector target_dir = target_ - getPosition();
float l = target_dir.length();
if (equalsZero(l)) return;
target_dir /= l;
float alpha = acosf(vecDot(&cur_dir, &target_dir));
if (alpha > agility_*dt )
{
Matrix m;
Vector axis;
vecCross(&axis, &target_dir, &cur_dir);
if (!equalsZero(axis.length()))
{
m.loadRotationVector(agility_*dt, axis);
target_dir = m.transformVector(cur_dir);
} else target_dir = cur_dir;
}
setGlobalLinearVel(target_dir*speed_);
}
bool ClientCartesianController::getPose(const int axis, Vector &x, Vector &o,
Stamp *stamp)
{
if (!connected)
return false;
Bottle command, reply;
command.addVocab(IKINCARTCTRL_VOCAB_CMD_GET);
command.addVocab(IKINCARTCTRL_VOCAB_OPT_POSE);
command.addInt(axis);
if (!portRpc.write(command,reply))
{
yError("unable to get reply from server!");
return false;
}
if (reply.get(0).asVocab()==IKINCARTCTRL_VOCAB_REP_ACK)
{
if (Bottle *posePart=reply.get(1).asList())
{
x.resize(3);
o.resize(posePart->size()-x.length());
for (size_t i=0; i<x.length(); i++)
x[i]=posePart->get(i).asDouble();
for (size_t i=0; i<o.length(); i++)
o[i]=posePart->get(x.length()+i).asDouble();
if ((reply.size()>2) && (stamp!=NULL))
{
if (Bottle *stampPart=reply.get(2).asList())
{
Stamp tmpStamp(stampPart->get(0).asInt(),
stampPart->get(1).asDouble());
*stamp=tmpStamp;
}
}
return true;
}
}
return false;
}
void BehaviorModule::getArmDependentOptions(Bottle &b, Vector &_gOrien,
Vector &_gDisp, Vector &_dOffs, Vector &_dLift, Vector &_home_x) {
if (Bottle *pB=b.find("grasp_orientation").asList()) {
int sz = pB->size();
int len = _gOrien.length();
int l = len < sz ? len : sz;
for (int i = 0; i < l; i++)
_gOrien[i] = pB->get(i).asDouble();
}
if (Bottle *pB=b.find("grasp_displacement").asList()) {
int sz = pB->size();
int len = _gDisp.length();
int l = len < sz ? len : sz;
for (int i = 0; i < l; i++)
_gDisp[i] = pB->get(i).asDouble();
}
if (Bottle *pB=b.find("systematic_error_displacement").asList()) {
int sz = pB->size();
int len = _dOffs.length();
int l = len < sz ? len : sz;
for (int i = 0; i < l; i++)
_dOffs[i] = pB->get(i).asDouble();
}
if (Bottle *pB=b.find("lifting_displacement").asList()) {
int sz = pB->size();
int len = _dLift.length();
int l = len < sz ? len : sz;
for (int i = 0; i < l; i++)
_dLift[i] = pB->get(i).asDouble();
}
if (Bottle *pB=b.find("home_position").asList()) {
int sz = pB->size();
int len = _home_x.length();
int l = len < sz ? len : sz;
for (int i = 0; i < l; i++)
_home_x[i] = pB->get(i).asDouble();
}
}
bool CalibModule::createTargets(const Vector &c, const Vector &size)
{
if ((c.length()<3) || (size.length()<2))
return false;
double a=std::max(fabs(size[0]),0.04);
double b=std::max(fabs(size[1]),0.02);
Vector ax1(4,0.0);
ax1[2]=1.0;
ax1[3]=90.0*CTRL_DEG2RAD;
Matrix H=axis2dcm(ax1);
H(0,3)=c[0];
H(1,3)=c[1];
H(2,3)=c[2];
H(3,3)=1.0;
targets.clear();
for (int i=0; i<2; i++)
{
for (double theta=0.0; theta<=2.0*M_PI; theta+=(2.0*M_PI)/50.0)
{
Vector x(4);
x[0]=a*cos(theta);
x[1]=b*sin(theta);
x[2]=0.0;
x[3]=1.0;
x=H*x;
x.pop_back();
targets.push_back(x);
yInfo("created point #%d=(%s)",(int)targets.size(),x.toString(3,3).c_str());
}
Vector ax2(4,0.0);
ax2[1]=1.0;
ax2[3]=90.0*CTRL_DEG2RAD;
H=axis2dcm(ax2)*axis2dcm(ax1);
H(0,3)=c[0];
H(1,3)=c[1];
H(2,3)=c[2];
H(3,3)=1.0;
}
curExplorationCenter=c;
return true;
}
bool getFeedback(Vector &fbTorso, Vector &fbHead, PolyDriver *drvTorso,
PolyDriver *drvHead, exchangeData *commData, double *timeStamp)
{
IEncodersTimed *encs;
int nJointsTorso=fbTorso.length();
int nJointsHead=fbHead.length();
Vector fb(std::max(nJointsTorso,nJointsHead));
Vector stamps(nJointsTorso+nJointsHead,0.0);
bool ret=true;
if (drvTorso!=NULL)
{
drvTorso->view(encs);
if (encs->getEncodersTimed(fb.data(),stamps.data()))
{
for (int i=0; i<nJointsTorso; i++)
fbTorso[i]=CTRL_DEG2RAD*fb[nJointsTorso-1-i]; // reversed order
}
else
ret=false;
}
else
fbTorso=0.0;
drvHead->view(encs);
if (encs->getEncodersTimed(fb.data(),stamps.data()+nJointsTorso))
{
for (int i=0; i<nJointsHead; i++){
fbHead[i]=CTRL_DEG2RAD*fb[i];
}
}
else
ret=false;
// impose vergence != 0.0
if (commData!=NULL)
if (fbHead[nJointsHead-1]<commData->get_minAllowedVergence())
fbHead[nJointsHead-1]=commData->get_minAllowedVergence();
// retrieve the highest encoders time stamp
if (timeStamp!=NULL)
*timeStamp=findMax(stamps);
return ret;
}
void Filter::init(const Vector &y0)
{
// if there is a last input then take it as guess for the next input
if (uold[0].size()>0)
init(y0,uold[0]);
else // otherwise use zero
init(y0,zeros(y0.length()));
}
bool setInitialGuess(const Vector &x0)
{
size_t len=std::min(x0.length(),this->x0.length());
for (size_t i=0; i<len; i++)
this->x0[i]=x0[i];
return true;
}
double Vector::angle(Vector vector) {
double out = 0;
if (length() && vector.length())
{
double tmp = dot(vector) / ( length() * vector.length() );
if (tmp > 1)
out = acos(1);
else if (tmp < -1)
out = acos(-1);
else
out = acos(tmp);
}
else
out = 0.0;
return out;
}
void EyePinvRefGen::setCounterRotGain(const Vector &gain)
{
LockGuard guard(mutex);
size_t len=std::min(counterRotGain.length(),gain.length());
for (size_t i=0; i<len; i++)
counterRotGain[i]=gain[i];
yInfo("counter-rotation gains set to (%s)",counterRotGain.toString(3,3).c_str());
}
void helperPID::addVectorToOption(Bottle &option, const char *key, const Vector &val)
{
Bottle &bKey=option.addList();
bKey.addString(key);
Bottle &bKeyContent=bKey.addList();
for (size_t i=0; i<val.length(); i++)
bKeyContent.addDouble(val[i]);
}
bool Constraints::distance(Particle& a, Particle& b, float d) {
Vector v = b.position - a.position;
float l = v.length();
v *= (l - d) / ((a.inverseMass + b.inverseMass) * l);
a.position += v * a.inverseMass;
b.position -= v * b.inverseMass;
return l > 0.f;
}
Point Sphere::support(const Vector& v) const {
Scalar s = v.length();
if (s > EPSILON) {
Scalar r = radius / s;
return Point(v[X] * r, v[Y] * r, v[Z] * r);
}
else return Point(0, 0, 0);
}
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
bool dynContact::checkVectorDim(const Vector &v, unsigned int dim, const string &descr){
if(v.length() != dim){
if(verbose)
fprintf(stderr, "Error in dynContact: unexpected dimension of vector %s, %d\n", descr.c_str(), (int)v.length());
return false;
}
return true;
}
void ff2LayNN::setItem(Property &options, const string &tag, const Vector &item) const
{
Bottle b; Bottle &v=b.addList();
for (size_t i=0; i<item.length(); i++)
v.addDouble(item[i]);
options.put(tag.c_str(),b.get(0));
}
bool get_starting_point(Ipopt::Index n, bool init_x, Ipopt::Number *x,
bool init_z, Ipopt::Number *z_L, Ipopt::Number *z_U,
Ipopt::Index m, bool init_lambda, Ipopt::Number *lambda)
{
x[0]=q0[q0.length()-2];
x[1]=q0[q0.length()-1];
return true;
}
AString
sp::BuildTypeName(Type *aType, Atom *name, TypeDiagFlags flags)
{
if (ArrayType *type = aType->asArray()) {
Vector<ArrayType *> stack;
Type *cursor = type;
Type *innermost = nullptr;
for (;;) {
if (!cursor->isArray()) {
innermost = cursor;
break;
}
stack.append(cursor->toArray());
cursor = cursor->toArray()->contained();
}
AutoString builder;
if (aType->isConst() || !!(flags & TypeDiagFlags::IsConst))
builder = "const ";
builder = builder + BuildTypeName(innermost, nullptr, flags & kDiagFlagsInnerMask);
bool hasFixedLengths = false;
AutoString brackets;
for (size_t i = 0; i < stack.length(); i++) {
if (!stack[i]->hasFixedLength()) {
brackets = brackets + "[]";
continue;
}
hasFixedLengths = true;
brackets = brackets + "[" + AutoString(stack[i]->fixedLength()) + "]";
}
if (name) {
if (hasFixedLengths)
builder = builder + " " + name->chars() + brackets;
else
builder = builder + brackets + " " + name->chars();
} else {
builder = builder + brackets;
}
return AString(builder.ptr());
}
AutoString builder;
if (FunctionType *type = aType->asFunction())
builder = BuildTypeFromSignature(type->signature(), flags & kDiagFlagsInnerMask);
else
builder = GetBaseTypeName(aType);
if (!!(flags & TypeDiagFlags::IsByRef)) {
builder = builder + "&";
}
if (name)
builder = builder + " " + name->chars();
return AString(builder.ptr());
}
void Arrow::draw ( const Point& from, const Point& to ) const
{
Vector v = to - from;
double length = v.length();
Vector n = v;
n.normalize();
Vector x( 1, 0, 0 ); // Canonical direction for drawing the arrow
double theta = n * x;
Vector r = x.cross( v ); // Rotation vector
double rl = r.length();
if ( fabs( rl ) > 1e-6 )
r.normalize();
else
r = Vector( 0, 0, 1 ); // theta == 0 anyway
// Also, figure out how to rotate the arrow head so that it is
// normal to the view normal, i.e., so you can always see the side
// of the arrow head. Start by applying the orienting rotation to
// the canonical arrow head normal.
Matrix m( Matrix::ROTATION, r[X], r[Y], r[Z], -acos( theta ) );
Vector z( 0, 0, 1 );// Canonical arrow head normal
Vector zr = m * z;
Vector y( 0, 1, 0 );//The "other" direction (in the plane of n)
Vector yr = m * y;
double dz = zr * view_normal_;
double dy = yr * view_normal_;
double phi = atan2( dy, dz );
glPushMatrix();
glTranslated( from[X], from[Y], from[Z] );
glRotated( -180 * phi / M_PI, n[X], n[Y], n[Z] );
glRotated( 180 * acos( theta ) / M_PI, r[X], r[Y], r[Z] );
switch ( style_ ) {
case lC::OPEN:
drawOpen( length ); break;
case lC::HOLLOW:
drawHollow( length ); break;
case lC::FILLED:
drawFilled( length ); break;
}
glPopMatrix();
}
bool
js::ParallelDo::hasScript(Vector<types::RecompileInfo> &scripts, JSScript *script)
{
for (uint32_t i = 0; i < scripts.length(); i++) {
if (scripts[i] == script->parallelIonScript()->recompileInfo())
return true;
}
return false;
}
void CaptureParticleWorker::handleSurfaceInteraction(int depth,
bool caustic, const Intersection &its, const Medium *medium,
const Spectrum &weight) {
const ProjectiveCamera *camera = static_cast<const ProjectiveCamera *>(m_camera.get());
Point2 screenSample;
if (camera->positionToSample(its.p, screenSample)) {
Point cameraPosition = camera->getPosition(screenSample);
Float t = dot(camera->getImagePlaneNormal(), its.p-cameraPosition);
if (t < camera->getNearClip() || t > camera->getFarClip())
return;
if (its.isMediumTransition())
medium = its.getTargetMedium(cameraPosition - its.p);
Spectrum transmittance = m_scene->getTransmittance(its.p,
cameraPosition, its.time, medium);
if (transmittance.isZero())
return;
const BSDF *bsdf = its.shape->getBSDF();
Vector wo = cameraPosition - its.p;
Float dist = wo.length(); wo /= dist;
BSDFQueryRecord bRec(its, its.toLocal(wo));
bRec.quantity = EImportance;
Float importance;
if (m_isPerspectiveCamera)
importance = ((const PerspectiveCamera *) camera)->importance(screenSample) / (dist * dist);
else
importance = 1/camera->areaDensity(screenSample);
Vector wi = its.toWorld(its.wi);
/* Prevent light leaks due to the use of shading normals -- [Veach, p. 158] */
Float wiDotGeoN = dot(its.geoFrame.n, wi),
woDotGeoN = dot(its.geoFrame.n, wo);
if (wiDotGeoN * Frame::cosTheta(bRec.wi) <= 0 ||
woDotGeoN * Frame::cosTheta(bRec.wo) <= 0)
return;
/* Adjoint BSDF for shading normals -- [Veach, p. 155] */
Float correction = std::abs(
(Frame::cosTheta(bRec.wi) * woDotGeoN)/
(Frame::cosTheta(bRec.wo) * wiDotGeoN));
/* Splat onto the accumulation buffer */
Ray ray(its.p, wo, 0, dist, its.time);
Spectrum sampleVal = weight * bsdf->fCos(bRec)
* transmittance * (importance * correction);
m_workResult->splat(screenSample, sampleVal, m_filter);
}
}
bool Constraints::maxDistance(Particle& a, Vector p, float d) {
Vector v = p - a.position;
float l = v.length();
if (l > d) {
a.position += v * ((l - d) / l);
return true;
}
return false;
}
Color Object::sample_perfect_specular(const Vector &wo, Vector &wi, const Vector &n, const Point &p, unsigned short *Xi, double &pdf)
{
pdf = 1;
// do not calculante mirror on direct illumination
if(wi.length() < 1.1) return Color();
wi = (wo - n * 2 * Vector::dot(n, wo)).normalize();
return get_color(p) * material.kR;
}
Vector::Vector(const Vector &rhs){
_length=rhs.length();
if(_length>0) {
_data = new double[_length];
memcpy(_data,rhs.data(),_length*sizeof(double) );
} else{
_data=0;
}
}
void TrajectoryWaypoint::convert2Vector(Vector& lv) {
if ( lv.length() != 3 ) {
lv.deletion();
lv.creation(3);
}
lv.setValueData(this->x,1);
lv.setValueData(this->y,2);
lv.setValueData(this->z,3);
}
void Face::calculate_normal() {
Point point1 = edge->vert->point,
point2 = edge->next->vert->point,
point3 = edge->prev->vert->point;
Vector normal = (point2 - point1).cross(point3 - point1);
mem_normal = normal.unit();
mem_area = normal.length();
}
bool
CollisionManager::isIntersecting(const Ray& ray, Entity* entity, Real& distance)
{
// get a pointer to the collision model
ColDet::CollisionModel3D* mColModel = modelMap[entity->getMesh()->getName()];
if(mColModel == NULL) return false;
// set the world transform for the entity
{
Matrix4 world;
entity->getParentSceneNode()->getWorldTransforms(&world);
float fMatrix[16];
fMatrix[0] = world[0][0];
fMatrix[1] = world[1][0];
fMatrix[2] = world[2][0];
fMatrix[3] = world[3][0];
fMatrix[4] = world[0][1];
fMatrix[5] = world[1][1];
fMatrix[6] = world[2][1];
fMatrix[7] = world[3][1];
fMatrix[8] = world[0][2];
fMatrix[9] = world[1][2];
fMatrix[10] = world[2][2];
fMatrix[11] = world[3][2];
fMatrix[12] = world[0][3];
fMatrix[13] = world[1][3];
fMatrix[14] = world[2][3];
fMatrix[15] = world[3][3];
mColModel->setTransform(fMatrix);
}
// convert the ray
float Origin[3], Direction[3];
Origin[0] = ray.getOrigin().x;
Origin[1] = ray.getOrigin().y;
Origin[2] = ray.getOrigin().z;
Direction[0] = ray.getDirection().x;
Direction[1] = ray.getDirection().y;
Direction[2] = ray.getDirection().z;
// check for a collision
bool col = mColModel->rayCollision(Origin, Direction);
// for testing purposes
// mColModel->getCollidingTriangles(t1, t2, false);
// mColModel->getCollisionPoint(colPoint, false);
float collisionPoint[3];
mColModel->getCollisionPoint(collisionPoint, false);
Vector displacement = ray.getOrigin() - Vector(collisionPoint[0], collisionPoint[1], collisionPoint[2]);
distance = displacement.length();
return col;
}
void sample(const Point &p, LuminaireSamplingRecord &lRec,
const Point2 &sample) const {
Vector lumToP = p - m_position;
Float invDist = 1.0f / lumToP.length();
lRec.sRec.p = m_position;
lRec.d = lumToP * invDist;
lRec.pdf = 1.0f;
lRec.value = m_intensity * (invDist*invDist);
}
void computeQuantities(const Ipopt::Number *x, const bool new_x)
{
if (new_x)
{
for (size_t i=0; i<v.length(); i++)
v[i]=x[i];
delta_x=xr-(x0+dt*(J0*v));
}
}
void computeQuantities(const Ipopt::Number *x, const bool new_x)
{
if (new_x)
{
for (size_t i=0; i<v.length(); i++)
v[i]=x[i]; //x are the primal variables optimized by Ipopt - in this case they are the joint velocities
delta_x=xr-(x0+dt*(J0*v)); //difference between target position and new end-eff position
}
}
T
vector_sum(Vector<T, Block> v)
{
// std::cout << "vector_sum(" << Block_name<Block>::name() << ")\n";
T total = T();
for (index_type i=0; i<v.length(); ++i)
total += v.get(i);
return total;
}
void Calibrator::update(int time, Vector v)
{
const float SIMILARITY = 0.8f; /* 36 degrees' deviation, 10 vectors per circle */
float vl = v.length();
if (vl == 0.0f) {
return;
}
Vector nv = v.divide(vl);
/* Require sampled vectors to point to fairly different directions
* before accepting another. */
float similarity = nv.dot(old_nv);
if (similarity > SIMILARITY) {
return;
}
old_nv = nv;
/* Check if we already have a vector nearly to same direction,
* if so replace that one. This helps in not destroying our
* history of vectors if user just jiggles device back and forth. */
for (int i = 0; i < PCR; i ++) {
if (point_cloud[i].time < time - validity) {
idx = i;
break;
}
Vector c = point_cloud[idx].v;
Vector nc = c.divide(c.length());
float similarity = nv.dot(nc);
if (similarity > SIMILARITY) {
idx = i;
break;
}
}
point_cloud[idx].time = time;
point_cloud[idx].v = v;
/* Round-robin vector reuse */
idx = (idx + 1) & (PCR - 1);
}
