V_PyramidMetric.cpp

/*=========================================================================

  Module:    V_PyramidMetric.cpp

  Copyright 2006 National Technology & Engineering Solutions of Sandia,
  LLC (NTESS). Under the terms of Contract DE-NA0003525 with NTESS,
  the U.S. Government retains certain rights in this software.

  See LICENSE for details.

=========================================================================*/


/*
 *
 * PyramidMetrics.cpp contains quality calculations for Pyramids
 *
 * This file is part of VERDICT
 *
 */


#include "verdict.h"
#include "VerdictVector.hpp"
#include "verdict_defines.hpp"
#include <memory.h>
#include <vector>
#include <algorithm>

static const double SQRT2_HALVES = sqrt(2.0)/2.0;

extern double quad_equiangle_skew( int num_nodes, double coordinates[][3] );
extern double tri_equiangle_skew( int num_nodes, double coordinates[][3] );

namespace VERDICT_NAMESPACE
{
// local methods
void make_pyramid_tets(double coordinates[][3], double tet1_coords[][3], double tet2_coords[][3],
                                                  double tet3_coords[][3], double tet4_coords[][3]);
void make_pyramid_faces(double coordinates[][3], double base[][3], double tri1[][3],
                          double tri2[][3], double tri3[][3], double tri4[][3]);
void make_pyramid_edges( VerdictVector edges[8], double coordinates[][3] );
double distance_point_to_pyramid_base( int num_nodes, double coordinates[][3], double &cos_angle);
double largest_pyramid_edge( double coordinates[][3] );
/*
  the pyramid element

       5
       ^
       |\ 
      /| \\_
     |  \   \
     |  | \_ \_
     /   \/4\  \
    |   /|    \ \_
    | /  \      \ \
    /     |       \
  1 \_    |      _/3
      \_   \   _/
        \_ | _/
          \_/
          2

    a quadrilateral base and a pointy peak like a pyramid
          
*/

double pyramid_equiangle_skew( int num_nodes, double coordinates[][3] )
{
  double base[4][3];
  double tri1[3][3];
  double tri2[3][3];
  double tri3[3][3];
  double tri4[3][3];
  make_pyramid_faces(coordinates, base,tri1,tri2,tri3,tri4);


  double quad_skew=quad_equiangle_skew( 4, base );
  double tri1_skew=tri_equiangle_skew(3,tri1);
  double tri2_skew=tri_equiangle_skew(3,tri2);
  double tri3_skew=tri_equiangle_skew(3,tri3);
  double tri4_skew=tri_equiangle_skew(3,tri4);

  double max_skew=quad_skew;
  max_skew = max_skew > tri1_skew      ? max_skew : tri1_skew;
  max_skew = max_skew > tri2_skew      ? max_skew : tri2_skew;
  max_skew = max_skew > tri3_skew      ? max_skew : tri3_skew;
  max_skew = max_skew > tri4_skew      ? max_skew : tri4_skew;

  return max_skew;
}

/*!
  the volume of a pyramid

  the volume is calculated by dividing the pyramid into
  2 tets and summing the volumes of the 2 tets.
*/

double pyramid_volume( int num_nodes, double coordinates[][3] )
{
    
  double center_coords[3];
  //calculate the center of the quads
  center_coords[0] =  (coordinates[0][0] + coordinates[1][0] +
      coordinates[2][0] + coordinates[3][0]) / 4;
  center_coords[1] =  (coordinates[0][1] + coordinates[1][1] +
      coordinates[2][1] + coordinates[3][1] ) / 4;
  center_coords[2] =  (coordinates[0][2] + coordinates[1][2] +
      coordinates[2][2] + coordinates[3][2] ) / 4;

  double tet_coords[4][4][3];
  tet_coords[0][0][0] = coordinates[0][0];
  tet_coords[0][0][1] = coordinates[0][1];
  tet_coords[0][0][2] = coordinates[0][2];
  tet_coords[0][1][0] = coordinates[1][0];
  tet_coords[0][1][1] = coordinates[1][1];
  tet_coords[0][1][2] = coordinates[1][2];
  tet_coords[0][2][0] = center_coords[0];
  tet_coords[0][2][1] = center_coords[1];
  tet_coords[0][2][2] = center_coords[2];
  tet_coords[0][3][0] = coordinates[4][0];
  tet_coords[0][3][1] = coordinates[4][1];
  tet_coords[0][3][2] = coordinates[4][2];

  tet_coords[1][0][0] = coordinates[1][0];
  tet_coords[1][0][1] = coordinates[1][1];
  tet_coords[1][0][2] = coordinates[1][2];
  tet_coords[1][1][0] = coordinates[2][0];
  tet_coords[1][1][1] = coordinates[2][1];
  tet_coords[1][1][2] = coordinates[2][2];
  tet_coords[1][2][0] = center_coords[0];
  tet_coords[1][2][1] = center_coords[1];
  tet_coords[1][2][2] = center_coords[2];
  tet_coords[1][3][0] = coordinates[4][0];
  tet_coords[1][3][1] = coordinates[4][1];
  tet_coords[1][3][2] = coordinates[4][2];

  tet_coords[2][0][0] = coordinates[2][0];
  tet_coords[2][0][1] = coordinates[2][1];
  tet_coords[2][0][2] = coordinates[2][2];
  tet_coords[2][1][0] = coordinates[3][0];
  tet_coords[2][1][1] = coordinates[3][1];
  tet_coords[2][1][2] = coordinates[3][2];
  tet_coords[2][2][0] = center_coords[0];
  tet_coords[2][2][1] = center_coords[1];
  tet_coords[2][2][2] = center_coords[2];
  tet_coords[2][3][0] = coordinates[4][0];
  tet_coords[2][3][1] = coordinates[4][1];
  tet_coords[2][3][2] = coordinates[4][2];

  tet_coords[3][0][0] = coordinates[3][0];
  tet_coords[3][0][1] = coordinates[3][1];
  tet_coords[3][0][2] = coordinates[3][2];
  tet_coords[3][1][0] = coordinates[0][0];
  tet_coords[3][1][1] = coordinates[0][1];
  tet_coords[3][1][2] = coordinates[0][2];
  tet_coords[3][2][0] = center_coords[0];
  tet_coords[3][2][1] = center_coords[1];
  tet_coords[3][2][2] = center_coords[2];
  tet_coords[3][3][0] = coordinates[4][0];
  tet_coords[3][3][1] = coordinates[4][1];
  tet_coords[3][3][2] = coordinates[4][2];




  double volume=0.0;
  for(int t=0;t<4;t++)
  {
    volume+=tet_volume(4,tet_coords[t]);
  }

  return (double)volume;

    
}

double pyramid_jacobian( int num_nodes, double coordinates[][3] )
{
  // break the pyramid into four tets return the minimum jacobian of the two tets
  double tet1_coords[4][3];
  double tet2_coords[4][3];
  double tet3_coords[4][3];
  double tet4_coords[4][3];

  make_pyramid_tets(coordinates, tet1_coords, tet2_coords, tet3_coords, tet4_coords);

  double j1 = tet_jacobian(4, tet1_coords);
  double j2 = tet_jacobian(4, tet2_coords);
  double j3 = tet_jacobian(4, tet3_coords);
  double j4 = tet_jacobian(4, tet4_coords);

  double p1 = j1 < j2 ? j1 : j2;
  double p2 = j3 < j4 ? j3 : j4;

  return p1 < p2 ? p1 : p2;
}

double pyramid_scaled_jacobian( int num_nodes, double coordinates[][3] )
{
  // break the pyramid into four tets return the minimum scaled jacobian of the tets
  double tet1_coords[4][3];
  double tet2_coords[4][3];
  double tet3_coords[4][3];
  double tet4_coords[4][3];

  make_pyramid_tets(coordinates, tet1_coords, tet2_coords, tet3_coords, tet4_coords);

  std::vector<double> scaled_jacob(4);
  scaled_jacob[0] = tet_scaled_jacobian(4, tet1_coords);
  scaled_jacob[1] = tet_scaled_jacobian(4, tet2_coords);
  scaled_jacob[2] = tet_scaled_jacobian(4, tet3_coords);
  scaled_jacob[3] = tet_scaled_jacobian(4, tet4_coords);

  std::vector<double>::iterator iter = std::min_element(scaled_jacob.begin(), scaled_jacob.end());

  // scale the minimum scaled jacobian so that a perfect pyramid has
  // a value of 1 and cap it to make sure it is not > 1.0 or < 0.0
  if (*iter <= 0.0)
    return 0.0;

  double min_jac = (*iter)*2.0/sqrt(2.0);
  return min_jac < 1.0 ? min_jac : 1.0 - (min_jac - 1.0);
}

double pyramid_shape( int num_nodes, double coordinates[][3] )
{
  // ideally there will be four equilateral triangles and one square.
  // Test each face
  double base[4][3];
  double tri1[3][3];
  double tri2[3][3];
  double tri3[3][3];
  double tri4[3][3];

  make_pyramid_faces(coordinates, base, tri1, tri2, tri3, tri4);

  double s1 = quad_shape(4, base);

  if (s1 == 0.0)
    return 0.0;

  double cos_angle;
  double dist_to_base = distance_point_to_pyramid_base(num_nodes, coordinates, cos_angle );

  if (dist_to_base <= 0 || cos_angle <= 0.0)
    return 0.0;

  double longest_edge = largest_pyramid_edge(coordinates) * SQRT2_HALVES;
  if (dist_to_base < longest_edge)
    dist_to_base = dist_to_base/longest_edge;
  else
    dist_to_base = longest_edge/dist_to_base;

  double shape = s1 * cos_angle * dist_to_base;

  return shape;
}

void make_pyramid_tets(double coordinates[][3], double tet1_coords[][3], double tet2_coords[][3],
                                                  double tet3_coords[][3], double tet4_coords[][3])
{
   // tet1
  tet1_coords[0][0] = coordinates[0][0];
  tet1_coords[0][1] = coordinates[0][1];
  tet1_coords[0][2] = coordinates[0][2];

  tet1_coords[1][0] = coordinates[1][0];
  tet1_coords[1][1] = coordinates[1][1];
  tet1_coords[1][2] = coordinates[1][2];

  tet1_coords[2][0] = coordinates[2][0];
  tet1_coords[2][1] = coordinates[2][1];
  tet1_coords[2][2] = coordinates[2][2];

  tet1_coords[3][0] = coordinates[4][0];
  tet1_coords[3][1] = coordinates[4][1];
  tet1_coords[3][2] = coordinates[4][2];

  // tet2
  tet2_coords[0][0] = coordinates[0][0];
  tet2_coords[0][1] = coordinates[0][1];
  tet2_coords[0][2] = coordinates[0][2];

  tet2_coords[1][0] = coordinates[2][0];
  tet2_coords[1][1] = coordinates[2][1];
  tet2_coords[1][2] = coordinates[2][2];

  tet2_coords[2][0] = coordinates[3][0];
  tet2_coords[2][1] = coordinates[3][1];
  tet2_coords[2][2] = coordinates[3][2];

  tet2_coords[3][0] = coordinates[4][0];
  tet2_coords[3][1] = coordinates[4][1];
  tet2_coords[3][2] = coordinates[4][2];

  // tet3
  tet3_coords[0][0] = coordinates[0][0];
  tet3_coords[0][1] = coordinates[0][1];
  tet3_coords[0][2] = coordinates[0][2];

  tet3_coords[1][0] = coordinates[1][0];
  tet3_coords[1][1] = coordinates[1][1];
  tet3_coords[1][2] = coordinates[1][2];

  tet3_coords[2][0] = coordinates[3][0];
  tet3_coords[2][1] = coordinates[3][1];
  tet3_coords[2][2] = coordinates[3][2];

  tet3_coords[3][0] = coordinates[4][0];
  tet3_coords[3][1] = coordinates[4][1];
  tet3_coords[3][2] = coordinates[4][2];

  // tet4
  tet4_coords[0][0] = coordinates[1][0];
  tet4_coords[0][1] = coordinates[1][1];
  tet4_coords[0][2] = coordinates[1][2];

  tet4_coords[1][0] = coordinates[2][0];
  tet4_coords[1][1] = coordinates[2][1];
  tet4_coords[1][2] = coordinates[2][2];

  tet4_coords[2][0] = coordinates[3][0];
  tet4_coords[2][1] = coordinates[3][1];
  tet4_coords[2][2] = coordinates[3][2];

  tet4_coords[3][0] = coordinates[4][0];
  tet4_coords[3][1] = coordinates[4][1];
  tet4_coords[3][2] = coordinates[4][2];
}

void make_pyramid_faces(double coordinates[][3], double base[][3], double tri1[][3],
                          double tri2[][3], double tri3[][3], double tri4[][3])
{
   // base
  base[0][0] = coordinates[0][0];
  base[0][1] = coordinates[0][1];
  base[0][2] = coordinates[0][2];

  base[1][0] = coordinates[1][0];
  base[1][1] = coordinates[1][1];
  base[1][2] = coordinates[1][2];

  base[2][0] = coordinates[2][0];
  base[2][1] = coordinates[2][1];
  base[2][2] = coordinates[2][2];

  base[3][0] = coordinates[3][0];
  base[3][1] = coordinates[3][1];
  base[3][2] = coordinates[3][2];

  // tri1
  tri1[0][0] = coordinates[0][0];
  tri1[0][1] = coordinates[0][1];
  tri1[0][2] = coordinates[0][2];

  tri1[1][0] = coordinates[1][0];
  tri1[1][1] = coordinates[1][1];
  tri1[1][2] = coordinates[1][2];

  tri1[2][0] = coordinates[4][0];
  tri1[2][1] = coordinates[4][1];
  tri1[2][2] = coordinates[4][2];

  // tri2
  tri2[0][0] = coordinates[1][0];
  tri2[0][1] = coordinates[1][1];
  tri2[0][2] = coordinates[1][2];

  tri2[1][0] = coordinates[2][0];
  tri2[1][1] = coordinates[2][1];
  tri2[1][2] = coordinates[2][2];

  tri2[2][0] = coordinates[4][0];
  tri2[2][1] = coordinates[4][1];
  tri2[2][2] = coordinates[4][2];

  // tri3
  tri3[0][0] = coordinates[2][0];
  tri3[0][1] = coordinates[2][1];
  tri3[0][2] = coordinates[2][2];

  tri3[1][0] = coordinates[3][0];
  tri3[1][1] = coordinates[3][1];
  tri3[1][2] = coordinates[3][2];

  tri3[2][0] = coordinates[4][0];
  tri3[2][1] = coordinates[4][1];
  tri3[2][2] = coordinates[4][2];

   // tri4
  tri4[0][0] = coordinates[3][0];
  tri4[0][1] = coordinates[3][1];
  tri4[0][2] = coordinates[3][2];

  tri4[1][0] = coordinates[0][0];
  tri4[1][1] = coordinates[0][1];
  tri4[1][2] = coordinates[0][2];

  tri4[2][0] = coordinates[4][0];
  tri4[2][1] = coordinates[4][1];
  tri4[2][2] = coordinates[4][2];
}

void make_pyramid_edges( VerdictVector edges[8], double coordinates[][3] )
{

  edges[0].set(
    coordinates[1][0] - coordinates[0][0],
    coordinates[1][1] - coordinates[0][1],
    coordinates[1][2] - coordinates[0][2]
  );
  edges[1].set(
    coordinates[2][0] - coordinates[1][0],
    coordinates[2][1] - coordinates[1][1],
    coordinates[2][2] - coordinates[1][2]
  );
  edges[2].set(
    coordinates[3][0] - coordinates[2][0],
    coordinates[3][1] - coordinates[2][1],
    coordinates[3][2] - coordinates[2][2]
  );
  edges[3].set(
    coordinates[0][0] - coordinates[3][0],
    coordinates[0][1] - coordinates[3][1],
    coordinates[0][2] - coordinates[3][2]
  );
  edges[4].set(
    coordinates[4][0] - coordinates[0][0],
    coordinates[4][1] - coordinates[0][1],
    coordinates[4][2] - coordinates[0][2]
  );
  edges[5].set(
    coordinates[4][0] - coordinates[1][0],
    coordinates[4][1] - coordinates[1][1],
    coordinates[4][2] - coordinates[1][2]
  );
  edges[6].set(
    coordinates[4][0] - coordinates[2][0],
    coordinates[4][1] - coordinates[2][1],
    coordinates[4][2] - coordinates[2][2]
  );
  edges[7].set(
    coordinates[4][0] - coordinates[3][0],
    coordinates[4][1] - coordinates[3][1],
    coordinates[4][2] - coordinates[3][2]
  );
}

double largest_pyramid_edge( double coordinates[][3] )
{
  VerdictVector edges[8];
  make_pyramid_edges(edges, coordinates);
  double l0 = edges[0].length_squared();
  double l1 = edges[1].length_squared();
  double l2 = edges[2].length_squared();
  double l3 = edges[3].length_squared();
  double l4 = edges[4].length_squared();
  double l5 = edges[5].length_squared();
  double l6 = edges[6].length_squared();
  double l7 = edges[7].length_squared();

  double max = std::min(l0, l1);
  max = std::max(max, l2);
  max = std::max(max, l3);
  max = std::max(max, l4);
  max = std::max(max, l5);
  max = std::max(max, l6);
  max = std::max(max, l7);

  return sqrt(max);
}

double distance_point_to_pyramid_base( int num_nodes, double coordinates[][3], double &cos_angle )
{
  VerdictVector a(coordinates[0]);
  VerdictVector b(coordinates[1]);
  VerdictVector c(coordinates[2]);
  VerdictVector d(coordinates[3]);
  VerdictVector peak(coordinates[4]);

  VerdictVector centroid = (a + b + c + d)/4.0;
  VerdictVector t1 = b - a;
  VerdictVector t2 = d - a;

  VerdictVector normal = t1 * t2;
  double normal_length = normal.length();

  VerdictVector pq = peak - centroid;

  double distance =  (pq % normal)/normal_length;
  cos_angle = distance/pq.length();

  return distance;
}

} // namespace verdict