FgMatrixC<T,dim,ncols>
operator*(const FgMatrixC<T,dim,ncols> & vec) const
{
FgMatrixC<T,dim,ncols> ret;
for (uint row=0; row<dim; ++row)
for (uint col=0; col<ncols; ++col)
ret.elm(col,row) = m_scales[row] * (vec.elm(col,row) + m_trans[row]);
return ret;
}
FgMatrixC<T,dim,2>
fgBoundsIntersection(
const FgMatrixC<T,dim,2> & b1,
const FgMatrixC<T,dim,2> & b2)
{
FgMatrixC<T,dim,2> ret(b1);
for (uint dd=0; dd<dim; ++dd)
{
fgSetIfGreater(ret.elm(0,dd),b2.elm(0,dd));
fgSetIfLess(ret.elm(1,dd),b2.elm(1,dd));
}
return ret;
}
// Construct from bounding box mapping:
FgAffineCwC(
const FgMatrixC<T,dim,2> & domainBounds, // Column vectors are lo and hi bounds resp.
const FgMatrixC<T,dim,2> & rangeBounds) // "
{
FgMatrixC<T,dim,1>
domainDelta = domainBounds.colVec(1) - domainBounds.colVec(0),
rangeDelta = rangeBounds.colVec(1) - rangeBounds.colVec(0);
// Note that the deltas can be negative if the transform inverts an axis:
FGASSERT(fgNoZeros(domainDelta) && fgNoZeros(rangeDelta));
for (uint dd=0; dd<dim; ++dd) {
m_scales[dd] = rangeDelta[dd] / domainDelta[dd];
m_trans[dd] = rangeBounds.elm(0,dd) - domainBounds.elm(0,dd) * m_scales[dd];
}
}
FgMatrixC<T,dim,2>
fgBounds(
const FgMatrixC<T,dim,1> & v0,
const FgMatrixC<T,dim,1> & v1,
const FgMatrixC<T,dim,1> & v2)
{
FgMatrixC<T,dim,2> ret;
for (uint dd=0; dd<dim; ++dd) {
ret.elm(0,dd) = ret.elm(1,dd) = v0[dd];
fgSetIfLess(ret.elm(0,dd),v1[dd]);
fgSetIfLess(ret.elm(0,dd),v2[dd]);
fgSetIfGreater(ret.elm(1,dd),v1[dd]);
fgSetIfGreater(ret.elm(1,dd),v2[dd]);
}
return ret;
}
bool
fgBoundsIntersect(
const FgMatrixC<T,dim,2> & bnds1,
const FgMatrixC<T,dim,2> & bnds2,
FgMatrixC<T,dim,2> & retval)
{
FgMatrixC<T,dim,2> tmp;
for (uint dd=0; dd<dim; ++dd)
{
tmp.elm(0,dd) = std::max(bnds1.elm(0,dd),bnds2.elm(0,dd));
tmp.elm(1,dd) = std::min(bnds1.elm(1,dd),bnds2.elm(1,dd));
if (tmp.elm(0,dd) > tmp.elm(1,dd))
return false;
}
retval = tmp;
return true;
}
FgAffineC<T,dim>
asAffine() const
{
FgMatrixC<T,dim,dim> scales;
for (uint ii=0; ii<dim; ++ii)
scales.elm(ii,ii) = m_scales[ii];
return FgAffineC<T,dim>(scales,m_trans);
}
FgMatrixC<T,nrows,1>
fgMaxColwise(const FgMatrixC<T,nrows,ncols> & mat)
{
FG_STATIC_ASSERT(ncols > 1);
FgMatrixC<T,nrows,1> ret(mat.colVec(0));
for (uint row=0; row<nrows; ++row)
for (uint col=1; col<ncols; ++col)
fgSetIfGreater(ret[row],mat.elm(col,row));
return ret;
}
bool
fgBoundsIncludes(
const FgMatrixC<T,dim,2> & inclusiveBounds,
const FgMatrixC<T,dim,1> & point)
{
for (uint dd=0; dd<dim; ++dd) {
if ((inclusiveBounds.elm(1,dd) < point[dd]) ||
(inclusiveBounds.elm(0,dd) > point[dd]))
return false;
}
return true;
}
FgMatrixC<T,dim,2>
fgBounds(const std::vector<FgMatrixC<T,dim,1> > & data)
{
FGASSERT(data.size() > 0);
FgMatrixC<T,dim,2> ret;
ret.setSubMatrix(data[0],0,0);
ret.setSubMatrix(data[0],0,1);
for (size_t ii=1; ii<data.size(); ++ii) {
for (uint dd=0; dd<dim; ++dd) {
fgSetIfLess (ret.elm(0,dd),data[ii][dd]);
fgSetIfGreater (ret.elm(1,dd),data[ii][dd]);
}
}
return ret;
}
