vector<T> matrix<T>::solve(vector<T> RHS)
{
#ifdef _DEBUG
if (this->dims[0] != this->dims[1])
FatalErrorST("Can only use Gaussian elimination on a square matrix.");
#endif
// Gaussian elimination with full pivoting
// not to be used where speed is paramount
vector<T> vec(this->dims[0]);
vector<T> solution(this->dims[0]);
vector<int> swap_0(this->dims[0],1);
vector<int> swap_1(this->dims[0],1);
vector<T> tmpRow(this->dims[0]);
matrix<T> LHS(*this);
// setup swap arrays
for(uint i=0; i<this->dims[0]; i++) {
swap_0[i]=i;
swap_1[i]=i;
}
// make triangular
for(uint k=0; k<this->dims[0]-1; k++) {
T max = 0;
T mag;
// find pivot
int pivot_i = 0;
int pivot_j = 0;
for(uint i=k; i<this->dims[0]; i++) {
for(uint j=k; j<this->dims[0]; j++) {
mag = LHS(i,j)*LHS(i,j);
if(mag>max) {
pivot_i = i;
pivot_j = j;
max = mag;
}
}
}
// swap the swap arrays
int itemp_0 = swap_0[k];
swap_0[k] = swap_0[pivot_i];
swap_0[pivot_i] = itemp_0;
itemp_0 = swap_1[k];
swap_1[k] = swap_1[pivot_j];
swap_1[pivot_j] = itemp_0;
// swap the columns
for(uint i=0; i<this->dims[0]; i++) {
tmpRow[i] = LHS(i,pivot_j);
LHS(i,pivot_j) = LHS(i,k);
LHS(i,k) = tmpRow[i];
}
// swap the rows
for(uint j=0; j<this->dims[0]; j++) {
tmpRow[j] = LHS(pivot_i,j);
LHS(pivot_i,j) = LHS(k,j);
LHS(k,j) = tmpRow[j];
}
T tmp = RHS[pivot_i];
RHS[pivot_i] = RHS[k];
RHS[k] = tmp;
// subtraction
for(uint i=k+1; i<this->dims[0]; i++) {
T first = LHS(i,k);
RHS[i] = RHS[i] - first/LHS(k,k)*RHS[k];
for(uint j=k; j<this->dims[0]; j++)
LHS(i,j) = LHS(i,j) - (first/LHS(k,k))*LHS(k,j);
}
// exact zero
for(uint j=0; j<k+1; j++) {
for(uint i=j+1; i<this->dims[0]; i++) {
LHS(i,j)=0.0;
}
}
}
// back substitute
for(int i=(int)this->dims[0]-1; i>=0; i--) {
T dtemp_0 = 0.0;
for(uint k = i+1; k<this->dims[0]; k++) {
dtemp_0 = dtemp_0 + (LHS(i,k)*vec[k]);
}
vec[i] = (RHS[i]-dtemp_0)/LHS(i,i);
}
// swap solution rows
for(uint i=0; i<this->dims[0]; i++)
solution[swap_1[i]] = vec[i];
return solution;
}
OutputIterator
ch_bykat_with_threshold(InputIterator first, InputIterator last,
OutputIterator result,
const Traits& ch_traits)
{
typedef typename Traits::Point_2 Point_2;
typedef typename Traits::Left_turn_2 Left_turn_2;
typedef typename Traits::Less_signed_distance_to_line_2
Less_dist;
typedef typename std::vector< Point_2 >::iterator
PointIterator;
typedef typename Traits::Equal_2 Equal_2;
Equal_2 equal_points = ch_traits.equal_2_object();
if (first == last) return result;
std::vector< Point_2 > P; // points in subsets
std::vector< Point_2 > H; // right endpoints of subproblems
P.reserve(16);
H.reserve(16);
std::vector< PointIterator > L; // start of subset range
std::vector< PointIterator > R; // end of subset range
L.reserve(16);
R.reserve(16);
PointIterator l;
PointIterator r;
Point_2 a,b,c;
PointIterator Pbegin, Pend;
P.push_back(Point_2() );
std::copy(first,last,std::back_inserter(P));
P.push_back(Point_2() );
Pbegin = successor(P.begin());
Pend = predecessor(P.end());
ch_we_point(Pbegin, Pend, l, r, ch_traits);
a = *l;
b = *r;
if (equal_points(a,b))
{
*result = a; ++result;
return result;
}
#if defined(CGAL_CH_NO_POSTCONDITIONS) || defined(CGAL_NO_POSTCONDITIONS) \
|| defined(NDEBUG)
OutputIterator res(result);
#else
Tee_for_output_iterator<OutputIterator,Point_2> res(result);
#endif // no postconditions ...
H.push_back( a );
L.push_back( Pbegin );
Left_turn_2 left_turn = ch_traits.left_turn_2_object();
R.push_back( l = std::partition( Pbegin, Pend,
bind_1(bind_1(left_turn, a), b) ) );
r = std::partition( l, Pend, bind_1(bind_1(left_turn,b),a) );
Less_dist less_dist = ch_traits.less_signed_distance_to_line_2_object();
for (;;)
{
if ( l != r)
{
if ( r-l > CGAL_ch_THRESHOLD )
{
c = *std::min_element( l, r, bind_1(bind_1(less_dist, a), b));
H.push_back( b );
L.push_back( l );
R.push_back( l = std::partition(l, r,
bind_1(bind_1(left_turn, b), c)) );
r = std::partition(l, r, bind_1(bind_1(left_turn, c), a));
b = c;
}
else
{
std::swap( a, *--l);
std::swap( b, *++r);
if ( ch_traits.less_xy_2_object()(*l,*r) )
{
std::sort(successor(l), r,
ch_traits.less_xy_2_object() );
}
else
{
std::sort(successor(l), r,
swap_1(ch_traits.less_xy_2_object()) );
}
ch__ref_graham_andrew_scan(l, successor(r), res, ch_traits);
std::swap( a, *l);
std::swap( b, *r);
if ( L.empty() ) break;
a = b;
b = H.back(); H.pop_back();
l = L.back(); L.pop_back();
r = R.back(); R.pop_back();
}
}
else
{
*res = a; ++res;
if ( L.empty() ) break;
a = b;
b = H.back(); H.pop_back();
l = L.back(); L.pop_back();
r = R.back(); R.pop_back();
}
}
CGAL_ch_postcondition( \
is_ccw_strongly_convex_2( res.output_so_far_begin(), \
res.output_so_far_end(), \
ch_traits));
CGAL_ch_expensive_postcondition( \
ch_brute_force_check_2( \
Pbegin, Pend, \
res.output_so_far_begin(), res.output_so_far_end(), \
ch_traits));
#if defined(CGAL_CH_NO_POSTCONDITIONS) || defined(CGAL_NO_POSTCONDITIONS) \
|| defined(NDEBUG)
return res;
#else
return res.to_output_iterator();
#endif // no postconditions ...
}
matrix<T> matrix<T>::invertMatrix(void)
{
#ifdef _DEBUG
if (this->dims[0] != this->dims[1])
FatalErrorST("Can only obtain inverse of a square matrix.");
#endif
// Gaussian elimination with full pivoting
// not to be used where speed is paramount
int pivot_i = 0;
int pivot_j = 0;
int itemp_0;
double mag;
double max;
double dtemp_0;
double first;
matrix <T> atemp_0(this->dims[0],1);
matrix <T> identity(this->dims[0],this->dims[0]);
matrix <T> input(this->dims[0],this->dims[0]);
matrix <T> inverse(this->dims[0],this->dims[0]);
matrix <T> inverse_out(this->dims[0],this->dims[0]);
matrix <int> swap_0(this->dims[0],1);
matrix <int> swap_1(this->dims[0],1);
input = *this;
// setup swap arrays
for(uint i=0;i<this->dims[0];i++) {
swap_0(i)=i;
swap_1(i)=i;
}
// setup identity array
for(uint i=0; i<this->dims[0]; i++) {
for(uint j=0; j<this->dims[0]; j++) {
identity(i,j)=0.0;
}
identity(i,i)=1.0;
}
// make triangular
for(uint k=0; k<this->dims[0]-1; k++) {
max=0;
// find pivot
for(uint i=k; i<this->dims[0]; i++) {
for(uint j=k; j<this->dims[0]; j++) {
mag=input(i,j)*input(i,j);
if(mag>max) {
pivot_i=i;
pivot_j=j;
max=mag;
}
}
}
// swap the swap arrays
itemp_0=swap_0(k);
swap_0(k)=swap_0(pivot_i);
swap_0(pivot_i)=itemp_0;
itemp_0=swap_1(k);
swap_1(k)=swap_1(pivot_j);
swap_1(pivot_j)=itemp_0;
// swap the columns
for(uint i=0; i<this->dims[0]; i++) {
atemp_0(i)=input(i,pivot_j);
input(i,pivot_j)=input(i,k);
input(i,k)=atemp_0(i);
}
// swap the rows
for(uint j=0; j<this->dims[0]; j++) {
atemp_0(j)=input(pivot_i,j);
input(pivot_i,j)=input(k,j);
input(k,j)=atemp_0(j);
atemp_0(j)=identity(pivot_i,j);
identity(pivot_i,j)=identity(k,j);
identity(k,j)=atemp_0(j);
}
// subtraction
for(uint i=k+1; i<this->dims[0]; i++) {
first=input(i,k);
for(uint j=0; j<this->dims[0]; j++) {
if(j>=k) {
input(i,j)=input(i,j)-((first/input(k,k))*input(k,j));
}
identity(i,j)=identity(i,j)-((first/input(k,k))*identity(k,j));
}
}
//exact zero
for(uint j=0; j<k+1; j++) {
for(uint i=j+1; i<this->dims[0]; i++) {
input(i,j)=0.0;
}
}
}
// back substitute
for(int i=(int)this->dims[0]-1; i>=0; i--) {
for(uint j=0; j<this->dims[0]; j++) {
dtemp_0=0.0;
for(uint k=i+1; k<this->dims[0]; k++) {
dtemp_0=dtemp_0+(input(i,k)*inverse(k,j));
}
inverse(i,j)=(identity(i,j)-dtemp_0)/input(i,i);
}
}
// swap solution rows
for(uint i=0; i<this->dims[0]; i++) {
for(uint j=0; j<this->dims[0]; j++) {
inverse_out(swap_1(i),j)=inverse(i,j);
}
}
return inverse_out;
}
