<commit_before>/*=========================================================================

  Program:   Visualization Toolkit
  Module:    vtkDataSet.cxx
  Language:  C++
  Date:      $Date$
  Version:   $Revision$

Copyright (c) 1993-2001 Ken Martin, Will Schroeder, Bill Lorensen
All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:

 * Redistributions of source code must retain the above copyright notice,
   this list of conditions and the following disclaimer.

 * Redistributions in binary form must reproduce the above copyright notice,
   this list of conditions and the following disclaimer in the documentation
   and/or other materials provided with the distribution.

 * Neither name of Ken Martin, Will Schroeder, or Bill Lorensen nor the names
   of any contributors may be used to endorse or promote products derived
   from this software without specific prior written permission.

 * Modified source versions must be plainly marked as such, and must not be
   misrepresented as being the original software.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS ``AS IS''
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHORS OR CONTRIBUTORS BE LIABLE FOR
ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

=========================================================================*/
//
// DataSet methods
//
#include <math.h>
#include "vtkDataSet.h"
#include "vtkSource.h"

//----------------------------------------------------------------------------
// Constructor with default bounds (0,1, 0,1, 0,1).
vtkDataSet::vtkDataSet ()
{
  this->Bounds[0] = 0.0;
  this->Bounds[1] = 1.0;
  this->Bounds[2] = 0.0;
  this->Bounds[3] = 1.0;
  this->Bounds[4] = 0.0;
  this->Bounds[5] = 1.0;

  this->PointData = vtkPointData::New();
  this->CellData = vtkCellData::New();
}

//----------------------------------------------------------------------------
vtkDataSet::~vtkDataSet ()
{
  this->PointData->Delete();
  this->CellData->Delete();
}

//----------------------------------------------------------------------------
void vtkDataSet::Initialize()
{
  // We don't modify ourselves because the "ReleaseData" methods depend upon
  // no modification when initialized.
  vtkDataObject::Initialize();

  this->CellData->Initialize();
  this->PointData->Initialize();
}

//----------------------------------------------------------------------------
// Compute the data bounding box from data points.
void vtkDataSet::ComputeBounds()
{
  int i, j;
  float *x;

  if ( this->GetMTime() > this->ComputeTime )
    {
    this->Bounds[0] = this->Bounds[2] = this->Bounds[4] =  VTK_LARGE_FLOAT;
    this->Bounds[1] = this->Bounds[3] = this->Bounds[5] = -VTK_LARGE_FLOAT;
    for (i=0; i<this->GetNumberOfPoints(); i++)
      {
      x = this->GetPoint(i);
      for (j=0; j<3; j++)
        {
        if ( x[j] < this->Bounds[2*j] )
          {
          this->Bounds[2*j] = x[j];
          }
        if ( x[j] > this->Bounds[2*j+1] )
          {
          this->Bounds[2*j+1] = x[j];
          }
        }
      }

    this->ComputeTime.Modified();
    }
}

//----------------------------------------------------------------------------
void vtkDataSet::GetScalarRange(float range[2])
{
  vtkScalars *ptScalars, *cellScalars;
  ptScalars = this->PointData->GetScalars();
  cellScalars = this->CellData->GetScalars();

  if ( ptScalars && cellScalars)
    {
    float r1[2], r2[2];
    ptScalars->GetRange(r1);
    cellScalars->GetRange(r2);
    range[0] = (r1[0] < r2[0] ? r1[0] : r2[0]);
    range[1] = (r1[1] > r2[1] ? r1[1] : r2[1]);
    }
  else if ( ptScalars )
    {
    ptScalars->GetRange(range);
    }
  else if ( cellScalars )
    {
    cellScalars->GetRange(range);
    }
  else
    {
    range[0] = 0.0;
    range[1] = 1.0;
    }
}

//----------------------------------------------------------------------------
float *vtkDataSet::GetScalarRange()
{
  this->GetScalarRange(this->ScalarRange);
  return this->ScalarRange;
}

//----------------------------------------------------------------------------
// Return a pointer to the geometry bounding box in the form
// (xmin,xmax, ymin,ymax, zmin,zmax).
float *vtkDataSet::GetBounds()
{
  this->ComputeBounds();
  return this->Bounds;
}

//----------------------------------------------------------------------------
void vtkDataSet::GetBounds(float bounds[6])
{
  this->ComputeBounds();
  for (int i=0; i<6; i++)
    {
    bounds[i] = this->Bounds[i];
    }
}

//----------------------------------------------------------------------------
// Get the center of the bounding box.
float *vtkDataSet::GetCenter()
{
  this->ComputeBounds();
  for (int i=0; i<3; i++)
    {
    this->Center[i] = (this->Bounds[2*i+1] + this->Bounds[2*i]) / 2.0;
    }
  return this->Center;
}

//----------------------------------------------------------------------------
void vtkDataSet::GetCenter(float center[3])
{
  this->ComputeBounds();
  for (int i=0; i<3; i++)
    {
    center[i] = (this->Bounds[2*i+1] + this->Bounds[2*i]) / 2.0;
    }
}

//----------------------------------------------------------------------------
// Return the length of the diagonal of the bounding box.
float vtkDataSet::GetLength()
{
  double diff, l=0.0;
  int i;

  this->ComputeBounds();
  for (i=0; i<3; i++)
    {
    diff = this->Bounds[2*i+1] - this->Bounds[2*i];
    l += diff * diff;
    }

  return (float)sqrt(l);
}

//----------------------------------------------------------------------------
unsigned long int vtkDataSet::GetMTime()
{
  unsigned long mtime, result;

  result = vtkDataObject::GetMTime();
  if (this->Source)
    {
    mtime = this->Source->GetMTime();
    result = ( mtime > result ? mtime : result );
    }

  mtime = this->PointData->GetMTime();
  result = ( mtime > result ? mtime : result );

  mtime = this->CellData->GetMTime();
  return ( mtime > result ? mtime : result );
}

//----------------------------------------------------------------------------
vtkCell *vtkDataSet::FindAndGetCell (float x[3], vtkCell *cell, int cellId,
                                     float tol2, int& subId,
                                     float pcoords[3], float *weights)
{
  int newCell = this->FindCell(x,cell,cellId,tol2,subId,pcoords,weights);
  if (newCell >= 0 )
    {
    cell = this->GetCell (newCell);
    }
  else
    {
    return NULL;
    }
  return cell;
}

//----------------------------------------------------------------------------
void vtkDataSet::GetCellNeighbors(int cellId, vtkIdList *ptIds,
                                  vtkIdList *cellIds)
{
  int i, numPts;
  vtkIdList *otherCells = vtkIdList::New();
  otherCells->Allocate(VTK_CELL_SIZE);

  // load list with candidate cells, remove current cell
  this->GetPointCells(ptIds->GetId(0), cellIds);
  cellIds->DeleteId(cellId);

  // now perform multiple intersections on list
  if ( cellIds->GetNumberOfIds() > 0 )
    {
    for ( numPts=ptIds->GetNumberOfIds(), i=1; i < numPts; i++)
      {
      this->GetPointCells(ptIds->GetId(i), otherCells);
      cellIds->IntersectWith(*otherCells);
      }
    }

  otherCells->Delete();
}

//----------------------------------------------------------------------------
void vtkDataSet::GetCellTypes(vtkCellTypes *types)
{
  int cellId, numCells=this->GetNumberOfCells();
  unsigned char type;

  types->Reset();
  for (cellId=0; cellId < numCells; cellId++)
    {
    type = this->GetCellType(cellId);
    if ( ! types->IsType(type) )
      {
      types->InsertNextType(type);
      }
    }
}

//----------------------------------------------------------------------------
// Default implementation. This is very slow way to compute this information.
// Subclasses should override this method for efficiency.
void vtkDataSet::GetCellBounds(int cellId, float bounds[6])
{
  vtkGenericCell *cell = vtkGenericCell::New();

  this->GetCell(cellId, cell);
  cell->GetBounds(bounds);

  cell->Delete();
}

//----------------------------------------------------------------------------
void vtkDataSet::Squeeze()
{
  this->CellData->Squeeze();
  this->PointData->Squeeze();
}

//----------------------------------------------------------------------------
unsigned long vtkDataSet::GetActualMemorySize()
{
  unsigned long size=this->vtkDataObject::GetActualMemorySize();
  size += this->PointData->GetActualMemorySize();
  size += this->CellData->GetActualMemorySize();
  return size;
}

//----------------------------------------------------------------------------
void vtkDataSet::ShallowCopy(vtkDataObject *dataObject)
{
  vtkDataSet *dataSet = vtkDataSet::SafeDownCast(dataObject);

  if ( dataSet != NULL )
    {
    this->InternalDataSetCopy(dataSet);
    this->CellData->ShallowCopy(dataSet->GetCellData());
    this->PointData->ShallowCopy(dataSet->GetPointData());
    }
  // Do superclass
  this->vtkDataObject::ShallowCopy(dataObject);
}

//----------------------------------------------------------------------------
void vtkDataSet::DeepCopy(vtkDataObject *dataObject)
{
  vtkDataSet *dataSet = vtkDataSet::SafeDownCast(dataObject);

  if ( dataSet != NULL )
    {
    this->InternalDataSetCopy(dataSet);
    this->CellData->DeepCopy(dataSet->GetCellData());
    this->PointData->DeepCopy(dataSet->GetPointData());
    }

  // Do superclass
  this->vtkDataObject::DeepCopy(dataObject);
}

//----------------------------------------------------------------------------
// This copies all the local variables (but not objects).
void vtkDataSet::InternalDataSetCopy(vtkDataSet *src)
{
  int idx;

  this->ComputeTime = src->ComputeTime;
  this->ScalarRange[0] = src->ScalarRange[0];
  this->ScalarRange[1] = src->ScalarRange[1];
  for (idx = 0; idx < 3; ++idx)
    {
    this->Bounds[idx] = src->Bounds[idx];
    this->Bounds[idx*2] = src->Bounds[idx*2];
    }
}

