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