Init.

Normand Briere

2014-10-23 2f3b3be15267b52f20585c8edb9ce68cf37a068a

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
/*
 * Java port of Bullet (c) 2008 Martin Dvorak <jezek2@advel.cz>
 *
 * Bullet Continuous Collision Detection and Physics Library
 * Copyright (c) 2003-2008 Erwin Coumans  http://www.bulletphysics.com/
 *
 * This software is provided 'as-is', without any express or implied warranty.
 * In no event will the authors be held liable for any damages arising from
 * the use of this software.
 * 
 * Permission is granted to anyone to use this software for any purpose, 
 * including commercial applications, and to alter it and redistribute it
 * freely, subject to the following restrictions:
 * 
 * 1. The origin of this software must not be misrepresented; you must not
 *    claim that you wrote the original software. If you use this software
 *    in a product, an acknowledgment in the product documentation would be
 *    appreciated but is not required.
 * 2. Altered source versions must be plainly marked as such, and must not be
 *    misrepresented as being the original software.
 * 3. This notice may not be removed or altered from any source distribution.
 */
 
package com.bulletphysics.dynamics.constraintsolver;
 
import com.bulletphysics.dynamics.RigidBody;
import com.bulletphysics.linearmath.Transform;
import com.bulletphysics.linearmath.TransformUtil;
import cz.advel.stack.Stack;
import javax.vecmath.Vector3f;
 
/**
 * SolverBody is an internal data structure for the constraint solver. Only necessary
 * data is packed to increase cache coherence/performance.
 * 
 * @author jezek2
 */
public class SolverBody  implements java.io.Serializable  {
    
    //protected final BulletStack stack = BulletStack.get();
 
    public final Vector3f angularVelocity = new Vector3f();
    public float angularFactor;
    public float invMass;
    public float friction;
    public RigidBody originalBody;
    public final Vector3f linearVelocity = new Vector3f();
    public final Vector3f centerOfMassPosition = new Vector3f();
 
    public final Vector3f pushVelocity = new Vector3f();
    public final Vector3f turnVelocity = new Vector3f();
    
    public void getVelocityInLocalPoint(Vector3f rel_pos, Vector3f velocity) {
        Vector3f tmp = Stack.alloc(Vector3f.class);
        tmp.cross(angularVelocity, rel_pos);
        velocity.add(linearVelocity, tmp);
    }
 
    /**
     * Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position.
     */
    public void internalApplyImpulse(Vector3f linearComponent, Vector3f angularComponent, float impulseMagnitude) {
        if (invMass != 0f) {
            linearVelocity.scaleAdd(impulseMagnitude, linearComponent, linearVelocity);
            angularVelocity.scaleAdd(impulseMagnitude * angularFactor, angularComponent, angularVelocity);
        }
    }
 
    public void internalApplyPushImpulse(Vector3f linearComponent, Vector3f angularComponent, float impulseMagnitude) {
        if (invMass != 0f) {
            pushVelocity.scaleAdd(impulseMagnitude, linearComponent, pushVelocity);
            turnVelocity.scaleAdd(impulseMagnitude * angularFactor, angularComponent, turnVelocity);
        }
    }
    
    public void writebackVelocity() {
        if (invMass != 0f) {
            originalBody.setLinearVelocity(linearVelocity);
            originalBody.setAngularVelocity(angularVelocity);
            //m_originalBody->setCompanionId(-1);
        }
    }
 
    public void writebackVelocity(float timeStep) {
        if (invMass != 0f) {
            originalBody.setLinearVelocity(linearVelocity);
            originalBody.setAngularVelocity(angularVelocity);
 
            // correct the position/orientation based on push/turn recovery
            Transform newTransform = Stack.alloc(Transform.class);
            Transform curTrans = originalBody.getWorldTransform(Stack.alloc(Transform.class));
            TransformUtil.integrateTransform(curTrans, pushVelocity, turnVelocity, timeStep, newTransform);
            originalBody.setWorldTransform(newTransform);
 
            //m_originalBody->setCompanionId(-1);
        }
    }
    
    public void readVelocity() {
        if (invMass != 0f) {
            originalBody.getLinearVelocity(linearVelocity);
            originalBody.getAngularVelocity(angularVelocity);
        }
    }
    
}