//----------------------------------------------------------------------------
void vtkDataSet::PrintSelf(ostream& os, vtkIndent indent)
{
  float *bounds;

  vtkDataObject::PrintSelf(os,indent);

  os << indent << "Number Of Points: " << this->GetNumberOfPoints() << "\n";
  os << indent << "Number Of Cells: " << this->GetNumberOfCells() << "\n";

  os << indent << "Cell Data:\n";
  this->CellData->PrintSelf(os,indent.GetNextIndent());

  os << indent << "Point Data:\n";
  this->PointData->PrintSelf(os,indent.GetNextIndent());

  bounds = this->GetBounds();
  os << indent << "Bounds: \n";
  os << indent << "  Xmin,Xmax: (" <<bounds[0] << ", " << bounds[1] << ")\n";
  os << indent << "  Ymin,Ymax: (" <<bounds[2] << ", " << bounds[3] << ")\n";
  os << indent << "  Zmin,Zmax: (" <<bounds[4] << ", " << bounds[5] << ")\n";
  os << indent << "Compute Time: " <<this->ComputeTime.GetMTime() << "\n";
  os << indent << "Release Data: " << (this->ReleaseDataFlag ? "On\n" : "Off\n");
}

<commit_msg>ERR:Bad copying of bounds<commit_after>/*=========================================================================

  Program:   Visualization Toolkit
  Module:    vtkDataSet.cxx
  Language:  C++
  Date:      $Date$
  Version:   $Revision$

Copyright (c) 1993-2001 Ken Martin, Will Schroeder, Bill Lorensen
All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:

 * Redistributions of source code must retain the above copyright notice,
   this list of conditions and the following disclaimer.

 * Redistributions in binary form must reproduce the above copyright notice,
   this list of conditions and the following disclaimer in the documentation
   and/or other materials provided with the distribution.

 * Neither name of Ken Martin, Will Schroeder, or Bill Lorensen nor the names
   of any contributors may be used to endorse or promote products derived
   from this software without specific prior written permission.

 * Modified source versions must be plainly marked as such, and must not be
   misrepresented as being the original software.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS ``AS IS''
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHORS OR CONTRIBUTORS BE LIABLE FOR
ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

=========================================================================*/
//
// DataSet methods
//
#include <math.h>
#include "vtkDataSet.h"
#include "vtkSource.h"

//----------------------------------------------------------------------------
// Constructor with default bounds (0,1, 0,1, 0,1).
vtkDataSet::vtkDataSet ()
{
  this->Bounds[0] = 0.0;
  this->Bounds[1] = 1.0;
  this->Bounds[2] = 0.0;
  this->Bounds[3] = 1.0;
  this->Bounds[4] = 0.0;
  this->Bounds[5] = 1.0;

  this->PointData = vtkPointData::New();
  this->CellData = vtkCellData::New();
}

//----------------------------------------------------------------------------
vtkDataSet::~vtkDataSet ()
{
  this->PointData->Delete();
  this->CellData->Delete();
}

//----------------------------------------------------------------------------
void vtkDataSet::Initialize()
{
  // We don't modify ourselves because the "ReleaseData" methods depend upon
  // no modification when initialized.
  vtkDataObject::Initialize();

  this->CellData->Initialize();
  this->PointData->Initialize();
}

//----------------------------------------------------------------------------
// Compute the data bounding box from data points.
void vtkDataSet::ComputeBounds()
{
  int i, j;
  float *x;

  if ( this->GetMTime() > this->ComputeTime )
    {
    this->Bounds[0] = this->Bounds[2] = this->Bounds[4] =  VTK_LARGE_FLOAT;
    this->Bounds[1] = this->Bounds[3] = this->Bounds[5] = -VTK_LARGE_FLOAT;
    for (i=0; i<this->GetNumberOfPoints(); i++)
      {
      x = this->GetPoint(i);
      for (j=0; j<3; j++)
        {
        if ( x[j] < this->Bounds[2*j] )
          {
          this->Bounds[2*j] = x[j];
          }
        if ( x[j] > this->Bounds[2*j+1] )
          {
          this->Bounds[2*j+1] = x[j];
          }
        }
      }

    this->ComputeTime.Modified();
    }
}

//----------------------------------------------------------------------------
void vtkDataSet::GetScalarRange(float range[2])
{
  vtkScalars *ptScalars, *cellScalars;
  ptScalars = this->PointData->GetScalars();
  cellScalars = this->CellData->GetScalars();

  if ( ptScalars && cellScalars)
    {
    float r1[2], r2[2];
    ptScalars->GetRange(r1);
    cellScalars->GetRange(r2);
    range[0] = (r1[0] < r2[0] ? r1[0] : r2[0]);
    range[1] = (r1[1] > r2[1] ? r1[1] : r2[1]);
    }
  else if ( ptScalars )
    {
    ptScalars->GetRange(range);
    }
  else if ( cellScalars )
    {
    cellScalars->GetRange(range);
    }
  else
    {
    range[0] = 0.0;
    range[1] = 1.0;
    }
}

//----------------------------------------------------------------------------
float *vtkDataSet::GetScalarRange()
{
  this->GetScalarRange(this->ScalarRange);
  return this->ScalarRange;
}

//----------------------------------------------------------------------------
// Return a pointer to the geometry bounding box in the form
// (xmin,xmax, ymin,ymax, zmin,zmax).
float *vtkDataSet::GetBounds()
{
  this->ComputeBounds();
  return this->Bounds;
}

//----------------------------------------------------------------------------
void vtkDataSet::GetBounds(float bounds[6])
{
  this->ComputeBounds();
  for (int i=0; i<6; i++)
    {
    bounds[i] = this->Bounds[i];
    }
}

//----------------------------------------------------------------------------
// Get the center of the bounding box.
float *vtkDataSet::GetCenter()
{
  this->ComputeBounds();
  for (int i=0; i<3; i++)
    {
    this->Center[i] = (this->Bounds[2*i+1] + this->Bounds[2*i]) / 2.0;
    }
  return this->Center;
}

//----------------------------------------------------------------------------
void vtkDataSet::GetCenter(float center[3])
{
  this->ComputeBounds();
  for (int i=0; i<3; i++)
    {
    center[i] = (this->Bounds[2*i+1] + this->Bounds[2*i]) / 2.0;
    }
}

//----------------------------------------------------------------------------
// Return the length of the diagonal of the bounding box.
float vtkDataSet::GetLength()
{
  double diff, l=0.0;
  int i;

  this->ComputeBounds();
  for (i=0; i<3; i++)
    {
    diff = this->Bounds[2*i+1] - this->Bounds[2*i];
    l += diff * diff;
    }

  return (float)sqrt(l);
}

//----------------------------------------------------------------------------
unsigned long int vtkDataSet::GetMTime()
{
  unsigned long mtime, result;

  result = vtkDataObject::GetMTime();
  if (this->Source)
    {
    mtime = this->Source->GetMTime();
    result = ( mtime > result ? mtime : result );
    }

  mtime = this->PointData->GetMTime();
  result = ( mtime > result ? mtime : result );

  mtime = this->CellData->GetMTime();
  return ( mtime > result ? mtime : result );
}

//----------------------------------------------------------------------------
vtkCell *vtkDataSet::FindAndGetCell (float x[3], vtkCell *cell, int cellId,
                                     float tol2, int& subId,
                                     float pcoords[3], float *weights)
{
  int newCell = this->FindCell(x,cell,cellId,tol2,subId,pcoords,weights);
  if (newCell >= 0 )
    {
    cell = this->GetCell (newCell);
    }
  else
    {
    return NULL;
    }
  return cell;
}

//----------------------------------------------------------------------------
void vtkDataSet::GetCellNeighbors(int cellId, vtkIdList *ptIds,
                                  vtkIdList *cellIds)
{
  int i, numPts;
  vtkIdList *otherCells = vtkIdList::New();
  otherCells->Allocate(VTK_CELL_SIZE);

  // load list with candidate cells, remove current cell
  this->GetPointCells(ptIds->GetId(0), cellIds);
  cellIds->DeleteId(cellId);

  // now perform multiple intersections on list
  if ( cellIds->GetNumberOfIds() > 0 )
    {
    for ( numPts=ptIds->GetNumberOfIds(), i=1; i < numPts; i++)
      {
      this->GetPointCells(ptIds->GetId(i), otherCells);
      cellIds->IntersectWith(*otherCells);
      }
    }

  otherCells->Delete();
}

//----------------------------------------------------------------------------
void vtkDataSet::GetCellTypes(vtkCellTypes *types)
{
  int cellId, numCells=this->GetNumberOfCells();
  unsigned char type;

  types->Reset();
  for (cellId=0; cellId < numCells; cellId++)
    {
    type = this->GetCellType(cellId);
    if ( ! types->IsType(type) )
      {
      types->InsertNextType(type);
      }
    }
}

//----------------------------------------------------------------------------
// Default implementation. This is very slow way to compute this information.
// Subclasses should override this method for efficiency.
void vtkDataSet::GetCellBounds(int cellId, float bounds[6])
{
  vtkGenericCell *cell = vtkGenericCell::New();

  this->GetCell(cellId, cell);
  cell->GetBounds(bounds);

  cell->Delete();
}

//----------------------------------------------------------------------------
void vtkDataSet::Squeeze()
{
  this->CellData->Squeeze();
  this->PointData->Squeeze();
}

//----------------------------------------------------------------------------
unsigned long vtkDataSet::GetActualMemorySize()
{
  unsigned long size=this->vtkDataObject::GetActualMemorySize();
  size += this->PointData->GetActualMemorySize();
  size += this->CellData->GetActualMemorySize();
  return size;
}

//----------------------------------------------------------------------------
void vtkDataSet::ShallowCopy(vtkDataObject *dataObject)
{
  vtkDataSet *dataSet = vtkDataSet::SafeDownCast(dataObject);

  if ( dataSet != NULL )
    {
    this->InternalDataSetCopy(dataSet);
    this->CellData->ShallowCopy(dataSet->GetCellData());
    this->PointData->ShallowCopy(dataSet->GetPointData());
    }
  // Do superclass
  this->vtkDataObject::ShallowCopy(dataObject);
}

