int ofxDelaunay::triangulate(vector<int> *indices ){
if(vertices.size() < 3){return NULL;}
// make clone not to destroy vertices
vector<XYZI> verticesTemp;
if(indices!=nullptr){
verticesTemp.resize(indices->size());
int j = 0;
for (auto & i:*indices){
verticesTemp[j] = vertices[i];
j++;
}
}
else{verticesTemp=vertices;}
qsort( &verticesTemp[0], verticesTemp.size(), sizeof( XYZI ), XYZICompare );
int nv = verticesTemp.size();
//vertices required for Triangulate
vector<XYZ> verticesXYZ(nv);
//copy XYZIs to XYZ
for (int i = 0; i < nv; i++) {
XYZ v;
verticesXYZ[i].x = verticesTemp.at(i).x;
verticesXYZ[i].y = verticesTemp.at(i).y;
verticesXYZ[i].z = verticesTemp.at(i).z;
}
//add 3 emptly slots, required by the Triangulate call
verticesXYZ.push_back(XYZ());
verticesXYZ.push_back(XYZ());
verticesXYZ.push_back(XYZ());
//allocate space for triangle indices
triangles.resize(3*nv);
Triangulate( nv, &verticesXYZ[0], &triangles[0], ntri );
//copy triangle data to ofxDelaunayTriangle.
triangleMesh.clear();
triangleMesh.setMode(OF_PRIMITIVE_TRIANGLES);
//copy vertices
for (int i = 0; i < vertices.size(); i++){
triangleMesh.addVertex(ofVec3f(vertices[i].x,vertices[i].y,vertices[i].z));
}
//copy triangles
for(int i = 0; i < ntri; i++){
triangleMesh.addIndex(verticesTemp.at(triangles[ i ].p1).i);
triangleMesh.addIndex(verticesTemp.at(triangles[ i ].p2).i);
triangleMesh.addIndex(verticesTemp.at(triangles[ i ].p3).i);
}
return ntri;
}
Float AngleTripletScore::evaluate_index(kernel::Model *m,
const kernel::ParticleIndexTriplet &pi,
DerivativeAccumulator *da) const {
IMP_CHECK_OBJECT(f_.get());
XYZ d0 = XYZ(m, pi[0]);
XYZ d1 = XYZ(m, pi[1]);
XYZ d2 = XYZ(m, pi[2]);
Float score;
if (da) {
algebra::Vector3D derv0, derv1, derv2;
double angle = internal::angle(d0, d1, d2, &derv0, &derv1, &derv2);
Float deriv;
boost::tie(score, deriv) = f_->evaluate_with_derivative(angle);
d0.add_to_derivatives(derv0 * deriv, *da);
d1.add_to_derivatives(derv1 * deriv, *da);
d2.add_to_derivatives(derv2 * deriv, *da);
} else {
double angle = internal::angle(d0, d1, d2, nullptr, nullptr, nullptr);
score = f_->evaluate(angle);
}
return score;
}
int ofxDelaunay::triangulate(){
int nv = vertices.size();
if (nv < 1) return 0; // crashes if try to triangulate 0 points
//add 3 emptly slots, required by the Triangulate call
vertices.push_back(XYZ());
vertices.push_back(XYZ());
vertices.push_back(XYZ());
//allocate space for triangle indices
triangles.resize(3*nv);
int ntri;
qsort( &vertices[0], vertices.size()-3, sizeof( XYZ ), XYZCompare );
Triangulate( nv, &vertices[0], &triangles[0], ntri );
// copy triangle data to ofxDelaunayTriangle.
triangleMesh.clear();
//copy vertices
for (int i = 0; i < nv; i++){
triangleMesh.addVertex(ofVec3f(vertices[i].x,vertices[i].y,vertices[i].z));
}
//copy triagles
for(int i = 0; i < ntri; i++){
triangleMesh.addIndex(triangles[ i ].p1);
triangleMesh.addIndex(triangles[ i ].p2);
triangleMesh.addIndex(triangles[ i ].p3);
}
return ntri;
}
void FireBox()
{
Vector vec;
vec = myCamera.target;
vec += XYZ(0,-1,0);
vec -= myCamera.position;
vec.makeUnit();
XYZ pos = myCamera.position;
RigidBox *myRigidBody = new RigidBox(boxObj);
myRigidBody->setPosition(pos);
myRigidBody->setScale(XYZ(1,1,1));
float fMass = 50;
myRigidBody->setMass(fMass);
myRigidBody->buildVertexLightMap();
myScene.bind(*myRigidBody);
float fForce = 15.0f;
btVector3 force = btVector3(vec.x,vec.y,vec.z)*(fForce * 1000.0f);
myRigidBody->getRigidBody().activate(true);
//myRigidBody->getRigidBody()->applyForce(force, btVector3(myCamera.target.x, myCamera.target.y, myCamera.target.z));
myRigidBody->getRigidBody().applyForce(force, btVector3(0,0,0));
}
void CArbitraryEnergyView::PaintSphere(double radius,unsigned char red,unsigned char green,unsigned char blue,bool half)
{
static const int loop_count=100;
for(int longitude_loop=0;longitude_loop<loop_count;longitude_loop++)
{
double start_longitude,end_longitude;
start_longitude=M_PI*2*longitude_loop/loop_count-M_PI;
end_longitude=M_PI*2*(longitude_loop+1)/loop_count-M_PI;
for(int latitude_loop=0;latitude_loop<loop_count;latitude_loop++)
{
if(half)
if(latitude_loop>=loop_count/2)
continue;
double start_latitude,end_latitude;
start_latitude=M_PI*latitude_loop/loop_count-M_PI/2;
end_latitude=M_PI*(latitude_loop+1)/loop_count-M_PI/2;
double x0,y0,z0,x1,y1,z1,x2,y2,z2,x3,y3,z3;
#define XYZ(x,y,z,latitude,longitude) \
x=radius*cos(latitude)*cos(longitude);\
y=radius*sin(latitude)*cos(longitude);\
z=radius*sin(longitude)
XYZ(x0,y0,z0,start_longitude,start_latitude);
XYZ(x1,y1,z1,start_longitude,end_latitude);
XYZ(x2,y2,z2,end_longitude,end_latitude);
XYZ(x3,y3,z3,end_longitude,start_latitude);
#undef XYZ
C3DWindow::Add(x0,y0,z0,red,green,blue,x1,y1,z1,red,green,blue,x2,y2,z2,red,green,blue);
C3DWindow::Add(x2,y2,z2,red,green,blue,x3,y3,z3,red,green,blue,x0,y0,z0,red,green,blue);
}
}
}
XYZ getColor(const SHSample& s) const
{
float r = getFloat(s);
if(r>0)
return XYZ(0,r,0);
else
return XYZ(-r,0,0);
}
double dist_directional_swc_1_2(V3DLONG & nseg1, V3DLONG & nseg1big, double & sum1big, const NeuronTree *p1, const NeuronTree *p2)
{
if (!p1 || !p2) return -1;
V3DLONG p1sz = p1->listNeuron.size(), p2sz = p2->listNeuron.size();
if (p1sz<2 || p2sz<2) return -1;
NeuronSWC *tp1, *tp2;
V3DLONG i, j;
double sum1=0;
nseg1=0;
nseg1big=0;
sum1big=0;
QHash<int, int> h1 = generate_neuron_swc_hash(p1); //generate a hash lookup table from a neuron swc graph
for (i=0;i<p1->listNeuron.size();i++)
{
//first find the two ends of a line seg
tp1 = (NeuronSWC *)(&(p1->listNeuron.at(i)));
if (tp1->pn < 0 || tp1->pn >= p1sz)
continue;
tp2 = (NeuronSWC *)(&(p1->listNeuron.at(h1.value(tp1->pn)))); //use hash table
//qDebug() << "i="<< i << " pn="<<tp1->pn - 1;
//now produce a series of points for the line seg
double len=dist_L2(XYZ(tp1->x,tp1->y,tp1->z), XYZ(tp2->x,tp2->y,tp2->z));
int N = int(1+len+0.5);
XYZ ptdiff;
if (N<=1)
{
qDebug() << "detect one very short segment, len=" << len;
ptdiff = XYZ(0,0,0);
}
else
{
double N1=1.0/(N-1);
ptdiff = XYZ(N1,N1,N1) * XYZ(tp2->x-tp1->x, tp2->y-tp1->y, tp2->z-tp1->z);
}
qDebug() << "N="<<N << "len=" <<len << "xd="<<ptdiff.x << " yd=" << ptdiff.y << " zd=" << ptdiff.z << " ";
for (j=0;j<N;j++)
{
XYZ curpt(tp1->x + ptdiff.x*j, tp1->y + ptdiff.y*j, tp1->z + ptdiff.z*j);
double cur_d = dist_pt_to_swc(curpt, p2);
sum1 += cur_d;
nseg1++;
if (cur_d>=2)
{
sum1big += cur_d;
nseg1big++;
}
}
}
qDebug() << "end directional neuronal distance computing";
return sum1;
}
int ofxDelaunay::triangulate(){
if(vertices.size() < 3){
return NULL;
}
int nv = vertices.size();
// make clone not to destroy vertices
vector<XYZI> verticesTemp = vertices;
qsort( &verticesTemp[0], verticesTemp.size(), sizeof( XYZI ), XYZICompare );
//vertices required for Triangulate
vector<XYZ> verticesXYZ;
//copy XYZIs to XYZ
for (int i = 0; i < nv; i++) {
XYZ v;
v.x = verticesTemp.at(i).x;
v.y = verticesTemp.at(i).y;
v.z = verticesTemp.at(i).z;
verticesXYZ.push_back(v);
}
//add 3 emptly slots, required by the Triangulate call
verticesXYZ.push_back(XYZ());
verticesXYZ.push_back(XYZ());
verticesXYZ.push_back(XYZ());
//allocate space for triangle indices
triangles.resize(3*nv);
Triangulate( nv, &verticesXYZ[0], &triangles[0], ntri );
//copy triangle data to ofxDelaunayTriangle.
triangleMesh.clear();
triangleMesh.setMode(OF_PRIMITIVE_TRIANGLES);
//copy vertices
for (int i = 0; i < nv; i++){
triangleMesh.addVertex(ofVec3f(vertices[i].x,vertices[i].y,vertices[i].z));
}
//copy triangles
for(int i = 0; i < ntri; i++){
triangleMesh.addIndex(verticesTemp.at(triangles[ i ].p1).i);
triangleMesh.addIndex(verticesTemp.at(triangles[ i ].p2).i);
triangleMesh.addIndex(verticesTemp.at(triangles[ i ].p3).i);
}
//erase the last three triangles (missing code from original)
vertices.erase(vertices.end()-1);
vertices.erase(vertices.end()-1);
vertices.erase(vertices.end()-1);
return ntri;
}
double NormalizedSphereDistancePairScore::evaluate_index(
Model *m, const ParticleIndexPair &pip,
DerivativeAccumulator *da) const {
Float ra = m->get_attribute(radius_, pip[0]);
Float rb = m->get_attribute(radius_, pip[1]);
Float mr = std::min(ra, rb);
// lambda is inefficient due to laziness
return internal::evaluate_distance_pair_score(
XYZ(m, pip[0]), XYZ(m, pip[1]), da, f_.get(),
boost::lambda::_1 / mr - (ra + rb) / mr);
}
double WeightedSphereDistancePairScore::evaluate_index(
Model *m, const ParticleIndexPair &p,
DerivativeAccumulator *da) const {
Float ra = m->get_attribute(radius_, p[0]);
Float rb = m->get_attribute(radius_, p[1]);
Float wa = m->get_attribute(weight_, p[0]);
Float wb = m->get_attribute(weight_, p[1]);
// lambda is inefficient due to laziness
return internal::evaluate_distance_pair_score(
XYZ(m, p[0]), XYZ(m, p[1]), da, f_.get(),
(boost::lambda::_1 - (ra + rb)) * (wa + wb));
}
void TriangleMeshViewer::reset()
{
fly_mode=false;
camera_position=XYZ(0.0f,-3.0f,0.0f);
camera_forward=XYZ(0.0f,1.0f,0.0f);
camera_up=XYZ(0.0f,0.0f,1.0f);
camera_velocity=0.0f;
camera_yaw_rate=0.0f;
camera_pitch_rate=0.0f;
camera_roll_rate=0.0f;
tilt_slider->setValue(30);
spinrate_slider->setValue(0);
object_rotation=0.0f;
statusbar->clearMessage();
}
Float NormalizedSphereDistancePairScore::evaluate(const ParticlePair &p,
DerivativeAccumulator *da) const
{
IMP_USAGE_CHECK(p[0]->has_attribute(radius_), "Particle " << p[0]->get_name()
<< "missing radius in NormalizedSphereDistancePairScore");
IMP_USAGE_CHECK(p[1]->has_attribute(radius_), "Particle " << p[1]->get_name()
<< "missing radius in NormalizedSphereDistancePairScore");
Float ra = p[0]->get_value(radius_);
Float rb = p[1]->get_value(radius_);
Float mr= std::min(ra, rb);
// lambda is inefficient due to laziness
return internal::evaluate_distance_pair_score(XYZ(p[0]),
XYZ(p[1]),
da, f_.get(),
boost::lambda::_1/mr-(ra+rb)/mr);
}
static void XYZ_to_sRGB_WhitePoint(FX_FLOAT X,
FX_FLOAT Y,
FX_FLOAT Z,
FX_FLOAT& R,
FX_FLOAT& G,
FX_FLOAT& B,
FX_FLOAT Xw,
FX_FLOAT Yw,
FX_FLOAT Zw) {
// The following RGB_xyz is based on
// sRGB value {Rx,Ry}={0.64, 0.33}, {Gx,Gy}={0.30, 0.60}, {Bx,By}={0.15, 0.06}
FX_FLOAT Rx = 0.64f, Ry = 0.33f;
FX_FLOAT Gx = 0.30f, Gy = 0.60f;
FX_FLOAT Bx = 0.15f, By = 0.06f;
CFX_Matrix_3by3 RGB_xyz(Rx, Gx, Bx, Ry, Gy, By, 1 - Rx - Ry, 1 - Gx - Gy,
1 - Bx - By);
CFX_Vector_3by1 whitePoint(Xw, Yw, Zw);
CFX_Vector_3by1 XYZ(X, Y, Z);
CFX_Vector_3by1 RGB_Sum_XYZ = RGB_xyz.Inverse().TransformVector(whitePoint);
CFX_Matrix_3by3 RGB_SUM_XYZ_DIAG(RGB_Sum_XYZ.a, 0, 0, 0, RGB_Sum_XYZ.b, 0, 0,
0, RGB_Sum_XYZ.c);
CFX_Matrix_3by3 M = RGB_xyz.Multiply(RGB_SUM_XYZ_DIAG);
CFX_Vector_3by1 RGB = M.Inverse().TransformVector(XYZ);
R = RGB_Conversion(RGB.a);
G = RGB_Conversion(RGB.b);
B = RGB_Conversion(RGB.c);
}
XYZ Z3DTexture::testRaster(const XYZ& ori)
{
if(!m_pTree) return XYZ(0);
raster->clear();
m_pTree->setSampleOpacity(0.05f);
m_pTree->setRaster(raster);
m_pTree->occlusionAccum(ori);
raster->draw();
XYZ coe[SH_N_BASES];
m_sh->zeroCoeff(coe);
float l, vl;
XYZ vn(0,1,0);
for(unsigned int j=0; j<SH_N_SAMPLES; j++) {
SHSample s = m_sh->getSample(j);
vl = vn.dot(s.vector);
if(vl>0) {
raster->readLight(s.vector, l);
l *= vl;
m_sh->projectASample(coe, j, l);
}
}
m_sh->reconstructAndDraw(coe);
return m_sh->integrate(m_sh->constantCoeff, coe);
}
void PViewDataList::reverseElement(int step, int ent, int ele)
{
if(step) return;
if(ele != _lastElement) _setLast(ele);
// copy data
std::vector<double> XYZ(3 * _lastNumNodes);
for(std::size_t i = 0; i < XYZ.size(); i++) XYZ[i] = _lastXYZ[i];
std::vector<double> V(_lastNumNodes * _lastNumComponents * getNumTimeSteps());
for(std::size_t i = 0; i < V.size(); i++) V[i] = _lastVal[i];
// reverse node order
for(int i = 0; i < _lastNumNodes; i++) {
_lastXYZ[i] = XYZ[_lastNumNodes - i - 1];
_lastXYZ[_lastNumNodes + i] = XYZ[2 * _lastNumNodes - i - 1];
_lastXYZ[2 * _lastNumNodes + i] = XYZ[3 * _lastNumNodes - i - 1];
}
for(int step = 0; step < getNumTimeSteps(); step++)
for(int i = 0; i < _lastNumNodes; i++)
for(int k = 0; k < _lastNumComponents; k++)
_lastVal[_lastNumComponents * _lastNumNodes * step +
_lastNumComponents * i + k] =
V[_lastNumComponents * _lastNumNodes * step +
_lastNumComponents * (_lastNumNodes - i - 1) + k];
}
unsigned long ReprojectXYZ(double x, double y, double z, int& u, int& v)
{
cv::Mat XYZ(3, 1, CV_64FC1);
cv::Mat UVW(3, 1, CV_64FC1);
double* d_ptr = 0;
double du = 0;
double dv = 0;
double dw = 0;
x *= 1000;
y *= 1000;
z *= 1000;
d_ptr = XYZ.ptr<double>(0);
d_ptr[0] = x;
d_ptr[1] = y;
d_ptr[2] = z;
UVW = camera_matrix_ * XYZ;
d_ptr = UVW.ptr<double>(0);
du = d_ptr[0];
dv = d_ptr[1];
dw = d_ptr[2];
u = cvRound(du/dw);
v = cvRound(dv/dw);
return ipa_Utils::RET_OK;
}
void RayTracingEnvironment::AddBSPFace(int id,dface_t const &face)
{
if (face.dispinfo!=-1) // displacements must be dealt with elsewhere
return;
texinfo_t *tx =(face.texinfo>=0)?&(texinfo[face.texinfo]):0;
// if (tx && (tx->flags & (SURF_SKY|SURF_NODRAW)))
// return;
if (tx)
{
printf("id %d flags=%x\n",id,tx->flags);
}
printf("side: ");
for(int v=0; v<face.numedges; v++)
{
printf("(%f %f %f) ",XYZ(VertCoord(face,v)));
}
printf("\n");
int ntris=face.numedges-2;
for(int tri=0; tri<ntris; tri++)
{
AddTriangle(id,VertCoord(face,0),VertCoord(face,(tri+1)%face.numedges),
VertCoord(face,(tri+2)%face.numedges),Vector(1,1,1)); //colors[id % NELEMS(colors)]);
}
}
XYZ getColor(const SHSample& s) const
{
float r, g, b;
img->getRGBByVector(s.vec[0], s.vec[1], s.vec[2], r,g,b);
return XYZ(r,g,b);
}
void ASMs1D::getElementEnds (int i, Vec3Vec& XC) const
{
RealArray::const_iterator uit = curv->basis().begin();
// Fetch parameter values of the element ends (knots)
RealArray u(2);
u[0] = uit[i-1];
u[1] = uit[i];
// Evaluate the spline curve at the knots to find physical coordinates
int dim = curv->dimension();
RealArray XYZ(dim*2);
curv->gridEvaluator(XYZ,u);
XC.clear();
XC.reserve(elmCS.empty() ? 2 : 3);
const double* pt = &XYZ.front();
for (int j = 0; j < 2; j++, pt += dim)
XC.push_back(Vec3(pt,dim));
if (elmCS.empty()) return;
// Add the local Z-axis as the third vector
int iel = i - curv->order();
XC.push_back(elmCS[iel][2]);
}
void CreateTower(int cnt_x, int cnt_y, int cnt_z, XYZ offset, XYZ size)
{
RigidBox *myRigidBody;
for(int y = 1; y <= cnt_y; y++)
{
for(int z = 1; z <= cnt_z; z++)
{
for(int x = 1; x <= cnt_x; x++)
{
float spacing = 0.01;
float heightSpacing = 0.01;
XYZ pos = XYZ(x * size.x + (spacing * x), y * size.y + (heightSpacing * y), z * size.z + (spacing * z));
pos += offset;
myRigidBody = new RigidBox(boxObj);
myRigidBody->setPosition(pos);
myRigidBody->setScale(size);
myRigidBody->buildVertexLightMap();
// myRigidBody->setRestitution(0.8); // bouncey fun land
myScene.bind(*myRigidBody);
}
}
}
}
bool ASMs1D::tesselate (ElementBlock& grid, const int* npe) const
{
// Compute parameter values of the nodal points
RealArray gpar;
if (!this->getGridParameters(gpar,npe[0]-1))
return false;
// Evaluate the spline curve at all points
size_t nx = gpar.size();
RealArray XYZ(curv->dimension()*nx);
curv->gridEvaluator(XYZ,gpar);
// Establish the block grid coordinates
size_t i, j, l;
grid.resize(nx);
for (i = l = 0; i < grid.getNoNodes(); i++, l += curv->dimension())
for (j = 0; j < nsd; j++)
grid.setCoor(i,j,XYZ[l+j]);
// Establish the block grid topology
int nse1 = npe[0] - 1;
int n[2], ie = 1, ip = 0;
n[0] = 0;
n[1] = n[0] + 1;
for (i = 1; i < nx; i++)
{
for (l = 0; l < 2; l++)
grid.setNode(ip++,n[l]++);
grid.setElmId(i,ie);
if (i%nse1 == 0) ie++;
}
return true;
}
Mesh *OBJLoader::load(const string fileName) {
objMesh = new Mesh();
float x, y, z;
string strline, op;
string mtllib;
vector<UV> uvs;
vector<XYZ> normals;
vector<string> ptData;
Material *currentMaterial = NULL;
string path_str,file_str,file_base,file_ext;
str_file_extract(fileName, path_str, file_str, file_base, file_ext);
Logger::log("Loading .OBJ Model [%s]\n",fileName.c_str());
ifstream File(fileName.c_str());
while (!File.eof())
{
getline(File,strline,'\n');
istringstream input(strline);
op.clear();
input >> op;
if (!op.length()) continue;
if (op.length() && op[0] == '#') {
Logger::log("%s\n",input.str().c_str());
continue;
}
if (op.compare("o")==0) { // Object Name
string objName = readStr(input);
Logger::log(" - Object Name [%s]\n",objName.c_str());
continue;
}
if (op.compare("mtllib")==0) { // Material Lib
mtllib = readStr(input);
Logger::log(" - Material Library [%s]\n",mtllib.c_str());
continue;
}
if (op.compare("v")==0) { // Vertex: X,Y,Z,[w]
input >> x; input >> y; input >> z;
objMesh->addPoint(XYZ(x,y,z));
continue;
}
if (op.compare("vt")==0) { // TexCoord: U,V,[w]
input >> x; input >> y;
uvs.push_back(UV(x,y));
continue;
}
void Active::SendSignalAround(const QString& signal) const {
if ( IsInside() ) return; // for blocks inside inventories
const World* const world = World::GetCWorld();
const AroundCoordinates arounds(X(), Y(), Z());
for (const XyzInt& xyz : arounds) {
world->GetBlock(XYZ(xyz))->ReceiveSignal(signal);
}
}
Geom::Box Geom::AABB::box()
{
Box b;
b.Center = center();
b.Axis = XYZ();
b.Extent = (bbmax - bbmin) * 0.5f;
return b;
}
void Transform::apply_index(Model *m, ParticleIndex pi) const {
if (!XYZ::get_is_setup(m, pi)) {
IMP_INTERNAL_CHECK(ignore_non_xyz_,
"The particle does not have XYZ attributes");
return;
}
XYZ xyz = XYZ(m, pi);
xyz.set_coordinates(t_.get_transformed(xyz.get_coordinates()));
}
void Transform::apply(Particle *p) const
{
if (!XYZ::particle_is_instance(p)) {
IMP_INTERNAL_CHECK(ignore_non_xyz_,
"The particle does not have XYZ attributes");
return;
}
XYZ xyz = XYZ(p);
xyz.set_coordinates(t_.get_transformed(xyz.get_coordinates()));
}
bool AccelerometerSensorChannelInterface::dataReceivedImpl()
{
QVector<AccelerationData> values;
if(!read<AccelerationData>(values))
return false;
if(!frameAvailableConnected || values.size() == 1)
{
foreach(const AccelerationData& data, values)
emit dataAvailable(XYZ(data));
}
Float WeightedSphereDistancePairScore::evaluate(const ParticlePair &p,
DerivativeAccumulator *da) const
{
IMP_USAGE_CHECK(p[0]->has_attribute(radius_), "Particle " << p[0]->get_name()
<< "missing radius in WeightedSphereDistancePairScore");
IMP_USAGE_CHECK(p[1]->has_attribute(radius_), "Particle " << p[1]->get_name()
<< "missing radius in WeightedSphereDistancePairScore");
IMP_USAGE_CHECK(p[0]->has_attribute(weight_), "Particle " << p[0]->get_name()
<< "missing weight in WeightedSphereDistancePairScore");
IMP_USAGE_CHECK(p[1]->has_attribute(weight_), "Particle " << p[1]->get_name()
<< "missing weight in WeightedSphereDistancePairScore");
Float ra = p[0]->get_value(radius_);
Float rb = p[1]->get_value(radius_);
Float wa = p[0]->get_value(weight_);
Float wb = p[1]->get_value(weight_);
// lambda is inefficient due to laziness
return internal::evaluate_distance_pair_score(
XYZ(p[0]),
XYZ(p[1]),
da, f_.get(),
(boost::lambda::_1-(ra+rb))*(wa+wb));
}
// -----------------------------------------------------------------------------------
//------------------------------------------------------------------------------------
// Compute the azimuth and nadir angle, in the satellite body frame,
// of receiver Position RX as seen at the satellite Position SV. The nadir angle
// is measured from the Z axis, which points to Earth center, and azimuth is
// measured from the X axis.
// @param Position SV Satellite position
// @param Position RX Receiver position
// @param Matrix<double> Rot Rotation matrix (3,3), output of SatelliteAttitude
// @param double nadir Output nadir angle in degrees
// @param double azimuth Output azimuth angle in degrees
void SatelliteNadirAzimuthAngles(const Position& SV,
const Position& RX,
const Matrix<double>& Rot,
double& nadir,
double& azimuth)
throw(Exception)
{
try {
if(Rot.rows() != 3 || Rot.cols() != 3) {
Exception e("Rotation matrix invalid");
GPSTK_THROW(e);
}
double d;
Position RmS;
// RmS points from satellite to receiver
RmS = RX - SV;
RmS.transformTo(Position::Cartesian);
d = RmS.mag();
if(d == 0.0) {
Exception e("Satellite and Receiver Positions identical");
GPSTK_THROW(e);
}
RmS = (1.0/d) * RmS;
Vector<double> XYZ(3),Body(3);
XYZ(0) = RmS.X();
XYZ(1) = RmS.Y();
XYZ(2) = RmS.Z();
Body = Rot * XYZ;
nadir = ::acos(Body(2)) * RAD_TO_DEG;
azimuth = ::atan2(Body(1),Body(0)) * RAD_TO_DEG;
if(azimuth < 0.0) azimuth += 360.;
}
catch(Exception& e) { GPSTK_RETHROW(e); }
catch(exception& e) {Exception E("std except: "+string(e.what())); GPSTK_THROW(E);}
catch(...) { Exception e("Unknown exception"); GPSTK_THROW(e); }
}
void StereoCameraModel::projectDisparityTo3d(const cv::Point2d& left_uv_rect, float disparity,
cv::Point3d& xyz) const
{
assert( initialized() );
// Do the math inline:
// [X Y Z W]^T = Q * [u v d 1]^T
// Point = (X/W, Y/W, Z/W)
// cv::perspectiveTransform could be used but with more overhead.
double u = left_uv_rect.x, v = left_uv_rect.y;
cv::Point3d XYZ(u + Q_(0,3), v + Q_(1,3), Q_(2,3));
double W = Q_(3,2)*disparity + Q_(3,3);
xyz = XYZ * (1.0/W);
}