| 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;
|
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.