1 /*
2  * Copyright (c) 2009-2010 jMonkeyEngine
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are
7  * met:
8  *
9  * * Redistributions of source code must retain the above copyright
10  *   notice, this list of conditions and the following disclaimer.
11  *
12  * * Redistributions in binary form must reproduce the above copyright
13  *   notice, this list of conditions and the following disclaimer in the
14  *   documentation and/or other materials provided with the distribution.
15  *
16  * * Neither the name of 'jMonkeyEngine' nor the names of its contributors
17  *   may be used to endorse or promote products derived from this software
18  *   without specific prior written permission.
19  *
20  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
21  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
22  * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
23  * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
24  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
25  * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
26  * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
27  * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
28  * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
29  * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
30  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
31  */
32 #include <math.h>
33 #include "jmeBulletUtil.h"
34 
35 /**
36  * Author: Normen Hansen,Empire Phoenix, Lutherion
37  */
convert(JNIEnv * env,jobject in,btVector3 * out)38 void jmeBulletUtil::convert(JNIEnv* env, jobject in, btVector3* out) {
39     if (in == NULL || out == NULL) {
40         jmeClasses::throwNPE(env);
41     }
42     float x = env->GetFloatField(in, jmeClasses::Vector3f_x); //env->CallFloatMethod(in, jmeClasses::Vector3f_getX);
43     if (env->ExceptionCheck()) {
44         env->Throw(env->ExceptionOccurred());
45         return;
46     }
47     float y = env->GetFloatField(in, jmeClasses::Vector3f_y); //env->CallFloatMethod(in, jmeClasses::Vector3f_getY);
48     if (env->ExceptionCheck()) {
49         env->Throw(env->ExceptionOccurred());
50         return;
51     }
52     float z = env->GetFloatField(in, jmeClasses::Vector3f_z); //env->CallFloatMethod(in, jmeClasses::Vector3f_getZ);
53     if (env->ExceptionCheck()) {
54         env->Throw(env->ExceptionOccurred());
55         return;
56     }
57     out->setX(x);
58     out->setY(y);
59     out->setZ(z);
60 }
61 
convert(JNIEnv * env,const btVector3 * in,jobject out)62 void jmeBulletUtil::convert(JNIEnv* env, const btVector3* in, jobject out) {
63     if (in == NULL || out == NULL) {
64         jmeClasses::throwNPE(env);
65     }
66     float x = in->getX();
67     float y = in->getY();
68     float z = in->getZ();
69     env->SetFloatField(out, jmeClasses::Vector3f_x, x);
70     if (env->ExceptionCheck()) {
71         env->Throw(env->ExceptionOccurred());
72         return;
73     }
74     env->SetFloatField(out, jmeClasses::Vector3f_y, y);
75     if (env->ExceptionCheck()) {
76         env->Throw(env->ExceptionOccurred());
77         return;
78     }
79     env->SetFloatField(out, jmeClasses::Vector3f_z, z);
80     //    env->CallObjectMethod(out, jmeClasses::Vector3f_set, x, y, z);
81     if (env->ExceptionCheck()) {
82         env->Throw(env->ExceptionOccurred());
83         return;
84     }
85 }
86 
convert(JNIEnv * env,jobject in,btMatrix3x3 * out)87 void jmeBulletUtil::convert(JNIEnv* env, jobject in, btMatrix3x3* out) {
88     if (in == NULL || out == NULL) {
89         jmeClasses::throwNPE(env);
90     }
91     float m00 = env->GetFloatField(in, jmeClasses::Matrix3f_m00);
92     if (env->ExceptionCheck()) {
93         env->Throw(env->ExceptionOccurred());
94         return;
95     }
96     float m01 = env->GetFloatField(in, jmeClasses::Matrix3f_m01);
97     if (env->ExceptionCheck()) {
98         env->Throw(env->ExceptionOccurred());
99         return;
100     }
101     float m02 = env->GetFloatField(in, jmeClasses::Matrix3f_m02);
102     if (env->ExceptionCheck()) {
103         env->Throw(env->ExceptionOccurred());
104         return;
105     }
106     float m10 = env->GetFloatField(in, jmeClasses::Matrix3f_m10);
107     if (env->ExceptionCheck()) {
108         env->Throw(env->ExceptionOccurred());
109         return;
110     }
111     float m11 = env->GetFloatField(in, jmeClasses::Matrix3f_m11);
112     if (env->ExceptionCheck()) {
113         env->Throw(env->ExceptionOccurred());
114         return;
115     }
116     float m12 = env->GetFloatField(in, jmeClasses::Matrix3f_m12);
117     if (env->ExceptionCheck()) {
118         env->Throw(env->ExceptionOccurred());
119         return;
120     }
121     float m20 = env->GetFloatField(in, jmeClasses::Matrix3f_m20);
122     if (env->ExceptionCheck()) {
123         env->Throw(env->ExceptionOccurred());
124         return;
125     }
126     float m21 = env->GetFloatField(in, jmeClasses::Matrix3f_m21);
127     if (env->ExceptionCheck()) {
128         env->Throw(env->ExceptionOccurred());
129         return;
130     }
131     float m22 = env->GetFloatField(in, jmeClasses::Matrix3f_m22);
132     if (env->ExceptionCheck()) {
133         env->Throw(env->ExceptionOccurred());
134         return;
135     }
136     out->setValue(m00, m01, m02, m10, m11, m12, m20, m21, m22);
137 }
138 
convert(JNIEnv * env,const btMatrix3x3 * in,jobject out)139 void jmeBulletUtil::convert(JNIEnv* env, const btMatrix3x3* in, jobject out) {
140     if (in == NULL || out == NULL) {
141         jmeClasses::throwNPE(env);
142     }
143     float m00 = in->getRow(0).m_floats[0];
144     float m01 = in->getRow(0).m_floats[1];
145     float m02 = in->getRow(0).m_floats[2];
146     float m10 = in->getRow(1).m_floats[0];
147     float m11 = in->getRow(1).m_floats[1];
148     float m12 = in->getRow(1).m_floats[2];
149     float m20 = in->getRow(2).m_floats[0];
150     float m21 = in->getRow(2).m_floats[1];
151     float m22 = in->getRow(2).m_floats[2];
152     env->SetFloatField(out, jmeClasses::Matrix3f_m00, m00);
153     if (env->ExceptionCheck()) {
154         env->Throw(env->ExceptionOccurred());
155         return;
156     }
157     env->SetFloatField(out, jmeClasses::Matrix3f_m01, m01);
158     if (env->ExceptionCheck()) {
159         env->Throw(env->ExceptionOccurred());
160         return;
161     }
162     env->SetFloatField(out, jmeClasses::Matrix3f_m02, m02);
163     if (env->ExceptionCheck()) {
164         env->Throw(env->ExceptionOccurred());
165         return;
166     }
167     env->SetFloatField(out, jmeClasses::Matrix3f_m10, m10);
168     if (env->ExceptionCheck()) {
169         env->Throw(env->ExceptionOccurred());
170         return;
171     }
172     env->SetFloatField(out, jmeClasses::Matrix3f_m11, m11);
173     if (env->ExceptionCheck()) {
174         env->Throw(env->ExceptionOccurred());
175         return;
176     }
177     env->SetFloatField(out, jmeClasses::Matrix3f_m12, m12);
178     if (env->ExceptionCheck()) {
179         env->Throw(env->ExceptionOccurred());
180         return;
181     }
182     env->SetFloatField(out, jmeClasses::Matrix3f_m20, m20);
183     if (env->ExceptionCheck()) {
184         env->Throw(env->ExceptionOccurred());
185         return;
186     }
187     env->SetFloatField(out, jmeClasses::Matrix3f_m21, m21);
188     if (env->ExceptionCheck()) {
189         env->Throw(env->ExceptionOccurred());
190         return;
191     }
192     env->SetFloatField(out, jmeClasses::Matrix3f_m22, m22);
193     if (env->ExceptionCheck()) {
194         env->Throw(env->ExceptionOccurred());
195         return;
196     }
197 }
198 
convertQuat(JNIEnv * env,jobject in,btMatrix3x3 * out)199 void jmeBulletUtil::convertQuat(JNIEnv* env, jobject in, btMatrix3x3* out) {
200     if (in == NULL || out == NULL) {
201         jmeClasses::throwNPE(env);
202     }
203     float x = env->GetFloatField(in, jmeClasses::Quaternion_x);
204     if (env->ExceptionCheck()) {
205         env->Throw(env->ExceptionOccurred());
206         return;
207     }
208     float y = env->GetFloatField(in, jmeClasses::Quaternion_y);
209     if (env->ExceptionCheck()) {
210         env->Throw(env->ExceptionOccurred());
211         return;
212     }
213     float z = env->GetFloatField(in, jmeClasses::Quaternion_z);
214     if (env->ExceptionCheck()) {
215         env->Throw(env->ExceptionOccurred());
216         return;
217     }
218     float w = env->GetFloatField(in, jmeClasses::Quaternion_w);
219     if (env->ExceptionCheck()) {
220         env->Throw(env->ExceptionOccurred());
221         return;
222     }
223 
224     float norm = w * w + x * x + y * y + z * z;
225     float s = (norm == 1.0) ? 2.0 : (norm > 0.1) ? 2.0 / norm : 0.0;
226 
227     // compute xs/ys/zs first to save 6 multiplications, since xs/ys/zs
228     // will be used 2-4 times each.
229     float xs = x * s;
230     float ys = y * s;
231     float zs = z * s;
232     float xx = x * xs;
233     float xy = x * ys;
234     float xz = x * zs;
235     float xw = w * xs;
236     float yy = y * ys;
237     float yz = y * zs;
238     float yw = w * ys;
239     float zz = z * zs;
240     float zw = w * zs;
241 
242     // using s=2/norm (instead of 1/norm) saves 9 multiplications by 2 here
243     out->setValue(1.0 - (yy + zz), (xy - zw), (xz + yw),
244             (xy + zw), 1 - (xx + zz), (yz - xw),
245             (xz - yw), (yz + xw), 1.0 - (xx + yy));
246 }
247 
convertQuat(JNIEnv * env,const btMatrix3x3 * in,jobject out)248 void jmeBulletUtil::convertQuat(JNIEnv* env, const btMatrix3x3* in, jobject out) {
249     if (in == NULL || out == NULL) {
250         jmeClasses::throwNPE(env);
251     }
252     // the trace is the sum of the diagonal elements; see
253     // http://mathworld.wolfram.com/MatrixTrace.html
254     float t = in->getRow(0).m_floats[0] + in->getRow(1).m_floats[1] + in->getRow(2).m_floats[2];
255     float w, x, y, z;
256     // we protect the division by s by ensuring that s>=1
257     if (t >= 0) { // |w| >= .5
258         float s = sqrt(t + 1); // |s|>=1 ...
259         w = 0.5f * s;
260         s = 0.5f / s; // so this division isn't bad
261         x = (in->getRow(2).m_floats[1] - in->getRow(1).m_floats[2]) * s;
262         y = (in->getRow(0).m_floats[2] - in->getRow(2).m_floats[0]) * s;
263         z = (in->getRow(1).m_floats[0] - in->getRow(0).m_floats[1]) * s;
264     } else if ((in->getRow(0).m_floats[0] > in->getRow(1).m_floats[1]) && (in->getRow(0).m_floats[0] > in->getRow(2).m_floats[2])) {
265         float s = sqrt(1.0f + in->getRow(0).m_floats[0] - in->getRow(1).m_floats[1] - in->getRow(2).m_floats[2]); // |s|>=1
266         x = s * 0.5f; // |x| >= .5
267         s = 0.5f / s;
268         y = (in->getRow(1).m_floats[0] + in->getRow(0).m_floats[1]) * s;
269         z = (in->getRow(0).m_floats[2] + in->getRow(2).m_floats[0]) * s;
270         w = (in->getRow(2).m_floats[1] - in->getRow(1).m_floats[2]) * s;
271     } else if (in->getRow(1).m_floats[1] > in->getRow(2).m_floats[2]) {
272         float s = sqrt(1.0f + in->getRow(1).m_floats[1] - in->getRow(0).m_floats[0] - in->getRow(2).m_floats[2]); // |s|>=1
273         y = s * 0.5f; // |y| >= .5
274         s = 0.5f / s;
275         x = (in->getRow(1).m_floats[0] + in->getRow(0).m_floats[1]) * s;
276         z = (in->getRow(2).m_floats[1] + in->getRow(1).m_floats[2]) * s;
277         w = (in->getRow(0).m_floats[2] - in->getRow(2).m_floats[0]) * s;
278     } else {
279         float s = sqrt(1.0f + in->getRow(2).m_floats[2] - in->getRow(0).m_floats[0] - in->getRow(1).m_floats[1]); // |s|>=1
280         z = s * 0.5f; // |z| >= .5
281         s = 0.5f / s;
282         x = (in->getRow(0).m_floats[2] + in->getRow(2).m_floats[0]) * s;
283         y = (in->getRow(2).m_floats[1] + in->getRow(1).m_floats[2]) * s;
284         w = (in->getRow(1).m_floats[0] - in->getRow(0).m_floats[1]) * s;
285     }
286 
287     env->SetFloatField(out, jmeClasses::Quaternion_x, x);
288     if (env->ExceptionCheck()) {
289         env->Throw(env->ExceptionOccurred());
290         return;
291     }
292     env->SetFloatField(out, jmeClasses::Quaternion_y, y);
293     if (env->ExceptionCheck()) {
294         env->Throw(env->ExceptionOccurred());
295         return;
296     }
297     env->SetFloatField(out, jmeClasses::Quaternion_z, z);
298     if (env->ExceptionCheck()) {
299         env->Throw(env->ExceptionOccurred());
300         return;
301     }
302     env->SetFloatField(out, jmeClasses::Quaternion_w, w);
303     //    env->CallObjectMethod(out, jmeClasses::Quaternion_set, x, y, z, w);
304     if (env->ExceptionCheck()) {
305         env->Throw(env->ExceptionOccurred());
306         return;
307     }
308 }
309 
addResult(JNIEnv * env,jobject resultlist,btVector3 hitnormal,btVector3 m_hitPointWorld,btScalar m_hitFraction,btCollisionObject * hitobject)310 void jmeBulletUtil::addResult(JNIEnv* env, jobject resultlist, btVector3 hitnormal, btVector3 m_hitPointWorld, btScalar m_hitFraction, btCollisionObject* hitobject) {
311 
312     jobject singleresult = env->AllocObject(jmeClasses::PhysicsRay_Class);
313     jobject hitnormalvec = env->AllocObject(jmeClasses::Vector3f);
314 
315     convert(env, const_cast<btVector3*> (&hitnormal), hitnormalvec);
316     jmeUserPointer *up1 = (jmeUserPointer*) hitobject -> getUserPointer();
317 
318     env->SetObjectField(singleresult, jmeClasses::PhysicsRay_normalInWorldSpace, hitnormalvec);
319     env->SetFloatField(singleresult, jmeClasses::PhysicsRay_hitfraction, m_hitFraction);
320 
321     env->SetObjectField(singleresult, jmeClasses::PhysicsRay_collisionObject, up1->javaCollisionObject);
322     env->CallVoidMethod(resultlist, jmeClasses::PhysicsRay_addmethod, singleresult);
323     if (env->ExceptionCheck()) {
324         env->Throw(env->ExceptionOccurred());
325         return;
326     }
327 }
328