real SocSystem_Analytical::getCosts(arr& R,arr& r,uint t,const arr& qt){
uint N=x.N;
R.resize(N,N); R.setZero();
r.resize(N); r.setZero();
real C=0.;
#ifndef USE_TRUNCATION //potentials for collision cost
arr J(1,qt.N),phiHatQ(1);
J.setZero();
phiHatQ.setZero();
for(uint i=0;i<obstacles.d0;i++){
real margin = .1;
real d = (1.-norm(x-obstacles[i])/margin);
if(d<0) continue;
phiHatQ(0) += d*d;
J += ((real)2.*d/margin)*(obstacles[i]-x)/norm(x-obstacles[i]);
}
J.reshape(1,J.N);
arr tJ,target(1);
target=(real)0.;
transpose(tJ,J);
real colprec = (real)5e2;
C += colprec*sqrDistance(target,phiHatQ);
R += colprec*tJ*J;
r += colprec*tJ*(target - phiHatQ + J*qt);
#endif
if(t!=T-1) return C;
R.setDiag(1.);
r = x1;
R *= prec;
r *= prec;
C += prec*sqrDistance(x1,x);
return C;
}
// [O] SPHERE - SPHERE
bool SphereCollider::Collide( const SphereCollider& other ) const
{
glm::vec3 distance = position - other.position;
glm::vec3 sqrDistance( glm::dot( distance, distance ) );
float radiusSum( radius + other.radius );
return sqrDistance.x <= (radiusSum * radiusSum);
}
// [O] CYLINDER - CYLINDER
bool CylinderCollider::Collide( const CylinderCollider& other ) const
{
float s, t;
glm::vec3 c1, c2;
glm::vec3 sqrDistance( ClosestPtSegmentSegment( position, end, other.position, other.end, s, t, c1, c2 ) );
float radiusSum = radius + other.radius;
return sqrDistance.x <= ( radiusSum * radiusSum );
}
static std::vector<NearestPhoton>::iterator nearest(std::vector<Photon>::const_iterator begin, std::vector<Photon>::const_iterator end, Segment3f const& bound, Point3f const& position, std::vector<NearestPhoton>::iterator nearestBegin, std::vector<NearestPhoton>::iterator nearestNext, std::vector<NearestPhoton>::iterator nearestEnd) {
if (begin == end) {
return nearestNext;
}
std::vector<Photon>::const_iterator median = begin + (end - begin) / 2;
float medianSqrDistance = sqrDistance(median->position, position);
if (nearestNext == nearestEnd) {
if (medianSqrDistance < nearestBegin->sqrDistance) {
std::pop_heap(nearestBegin, nearestNext--, SqrDistanceLess());
nearestNext->photon = median;
nearestNext->sqrDistance = medianSqrDistance;
std::push_heap(nearestBegin, ++nearestNext, SqrDistanceLess());
}
} else {
nearestNext->photon = median;
nearestNext->sqrDistance = medianSqrDistance;
std::push_heap(nearestBegin, ++nearestNext, SqrDistanceLess());
}
int splitAxis = maxAxis(bound);
if (position[splitAxis] <= median->position[splitAxis]) {
Segment3f belowBound = bound;
belowBound.max[splitAxis] = median->position[splitAxis];
nearestNext = nearest(begin, median, belowBound, position, nearestBegin, nearestNext, nearestEnd);
} else {
Segment3f aboveBound = bound;
aboveBound.min[splitAxis] = median->position[splitAxis];
nearestNext = nearest(median + 1, end, aboveBound, position, nearestBegin, nearestNext, nearestEnd);
}
if (nearestNext != nearestEnd || sqr(median->position[splitAxis] - position[splitAxis]) < nearestBegin->sqrDistance) {
if (position[splitAxis] <= median->position[splitAxis]) {
Segment3f aboveBound = bound;
aboveBound.min[splitAxis] = median->position[splitAxis];
nearestNext = nearest(median + 1, end, aboveBound, position, nearestBegin, nearestNext, nearestEnd);
} else {
Segment3f belowBound = bound;
belowBound.max[splitAxis] = median->position[splitAxis];
nearestNext = nearest(begin, median, belowBound, position, nearestBegin, nearestNext, nearestEnd);
}
}
return nearestNext;
}
float sqrDistance(const LineSegment3& x, const Vector3& q)
{
return sqrDistance(closestPoint(x, q), q);
}
bool Line::contains(const Vector& p) const {
return IS_ZERO(sqrDistance(p));
}
double Line::distance(const Vector& p) const {
return sqrt(sqrDistance(p));
}