//----------------------------------------------------------------------------
void vtkDataSet::DeepCopy(vtkDataObject *dataObject)
{
  vtkDataSet *dataSet = vtkDataSet::SafeDownCast(dataObject);

  if ( dataSet != NULL )
    {
    this->InternalDataSetCopy(dataSet);
    this->CellData->DeepCopy(dataSet->GetCellData());
    this->PointData->DeepCopy(dataSet->GetPointData());
    }

  // Do superclass
  this->vtkDataObject::DeepCopy(dataObject);
}

//----------------------------------------------------------------------------
// This copies all the local variables (but not objects).
void vtkDataSet::InternalDataSetCopy(vtkDataSet *src)
{
  int idx;

  this->ComputeTime = src->ComputeTime;
  this->ScalarRange[0] = src->ScalarRange[0];
  this->ScalarRange[1] = src->ScalarRange[1];
  for (idx = 0; idx < 3; ++idx)
    {
    this->Bounds[2*idx] = src->Bounds[2*idx];
    this->Bounds[2*idx+1] = src->Bounds[2*idx+1];
    }
}

//----------------------------------------------------------------------------
void vtkDataSet::PrintSelf(ostream& os, vtkIndent indent)
{
  float *bounds;

  vtkDataObject::PrintSelf(os,indent);

  os << indent << "Number Of Points: " << this->GetNumberOfPoints() << "\n";
  os << indent << "Number Of Cells: " << this->GetNumberOfCells() << "\n";

  os << indent << "Cell Data:\n";
  this->CellData->PrintSelf(os,indent.GetNextIndent());

  os << indent << "Point Data:\n";
  this->PointData->PrintSelf(os,indent.GetNextIndent());

  bounds = this->GetBounds();
  os << indent << "Bounds: \n";
  os << indent << "  Xmin,Xmax: (" <<bounds[0] << ", " << bounds[1] << ")\n";
  os << indent << "  Ymin,Ymax: (" <<bounds[2] << ", " << bounds[3] << ")\n";
  os << indent << "  Zmin,Zmax: (" <<bounds[4] << ", " << bounds[5] << ")\n";
  os << indent << "Compute Time: " <<this->ComputeTime.GetMTime() << "\n";
  os << indent << "Release Data: " << (this->ReleaseDataFlag ? "On\n" : "Off\n");
}

<|endoftext|>


<commit_before>/*  This file is part of the Vc library.

    Copyright (C) 2009 Matthias Kretz <kretz@kde.org>

    Vc is free software: you can redistribute it and/or modify
    it under the terms of the GNU Lesser General Public License as
    published by the Free Software Foundation, either version 3 of
    the License, or (at your option) any later version.

    Vc is distributed in the hope that it will be useful, but
    WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU Lesser General Public License for more details.

    You should have received a copy of the GNU Lesser General Public
    License along with Vc.  If not, see <http://www.gnu.org/licenses/>.

*/

#include <Vc/float_v>
#include "benchmark.h"
#include <cstdio>
#include <cstdlib>

using namespace Vc;

static const int factor = 1000000;

static float randomF(float min, float max)
{
    const float delta = max - min;
    return min + delta * rand() / RAND_MAX;
}

static float randomF12() { return randomF(1.f, 2.f); }

int main()
{
    int blackHole = true;
    {
        Benchmark timer("SAXPY", 8. * float_v::Size * factor, "FLOP");
        for (int repetitions = 0; repetitions < 10; ++repetitions) {
            const float_v alpha[4] = {
                float_v(repetitions + randomF(.1f, .2f)),
                float_v(repetitions - randomF(.1f, .2f)),
                float_v(repetitions + randomF(.1f, .2f)),
                float_v(repetitions - randomF(.1f, .2f))
            };
            float_v x[4] = { randomF12(), randomF12(), randomF12(), randomF12() };
            const float_v y[4] = { randomF12(), randomF12(), randomF12(), randomF12() };

            // force the x vectors to registers, otherwise GCC decides to work on the stack and
            // lose half of the performance
            forceToRegisters(x[0], x[1], x[2], x[3]);

            timer.Start();
            ///////////////////////////////////////

            for (int i = 0; i < factor; ++i) {
                    x[0] = alpha[0] * x[0] + y[0];
                    x[1] = alpha[1] * x[1] + y[1];
                    x[2] = alpha[2] * x[2] + y[2];
                    x[3] = alpha[3] * x[3] + y[3];
            }

            ///////////////////////////////////////
            timer.Stop();

            const int k = (x[0] < x[1]) && (x[2] < x[3]);
            blackHole &= k;
        }
        timer.Print(Benchmark::PrintAverage);
    }

    // reference
    {
        Benchmark timer("SAXPY (reference)", 8. * float_v::Size * factor, "FLOP");
        for (int repetitions = 0; repetitions < 10; ++repetitions) {
#ifdef USE_SSE
            __m128 tmp = _mm_set1_ps(static_cast<float>(repetitions));
            const __m128 oPoint2 = _mm_set1_ps(randomF(.1f, .2f));
            const __m128 oPoint1 = _mm_set1_ps(randomF(.1f, .2f));
            const __m128 alpha[4] = {
                _mm_add_ps(tmp, oPoint2),
                _mm_sub_ps(tmp, oPoint2),
                _mm_add_ps(tmp, oPoint1),
                _mm_sub_ps(tmp, oPoint1)
            };
            __m128 x[4] = { _mm_set1_ps(randomF12()), _mm_set1_ps(randomF12()), _mm_set1_ps(randomF12()), _mm_set1_ps(randomF12()) };
            const __m128 y[4] = { _mm_set1_ps(randomF12()), _mm_set1_ps(randomF12()), _mm_set1_ps(randomF12()), _mm_set1_ps(randomF12()) };

            timer.Start();
            ///////////////////////////////////////

            for (int i = 0; i < factor; ++i) {
                    x[0] = _mm_add_ps(_mm_mul_ps(alpha[0], x[0]), y[0]);
                    x[1] = _mm_add_ps(_mm_mul_ps(alpha[1], x[1]), y[1]);
                    x[2] = _mm_add_ps(_mm_mul_ps(alpha[2], x[2]), y[2]);
                    x[3] = _mm_add_ps(_mm_mul_ps(alpha[3], x[3]), y[3]);
            }

            ///////////////////////////////////////
            timer.Stop();

            const int k = _mm_movemask_ps(_mm_add_ps(_mm_add_ps(x[0], x[1]), _mm_add_ps(x[2], x[3])));
            blackHole &= k;
#elif defined(ENABLE_LARRABEE)
            __m512 tmp = _mm512_set_1to16_ps(static_cast<float>(repetitions));
            const __m512 oPoint2 = _mm512_set_1to16_ps(randomF(.1f, .2f));
            const __m512 oPoint1 = _mm512_set_1to16_ps(randomF(.1f, .2f));
            const __m512 alpha[4] = {
                _mm512_add_ps(tmp, oPoint2),
                _mm512_sub_ps(tmp, oPoint2),
                _mm512_add_ps(tmp, oPoint1),
                _mm512_sub_ps(tmp, oPoint1)
            };
            __m512 x[4] = { _mm512_set_1to16_ps(randomF12()), _mm512_set_1to16_ps(randomF12()), _mm512_set_1to16_ps(randomF12()), _mm512_set_1to16_ps(randomF12()) };
            const __m512 y[4] = { _mm512_set_1to16_ps(randomF12()), _mm512_set_1to16_ps(randomF12()), _mm512_set_1to16_ps(randomF12()), _mm512_set_1to16_ps(randomF12()) };

            timer.Start();
            ///////////////////////////////////////

            for (int i = 0; i < factor; ++i) {
                    x[0] = _mm512_madd132_ps(alpha[0], y[0], x[0]);
                    x[1] = _mm512_madd132_ps(alpha[1], y[1], x[1]);
                    x[2] = _mm512_madd132_ps(alpha[2], y[2], x[2]);
                    x[3] = _mm512_madd132_ps(alpha[3], y[3], x[3]);
            }

            ///////////////////////////////////////
            timer.Stop();

            const int k = _mm512_cmpeq_ps(_mm512_add_ps(x[0], x[1]), _mm512_add_ps(x[2], x[3]));
            blackHole &= k;
#else
            const float alpha[4] = {
                float(repetitions + randomF(.1f, .2f)),
                float(repetitions - randomF(.1f, .2f)),
                float(repetitions + randomF(.1f, .2f)),
                float(repetitions - randomF(.1f, .2f))
            };
            float x[4] = { randomF12(), randomF12(), randomF12(), randomF12() };
            const float y[4] = { randomF12(), randomF12(), randomF12(), randomF12() };

            timer.Start();
            ///////////////////////////////////////

            for (int i = 0; i < factor; ++i) {
                    x[0] = alpha[0] * x[0] + y[0];
                    x[1] = alpha[1] * x[1] + y[1];
                    x[2] = alpha[2] * x[2] + y[2];
                    x[3] = alpha[3] * x[3] + y[3];
            }

            ///////////////////////////////////////
            timer.Stop();

            const int k = (x[0] < x[1]) && (x[2] < x[3]);
            blackHole &= k;
#endif
        }
        timer.Print(Benchmark::PrintAverage);
    }
    if (blackHole != 0) {
        std::cout << std::endl;
    }
    return 0;
}
<commit_msg>switch alpha and x to get the best of madd132<commit_after>/*  This file is part of the Vc library.

    Copyright (C) 2009 Matthias Kretz <kretz@kde.org>

    Vc is free software: you can redistribute it and/or modify
    it under the terms of the GNU Lesser General Public License as
    published by the Free Software Foundation, either version 3 of
    the License, or (at your option) any later version.

    Vc is distributed in the hope that it will be useful, but
    WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU Lesser General Public License for more details.

    You should have received a copy of the GNU Lesser General Public
    License along with Vc.  If not, see <http://www.gnu.org/licenses/>.

*/

