Revision b28f909c
Added by Leszek Koltunski about 2 months ago
src/main/java/org/distorted/objectlib/touchcontrol/TouchControlShapeChanging.java | ||
---|---|---|
49 | 49 |
} |
50 | 50 |
|
51 | 51 |
// assuming the first three vertices are linearly independent |
52 |
float a1 = vertices[0][0] - vertices[1][0]; |
|
53 |
float a2 = vertices[0][1] - vertices[1][1]; |
|
54 |
float a3 = vertices[0][2] - vertices[1][2]; |
|
55 |
float b1 = vertices[1][0] - vertices[2][0]; |
|
56 |
float b2 = vertices[1][1] - vertices[2][1]; |
|
57 |
float b3 = vertices[1][2] - vertices[2][2]; |
|
52 |
float[] v0 = vertices[0]; |
|
53 |
float[] v1 = vertices[1]; |
|
54 |
float[] v2 = vertices[2]; |
|
55 |
|
|
56 |
float a1 = v0[0] - v1[0]; |
|
57 |
float a2 = v0[1] - v1[1]; |
|
58 |
float a3 = v0[2] - v1[2]; |
|
59 |
float b1 = v1[0] - v2[0]; |
|
60 |
float b2 = v1[1] - v2[1]; |
|
61 |
float b3 = v1[2] - v2[2]; |
|
58 | 62 |
|
59 | 63 |
float vx = a2*b3-a3*b2; |
60 | 64 |
float vy = a3*b1-a1*b3; |
... | ... | |
66 | 70 |
vy/=len; |
67 | 71 |
vz/=len; |
68 | 72 |
|
69 |
distance = vx*vertices[0][0] + vy*vertices[0][1] + vz*vertices[0][2]; |
|
70 |
|
|
71 | 73 |
normal = new float[4]; |
72 | 74 |
normal[0] = vx; |
73 | 75 |
normal[1] = vy; |
74 | 76 |
normal[2] = vz; |
75 | 77 |
normal[3] = 0.0f; |
78 |
|
|
79 |
if( totalAngle(vertices,normal)<0 ) |
|
80 |
{ |
|
81 |
normal[0] *= -1; |
|
82 |
normal[1] *= -1; |
|
83 |
normal[2] *= -1; |
|
84 |
} |
|
85 |
|
|
86 |
distance = normal[0]*v0[0] + normal[1]*v0[1] + normal[2]*v0[2]; |
|
87 |
} |
|
88 |
|
|
89 |
////////////////////////////////////////////////////////// |
|
90 |
// this returns the total angle we get rotated about when |
|
91 |
// we travel from the first vert to the last. |
|
92 |
// It's always should be +2PI or -2PI, depending on if the |
|
93 |
// verts are CW or CCW (when looking at the plane formed by |
|
94 |
// the vertices from the direction the 'normal' vector |
|
95 |
// points towards) |
|
96 |
// |
|
97 |
// The point: this way we can detect if the 'normal' vector |
|
98 |
// is correct, i.e. if it points in the right direction. |
|
99 |
// Sometimes it does not, when the first three vertices |
|
100 |
// (from which the vector is computed) are 'concave'. |
|
101 |
|
|
102 |
private float totalAngle(float[][] vert, float[] normal) |
|
103 |
{ |
|
104 |
float ret = 0; |
|
105 |
int num = vert.length; |
|
106 |
|
|
107 |
for(int c=0; c<num; c++) |
|
108 |
{ |
|
109 |
int n = (c==num-1 ? 0 : c+1); |
|
110 |
int m = (n==num-1 ? 0 : n+1); |
|
111 |
ret += angle( vert[c],vert[n],vert[m], normal); |
|
112 |
} |
|
113 |
|
|
114 |
return ret; |
|
115 |
} |
|
116 |
|
|
117 |
////////////////////////////////////////////////////////// |
|
118 |
|
|
119 |
private float angle(float[] v0, float[] v1, float[] v2, float[] normal) |
|
120 |
{ |
|
121 |
float px = v1[0] - v0[0]; |
|
122 |
float py = v1[1] - v0[1]; |
|
123 |
float pz = v1[2] - v0[2]; |
|
124 |
|
|
125 |
float rx = v2[0] - v1[0]; |
|
126 |
float ry = v2[1] - v1[1]; |
|
127 |
float rz = v2[2] - v1[2]; |
|
128 |
|
|
129 |
float l1 = (float)Math.sqrt(px*px + py*py + pz*pz); |
|
130 |
float l2 = (float)Math.sqrt(rx*rx + ry*ry + rz*rz); |
|
131 |
|
|
132 |
px /= l1; |
|
133 |
py /= l1; |
|
134 |
pz /= l1; |
|
135 |
|
|
136 |
rx /= l2; |
|
137 |
ry /= l2; |
|
138 |
rz /= l2; |
|
139 |
|
|
140 |
float s = px*rx + py*ry + pz*rz; |
|
141 |
|
|
142 |
if( s> 1 ) s= 1; |
|
143 |
if( s<-1 ) s=-1; |
|
144 |
|
|
145 |
float a = (float) Math.acos(s); |
|
146 |
float[] cross = crossProduct(px,py,pz,rx,ry,rz); |
|
147 |
|
|
148 |
float ax = cross[0] + normal[0]; |
|
149 |
float ay = cross[1] + normal[1]; |
|
150 |
float az = cross[2] + normal[2]; |
|
151 |
|
|
152 |
float bx = cross[0] - normal[0]; |
|
153 |
float by = cross[1] - normal[1]; |
|
154 |
float bz = cross[2] - normal[2]; |
|
155 |
|
|
156 |
float f1 = ax*ax + ay*ay + az*az; |
|
157 |
float f2 = bx*bx + by*by + bz*bz; |
|
158 |
|
|
159 |
return f1>f2 ? a : -a; |
|
76 | 160 |
} |
77 | 161 |
|
78 | 162 |
////////////////////////////////////////////////////////// |
79 | 163 |
|
164 |
private float[] crossProduct(float a1, float a2, float a3, float b1, float b2, float b3) |
|
165 |
{ |
|
166 |
float[] ret = new float[3]; |
|
167 |
|
|
168 |
ret[0] = a2*b3 - a3*b2; |
|
169 |
ret[1] = a3*b1 - a1*b3; |
|
170 |
ret[2] = a1*b2 - a2*b1; |
|
171 |
|
|
172 |
return ret; |
|
173 |
} |
|
174 |
|
|
175 |
////////////////////////////////////////////////////////// |
|
80 | 176 |
public float[] getNormal() |
81 | 177 |
{ |
82 | 178 |
return normal; |
83 | 179 |
} |
84 | 180 |
} |
181 |
//////////////////////////////////////////////////////////// |
|
182 |
// end FaceInfo |
|
85 | 183 |
|
86 | 184 |
private final float[] mTouch, mLastT; |
87 | 185 |
private final Static4D mTmpAxis; |
Also available in: Unified diff
Important fix for the way vectors normal to the surfaces of the faces of the cubits are computed.
Before this was buggy in case of concave faces (CoinTetrahedron, CoinHexahedron, O2) - direction of the normal vector was wrong.