Page Menu
Home
c4science
Search
Configure Global Search
Log In
Files
F91374187
pair_smd_triangulated_surface.cpp
No One
Temporary
Actions
Download File
Edit File
Delete File
View Transforms
Subscribe
Mute Notifications
Award Token
Subscribers
None
File Metadata
Details
File Info
Storage
Attached
Created
Sun, Nov 10, 11:43
Size
21 KB
Mime Type
text/x-c
Expires
Tue, Nov 12, 11:43 (2 d)
Engine
blob
Format
Raw Data
Handle
22218366
Attached To
rLAMMPS lammps
pair_smd_triangulated_surface.cpp
View Options
/* ----------------------------------------------------------------------
*
* *** Smooth Mach Dynamics ***
*
* This file is part of the USER-SMD package for LAMMPS.
* Copyright (2014) Georg C. Ganzenmueller, georg.ganzenmueller@emi.fhg.de
* Fraunhofer Ernst-Mach Institute for High-Speed Dynamics, EMI,
* Eckerstrasse 4, D-79104 Freiburg i.Br, Germany.
*
* ----------------------------------------------------------------------- */
/* ----------------------------------------------------------------------
LAMMPS - Large-scale Atomic/Molecular Massively Parallel Simulator
http://lammps.sandia.gov, Sandia National Laboratories
Steve Plimpton, sjplimp@sandia.gov
Copyright (2003) Sandia Corporation. Under the terms of Contract
DE-AC04-94AL85000 with Sandia Corporation, the U.S. Government retains
certain rights in this software. This software is distributed under
the GNU General Public License.
See the README file in the top-level LAMMPS directory.
------------------------------------------------------------------------- */
/* ----------------------------------------------------------------------
Contributing author: Mike Parks (SNL)
------------------------------------------------------------------------- */
#include <math.h>
#include <float.h>
#include <stdlib.h>
#include <string.h>
#include "pair_smd_triangulated_surface.h"
#include "atom.h"
#include "domain.h"
#include "force.h"
#include "update.h"
#include "modify.h"
#include "fix.h"
#include "comm.h"
#include "neighbor.h"
#include "neigh_list.h"
#include "neigh_request.h"
#include "memory.h"
#include "error.h"
#include <Eigen/Eigen>
#include <stdio.h>
#include <iostream>
using
namespace
std
;
using
namespace
LAMMPS_NS
;
using
namespace
Eigen
;
#define SQRT2 1.414213562e0
/* ---------------------------------------------------------------------- */
PairTriSurf
::
PairTriSurf
(
LAMMPS
*
lmp
)
:
Pair
(
lmp
)
{
onerad_dynamic
=
onerad_frozen
=
maxrad_dynamic
=
maxrad_frozen
=
NULL
;
bulkmodulus
=
NULL
;
kn
=
NULL
;
scale
=
1.0
;
}
/* ---------------------------------------------------------------------- */
PairTriSurf
::~
PairTriSurf
()
{
if
(
allocated
)
{
memory
->
destroy
(
setflag
);
memory
->
destroy
(
cutsq
);
memory
->
destroy
(
bulkmodulus
);
memory
->
destroy
(
kn
);
delete
[]
onerad_dynamic
;
delete
[]
onerad_frozen
;
delete
[]
maxrad_dynamic
;
delete
[]
maxrad_frozen
;
}
}
/* ---------------------------------------------------------------------- */
void
PairTriSurf
::
compute
(
int
eflag
,
int
vflag
)
{
int
i
,
j
,
ii
,
jj
,
inum
,
jnum
,
itype
,
jtype
;
double
rsq
,
r
,
evdwl
,
fpair
;
int
*
ilist
,
*
jlist
,
*
numneigh
,
**
firstneigh
;
double
rcut
,
r_geom
,
delta
,
r_tri
,
r_particle
,
touch_distance
,
dt_crit
;
int
tri
,
particle
;
Vector3d
normal
,
x1
,
x2
,
x3
,
x4
,
x13
,
x23
,
x43
,
w
,
cp
,
x4cp
,
vnew
,
v_old
;
;
Vector3d
xi
,
x_center
,
dx
;
Matrix2d
C
;
Vector2d
w2d
,
rhs
;
evdwl
=
0.0
;
if
(
eflag
||
vflag
)
ev_setup
(
eflag
,
vflag
);
else
evflag
=
vflag_fdotr
=
0
;
tagint
*
mol
=
atom
->
molecule
;
double
**
f
=
atom
->
f
;
double
**
smd_data_9
=
atom
->
smd_data_9
;
double
**
x
=
atom
->
x
;
double
**
x0
=
atom
->
x0
;
double
**
v
=
atom
->
v
;
double
*
rmass
=
atom
->
rmass
;
int
*
type
=
atom
->
type
;
int
nlocal
=
atom
->
nlocal
;
double
*
radius
=
atom
->
contact_radius
;
double
rcutSq
;
Vector3d
offset
;
int
newton_pair
=
force
->
newton_pair
;
int
periodic
=
(
domain
->
xperiodic
||
domain
->
yperiodic
||
domain
->
zperiodic
);
inum
=
list
->
inum
;
ilist
=
list
->
ilist
;
numneigh
=
list
->
numneigh
;
firstneigh
=
list
->
firstneigh
;
int
max_neighs
=
0
;
stable_time_increment
=
1.0e22
;
// loop over neighbors of my atoms using a half neighbor list
for
(
ii
=
0
;
ii
<
inum
;
ii
++
)
{
i
=
ilist
[
ii
];
itype
=
type
[
i
];
jlist
=
firstneigh
[
i
];
jnum
=
numneigh
[
i
];
max_neighs
=
MAX
(
max_neighs
,
jnum
);
for
(
jj
=
0
;
jj
<
jnum
;
jj
++
)
{
j
=
jlist
[
jj
];
j
&=
NEIGHMASK
;
jtype
=
type
[
j
];
/*
* decide which one of i, j is triangle and which is particle
*/
if
((
mol
[
i
]
<
65535
)
&&
(
mol
[
j
]
>=
65535
))
{
particle
=
i
;
tri
=
j
;
}
else
if
((
mol
[
j
]
<
65535
)
&&
(
mol
[
i
]
>=
65535
))
{
particle
=
j
;
tri
=
i
;
}
else
{
error
->
one
(
FLERR
,
"unknown case"
);
}
//x_center << x[tri][0], x[tri][1], x[tri][2]; // center of triangle
x_center
(
0
)
=
x
[
tri
][
0
];
x_center
(
1
)
=
x
[
tri
][
1
];
x_center
(
2
)
=
x
[
tri
][
2
];
//x4 << x[particle][0], x[particle][1], x[particle][2];
x4
(
0
)
=
x
[
particle
][
0
];
x4
(
1
)
=
x
[
particle
][
1
];
x4
(
2
)
=
x
[
particle
][
2
];
dx
=
x_center
-
x4
;
//
if
(
periodic
)
{
domain
->
minimum_image
(
dx
(
0
),
dx
(
1
),
dx
(
2
));
}
rsq
=
dx
.
squaredNorm
();
r_tri
=
scale
*
radius
[
tri
];
r_particle
=
scale
*
radius
[
particle
];
rcut
=
r_tri
+
r_particle
;
rcutSq
=
rcut
*
rcut
;
//printf("type i=%d, type j=%d, r=%f, ri=%f, rj=%f\n", itype, jtype, sqrt(rsq), ri, rj);
if
(
rsq
<
rcutSq
)
{
/*
* gather triangle information
*/
normal
(
0
)
=
x0
[
tri
][
0
];
normal
(
1
)
=
x0
[
tri
][
1
];
normal
(
2
)
=
x0
[
tri
][
2
];
/*
* distance check: is particle closer than its radius to the triangle plane?
*/
if
(
fabs
(
dx
.
dot
(
normal
))
<
radius
[
particle
])
{
/*
* get other two triangle vertices
*/
x1
(
0
)
=
smd_data_9
[
tri
][
0
];
x1
(
1
)
=
smd_data_9
[
tri
][
1
];
x1
(
2
)
=
smd_data_9
[
tri
][
2
];
x2
(
0
)
=
smd_data_9
[
tri
][
3
];
x2
(
1
)
=
smd_data_9
[
tri
][
4
];
x2
(
2
)
=
smd_data_9
[
tri
][
5
];
x3
(
0
)
=
smd_data_9
[
tri
][
6
];
x3
(
1
)
=
smd_data_9
[
tri
][
7
];
x3
(
2
)
=
smd_data_9
[
tri
][
8
];
PointTriangleDistance
(
x4
,
x1
,
x2
,
x3
,
cp
,
r
);
/*
* distance to closest point
*/
x4cp
=
x4
-
cp
;
/*
* flip normal to point in direction of x4cp
*/
if
(
x4cp
.
dot
(
normal
)
<
0.0
)
{
normal
*=
-
1.0
;
}
/*
* penalty force pushes particle away from triangle
*/
if
(
r
<
1.0
*
radius
[
particle
])
{
delta
=
radius
[
particle
]
-
r
;
// overlap distance
r_geom
=
radius
[
particle
];
fpair
=
1.066666667e0
*
bulkmodulus
[
itype
][
jtype
]
*
delta
*
sqrt
(
delta
*
r_geom
);
dt_crit
=
3.14
*
sqrt
(
rmass
[
particle
]
/
(
fpair
/
delta
));
stable_time_increment
=
MIN
(
stable_time_increment
,
dt_crit
);
evdwl
=
r
*
fpair
*
0.4e0
*
delta
;
// GCG 25 April: this expression conserves total energy
fpair
/=
(
r
+
1.0e-2
*
radius
[
particle
]);
// divide by r + softening and multiply with non-normalized distance vector
if
(
particle
<
nlocal
)
{
f
[
particle
][
0
]
+=
x4cp
(
0
)
*
fpair
;
f
[
particle
][
1
]
+=
x4cp
(
1
)
*
fpair
;
f
[
particle
][
2
]
+=
x4cp
(
2
)
*
fpair
;
}
if
(
tri
<
nlocal
)
{
f
[
tri
][
0
]
-=
x4cp
(
0
)
*
fpair
;
f
[
tri
][
1
]
-=
x4cp
(
1
)
*
fpair
;
f
[
tri
][
2
]
-=
x4cp
(
2
)
*
fpair
;
}
if
(
evflag
)
{
ev_tally
(
i
,
j
,
nlocal
,
newton_pair
,
evdwl
,
0.0
,
fpair
,
x4cp
(
0
),
x4cp
(
1
),
x4cp
(
2
));
}
}
/*
* if particle comes too close to triangle, reflect its velocity and explicitely move it away
*/
touch_distance
=
1.0
*
radius
[
particle
];
if
(
r
<
touch_distance
)
{
/*
* reflect velocity if it points toward triangle
*/
normal
=
x4cp
/
r
;
//v_old << v[particle][0], v[particle][1], v[particle][2];
v_old
(
0
)
=
v
[
particle
][
0
];
v_old
(
1
)
=
v
[
particle
][
1
];
v_old
(
2
)
=
v
[
particle
][
2
];
if
(
v_old
.
dot
(
normal
)
<
0.0
)
{
//printf("flipping velocity\n");
vnew
=
1.0
*
(
-
2.0
*
v_old
.
dot
(
normal
)
*
normal
+
v_old
);
v
[
particle
][
0
]
=
vnew
(
0
);
v
[
particle
][
1
]
=
vnew
(
1
);
v
[
particle
][
2
]
=
vnew
(
2
);
}
//printf("moving particle on top of triangle\n");
x
[
particle
][
0
]
=
cp
(
0
)
+
touch_distance
*
normal
(
0
);
x
[
particle
][
1
]
=
cp
(
1
)
+
touch_distance
*
normal
(
1
);
x
[
particle
][
2
]
=
cp
(
2
)
+
touch_distance
*
normal
(
2
);
}
}
}
}
}
// int max_neighs_all = 0;
// MPI_Allreduce(&max_neighs, &max_neighs_all, 1, MPI_INT, MPI_MAX, world);
// if (comm->me == 0) {
// printf("max. neighs in tri pair is %d\n", max_neighs_all);
// }
//
// double stable_time_increment_all = 0.0;
// MPI_Allreduce(&stable_time_increment, &stable_time_increment_all, 1, MPI_DOUBLE, MPI_MIN, world);
// if (comm->me == 0) {
// printf("stable time step tri pair is %f\n", stable_time_increment_all);
// }
}
/* ----------------------------------------------------------------------
allocate all arrays
------------------------------------------------------------------------- */
void
PairTriSurf
::
allocate
()
{
allocated
=
1
;
int
n
=
atom
->
ntypes
;
memory
->
create
(
setflag
,
n
+
1
,
n
+
1
,
"pair:setflag"
);
for
(
int
i
=
1
;
i
<=
n
;
i
++
)
for
(
int
j
=
i
;
j
<=
n
;
j
++
)
setflag
[
i
][
j
]
=
0
;
memory
->
create
(
bulkmodulus
,
n
+
1
,
n
+
1
,
"pair:kspring"
);
memory
->
create
(
kn
,
n
+
1
,
n
+
1
,
"pair:kn"
);
memory
->
create
(
cutsq
,
n
+
1
,
n
+
1
,
"pair:cutsq"
);
// always needs to be allocated, even with granular neighborlist
onerad_dynamic
=
new
double
[
n
+
1
];
onerad_frozen
=
new
double
[
n
+
1
];
maxrad_dynamic
=
new
double
[
n
+
1
];
maxrad_frozen
=
new
double
[
n
+
1
];
}
/* ----------------------------------------------------------------------
global settings
------------------------------------------------------------------------- */
void
PairTriSurf
::
settings
(
int
narg
,
char
**
arg
)
{
if
(
narg
!=
1
)
error
->
all
(
FLERR
,
"Illegal number of args for pair_style smd/tri_surface"
);
scale
=
force
->
numeric
(
FLERR
,
arg
[
0
]);
if
(
comm
->
me
==
0
)
{
printf
(
"
\n
>>========>>========>>========>>========>>========>>========>>========>>========
\n
"
);
printf
(
"SMD/TRI_SURFACE CONTACT SETTINGS:
\n
"
);
printf
(
"... effective contact radius is scaled by %f
\n
"
,
scale
);
printf
(
">>========>>========>>========>>========>>========>>========>>========>>========
\n
"
);
}
}
/* ----------------------------------------------------------------------
set coeffs for one or more type pairs
------------------------------------------------------------------------- */
void
PairTriSurf
::
coeff
(
int
narg
,
char
**
arg
)
{
if
(
narg
!=
3
)
error
->
all
(
FLERR
,
"Incorrect args for pair coefficients"
);
if
(
!
allocated
)
allocate
();
int
ilo
,
ihi
,
jlo
,
jhi
;
force
->
bounds
(
arg
[
0
],
atom
->
ntypes
,
ilo
,
ihi
);
force
->
bounds
(
arg
[
1
],
atom
->
ntypes
,
jlo
,
jhi
);
double
bulkmodulus_one
=
atof
(
arg
[
2
]);
// set short-range force constant
double
kn_one
=
0.0
;
if
(
domain
->
dimension
==
3
)
{
kn_one
=
(
16.
/
15.
)
*
bulkmodulus_one
;
//assuming poisson ratio = 1/4 for 3d
}
else
{
kn_one
=
0.251856195
*
(
2.
/
3.
)
*
bulkmodulus_one
;
//assuming poisson ratio = 1/3 for 2d
}
int
count
=
0
;
for
(
int
i
=
ilo
;
i
<=
ihi
;
i
++
)
{
for
(
int
j
=
MAX
(
jlo
,
i
);
j
<=
jhi
;
j
++
)
{
bulkmodulus
[
i
][
j
]
=
bulkmodulus_one
;
kn
[
i
][
j
]
=
kn_one
;
setflag
[
i
][
j
]
=
1
;
count
++
;
}
}
if
(
count
==
0
)
error
->
all
(
FLERR
,
"Incorrect args for pair coefficients"
);
}
/* ----------------------------------------------------------------------
init for one type pair i,j and corresponding j,i
------------------------------------------------------------------------- */
double
PairTriSurf
::
init_one
(
int
i
,
int
j
)
{
if
(
!
allocated
)
allocate
();
if
(
setflag
[
i
][
j
]
==
0
)
error
->
all
(
FLERR
,
"All pair coeffs are not set"
);
bulkmodulus
[
j
][
i
]
=
bulkmodulus
[
i
][
j
];
kn
[
j
][
i
]
=
kn
[
i
][
j
];
// cutoff = sum of max I,J radii for
// dynamic/dynamic & dynamic/frozen interactions, but not frozen/frozen
double
cutoff
=
maxrad_dynamic
[
i
]
+
maxrad_dynamic
[
j
];
cutoff
=
MAX
(
cutoff
,
maxrad_frozen
[
i
]
+
maxrad_dynamic
[
j
]);
cutoff
=
MAX
(
cutoff
,
maxrad_dynamic
[
i
]
+
maxrad_frozen
[
j
]);
if
(
comm
->
me
==
0
)
{
printf
(
"cutoff for pair smd/smd/tri_surface = %f
\n
"
,
cutoff
);
}
return
cutoff
;
}
/* ----------------------------------------------------------------------
init specific to this pair style
------------------------------------------------------------------------- */
void
PairTriSurf
::
init_style
()
{
int
i
;
// error checks
if
(
!
atom
->
contact_radius_flag
)
error
->
all
(
FLERR
,
"Pair style smd/smd/tri_surface requires atom style with contact_radius"
);
// old: half list
int
irequest
=
neighbor
->
request
(
this
);
neighbor
->
requests
[
irequest
]
->
half
=
0
;
neighbor
->
requests
[
irequest
]
->
gran
=
1
;
// need a full neighbor list
// int irequest = neighbor->request(this);
// neighbor->requests[irequest]->half = 0;
// neighbor->requests[irequest]->full = 1;
// set maxrad_dynamic and maxrad_frozen for each type
// include future Fix pour particles as dynamic
for
(
i
=
1
;
i
<=
atom
->
ntypes
;
i
++
)
onerad_dynamic
[
i
]
=
onerad_frozen
[
i
]
=
0.0
;
double
*
radius
=
atom
->
radius
;
int
*
type
=
atom
->
type
;
int
nlocal
=
atom
->
nlocal
;
for
(
i
=
0
;
i
<
nlocal
;
i
++
)
{
onerad_dynamic
[
type
[
i
]]
=
MAX
(
onerad_dynamic
[
type
[
i
]],
radius
[
i
]);
}
MPI_Allreduce
(
&
onerad_dynamic
[
1
],
&
maxrad_dynamic
[
1
],
atom
->
ntypes
,
MPI_DOUBLE
,
MPI_MAX
,
world
);
MPI_Allreduce
(
&
onerad_frozen
[
1
],
&
maxrad_frozen
[
1
],
atom
->
ntypes
,
MPI_DOUBLE
,
MPI_MAX
,
world
);
}
/* ----------------------------------------------------------------------
neighbor callback to inform pair style of neighbor list to use
optional granular history list
------------------------------------------------------------------------- */
void
PairTriSurf
::
init_list
(
int
id
,
NeighList
*
ptr
)
{
if
(
id
==
0
)
list
=
ptr
;
}
/* ----------------------------------------------------------------------
memory usage of local atom-based arrays
------------------------------------------------------------------------- */
double
PairTriSurf
::
memory_usage
()
{
return
0.0
;
}
/*
* distance between triangle and point
*/
/*
function [dist,PP0] = pointTriangleDistance(TRI,P)
% calculate distance between a point and a triangle in 3D
% SYNTAX
% dist = pointTriangleDistance(TRI,P)
% [dist,PP0] = pointTriangleDistance(TRI,P)
%
% DESCRIPTION
% Calculate the distance of a given point P from a triangle TRI.
% Point P is a row vector of the form 1x3. The triangle is a matrix
% formed by three rows of points TRI = [P1;P2;P3] each of size 1x3.
% dist = pointTriangleDistance(TRI,P) returns the distance of the point P
% to the triangle TRI.
% [dist,PP0] = pointTriangleDistance(TRI,P) additionally returns the
% closest point PP0 to P on the triangle TRI.
%
% Author: Gwendolyn Fischer
% Release: 1.0
% Release date: 09/02/02
% Release: 1.1 Fixed Bug because of normalization
% Release: 1.2 Fixed Bug because of typo in region 5 20101013
% Release: 1.3 Fixed Bug because of typo in region 2 20101014
% Possible extention could be a version tailored not to return the distance
% and additionally the closest point, but instead return only the closest
% point. Could lead to a small speed gain.
% Example:
% %% The Problem
% P0 = [0.5 -0.3 0.5];
%
% P1 = [0 -1 0];
% P2 = [1 0 0];
% P3 = [0 0 0];
%
% vertices = [P1; P2; P3];
% faces = [1 2 3];
%
% %% The Engine
% [dist,PP0] = pointTriangleDistance([P1;P2;P3],P0);
%
% %% Visualization
% [x,y,z] = sphere(20);
% x = dist*x+P0(1);
% y = dist*y+P0(2);
% z = dist*z+P0(3);
%
% figure
% hold all
% patch('Vertices',vertices,'Faces',faces,'FaceColor','r','FaceAlpha',0.8);
% plot3(P0(1),P0(2),P0(3),'b*');
% plot3(PP0(1),PP0(2),PP0(3),'*g')
% surf(x,y,z,'FaceColor','b','FaceAlpha',0.3)
% view(3)
% The algorithm is based on
% "David Eberly, 'Distance Between Point and Triangle in 3D',
% Geometric Tools, LLC, (1999)"
% http:\\www.geometrictools.com/Documentation/DistancePoint3Triangle3.pdf
%
% ^t
% \ |
% \reg2|
% \ |
% \ |
% \ |
% \|
% *P2
% |\
% | \
% reg3 | \ reg1
% | \
% |reg0\
% | \
% | \ P1
% -------*-------*------->s
% |P0 \
% reg4 | reg5 \ reg6
*/
//void PairTriSurf::PointTriangleDistance(const Vector3d P, const Vector3d TRI1, const Vector3d TRI2, const Vector3d TRI3,
// Vector3d &CP, double &dist) {
//
// Vector3d B, E0, E1, D;
// double a, b, c, d, e, f;
// double det, s, t, sqrDistance, tmp0, tmp1, numer, denom, invDet;
//
// // rewrite triangle in normal form
// B = TRI1;
// E0 = TRI2 - B;
// E1 = TRI3 - B;
//
// D = B - P;
// a = E0.dot(E0);
// b = E0.dot(E1);
// c = E1.dot(E1);
// d = E0.dot(D);
// e = E1.dot(D);
// f = D.dot(D);
//
// det = a * c - b * b;
// //% do we have to use abs here?
// s = b * e - c * d;
// t = b * d - a * e;
//
// //% Terible tree of conditionals to determine in which region of the diagram
// //% shown above the projection of the point into the triangle-plane lies.
// if ((s + t) <= det) {
// if (s < 0) {
// if (t < 0) {
// // %region4
// if (d < 0) {
// t = 0;
// if (-d >= a) {
// s = 1;
// sqrDistance = a + 2 * d + f;
// } else {
// s = -d / a;
// sqrDistance = d * s + f;
// }
// } else {
// s = 0;
// if (e >= 0) {
// t = 0;
// sqrDistance = f;
// } else {
// if (-e >= c) {
// t = 1;
// sqrDistance = c + 2 * e + f;
// } else {
// t = -e / c;
// sqrDistance = e * t + f;
// }
// }
// }
// // end % of region 4
// } else {
// // % region 3
// s = 0;
// if (e >= 0) {
// t = 0;
// sqrDistance = f;
// } else {
// if (-e >= c) {
// t = 1;
// sqrDistance = c + 2 * e + f;
// } else {
// t = -e / c;
// sqrDistance = e * t + f;
// }
// }
// }
// // end of region 3
// } else {
// if (t < 0) {
// //% region 5
// t = 0;
// if (d >= 0) {
// s = 0;
// sqrDistance = f;
// } else {
// if (-d >= a) {
// s = 1;
// sqrDistance = a + 2 * d + f;
// } else {
// s = -d / a;
// sqrDistance = d * s + f;
// }
// }
// } else {
// // region 0
// invDet = 1 / det;
// s = s * invDet;
// t = t * invDet;
// sqrDistance = s * (a * s + b * t + 2 * d) + t * (b * s + c * t + 2 * e) + f;
// }
// }
// } else {
// if (s < 0) {
// // % region 2
// tmp0 = b + d;
// tmp1 = c + e;
// if (tmp1 > tmp0) { //% minimum on edge s+t=1
// numer = tmp1 - tmp0;
// denom = a - 2 * b + c;
// if (numer >= denom) {
// s = 1;
// t = 0;
// sqrDistance = a + 2 * d + f;
// } else {
// s = numer / denom;
// t = 1 - s;
// sqrDistance = s * (a * s + b * t + 2 * d) + t * (b * s + c * t + 2 * e) + f;
// }
// } else
// // % minimum on edge s=0
// s = 0;
// if (tmp1 <= 0) {
// t = 1;
// sqrDistance = c + 2 * e + f;
// } else {
// if (e >= 0) {
// t = 0;
// sqrDistance = f;
// } else {
// t = -e / c;
// sqrDistance = e * t + f;
// }
// }
// } //end % of region 2
// else {
// if (t < 0) {
// // %region6
// tmp0 = b + e;
// tmp1 = a + d;
// if (tmp1 > tmp0) {
// numer = tmp1 - tmp0;
// denom = a - 2 * b + c;
// if (numer >= denom) {
// t = 1;
// s = 0;
// sqrDistance = c + 2 * e + f;
// } else {
// t = numer / denom;
// s = 1 - t;
// sqrDistance = s * (a * s + b * t + 2 * d) + t * (b * s + c * t + 2 * e) + f;
// }
// } else {
// t = 0;
// if (tmp1 <= 0) {
// s = 1;
// sqrDistance = a + 2 * d + f;
// } else {
// if (d >= 0) {
// s = 0;
// sqrDistance = f;
// } else {
// s = -d / a;
// sqrDistance = d * s + f;
// }
// }
// } // % end region 6
// } else {
// //% region 1
// numer = c + e - b - d;
// if (numer <= 0) {
// s = 0;
// t = 1;
// sqrDistance = c + 2 * e + f;
// } else {
// denom = a - 2 * b + c;
// if (numer >= denom) {
// s = 1;
// t = 0;
// sqrDistance = a + 2 * d + f;
// } else {
// s = numer / denom;
// t = 1 - s;
// sqrDistance = s * (a * s + b * t + 2 * d) + t * (b * s + c * t + 2 * e) + f;
// }
// } //% end of region 1
// }
// }
// }
//
// // % account for numerical round-off error
// if (sqrDistance < 0) {
// sqrDistance = 0;
// }
//
// dist = sqrt(sqrDistance);
//
// // closest point
// CP = B + s * E0 + t * E1;
//
//}
/*
* % The algorithm is based on
% "David Eberly, 'Distance Between Point and Triangle in 3D',
% Geometric Tools, LLC, (1999)"
% http:\\www.geometrictools.com/Documentation/DistancePoint3Triangle3.pdf
*/
void
PairTriSurf
::
PointTriangleDistance
(
const
Vector3d
sourcePosition
,
const
Vector3d
TRI0
,
const
Vector3d
TRI1
,
const
Vector3d
TRI2
,
Vector3d
&
CP
,
double
&
dist
)
{
Vector3d
edge0
=
TRI1
-
TRI0
;
Vector3d
edge1
=
TRI2
-
TRI0
;
Vector3d
v0
=
TRI0
-
sourcePosition
;
double
a
=
edge0
.
dot
(
edge0
);
double
b
=
edge0
.
dot
(
edge1
);
double
c
=
edge1
.
dot
(
edge1
);
double
d
=
edge0
.
dot
(
v0
);
double
e
=
edge1
.
dot
(
v0
);
double
det
=
a
*
c
-
b
*
b
;
double
s
=
b
*
e
-
c
*
d
;
double
t
=
b
*
d
-
a
*
e
;
if
(
s
+
t
<
det
)
{
if
(
s
<
0.f
)
{
if
(
t
<
0.f
)
{
if
(
d
<
0.f
)
{
s
=
clamp
(
-
d
/
a
,
0.f
,
1.f
);
t
=
0.f
;
}
else
{
s
=
0.f
;
t
=
clamp
(
-
e
/
c
,
0.f
,
1.f
);
}
}
else
{
s
=
0.f
;
t
=
clamp
(
-
e
/
c
,
0.f
,
1.f
);
}
}
else
if
(
t
<
0.f
)
{
s
=
clamp
(
-
d
/
a
,
0.f
,
1.f
);
t
=
0.f
;
}
else
{
float
invDet
=
1.f
/
det
;
s
*=
invDet
;
t
*=
invDet
;
}
}
else
{
if
(
s
<
0.f
)
{
float
tmp0
=
b
+
d
;
float
tmp1
=
c
+
e
;
if
(
tmp1
>
tmp0
)
{
float
numer
=
tmp1
-
tmp0
;
float
denom
=
a
-
2
*
b
+
c
;
s
=
clamp
(
numer
/
denom
,
0.f
,
1.f
);
t
=
1
-
s
;
}
else
{
t
=
clamp
(
-
e
/
c
,
0.f
,
1.f
);
s
=
0.f
;
}
}
else
if
(
t
<
0.f
)
{
if
(
a
+
d
>
b
+
e
)
{
float
numer
=
c
+
e
-
b
-
d
;
float
denom
=
a
-
2
*
b
+
c
;
s
=
clamp
(
numer
/
denom
,
0.f
,
1.f
);
t
=
1
-
s
;
}
else
{
s
=
clamp
(
-
e
/
c
,
0.f
,
1.f
);
t
=
0.f
;
}
}
else
{
float
numer
=
c
+
e
-
b
-
d
;
float
denom
=
a
-
2
*
b
+
c
;
s
=
clamp
(
numer
/
denom
,
0.f
,
1.f
);
t
=
1.f
-
s
;
}
}
CP
=
TRI0
+
s
*
edge0
+
t
*
edge1
;
dist
=
(
CP
-
sourcePosition
).
norm
();
}
double
PairTriSurf
::
clamp
(
const
double
a
,
const
double
min
,
const
double
max
)
{
if
(
a
<
min
)
{
return
min
;
}
else
if
(
a
>
max
)
{
return
max
;
}
else
{
return
a
;
}
}
void
*
PairTriSurf
::
extract
(
const
char
*
str
,
int
&
i
)
{
//printf("in PairTriSurf::extract\n");
if
(
strcmp
(
str
,
"smd/tri_surface/stable_time_increment_ptr"
)
==
0
)
{
return
(
void
*
)
&
stable_time_increment
;
}
return
NULL
;
}
Event Timeline
Log In to Comment