#include <Vc/float_v>
#include "benchmark.h"
#include <cstdio>
#include <cstdlib>

using namespace Vc;

static const int factor = 1000000;

static float randomF(float min, float max)
{
    const float delta = max - min;
    return min + delta * rand() / RAND_MAX;
}

static float randomF12() { return randomF(1.f, 2.f); }

int main()
{
    int blackHole = true;
    {
        Benchmark timer("SAXPY", 8. * float_v::Size * factor, "FLOP");
        for (int repetitions = 0; repetitions < 10; ++repetitions) {
            const float_v alpha[4] = {
                float_v(repetitions + randomF(.1f, .2f)),
                float_v(repetitions - randomF(.1f, .2f)),
                float_v(repetitions + randomF(.1f, .2f)),
                float_v(repetitions - randomF(.1f, .2f))
            };
            float_v x[4] = { randomF12(), randomF12(), randomF12(), randomF12() };
            const float_v y[4] = { randomF12(), randomF12(), randomF12(), randomF12() };

            // force the x vectors to registers, otherwise GCC decides to work on the stack and
            // lose half of the performance
            forceToRegisters(x[0], x[1], x[2], x[3]);

            timer.Start();
            ///////////////////////////////////////

            for (int i = 0; i < factor; ++i) {
                    x[0] = alpha[0] * x[0] + y[0];
                    x[1] = alpha[1] * x[1] + y[1];
                    x[2] = alpha[2] * x[2] + y[2];
                    x[3] = alpha[3] * x[3] + y[3];
            }

            ///////////////////////////////////////
            timer.Stop();

            const int k = (x[0] < x[1]) && (x[2] < x[3]);
            blackHole &= k;
        }
        timer.Print(Benchmark::PrintAverage);
    }

    // reference
    {
        Benchmark timer("SAXPY (reference)", 8. * float_v::Size * factor, "FLOP");
        for (int repetitions = 0; repetitions < 10; ++repetitions) {
#ifdef USE_SSE
            __m128 tmp = _mm_set1_ps(static_cast<float>(repetitions));
            const __m128 oPoint2 = _mm_set1_ps(randomF(.1f, .2f));
            const __m128 oPoint1 = _mm_set1_ps(randomF(.1f, .2f));
            const __m128 alpha[4] = {
                _mm_add_ps(tmp, oPoint2),
                _mm_sub_ps(tmp, oPoint2),
                _mm_add_ps(tmp, oPoint1),
                _mm_sub_ps(tmp, oPoint1)
            };
            __m128 x[4] = { _mm_set1_ps(randomF12()), _mm_set1_ps(randomF12()), _mm_set1_ps(randomF12()), _mm_set1_ps(randomF12()) };
            const __m128 y[4] = { _mm_set1_ps(randomF12()), _mm_set1_ps(randomF12()), _mm_set1_ps(randomF12()), _mm_set1_ps(randomF12()) };

            timer.Start();
            ///////////////////////////////////////

            for (int i = 0; i < factor; ++i) {
                    x[0] = _mm_add_ps(_mm_mul_ps(alpha[0], x[0]), y[0]);
                    x[1] = _mm_add_ps(_mm_mul_ps(alpha[1], x[1]), y[1]);
                    x[2] = _mm_add_ps(_mm_mul_ps(alpha[2], x[2]), y[2]);
                    x[3] = _mm_add_ps(_mm_mul_ps(alpha[3], x[3]), y[3]);
            }

            ///////////////////////////////////////
            timer.Stop();

            const int k = _mm_movemask_ps(_mm_add_ps(_mm_add_ps(x[0], x[1]), _mm_add_ps(x[2], x[3])));
            blackHole &= k;
#elif defined(ENABLE_LARRABEE)
            __m512 tmp = _mm512_set_1to16_ps(static_cast<float>(repetitions));
            const __m512 oPoint2 = _mm512_set_1to16_ps(randomF(.1f, .2f));
            const __m512 oPoint1 = _mm512_set_1to16_ps(randomF(.1f, .2f));
            const __m512 alpha[4] = {
                _mm512_add_ps(tmp, oPoint2),
                _mm512_sub_ps(tmp, oPoint2),
                _mm512_add_ps(tmp, oPoint1),
                _mm512_sub_ps(tmp, oPoint1)
            };
            __m512 x[4] = { _mm512_set_1to16_ps(randomF12()), _mm512_set_1to16_ps(randomF12()), _mm512_set_1to16_ps(randomF12()), _mm512_set_1to16_ps(randomF12()) };
            const __m512 y[4] = { _mm512_set_1to16_ps(randomF12()), _mm512_set_1to16_ps(randomF12()), _mm512_set_1to16_ps(randomF12()), _mm512_set_1to16_ps(randomF12()) };

            timer.Start();
            ///////////////////////////////////////

            for (int i = 0; i < factor; ++i) {
                    x[0] = _mm512_madd132_ps(x[0], y[0], alpha[0]);
                    x[1] = _mm512_madd132_ps(x[1], y[1], alpha[1]);
                    x[2] = _mm512_madd132_ps(x[2], y[2], alpha[2]);
                    x[3] = _mm512_madd132_ps(x[3], y[3], alpha[3]);
            }

            ///////////////////////////////////////
            timer.Stop();

            const int k = _mm512_cmpeq_ps(_mm512_add_ps(x[0], x[1]), _mm512_add_ps(x[2], x[3]));
            blackHole &= k;
#else
            const float alpha[4] = {
                float(repetitions + randomF(.1f, .2f)),
                float(repetitions - randomF(.1f, .2f)),
                float(repetitions + randomF(.1f, .2f)),
                float(repetitions - randomF(.1f, .2f))
            };
            float x[4] = { randomF12(), randomF12(), randomF12(), randomF12() };
            const float y[4] = { randomF12(), randomF12(), randomF12(), randomF12() };

            timer.Start();
            ///////////////////////////////////////

            for (int i = 0; i < factor; ++i) {
                    x[0] = alpha[0] * x[0] + y[0];
                    x[1] = alpha[1] * x[1] + y[1];
                    x[2] = alpha[2] * x[2] + y[2];
                    x[3] = alpha[3] * x[3] + y[3];
            }

            ///////////////////////////////////////
            timer.Stop();

            const int k = (x[0] < x[1]) && (x[2] < x[3]);
            blackHole &= k;
#endif
        }
        timer.Print(Benchmark::PrintAverage);
    }
    if (blackHole != 0) {
        std::cout << std::endl;
    }
    return 0;
}
<|endoftext|>


<commit_before>
#include "VolumeIntegration.h"
#include <stdio.h>

using namespace std;
using namespace Eigen;
using namespace Utilities;

#define SQR(x) ((x)*(x))
#define CUBE(x) ((x)*(x)*(x))

VolumeIntegration::VolumeIntegration(const unsigned int nVertices, const unsigned int nFaces, Vector3r * const vertices, const unsigned int* indices)
		: m_nVertices(nVertices), m_nFaces(nFaces), m_indices(indices), m_face_normals(nFaces), m_weights(nFaces)
{
	// compute center of mass
	m_x.setZero();
	for (unsigned int i(0); i < m_nVertices; ++i)
		m_x += vertices[i];
	m_x /= (Real)m_nVertices;

	m_vertices.reserve(nVertices);
	for (unsigned int i(0); i < m_nVertices; ++i)
		m_vertices[i] = vertices[i] - m_x;

	for (unsigned int i(0); i < m_nFaces; ++i)
    {
		const Vector3r &a = m_vertices[m_indices[3 * i]];
		const Vector3r &b = m_vertices[m_indices[3 * i + 1]];
		const Vector3r &c = m_vertices[m_indices[3 * i + 2]];

		const Vector3r d1 = b - a;
		const Vector3r d2 = c - a;

		m_face_normals[i] = d1.cross(d2);
        if (m_face_normals[i].isZero(1.e-10))
			m_face_normals[i].setZero();
        else
			m_face_normals[i].normalize();

		m_weights[i] = -m_face_normals[i].dot(a);
    }
}

void VolumeIntegration::compute_inertia_tensor(Real density)
{
    volume_integrals();
    m_volume = static_cast<Real>(T0);

    m_mass = static_cast<Real>(density * T0);

    /* compute center of mass */
    m_r[0] = static_cast<Real>(T1[0] / T0);
    m_r[1] = static_cast<Real>(T1[1] / T0);
    m_r[2] = static_cast<Real>(T1[2] / T0);

    /* compute inertia tensor */
    m_theta(0, 0) = static_cast<Real>(density * (T2[1] + T2[2]));
    m_theta(1, 1) = static_cast<Real>(density * (T2[2] + T2[0]));
    m_theta(2, 2) = static_cast<Real>(density * (T2[0] + T2[1]));
    m_theta(0, 1) = m_theta(1, 0) = -density * static_cast<Real>(TP[0]);
    m_theta(1, 2) = m_theta(2, 1) = -density * static_cast<Real>(TP[1]);
    m_theta(2, 0) = m_theta(0, 2) = -density * static_cast<Real>(TP[2]);

    /* translate inertia tensor to center of mass */
    m_theta(0, 0) -= m_mass * (m_r[1]*m_r[1] + m_r[2]*m_r[2]);
    m_theta(1, 1) -= m_mass * (m_r[2]*m_r[2] + m_r[0]*m_r[0]);
    m_theta(2, 2) -= m_mass * (m_r[0]*m_r[0] + m_r[1]*m_r[1]);
    m_theta(0, 1) = m_theta(1, 0) += m_mass * m_r[0] * m_r[1];
    m_theta(1, 2) = m_theta(2, 1) += m_mass * m_r[1] * m_r[2];
    m_theta(2, 0) = m_theta(0, 2) += m_mass * m_r[2] * m_r[0];

	m_r += m_x;
}

