本文整理汇总了C++中Vector3r类的典型用法代码示例。如果您正苦于以下问题:C++ Vector3r类的具体用法?C++ Vector3r怎么用?C++ Vector3r使用的例子?那么恭喜您, 这里精选的类代码示例或许可以为您提供帮助。
在下文中一共展示了Vector3r类的20个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于我们的系统推荐出更棒的C++代码示例。
示例1: operator
bool operator()(const Vector3r& pt, Real pad=0.) const {
Real u=(pt.dot(c12)-c1.dot(c12))/(ht*ht); // normalized coordinate along the c1--c2 axis
if((u*ht<0+pad) || (u*ht>ht-pad)) return false; // out of cylinder along the axis
Real axisDist=((pt-c1).cross(pt-c2)).norm()/ht;
if(axisDist>radius-pad) return false;
return true;
}
开发者ID:DEMANY,项目名称:trunk,代码行数:7,代码来源:_packPredicates.cpp
示例2:
// http://en.wikipedia.org/wiki/Inertia_tensor_of_triangle
Matrix3r woo::Volumetric::triangleInertia(const Vector3r& v0, const Vector3r& v1, const Vector3r& v2){
Matrix3r V; V<<v0.transpose(),v1.transpose(),v2.transpose(); // rows!
Real a=((v1-v0).cross(v2-v0)).norm(); // twice the triangle area
Matrix3r S; S<<2,1,1, 1,2,1, 1,1,2; S*=(1/24.);
Matrix3r C=a*V.transpose()*S*V;
return Matrix3r::Identity()*C.trace()-C;
};
开发者ID:Azeko2xo,项目名称:woodem,代码行数:8,代码来源:Volumetric.cpp
示例3: in
/*
The following code is based on GPL-licensed K-3d importer module from
https://github.com/K-3D/k3d/blob/master/modules/stl_io/mesh_reader.cpp
TODO: read color/material, convert to scalar color in Woo
*/
vector<shared_ptr<Particle>> DemFuncs::importSTL(const string& filename, const shared_ptr<Material>& mat, int mask, Real color, Real scale, const Vector3r& shift, const Quaternionr& ori, Real threshold, Real maxBox, bool readColors, bool flex, Real thickness){
vector<shared_ptr<Particle>> ret;
std::ifstream in(filename,std::ios::in|std::ios::binary);
if(!in) throw std::runtime_error("Error opening "+filename+" for reading (STL import).");
char buffer[80]; in.read(buffer, 80); in.seekg(0, std::ios::beg);
bool isAscii=boost::algorithm::starts_with(buffer,"solid");
// linear array of vertices, each triplet is one face
// this is filled from ASCII and binary formats as intermediary representation
// coordinate system change using (ori, scale, shift) is done already when reading vertices from the file
vector<Vector3r> vertices;
if(isAscii){
LOG_TRACE("STL: ascii format detected");
string lineBuf;
long lineNo=-1;
int fVertsNum=0; // number of vertices in this facet (for checking)
for(std::getline(in,lineBuf); in; getline(in,lineBuf)){
lineNo++;
string tok;
std::istringstream line(lineBuf);
line>>tok;
if(tok=="facet"){
string tok2; line>>tok2;
if(tok2!="normal") LOG_WARN("STL: 'normal' expected after 'facet' (line "+to_string(lineNo)+")");
// we ignore normal values:
// Vector3r normal; line>>normal.x(); line>>normal.y(); line>>normal.z();
} else if(tok=="vertex"){
Vector3r p; line>>p.x(); line>>p.y(); line>>p.z();
vertices.push_back(ori*(p*scale+shift));
fVertsNum++;
} else if(tok=="endfacet"){
开发者ID:einoo,项目名称:woodem,代码行数:39,代码来源:Funcs.cpp
示例4: initConstraint
bool DihedralConstraint::initConstraint(SimulationModel &model, const unsigned int particle1, const unsigned int particle2,
const unsigned int particle3, const unsigned int particle4)
{
m_bodies[0] = particle1;
m_bodies[1] = particle2;
m_bodies[2] = particle3;
m_bodies[3] = particle4;
ParticleData &pd = model.getParticles();
const Vector3r &p0 = pd.getPosition0(particle1);
const Vector3r &p1 = pd.getPosition0(particle2);
const Vector3r &p2 = pd.getPosition0(particle3);
const Vector3r &p3 = pd.getPosition0(particle4);
Vector3r e = p3 - p2;
Real elen = e.norm();
if (elen < 1e-6)
return false;
Real invElen = 1.0 / elen;
Vector3r n1 = (p2 - p0).cross(p3 - p0); n1 /= n1.squaredNorm();
Vector3r n2 = (p3 - p1).cross(p2 - p1); n2 /= n2.squaredNorm();
n1.normalize();
n2.normalize();
Real dot = n1.dot(n2);
if (dot < -1.0) dot = -1.0;
if (dot > 1.0) dot = 1.0;
m_restAngle = acos(dot);
return true;
}
开发者ID:PeterZs,项目名称:PositionBasedDynamics,代码行数:35,代码来源:Constraints.cpp
示例5: notInNotch
notInNotch(const Vector3r& _c, const Vector3r& _edge, const Vector3r& _normal, Real _aperture){
c=_c;
edge=_edge; edge.normalize();
normal=_normal; normal-=edge*edge.dot(normal); normal.normalize();
inside=edge.cross(normal);
aperture=_aperture;
// LOG_DEBUG("edge="<<edge<<", normal="<<normal<<", inside="<<inside<<", aperture="<<aperture);
}
开发者ID:DEMANY,项目名称:trunk,代码行数:8,代码来源:_packPredicates.cpp
示例6: FOREACH
// function used to check if the newly broken bond belongs in a cluster or not, if so attach to proper cluster and set appropriate flags
void Law2_ScGeom_JCFpmPhys_JointedCohesiveFrictionalPM::checkForCluster(JCFpmPhys* phys, ScGeom* geom, Body* b1, Body* b2, Interaction* contact){
const shared_ptr<Shape>& sphere1 = b1->shape;
const shared_ptr<Shape>& sphere2 = b2->shape;
const Real sphereRadius1 = static_cast<Sphere*>(sphere1.get())->radius;
const Real sphereRadius2 = static_cast<Sphere*>(sphere2.get())->radius;
const Real avgDiameter = (sphereRadius1+sphereRadius2);
Vector3r& brokenInteractionLocation = geom->contactPoint;
phys->nearbyFound=0;
FOREACH(const shared_ptr<Interaction>& I, *scene->interactions){
//#endif
JCFpmPhys* nearbyPhys;
const ScGeom* nearbyGeom;
if (!I || !I->geom.get() || !I->phys.get()) continue;
if (I && I->isReal() && JCFpmPhys::getClassIndexStatic()==I->phys->getClassIndex()){
nearbyPhys = YADE_CAST<JCFpmPhys*>(I->phys.get());
if (!nearbyPhys) continue;
if (I->geom.get() /*&& !nearbyPhys->momentBroken*/){
nearbyGeom = YADE_CAST<ScGeom*> (I->geom.get());
if (!nearbyGeom) continue;
Vector3r nearbyInteractionLocation = nearbyGeom->contactPoint;
Vector3r proximityVector = nearbyInteractionLocation-brokenInteractionLocation;
Real proximity = proximityVector.norm();
// this logic is finding interactions within a radius of the broken one, and identifiying if it is an original event or if it belongs in a cluster
if (proximity < avgDiameter*momentRadiusFactor && proximity != 0){
if (nearbyPhys->originalClusterEvent && !nearbyPhys->momentCalculated && !phys->clusteredEvent && clusterMoments) {
phys->eventNumber = nearbyPhys->eventNumber;
phys->clusteredEvent = true;
phys->originalEvent = I.get();
} else if (nearbyPhys->clusteredEvent && !phys->clusteredEvent && clusterMoments){
JCFpmPhys* originalPhys = YADE_CAST<JCFpmPhys*>(nearbyPhys->originalEvent->phys.get());
if (!originalPhys->momentCalculated){
phys->eventNumber = nearbyPhys->eventNumber;
phys->clusteredEvent = true;
phys->originalEvent = nearbyPhys->originalEvent;
}
}
if (nearbyPhys->momentBroken) continue;
phys->nearbyInts.push_back(I.get());
}
}
}
}
if (!phys->clusteredEvent) {
phys->originalClusterEvent = true; // if not clustered, its an original event. We use this interaction as the master for the cluster. Its list of nearbyInts will expand if other ints break nearby.
phys->eventBeginTime=scene->time;
phys->originalEvent = contact;
eventNumber += 1;
phys->eventNumber = eventNumber;
}
phys->checkedForCluster = true;
}
开发者ID:Kubeu,项目名称:trunk,代码行数:56,代码来源:JointedCohesiveFrictionalPM.cpp
示例7: gradC_i
// ----------------------------------------------------------------------------------------------
bool PositionBasedFluids::computePBFLagrangeMultiplier(
const unsigned int particleIndex,
const unsigned int numberOfParticles,
const Vector3r x[],
const Real mass[],
const Vector3r boundaryX[],
const Real boundaryPsi[],
const Real density,
const unsigned int numNeighbors,
const unsigned int neighbors[],
const Real density0,
const bool boundaryHandling,
Real &lambda)
{
const Real eps = 1.0e-6;
// Evaluate constraint function
const Real C = std::max(density / density0 - 1.0, 0.0); // clamp to prevent particle clumping at surface
if (C != 0.0)
{
// Compute gradients dC/dx_j
Real sum_grad_C2 = 0.0;
Vector3r gradC_i(0.0, 0.0, 0.0);
for (unsigned int j = 0; j < numNeighbors; j++)
{
const unsigned int neighborIndex = neighbors[j];
if (neighborIndex < numberOfParticles) // Test if fluid particle
{
const Vector3r gradC_j = -mass[neighborIndex] / density0 * CubicKernel::gradW(x[particleIndex] - x[neighborIndex]);
sum_grad_C2 += gradC_j.squaredNorm();
gradC_i -= gradC_j;
}
else if (boundaryHandling)
{
// Boundary: Akinci2012
const Vector3r gradC_j = -boundaryPsi[neighborIndex - numberOfParticles] / density0 * CubicKernel::gradW(x[particleIndex] - boundaryX[neighborIndex - numberOfParticles]);
sum_grad_C2 += gradC_j.squaredNorm();
gradC_i -= gradC_j;
}
}
sum_grad_C2 += gradC_i.squaredNorm();
// Compute lambda
lambda = -C / (sum_grad_C2 + eps);
}
else
lambda = 0.0;
return true;
}
开发者ID:PeterZs,项目名称:PositionBasedDynamics,代码行数:54,代码来源:PositionBasedFluids.cpp
示例8: checkFence
void SafetyEval::isSafeDestination(const Vector3r& dest_pos, const Vector3r& cur_pos, const Quaternionr& quaternion, SafetyEval::EvalResult& result)
{
//this function should work even when dest_pos == cur_pos
result.dest_pos = dest_pos;
result.cur_pos = cur_pos;
//is this dest_pos cur_pos within the fence?
checkFence(dest_pos, cur_pos, result);
if (!(enable_reasons_ & SafetyViolationType_::Obstacle))
return;
//transform dest_pos vector to body frame
const Vector3r cur_dest = dest_pos - cur_pos;
float cur_dest_norm = cur_dest.norm();
//check for approx zero vectors to avoid random yaw angles
if (cur_dest_norm < vehicle_params_.distance_accuracy) {
//we are hovering
result.dest_risk_dist = Utils::nan<float>();
isCurrentSafer(result);
}
else { //see if we have obstacle in direction
result.cur_dest_body = VectorMath::transformToBodyFrame(cur_dest, quaternion, true);
//get yaw in body frame, ie, front is always 0 radians
float point_angle = std::atan2(result.cur_dest_body[1], result.cur_dest_body[0]);
//yaw to ticks
int point_tick = obs_xy_ptr_->angleToTick(point_angle);
//get obstacles in the window at the tick direction around the window
result.dest_obs = obs_xy_ptr_->hasObstacle(point_tick - vehicle_params_.obs_window, point_tick + vehicle_params_.obs_window);
//less risk distance is better
result.dest_risk_dist = cur_dest_norm + adjustClearanceForPrStl(vehicle_params_.obs_clearance, result.dest_obs.confidence) - result.dest_obs.distance;
if (result.dest_risk_dist >= 0) { //potential collision
//check obstacles around current position and see if it has lower risk
isCurrentSafer(result);
}
//else obstacle is too far
}
//if we detected unsafe condition due to obstacle, find direction to move away to
if (!result.is_safe && result.reason & SafetyViolationType_::Obstacle) {
//look for each surrounding tick to see if we have obstacle free angle
setSuggestedVelocity(result, quaternion);
Utils::log(Utils::stringf("isSafeDestination: cur_dest_norm=%f, result=%s, quaternion=%s",
cur_dest_norm, result.toString().c_str(), VectorMath::toString(quaternion, true).c_str()));
}
//else no suggestions required
}
开发者ID:5guo,项目名称:AirSim,代码行数:53,代码来源:SafetyEval.cpp
示例9: a
bool Law2_ScGeom_ElectrostaticPhys::go(shared_ptr<IGeom>& iGeom, shared_ptr<IPhys>& iPhys, Interaction* interaction)
{
// Physic
ElectrostaticPhys* phys=static_cast<ElectrostaticPhys*>(iPhys.get());
// Geometry
ScGeom* geom=static_cast<ScGeom*>(iGeom.get());
// Get bodies properties
Body::id_t id1 = interaction->getId1();
Body::id_t id2 = interaction->getId2();
const shared_ptr<Body> b1 = Body::byId(id1,scene);
const shared_ptr<Body> b2 = Body::byId(id2,scene);
State* s1 = b1->state.get();
State* s2 = b2->state.get();
// geometric parameters
Real a((geom->radius1+geom->radius2)/2.);
bool isNew=false;
// Speeds
Vector3r shiftVel=scene->isPeriodic ? Vector3r(scene->cell->velGrad*scene->cell->hSize*interaction->cellDist.cast<Real>()) : Vector3r::Zero();
Vector3r shift2 = scene->isPeriodic ? Vector3r(scene->cell->hSize*interaction->cellDist.cast<Real>()): Vector3r::Zero();
Vector3r relV = geom->getIncidentVel(s1, s2, scene->dt, shift2, shiftVel, false );
Real undot = relV.dot(geom->normal); // Normal velocity norm
if(-geom->penetrationDepth > a && -geom->penetrationDepth > 10.*phys->DebyeLength) { return false; }
// inititalization
if(phys->u == -1. ) {phys->u = -geom->penetrationDepth; isNew=true;}
// Solve normal
normalForce_DL_Adim(phys,geom, undot,isNew);
// Solve shear and torques
Vector3r C1 = Vector3r::Zero();
Vector3r C2 = Vector3r::Zero();
computeShearForceAndTorques_log(phys, geom, s1, s2, C1, C2);
// Apply!
scene->forces.addForce(id1,phys->normalForce+phys->shearForce);
scene->forces.addTorque(id1,C1);
scene->forces.addForce(id2,-(phys->normalForce+phys->shearForce));
scene->forces.addTorque(id2,C2);
return true;
}
开发者ID:yade,项目名称:trunk,代码行数:50,代码来源:ElectrostaticMat.cpp
示例10: inParallelepiped
inParallelepiped(const Vector3r& o, const Vector3r& a, const Vector3r& b, const Vector3r& c){
Vector3r A(o), B(a), C(a+(b-o)), D(b), E(c), F(c+(a-o)), G(c+(a-o)+(b-o)), H(c+(b-o));
Vector3r x(B-A), y(D-A), z(E-A);
n[0]=-y.cross(z).normalized(); n[1]=-n[0]; pts[0]=A; pts[1]=B;
n[2]=-z.cross(x).normalized(); n[3]=-n[2]; pts[2]=A; pts[3]=D;
n[4]=-x.cross(y).normalized(); n[5]=-n[4]; pts[4]=A; pts[5]=E;
// bounding box
Vector3r vertices[8]={A,B,C,D,E,F,G,H};
mn=mx=vertices[0];
for(int i=1; i<8; i++){
mn=mn.cwiseMin(vertices[i]);
mx=mx.cwiseMax(vertices[i]);
}
}
开发者ID:DEMANY,项目名称:trunk,代码行数:14,代码来源:_packPredicates.cpp
示例11: GenerateCloud_water
string GenerateCloud_water(vector<BasicSphere>& sphere_list, Vector3r lowerCorner, Vector3r upperCorner, long number, Real rad_std_dev, Real porosity)
{
typedef boost::minstd_rand StdGenerator;
static StdGenerator generator;
static boost::variate_generator<StdGenerator&, boost::uniform_real<> >
random1(generator, boost::uniform_real<>(0,1));
// static boost::variate_generator<StdGenerator&, boost::normal_distribution<> >
// randomN(generator, boost::normal_distribution<>(aggregateMeanRadius,aggregateSigmaRadius));
sphere_list.clear();
long tries = 1000; //nb of tries for positionning the next sphere
Vector3r dimensions = upperCorner - lowerCorner;
Real mean_radius = std::pow(dimensions.x()*dimensions.y()*dimensions.z()*(1-porosity)/(3.1416*1.3333*number),0.333333);
//cerr << mean_radius;
Real Rmin=mean_radius, Rmax=mean_radius;
std::cerr << "generating aggregates ... ";
long t, i;
for (i=0; i<number; ++i) {
BasicSphere s;
s.second = (random1()-0.5)*rad_std_dev*mean_radius+mean_radius;
for (t=0; t<tries; ++t) {
s.first.x() = lowerCorner.x()+s.second+(dimensions.x()-2*s.second)*random1();
s.first.y() = lowerCorner.y()+s.second+(dimensions.y()-2*s.second)*random1();
s.first.z() = lowerCorner.z()+s.second+(dimensions.z()-2*s.second)*random1();
bool overlap=false;
for (long j=0; (j<i && !overlap); j++)
if ( pow(sphere_list[j].second+s.second, 2) > (sphere_list[j].first-s.first).squaredNorm()) overlap=true;
if (!overlap){
sphere_list.push_back(s);
Rmin = std::min(Rmin,s.second);
Rmax = std::max(Rmax,s.second);
break;}
}
if (t==tries) return "More than " + lexical_cast<string>(tries) +
" tries while generating sphere number " +
lexical_cast<string>(i+1) + "/" + lexical_cast<string>(number) + ".";
}
return "Generated a sample with " + lexical_cast<string>(number) + "spheres inside box of dimensions: ("
+ lexical_cast<string>(dimensions[0]) + ","
+ lexical_cast<string>(dimensions[1]) + ","
+ lexical_cast<string>(dimensions[2]) + ")."
+ " mean radius=" + lexical_cast<string>(mean_radius) +
+ " Rmin =" + lexical_cast<string>(Rmin) +
+ " Rmax =" + lexical_cast<string>(Rmax) + ".";
}
开发者ID:8803104,项目名称:trunk,代码行数:48,代码来源:CapillaryTriaxialTest.cpp
示例12: mat
void SimpleShear::createSphere(shared_ptr<Body>& body, Vector3r position, Real radius)
{
body = shared_ptr<Body>(new Body); body->groupMask=1;
shared_ptr<NormalInelasticMat> mat(new NormalInelasticMat);
shared_ptr<Aabb> aabb(new Aabb);
shared_ptr<Sphere> iSphere(new Sphere);
body->state->pos =position;
body->state->ori =Quaternionr::Identity();
body->state->vel =Vector3r(0,0,0);
body->state->angVel =Vector3r(0,0,0);
Real masse =4.0/3.0*Mathr::PI*radius*radius*radius*density;
body->state->mass =masse;
body->state->inertia = Vector3r(2.0/5.0*masse*radius*radius,2.0/5.0*masse*radius*radius,2.0/5.0*masse*radius*radius);
mat->young = sphereYoungModulus;
mat->poisson = spherePoissonRatio;
mat->frictionAngle = sphereFrictionDeg * Mathr::PI/180.0;
body->material = mat;
aabb->color = Vector3r(0,1,0);
iSphere->radius = radius;
iSphere->color = ((int)(floor(8*position.x()/length)))%2?Vector3r(0.7,0.7,0.7):Vector3r(0.45,0.45,0.45);// so that we have eight different colour bands
body->shape = iSphere;
body->bound = aabb;
}
开发者ID:Papachristos,项目名称:trunk,代码行数:29,代码来源:SimpleShear.cpp
示例13: if
// ----------------------------------------------------------------------------------------------
bool PositionBasedFluids::solveDensityConstraint(
const unsigned int particleIndex,
const unsigned int numberOfParticles,
const Vector3r x[],
const Real mass[],
const Vector3r boundaryX[],
const Real boundaryPsi[],
const unsigned int numNeighbors,
const unsigned int neighbors[],
const Real density0,
const bool boundaryHandling,
const Real lambda[],
Vector3r &corr)
{
// Compute position correction
corr.setZero();
for (unsigned int j = 0; j < numNeighbors; j++)
{
const unsigned int neighborIndex = neighbors[j];
if (neighborIndex < numberOfParticles) // Test if fluid particle
{
const Vector3r gradC_j = -mass[neighborIndex] / density0 * CubicKernel::gradW(x[particleIndex] - x[neighborIndex]);
corr -= (lambda[particleIndex] + lambda[neighborIndex]) * gradC_j;
}
else if (boundaryHandling)
{
// Boundary: Akinci2012
const Vector3r gradC_j = -boundaryPsi[neighborIndex - numberOfParticles] / density0 * CubicKernel::gradW(x[particleIndex] - boundaryX[neighborIndex - numberOfParticles]);
corr -= (lambda[particleIndex]) * gradC_j;
}
}
return true;
}
开发者ID:PeterZs,项目名称:PositionBasedDynamics,代码行数:35,代码来源:PositionBasedFluids.cpp
示例14: GenerateCloud
string SimpleShear::GenerateCloud(vector<BasicSphere>& sphere_list,Vector3r lowerCorner,Vector3r upperCorner,long number,Real rad_std_dev, Real porosity)
{
sphere_list.clear();
long tries = 1000; //nb max of tries for positionning the next sphere
Vector3r dimensions = upperCorner - lowerCorner;
Real mean_radius = pow(dimensions.x()*dimensions.y()*dimensions.z()*(1-porosity)/(4.0/3.0*Mathr::PI*number),1.0/3.0);
cerr << " mean radius " << mean_radius << endl;;
// std::cerr << "generating aggregates ... ";
long t, i;
for (i=0; i<number; ++i)
{
BasicSphere s;
for (t=0; t<tries; ++t)
{
s.second = (Mathr::UnitRandom()-0.5)*rad_std_dev*mean_radius+mean_radius;
s.first.x() = lowerCorner.x()+s.second+(dimensions.x()-2*s.second)*Mathr::UnitRandom();
s.first.y() = lowerCorner.y()+s.second+(dimensions.y()-2*s.second)*Mathr::UnitRandom();
s.first.z() = lowerCorner.z()+s.second+(dimensions.z()-2*s.second)*Mathr::UnitRandom();
bool overlap=false;
for (long j=0; (j<i && !overlap); j++)
if ( pow(sphere_list[j].second+s.second, 2) > (sphere_list[j].first-s.first).squaredNorm()) overlap=true;
if (!overlap)
{
sphere_list.push_back(s);
// cout << "j'ai bien rajoute une sphere dans la liste" << endl;
break;
}
}
if (t==tries)
{
string str1="Generated a sample with " + boost::lexical_cast<string>(i) + " spheres inside box of dimensions: ("
+ boost::lexical_cast<string>(dimensions[0]) + ","
+ boost::lexical_cast<string>(dimensions[1]) + ","
+ boost::lexical_cast<string>(dimensions[2]) + ").\n";
return str1 + "More than " + boost::lexical_cast<string>(tries) + " tries while generating sphere number " +
boost::lexical_cast<string>(i+1) + "/" + boost::lexical_cast<string>(number) + ".";
}
}
return "Generated a sample with " + boost::lexical_cast<string>(number) + " spheres inside box of dimensions: ("
+ boost::lexical_cast<string>(dimensions[0]) + ","
+ boost::lexical_cast<string>(dimensions[1]) + ","
+ boost::lexical_cast<string>(dimensions[2]) + ").";
}
开发者ID:Papachristos,项目名称:trunk,代码行数:45,代码来源:SimpleShear.cpp
示例15: updateEnergy
void NewtonIntegrator::updateEnergy(const shared_ptr<Body>& b, const State* state, const Vector3r& fluctVel, const Vector3r& f, const Vector3r& m){
assert(b->isStandalone() || b->isClump());
// always positive dissipation, by-component: |F_i|*|v_i|*damping*dt (|T_i|*|ω_i|*damping*dt for rotations)
if(damping!=0. && state->isDamped){
scene->energy->add(fluctVel.cwise().abs().dot(f.cwise().abs())*damping*scene->dt,"nonviscDamp",nonviscDampIx,/*non-incremental*/false);
// when the aspherical integrator is used, torque is damped instead of ang acceleration; this code is only approximate
scene->energy->add(state->angVel.cwise().abs().dot(m.cwise().abs())*damping*scene->dt,"nonviscDamp",nonviscDampIx,false);
}
// kinetic energy
Real Etrans=.5*state->mass*fluctVel.squaredNorm();
Real Erot;
// rotational terms
if(b->isAspherical()){
Matrix3r mI; mI<<state->inertia[0],0,0, 0,state->inertia[1],0, 0,0,state->inertia[2];
Matrix3r T(state->ori);
Erot=.5*b->state->angVel.transpose().dot((T.transpose()*mI*T)*b->state->angVel);
} else { Erot=0.5*state->angVel.dot(state->inertia.cwise()*state->angVel); }
if(!kinSplit) scene->energy->add(Etrans+Erot,"kinetic",kinEnergyIx,/*non-incremental*/true);
else{ scene->energy->add(Etrans,"kinTrans",kinEnergyTransIx,true); scene->energy->add(Erot,"kinRot",kinEnergyRotIx,true); }
}
开发者ID:HumaNait,项目名称:trunk,代码行数:20,代码来源:NewtonIntegrator.cpp
示例16: getDestination
Vector3r SafetyEval::getDestination(const Vector3r& cur_pos, const Vector3r& velocity) const
{
//breaking distance at this velocity
float velocity_mag = velocity.norm();
float dest_pos_dist = std::max(velocity_mag * vehicle_params_.vel_to_breaking_dist,
vehicle_params_.min_breaking_dist);
//calculate dest_pos cur_pos we will be if we had to break suddenly
return velocity_mag >= vehicle_params_.distance_accuracy ?
(cur_pos + (velocity / velocity_mag) * dest_pos_dist) : cur_pos;
}
开发者ID:5guo,项目名称:AirSim,代码行数:11,代码来源:SafetyEval.cpp
示例17: _gts_face_to_stl
void _gts_face_to_stl(GtsTriangle* t,_gts_face_to_stl_data* data){
GtsVertex* v[3];
Vector3r n;
gts_triangle_vertices(t,&v[0],&v[1],&v[2]);
if(data->clipCell && data->scene->isPeriodic){
for(short ax:{0,1,2}){
Vector3r p(GTS_POINT(v[ax])->x,GTS_POINT(v[ax])->y,GTS_POINT(v[ax])->z);
if(!data->scene->cell->isCanonical(p)) return;
}
}
gts_triangle_normal(t,&n[0],&n[1],&n[2]);
n.normalize();
std::ofstream& stl(data->stl);
stl<<" facet normal "<<n[0]<<" "<<n[1]<<" "<<n[2]<<"\n";
stl<<" outer loop\n";
for(GtsVertex* _v: v){
stl<<" vertex "<<GTS_POINT(_v)->x<<" "<<GTS_POINT(_v)->y<<" "<<GTS_POINT(_v)->z<<"\n";
}
stl<<" endloop\n";
stl<<" endfacet\n";
data->numTri+=3;
}
开发者ID:CrazyHeex,项目名称:woo,代码行数:22,代码来源:_triangulated.cpp
示例18: make_tuple
std::tuple<vector<shared_ptr<Node>>,vector<shared_ptr<Particle>>> SphereClumpGeom::makeParticles(const shared_ptr<Material>& mat, const Vector3r& clumpPos, const Quaternionr& clumpOri, int mask, Real scale){
ensureOk();
assert(centers.size()==radii.size());
const auto N=centers.size();
if(N==1){ // fast path for a single sphere (do not clump at all)
auto s=DemFuncs::makeSphere(radii[0]*scale,mat);
s->mask=mask;
s->shape->nodes[0]->pos=(isnan(clumpPos.maxCoeff())?centers[0]:clumpPos); // natural or forced position
return std::make_tuple(vector<shared_ptr<Node>>({s->shape->nodes[0]}),vector<shared_ptr<Particle>>({s}));
}
vector<shared_ptr<Particle>> par(N);
auto n=make_shared<Node>();
auto cd=make_shared<ClumpData>();
n->setData<DemData>(cd);
n->pos=(isnan(clumpPos.maxCoeff())?pos:clumpPos);
n->ori=clumpOri;
cd->nodes.resize(N);
cd->relPos.resize(N);
cd->relOri.resize(N);
for(size_t i=0; i<N; i++){
par[i]=DemFuncs::makeSphere(radii[i]*scale,mat);
par[i]->mask=mask;
cd->nodes[i]=par[i]->shape->nodes[0];
cd->nodes[i]->getData<DemData>().setClumped(n); // sets flag and assigned master node
cd->relPos[i]=(centers[i]-pos)*scale;
cd->relOri[i]=ori.conjugate(); // nice to set, but not really important
}
// sets particles in global space based on relPos, relOri
ClumpData::applyToMembers(n);
// set clump properties
assert(!isnan(volume));
cd->setClump(); assert(cd->isClump());
// scale = length scale (but not density scale)
cd->mass=mat->density*volume*pow(scale,3);
cd->inertia=mat->density*inertia*pow(scale,5);
cd->equivRad=equivRad;
return std::make_tuple(vector<shared_ptr<Node>>({n}),par);
}
开发者ID:CrazyHeex,项目名称:woo,代码行数:38,代码来源:Clump.cpp
示例19: ori
void Leapfrog::leapfrogAsphericalRotate(const shared_ptr<Node>& node, const Vector3r& M){
Quaternionr& ori(node->ori); DemData& dyn(node->getData<DemData>());
Vector3r& angMom(dyn.angMom); Vector3r& angVel(dyn.angVel); const Vector3r& inertia(dyn.inertia);
// initialize angular momentum; it is in local coordinates, therefore angVel must be rotated
if(isnan(angMom.minCoeff())){
angMom=dyn.inertia.asDiagonal()*node->ori.conjugate()*dyn.angVel;
}
Matrix3r A=ori.conjugate().toRotationMatrix(); // rotation matrix from global to local r.f.
const Vector3r l_n = angMom + dt/2 * M; // global angular momentum at time n
const Vector3r l_b_n = A*l_n; // local angular momentum at time n
const Vector3r angVel_b_n = (l_b_n.array()/inertia.array()).matrix(); // local angular velocity at time n
const Quaternionr dotQ_n=DotQ(angVel_b_n,ori); // dQ/dt at time n
const Quaternionr Q_half = ori + dt/2 * dotQ_n; // Q at time n+1/2
angMom+=dt*M; // global angular momentum at time n+1/2
const Vector3r l_b_half = A*angMom; // local angular momentum at time n+1/2
Vector3r angVel_b_half = (l_b_half.array()/inertia.array()).matrix(); // local angular velocity at time n+1/2
const Quaternionr dotQ_half=DotQ(angVel_b_half,Q_half); // dQ/dt at time n+1/2
ori=ori+dt*dotQ_half; // Q at time n+1
angVel=ori*angVel_b_half; // global angular velocity at time n+1/2
ori.normalize();
}
开发者ID:CrazyHeex,项目名称:woo,代码行数:23,代码来源:Leapfrog.cpp
示例20: setSafety
//float/vec parameters can have NaN which makes them optional
void SafetyEval::setSafety(SafetyViolationType enable_reasons, float obs_clearance, SafetyEval::ObsAvoidanceStrategy obs_strategy,
const Vector3r& origin, float xy_length, float max_z, float min_z)
{
if (!origin.hasNaN() && !std::isnan(xy_length) && !std::isnan(max_z) && !std::isnan(min_z))
fence_ptr_->setBoundry(origin, xy_length, max_z, min_z);
if (!std::isnan(obs_clearance))
vehicle_params_.obs_clearance = obs_clearance;
enable_reasons_ = enable_reasons;
setObsAvoidanceStrategy(obs_strategy);
Utils::log(Utils::stringf("enable_reasons: %X", uint(enable_reasons)));
}
开发者ID:5guo,项目名称:AirSim,代码行数:15,代码来源:SafetyEval.cpp
注:本文中的Vector3r类示例由纯净天空整理自Github/MSDocs等源码及文档管理平台,相关代码片段筛选自各路编程大神贡献的开源项目,源码版权归原作者所有,传播和使用请参考对应项目的License;未经允许,请勿转载。 |
请发表评论