bool rayIntersects(RayN<N, T> const & ray, T max_time = -1) const
{
if (max_time >= 0)
return rayIntersectionTime(ray, max_time) >= 0;
VectorT co = ray.getOrigin() - center;
T c = co.squaredNorm() - radius * radius;
if (c <= 0) // origin is inside ball
return true;
T a = ray.getDirection().squaredNorm();
T b = 2 * co.dot(ray.getDirection());
// Solve quadratic a * t^2 + b * t + c = 0
T b2 = b * b;
T det = b2 - 4 * a * c;
if (det < 0) return false;
if (a > 0)
return b <= 0 || det >= b2;
else if (a < 0)
return b >= 0 || det >= b2;
else
return false;
}
Ipp32f BhattacharyyaDistance<Ipp32f>::doCalculate(const VectorT<Ipp32f>& v1, const VectorT<Ipp32f>& v2) const {
if(v1.size()!=v2.size())
fthrow(Exception, "Input vectors must have the same size.");
Ipp32f B;
#ifdef NICE_USELIB_IPP
VectorT<Ipp32f> v1f(v1);
v1f *= v2;
ippsSqrt(v1f.getDataPointer(), v1f.getDataPointer(), v1f.size());
ippsSum(v1f.getDataPointer(), v1f.size(), &B);
#else // NICE_USELIB_IPP
B = 0.0;
for(uint i=0; i<v1.size(); ++i)
B += std::sqrt(v1[i]*v2[i]);
#endif // NICE_USELIB_IPP
return std::sqrt(1-B);
}
void operator()(LinPdeSysT const & pde_system,
SegmentT const & segment,
StorageType & storage,
MatrixT & system_matrix,
VectorT & load_vector)
{
typedef viennamath::equation equ_type;
typedef viennamath::expr expr_type;
typedef typename expr_type::interface_type interface_type;
typedef typename expr_type::numeric_type numeric_type;
typedef typename viennagrid::result_of::cell_tag<SegmentT>::type CellTag;
std::size_t map_index = viennafvm::create_mapping(pde_system, segment, storage);
system_matrix.clear();
system_matrix.resize(map_index, map_index, false);
load_vector.clear();
load_vector.resize(map_index);
for (std::size_t pde_index = 0; pde_index < pde_system.size(); ++pde_index)
{
#ifdef VIENNAFVM_DEBUG
std::cout << std::endl;
std::cout << "//" << std::endl;
std::cout << "// Equation " << pde_index << std::endl;
std::cout << "//" << std::endl;
#endif
assemble(pde_system, pde_index,
segment, storage,
system_matrix, load_vector);
} // for pde_index
} // functor
void RowEchelon<T>::backSub(VectorT& x) const
{
Assert(EB.n == 1);
Assert(EB.m == R.m);
x.resize(R.n);
VectorT b;
EB.getColRef(0,b);
Assert((int)firstEntry.size() == R.m+1);
x.setZero();
int m=R.m,n=R.n;
for(int i=m-1;i>=0;i--) {
VectorT ri; R.getRowRef(i,ri);
//solve to set R*x = b[i]
//calculate alpha = dot between rest of x[>ji] and R[i]
int ji=firstEntry[i];
if(ji == n) continue;
int ji2=firstEntry[i+1]; //(i+1==m?n:firstEntry[i+1]);
T alpha;
if(ji2 == n) alpha = Zero;
else {
VectorT rji2; rji2.setRef(ri,ji2,1,R.n-ji2);
VectorT xji2; xji2.setRef(x,ji2,1,R.n-ji2);
alpha = xji2.dot(rji2);
}
x[ji] = (b[i]-alpha)/ri[ji];
}
}
void interval_set_element_iter_4_discrete_types()
{
typedef IntervalSet<T> IntervalSetT;
typedef typename IntervalSetT::interval_type IntervalT;
typedef std::vector<T> VectorT;
IntervalSetT set_a;
set_a.add(I_I(1,3)).add(I_I(6,7));
VectorT vec(5), cev(5);
vec[0]=MK_v(1);vec[1]=MK_v(2);vec[2]=MK_v(3);vec[3]=MK_v(6);vec[4]=MK_v(7);
cev[0]=MK_v(7);cev[1]=MK_v(6);cev[2]=MK_v(3);cev[3]=MK_v(2);cev[4]=MK_v(1);
VectorT dest;
std::copy(elements_begin(set_a), elements_end(set_a), std::back_inserter(dest));
BOOST_CHECK_EQUAL( vec == dest, true );
dest.clear();
std::copy(elements_rbegin(set_a), elements_rend(set_a), std::back_inserter(dest));
BOOST_CHECK_EQUAL( cev == dest, true );
dest.clear();
std::reverse_copy(elements_begin(set_a), elements_end(set_a), std::back_inserter(dest));
BOOST_CHECK_EQUAL( cev == dest, true );
dest.clear();
std::reverse_copy(elements_rbegin(set_a), elements_rend(set_a), std::back_inserter(dest));
BOOST_CHECK_EQUAL( vec == dest, true );
}
void EngineEntityT::Draw(bool FirstPersonView, const VectorT& ViewerPos) const
{
MatSys::Renderer->PushMatrix(MatSys::RendererI::MODEL_TO_WORLD);
MatSys::Renderer->PushLightingParameters();
unsigned short Ent_Heading;
unsigned short Ent_Pitch;
unsigned short Ent_Bank;
Entity->GetBodyOrientation(Ent_Heading, Ent_Pitch, Ent_Bank);
// Get the currently set lighting parameters.
const float* PosL=MatSys::Renderer->GetCurrentLightSourcePosition();
VectorT LightSourcePos =VectorT(PosL[0], PosL[1], PosL[2]);
float LightSourceRadius=MatSys::Renderer->GetCurrentLightSourceRadius();
const float* PosE=MatSys::Renderer->GetCurrentEyePosition();
VectorT EyePos=VectorT(PosE[0], PosE[1], PosE[2]);
// Starting from world space, compute the position of the light source in model space.
LightSourcePos=LightSourcePos-Entity->GetOrigin(); // Convert into unrotated model space.
LightSourcePos=LightSourcePos.GetRotZ(-90.0+float(Ent_Heading)/8192.0*45.0);
LightSourcePos=scale(LightSourcePos, 1.0/25.4);
// Don't forget to scale the radius of the light source appropriately down (into model space), too.
LightSourceRadius/=25.4f;
// Do the same for the eye: Starting from world space, compute the position of the eye in model space.
EyePos=EyePos-Entity->GetOrigin(); // Convert into unrotated model space.
EyePos=EyePos.GetRotZ(-90.0+float(Ent_Heading)/8192.0*45.0);
EyePos=scale(EyePos, 1.0/25.4);
// Set the modified (now in model space) lighting parameters.
MatSys::Renderer->SetCurrentLightSourcePosition(float(LightSourcePos.x), float(LightSourcePos.y), float(LightSourcePos.z));
MatSys::Renderer->SetCurrentLightSourceRadius(LightSourceRadius);
MatSys::Renderer->SetCurrentEyePosition(float(EyePos.x), float(EyePos.y), float(EyePos.z));
// Set the ambient light color for this entity.
// Paradoxically, this is not a global, but rather a per-entity value that is derived from the lightmaps that are close to that entity.
const Vector3fT AmbientEntityLight=Entity->GetGameWorld()->GetAmbientLightColorFromBB(Entity->GetDimensions(), Entity->GetOrigin());
MatSys::Renderer->SetCurrentAmbientLightColor(AmbientEntityLight.x, AmbientEntityLight.y, AmbientEntityLight.z);
MatSys::Renderer->Translate(MatSys::RendererI::MODEL_TO_WORLD, float(Entity->GetOrigin().x), float(Entity->GetOrigin().y), float(Entity->GetOrigin().z));
MatSys::Renderer->RotateZ (MatSys::RendererI::MODEL_TO_WORLD, 90.0f-float(Ent_Heading)/8192.0f*45.0f);
MatSys::Renderer->Scale (MatSys::RendererI::MODEL_TO_WORLD, 25.4f);
Entity->Draw(FirstPersonView, (float)length(ViewerPos-Entity->GetOrigin()));
MatSys::Renderer->PopLightingParameters();
MatSys::Renderer->PopMatrix(MatSys::RendererI::MODEL_TO_WORLD);
}
void set_row(const Uint array_idx, const VectorT& row)
{
cf3_assert(row.size() == row_size());
Row row_to_set = m_array[array_idx];
for(Uint j=0; j<row.size(); ++j)
row_to_set[j] = row[j];
}
void set_row(const Uint array_idx, const VectorT& row)
{
if (row.size() != row_size(array_idx))
m_array[array_idx].resize(row.size());
Uint j=0;
boost_foreach( const typename VectorT::value_type& v, row)
m_array[array_idx][j++] = v;
}
void vector_test(const VectorT& A, const VectorT& B, Accumulator& Result)
{
const Uint sizeA = A.size();
const Uint sizeB = B.size();
for(Uint i = 0, j = 0; i != sizeA && j != sizeB; ++i, ++j)
test(A[i], B[j], Result);
Result.exact(sizeA == sizeB);
};
int main()
{
VectorT<int> vint;
VectorT<double> vdouble;
vint.Normalize();
vdouble.Normalize();
return 0;
}
Ipp32f EuclidianDistance<Ipp32f>::doCalculate(const VectorT<Ipp32f>& v1, const VectorT<Ipp32f>& v2) const {
if(v1.size()!=v2.size())
fthrow(Exception, "Input vectors must have the same size.");
Ipp32f dist = 0;
#ifdef NICE_USELIB_IPP
VectorT<Ipp32f> res(v1.size());
ippsSub_32f(v1.getDataPointer(), v2.getDataPointer(), res.getDataPointer(), v1.size());
ippsSqr_32f(res.getDataPointer(), res.getDataPointer(), res.size());
dist = res.Sum();
#else // NICE_USELIB_IPP
const Ipp32f* pSrc1 = v1.getDataPointer();
const Ipp32f* pSrc2 = v2.getDataPointer();
for(Ipp32u i=0; i<v1.size(); ++i,++pSrc1,++pSrc2)
dist += (*pSrc1-*pSrc2)*(*pSrc1-*pSrc2);
#endif // NICE_USELIB_IPP
dist = std::sqrt(dist);
return dist;
}
void DiagonalMatrixTemplate<T>::mulPseudoInverse(const VectorT& a, VectorT& b) const
{
if(BaseT::n != a.n) FatalError(MatrixError_ArgIncompatibleDimensions);
if(b.n == 0)
b.resize(this->n);
else if(b.n != this->n) FatalError(MatrixError_DestIncompatibleDimensions);
ItT v=this->begin();
VectorIterator<T> va=a.begin(),vb=b.begin();
for(int i=0; i<this->n; i++, v++,va++,vb++)
*vb = *va * PseudoInv(*v);
}
T rayIntersectionTime(RayN<N, T> const & ray, T max_time = -1) const
{
VectorT co = ray.getOrigin() - center;
T c = co.squaredNorm() - radius * radius;
if (c <= 0) // origin is inside ball
return 0;
// We could do an early test to see if the distance from the ray origin to the ball is less than
// max_time * ray.getDirection().norm(), but it would involve a square root so might as well solve the quadratic.
T a = ray.getDirection().squaredNorm();
T b = 2 * co.dot(ray.getDirection());
// Solve quadratic a * t^2 + b * t + c = 0
T det = b * b - 4 * a * c;
if (det < 0) return -1;
T d = std::sqrt(det);
T t = -1;
if (a > 0)
{
T s0 = -b - d;
if (s0 >= 0)
t = s0 / (2 * a);
else
{
T s1 = -b + d;
if (s1 >= 0)
t = s1 / (2 * a);
}
}
else if (a < 0)
{
T s0 = -b + d;
if (s0 <= 0)
t = s0 / (2 * a);
else
{
T s1 = -b - d;
if (s1 <= 0)
t = s1 / (2 * a);
}
}
if (max_time >= 0 && t > max_time)
return -1;
else
return t;
}
void MatrixTemplate<T>::copyCols(const VectorT* cols)
{
CHECKEMPTY();
for(int j=0; j<n; j++)
{
if(cols[j].n != m)
{
RaiseErrorFmt(WHERE_AM_I,MatrixError_IncompatibleDimensions,m,n,cols[j].n,-1);
}
VectorT tempcol;
getColRef(j,tempcol);
tempcol.copy(cols[j]);
}
}
void MatrixTemplate<T>::copyRows(const VectorT* rows)
{
CHECKEMPTY();
for(int i=0; i<m; i++)
{
if(rows[i].n != n)
{
RaiseErrorFmt(WHERE_AM_I,MatrixError_IncompatibleDimensions,m,n,-1,rows[i].n);
}
VectorT temprow;
getRowRef(i,temprow);
temprow.copy(rows[i]);
}
}
void SparseMatrixTemplate_RM<T>::mulTranspose(const VectorT& a,VectorT& x) const
{
if(x.n == 0) x.resize(n);
if(x.n != n) {
FatalError("Destination vector has incorrect dimensions");
}
if(a.n != m) {
FatalError("Source vector has incorrect dimensions");
}
x.setZero();
for(int i=0;i<m;i++) {
for(ConstRowIterator it=rows[i].begin();it!=rows[i].end();it++)
x(it->first) += it->second*a(i);
}
}
MatrixT diag(VectorT const &v,
typename MatrixT::ScalarType zero =
static_cast<typename MatrixT::ScalarType>(0))
{
MatrixT diag(v.size(), v.size(), zero);
diag.set_zero(zero);
//populate diagnals:
std::vector<IndexType> indices;
for (graphblas::IndexType ix = 0; ix < v.size(); ++ix) {
indices.push_back(ix);
}
graphblas::buildmatrix(diag, indices.begin(), indices.begin(), v.begin(), v.size());
return diag;
}
/**
* Compute the search direction based on the current (inverse) Hessian
* approximation and given gradient.
*
* @param[out] pk The negative product of the inverse Hessian and gradient
* direction gk.
* @param[in] gk Gradient direction.
**/
inline void search_direction(VectorT &pk, const VectorT &gk) const {
std::vector<Scalar> alphas(_buf.size());
typename
boost::circular_buffer<UpdateT>::const_reverse_iterator buf_rit;
typename boost::circular_buffer<UpdateT>::const_iterator buf_it;
typename std::vector<Scalar>::const_iterator alpha_it;
typename std::vector<Scalar>::reverse_iterator alpha_rit;
pk.noalias() = -gk;
for (buf_rit = _buf.rbegin(), alpha_rit = alphas.rbegin();
buf_rit != _buf.rend();
buf_rit++, alpha_rit++) {
Scalar alpha;
const Scalar &rhoi(boost::get<0>(*buf_rit));
const VectorT &yi(boost::get<1>(*buf_rit));
const VectorT &si(boost::get<2>(*buf_rit));
alpha = rhoi * si.dot(pk);
pk -= alpha * yi;
*alpha_rit = alpha;
}
pk *= _gammak;
for (buf_it = _buf.begin(), alpha_it = alphas.begin();
buf_it != _buf.end();
buf_it++, alpha_it++) {
Scalar beta;
const Scalar &rhoi(boost::get<0>(*buf_it));
const VectorT &yi(boost::get<1>(*buf_it));
const VectorT &si(boost::get<2>(*buf_it));
beta = rhoi*yi.dot(pk);
pk += (*alpha_it - beta)*si;
}
}
void QRDecomposition<T>::backSub(const VectorT& b, VectorT& x) const
{
if(x.n == 0) x.resize(QR.n);
Assert(QR.m == b.n);
Assert(QR.n == x.n);
/* compute rhs = Q^T b */
VectorT rhs;
QtMul(b,rhs);
if(QR.m == QR.n) {
/* Solve R x = rhs, storing x in-place */
UBackSubstitute(QR,rhs,x);
//UBackSubstitute(QR,x,x);
}
else if(QR.m > QR.n) {
//solve the top part of R x = rhs
MatrixT R1; R1.setRef(QR,0,0,1,1,QR.n,QR.n);
VectorT rhs1; rhs1.setRef(rhs,0,1,QR.n);
UBackSubstitute(R1,rhs1,x);
}
else {
cerr<<"What do we do with m < n?"<<endl;
MatrixPrinter mprint(QR); mprint.mode = MatrixPrinter::AsciiShade;
cerr<<mprint<<endl;
//solve the left part of R x = rhs
MatrixT R1; R1.setRef(QR,0,0,1,1,QR.m,QR.m);
VectorT x1; x1.setRef(x,0,1,QR.m);
UBackSubstitute(R1,rhs,x1);
getchar();
}
}
Requires_t<mtl::traits::is_distributed<VectorT>>
initVector(VectorT& x) const
{
#ifdef HAVE_PARALLEL_MTL4
x.init_distribution(col_distribution(*fullMatrix), num_cols(*fullMatrix));
#endif
set_to_zero(x);
}
bool equivalent (const Line<D, NumericT> & other) {
// Are we pointing in the same direction
if (!_direction.equivalent(other._direction))
return false;
// Is the distance between the parallel lines equivalent to zero?
return Numerics::equivalent(shortest_distance_to_point(other._point), 0);
}
void
set_v(
VectorT vec,
IndexObj const& obj)
{
for (index_type p=0; p<vec.size(); ++p)
vec(p) = obj(p);
}
void
check_v(
VectorT vec,
IndexObj const& obj)
{
for (index_type p=0; p<vec.size(); ++p)
test_assert(vec(p) == obj(p));
}
void print_vector(StreamT& stream, const VectorT& vector, const std::string& sep=" ", const std::string& prefix = "", const std::string& suffix = "")
{
stream << prefix;
const Uint vector_size = vector.size();
for(Uint i = 0; i != vector_size; ++i)
{
stream << (i != 0 ? sep : "") << vector[i];
}
stream << suffix;
}
/**
* Add a new set of update vectors to the history.
*
* @param yk Difference between the current and previous gradient vector.
* @param sk Difference between the current and previous state vector.
* @param reset Whether to reset the approximation, forgetting about
* previous values.
* @return In the case of a reset, returns the optimal scaling of the
* initial Hessian
* approximation which is useful for predicting step-sizes.
**/
inline Scalar update(const VectorT &yk, const VectorT &sk,
bool reset = false) {
Scalar skyk = yk.dot(sk);
Scalar B0fact;
if (reset) {
B0fact = yk.squaredNorm()/skyk;
_buf.clear();
} else {
B0fact = 1.0;
}
// New updates are pushed to the "back" of the circular buffer
Scalar invskyk = 1.0/skyk;
_gammak = skyk/yk.squaredNorm();
_buf.push_back();
_buf.back() = boost::tie(invskyk, yk, sk);
return B0fact;
}
void SparseVectorTemplate<T>::get(VectorT& v) const
{
v.resize(this->n);
int k=0;
for(const_iterator i=this->begin();i!=this->end();i++) {
while(k < i->first) { v(k)=0; k++; }
v(k) = i->second;
k=i->first+1;
}
while(k < (int)this->n) { v(k)=0; k++; }
}
void MatrixTemplate<T>::maddTranspose(const VectorT& a, VectorT& b) const
{
if(m != a.n)
{
RaiseErrorFmt(WHERE_AM_I,MatrixError_ArgIncompatibleDimensions);
}
if(b.n == 0)
{
b.resize(n);
}
else if(b.n != n)
{
RaiseErrorFmt(WHERE_AM_I,MatrixError_DestIncompatibleDimensions);
}
gen_array2d_vector_madd_transpose(b.getStart(),b.stride,
getStart(),istride,jstride,
a.getStart(),a.stride,
m, n);
}
/// Returns the time on the line where a point is closest to the given point.
NumericT time_for_closest_point (const VectorT & p3) const {
auto p1 = _point;
auto p2 = _point + _direction;
auto d = _direction.length_squared();
NumericT t = 0;
for (dimension i = 0; i < D; ++i)
t += (p3[i] - p1[i]) * (p2[i] - p1[i]);
return t / d;
}
void SparseVectorCompressed<T>::get(VectorT& v) const
{
v.resize(n);
int i=0;
for(int k=0;k<num_entries;k++) {
for(;i<indices[k];i++)
v[i]=Zero;
v[i]=vals[k];
}
for(;i<this->n;i++)
v[i]=Zero;
}
typename VectorT::value_type
norm_2(VectorT const& v1,
typename viennacl::tools::enable_if< viennacl::is_stl< typename viennacl::traits::tag_of< VectorT >::type >::value
>::type* dummy = 0)
{
//std::cout << "stl .. " << std::endl;
typename VectorT::value_type result = 0;
for (typename VectorT::size_type i=0; i<v1.size(); ++i)
result += v1[i] * v1[i];
return sqrt(result);
}