void VolumeIntegration::projection_integrals(unsigned int f)
{
    Real a0, a1, da;
    Real b0, b1, db;
    Real a0_2, a0_3, a0_4, b0_2, b0_3, b0_4;
    Real a1_2, a1_3, b1_2, b1_3;
    Real C1, Ca, Caa, Caaa, Cb, Cbb, Cbbb;
    Real Cab, Kab, Caab, Kaab, Cabb, Kabb;

    P1 = Pa = Pb = Paa = Pab = Pbb = Paaa = Paab = Pabb = Pbbb = 0.0;

    for (int i = 0; i < 3; i++)
    {
		a0 = m_vertices[m_indices[3 * f + i]][A];
		b0 = m_vertices[m_indices[3 * f + i]][B];
		a1 = m_vertices[m_indices[3 * f + ((i + 1) % 3)]][A];
		b1 = m_vertices[m_indices[3 * f + ((i + 1) % 3)]][B];

        da = a1 - a0;
        db = b1 - b0;
        a0_2 = a0 * a0; a0_3 = a0_2 * a0; a0_4 = a0_3 * a0;
        b0_2 = b0 * b0; b0_3 = b0_2 * b0; b0_4 = b0_3 * b0;
        a1_2 = a1 * a1; a1_3 = a1_2 * a1;
        b1_2 = b1 * b1; b1_3 = b1_2 * b1;

        C1 = a1 + a0;
        Ca = a1*C1 + a0_2; Caa = a1*Ca + a0_3; Caaa = a1*Caa + a0_4;
        Cb = b1*(b1 + b0) + b0_2; Cbb = b1*Cb + b0_3; Cbbb = b1*Cbb + b0_4;
        Cab = 3 * a1_2 + 2 * a1*a0 + a0_2; Kab = a1_2 + 2 * a1*a0 + 3 * a0_2;
        Caab = a0*Cab + 4 * a1_3; Kaab = a1*Kab + 4 * a0_3;
        Cabb = 4 * b1_3 + 3 * b1_2*b0 + 2 * b1*b0_2 + b0_3;
        Kabb = b1_3 + 2 * b1_2*b0 + 3 * b1*b0_2 + 4 * b0_3;

        P1 += db*C1;
        Pa += db*Ca;
        Paa += db*Caa;
        Paaa += db*Caaa;
        Pb += da*Cb;
        Pbb += da*Cbb;
        Pbbb += da*Cbbb;
        Pab += db*(b1*Cab + b0*Kab);
        Paab += db*(b1*Caab + b0*Kaab);
        Pabb += da*(a1*Cabb + a0*Kabb);
    }

    P1 /= 2.0;
    Pa /= 6.0;
    Paa /= 12.0;
    Paaa /= 20.0;
    Pb /= -6.0;
    Pbb /= -12.0;
    Pbbb /= -20.0;
    Pab /= 24.0;
    Paab /= 60.0;
    Pabb /= -60.0;
}

void VolumeIntegration::face_integrals(unsigned int f)
{
  Real w;
  Vector3r n;
  Real k1, k2, k3, k4;

  projection_integrals(f);

  w = m_weights[f];
  n = m_face_normals[f];
  k1 = 1 / n[C]; k2 = k1 * k1; k3 = k2 * k1; k4 = k3 * k1;

  Fa = k1 * Pa;
  Fb = k1 * Pb;
  Fc = -k2 * (n[A]*Pa + n[B]*Pb + w*P1);

  Faa = k1 * Paa;
  Fbb = k1 * Pbb;
  Fcc = k3 * (SQR(n[A])*Paa + 2*n[A]*n[B]*Pab + SQR(n[B])*Pbb
     + w*(2*(n[A]*Pa + n[B]*Pb) + w*P1));

  Faaa = k1 * Paaa;
  Fbbb = k1 * Pbbb;
  Fccc = -k4 * (CUBE(n[A])*Paaa + 3*SQR(n[A])*n[B]*Paab
       + 3*n[A]*SQR(n[B])*Pabb + CUBE(n[B])*Pbbb
       + 3*w*(SQR(n[A])*Paa + 2*n[A]*n[B]*Pab + SQR(n[B])*Pbb)
       + w*w*(3*(n[A]*Pa + n[B]*Pb) + w*P1));

  Faab = k1 * Paab;
  Fbbc = -k2 * (n[A]*Pabb + n[B]*Pbbb + w*Pbb);
  Fcca = k3 * (SQR(n[A])*Paaa + 2*n[A]*n[B]*Paab + SQR(n[B])*Pabb
     + w*(2*(n[A]*Paa + n[B]*Pab) + w*Pa));
}

void VolumeIntegration::volume_integrals()
{
    Real nx, ny, nz;

    T0  = T1[0] = T1[1] = T1[2]
        = T2[0] = T2[1] = T2[2]
        = TP[0] = TP[1] = TP[2] = 0;

	for (unsigned int i(0); i < m_nFaces; ++i)
    {
        Vector3r const& n = m_face_normals[i];
        nx = std::abs(n[0]);
        ny = std::abs(n[1]);
        nz = std::abs(n[2]);
        if (nx > ny && nx > nz)
            C = 0;
        else
            C = (ny > nz) ? 1 : 2;
        A = (C + 1) % 3;
        B = (A + 1) % 3;

        face_integrals(i);

        T0 += n[0] * ((A == 0) ? Fa : ((B == 0) ? Fb : Fc));

        T1[A] += n[A] * Faa;
        T1[B] += n[B] * Fbb;
        T1[C] += n[C] * Fcc;
        T2[A] += n[A] * Faaa;
        T2[B] += n[B] * Fbbb;
        T2[C] += n[C] * Fccc;
        TP[A] += n[A] * Faab;
        TP[B] += n[B] * Fbbc;
        TP[C] += n[C] * Fcca;
    }

    T1[0] /= 2; T1[1] /= 2; T1[2] /= 2;
    T2[0] /= 3; T2[1] /= 3; T2[2] /= 3;
    TP[0] /= 2; TP[1] /= 2; TP[2] /= 2;
}
<commit_msg>- bugfix<commit_after>
#include "VolumeIntegration.h"
#include <stdio.h>

using namespace std;
using namespace Eigen;
using namespace Utilities;

#define SQR(x) ((x)*(x))
#define CUBE(x) ((x)*(x)*(x))

VolumeIntegration::VolumeIntegration(const unsigned int nVertices, const unsigned int nFaces, Vector3r * const vertices, const unsigned int* indices)
		: m_nVertices(nVertices), m_nFaces(nFaces), m_indices(indices), m_face_normals(nFaces), m_weights(nFaces)
{
	// compute center of mass
	m_x.setZero();
	for (unsigned int i(0); i < m_nVertices; ++i)
		m_x += vertices[i];
	m_x /= (Real)m_nVertices;

	m_vertices.resize(nVertices);
	for (unsigned int i(0); i < m_nVertices; ++i)
		m_vertices[i] = vertices[i] - m_x;

	for (unsigned int i(0); i < m_nFaces; ++i)
    {
		const Vector3r &a = m_vertices[m_indices[3 * i]];
		const Vector3r &b = m_vertices[m_indices[3 * i + 1]];
		const Vector3r &c = m_vertices[m_indices[3 * i + 2]];

		const Vector3r d1 = b - a;
		const Vector3r d2 = c - a;

		m_face_normals[i] = d1.cross(d2);
        if (m_face_normals[i].isZero(1.e-10))
			m_face_normals[i].setZero();
        else
			m_face_normals[i].normalize();

		m_weights[i] = -m_face_normals[i].dot(a);
    }
}

void VolumeIntegration::compute_inertia_tensor(Real density)
{
    volume_integrals();
    m_volume = static_cast<Real>(T0);

    m_mass = static_cast<Real>(density * T0);

    /* compute center of mass */
    m_r[0] = static_cast<Real>(T1[0] / T0);
    m_r[1] = static_cast<Real>(T1[1] / T0);
    m_r[2] = static_cast<Real>(T1[2] / T0);

    /* compute inertia tensor */
    m_theta(0, 0) = static_cast<Real>(density * (T2[1] + T2[2]));
    m_theta(1, 1) = static_cast<Real>(density * (T2[2] + T2[0]));
    m_theta(2, 2) = static_cast<Real>(density * (T2[0] + T2[1]));
    m_theta(0, 1) = m_theta(1, 0) = -density * static_cast<Real>(TP[0]);
    m_theta(1, 2) = m_theta(2, 1) = -density * static_cast<Real>(TP[1]);
    m_theta(2, 0) = m_theta(0, 2) = -density * static_cast<Real>(TP[2]);

    /* translate inertia tensor to center of mass */
    m_theta(0, 0) -= m_mass * (m_r[1]*m_r[1] + m_r[2]*m_r[2]);
    m_theta(1, 1) -= m_mass * (m_r[2]*m_r[2] + m_r[0]*m_r[0]);
    m_theta(2, 2) -= m_mass * (m_r[0]*m_r[0] + m_r[1]*m_r[1]);
    m_theta(0, 1) = m_theta(1, 0) += m_mass * m_r[0] * m_r[1];
    m_theta(1, 2) = m_theta(2, 1) += m_mass * m_r[1] * m_r[2];
    m_theta(2, 0) = m_theta(0, 2) += m_mass * m_r[2] * m_r[0];

	m_r += m_x;
}

