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 package com.jme3.bullet.joints;
33 
34 import com.jme3.bullet.joints.motors.RotationalLimitMotor;
35 import com.jme3.bullet.joints.motors.TranslationalLimitMotor;
36 import com.jme3.bullet.objects.PhysicsRigidBody;
37 import com.jme3.export.InputCapsule;
38 import com.jme3.export.JmeExporter;
39 import com.jme3.export.JmeImporter;
40 import com.jme3.export.OutputCapsule;
41 import com.jme3.math.Matrix3f;
42 import com.jme3.math.Vector3f;
43 import java.io.IOException;
44 import java.util.Iterator;
45 import java.util.LinkedList;
46 import java.util.logging.Level;
47 import java.util.logging.Logger;
48 
49 /**
50  * <i>From bullet manual:</i><br>
51  * This generic constraint can emulate a variety of standard constraints,
52  * by configuring each of the 6 degrees of freedom (dof).
53  * The first 3 dof axis are linear axis, which represent translation of rigidbodies,
54  * and the latter 3 dof axis represent the angular motion. Each axis can be either locked,
55  * free or limited. On construction of a new btGeneric6DofConstraint, all axis are locked.
56  * Afterwards the axis can be reconfigured. Note that several combinations that
57  * include free and/or limited angular degrees of freedom are undefined.
58  * @author normenhansen
59  */
60 public class SixDofJoint extends PhysicsJoint {
61 
62     Matrix3f rotA, rotB;
63     boolean useLinearReferenceFrameA;
64     LinkedList<RotationalLimitMotor> rotationalMotors = new LinkedList<RotationalLimitMotor>();
65     TranslationalLimitMotor translationalMotor;
66     Vector3f angularUpperLimit = new Vector3f(Vector3f.POSITIVE_INFINITY);
67     Vector3f angularLowerLimit = new Vector3f(Vector3f.NEGATIVE_INFINITY);
68     Vector3f linearUpperLimit = new Vector3f(Vector3f.POSITIVE_INFINITY);
69     Vector3f linearLowerLimit = new Vector3f(Vector3f.NEGATIVE_INFINITY);
70 
SixDofJoint()71     public SixDofJoint() {
72     }
73 
74     /**
75      * @param pivotA local translation of the joint connection point in node A
76      * @param pivotB local translation of the joint connection point in node B
77      */
SixDofJoint(PhysicsRigidBody nodeA, PhysicsRigidBody nodeB, Vector3f pivotA, Vector3f pivotB, Matrix3f rotA, Matrix3f rotB, boolean useLinearReferenceFrameA)78     public SixDofJoint(PhysicsRigidBody nodeA, PhysicsRigidBody nodeB, Vector3f pivotA, Vector3f pivotB, Matrix3f rotA, Matrix3f rotB, boolean useLinearReferenceFrameA) {
79         super(nodeA, nodeB, pivotA, pivotB);
80         this.useLinearReferenceFrameA = useLinearReferenceFrameA;
81         this.rotA = rotA;
82         this.rotB = rotB;
83 
84         objectId = createJoint(nodeA.getObjectId(), nodeB.getObjectId(), pivotA, rotA, pivotB, rotB, useLinearReferenceFrameA);
85         Logger.getLogger(this.getClass().getName()).log(Level.INFO, "Created Joint {0}", Long.toHexString(objectId));
86         gatherMotors();
87     }
88 
89     /**
90      * @param pivotA local translation of the joint connection point in node A
91      * @param pivotB local translation of the joint connection point in node B
92      */
SixDofJoint(PhysicsRigidBody nodeA, PhysicsRigidBody nodeB, Vector3f pivotA, Vector3f pivotB, boolean useLinearReferenceFrameA)93     public SixDofJoint(PhysicsRigidBody nodeA, PhysicsRigidBody nodeB, Vector3f pivotA, Vector3f pivotB, boolean useLinearReferenceFrameA) {
94         super(nodeA, nodeB, pivotA, pivotB);
95         this.useLinearReferenceFrameA = useLinearReferenceFrameA;
96         rotA = new Matrix3f();
97         rotB = new Matrix3f();
98 
99         objectId = createJoint(nodeA.getObjectId(), nodeB.getObjectId(), pivotA, rotA, pivotB, rotB, useLinearReferenceFrameA);
100         Logger.getLogger(this.getClass().getName()).log(Level.INFO, "Created Joint {0}", Long.toHexString(objectId));
101         gatherMotors();
102     }
103 
gatherMotors()104     private void gatherMotors() {
105         for (int i = 0; i < 3; i++) {
106             RotationalLimitMotor rmot = new RotationalLimitMotor(getRotationalLimitMotor(objectId, i));
107             rotationalMotors.add(rmot);
108         }
109         translationalMotor = new TranslationalLimitMotor(getTranslationalLimitMotor(objectId));
110     }
111 
getRotationalLimitMotor(long objectId, int index)112     private native long getRotationalLimitMotor(long objectId, int index);
113 
getTranslationalLimitMotor(long objectId)114     private native long getTranslationalLimitMotor(long objectId);
115 
116     /**
117      * returns the TranslationalLimitMotor of this 6DofJoint which allows
118      * manipulating the translational axis
119      * @return the TranslationalLimitMotor
120      */
getTranslationalLimitMotor()121     public TranslationalLimitMotor getTranslationalLimitMotor() {
122         return translationalMotor;
123     }
124 
125     /**
126      * returns one of the three RotationalLimitMotors of this 6DofJoint which
127      * allow manipulating the rotational axes
128      * @param index the index of the RotationalLimitMotor
129      * @return the RotationalLimitMotor at the given index
130      */
getRotationalLimitMotor(int index)131     public RotationalLimitMotor getRotationalLimitMotor(int index) {
132         return rotationalMotors.get(index);
133     }
134 
setLinearUpperLimit(Vector3f vector)135     public void setLinearUpperLimit(Vector3f vector) {
136         linearUpperLimit.set(vector);
137         setLinearUpperLimit(objectId, vector);
138     }
139 
setLinearUpperLimit(long objctId, Vector3f vector)140     private native void setLinearUpperLimit(long objctId, Vector3f vector);
141 
setLinearLowerLimit(Vector3f vector)142     public void setLinearLowerLimit(Vector3f vector) {
143         linearLowerLimit.set(vector);
144         setLinearLowerLimit(objectId, vector);
145     }
146 
setLinearLowerLimit(long objctId, Vector3f vector)147     private native void setLinearLowerLimit(long objctId, Vector3f vector);
148 
setAngularUpperLimit(Vector3f vector)149     public void setAngularUpperLimit(Vector3f vector) {
150         angularUpperLimit.set(vector);
151         setAngularUpperLimit(objectId, vector);
152     }
153 
setAngularUpperLimit(long objctId, Vector3f vector)154     private native void setAngularUpperLimit(long objctId, Vector3f vector);
155 
setAngularLowerLimit(Vector3f vector)156     public void setAngularLowerLimit(Vector3f vector) {
157         angularLowerLimit.set(vector);
158         setAngularLowerLimit(objectId, vector);
159     }
160 
setAngularLowerLimit(long objctId, Vector3f vector)161     private native void setAngularLowerLimit(long objctId, Vector3f vector);
162 
createJoint(long objectIdA, long objectIdB, Vector3f pivotA, Matrix3f rotA, Vector3f pivotB, Matrix3f rotB, boolean useLinearReferenceFrameA)163     native long createJoint(long objectIdA, long objectIdB, Vector3f pivotA, Matrix3f rotA, Vector3f pivotB, Matrix3f rotB, boolean useLinearReferenceFrameA);
164 
165     @Override
read(JmeImporter im)166     public void read(JmeImporter im) throws IOException {
167         super.read(im);
168         InputCapsule capsule = im.getCapsule(this);
169 
170         objectId = createJoint(nodeA.getObjectId(), nodeB.getObjectId(), pivotA, rotA, pivotB, rotB, useLinearReferenceFrameA);
171         Logger.getLogger(this.getClass().getName()).log(Level.INFO, "Created Joint {0}", Long.toHexString(objectId));
172         gatherMotors();
173 
174         setAngularUpperLimit((Vector3f) capsule.readSavable("angularUpperLimit", new Vector3f(Vector3f.POSITIVE_INFINITY)));
175         setAngularLowerLimit((Vector3f) capsule.readSavable("angularLowerLimit", new Vector3f(Vector3f.NEGATIVE_INFINITY)));
176         setLinearUpperLimit((Vector3f) capsule.readSavable("linearUpperLimit", new Vector3f(Vector3f.POSITIVE_INFINITY)));
177         setLinearLowerLimit((Vector3f) capsule.readSavable("linearLowerLimit", new Vector3f(Vector3f.NEGATIVE_INFINITY)));
178 
179         for (int i = 0; i < 3; i++) {
180             RotationalLimitMotor rotationalLimitMotor = getRotationalLimitMotor(i);
181             rotationalLimitMotor.setBounce(capsule.readFloat("rotMotor" + i + "_Bounce", 0.0f));
182             rotationalLimitMotor.setDamping(capsule.readFloat("rotMotor" + i + "_Damping", 1.0f));
183             rotationalLimitMotor.setERP(capsule.readFloat("rotMotor" + i + "_ERP", 0.5f));
184             rotationalLimitMotor.setHiLimit(capsule.readFloat("rotMotor" + i + "_HiLimit", Float.POSITIVE_INFINITY));
185             rotationalLimitMotor.setLimitSoftness(capsule.readFloat("rotMotor" + i + "_LimitSoftness", 0.5f));
186             rotationalLimitMotor.setLoLimit(capsule.readFloat("rotMotor" + i + "_LoLimit", Float.NEGATIVE_INFINITY));
187             rotationalLimitMotor.setMaxLimitForce(capsule.readFloat("rotMotor" + i + "_MaxLimitForce", 300.0f));
188             rotationalLimitMotor.setMaxMotorForce(capsule.readFloat("rotMotor" + i + "_MaxMotorForce", 0.1f));
189             rotationalLimitMotor.setTargetVelocity(capsule.readFloat("rotMotor" + i + "_TargetVelocity", 0));
190             rotationalLimitMotor.setEnableMotor(capsule.readBoolean("rotMotor" + i + "_EnableMotor", false));
191         }
192         getTranslationalLimitMotor().setAccumulatedImpulse((Vector3f) capsule.readSavable("transMotor_AccumulatedImpulse", Vector3f.ZERO));
193         getTranslationalLimitMotor().setDamping(capsule.readFloat("transMotor_Damping", 1.0f));
194         getTranslationalLimitMotor().setLimitSoftness(capsule.readFloat("transMotor_LimitSoftness", 0.7f));
195         getTranslationalLimitMotor().setLowerLimit((Vector3f) capsule.readSavable("transMotor_LowerLimit", Vector3f.ZERO));
196         getTranslationalLimitMotor().setRestitution(capsule.readFloat("transMotor_Restitution", 0.5f));
197         getTranslationalLimitMotor().setUpperLimit((Vector3f) capsule.readSavable("transMotor_UpperLimit", Vector3f.ZERO));
198     }
199 
200     @Override
write(JmeExporter ex)201     public void write(JmeExporter ex) throws IOException {
202         super.write(ex);
203         OutputCapsule capsule = ex.getCapsule(this);
204         capsule.write(angularUpperLimit, "angularUpperLimit", new Vector3f(Vector3f.POSITIVE_INFINITY));
205         capsule.write(angularLowerLimit, "angularLowerLimit", new Vector3f(Vector3f.NEGATIVE_INFINITY));
206         capsule.write(linearUpperLimit, "linearUpperLimit", new Vector3f(Vector3f.POSITIVE_INFINITY));
207         capsule.write(linearLowerLimit, "linearLowerLimit", new Vector3f(Vector3f.NEGATIVE_INFINITY));
208         int i = 0;
209         for (Iterator<RotationalLimitMotor> it = rotationalMotors.iterator(); it.hasNext();) {
210             RotationalLimitMotor rotationalLimitMotor = it.next();
211             capsule.write(rotationalLimitMotor.getBounce(), "rotMotor" + i + "_Bounce", 0.0f);
212             capsule.write(rotationalLimitMotor.getDamping(), "rotMotor" + i + "_Damping", 1.0f);
213             capsule.write(rotationalLimitMotor.getERP(), "rotMotor" + i + "_ERP", 0.5f);
214             capsule.write(rotationalLimitMotor.getHiLimit(), "rotMotor" + i + "_HiLimit", Float.POSITIVE_INFINITY);
215             capsule.write(rotationalLimitMotor.getLimitSoftness(), "rotMotor" + i + "_LimitSoftness", 0.5f);
216             capsule.write(rotationalLimitMotor.getLoLimit(), "rotMotor" + i + "_LoLimit", Float.NEGATIVE_INFINITY);
217             capsule.write(rotationalLimitMotor.getMaxLimitForce(), "rotMotor" + i + "_MaxLimitForce", 300.0f);
218             capsule.write(rotationalLimitMotor.getMaxMotorForce(), "rotMotor" + i + "_MaxMotorForce", 0.1f);
219             capsule.write(rotationalLimitMotor.getTargetVelocity(), "rotMotor" + i + "_TargetVelocity", 0);
220             capsule.write(rotationalLimitMotor.isEnableMotor(), "rotMotor" + i + "_EnableMotor", false);
221             i++;
222         }
223         capsule.write(getTranslationalLimitMotor().getAccumulatedImpulse(), "transMotor_AccumulatedImpulse", Vector3f.ZERO);
224         capsule.write(getTranslationalLimitMotor().getDamping(), "transMotor_Damping", 1.0f);
225         capsule.write(getTranslationalLimitMotor().getLimitSoftness(), "transMotor_LimitSoftness", 0.7f);
226         capsule.write(getTranslationalLimitMotor().getLowerLimit(), "transMotor_LowerLimit", Vector3f.ZERO);
227         capsule.write(getTranslationalLimitMotor().getRestitution(), "transMotor_Restitution", 0.5f);
228         capsule.write(getTranslationalLimitMotor().getUpperLimit(), "transMotor_UpperLimit", Vector3f.ZERO);
229     }
230 }
231