material-point-method-sim/interpolation.cpp at master · JoshWolper/material-point-method-sim · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
//
// Created by ziyinqu on 11/9/17.
//

#include "interpolation.h"

Eigen::Vector3f calcWeights(float index_space, int& baseNode)
{
    baseNode = std::floor(index_space-0.5);

    Eigen::Vector3f output;

    float d0 = index_space - (float)baseNode; // 0.5<d0<1.5
    float z = 1.5f - d0;
    output[0] = 0.5 * z * z;

    float d1 = d0 - 1;          // -0.5<d1<0.5
    output[1] = 0.75 - d1 * d1;

    float d2 = 1 - d1;          // 0.5<d2<1.5
    float zz = 1.5f - d2;
    output[2] = 0.5 * zz * zz;

    return output;
}

Eigen::Vector4f calcCubicWeights(float index_space, int& baseNode)
{
    baseNode = std::floor(index_space-1);

    Eigen::Vector4f output;

    float d0 = index_space - (float)baseNode; // -2 < d0 < -1
    float z = 2 - d0;
    output[0] =  z * z * z / 6;

    float d1 = d0 - 1;          // -1 < d1 < 0
    output[1] = 0.5 * d1 * d1 * d1 - d1 * d1 + 2/3;

    float d2 = 1 - d1;          //  0 < d2 < 1
    output[2] = 0.5 * d2 * d2 * d2 - d2 * d2 + 2/3;

    float d3 = d2 + 1;          //  1 < d3 < 2
    output[3] = d3 * d3 * d3 / 6;

    return output;
}

Eigen::Vector3f calcGradWeights(float index_space, int baseNode)
{
    Eigen::Vector3f graInt;

    float d0 = index_space - (float)baseNode;
    float z = 1.5f - d0;

    float d1 = d0 - 1;

    float d2 = 1 - d1;
    float zz = 1.5f - d2;

    graInt[0] = -z;
    graInt[1] = -2*d1;
    graInt[2] = zz;

    return graInt;
}

Eigen::Vector4f calcCubicGradWeights(float index_space, int baseNode)
{
    baseNode = std::floor(index_space-1);

    Eigen::Vector4f output;

    float d0 = index_space - (float)baseNode; // -2 < d0 < -1
    float z = 2 - d0;
    output[0] =  0.5 * z * z;

    float d1 = d0 - 1;          // -1 < d1 < 0
    output[1] = 1.5 * d1 * d1 - 2 * d1;

    float d2 = 1 - d1;          //  0 < d2 < 1
    output[2] = 1.5 * d2 * d2 - 2 * d2;

    float d3 = d2 + 1;          //  1 < d3 < 2
    output[3] = 0.5 * d3 * d3;

    return output;

}

void QuadraticInterpolation(Eigen::Vector3f particlePos, Eigen::Vector3i& baseNode, Eigen::Matrix3f& wp, Eigen::Matrix3f& dwp)
{

    Eigen::Vector3f interX = calcWeights(particlePos[0], baseNode[0]);
    Eigen::Vector3f interY = calcWeights(particlePos[1], baseNode[1]);
    Eigen::Vector3f interZ = calcWeights(particlePos[2], baseNode[2]);

    // calculate weight matrix
    wp(0,0)= interX[0]; wp(0,1) = interX[1]; wp(0,2)=interX[2];
    wp(1,0)= interY[0]; wp(1,1) = interY[1]; wp(1,2)=interY[2];
    wp(2,0)= interZ[0]; wp(2,1) = interZ[1]; wp(2,2)=interZ[2];

    Eigen::Vector3f graIntX = calcGradWeights(particlePos[0], baseNode[0]);
    Eigen::Vector3f graIntY = calcGradWeights(particlePos[1], baseNode[1]);
    Eigen::Vector3f graIntZ = calcGradWeights(particlePos[2], baseNode[2]);

    // calculate gradient weight matrix
    dwp(0,0)= graIntX[0]; dwp(0,1) = graIntX[1]; dwp(0,2)=graIntX[2];
    dwp(1,0)= graIntY[0]; dwp(1,1) = graIntY[1]; dwp(1,2)=graIntY[2];
    dwp(2,0)= graIntZ[0]; dwp(2,1) = graIntZ[1]; dwp(2,2)=graIntZ[2];
}

void CubicInterpolation(Eigen::Vector3f particlePos, Eigen::Vector3i& baseNode, Eigen::Matrix4f& wp, Eigen::Matrix4f& dwp)
{

    Eigen::Vector4f interX = calcCubicWeights(particlePos[0], baseNode[0]);
    Eigen::Vector4f interY = calcCubicWeights(particlePos[1], baseNode[1]);
    Eigen::Vector4f interZ = calcCubicWeights(particlePos[2], baseNode[2]);

    // calculate weight matrix
    wp(0,0)= interX[0]; wp(0,1) = interX[1]; wp(0,2)=interX[2]; wp(0,3)=interX[3];
    wp(1,0)= interY[0]; wp(1,1) = interY[1]; wp(1,2)=interY[2]; wp(1,3)=interY[3];
    wp(2,0)= interZ[0]; wp(2,1) = interZ[1]; wp(2,2)=interZ[2]; wp(2,3)=interZ[3];

    Eigen::Vector4f graIntX = calcCubicGradWeights(particlePos[0], baseNode[0]);
    Eigen::Vector4f graIntY = calcCubicGradWeights(particlePos[1], baseNode[1]);
    Eigen::Vector4f graIntZ = calcCubicGradWeights(particlePos[2], baseNode[2]);

    // calculate gradient weight matrix
    dwp(0,0)= graIntX[0]; dwp(0,1) = graIntX[1]; dwp(0,2)=graIntX[2]; dwp(0,3)=graIntX[3];
    dwp(1,0)= graIntY[0]; dwp(1,1) = graIntY[1]; dwp(1,2)=graIntY[2]; dwp(1,3)=graIntY[3];
    dwp(2,0)= graIntZ[0]; dwp(2,1) = graIntZ[1]; dwp(2,2)=graIntZ[2]; dwp(2,3)=graIntZ[3];
}