void VolumeIntegration::projection_integrals(unsigned int f)
{
    Real a0, a1, da;
    Real b0, b1, db;
    Real a0_2, a0_3, a0_4, b0_2, b0_3, b0_4;
    Real a1_2, a1_3, b1_2, b1_3;
    Real C1, Ca, Caa, Caaa, Cb, Cbb, Cbbb;
    Real Cab, Kab, Caab, Kaab, Cabb, Kabb;

    P1 = Pa = Pb = Paa = Pab = Pbb = Paaa = Paab = Pabb = Pbbb = 0.0;

    for (int i = 0; i < 3; i++)
    {
		a0 = m_vertices[m_indices[3 * f + i]][A];
		b0 = m_vertices[m_indices[3 * f + i]][B];
		a1 = m_vertices[m_indices[3 * f + ((i + 1) % 3)]][A];
		b1 = m_vertices[m_indices[3 * f + ((i + 1) % 3)]][B];

        da = a1 - a0;
        db = b1 - b0;
        a0_2 = a0 * a0; a0_3 = a0_2 * a0; a0_4 = a0_3 * a0;
        b0_2 = b0 * b0; b0_3 = b0_2 * b0; b0_4 = b0_3 * b0;
        a1_2 = a1 * a1; a1_3 = a1_2 * a1;
        b1_2 = b1 * b1; b1_3 = b1_2 * b1;

        C1 = a1 + a0;
        Ca = a1*C1 + a0_2; Caa = a1*Ca + a0_3; Caaa = a1*Caa + a0_4;
        Cb = b1*(b1 + b0) + b0_2; Cbb = b1*Cb + b0_3; Cbbb = b1*Cbb + b0_4;
        Cab = 3 * a1_2 + 2 * a1*a0 + a0_2; Kab = a1_2 + 2 * a1*a0 + 3 * a0_2;
        Caab = a0*Cab + 4 * a1_3; Kaab = a1*Kab + 4 * a0_3;
        Cabb = 4 * b1_3 + 3 * b1_2*b0 + 2 * b1*b0_2 + b0_3;
        Kabb = b1_3 + 2 * b1_2*b0 + 3 * b1*b0_2 + 4 * b0_3;

        P1 += db*C1;
        Pa += db*Ca;
        Paa += db*Caa;
        Paaa += db*Caaa;
        Pb += da*Cb;
        Pbb += da*Cbb;
        Pbbb += da*Cbbb;
        Pab += db*(b1*Cab + b0*Kab);
        Paab += db*(b1*Caab + b0*Kaab);
        Pabb += da*(a1*Cabb + a0*Kabb);
    }

    P1 /= 2.0;
    Pa /= 6.0;
    Paa /= 12.0;
    Paaa /= 20.0;
    Pb /= -6.0;
    Pbb /= -12.0;
    Pbbb /= -20.0;
    Pab /= 24.0;
    Paab /= 60.0;
    Pabb /= -60.0;
}

void VolumeIntegration::face_integrals(unsigned int f)
{
  Real w;
  Vector3r n;
  Real k1, k2, k3, k4;

  projection_integrals(f);

  w = m_weights[f];
  n = m_face_normals[f];
  k1 = 1 / n[C]; k2 = k1 * k1; k3 = k2 * k1; k4 = k3 * k1;

  Fa = k1 * Pa;
  Fb = k1 * Pb;
  Fc = -k2 * (n[A]*Pa + n[B]*Pb + w*P1);

  Faa = k1 * Paa;
  Fbb = k1 * Pbb;
  Fcc = k3 * (SQR(n[A])*Paa + 2*n[A]*n[B]*Pab + SQR(n[B])*Pbb
     + w*(2*(n[A]*Pa + n[B]*Pb) + w*P1));

  Faaa = k1 * Paaa;
  Fbbb = k1 * Pbbb;
  Fccc = -k4 * (CUBE(n[A])*Paaa + 3*SQR(n[A])*n[B]*Paab
       + 3*n[A]*SQR(n[B])*Pabb + CUBE(n[B])*Pbbb
       + 3*w*(SQR(n[A])*Paa + 2*n[A]*n[B]*Pab + SQR(n[B])*Pbb)
       + w*w*(3*(n[A]*Pa + n[B]*Pb) + w*P1));

  Faab = k1 * Paab;
  Fbbc = -k2 * (n[A]*Pabb + n[B]*Pbbb + w*Pbb);
  Fcca = k3 * (SQR(n[A])*Paaa + 2*n[A]*n[B]*Paab + SQR(n[B])*Pabb
     + w*(2*(n[A]*Paa + n[B]*Pab) + w*Pa));
}

void VolumeIntegration::volume_integrals()
{
    Real nx, ny, nz;

    T0  = T1[0] = T1[1] = T1[2]
        = T2[0] = T2[1] = T2[2]
        = TP[0] = TP[1] = TP[2] = 0;

	for (unsigned int i(0); i < m_nFaces; ++i)
    {
        Vector3r const& n = m_face_normals[i];
        nx = std::abs(n[0]);
        ny = std::abs(n[1]);
        nz = std::abs(n[2]);
        if (nx > ny && nx > nz)
            C = 0;
        else
            C = (ny > nz) ? 1 : 2;
        A = (C + 1) % 3;
        B = (A + 1) % 3;

        face_integrals(i);

        T0 += n[0] * ((A == 0) ? Fa : ((B == 0) ? Fb : Fc));

        T1[A] += n[A] * Faa;
        T1[B] += n[B] * Fbb;
        T1[C] += n[C] * Fcc;
        T2[A] += n[A] * Faaa;
        T2[B] += n[B] * Fbbb;
        T2[C] += n[C] * Fccc;
        TP[A] += n[A] * Faab;
        TP[B] += n[B] * Fbbc;
        TP[C] += n[C] * Fcca;
    }

    T1[0] /= 2; T1[1] /= 2; T1[2] /= 2;
    T2[0] /= 3; T2[1] /= 3; T2[2] /= 3;
    TP[0] /= 2; TP[1] /= 2; TP[2] /= 2;
}
<|endoftext|>


<commit_before>#include "NoteManager.h"

Note* NoteManager::_notes;

NoteManager::NoteManager() {
	uint8_t hue_inc = 0;
	if( NUM_NOTES != 0 )
		hue_inc = 0xFF / NUM_NOTES;
	else
		hue_inc = 0x11;
	for(uint8_t i = 0; i < NUM_NOTES; i++ ) {
		_notes[i] = Note(hue_inc * i, i, NUM_LEDS);
	}
}

void NoteManager::update(CRGB leds[]) {
	//	update our LEDs based on which notes are playing
	for(uint8_t i = 0; i < NUM_NOTES; i++ ) {
		Note note = _notes[i];
		if( note.isPlaying() ) {
			uint16_t start = note.range().start;
			uint16_t end = note.range().end;
			uint8_t hue = note.hue();
			uint8_t bright = 0xFF;
			for(uint16_t i = start; i <= end; i++ ) {
				leds[i] = CHSV(hue,255,255);
			}
		}
	}
}

void NoteManager::play(uint8_t touchIndex) {
	if( touchIndex >= NUM_NOTES ) {
		return;
	}
	_notes[touchIndex].setPlaying(true);
}

void NoteManager::release(uint8_t touchIndex) {
	if( touchIndex >= NUM_NOTES ) {
		return;
	}
	_notes[touchIndex].setPlaying(false);
}

uint8_t NoteManager::noteIndexForTouch(uint8_t touchIndex) {
	if( touchIndex > NUM_NOTES )
		return NUM_NOTES - touchIndex;
	else
		return 0;
}
<commit_msg>fixed issues with Note playing<commit_after>#include "NoteManager.h"

//	define this symbols to debug to serial connection
#undef DEBUG_NOTE_CREATION
#undef DEBUG_NOTE_PLAYING

note_t NoteManager::_notes[NUM_NOTES];

NoteManager::NoteManager() {

}

void NoteManager::begin() {
	uint8_t hue_inc = 0;
	if( NUM_NOTES != 0 )
		hue_inc = 0xFF / NUM_NOTES;
	else
		hue_inc = 0x11;
	for(uint8_t i = 0; i < NUM_NOTES; i++ ) {
		uint8_t length = NUM_LEDS / NUM_NOTES;
		uint16_t start = i * length;
		uint16_t end = start + length;
		if( i == NUM_NOTES - 1 ) {
			end++;
		}
		note_t temp_note;
		temp_note.start = start;
		temp_note.end = end;
		temp_note.hue = hue_inc * i;
		temp_note.playing = false;
		_notes[i] = temp_note;
		note_t n = _notes[i];
		#ifdef DEBUG_NOTE_CREATION
			//Serial.print("touch idx = "); Serial.println(touchIndex);
			Serial.print("note idx = "); Serial.println(i);
			Serial.print("range: "); Serial.print(n.start); Serial.print("->"); Serial.println(n.end);
			Serial.println("");
		#endif
	}
}

void NoteManager::update(CRGB leds[]) {
	//	update our LEDs based on which notes are playing
	for(uint8_t i = 0; i < NUM_NOTES; i++ ) {
		note_t note = _notes[i];
		if( note.playing ) {
			uint16_t start = note.start;
			uint16_t end = note.end;
			uint8_t hue = note.hue;
			uint8_t bright = 0xFF;
			for(uint16_t i = start; i <= end; i++ ) {
				leds[i] = CHSV(hue,255,255);
			}
		}
	}
}

void NoteManager::play(uint8_t touchIndex) {
	if( touchIndex >= NUM_NOTES ) {
		return;
	}
	uint8_t noteIndex = noteIndexForTouch(touchIndex);
	_notes[noteIndex].playing = true;

	#ifdef DEBUG_NOTE_PLAYING
		note_t n = _notes[noteIndex];
		Serial.print("touch idx = "); Serial.println(touchIndex);
		Serial.print("note idx = "); Serial.println(noteIndex);
		Serial.print("range: "); Serial.print(n.start); Serial.print("->"); Serial.println(n.end);
		Serial.println("");
	#endif

}

void NoteManager::release(uint8_t touchIndex) {
	if( touchIndex >= NUM_NOTES ) {
		return;
	}
	uint8_t noteIndex = noteIndexForTouch(touchIndex);
	_notes[noteIndex].playing = false;
}

uint8_t NoteManager::noteIndexForTouch(uint8_t touchIndex) {
	if( touchIndex > NUM_NOTES )
		return 0;
	return NUM_NOTES - touchIndex - 1;
}
<|endoftext|>


<commit_before>#ifndef ALIB_COMM_MSG_IS_DEFINED
#define ALIB_COMM_MSG_IS_DEFINED

#ifdef ARDUINO
#include <Arduino.h>
#include <new.h>
#else
#include <iostream>
#endif

namespace alib
{
	namespace comm
	{
		/* Generic list of constructors all inheriting objects should implement.
		 * Note that these are not required, but greatly simplify creation of most Msg objects. */
#define ALIB_COMM_MSG_INHERITED_CONSTRUCTORS(DERIVED_CLASS, BASE_CLASS) \
		DERIVED_CLASS(const Msg& msg, size_t buffSize = 0, bool referenceBuff = true) :BASE_CLASS(msg, buffSize, referenceBuff) {}

		/* Generic list of constructors all inheriting objects should implement.
		 * Note that these are not required, but greatly simplify creation of most Msg objects.*/
#define ALIB_COMM_MSG_INHERITED_CONSTRUCTORS_STATIC_SIZE(DERIVED_CLASS, BASE_CLASS) \
		DERIVED_CLASS(const Msg& msg, bool referenceBuff = true) : BASE_CLASS(msg, referenceBuff) {}

		/* Set of functions that MUST BE IMPLEMENTED but should always be the same with each derivative.
		 * Note that implementing this MUST HAVE permissions scoping re-declared as it defines functions of
		 * multiple permission type. */
#define ALIB_COMM_MSG_INHERITED_FUNCTIONS(DERIVED_CLASS, BASE_CLASS, UTILIZED_SIZE)\
	protected:\
		virtual size_t _getBaseSize()const{return(BASE_CLASS::getUtilizedSize());}\
	public:\
		virtual size_t getUtilizedSize()const{return(DERIVED_CLASS::_getBaseSize() + UTILIZED_SIZE);}

		/* Abstract class that simply wraps a buffer for communication. */
		class Msg
		{
		private:
			void* _buff;
			size_t _buffSize = 0;
			bool _freeOnDestroy;

		protected:
			/* Functions */
				/* Constructors */
			/* Basically sets all values to empty, will not allocate memory.  This should be followed by a call to '_init()'.
			 * This is really useful whenever inheriting classes want to provide an interface for converting a buffer into a Msg object.
			 * Originally it was desired to place such a function within Msg itself, however to do this would require Msg not be abstract... */
			Msg() :_buff(NULL), _freeOnDestroy(false) {}

			/* Constructs a Msg object and sets the internal buffer to point to the given one.  If 'buff' is NULL and 'buffSize' is not zero, then
			 * the buffer will be dynamically created and its lifecycle will be handled by the object. */
			Msg(void* buff, size_t buffSize = 0, bool own = false) { _init(buff, buffSize, own); }
				/****************/

				/* Virtual */
					/* Getters */
			virtual size_t _getBaseSize()const { return(0); }
					/***********/
				/***********/

			/* Initializes a Msg object with the given buffer.  Should be used after calling the default constructor.
			*
			* Parameters:
			*		buff: The buff to either reference or copy.
			*		buffSize: The length of 'buff' or the number of bytes to allocate.
			*		own: If true, the Msg object will own the buffer and handle its lifecycle.
			*/
			void _init(void* buff, size_t buffSize = 0, bool own = false)
			{
				_buffSize = buffSize;

				if (!buff)
					own = true;
				_freeOnDestroy = own;

				if (buff)
					_buff = buff;
				else if (buffSize > 0)
					_buff = new uint8_t[buffSize];
				else
					_buff = NULL;
			}

			/* Clears the derived buff up to count 'byteCount'.
			 * if 'byteCount' == 0, then the entire derived buff will be cleared. */
			void _clearDerivedBuff(size_t baseSize, size_t byteCount = 0)
			{
				if (!byteCount)
					byteCount = _buffSize - baseSize;

				void* derBuff = _getDerivedBuff(baseSize);
				memset(_getDerivedBuff(baseSize), 0, byteCount);
			}

				/* Getters */
			/* Gets the buffer for the derived class (this is based off of the logical offset). */
			void* _getDerivedBuff(size_t baseSize)
			{
				if (!_buff)return(NULL);

				return(((uint8_t*)_buff) + baseSize);
			}
			/* Gets the buffer for the derived class (this is based off of the logical offset). */
			const void* _getDerivedBuff(size_t baseSize)const
			{
				if (!_buff)return(NULL);

				return(((uint8_t*)_buff) + baseSize);
			}

			/* Gets the offset of the derived buff and returns the requested type.
			 *
			 * Parameters:
			 *		offset: The number of bytes to offset from the derived buffer.
			 *		baseSize: Utilized size of the base class. */
			template<typename T>
			T _getDerivedOffset(size_t offset, size_t baseSize)
			{
				if (!_buff)return(T());

				return(*((T*)((uint8_t*)_getDerivedBuff(baseSize) + offset)));
			}
			/* Gets the offset of the derived buff and returns the requested type.
			*
			* Parameters:
			*		offset: The number of bytes to offset from the derived buffer.
			*		baseSize: Utilized size of the base class. */
			template<typename T>
			const T _getDerivedOffset(size_t offset, size_t baseSize) const
			{
				if (!_buff)return(T());

				return(*((T*)((uint8_t*)_getDerivedBuff(baseSize) + offset)));
			}
				/***********/

				/* Setters */
			/* Sets the value at the provided offset. */
			template<typename T>
			void _setDerivedOffset(size_t offset, size_t baseSize, T val)
			{
				if (!_buff)return;

				*((T*)(((uint8_t*)_getDerivedBuff(baseSize)) + offset)) = val;
			}
				/***********/
			/*************/

		public:
			/* Constructors */
			/* Copy constructor.
			 * This will either create a shallow copy (referencing the same buffer), or
			 * create a deep copy (allocating a new buffer, and copying all of 'msg's buffer data into the new buffer).
			 *
			 * Note: As with all Msg objects, using a referenced buffer means that the buffer must outlive the Msg object.
			 *
			 * Parameters:
			 *		msg: The message to copy.
			 *		buffSize: The length of the buffer, in bytes. If 0, the buffer from 'msg' will be used.
			 *		referenceBuff: If true, the internal buffer of 'msg' will be referenced and this object
			 *						will not own it. */
			Msg(const Msg& msg, size_t buffSize = 0, bool referenceBuff = true) { _init(((referenceBuff) ? msg._buff : NULL), (buffSize) ? buffSize : msg._buffSize); }
			/****************/

			/* Functions */
				/* Virtual */
					/* Getters */
			/* Returns the number of bytes utilized by the message.  This differs from the actual buffer size.
			 * i.e. buffer size >= utilized size */
			virtual size_t getUtilizedSize()const { return(0); }
			/* Ensures that a Msg is actually valid.
			 * This is useful if the object is constructed from binary. */
			virtual bool isValid()const = 0;
					/***********/
				/***********/

			/* Copies a buffer into the given message. */
			void copy(const void* data, size_t dataLen = 0)
			{
				if (!data)return;
				if (dataLen == 0)
					dataLen = getUtilizedSize();

				memcpy(_buff, data, dataLen);
			}
			/* Copies the buffer of the given message. */
			void copy(const Msg& msg) { copy(msg._buff, msg._buffSize); }

			/* Attempts to take ownership of the internal buffer from the given Msg object.
			 * This will only work if both messages are referencing the same buffer and the given 'msg' is
			 * the owner of the buffer.
			 *
			 * Returns:
			 *		true: Ownership was taken.
			 *		false: Ownership could not be taken, either due to the fact that 'msg' is not the buffer's owner
			 *			   or that 'msg' and this object are not pointing to the same buffer. */
			bool takeOwnership(Msg& msg)
			{
				if (msg._freeOnDestroy && msg._buff == _buff)
				{
					msg._freeOnDestroy = false;
					_freeOnDestroy = true;
					return(true);
				}
				else
					return(false);
			}

				/* Getters */
			/* Returns true if the object owns the buffer (will free on destruction). */
			bool buffIsOwned()const { return(_freeOnDestroy); }
			/* Returns the size of the buffer in bytes.
			 * 0 is returned when the buffer size was not specified. */
			size_t getBuffSize()const { return(_buffSize); }
			/* Gets a pointer to the internal buffer. */
			const void* getBuff()const { return(_buff); }
			/* Returns the number of bytes that are unutilized. */
			size_t getRemainingSize()const { return(getBuffSize() - getUtilizedSize()); }
				/***********/
			/*************/

			~Msg()
			{
				if (_freeOnDestroy && _buff)
					delete[](_buff);
			}
		};

		class MsgBuilder : public Msg
		{
		private:
			MsgBuilder() {}
		public:
			/* Static */
			/* Creates a Msg from the buffer.  This is used to build an Msg from a buffer and read it without
			* any modification to the underlying data.  I.E. You have an array of data you received from a slave/client, and you
			* wish to parse that data, simply call this function and pass the returned value to the copy constructor of the object
			* you wish to cast to. */
			static MsgBuilder fromBuff(void* buff, size_t buffSize = 0, bool own = false)
			{
				MsgBuilder msg{};
				msg._init(buff, buffSize, own);

				return(msg);
			}
			/**********/

			/* Methods */
				/* Virtual */
			virtual bool isValid()const { return(true); }
			virtual size_t getUtilizedSize()const { return(0); }
				/***********/
			/***********/
		};

		template<size_t BUFF_SIZE>
		class MsgStaticSized : public Msg
		{
		protected:
			MsgStaticSized() : Msg() {}
			MsgStaticSized(void* buff, bool own = false) : Msg(buff, BUFF_SIZE, own) {}

		public:
			MsgStaticSized(const Msg& msg, bool referenceBuff = true) : Msg(msg, BUFF_SIZE, referenceBuff){}

			/* Getters */
			static constexpr size_t getBuffSize() { return(BUFF_SIZE); }
			/***********/

			/* Virtual */
				/* Getters */
			virtual bool isValid()const
			{
				uint8_t buffSize = (uint8_t)Msg::getBuffSize();
				return(!buffSize || buffSize == BUFF_SIZE);
			}
				/***********/
			/***********/
		};
	}
}

#endif<commit_msg>Minor edit to Msg object for how buffers are copied.<commit_after>#ifndef ALIB_COMM_MSG_IS_DEFINED
#define ALIB_COMM_MSG_IS_DEFINED

#ifdef ARDUINO
#include <Arduino.h>
#include <new.h>
#else
#include <iostream>
#endif

namespace alib
{
	namespace comm
	{
		/* Generic list of constructors all inheriting objects should implement.
		 * Note that these are not required, but greatly simplify creation of most Msg objects. */
#define ALIB_COMM_MSG_INHERITED_CONSTRUCTORS(DERIVED_CLASS, BASE_CLASS) \
		DERIVED_CLASS(const Msg& msg, size_t buffSize = 0, bool referenceBuff = true) :BASE_CLASS(msg, buffSize, referenceBuff) {}

		/* Generic list of constructors all inheriting objects should implement.
		 * Note that these are not required, but greatly simplify creation of most Msg objects.*/
#define ALIB_COMM_MSG_INHERITED_CONSTRUCTORS_STATIC_SIZE(DERIVED_CLASS, BASE_CLASS) \
		DERIVED_CLASS(const Msg& msg, bool referenceBuff = true) : BASE_CLASS(msg, referenceBuff) {}

		/* Set of functions that MUST BE IMPLEMENTED but should always be the same with each derivative.
		 * Note that implementing this MUST HAVE permissions scoping re-declared as it defines functions of
		 * multiple permission type. */
#define ALIB_COMM_MSG_INHERITED_FUNCTIONS(DERIVED_CLASS, BASE_CLASS, UTILIZED_SIZE)\
	protected:\
		virtual size_t _getBaseSize()const{return(BASE_CLASS::getUtilizedSize());}\
	public:\
		virtual size_t getUtilizedSize()const{return(DERIVED_CLASS::_getBaseSize() + UTILIZED_SIZE);}

		/* Abstract class that simply wraps a buffer for communication. */
		class Msg
		{
		private:
			void* _buff;
			size_t _buffSize = 0;
			bool _freeOnDestroy;

		protected:
			/* Functions */
				/* Constructors */
			/* Basically sets all values to empty, will not allocate memory.  This should be followed by a call to '_init()'.
			 * This is really useful whenever inheriting classes want to provide an interface for converting a buffer into a Msg object.
			 * Originally it was desired to place such a function within Msg itself, however to do this would require Msg not be abstract... */
			Msg() :_buff(NULL), _freeOnDestroy(false) {}

			/* Constructs a Msg object and sets the internal buffer to point to the given one.  If 'buff' is NULL and 'buffSize' is not zero, then
			 * the buffer will be dynamically created and its lifecycle will be handled by the object. */
			Msg(void* buff, size_t buffSize = 0, bool own = false) { _init(buff, buffSize, own); }
				/****************/

				/* Virtual */
					/* Getters */
			virtual size_t _getBaseSize()const { return(0); }
					/***********/
				/***********/

			/* Initializes a Msg object with the given buffer.  Should be used after calling the default constructor.
			*
			* Parameters:
			*		buff: The buff to either reference or copy.
			*		buffSize: The length of 'buff' or the number of bytes to allocate.
			*		own: If true, the Msg object will own the buffer and handle its lifecycle.
			*/
			void _init(void* buff, size_t buffSize = 0, bool own = false)
			{
				_buffSize = buffSize;

				if (!buff)
					own = true;
				_freeOnDestroy = own;

				if (buff)
					_buff = buff;
				else if (buffSize > 0)
					_buff = new uint8_t[buffSize];
				else
					_buff = NULL;
			}

			/* Clears the derived buff up to count 'byteCount'.
			 * if 'byteCount' == 0, then the entire derived buff will be cleared. */
			void _clearDerivedBuff(size_t baseSize, size_t byteCount = 0)
			{
				if (!byteCount)
					byteCount = _buffSize - baseSize;

				void* derBuff = _getDerivedBuff(baseSize);
				memset(_getDerivedBuff(baseSize), 0, byteCount);
			}

				/* Getters */
			/* Gets the buffer for the derived class (this is based off of the logical offset). */
			void* _getDerivedBuff(size_t baseSize)
			{
				if (!_buff)return(NULL);

				return(((uint8_t*)_buff) + baseSize);
			}
			/* Gets the buffer for the derived class (this is based off of the logical offset). */
			const void* _getDerivedBuff(size_t baseSize)const
			{
				if (!_buff)return(NULL);

				return(((uint8_t*)_buff) + baseSize);
			}

			/* Gets the offset of the derived buff and returns the requested type.
			 *
			 * Parameters:
			 *		offset: The number of bytes to offset from the derived buffer.
			 *		baseSize: Utilized size of the base class. */
			template<typename T>
			T _getDerivedOffset(size_t offset, size_t baseSize)
			{
				if (!_buff)return(T());

				return(*((T*)((uint8_t*)_getDerivedBuff(baseSize) + offset)));
			}
			/* Gets the offset of the derived buff and returns the requested type.
			*
			* Parameters:
			*		offset: The number of bytes to offset from the derived buffer.
			*		baseSize: Utilized size of the base class. */
			template<typename T>
			const T _getDerivedOffset(size_t offset, size_t baseSize) const
			{
				if (!_buff)return(T());

				return(*((T*)((uint8_t*)_getDerivedBuff(baseSize) + offset)));
			}
				/***********/

				/* Setters */
			/* Sets the value at the provided offset. */
			template<typename T>
			void _setDerivedOffset(size_t offset, size_t baseSize, T val)
			{
				if (!_buff)return;

				*((T*)(((uint8_t*)_getDerivedBuff(baseSize)) + offset)) = val;
			}
				/***********/
			/*************/

		public:
			/* Constructors */
			/* Copy constructor.
			 * This will either create a shallow copy (referencing the same buffer), or
			 * create a deep copy (allocating a new buffer, and copying all of 'msg's buffer data into the new buffer).
			 *
			 * Note: As with all Msg objects, using a referenced buffer means that the buffer must outlive the Msg object.
			 *
			 * Parameters:
			 *		msg: The message to copy.
			 *		buffSize: The length of the buffer, in bytes. If 0, the buffer from 'msg' will be used.
			 *		referenceBuff: If true, the internal buffer of 'msg' will be referenced and this object
			 *						will not own it. */
			Msg(const Msg& msg, size_t buffSize = 0, bool referenceBuff = true) { _init(((referenceBuff) ? msg._buff : NULL), (buffSize) ? buffSize : msg._buffSize); }
			/****************/

			/* Functions */
				/* Virtual */
					/* Getters */
			/* Returns the number of bytes utilized by the message.  This differs from the actual buffer size.
			 * i.e. buffer size >= utilized size */
			virtual size_t getUtilizedSize()const { return(0); }
			/* Ensures that a Msg is actually valid.
			 * This is useful if the object is constructed from binary. */
			virtual bool isValid()const = 0;
					/***********/
				/***********/

			/* Copies a buffer into the given message. */
			void copy(const void* data, size_t dataLen = 0)
			{
				if (!data)return;
				if (dataLen == 0)
					dataLen = getBuffSize();

				memcpy(_buff, data, dataLen);
			}
			/* Copies the buffer of the given message. */
			void copy(const Msg& msg) { copy(msg._buff, msg._buffSize); }

			/* Attempts to take ownership of the internal buffer from the given Msg object.
			 * This will only work if both messages are referencing the same buffer and the given 'msg' is
			 * the owner of the buffer.
			 *
			 * Returns:
			 *		true: Ownership was taken.
			 *		false: Ownership could not be taken, either due to the fact that 'msg' is not the buffer's owner
			 *			   or that 'msg' and this object are not pointing to the same buffer. */
			bool takeOwnership(Msg& msg)
			{
				if (msg._freeOnDestroy && msg._buff == _buff)
				{
					msg._freeOnDestroy = false;
					_freeOnDestroy = true;
					return(true);
				}
				else
					return(false);
			}

				/* Getters */
			/* Returns true if the object owns the buffer (will free on destruction). */
			bool buffIsOwned()const { return(_freeOnDestroy); }
			/* Returns the size of the buffer in bytes.
			 * 0 is returned when the buffer size was not specified. */
			size_t getBuffSize()const { return(_buffSize); }
			/* Gets a pointer to the internal buffer. */
			const void* getBuff()const { return(_buff); }
			/* Returns the number of bytes that are unutilized. */
			size_t getRemainingSize()const { return(getBuffSize() - getUtilizedSize()); }
				/***********/
			/*************/

			~Msg()
			{
				if (_freeOnDestroy && _buff)
					delete[](_buff);
			}
		};

		class MsgBuilder : public Msg
		{
		private:
			MsgBuilder() {}
		public:
			/* Static */
			/* Creates a Msg from the buffer.  This is used to build an Msg from a buffer and read it without
			* any modification to the underlying data.  I.E. You have an array of data you received from a slave/client, and you
			* wish to parse that data, simply call this function and pass the returned value to the copy constructor of the object
			* you wish to cast to. */
			static MsgBuilder fromBuff(void* buff, size_t buffSize = 0, bool own = false)
			{
				MsgBuilder msg{};
				msg._init(buff, buffSize, own);

				return(msg);
			}
			/**********/

			/* Methods */
				/* Virtual */
			virtual bool isValid()const { return(true); }
			virtual size_t getUtilizedSize()const { return(0); }
				/***********/
			/***********/
		};

		template<size_t BUFF_SIZE>
		class MsgStaticSized : public Msg
		{
		protected:
			MsgStaticSized() : Msg() {}
			MsgStaticSized(void* buff, bool own = false) : Msg(buff, BUFF_SIZE, own) {}

		public:
			MsgStaticSized(const Msg& msg, bool referenceBuff = true) : Msg(msg, BUFF_SIZE, referenceBuff){}

			/* Getters */
			static constexpr size_t getBuffSize() { return(BUFF_SIZE); }
			/***********/

			/* Virtual */
				/* Getters */
			virtual bool isValid()const
			{
				uint8_t buffSize = (uint8_t)Msg::getBuffSize();
				return(!buffSize || buffSize == BUFF_SIZE);
			}
				/***********/
			/***********/
		};
	}
}

#endif<|endoftext|>