[DEV] port some element of ephysiscs
This commit is contained in:
parent
7b6855ec0e
commit
f5ab47bade
@ -0,0 +1,12 @@
|
|||||||
|
package org.atriaSoft.ephysics;
|
||||||
|
|
||||||
|
/// Position correction technique used in the contact solver (for contacts)
|
||||||
|
/// BAUMGARTECONTACTS : Faster but can be innacurate and can lead to unexpected bounciness
|
||||||
|
/// in some situations (due to error correction factor being added to
|
||||||
|
/// the bodies momentum).
|
||||||
|
/// SPLITIMPULSES : A bit slower but the error correction factor is not added to the
|
||||||
|
/// bodies momentum. This is the option used by default.
|
||||||
|
public enum ContactsPositionCorrectionTechnique {
|
||||||
|
BAUMGARTECONTACTS,
|
||||||
|
SPLITIMPULSES,
|
||||||
|
}
|
@ -0,0 +1,9 @@
|
|||||||
|
package org.atriaSoft.ephysics;
|
||||||
|
|
||||||
|
/// Position correction technique used in the raint solver (for joints).
|
||||||
|
/// BAUMGARTEJOINTS : Faster but can be innacurate in some situations.
|
||||||
|
/// NONLINEARGAUSSSEIDEL : Slower but more precise. This is the option used by default.
|
||||||
|
public enum JointsPositionCorrectionTechnique {
|
||||||
|
BAUMGARTEJOINTS,
|
||||||
|
NONLINEARGAUSSSEIDEL
|
||||||
|
}
|
29
src/org/atriaSoft/ephysics/Log.java
Normal file
29
src/org/atriaSoft/ephysics/Log.java
Normal file
@ -0,0 +1,29 @@
|
|||||||
|
package org.atriaSoft.ephysics;
|
||||||
|
|
||||||
|
public class Log {
|
||||||
|
private static String LIBNAME = "ephysic";
|
||||||
|
public static void print(String data) {
|
||||||
|
System.out.println(data);
|
||||||
|
}
|
||||||
|
public static void critical(String data) {
|
||||||
|
System.out.println("[C] " + LIBNAME + " | " + data);
|
||||||
|
}
|
||||||
|
public static void error(String data) {
|
||||||
|
System.out.println("[E] " + LIBNAME + " | " + data);
|
||||||
|
}
|
||||||
|
public static void warning(String data) {
|
||||||
|
System.out.println("[W] " + LIBNAME + " | " + data);
|
||||||
|
}
|
||||||
|
public static void info(String data) {
|
||||||
|
System.out.println("[I] " + LIBNAME + " | " + data);
|
||||||
|
}
|
||||||
|
public static void debug(String data) {
|
||||||
|
System.out.println("[D] " + LIBNAME + " | " + data);
|
||||||
|
}
|
||||||
|
public static void verbose(String data) {
|
||||||
|
System.out.println("[V] " + LIBNAME + " | " + data);
|
||||||
|
}
|
||||||
|
public static void todo(String data) {
|
||||||
|
System.out.println("[TODO] " + LIBNAME + " | " + data);
|
||||||
|
}
|
||||||
|
}
|
70
src/org/atriaSoft/ephysics/Property.java
Normal file
70
src/org/atriaSoft/ephysics/Property.java
Normal file
@ -0,0 +1,70 @@
|
|||||||
|
package org.atriaSoft.ephysics;
|
||||||
|
|
||||||
|
public class Property {
|
||||||
|
// ------------------- Type definitions ------------------- //
|
||||||
|
//typedef Pair<long, long> longpair;
|
||||||
|
// ------------------- Constants ------------------- //
|
||||||
|
public final static float FLTEPSILON = 0.00001f; // TODO: check this ...
|
||||||
|
/// Pi ant
|
||||||
|
public final static float PI = 3.14159265f;
|
||||||
|
|
||||||
|
/// 2*Pi ant
|
||||||
|
public final static float PITIMES2 = 6.28318530f;
|
||||||
|
|
||||||
|
/// Default friction coefficient for a rigid body
|
||||||
|
public final static float DEFAULTFRICTIONCOEFFICIENT = 0.3f;
|
||||||
|
|
||||||
|
/// Default bounciness factor for a rigid body
|
||||||
|
public final static float DEFAULTBOUNCINESS = 0.5f;
|
||||||
|
|
||||||
|
/// Default rolling resistance
|
||||||
|
public final static float DEFAULTROLLINGRESISTANCE = 0.0f;
|
||||||
|
|
||||||
|
/// True if the spleeping technique is enabled
|
||||||
|
public final static boolean SPLEEPINGENABLED = true;
|
||||||
|
|
||||||
|
/// Object margin for collision detection in meters (for the GJK-EPA Algorithm)
|
||||||
|
public final static float OBJECTMARGIN = 0.04f;
|
||||||
|
|
||||||
|
/// Distance threshold for two contact points for a valid persistent contact (in meters)
|
||||||
|
public final static float PERSISTENTCONTACTDISTTHRESHOLD = 0.03f;
|
||||||
|
|
||||||
|
/// Velocity threshold for contact velocity restitution
|
||||||
|
public final static float RESTITUTIONVELOCITYTHRESHOLD = 1.0f;
|
||||||
|
|
||||||
|
/// Number of iterations when solving the velocity raints of the Sequential Impulse technique
|
||||||
|
public final static int DEFAULTVELOCITYSOLVERNBITERATIONS = 10;
|
||||||
|
|
||||||
|
/// Number of iterations when solving the position raints of the Sequential Impulse technique
|
||||||
|
public final static int DEFAULTPOSITIONSOLVERNBITERATIONS = 5;
|
||||||
|
|
||||||
|
/// Time (in seconds) that a body must stay still to be considered sleeping
|
||||||
|
public final static float DEFAULTTIMEBEFORESLEEP = 1.0f;
|
||||||
|
|
||||||
|
/// A body with a linear velocity smaller than the sleep linear velocity (in m/s)
|
||||||
|
/// might enter sleeping mode.
|
||||||
|
public final static float DEFAULTSLEEPLINEARVELOCITY = 0.02f;
|
||||||
|
|
||||||
|
/// A body with angular velocity smaller than the sleep angular velocity (in rad/s)
|
||||||
|
/// might enter sleeping mode
|
||||||
|
public final static float DEFAULTSLEEPANGULARVELOCITY = 3.0f * (PI / 180.0f);
|
||||||
|
|
||||||
|
/// In the broad-phase collision detection (dynamic AABB tree), the AABBs are
|
||||||
|
/// inflated with a ant gap to allow the collision shape to move a little bit
|
||||||
|
/// without triggering a large modification of the tree which can be costly
|
||||||
|
public final static float DYNAMICTREEAABBGAP = 0.1f;
|
||||||
|
|
||||||
|
/// In the broad-phase collision detection (dynamic AABB tree), the AABBs are
|
||||||
|
/// also inflated in direction of the linear motion of the body by mutliplying the
|
||||||
|
/// followin ant with the linear velocity and the elapsed time between two frames.
|
||||||
|
public final static float DYNAMICTREEAABBLINGAPMULTIPLIER = 1.7f;
|
||||||
|
|
||||||
|
/// Maximum number of contact manifolds in an overlapping pair that involves two
|
||||||
|
/// convex collision shapes.
|
||||||
|
public final static int NBMAXCONTACTMANIFOLDSCONVEXSHAPE = 1;
|
||||||
|
|
||||||
|
/// Maximum number of contact manifolds in an overlapping pair that involves at
|
||||||
|
/// least one concave collision shape.
|
||||||
|
public final static int NBMAXCONTACTMANIFOLDSCONCAVESHAPE = 3;
|
||||||
|
}
|
||||||
|
|
@ -112,7 +112,7 @@ class Body {
|
|||||||
* @return true if the current element is smaller
|
* @return true if the current element is smaller
|
||||||
*/
|
*/
|
||||||
public boolean isLess( Body obj) {
|
public boolean isLess( Body obj) {
|
||||||
return (this.id < obj.this.id);
|
return (this.id < obj.id);
|
||||||
}
|
}
|
||||||
/**
|
/**
|
||||||
* @brief Larger than operator
|
* @brief Larger than operator
|
||||||
@ -120,7 +120,7 @@ class Body {
|
|||||||
* @return true if the current element is Bigger
|
* @return true if the current element is Bigger
|
||||||
*/
|
*/
|
||||||
public boolean isUpper( Body obj) {
|
public boolean isUpper( Body obj) {
|
||||||
return (this.id > obj.this.id);
|
return (this.id > obj.id);
|
||||||
}
|
}
|
||||||
/**
|
/**
|
||||||
* @brief Equal operator
|
* @brief Equal operator
|
||||||
@ -128,7 +128,7 @@ class Body {
|
|||||||
* @return true if the curretn element is equal
|
* @return true if the curretn element is equal
|
||||||
*/
|
*/
|
||||||
public boolean isEqual( Body obj) {
|
public boolean isEqual( Body obj) {
|
||||||
return (this.id == obj.this.id);
|
return (this.id == obj.id);
|
||||||
}
|
}
|
||||||
/**
|
/**
|
||||||
* @brief Not equal operator
|
* @brief Not equal operator
|
||||||
@ -136,6 +136,6 @@ class Body {
|
|||||||
* @return true if the curretn element is NOT equal
|
* @return true if the curretn element is NOT equal
|
||||||
*/
|
*/
|
||||||
public boolean isDifferent( Body obj) {
|
public boolean isDifferent( Body obj) {
|
||||||
return (this.id != obj.this.id);
|
return (this.id != obj.id);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
7
src/org/atriaSoft/ephysics/body/BodyType.java
Normal file
7
src/org/atriaSoft/ephysics/body/BodyType.java
Normal file
@ -0,0 +1,7 @@
|
|||||||
|
package org.atriaSoft.ephysics.body;
|
||||||
|
|
||||||
|
public enum BodyType {
|
||||||
|
STATIC, //!< A static body has infinite mass, zero velocity but the position can be changed manually. A static body does not collide with other static or kinematic bodies.
|
||||||
|
KINEMATIC, //!< A kinematic body has infinite mass, the velocity can be changed manually and its position is computed by the physics engine. A kinematic body does not collide with other static or kinematic bodies.
|
||||||
|
DYNAMIC //!< A dynamic body has non-zero mass, non-zero velocity determined by forces and its position is determined by the physics engine. A dynamic body can collide with other dynamic, static or kinematic bodies.
|
||||||
|
}
|
@ -1,238 +0,0 @@
|
|||||||
/** @file
|
|
||||||
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
|
|
||||||
* @author Daniel CHAPPUIS
|
|
||||||
* @author Edouard DUPIN
|
|
||||||
* @copyright 2010-2016, Daniel Chappuis
|
|
||||||
* @copyright 2017, Edouard DUPIN
|
|
||||||
* @license MPL v2.0 (see license file)
|
|
||||||
*/
|
|
||||||
#include <ephysics/body/CollisionBody.hpp>
|
|
||||||
#include <ephysics/engine/CollisionWorld.hpp>
|
|
||||||
#include <ephysics/collision/ContactManifold.hpp>
|
|
||||||
|
|
||||||
// We want to use the ReactPhysics3D namespace
|
|
||||||
using namespace ephysics;
|
|
||||||
|
|
||||||
|
|
||||||
CollisionBody::CollisionBody( etk::Transform3D transform, CollisionWorld world, long id):
|
|
||||||
Body(id),
|
|
||||||
this.type(DYNAMIC),
|
|
||||||
this.transform(transform),
|
|
||||||
this.proxyCollisionShapes(null),
|
|
||||||
this.numberCollisionShapes(0),
|
|
||||||
this.contactManifoldsList(null),
|
|
||||||
this.world(world) {
|
|
||||||
|
|
||||||
Log.debug(" set transform: " << transform);
|
|
||||||
if (isnan(transform.getPosition().x()) == true) { // check NAN
|
|
||||||
Log.critical(" set transform: " << transform);
|
|
||||||
}
|
|
||||||
if (isinf(transform.getOrientation().z()) == true) {
|
|
||||||
Log.critical(" set transform: " << transform);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
CollisionBody::~CollisionBody() {
|
|
||||||
assert(this.contactManifoldsList == null);
|
|
||||||
|
|
||||||
// Remove all the proxy collision shapes of the body
|
|
||||||
removeAllCollisionShapes();
|
|
||||||
}
|
|
||||||
|
|
||||||
inline void CollisionBody::setType(BodyType type) {
|
|
||||||
this.type = type;
|
|
||||||
if (this.type == STATIC) {
|
|
||||||
// Update the broad-phase state of the body
|
|
||||||
updateBroadPhaseState();
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void CollisionBody::setTransform( etk::Transform3D transform) {
|
|
||||||
Log.debug(" set transform: " << this.transform << " ==> " << transform);
|
|
||||||
if (isnan(transform.getPosition().x()) == true) { // check NAN
|
|
||||||
Log.critical(" set transform: " << this.transform << " ==> " << transform);
|
|
||||||
}
|
|
||||||
if (isinf(transform.getOrientation().z()) == true) {
|
|
||||||
Log.critical(" set transform: " << this.transform << " ==> " << transform);
|
|
||||||
}
|
|
||||||
this.transform = transform;
|
|
||||||
updateBroadPhaseState();
|
|
||||||
}
|
|
||||||
|
|
||||||
ProxyShape* CollisionBody::addCollisionShape(CollisionShape* collisionShape,
|
|
||||||
etk::Transform3D transform) {
|
|
||||||
// Create a proxy collision shape to attach the collision shape to the body
|
|
||||||
ProxyShape* proxyShape = ETKNEW(ProxyShape, this, collisionShape,transform, float(1));
|
|
||||||
// Add it to the list of proxy collision shapes of the body
|
|
||||||
if (this.proxyCollisionShapes == null) {
|
|
||||||
this.proxyCollisionShapes = proxyShape;
|
|
||||||
} else {
|
|
||||||
proxyShape->this.next = this.proxyCollisionShapes;
|
|
||||||
this.proxyCollisionShapes = proxyShape;
|
|
||||||
}
|
|
||||||
AABB aabb;
|
|
||||||
collisionShape->computeAABB(aabb, this.transform * transform);
|
|
||||||
this.world.this.collisionDetection.addProxyCollisionShape(proxyShape, aabb);
|
|
||||||
this.numberCollisionShapes++;
|
|
||||||
return proxyShape;
|
|
||||||
}
|
|
||||||
|
|
||||||
void CollisionBody::removeCollisionShape( ProxyShape* proxyShape) {
|
|
||||||
ProxyShape* current = this.proxyCollisionShapes;
|
|
||||||
// If the the first proxy shape is the one to remove
|
|
||||||
if (current == proxyShape) {
|
|
||||||
this.proxyCollisionShapes = current->this.next;
|
|
||||||
if (this.isActive) {
|
|
||||||
this.world.this.collisionDetection.removeProxyCollisionShape(current);
|
|
||||||
}
|
|
||||||
ETKDELETE(ProxyShape, current);
|
|
||||||
current = null;
|
|
||||||
this.numberCollisionShapes--;
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
// Look for the proxy shape that contains the collision shape in parameter
|
|
||||||
while(current->this.next != null) {
|
|
||||||
// If we have found the collision shape to remove
|
|
||||||
if (current->this.next == proxyShape) {
|
|
||||||
// Remove the proxy collision shape
|
|
||||||
ProxyShape* elementToRemove = current->this.next;
|
|
||||||
current->this.next = elementToRemove->this.next;
|
|
||||||
if (this.isActive) {
|
|
||||||
this.world.this.collisionDetection.removeProxyCollisionShape(elementToRemove);
|
|
||||||
}
|
|
||||||
ETKDELETE(ProxyShape, elementToRemove);
|
|
||||||
elementToRemove = null;
|
|
||||||
this.numberCollisionShapes--;
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
// Get the next element in the list
|
|
||||||
current = current->this.next;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
void CollisionBody::removeAllCollisionShapes() {
|
|
||||||
ProxyShape* current = this.proxyCollisionShapes;
|
|
||||||
// Look for the proxy shape that contains the collision shape in parameter
|
|
||||||
while(current != null) {
|
|
||||||
// Remove the proxy collision shape
|
|
||||||
ProxyShape* nextElement = current->this.next;
|
|
||||||
if (this.isActive) {
|
|
||||||
this.world.this.collisionDetection.removeProxyCollisionShape(current);
|
|
||||||
}
|
|
||||||
ETKDELETE(ProxyShape, current);
|
|
||||||
// Get the next element in the list
|
|
||||||
current = nextElement;
|
|
||||||
}
|
|
||||||
this.proxyCollisionShapes = null;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
void CollisionBody::resetContactManifoldsList() {
|
|
||||||
// Delete the linked list of contact manifolds of that body
|
|
||||||
ContactManifoldListElement* currentElement = this.contactManifoldsList;
|
|
||||||
while (currentElement != null) {
|
|
||||||
ContactManifoldListElement* nextElement = currentElement->next;
|
|
||||||
// Delete the current element
|
|
||||||
ETKDELETE(ContactManifoldListElement, currentElement);
|
|
||||||
currentElement = nextElement;
|
|
||||||
}
|
|
||||||
this.contactManifoldsList = null;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
void CollisionBody::updateBroadPhaseState() {
|
|
||||||
// For all the proxy collision shapes of the body
|
|
||||||
for (ProxyShape* shape = this.proxyCollisionShapes; shape != null; shape = shape->this.next) {
|
|
||||||
// Update the proxy
|
|
||||||
updateProxyShapeInBroadPhase(shape);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
void CollisionBody::updateProxyShapeInBroadPhase(ProxyShape* proxyShape, boolean forceReinsert) {
|
|
||||||
AABB aabb;
|
|
||||||
proxyShape->getCollisionShape()->computeAABB(aabb, this.transform * proxyShape->getLocalToBodyTransform());
|
|
||||||
this.world.this.collisionDetection.updateProxyCollisionShape(proxyShape, aabb, vec3(0, 0, 0), forceReinsert);
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
void CollisionBody::setIsActive(boolean isActive) {
|
|
||||||
// If the state does not change
|
|
||||||
if (this.isActive == isActive) {
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
Body::setIsActive(isActive);
|
|
||||||
// If we have to activate the body
|
|
||||||
if (isActive == true) {
|
|
||||||
for (ProxyShape* shape = this.proxyCollisionShapes; shape != null; shape = shape->this.next) {
|
|
||||||
AABB aabb;
|
|
||||||
shape->getCollisionShape()->computeAABB(aabb, this.transform * shape->this.localToBodyTransform);
|
|
||||||
this.world.this.collisionDetection.addProxyCollisionShape(shape, aabb);
|
|
||||||
}
|
|
||||||
} else {
|
|
||||||
for (ProxyShape* shape = this.proxyCollisionShapes; shape != null; shape = shape->this.next) {
|
|
||||||
this.world.this.collisionDetection.removeProxyCollisionShape(shape);
|
|
||||||
}
|
|
||||||
resetContactManifoldsList();
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
void CollisionBody::askForBroadPhaseCollisionCheck() {
|
|
||||||
for (ProxyShape* shape = this.proxyCollisionShapes; shape != null; shape = shape->this.next) {
|
|
||||||
this.world.this.collisionDetection.askForBroadPhaseCollisionCheck(shape);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
int CollisionBody::resetIsAlreadyInIslandAndCountManifolds() {
|
|
||||||
this.isAlreadyInIsland = false;
|
|
||||||
int nbManifolds = 0;
|
|
||||||
// Reset the this.isAlreadyInIsland variable of the contact manifolds for this body
|
|
||||||
ContactManifoldListElement* currentElement = this.contactManifoldsList;
|
|
||||||
while (currentElement != null) {
|
|
||||||
currentElement->contactManifold->this.isAlreadyInIsland = false;
|
|
||||||
currentElement = currentElement->next;
|
|
||||||
nbManifolds++;
|
|
||||||
}
|
|
||||||
return nbManifolds;
|
|
||||||
}
|
|
||||||
|
|
||||||
boolean CollisionBody::testPointInside( vec3 worldPoint) {
|
|
||||||
for (ProxyShape* shape = this.proxyCollisionShapes; shape != null; shape = shape->this.next) {
|
|
||||||
if (shape->testPointInside(worldPoint)) return true;
|
|
||||||
}
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
boolean CollisionBody::raycast( Ray ray, RaycastInfo raycastInfo) {
|
|
||||||
if (this.isActive == false) {
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
boolean isHit = false;
|
|
||||||
Ray rayTemp(ray);
|
|
||||||
for (ProxyShape* shape = this.proxyCollisionShapes; shape != null; shape = shape->this.next) {
|
|
||||||
// Test if the ray hits the collision shape
|
|
||||||
if (shape->raycast(rayTemp, raycastInfo)) {
|
|
||||||
rayTemp.maxFraction = raycastInfo.hitFraction;
|
|
||||||
isHit = true;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
return isHit;
|
|
||||||
}
|
|
||||||
|
|
||||||
AABB CollisionBody::getAABB() {
|
|
||||||
AABB bodyAABB;
|
|
||||||
if (this.proxyCollisionShapes == null) {
|
|
||||||
return bodyAABB;
|
|
||||||
}
|
|
||||||
this.proxyCollisionShapes->getCollisionShape()->computeAABB(bodyAABB, this.transform * this.proxyCollisionShapes->getLocalToBodyTransform());
|
|
||||||
for (ProxyShape* shape = this.proxyCollisionShapes->this.next; shape != null; shape = shape->this.next) {
|
|
||||||
AABB aabb;
|
|
||||||
shape->getCollisionShape()->computeAABB(aabb, this.transform * shape->getLocalToBodyTransform());
|
|
||||||
bodyAABB.mergeWithAABB(aabb);
|
|
||||||
}
|
|
||||||
return bodyAABB;
|
|
||||||
}
|
|
||||||
|
|
@ -1,209 +0,0 @@
|
|||||||
/** @file
|
|
||||||
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
|
|
||||||
* @author Daniel CHAPPUIS
|
|
||||||
* @author Edouard DUPIN
|
|
||||||
* @copyright 2010-2016, Daniel Chappuis
|
|
||||||
* @copyright 2017, Edouard DUPIN
|
|
||||||
* @license MPL v2.0 (see license file)
|
|
||||||
*/
|
|
||||||
#pragma once
|
|
||||||
#include <ephysics/body/Body.hpp>
|
|
||||||
#include <etk/math/Transform3D.hpp>
|
|
||||||
#include <ephysics/collision/shapes/AABB.hpp>
|
|
||||||
#include <ephysics/collision/shapes/CollisionShape.hpp>
|
|
||||||
#include <ephysics/collision/RaycastInfo.hpp>
|
|
||||||
#include <ephysics/configuration.hpp>
|
|
||||||
|
|
||||||
namespace ephysics {
|
|
||||||
struct ContactManifoldListElement;
|
|
||||||
class ProxyShape;
|
|
||||||
class CollisionWorld;
|
|
||||||
/**
|
|
||||||
* @brief Define the type of the body
|
|
||||||
*/
|
|
||||||
enum BodyType {
|
|
||||||
STATIC, //!< A static body has infinite mass, zero velocity but the position can be changed manually. A static body does not collide with other static or kinematic bodies.
|
|
||||||
KINEMATIC, //!< A kinematic body has infinite mass, the velocity can be changed manually and its position is computed by the physics engine. A kinematic body does not collide with other static or kinematic bodies.
|
|
||||||
DYNAMIC //!< A dynamic body has non-zero mass, non-zero velocity determined by forces and its position is determined by the physics engine. A dynamic body can collide with other dynamic, static or kinematic bodies.
|
|
||||||
};
|
|
||||||
/**
|
|
||||||
* @brief This class represents a body that is able to collide with others bodies. This class inherits from the Body class.
|
|
||||||
*/
|
|
||||||
class CollisionBody : public Body {
|
|
||||||
protected :
|
|
||||||
BodyType this.type; //!< Type of body (static, kinematic or dynamic)
|
|
||||||
etk::Transform3D this.transform; //!< Position and orientation of the body
|
|
||||||
ProxyShape* this.proxyCollisionShapes; //!< First element of the linked list of proxy collision shapes of this body
|
|
||||||
int this.numberCollisionShapes; //!< Number of collision shapes
|
|
||||||
ContactManifoldListElement* this.contactManifoldsList; //!< First element of the linked list of contact manifolds involving this body
|
|
||||||
CollisionWorld this.world; //!< Reference to the world the body belongs to
|
|
||||||
/// Private copy-ructor
|
|
||||||
CollisionBody( CollisionBody body) = delete;
|
|
||||||
/// Private assignment operator
|
|
||||||
CollisionBody operator=( CollisionBody body) = delete;
|
|
||||||
/**
|
|
||||||
* @brief Reset the contact manifold lists
|
|
||||||
*/
|
|
||||||
void resetContactManifoldsList();
|
|
||||||
/**
|
|
||||||
* @brief Remove all the collision shapes
|
|
||||||
*/
|
|
||||||
void removeAllCollisionShapes();
|
|
||||||
/**
|
|
||||||
* @brief Update the broad-phase state for this body (because it has moved for instance)
|
|
||||||
*/
|
|
||||||
virtual void updateBroadPhaseState() ;
|
|
||||||
/**
|
|
||||||
* @brief Update the broad-phase state of a proxy collision shape of the body
|
|
||||||
*/
|
|
||||||
void updateProxyShapeInBroadPhase(ProxyShape* proxyShape, boolean forceReinsert = false) ;
|
|
||||||
/**
|
|
||||||
* @brief Ask the broad-phase to test again the collision shapes of the body for collision (as if the body has moved).
|
|
||||||
*/
|
|
||||||
void askForBroadPhaseCollisionCheck() ;
|
|
||||||
/**
|
|
||||||
* @brief Reset the this.isAlreadyInIsland variable of the body and contact manifolds.
|
|
||||||
* This method also returns the number of contact manifolds of the body.
|
|
||||||
*/
|
|
||||||
int resetIsAlreadyInIslandAndCountManifolds();
|
|
||||||
public :
|
|
||||||
/**
|
|
||||||
* @brief Constructor
|
|
||||||
* @param[in] transform The transform of the body
|
|
||||||
* @param[in] world The physics world where the body is created
|
|
||||||
* @param[in] id ID of the body
|
|
||||||
*/
|
|
||||||
CollisionBody( etk::Transform3D transform, CollisionWorld world, long id);
|
|
||||||
/**
|
|
||||||
* @brief Destructor
|
|
||||||
*/
|
|
||||||
virtual ~CollisionBody();
|
|
||||||
/**
|
|
||||||
* @brief Return the type of the body
|
|
||||||
* @return the type of the body (STATIC, KINEMATIC, DYNAMIC)
|
|
||||||
*/
|
|
||||||
BodyType getType() {
|
|
||||||
return this.type;
|
|
||||||
}
|
|
||||||
/**
|
|
||||||
* @brief Set the type of the body
|
|
||||||
* @param[in] type The type of the body (STATIC, KINEMATIC, DYNAMIC)
|
|
||||||
*/
|
|
||||||
virtual void setType(BodyType type);
|
|
||||||
/**
|
|
||||||
* @brief Set whether or not the body is active
|
|
||||||
* @param[in] isActive True if you want to activate the body
|
|
||||||
*/
|
|
||||||
virtual void setIsActive(boolean isActive);
|
|
||||||
/**
|
|
||||||
* @brief Return the current position and orientation
|
|
||||||
* @return The current transformation of the body that transforms the local-space of the body into world-space
|
|
||||||
*/
|
|
||||||
etk::Transform3D getTransform() {
|
|
||||||
return this.transform;
|
|
||||||
}
|
|
||||||
/**
|
|
||||||
* @brief Set the current position and orientation
|
|
||||||
* @param transform The transformation of the body that transforms the local-space of the body into world-space
|
|
||||||
*/
|
|
||||||
virtual void setTransform( etk::Transform3D transform);
|
|
||||||
/**
|
|
||||||
* @brief Add a collision shape to the body. Note that you can share a collision shape between several bodies using the same collision shape instance to
|
|
||||||
* when you add the shape to the different bodies. Do not forget to delete the collision shape you have created at the end of your program.
|
|
||||||
*
|
|
||||||
* This method will return a pointer to a new proxy shape. A proxy shape is an object that links a collision shape and a given body. You can use the
|
|
||||||
* returned proxy shape to get and set information about the corresponding collision shape for that body.
|
|
||||||
* @param[in] collisionShape A pointer to the collision shape you want to add to the body
|
|
||||||
* @param[in] transform The transformation of the collision shape that transforms the local-space of the collision shape into the local-space of the body
|
|
||||||
* @return A pointer to the proxy shape that has been created to link the body to the new collision shape you have added.
|
|
||||||
*/
|
|
||||||
ProxyShape* addCollisionShape(CollisionShape* collisionShape, etk::Transform3D transform);
|
|
||||||
/**
|
|
||||||
* @brief Remove a collision shape from the body
|
|
||||||
* To remove a collision shape, you need to specify the pointer to the proxy shape that has been returned when you have added the collision shape to the body
|
|
||||||
* @param[in] proxyShape The pointer of the proxy shape you want to remove
|
|
||||||
*/
|
|
||||||
virtual void removeCollisionShape( ProxyShape* proxyShape);
|
|
||||||
/**
|
|
||||||
* @brief Get the first element of the linked list of contact manifolds involving this body
|
|
||||||
* @return A pointer to the first element of the linked-list with the contact manifolds of this body
|
|
||||||
*/
|
|
||||||
ContactManifoldListElement* getContactManifoldsList() {
|
|
||||||
return this.contactManifoldsList;
|
|
||||||
}
|
|
||||||
/**
|
|
||||||
* @brief Return true if a point is inside the collision body
|
|
||||||
* This method returns true if a point is inside any collision shape of the body
|
|
||||||
* @param[in] worldPoint The point to test (in world-space coordinates)
|
|
||||||
* @return True if the point is inside the body
|
|
||||||
*/
|
|
||||||
boolean testPointInside( vec3 worldPoint) ;
|
|
||||||
/**
|
|
||||||
* @brief Raycast method with feedback information
|
|
||||||
* The method returns the closest hit among all the collision shapes of the body
|
|
||||||
* @param[in] ray The ray used to raycast agains the body
|
|
||||||
* @param[out] raycastInfo Structure that contains the result of the raycasting (valid only if the method returned true)
|
|
||||||
* @return True if the ray hit the body and false otherwise
|
|
||||||
*/
|
|
||||||
boolean raycast( Ray ray, RaycastInfo raycastInfo);
|
|
||||||
/**
|
|
||||||
* @brief Compute and return the AABB of the body by merging all proxy shapes AABBs
|
|
||||||
* @return The axis-aligned bounding box (AABB) of the body in world-space coordinates
|
|
||||||
*/
|
|
||||||
AABB getAABB() ;
|
|
||||||
/**
|
|
||||||
* @brief Get the linked list of proxy shapes of that body
|
|
||||||
* @return The pointer of the first proxy shape of the linked-list of all the
|
|
||||||
* proxy shapes of the body
|
|
||||||
*/
|
|
||||||
ProxyShape* getProxyShapesList() {
|
|
||||||
return this.proxyCollisionShapes;
|
|
||||||
}
|
|
||||||
/**
|
|
||||||
* @brief Get the linked list of proxy shapes of that body
|
|
||||||
* @return The pointer of the first proxy shape of the linked-list of all the proxy shapes of the body
|
|
||||||
*/
|
|
||||||
ProxyShape* getProxyShapesList() {
|
|
||||||
return this.proxyCollisionShapes;
|
|
||||||
}
|
|
||||||
/**
|
|
||||||
* @brief Get the world-space coordinates of a point given the local-space coordinates of the body
|
|
||||||
* @param[in] localPoint A point in the local-space coordinates of the body
|
|
||||||
* @return The point in world-space coordinates
|
|
||||||
*/
|
|
||||||
vec3 getWorldPoint( vec3 localPoint) {
|
|
||||||
return this.transform * localPoint;
|
|
||||||
}
|
|
||||||
/**
|
|
||||||
* @brief Get the world-space vector of a vector given in local-space coordinates of the body
|
|
||||||
* @param[in] localVector A vector in the local-space coordinates of the body
|
|
||||||
* @return The vector in world-space coordinates
|
|
||||||
*/
|
|
||||||
vec3 getWorldVector( vec3 localVector) {
|
|
||||||
return this.transform.getOrientation() * localVector;
|
|
||||||
}
|
|
||||||
/**
|
|
||||||
* @brief Get the body local-space coordinates of a point given in the world-space coordinates
|
|
||||||
* @param[in] worldPoint A point in world-space coordinates
|
|
||||||
* @return The point in the local-space coordinates of the body
|
|
||||||
*/
|
|
||||||
vec3 getLocalPoint( vec3 worldPoint) {
|
|
||||||
return this.transform.getInverse() * worldPoint;
|
|
||||||
}
|
|
||||||
/**
|
|
||||||
* @brief Get the body local-space coordinates of a vector given in the world-space coordinates
|
|
||||||
* @param[in] worldVector A vector in world-space coordinates
|
|
||||||
* @return The vector in the local-space coordinates of the body
|
|
||||||
*/
|
|
||||||
vec3 getLocalVector( vec3 worldVector) {
|
|
||||||
return this.transform.getOrientation().getInverse() * worldVector;
|
|
||||||
}
|
|
||||||
friend class CollisionWorld;
|
|
||||||
friend class DynamicsWorld;
|
|
||||||
friend class CollisionDetection;
|
|
||||||
friend class BroadPhaseAlgorithm;
|
|
||||||
friend class ConvexMeshShape;
|
|
||||||
friend class ProxyShape;
|
|
||||||
};
|
|
||||||
}
|
|
||||||
|
|
345
src/org/atriaSoft/ephysics/body/CollisionBody.java
Normal file
345
src/org/atriaSoft/ephysics/body/CollisionBody.java
Normal file
@ -0,0 +1,345 @@
|
|||||||
|
package org.atriaSoft.ephysics.body;
|
||||||
|
|
||||||
|
import org.atriaSoft.etk.math.Transform3D;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief This class represents a body that is able to collide with others bodies. This class inherits from the Body class.
|
||||||
|
*/
|
||||||
|
class CollisionBody extends Body {
|
||||||
|
protected BodyType type; //!< Type of body (static, kinematic or dynamic)
|
||||||
|
protected Transform3D transform; //!< Position and orientation of the body
|
||||||
|
protected ProxyShape proxyCollisionShapes; //!< First element of the linked list of proxy collision shapes of this body
|
||||||
|
protected int numberCollisionShapes; //!< Number of collision shapes
|
||||||
|
protected ContactManifoldListElement* contactManifoldsList; //!< First element of the linked list of contact manifolds involving this body
|
||||||
|
protected CollisionWorld world; //!< Reference to the world the body belongs to
|
||||||
|
/**
|
||||||
|
* @brief Reset the contact manifold lists
|
||||||
|
*/
|
||||||
|
protected void resetContactManifoldsList() {
|
||||||
|
// Delete the linked list of contact manifolds of that body
|
||||||
|
ContactManifoldListElement* currentElement = this.contactManifoldsList;
|
||||||
|
while (currentElement != null) {
|
||||||
|
ContactManifoldListElement* nextElement = currentElement.next;
|
||||||
|
// Delete the current element
|
||||||
|
ETKDELETE(ContactManifoldListElement, currentElement);
|
||||||
|
currentElement = nextElement;
|
||||||
|
}
|
||||||
|
this.contactManifoldsList = null;
|
||||||
|
}
|
||||||
|
/**
|
||||||
|
* @brief Remove all the collision shapes
|
||||||
|
*/
|
||||||
|
protected void removeAllCollisionShapes() {
|
||||||
|
ProxyShape* current = this.proxyCollisionShapes;
|
||||||
|
// Look for the proxy shape that contains the collision shape in parameter
|
||||||
|
while(current != null) {
|
||||||
|
// Remove the proxy collision shape
|
||||||
|
ProxyShape* nextElement = current.this.next;
|
||||||
|
if (this.isActive) {
|
||||||
|
this.world.collisionDetection.removeProxyCollisionShape(current);
|
||||||
|
}
|
||||||
|
ETKDELETE(ProxyShape, current);
|
||||||
|
// Get the next element in the list
|
||||||
|
current = nextElement;
|
||||||
|
}
|
||||||
|
this.proxyCollisionShapes = null;
|
||||||
|
}
|
||||||
|
/**
|
||||||
|
* @brief Update the broad-phase state for this body (because it has moved for instance)
|
||||||
|
*/
|
||||||
|
protected void updateBroadPhaseState() {
|
||||||
|
// For all the proxy collision shapes of the body
|
||||||
|
for (ProxyShape* shape = this.proxyCollisionShapes; shape != null; shape = shape.this.next) {
|
||||||
|
// Update the proxy
|
||||||
|
updateProxyShapeInBroadPhase(shape);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
/**
|
||||||
|
* @brief Update the broad-phase state of a proxy collision shape of the body
|
||||||
|
*/
|
||||||
|
protected void updateProxyShapeInBroadPhase(ProxyShape* proxyShape, boolean forceReinsert = false) {
|
||||||
|
AABB aabb;
|
||||||
|
proxyShape.getCollisionShape().computeAABB(aabb, this.transform * proxyShape.getLocalToBodyTransform());
|
||||||
|
this.world.collisionDetection.updateProxyCollisionShape(proxyShape, aabb, Vector3f(0, 0, 0), forceReinsert);
|
||||||
|
}
|
||||||
|
/**
|
||||||
|
* @brief Ask the broad-phase to test again the collision shapes of the body for collision (as if the body has moved).
|
||||||
|
*/
|
||||||
|
protected void askForBroadPhaseCollisionCheck() {
|
||||||
|
for (ProxyShape* shape = this.proxyCollisionShapes; shape != null; shape = shape.this.next) {
|
||||||
|
this.world.collisionDetection.askForBroadPhaseCollisionCheck(shape);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
/**
|
||||||
|
* @brief Reset the this.isAlreadyInIsland variable of the body and contact manifolds.
|
||||||
|
* This method also returns the number of contact manifolds of the body.
|
||||||
|
*/
|
||||||
|
protected int resetIsAlreadyInIslandAndCountManifolds() {
|
||||||
|
this.isAlreadyInIsland = false;
|
||||||
|
int nbManifolds = 0;
|
||||||
|
// Reset the this.isAlreadyInIsland variable of the contact manifolds for this body
|
||||||
|
ContactManifoldListElement* currentElement = this.contactManifoldsList;
|
||||||
|
while (currentElement != null) {
|
||||||
|
currentElement.contactManifold.this.isAlreadyInIsland = false;
|
||||||
|
currentElement = currentElement.next;
|
||||||
|
nbManifolds++;
|
||||||
|
}
|
||||||
|
return nbManifolds;
|
||||||
|
}
|
||||||
|
/**
|
||||||
|
* @brief Constructor
|
||||||
|
* @param[in] transform The transform of the body
|
||||||
|
* @param[in] world The physics world where the body is created
|
||||||
|
* @param[in] id ID of the body
|
||||||
|
*/
|
||||||
|
public CollisionBody( Transform3D transform, CollisionWorld world, long id) {
|
||||||
|
super(id);
|
||||||
|
this.type = DYNAMIC;
|
||||||
|
this.transform = transform;
|
||||||
|
this.proxyCollisionShapes = null;
|
||||||
|
this.numberCollisionShapes = 0;
|
||||||
|
this.contactManifoldsList = null;
|
||||||
|
this.world(world);
|
||||||
|
|
||||||
|
Log.debug(" set transform: " + transform);
|
||||||
|
if (isnan(transform.getPosition().x()) == true) { // check NAN
|
||||||
|
Log.critical(" set transform: " + transform);
|
||||||
|
}
|
||||||
|
if (isinf(transform.getOrientation().z()) == true) {
|
||||||
|
Log.critical(" set transform: " + transform);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
/**
|
||||||
|
* @brief Return the type of the body
|
||||||
|
* @return the type of the body (STATIC, KINEMATIC, DYNAMIC)
|
||||||
|
*/
|
||||||
|
public BodyType getType() {
|
||||||
|
return this.type;
|
||||||
|
}
|
||||||
|
/**
|
||||||
|
* @brief Set the type of the body
|
||||||
|
* @param[in] type The type of the body (STATIC, KINEMATIC, DYNAMIC)
|
||||||
|
*/
|
||||||
|
public void setType(BodyType type) {
|
||||||
|
this.type = type;
|
||||||
|
if (this.type == STATIC) {
|
||||||
|
// Update the broad-phase state of the body
|
||||||
|
updateBroadPhaseState();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
/**
|
||||||
|
* @brief Set whether or not the body is active
|
||||||
|
* @param[in] isActive True if you want to activate the body
|
||||||
|
*/
|
||||||
|
public void setIsActive(boolean isActive) {
|
||||||
|
// If the state does not change
|
||||||
|
if (this.isActive == isActive) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
Body::setIsActive(isActive);
|
||||||
|
// If we have to activate the body
|
||||||
|
if (isActive == true) {
|
||||||
|
for (ProxyShape* shape = this.proxyCollisionShapes; shape != null; shape = shape.this.next) {
|
||||||
|
AABB aabb;
|
||||||
|
shape.getCollisionShape().computeAABB(aabb, this.transform * shape.this.localToBodyTransform);
|
||||||
|
this.world.collisionDetection.addProxyCollisionShape(shape, aabb);
|
||||||
|
}
|
||||||
|
} else {
|
||||||
|
for (ProxyShape* shape = this.proxyCollisionShapes; shape != null; shape = shape.this.next) {
|
||||||
|
this.world.collisionDetection.removeProxyCollisionShape(shape);
|
||||||
|
}
|
||||||
|
resetContactManifoldsList();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
/**
|
||||||
|
* @brief Return the current position and orientation
|
||||||
|
* @return The current transformation of the body that transforms the local-space of the body into world-space
|
||||||
|
*/
|
||||||
|
public Transform3D getTransform() {
|
||||||
|
return this.transform;
|
||||||
|
}
|
||||||
|
/**
|
||||||
|
* @brief Set the current position and orientation
|
||||||
|
* @param transform The transformation of the body that transforms the local-space of the body into world-space
|
||||||
|
*/
|
||||||
|
public void setTransform( Transform3D transform) {
|
||||||
|
Log.debug(" set transform: " + this.transform + " ==> " + transform);
|
||||||
|
if (isnan(transform.getPosition().x()) == true) { // check NAN
|
||||||
|
Log.critical(" set transform: " + this.transform + " ==> " + transform);
|
||||||
|
}
|
||||||
|
if (isinf(transform.getOrientation().z()) == true) {
|
||||||
|
Log.critical(" set transform: " + this.transform + " ==> " + transform);
|
||||||
|
}
|
||||||
|
this.transform = transform;
|
||||||
|
updateBroadPhaseState();
|
||||||
|
}
|
||||||
|
/**
|
||||||
|
* @brief Add a collision shape to the body. Note that you can share a collision shape between several bodies using the same collision shape instance to
|
||||||
|
* when you add the shape to the different bodies. Do not forget to delete the collision shape you have created at the end of your program.
|
||||||
|
*
|
||||||
|
* This method will return a pointer to a new proxy shape. A proxy shape is an object that links a collision shape and a given body. You can use the
|
||||||
|
* returned proxy shape to get and set information about the corresponding collision shape for that body.
|
||||||
|
* @param[in] collisionShape A pointer to the collision shape you want to add to the body
|
||||||
|
* @param[in] transform The transformation of the collision shape that transforms the local-space of the collision shape into the local-space of the body
|
||||||
|
* @return A pointer to the proxy shape that has been created to link the body to the new collision shape you have added.
|
||||||
|
*/
|
||||||
|
public ProxyShape addCollisionShape(CollisionShape collisionShape, Transform3D transform) {
|
||||||
|
// Create a proxy collision shape to attach the collision shape to the body
|
||||||
|
ProxyShape* proxyShape = ETKNEW(ProxyShape, this, collisionShape,transform, float(1));
|
||||||
|
// Add it to the list of proxy collision shapes of the body
|
||||||
|
if (this.proxyCollisionShapes == null) {
|
||||||
|
this.proxyCollisionShapes = proxyShape;
|
||||||
|
} else {
|
||||||
|
proxyShape.this.next = this.proxyCollisionShapes;
|
||||||
|
this.proxyCollisionShapes = proxyShape;
|
||||||
|
}
|
||||||
|
AABB aabb;
|
||||||
|
collisionShape.computeAABB(aabb, this.transform * transform);
|
||||||
|
this.world.collisionDetection.addProxyCollisionShape(proxyShape, aabb);
|
||||||
|
this.numberCollisionShapes++;
|
||||||
|
return proxyShape;
|
||||||
|
}
|
||||||
|
/**
|
||||||
|
* @brief Remove a collision shape from the body
|
||||||
|
* To remove a collision shape, you need to specify the pointer to the proxy shape that has been returned when you have added the collision shape to the body
|
||||||
|
* @param[in] proxyShape The pointer of the proxy shape you want to remove
|
||||||
|
*/
|
||||||
|
public void removeCollisionShape(ProxyShape proxyShape) {
|
||||||
|
ProxyShape* current = this.proxyCollisionShapes;
|
||||||
|
// If the the first proxy shape is the one to remove
|
||||||
|
if (current == proxyShape) {
|
||||||
|
this.proxyCollisionShapes = current.this.next;
|
||||||
|
if (this.isActive) {
|
||||||
|
this.world.collisionDetection.removeProxyCollisionShape(current);
|
||||||
|
}
|
||||||
|
ETKDELETE(ProxyShape, current);
|
||||||
|
current = null;
|
||||||
|
this.numberCollisionShapes--;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
// Look for the proxy shape that contains the collision shape in parameter
|
||||||
|
while(current.this.next != null) {
|
||||||
|
// If we have found the collision shape to remove
|
||||||
|
if (current.this.next == proxyShape) {
|
||||||
|
// Remove the proxy collision shape
|
||||||
|
ProxyShape* elementToRemove = current.this.next;
|
||||||
|
current.this.next = elementToRemove.this.next;
|
||||||
|
if (this.isActive) {
|
||||||
|
this.world.collisionDetection.removeProxyCollisionShape(elementToRemove);
|
||||||
|
}
|
||||||
|
ETKDELETE(ProxyShape, elementToRemove);
|
||||||
|
elementToRemove = null;
|
||||||
|
this.numberCollisionShapes--;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
// Get the next element in the list
|
||||||
|
current = current.this.next;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
/**
|
||||||
|
* @brief Get the first element of the linked list of contact manifolds involving this body
|
||||||
|
* @return A pointer to the first element of the linked-list with the contact manifolds of this body
|
||||||
|
*/
|
||||||
|
public ContactManifoldListElement getContactManifoldsList() {
|
||||||
|
return this.contactManifoldsList;
|
||||||
|
}
|
||||||
|
/**
|
||||||
|
* @brief Return true if a point is inside the collision body
|
||||||
|
* This method returns true if a point is inside any collision shape of the body
|
||||||
|
* @param[in] worldPoint The point to test (in world-space coordinates)
|
||||||
|
* @return True if the point is inside the body
|
||||||
|
*/
|
||||||
|
public boolean testPointInside( Vector3f worldPoint) {
|
||||||
|
for (ProxyShape* shape = this.proxyCollisionShapes; shape != null; shape = shape.this.next) {
|
||||||
|
if (shape.testPointInside(worldPoint)) return true;
|
||||||
|
}
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
/**
|
||||||
|
* @brief Raycast method with feedback information
|
||||||
|
* The method returns the closest hit among all the collision shapes of the body
|
||||||
|
* @param[in] ray The ray used to raycast agains the body
|
||||||
|
* @param[out] raycastInfo Structure that contains the result of the raycasting (valid only if the method returned true)
|
||||||
|
* @return True if the ray hit the body and false otherwise
|
||||||
|
*/
|
||||||
|
public boolean raycast( Ray ray, RaycastInfo raycastInfo) {
|
||||||
|
if (this.isActive == false) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
boolean isHit = false;
|
||||||
|
Ray rayTemp(ray);
|
||||||
|
for (ProxyShape* shape = this.proxyCollisionShapes; shape != null; shape = shape.this.next) {
|
||||||
|
// Test if the ray hits the collision shape
|
||||||
|
if (shape.raycast(rayTemp, raycastInfo)) {
|
||||||
|
rayTemp.maxFraction = raycastInfo.hitFraction;
|
||||||
|
isHit = true;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return isHit;
|
||||||
|
}
|
||||||
|
/**
|
||||||
|
* @brief Compute and return the AABB of the body by merging all proxy shapes AABBs
|
||||||
|
* @return The axis-aligned bounding box (AABB) of the body in world-space coordinates
|
||||||
|
*/
|
||||||
|
public AABB getAABB() {
|
||||||
|
AABB bodyAABB;
|
||||||
|
if (this.proxyCollisionShapes == null) {
|
||||||
|
return bodyAABB;
|
||||||
|
}
|
||||||
|
this.proxyCollisionShapes.getCollisionShape().computeAABB(bodyAABB, this.transform * this.proxyCollisionShapes.getLocalToBodyTransform());
|
||||||
|
for (ProxyShape* shape = this.proxyCollisionShapes.this.next; shape != null; shape = shape.this.next) {
|
||||||
|
AABB aabb;
|
||||||
|
shape.getCollisionShape().computeAABB(aabb, this.transform * shape.getLocalToBodyTransform());
|
||||||
|
bodyAABB.mergeWithAABB(aabb);
|
||||||
|
}
|
||||||
|
return bodyAABB;
|
||||||
|
}
|
||||||
|
/**
|
||||||
|
* @brief Get the linked list of proxy shapes of that body
|
||||||
|
* @return The pointer of the first proxy shape of the linked-list of all the
|
||||||
|
* proxy shapes of the body
|
||||||
|
*/
|
||||||
|
public ProxyShape getProxyShapesList() {
|
||||||
|
return this.proxyCollisionShapes;
|
||||||
|
}
|
||||||
|
/**
|
||||||
|
* @brief Get the linked list of proxy shapes of that body
|
||||||
|
* @return The pointer of the first proxy shape of the linked-list of all the proxy shapes of the body
|
||||||
|
*/
|
||||||
|
public ProxyShape getProxyShapesList() {
|
||||||
|
return this.proxyCollisionShapes;
|
||||||
|
}
|
||||||
|
/**
|
||||||
|
* @brief Get the world-space coordinates of a point given the local-space coordinates of the body
|
||||||
|
* @param[in] localPoint A point in the local-space coordinates of the body
|
||||||
|
* @return The point in world-space coordinates
|
||||||
|
*/
|
||||||
|
public Vector3f getWorldPoint( Vector3f localPoint) {
|
||||||
|
return this.transform * localPoint;
|
||||||
|
}
|
||||||
|
/**
|
||||||
|
* @brief Get the world-space vector of a vector given in local-space coordinates of the body
|
||||||
|
* @param[in] localVector A vector in the local-space coordinates of the body
|
||||||
|
* @return The vector in world-space coordinates
|
||||||
|
*/
|
||||||
|
public Vector3f getWorldVector( Vector3f localVector) {
|
||||||
|
return this.transform.getOrientation() * localVector;
|
||||||
|
}
|
||||||
|
/**
|
||||||
|
* @brief Get the body local-space coordinates of a point given in the world-space coordinates
|
||||||
|
* @param[in] worldPoint A point in world-space coordinates
|
||||||
|
* @return The point in the local-space coordinates of the body
|
||||||
|
*/
|
||||||
|
public Vector3f getLocalPoint( Vector3f worldPoint) {
|
||||||
|
return this.transform.getInverse() * worldPoint;
|
||||||
|
}
|
||||||
|
/**
|
||||||
|
* @brief Get the body local-space coordinates of a vector given in the world-space coordinates
|
||||||
|
* @param[in] worldVector A vector in world-space coordinates
|
||||||
|
* @return The vector in the local-space coordinates of the body
|
||||||
|
*/
|
||||||
|
public Vector3f getLocalVector( Vector3f worldVector) {
|
||||||
|
return this.transform.getOrientation().getInverse() * worldVector;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
@ -1,316 +0,0 @@
|
|||||||
/** @file
|
|
||||||
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
|
|
||||||
* @author Daniel CHAPPUIS
|
|
||||||
* @author Edouard DUPIN
|
|
||||||
* @copyright 2010-2016, Daniel Chappuis
|
|
||||||
* @copyright 2017, Edouard DUPIN
|
|
||||||
* @license MPL v2.0 (see license file)
|
|
||||||
*/
|
|
||||||
#include <ephysics/body/RigidBody.hpp>
|
|
||||||
#include <ephysics/raint/Joint.hpp>
|
|
||||||
#include <ephysics/collision/shapes/CollisionShape.hpp>
|
|
||||||
#include <ephysics/engine/DynamicsWorld.hpp>
|
|
||||||
#include <ephysics/debug.hpp>
|
|
||||||
|
|
||||||
using namespace ephysics;
|
|
||||||
|
|
||||||
|
|
||||||
RigidBody::RigidBody( etk::Transform3D transform, CollisionWorld world, long id):
|
|
||||||
CollisionBody(transform, world, id),
|
|
||||||
this.initMass(1.0f),
|
|
||||||
this.centerOfMassLocal(0, 0, 0),
|
|
||||||
this.centerOfMassWorld(transform.getPosition()),
|
|
||||||
this.isGravityEnabled(true),
|
|
||||||
this.linearDamping(0.0f),
|
|
||||||
this.angularDamping(float(0.0)),
|
|
||||||
this.jointsList(null) {
|
|
||||||
// Compute the inverse mass
|
|
||||||
this.massInverse = 1.0f / this.initMass;
|
|
||||||
}
|
|
||||||
|
|
||||||
RigidBody::~RigidBody() {
|
|
||||||
assert(this.jointsList == null);
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
void RigidBody::setType(BodyType type) {
|
|
||||||
if (this.type == type) {
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
CollisionBody::setType(type);
|
|
||||||
recomputeMassInformation();
|
|
||||||
if (this.type == STATIC) {
|
|
||||||
// Reset the velocity to zero
|
|
||||||
this.linearVelocity.setZero();
|
|
||||||
this.angularVelocity.setZero();
|
|
||||||
}
|
|
||||||
if ( this.type == STATIC
|
|
||||||
|| this.type == KINEMATIC) {
|
|
||||||
// Reset the inverse mass and inverse inertia tensor to zero
|
|
||||||
this.massInverse = 0.0f;
|
|
||||||
this.inertiaTensorLocal.setZero();
|
|
||||||
this.inertiaTensorLocalInverse.setZero();
|
|
||||||
} else {
|
|
||||||
this.massInverse = 1.0f / this.initMass;
|
|
||||||
this.inertiaTensorLocalInverse = this.inertiaTensorLocal.getInverse();
|
|
||||||
}
|
|
||||||
setIsSleeping(false);
|
|
||||||
resetContactManifoldsList();
|
|
||||||
// Ask the broad-phase to test again the collision shapes of the body for collision detection (as if the body has moved)
|
|
||||||
askForBroadPhaseCollisionCheck();
|
|
||||||
this.externalForce.setZero();
|
|
||||||
this.externalTorque.setZero();
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
void RigidBody::setInertiaTensorLocal( etk::Matrix3x3 inertiaTensorLocal) {
|
|
||||||
if (this.type != DYNAMIC) {
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
this.inertiaTensorLocal = inertiaTensorLocal;
|
|
||||||
this.inertiaTensorLocalInverse = this.inertiaTensorLocal.getInverse();
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
void RigidBody::setCenterOfMassLocal( vec3 centerOfMassLocal) {
|
|
||||||
if (this.type != DYNAMIC) {
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
vec3 oldCenterOfMass = this.centerOfMassWorld;
|
|
||||||
this.centerOfMassLocal = centerOfMassLocal;
|
|
||||||
this.centerOfMassWorld = this.transform * this.centerOfMassLocal;
|
|
||||||
this.linearVelocity += this.angularVelocity.cross(this.centerOfMassWorld - oldCenterOfMass);
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
void RigidBody::setMass(float mass) {
|
|
||||||
if (this.type != DYNAMIC) {
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
this.initMass = mass;
|
|
||||||
if (this.initMass > 0.0f) {
|
|
||||||
this.massInverse = 1.0f / this.initMass;
|
|
||||||
} else {
|
|
||||||
this.initMass = 1.0f;
|
|
||||||
this.massInverse = 1.0f;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void RigidBody::removeJointFrothis.jointsList( Joint* joint) {
|
|
||||||
assert(joint != null);
|
|
||||||
assert(this.jointsList != null);
|
|
||||||
// Remove the joint from the linked list of the joints of the first body
|
|
||||||
if (this.jointsList->joint == joint) { // If the first element is the one to remove
|
|
||||||
JointListElement* elementToRemove = this.jointsList;
|
|
||||||
this.jointsList = elementToRemove->next;
|
|
||||||
ETKDELETE(JointListElement, elementToRemove);
|
|
||||||
elementToRemove = null;
|
|
||||||
}
|
|
||||||
else { // If the element to remove is not the first one in the list
|
|
||||||
JointListElement* currentElement = this.jointsList;
|
|
||||||
while (currentElement->next != null) {
|
|
||||||
if (currentElement->next->joint == joint) {
|
|
||||||
JointListElement* elementToRemove = currentElement->next;
|
|
||||||
currentElement->next = elementToRemove->next;
|
|
||||||
ETKDELETE(JointListElement, elementToRemove);
|
|
||||||
elementToRemove = null;
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
currentElement = currentElement->next;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
ProxyShape* RigidBody::addCollisionShape(CollisionShape* collisionShape,
|
|
||||||
etk::Transform3D transform,
|
|
||||||
float mass) {
|
|
||||||
assert(mass > 0.0f);
|
|
||||||
// Create a new proxy collision shape to attach the collision shape to the body
|
|
||||||
ProxyShape* proxyShape = ETKNEW(ProxyShape, this, collisionShape, transform, mass);
|
|
||||||
// Add it to the list of proxy collision shapes of the body
|
|
||||||
if (this.proxyCollisionShapes == null) {
|
|
||||||
this.proxyCollisionShapes = proxyShape;
|
|
||||||
} else {
|
|
||||||
proxyShape->this.next = this.proxyCollisionShapes;
|
|
||||||
this.proxyCollisionShapes = proxyShape;
|
|
||||||
}
|
|
||||||
// Compute the world-space AABB of the new collision shape
|
|
||||||
AABB aabb;
|
|
||||||
collisionShape->computeAABB(aabb, this.transform * transform);
|
|
||||||
// Notify the collision detection about this new collision shape
|
|
||||||
this.world.this.collisionDetection.addProxyCollisionShape(proxyShape, aabb);
|
|
||||||
this.numberCollisionShapes++;
|
|
||||||
recomputeMassInformation();
|
|
||||||
return proxyShape;
|
|
||||||
}
|
|
||||||
|
|
||||||
void RigidBody::removeCollisionShape( ProxyShape* proxyShape) {
|
|
||||||
CollisionBody::removeCollisionShape(proxyShape);
|
|
||||||
recomputeMassInformation();
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
void RigidBody::setLinearVelocity( vec3 linearVelocity) {
|
|
||||||
if (this.type == STATIC) {
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
this.linearVelocity = linearVelocity;
|
|
||||||
if (this.linearVelocity.length2() > 0.0f) {
|
|
||||||
setIsSleeping(false);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
void RigidBody::setAngularVelocity( vec3 angularVelocity) {
|
|
||||||
if (this.type == STATIC) {
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
this.angularVelocity = angularVelocity;
|
|
||||||
if (this.angularVelocity.length2() > 0.0f) {
|
|
||||||
setIsSleeping(false);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void RigidBody::setIsSleeping(boolean isSleeping) {
|
|
||||||
if (isSleeping) {
|
|
||||||
this.linearVelocity.setZero();
|
|
||||||
this.angularVelocity.setZero();
|
|
||||||
this.externalForce.setZero();
|
|
||||||
this.externalTorque.setZero();
|
|
||||||
}
|
|
||||||
Body::setIsSleeping(isSleeping);
|
|
||||||
}
|
|
||||||
|
|
||||||
void RigidBody::updateTransformWithCenterOfMass() {
|
|
||||||
// Translate the body according to the translation of the center of mass position
|
|
||||||
this.transform.setPosition(this.centerOfMassWorld - this.transform.getOrientation() * this.centerOfMassLocal);
|
|
||||||
if (isnan(this.transform.getPosition().x()) == true) {
|
|
||||||
Log.critical("updateTransformWithCenterOfMass: " << this.transform);
|
|
||||||
}
|
|
||||||
if (isinf(this.transform.getOrientation().z()) == true) {
|
|
||||||
Log.critical(" set transform: " << this.transform);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void RigidBody::setTransform( etk::Transform3D transform) {
|
|
||||||
Log.debug(" set transform: " << this.transform << " ==> " << transform);
|
|
||||||
if (isnan(transform.getPosition().x()) == true) {
|
|
||||||
Log.critical(" set transform: " << this.transform << " ==> " << transform);
|
|
||||||
}
|
|
||||||
if (isinf(transform.getOrientation().z()) == true) {
|
|
||||||
Log.critical(" set transform: " << this.transform << " ==> " << transform);
|
|
||||||
}
|
|
||||||
this.transform = transform;
|
|
||||||
vec3 oldCenterOfMass = this.centerOfMassWorld;
|
|
||||||
// Compute the new center of mass in world-space coordinates
|
|
||||||
this.centerOfMassWorld = this.transform * this.centerOfMassLocal;
|
|
||||||
// Update the linear velocity of the center of mass
|
|
||||||
this.linearVelocity += this.angularVelocity.cross(this.centerOfMassWorld - oldCenterOfMass);
|
|
||||||
updateBroadPhaseState();
|
|
||||||
}
|
|
||||||
|
|
||||||
void RigidBody::recomputeMassInformation() {
|
|
||||||
this.initMass = 0.0f;
|
|
||||||
this.massInverse = 0.0f;
|
|
||||||
this.inertiaTensorLocal.setZero();
|
|
||||||
this.inertiaTensorLocalInverse.setZero();
|
|
||||||
this.centerOfMassLocal.setZero();
|
|
||||||
// If it is STATIC or KINEMATIC body
|
|
||||||
if (this.type == STATIC || this.type == KINEMATIC) {
|
|
||||||
this.centerOfMassWorld = this.transform.getPosition();
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
assert(this.type == DYNAMIC);
|
|
||||||
// Compute the total mass of the body
|
|
||||||
for (ProxyShape* shape = this.proxyCollisionShapes; shape != NULL; shape = shape->this.next) {
|
|
||||||
this.initMass += shape->getMass();
|
|
||||||
this.centerOfMassLocal += shape->getLocalToBodyTransform().getPosition() * shape->getMass();
|
|
||||||
}
|
|
||||||
if (this.initMass > 0.0f) {
|
|
||||||
this.massInverse = 1.0f / this.initMass;
|
|
||||||
} else {
|
|
||||||
this.initMass = 1.0f;
|
|
||||||
this.massInverse = 1.0f;
|
|
||||||
}
|
|
||||||
// Compute the center of mass
|
|
||||||
vec3 oldCenterOfMass = this.centerOfMassWorld;
|
|
||||||
this.centerOfMassLocal *= this.massInverse;
|
|
||||||
this.centerOfMassWorld = this.transform * this.centerOfMassLocal;
|
|
||||||
// Compute the total mass and inertia tensor using all the collision shapes
|
|
||||||
for (ProxyShape* shape = this.proxyCollisionShapes; shape != null; shape = shape->this.next) {
|
|
||||||
// Get the inertia tensor of the collision shape in its local-space
|
|
||||||
etk::Matrix3x3 inertiaTensor;
|
|
||||||
shape->getCollisionShape()->computeLocalInertiaTensor(inertiaTensor, shape->getMass());
|
|
||||||
// Convert the collision shape inertia tensor into the local-space of the body
|
|
||||||
etk::Transform3D shapeTransform = shape->getLocalToBodyTransform();
|
|
||||||
etk::Matrix3x3 rotationMatrix = shapeTransform.getOrientation().getMatrix();
|
|
||||||
inertiaTensor = rotationMatrix * inertiaTensor * rotationMatrix.getTranspose();
|
|
||||||
// Use the parallel axis theorem to convert the inertia tensor w.r.t the collision shape
|
|
||||||
// center into a inertia tensor w.r.t to the body origin.
|
|
||||||
vec3 offset = shapeTransform.getPosition() - this.centerOfMassLocal;
|
|
||||||
float offsetSquare = offset.length2();
|
|
||||||
vec3 off1 = offset * (-offset.x());
|
|
||||||
vec3 off2 = offset * (-offset.y());
|
|
||||||
vec3 off3 = offset * (-offset.z());
|
|
||||||
etk::Matrix3x3 offsetMatrix(off1.x()+offsetSquare, off1.y(), off1.z(),
|
|
||||||
off2.x(), off2.y()+offsetSquare, off2.z(),
|
|
||||||
off3.x(), off3.y(), off3.z()+offsetSquare);
|
|
||||||
offsetMatrix *= shape->getMass();
|
|
||||||
this.inertiaTensorLocal += inertiaTensor + offsetMatrix;
|
|
||||||
}
|
|
||||||
// Compute the local inverse inertia tensor
|
|
||||||
this.inertiaTensorLocalInverse = this.inertiaTensorLocal.getInverse();
|
|
||||||
// Update the linear velocity of the center of mass
|
|
||||||
this.linearVelocity += this.angularVelocity.cross(this.centerOfMassWorld - oldCenterOfMass);
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
void RigidBody::updateBroadPhaseState() {
|
|
||||||
PROFILE("RigidBody::updateBroadPhaseState()");
|
|
||||||
DynamicsWorld world = staticcast<DynamicsWorld>(this.world);
|
|
||||||
vec3 displacement = world.this.timeStep * this.linearVelocity;
|
|
||||||
// For all the proxy collision shapes of the body
|
|
||||||
for (ProxyShape* shape = this.proxyCollisionShapes; shape != null; shape = shape->this.next) {
|
|
||||||
// Recompute the world-space AABB of the collision shape
|
|
||||||
AABB aabb;
|
|
||||||
Log.verbose(" : " << aabb.getMin() << " " << aabb.getMax());
|
|
||||||
Log.verbose(" this.transform: " << this.transform);
|
|
||||||
shape->getCollisionShape()->computeAABB(aabb, this.transform *shape->getLocalToBodyTransform());
|
|
||||||
Log.verbose(" : " << aabb.getMin() << " " << aabb.getMax());
|
|
||||||
// Update the broad-phase state for the proxy collision shape
|
|
||||||
this.world.this.collisionDetection.updateProxyCollisionShape(shape, aabb, displacement);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
void RigidBody::applyForceToCenterOfMass( vec3 force) {
|
|
||||||
if (this.type != DYNAMIC) {
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
if (this.isSleeping) {
|
|
||||||
setIsSleeping(false);
|
|
||||||
}
|
|
||||||
this.externalForce += force;
|
|
||||||
}
|
|
||||||
|
|
||||||
void RigidBody::applyForce( vec3 force, vec3 point) {
|
|
||||||
if (this.type != DYNAMIC) {
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
if (this.isSleeping) {
|
|
||||||
setIsSleeping(false);
|
|
||||||
}
|
|
||||||
this.externalForce += force;
|
|
||||||
this.externalTorque += (point - this.centerOfMassWorld).cross(force);
|
|
||||||
}
|
|
||||||
|
|
||||||
void RigidBody::applyTorque( vec3 torque) {
|
|
||||||
if (this.type != DYNAMIC) {
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
if (this.isSleeping) {
|
|
||||||
setIsSleeping(false);
|
|
||||||
}
|
|
||||||
this.externalTorque += torque;
|
|
||||||
}
|
|
@ -1,294 +0,0 @@
|
|||||||
/** @file
|
|
||||||
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
|
|
||||||
* @author Daniel CHAPPUIS
|
|
||||||
* @author Edouard DUPIN
|
|
||||||
* @copyright 2010-2016, Daniel Chappuis
|
|
||||||
* @copyright 2017, Edouard DUPIN
|
|
||||||
* @license MPL v2.0 (see license file)
|
|
||||||
*/
|
|
||||||
#pragma once
|
|
||||||
|
|
||||||
#include <ephysics/body/CollisionBody.hpp>
|
|
||||||
#include <ephysics/engine/Material.hpp>
|
|
||||||
#include <ephysics/mathematics/mathematics.hpp>
|
|
||||||
|
|
||||||
namespace ephysics {
|
|
||||||
|
|
||||||
// Class declarations
|
|
||||||
struct JointListElement;
|
|
||||||
class Joint;
|
|
||||||
class DynamicsWorld;
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief This class represents a rigid body of the physics
|
|
||||||
* engine. A rigid body is a non-deformable body that
|
|
||||||
* has a ant mass. This class inherits from the
|
|
||||||
* CollisionBody class.
|
|
||||||
*/
|
|
||||||
class RigidBody : public CollisionBody {
|
|
||||||
protected :
|
|
||||||
float this.initMass; //!< Intial mass of the body
|
|
||||||
vec3 this.centerOfMassLocal; //!< Center of mass of the body in local-space coordinates. The center of mass can therefore be different from the body origin
|
|
||||||
vec3 this.centerOfMassWorld; //!< Center of mass of the body in world-space coordinates
|
|
||||||
vec3 this.linearVelocity; //!< Linear velocity of the body
|
|
||||||
vec3 this.angularVelocity; //!< Angular velocity of the body
|
|
||||||
vec3 this.externalForce; //!< Current external force on the body
|
|
||||||
vec3 this.externalTorque; //!< Current external torque on the body
|
|
||||||
etk::Matrix3x3 this.inertiaTensorLocal; //!< Local inertia tensor of the body (in local-space) with respect to the center of mass of the body
|
|
||||||
etk::Matrix3x3 this.inertiaTensorLocalInverse; //!< Inverse of the inertia tensor of the body
|
|
||||||
float this.massInverse; //!< Inverse of the mass of the body
|
|
||||||
boolean this.isGravityEnabled; //!< True if the gravity needs to be applied to this rigid body
|
|
||||||
Material this.material; //!< Material properties of the rigid body
|
|
||||||
float this.linearDamping; //!< Linear velocity damping factor
|
|
||||||
float this.angularDamping; //!< Angular velocity damping factor
|
|
||||||
JointListElement* this.jointsList; //!< First element of the linked list of joints involving this body
|
|
||||||
/// Private copy-ructor
|
|
||||||
RigidBody( RigidBody body);
|
|
||||||
/// Private assignment operator
|
|
||||||
RigidBody operator=( RigidBody body);
|
|
||||||
/**
|
|
||||||
* @brief Remove a joint from the joints list
|
|
||||||
*/
|
|
||||||
void removeJointFrothis.jointsList( Joint* joint);
|
|
||||||
/**
|
|
||||||
* @brief Update the transform of the body after a change of the center of mass
|
|
||||||
*/
|
|
||||||
void updateTransformWithCenterOfMass();
|
|
||||||
void updateBroadPhaseState() override;
|
|
||||||
public :
|
|
||||||
/**
|
|
||||||
* @brief Constructor
|
|
||||||
* @param transform The transformation of the body
|
|
||||||
* @param world The world where the body has been added
|
|
||||||
* @param id The ID of the body
|
|
||||||
*/
|
|
||||||
RigidBody( etk::Transform3D transform, CollisionWorld world, long id);
|
|
||||||
/**
|
|
||||||
* @brief Virtual Destructor
|
|
||||||
*/
|
|
||||||
virtual ~RigidBody();
|
|
||||||
void setType(BodyType type) override;
|
|
||||||
/**
|
|
||||||
* @brief Set the current position and orientation
|
|
||||||
* @param[in] transform The transformation of the body that transforms the local-space of the body into world-space
|
|
||||||
*/
|
|
||||||
void setTransform( etk::Transform3D transform) override;
|
|
||||||
/**
|
|
||||||
* @brief Get the mass of the body
|
|
||||||
* @return The mass (in kilograms) of the body
|
|
||||||
*/
|
|
||||||
float getMass() {
|
|
||||||
return this.initMass;
|
|
||||||
}
|
|
||||||
/**
|
|
||||||
* @brief Get the linear velocity
|
|
||||||
* @return The linear velocity vector of the body
|
|
||||||
*/
|
|
||||||
vec3 getLinearVelocity() {
|
|
||||||
return this.linearVelocity;
|
|
||||||
}
|
|
||||||
/**
|
|
||||||
* @brief Set the linear velocity of the rigid body.
|
|
||||||
* @param[in] linearVelocity Linear velocity vector of the body
|
|
||||||
*/
|
|
||||||
void setLinearVelocity( vec3 linearVelocity);
|
|
||||||
/**
|
|
||||||
* @brief Get the angular velocity of the body
|
|
||||||
* @return The angular velocity vector of the body
|
|
||||||
*/
|
|
||||||
vec3 getAngularVelocity() {
|
|
||||||
return this.angularVelocity;
|
|
||||||
}
|
|
||||||
/**
|
|
||||||
* @brief Set the angular velocity.
|
|
||||||
* @param[in] angularVelocity The angular velocity vector of the body
|
|
||||||
*/
|
|
||||||
void setAngularVelocity( vec3 angularVelocity);
|
|
||||||
/**
|
|
||||||
* @brief Set the variable to know whether or not the body is sleeping
|
|
||||||
* @param[in] isSleeping New sleeping state of the body
|
|
||||||
*/
|
|
||||||
virtual void setIsSleeping(boolean isSleeping);
|
|
||||||
/**
|
|
||||||
* @brief Get the local inertia tensor of the body (in local-space coordinates)
|
|
||||||
* @return The 3x3 inertia tensor matrix of the body
|
|
||||||
*/
|
|
||||||
etk::Matrix3x3 getInertiaTensorLocal() {
|
|
||||||
return this.inertiaTensorLocal;
|
|
||||||
}
|
|
||||||
/**
|
|
||||||
* @brief Set the local inertia tensor of the body (in local-space coordinates)
|
|
||||||
* @param[in] inertiaTensorLocal The 3x3 inertia tensor matrix of the body in local-space coordinates
|
|
||||||
*/
|
|
||||||
void setInertiaTensorLocal( etk::Matrix3x3 inertiaTensorLocal);
|
|
||||||
/**
|
|
||||||
* @brief Set the local center of mass of the body (in local-space coordinates)
|
|
||||||
* @param[in] centerOfMassLocal The center of mass of the body in local-space coordinates
|
|
||||||
*/
|
|
||||||
void setCenterOfMassLocal( vec3 centerOfMassLocal);
|
|
||||||
/**
|
|
||||||
* @brief Set the mass of the rigid body
|
|
||||||
* @param[in] mass The mass (in kilograms) of the body
|
|
||||||
*/
|
|
||||||
void setMass(float mass);
|
|
||||||
/**
|
|
||||||
* @brief Get the inertia tensor in world coordinates.
|
|
||||||
* The inertia tensor Iw in world coordinates is computed
|
|
||||||
* with the local inertia tensor Ib in body coordinates
|
|
||||||
* by Iw = R * Ib * R^T
|
|
||||||
* where R is the rotation matrix (and R^T its transpose) of
|
|
||||||
* the current orientation quaternion of the body
|
|
||||||
* @return The 3x3 inertia tensor matrix of the body in world-space coordinates
|
|
||||||
*/
|
|
||||||
etk::Matrix3x3 getInertiaTensorWorld() {
|
|
||||||
// Compute and return the inertia tensor in world coordinates
|
|
||||||
return this.transform.getOrientation().getMatrix() * this.inertiaTensorLocal *
|
|
||||||
this.transform.getOrientation().getMatrix().getTranspose();
|
|
||||||
}
|
|
||||||
/**
|
|
||||||
* @brief Get the inverse of the inertia tensor in world coordinates.
|
|
||||||
* The inertia tensor Iw in world coordinates is computed with the
|
|
||||||
* local inverse inertia tensor Ib^-1 in body coordinates
|
|
||||||
* by Iw = R * Ib^-1 * R^T
|
|
||||||
* where R is the rotation matrix (and R^T its transpose) of the
|
|
||||||
* current orientation quaternion of the body
|
|
||||||
* @return The 3x3 inverse inertia tensor matrix of the body in world-space coordinates
|
|
||||||
*/
|
|
||||||
etk::Matrix3x3 getInertiaTensorInverseWorld() {
|
|
||||||
// TODO : DO NOT RECOMPUTE THE MATRIX MULTIPLICATION EVERY TIME. WE NEED TO STORE THE
|
|
||||||
// INVERSE WORLD TENSOR IN THE CLASS AND UPLDATE IT WHEN THE ORIENTATION OF THE BODY CHANGES
|
|
||||||
// Compute and return the inertia tensor in world coordinates
|
|
||||||
return this.transform.getOrientation().getMatrix() * this.inertiaTensorLocalInverse *
|
|
||||||
this.transform.getOrientation().getMatrix().getTranspose();
|
|
||||||
}
|
|
||||||
/**
|
|
||||||
* @brief get the need of gravity appling to this rigid body
|
|
||||||
* @return True if the gravity is applied to the body
|
|
||||||
*/
|
|
||||||
boolean isGravityEnabled() {
|
|
||||||
return this.isGravityEnabled;
|
|
||||||
}
|
|
||||||
/**
|
|
||||||
* @brief Set the variable to know if the gravity is applied to this rigid body
|
|
||||||
* @param[in] isEnabled True if you want the gravity to be applied to this body
|
|
||||||
*/
|
|
||||||
void enableGravity(boolean isEnabled) {
|
|
||||||
this.isGravityEnabled = isEnabled;
|
|
||||||
}
|
|
||||||
/**
|
|
||||||
* @brief get a reference to the material properties of the rigid body
|
|
||||||
* @return A reference to the material of the body
|
|
||||||
*/
|
|
||||||
Material getMaterial() {
|
|
||||||
return this.material;
|
|
||||||
}
|
|
||||||
/**
|
|
||||||
* @brief Set a new material for this rigid body
|
|
||||||
* @param[in] material The material you want to set to the body
|
|
||||||
*/
|
|
||||||
void setMaterial( Material material) {
|
|
||||||
this.material = material;
|
|
||||||
}
|
|
||||||
/**
|
|
||||||
* @brief Get the linear velocity damping factor
|
|
||||||
* @return The linear damping factor of this body
|
|
||||||
*/
|
|
||||||
float getLinearDamping() {
|
|
||||||
return this.linearDamping;
|
|
||||||
}
|
|
||||||
/**
|
|
||||||
* @brief Set the linear damping factor. This is the ratio of the linear velocity that the body will lose every at seconds of simulation.
|
|
||||||
* @param[in] linearDamping The linear damping factor of this body
|
|
||||||
*/
|
|
||||||
void setLinearDamping(float linearDamping) {
|
|
||||||
assert(linearDamping >= 0.0f);
|
|
||||||
this.linearDamping = linearDamping;
|
|
||||||
}
|
|
||||||
/**
|
|
||||||
* @brief Get the angular velocity damping factor
|
|
||||||
* @return The angular damping factor of this body
|
|
||||||
*/
|
|
||||||
float getAngularDamping() {
|
|
||||||
return this.angularDamping;
|
|
||||||
}
|
|
||||||
/**
|
|
||||||
* @brief Set the angular damping factor. This is the ratio of the angular velocity that the body will lose at every seconds of simulation.
|
|
||||||
* @param[in] angularDamping The angular damping factor of this body
|
|
||||||
*/
|
|
||||||
void setAngularDamping(float angularDamping) {
|
|
||||||
assert(angularDamping >= 0.0f);
|
|
||||||
this.angularDamping = angularDamping;
|
|
||||||
}
|
|
||||||
/**
|
|
||||||
* @brief Get the first element of the linked list of joints involving this body
|
|
||||||
* @return The first element of the linked-list of all the joints involving this body
|
|
||||||
*/
|
|
||||||
JointListElement* getJointsList() {
|
|
||||||
return this.jointsList;
|
|
||||||
}
|
|
||||||
/**
|
|
||||||
* @brief Get the first element of the linked list of joints involving this body
|
|
||||||
* @return The first element of the linked-list of all the joints involving this body
|
|
||||||
*/
|
|
||||||
JointListElement* getJointsList() {
|
|
||||||
return this.jointsList;
|
|
||||||
}
|
|
||||||
/**
|
|
||||||
* @brief Apply an external force to the body at its center of mass.
|
|
||||||
* If the body is sleeping, calling this method will wake it up. Note that the
|
|
||||||
* force will we added to the sum of the applied forces and that this sum will be
|
|
||||||
* reset to zero at the end of each call of the DynamicsWorld::update() method.
|
|
||||||
* You can only apply a force to a dynamic body otherwise, this method will do nothing.
|
|
||||||
* @param[in] force The external force to apply on the center of mass of the body
|
|
||||||
*/
|
|
||||||
void applyForceToCenterOfMass( vec3 force);
|
|
||||||
/**
|
|
||||||
* @brief Apply an external force to the body at a given point (in world-space coordinates).
|
|
||||||
* If the point is not at the center of mass of the body, it will also
|
|
||||||
* generate some torque and therefore, change the angular velocity of the body.
|
|
||||||
* If the body is sleeping, calling this method will wake it up. Note that the
|
|
||||||
* force will we added to the sum of the applied forces and that this sum will be
|
|
||||||
* reset to zero at the end of each call of the DynamicsWorld::update() method.
|
|
||||||
* You can only apply a force to a dynamic body otherwise, this method will do nothing.
|
|
||||||
* @param[in] force The force to apply on the body
|
|
||||||
* @param[in] point The point where the force is applied (in world-space coordinates)
|
|
||||||
*/
|
|
||||||
void applyForce( vec3 force, vec3 point);
|
|
||||||
/**
|
|
||||||
* @brief Apply an external torque to the body.
|
|
||||||
* If the body is sleeping, calling this method will wake it up. Note that the
|
|
||||||
* force will we added to the sum of the applied torques and that this sum will be
|
|
||||||
* reset to zero at the end of each call of the DynamicsWorld::update() method.
|
|
||||||
* You can only apply a force to a dynamic body otherwise, this method will do nothing.
|
|
||||||
* @param[in] torque The external torque to apply on the body
|
|
||||||
*/
|
|
||||||
void applyTorque( vec3 torque);
|
|
||||||
/**
|
|
||||||
* @brief Add a collision shape to the body.
|
|
||||||
* When you add a collision shape to the body, an intternal copy of this collision shape will be created internally.
|
|
||||||
* Therefore, you can delete it right after calling this method or use it later to add it to another body.
|
|
||||||
* This method will return a pointer to a new proxy shape. A proxy shape is an object that links a collision shape and a given body.
|
|
||||||
* You can use the returned proxy shape to get and set information about the corresponding collision shape for that body.
|
|
||||||
* @param[in] collisionShape The collision shape you want to add to the body
|
|
||||||
* @param[in] transform The transformation of the collision shape that transforms the local-space of the collision shape into the local-space of the body
|
|
||||||
* @param[in] mass Mass (in kilograms) of the collision shape you want to add
|
|
||||||
* @return A pointer to the proxy shape that has been created to link the body to the new collision shape you have added.
|
|
||||||
*/
|
|
||||||
ProxyShape* addCollisionShape(CollisionShape* collisionShape,
|
|
||||||
etk::Transform3D transform,
|
|
||||||
float mass);
|
|
||||||
virtual void removeCollisionShape( ProxyShape* proxyShape) override;
|
|
||||||
/**
|
|
||||||
* @brief Recompute the center of mass, total mass and inertia tensor of the body using all the collision shapes attached to the body.
|
|
||||||
*/
|
|
||||||
void recomputeMassInformation();
|
|
||||||
friend class DynamicsWorld;
|
|
||||||
friend class ContactSolver;
|
|
||||||
friend class BallAndSocketJoint;
|
|
||||||
friend class SliderJoint;
|
|
||||||
friend class HingeJoint;
|
|
||||||
friend class FixedJoint;
|
|
||||||
};
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
514
src/org/atriaSoft/ephysics/body/RigidBody.java
Normal file
514
src/org/atriaSoft/ephysics/body/RigidBody.java
Normal file
@ -0,0 +1,514 @@
|
|||||||
|
package org.atriaSoft.ephysics.body;
|
||||||
|
|
||||||
|
import org.atriaSoft.etk.math.Matrix3f;
|
||||||
|
import org.atriaSoft.etk.math.Transform3D;
|
||||||
|
import org.atriaSoft.etk.math.Vector3f;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief This class represents a rigid body of the physics
|
||||||
|
* engine. A rigid body is a non-deformable body that
|
||||||
|
* has a ant mass. This class inherits from the
|
||||||
|
* CollisionBody class.
|
||||||
|
*/
|
||||||
|
class RigidBody extends CollisionBody {
|
||||||
|
protected float initMass; //!< Intial mass of the body
|
||||||
|
protected Vector3f centerOfMassLocal; //!< Center of mass of the body in local-space coordinates. The center of mass can therefore be different from the body origin
|
||||||
|
protected Vector3f centerOfMassWorld; //!< Center of mass of the body in world-space coordinates
|
||||||
|
protected Vector3f linearVelocity; //!< Linear velocity of the body
|
||||||
|
protected Vector3f angularVelocity; //!< Angular velocity of the body
|
||||||
|
protected Vector3f externalForce; //!< Current external force on the body
|
||||||
|
protected Vector3f externalTorque; //!< Current external torque on the body
|
||||||
|
protected Matrix3f inertiaTensorLocal; //!< Local inertia tensor of the body (in local-space) with respect to the center of mass of the body
|
||||||
|
protected Matrix3f inertiaTensorLocalInverse; //!< Inverse of the inertia tensor of the body
|
||||||
|
protected float massInverse; //!< Inverse of the mass of the body
|
||||||
|
protected boolean isGravityEnabled; //!< True if the gravity needs to be applied to this rigid body
|
||||||
|
protected Material material; //!< Material properties of the rigid body
|
||||||
|
protected float linearDamping; //!< Linear velocity damping factor
|
||||||
|
protected float angularDamping; //!< Angular velocity damping factor
|
||||||
|
protected JointListElement jointsList; //!< First element of the linked list of joints involving this body
|
||||||
|
/**
|
||||||
|
* @brief Remove a joint from the joints list
|
||||||
|
*/
|
||||||
|
protected void removeJointFrom_jointsList( Joint joint) {
|
||||||
|
assert(joint != null);
|
||||||
|
assert(this.jointsList != null);
|
||||||
|
// Remove the joint from the linked list of the joints of the first body
|
||||||
|
if (this.jointsList.joint == joint) { // If the first element is the one to remove
|
||||||
|
JointListElement* elementToRemove = this.jointsList;
|
||||||
|
this.jointsList = elementToRemove.next;
|
||||||
|
ETKDELETE(JointListElement, elementToRemove);
|
||||||
|
elementToRemove = null;
|
||||||
|
}
|
||||||
|
else { // If the element to remove is not the first one in the list
|
||||||
|
JointListElement* currentElement = this.jointsList;
|
||||||
|
while (currentElement.next != null) {
|
||||||
|
if (currentElement.next.joint == joint) {
|
||||||
|
JointListElement* elementToRemove = currentElement.next;
|
||||||
|
currentElement.next = elementToRemove.next;
|
||||||
|
ETKDELETE(JointListElement, elementToRemove);
|
||||||
|
elementToRemove = null;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
currentElement = currentElement.next;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
/**
|
||||||
|
* @brief Update the transform of the body after a change of the center of mass
|
||||||
|
*/
|
||||||
|
protected void updateTransformWithCenterOfMass() {
|
||||||
|
// Translate the body according to the translation of the center of mass position
|
||||||
|
this.transform.setPosition(this.centerOfMassWorld - this.transform.getOrientation() * this.centerOfMassLocal);
|
||||||
|
if (isnan(this.transform.getPosition().x()) == true) {
|
||||||
|
Log.critical("updateTransformWithCenterOfMass: " + this.transform);
|
||||||
|
}
|
||||||
|
if (isinf(this.transform.getOrientation().z()) == true) {
|
||||||
|
Log.critical(" set transform: " + this.transform);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
@Override
|
||||||
|
protected void updateBroadPhaseState() {
|
||||||
|
PROFILE("RigidBody::updateBroadPhaseState()");
|
||||||
|
DynamicsWorld world = staticcast<DynamicsWorld>(this.world);
|
||||||
|
Vector3f displacement = world.timeStep * this.linearVelocity;
|
||||||
|
// For all the proxy collision shapes of the body
|
||||||
|
for (ProxyShape* shape = this.proxyCollisionShapes; shape != null; shape = shape.this.next) {
|
||||||
|
// Recompute the world-space AABB of the collision shape
|
||||||
|
AABB aabb;
|
||||||
|
Log.verbose(" : " + aabb.getMin() + " " + aabb.getMax());
|
||||||
|
Log.verbose(" this.transform: " + this.transform);
|
||||||
|
shape.getCollisionShape().computeAABB(aabb, this.transform *shape.getLocalToBodyTransform());
|
||||||
|
Log.verbose(" : " + aabb.getMin() + " " + aabb.getMax());
|
||||||
|
// Update the broad-phase state for the proxy collision shape
|
||||||
|
this.world.collisionDetection.updateProxyCollisionShape(shape, aabb, displacement);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
/**
|
||||||
|
* @brief Constructor
|
||||||
|
* @param transform The transformation of the body
|
||||||
|
* @param world The world where the body has been added
|
||||||
|
* @param id The ID of the body
|
||||||
|
*/
|
||||||
|
public RigidBody( Transform3D transform, CollisionWorld world, long id) {
|
||||||
|
super(transform, world, id);
|
||||||
|
this.initMass = 1.0f;
|
||||||
|
this.centerOfMassLocal = new Vector3f(0, 0, 0;
|
||||||
|
this.centerOfMassWorld = transform.getPosition().clone();
|
||||||
|
this.isGravityEnabled = true;
|
||||||
|
this.linearDamping = 0.0f;
|
||||||
|
this.angularDamping = 0.0f;
|
||||||
|
this.jointsList = null;
|
||||||
|
// Compute the inverse mass
|
||||||
|
this.massInverse = 1.0f / this.initMass;
|
||||||
|
}
|
||||||
|
@Override
|
||||||
|
public void setType(BodyType type) {
|
||||||
|
if (this.type == type) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
super.setType(type);
|
||||||
|
recomputeMassInformation();
|
||||||
|
if (this.type == STATIC) {
|
||||||
|
// Reset the velocity to zero
|
||||||
|
this.linearVelocity.setZero();
|
||||||
|
this.angularVelocity.setZero();
|
||||||
|
}
|
||||||
|
if ( this.type == STATIC
|
||||||
|
|| this.type == KINEMATIC) {
|
||||||
|
// Reset the inverse mass and inverse inertia tensor to zero
|
||||||
|
this.massInverse = 0.0f;
|
||||||
|
this.inertiaTensorLocal.setZero();
|
||||||
|
this.inertiaTensorLocalInverse.setZero();
|
||||||
|
} else {
|
||||||
|
this.massInverse = 1.0f / this.initMass;
|
||||||
|
this.inertiaTensorLocalInverse = this.inertiaTensorLocal.getInverse();
|
||||||
|
}
|
||||||
|
setIsSleeping(false);
|
||||||
|
resetContactManifoldsList();
|
||||||
|
// Ask the broad-phase to test again the collision shapes of the body for collision detection (as if the body has moved)
|
||||||
|
askForBroadPhaseCollisionCheck();
|
||||||
|
this.externalForce.setZero();
|
||||||
|
this.externalTorque.setZero();
|
||||||
|
}
|
||||||
|
/**
|
||||||
|
* @brief Set the current position and orientation
|
||||||
|
* @param[in] transform The transformation of the body that transforms the local-space of the body into world-space
|
||||||
|
*/
|
||||||
|
public void setTransform( Transform3D transform) {
|
||||||
|
Log.debug(" set transform: " + this.transform + " ==> " + transform);
|
||||||
|
if (isnan(transform.getPosition().x()) == true) {
|
||||||
|
Log.critical(" set transform: " + this.transform + " ==> " + transform);
|
||||||
|
}
|
||||||
|
if (isinf(transform.getOrientation().z()) == true) {
|
||||||
|
Log.critical(" set transform: " + this.transform + " ==> " + transform);
|
||||||
|
}
|
||||||
|
this.transform = transform;
|
||||||
|
Vector3f oldCenterOfMass = this.centerOfMassWorld;
|
||||||
|
// Compute the new center of mass in world-space coordinates
|
||||||
|
this.centerOfMassWorld = this.transform * this.centerOfMassLocal;
|
||||||
|
// Update the linear velocity of the center of mass
|
||||||
|
this.linearVelocity += this.angularVelocity.cross(this.centerOfMassWorld - oldCenterOfMass);
|
||||||
|
updateBroadPhaseState();
|
||||||
|
}
|
||||||
|
/**
|
||||||
|
* @brief Get the mass of the body
|
||||||
|
* @return The mass (in kilograms) of the body
|
||||||
|
*/
|
||||||
|
public float getMass() {
|
||||||
|
return this.initMass;
|
||||||
|
}
|
||||||
|
/**
|
||||||
|
* @brief Get the linear velocity
|
||||||
|
* @return The linear velocity vector of the body
|
||||||
|
*/
|
||||||
|
public Vector3f getLinearVelocity() {
|
||||||
|
return this.linearVelocity;
|
||||||
|
}
|
||||||
|
/**
|
||||||
|
* @brief Set the linear velocity of the rigid body.
|
||||||
|
* @param[in] linearVelocity Linear velocity vector of the body
|
||||||
|
*/
|
||||||
|
public void setLinearVelocity( Vector3f linearVelocity) {
|
||||||
|
if (this.type == STATIC) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
this.linearVelocity = linearVelocity;
|
||||||
|
if (this.linearVelocity.length2() > 0.0f) {
|
||||||
|
setIsSleeping(false);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
/**
|
||||||
|
* @brief Get the angular velocity of the body
|
||||||
|
* @return The angular velocity vector of the body
|
||||||
|
*/
|
||||||
|
public Vector3f getAngularVelocity() {
|
||||||
|
return this.angularVelocity;
|
||||||
|
}
|
||||||
|
/**
|
||||||
|
* @brief Set the angular velocity.
|
||||||
|
* @param[in] angularVelocity The angular velocity vector of the body
|
||||||
|
*/
|
||||||
|
public void setAngularVelocity( Vector3f angularVelocity) {
|
||||||
|
if (this.type == STATIC) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
this.angularVelocity = angularVelocity;
|
||||||
|
if (this.angularVelocity.length2() > 0.0f) {
|
||||||
|
setIsSleeping(false);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
/**
|
||||||
|
* @brief Set the variable to know whether or not the body is sleeping
|
||||||
|
* @param[in] isSleeping New sleeping state of the body
|
||||||
|
*/
|
||||||
|
public void setIsSleeping(boolean isSleeping) {
|
||||||
|
if (isSleeping) {
|
||||||
|
this.linearVelocity.setZero();
|
||||||
|
this.angularVelocity.setZero();
|
||||||
|
this.externalForce.setZero();
|
||||||
|
this.externalTorque.setZero();
|
||||||
|
}
|
||||||
|
Body::setIsSleeping(isSleeping);
|
||||||
|
}
|
||||||
|
/**
|
||||||
|
* @brief Get the local inertia tensor of the body (in local-space coordinates)
|
||||||
|
* @return The 3x3 inertia tensor matrix of the body
|
||||||
|
*/
|
||||||
|
public Matrix3f getInertiaTensorLocal() {
|
||||||
|
return this.inertiaTensorLocal;
|
||||||
|
}
|
||||||
|
/**
|
||||||
|
* @brief Set the local inertia tensor of the body (in local-space coordinates)
|
||||||
|
* @param[in] inertiaTensorLocal The 3x3 inertia tensor matrix of the body in local-space coordinates
|
||||||
|
*/
|
||||||
|
public void setInertiaTensorLocal( Matrix3f inertiaTensorLocal) {
|
||||||
|
if (this.type != DYNAMIC) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
this.inertiaTensorLocal = inertiaTensorLocal;
|
||||||
|
this.inertiaTensorLocalInverse = this.inertiaTensorLocal.getInverse();
|
||||||
|
}
|
||||||
|
/**
|
||||||
|
* @brief Set the local center of mass of the body (in local-space coordinates)
|
||||||
|
* @param[in] centerOfMassLocal The center of mass of the body in local-space coordinates
|
||||||
|
*/
|
||||||
|
public void setCenterOfMassLocal( Vector3f centerOfMassLocal) {
|
||||||
|
if (this.type != DYNAMIC) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
Vector3f oldCenterOfMass = this.centerOfMassWorld;
|
||||||
|
this.centerOfMassLocal = centerOfMassLocal;
|
||||||
|
this.centerOfMassWorld = this.transform * this.centerOfMassLocal;
|
||||||
|
this.linearVelocity += this.angularVelocity.cross(this.centerOfMassWorld - oldCenterOfMass);
|
||||||
|
}
|
||||||
|
/**
|
||||||
|
* @brief Set the mass of the rigid body
|
||||||
|
* @param[in] mass The mass (in kilograms) of the body
|
||||||
|
*/
|
||||||
|
public void setMass(float mass) {
|
||||||
|
if (this.type != DYNAMIC) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
this.initMass = mass;
|
||||||
|
if (this.initMass > 0.0f) {
|
||||||
|
this.massInverse = 1.0f / this.initMass;
|
||||||
|
} else {
|
||||||
|
this.initMass = 1.0f;
|
||||||
|
this.massInverse = 1.0f;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
/**
|
||||||
|
* @brief Get the inertia tensor in world coordinates.
|
||||||
|
* The inertia tensor Iw in world coordinates is computed
|
||||||
|
* with the local inertia tensor Ib in body coordinates
|
||||||
|
* by Iw = R * Ib * R^T
|
||||||
|
* where R is the rotation matrix (and R^T its transpose) of
|
||||||
|
* the current orientation quaternion of the body
|
||||||
|
* @return The 3x3 inertia tensor matrix of the body in world-space coordinates
|
||||||
|
*/
|
||||||
|
public Matrix3f getInertiaTensorWorld() {
|
||||||
|
// Compute and return the inertia tensor in world coordinates
|
||||||
|
return this.transform.getOrientation().getMatrix() * this.inertiaTensorLocal *
|
||||||
|
this.transform.getOrientation().getMatrix().getTranspose();
|
||||||
|
}
|
||||||
|
/**
|
||||||
|
* @brief Get the inverse of the inertia tensor in world coordinates.
|
||||||
|
* The inertia tensor Iw in world coordinates is computed with the
|
||||||
|
* local inverse inertia tensor Ib^-1 in body coordinates
|
||||||
|
* by Iw = R * Ib^-1 * R^T
|
||||||
|
* where R is the rotation matrix (and R^T its transpose) of the
|
||||||
|
* current orientation quaternion of the body
|
||||||
|
* @return The 3x3 inverse inertia tensor matrix of the body in world-space coordinates
|
||||||
|
*/
|
||||||
|
public Matrix3f getInertiaTensorInverseWorld() {
|
||||||
|
// TODO : DO NOT RECOMPUTE THE MATRIX MULTIPLICATION EVERY TIME. WE NEED TO STORE THE
|
||||||
|
// INVERSE WORLD TENSOR IN THE CLASS AND UPLDATE IT WHEN THE ORIENTATION OF THE BODY CHANGES
|
||||||
|
// Compute and return the inertia tensor in world coordinates
|
||||||
|
return this.transform.getOrientation().getMatrix() * this.inertiaTensorLocalInverse *
|
||||||
|
this.transform.getOrientation().getMatrix().getTranspose();
|
||||||
|
}
|
||||||
|
/**
|
||||||
|
* @brief get the need of gravity appling to this rigid body
|
||||||
|
* @return True if the gravity is applied to the body
|
||||||
|
*/
|
||||||
|
public boolean isGravityEnabled() {
|
||||||
|
return this.isGravityEnabled;
|
||||||
|
}
|
||||||
|
/**
|
||||||
|
* @brief Set the variable to know if the gravity is applied to this rigid body
|
||||||
|
* @param[in] isEnabled True if you want the gravity to be applied to this body
|
||||||
|
*/
|
||||||
|
public void enableGravity(boolean isEnabled) {
|
||||||
|
this.isGravityEnabled = isEnabled;
|
||||||
|
}
|
||||||
|
/**
|
||||||
|
* @brief get a reference to the material properties of the rigid body
|
||||||
|
* @return A reference to the material of the body
|
||||||
|
*/
|
||||||
|
public Material getMaterial() {
|
||||||
|
return this.material;
|
||||||
|
}
|
||||||
|
/**
|
||||||
|
* @brief Set a new material for this rigid body
|
||||||
|
* @param[in] material The material you want to set to the body
|
||||||
|
*/
|
||||||
|
public void setMaterial( Material material) {
|
||||||
|
this.material = material;
|
||||||
|
}
|
||||||
|
/**
|
||||||
|
* @brief Get the linear velocity damping factor
|
||||||
|
* @return The linear damping factor of this body
|
||||||
|
*/
|
||||||
|
public float getLinearDamping() {
|
||||||
|
return this.linearDamping;
|
||||||
|
}
|
||||||
|
/**
|
||||||
|
* @brief Set the linear damping factor. This is the ratio of the linear velocity that the body will lose every at seconds of simulation.
|
||||||
|
* @param[in] linearDamping The linear damping factor of this body
|
||||||
|
*/
|
||||||
|
public void setLinearDamping(float linearDamping) {
|
||||||
|
assert(linearDamping >= 0.0f);
|
||||||
|
this.linearDamping = linearDamping;
|
||||||
|
}
|
||||||
|
/**
|
||||||
|
* @brief Get the angular velocity damping factor
|
||||||
|
* @return The angular damping factor of this body
|
||||||
|
*/
|
||||||
|
public float getAngularDamping() {
|
||||||
|
return this.angularDamping;
|
||||||
|
}
|
||||||
|
/**
|
||||||
|
* @brief Set the angular damping factor. This is the ratio of the angular velocity that the body will lose at every seconds of simulation.
|
||||||
|
* @param[in] angularDamping The angular damping factor of this body
|
||||||
|
*/
|
||||||
|
public void setAngularDamping(float angularDamping) {
|
||||||
|
assert(angularDamping >= 0.0f);
|
||||||
|
this.angularDamping = angularDamping;
|
||||||
|
}
|
||||||
|
/**
|
||||||
|
* @brief Get the first element of the linked list of joints involving this body
|
||||||
|
* @return The first element of the linked-list of all the joints involving this body
|
||||||
|
*/
|
||||||
|
public JointListElement* getJointsList() {
|
||||||
|
return this.jointsList;
|
||||||
|
}
|
||||||
|
/**
|
||||||
|
* @brief Get the first element of the linked list of joints involving this body
|
||||||
|
* @return The first element of the linked-list of all the joints involving this body
|
||||||
|
*/
|
||||||
|
public JointListElement* getJointsList() {
|
||||||
|
return this.jointsList;
|
||||||
|
}
|
||||||
|
/**
|
||||||
|
* @brief Apply an external force to the body at its center of mass.
|
||||||
|
* If the body is sleeping, calling this method will wake it up. Note that the
|
||||||
|
* force will we added to the sum of the applied forces and that this sum will be
|
||||||
|
* reset to zero at the end of each call of the DynamicsWorld::update() method.
|
||||||
|
* You can only apply a force to a dynamic body otherwise, this method will do nothing.
|
||||||
|
* @param[in] force The external force to apply on the center of mass of the body
|
||||||
|
*/
|
||||||
|
public void applyForceToCenterOfMass( Vector3f force) {
|
||||||
|
if (this.type != DYNAMIC) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
if (this.isSleeping) {
|
||||||
|
setIsSleeping(false);
|
||||||
|
}
|
||||||
|
this.externalForce += force;
|
||||||
|
}
|
||||||
|
/**
|
||||||
|
* @brief Apply an external force to the body at a given point (in world-space coordinates).
|
||||||
|
* If the point is not at the center of mass of the body, it will also
|
||||||
|
* generate some torque and therefore, change the angular velocity of the body.
|
||||||
|
* If the body is sleeping, calling this method will wake it up. Note that the
|
||||||
|
* force will we added to the sum of the applied forces and that this sum will be
|
||||||
|
* reset to zero at the end of each call of the DynamicsWorld::update() method.
|
||||||
|
* You can only apply a force to a dynamic body otherwise, this method will do nothing.
|
||||||
|
* @param[in] force The force to apply on the body
|
||||||
|
* @param[in] point The point where the force is applied (in world-space coordinates)
|
||||||
|
*/
|
||||||
|
public void applyForce( Vector3f force, Vector3f point) {
|
||||||
|
if (this.type != DYNAMIC) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
if (this.isSleeping) {
|
||||||
|
setIsSleeping(false);
|
||||||
|
}
|
||||||
|
this.externalForce += force;
|
||||||
|
this.externalTorque += (point - this.centerOfMassWorld).cross(force);
|
||||||
|
}
|
||||||
|
/**
|
||||||
|
* @brief Apply an external torque to the body.
|
||||||
|
* If the body is sleeping, calling this method will wake it up. Note that the
|
||||||
|
* force will we added to the sum of the applied torques and that this sum will be
|
||||||
|
* reset to zero at the end of each call of the DynamicsWorld::update() method.
|
||||||
|
* You can only apply a force to a dynamic body otherwise, this method will do nothing.
|
||||||
|
* @param[in] torque The external torque to apply on the body
|
||||||
|
*/
|
||||||
|
public void applyTorque( Vector3f torque) {
|
||||||
|
if (this.type != DYNAMIC) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
if (this.isSleeping) {
|
||||||
|
setIsSleeping(false);
|
||||||
|
}
|
||||||
|
this.externalTorque += torque;
|
||||||
|
}
|
||||||
|
/**
|
||||||
|
* @brief Add a collision shape to the body.
|
||||||
|
* When you add a collision shape to the body, an intternal copy of this collision shape will be created internally.
|
||||||
|
* Therefore, you can delete it right after calling this method or use it later to add it to another body.
|
||||||
|
* This method will return a pointer to a new proxy shape. A proxy shape is an object that links a collision shape and a given body.
|
||||||
|
* You can use the returned proxy shape to get and set information about the corresponding collision shape for that body.
|
||||||
|
* @param[in] collisionShape The collision shape you want to add to the body
|
||||||
|
* @param[in] transform The transformation of the collision shape that transforms the local-space of the collision shape into the local-space of the body
|
||||||
|
* @param[in] mass Mass (in kilograms) of the collision shape you want to add
|
||||||
|
* @return A pointer to the proxy shape that has been created to link the body to the new collision shape you have added.
|
||||||
|
*/
|
||||||
|
public ProxyShape addCollisionShape(CollisionShape collisionShape,
|
||||||
|
Transform3D transform,
|
||||||
|
float mass) {
|
||||||
|
assert(mass > 0.0f);
|
||||||
|
// Create a new proxy collision shape to attach the collision shape to the body
|
||||||
|
ProxyShape* proxyShape = ETKNEW(ProxyShape, this, collisionShape, transform, mass);
|
||||||
|
// Add it to the list of proxy collision shapes of the body
|
||||||
|
if (this.proxyCollisionShapes == null) {
|
||||||
|
this.proxyCollisionShapes = proxyShape;
|
||||||
|
} else {
|
||||||
|
proxyShape.this.next = this.proxyCollisionShapes;
|
||||||
|
this.proxyCollisionShapes = proxyShape;
|
||||||
|
}
|
||||||
|
// Compute the world-space AABB of the new collision shape
|
||||||
|
AABB aabb;
|
||||||
|
collisionShape.computeAABB(aabb, this.transform * transform);
|
||||||
|
// Notify the collision detection about this new collision shape
|
||||||
|
this.world.collisionDetection.addProxyCollisionShape(proxyShape, aabb);
|
||||||
|
this.numberCollisionShapes++;
|
||||||
|
recomputeMassInformation();
|
||||||
|
return proxyShape;
|
||||||
|
}
|
||||||
|
public void removeCollisionShape(ProxyShape proxyShape) {
|
||||||
|
CollisionBody::removeCollisionShape(proxyShape);
|
||||||
|
recomputeMassInformation();
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Recompute the center of mass, total mass and inertia tensor of the body using all the collision shapes attached to the body.
|
||||||
|
*/
|
||||||
|
public void recomputeMassInformation() {
|
||||||
|
this.initMass = 0.0f;
|
||||||
|
this.massInverse = 0.0f;
|
||||||
|
this.inertiaTensorLocal.setZero();
|
||||||
|
this.inertiaTensorLocalInverse.setZero();
|
||||||
|
this.centerOfMassLocal.setZero();
|
||||||
|
// If it is STATIC or KINEMATIC body
|
||||||
|
if (this.type == STATIC || this.type == KINEMATIC) {
|
||||||
|
this.centerOfMassWorld = this.transform.getPosition();
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
assert(this.type == DYNAMIC);
|
||||||
|
// Compute the total mass of the body
|
||||||
|
for (ProxyShape* shape = this.proxyCollisionShapes; shape != NULL; shape = shape.this.next) {
|
||||||
|
this.initMass += shape.getMass();
|
||||||
|
this.centerOfMassLocal += shape.getLocalToBodyTransform().getPosition() * shape.getMass();
|
||||||
|
}
|
||||||
|
if (this.initMass > 0.0f) {
|
||||||
|
this.massInverse = 1.0f / this.initMass;
|
||||||
|
} else {
|
||||||
|
this.initMass = 1.0f;
|
||||||
|
this.massInverse = 1.0f;
|
||||||
|
}
|
||||||
|
// Compute the center of mass
|
||||||
|
Vector3f oldCenterOfMass = this.centerOfMassWorld;
|
||||||
|
this.centerOfMassLocal *= this.massInverse;
|
||||||
|
this.centerOfMassWorld = this.transform * this.centerOfMassLocal;
|
||||||
|
// Compute the total mass and inertia tensor using all the collision shapes
|
||||||
|
for (ProxyShape* shape = this.proxyCollisionShapes; shape != null; shape = shape.this.next) {
|
||||||
|
// Get the inertia tensor of the collision shape in its local-space
|
||||||
|
Matrix3f inertiaTensor;
|
||||||
|
shape.getCollisionShape().computeLocalInertiaTensor(inertiaTensor, shape.getMass());
|
||||||
|
// Convert the collision shape inertia tensor into the local-space of the body
|
||||||
|
Transform3D shapeTransform = shape.getLocalToBodyTransform();
|
||||||
|
Matrix3f rotationMatrix = shapeTransform.getOrientation().getMatrix();
|
||||||
|
inertiaTensor = rotationMatrix * inertiaTensor * rotationMatrix.getTranspose();
|
||||||
|
// Use the parallel axis theorem to convert the inertia tensor w.r.t the collision shape
|
||||||
|
// center into a inertia tensor w.r.t to the body origin.
|
||||||
|
Vector3f offset = shapeTransform.getPosition() - this.centerOfMassLocal;
|
||||||
|
float offsetSquare = offset.length2();
|
||||||
|
Vector3f off1 = offset * (-offset.x());
|
||||||
|
Vector3f off2 = offset * (-offset.y());
|
||||||
|
Vector3f off3 = offset * (-offset.z());
|
||||||
|
Matrix3f offsetMatrix(off1.x()+offsetSquare, off1.y(), off1.z(),
|
||||||
|
off2.x(), off2.y()+offsetSquare, off2.z(),
|
||||||
|
off3.x(), off3.y(), off3.z()+offsetSquare);
|
||||||
|
offsetMatrix *= shape.getMass();
|
||||||
|
this.inertiaTensorLocal += inertiaTensor + offsetMatrix;
|
||||||
|
}
|
||||||
|
// Compute the local inverse inertia tensor
|
||||||
|
this.inertiaTensorLocalInverse = this.inertiaTensorLocal.getInverse();
|
||||||
|
// Update the linear velocity of the center of mass
|
||||||
|
this.linearVelocity += this.angularVelocity.cross(this.centerOfMassWorld - oldCenterOfMass);
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
|
@ -40,7 +40,7 @@ void CollisionDetection::computeCollisionDetection() {
|
|||||||
computeNarrowPhase();
|
computeNarrowPhase();
|
||||||
}
|
}
|
||||||
|
|
||||||
void CollisionDetection::testCollisionBetweenShapes(CollisionCallback* callback, etk::Set<int> shapes1, etk::Set<int> shapes2) {
|
void CollisionDetection::testCollisionBetweenShapes(CollisionCallback* callback, Set<int> shapes1, Set<int> shapes2) {
|
||||||
// Compute the broad-phase collision detection
|
// Compute the broad-phase collision detection
|
||||||
computeBroadPhase();
|
computeBroadPhase();
|
||||||
// Delete all the contact points in the currently overlapping pairs
|
// Delete all the contact points in the currently overlapping pairs
|
||||||
@ -49,54 +49,54 @@ void CollisionDetection::testCollisionBetweenShapes(CollisionCallback* callback,
|
|||||||
computeNarrowPhaseBetweenShapes(callback, shapes1, shapes2);
|
computeNarrowPhaseBetweenShapes(callback, shapes1, shapes2);
|
||||||
}
|
}
|
||||||
|
|
||||||
void CollisionDetection::reportCollisionBetweenShapes(CollisionCallback* callback, etk::Set<int> shapes1, etk::Set<int> shapes2) {
|
void CollisionDetection::reportCollisionBetweenShapes(CollisionCallback* callback, Set<int> shapes1, Set<int> shapes2) {
|
||||||
// For each possible collision pair of bodies
|
// For each possible collision pair of bodies
|
||||||
etk::Map<overlappingpairid, OverlappingPair*>::Iterator it;
|
Map<overlappingpairid, OverlappingPair*>::Iterator it;
|
||||||
for (it = this.overlappingPairs.begin(); it != this.overlappingPairs.end(); ++it) {
|
for (it = this.overlappingPairs.begin(); it != this.overlappingPairs.end(); ++it) {
|
||||||
OverlappingPair* pair = it->second;
|
OverlappingPair* pair = it.second;
|
||||||
ProxyShape* shape1 = pair->getShape1();
|
ProxyShape* shape1 = pair.getShape1();
|
||||||
ProxyShape* shape2 = pair->getShape2();
|
ProxyShape* shape2 = pair.getShape2();
|
||||||
assert(shape1->this.broadPhaseID != shape2->this.broadPhaseID);
|
assert(shape1.this.broadPhaseID != shape2.this.broadPhaseID);
|
||||||
// If both shapes1 and shapes2 sets are non-empty, we check that
|
// If both shapes1 and shapes2 sets are non-empty, we check that
|
||||||
// shape1 is among on set and shape2 is among the other one
|
// shape1 is among on set and shape2 is among the other one
|
||||||
if ( !shapes1.empty()
|
if ( !shapes1.empty()
|
||||||
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj !shapes2.empty()
|
&& !shapes2.empty()
|
||||||
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj ( shapes1.count(shape1->this.broadPhaseID) == 0
|
&& ( shapes1.count(shape1.this.broadPhaseID) == 0
|
||||||
|| shapes2.count(shape2->this.broadPhaseID) == 0 )
|
|| shapes2.count(shape2.this.broadPhaseID) == 0 )
|
||||||
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj ( shapes1.count(shape2->this.broadPhaseID) == 0
|
&& ( shapes1.count(shape2.this.broadPhaseID) == 0
|
||||||
|| shapes2.count(shape1->this.broadPhaseID) == 0 ) ) {
|
|| shapes2.count(shape1.this.broadPhaseID) == 0 ) ) {
|
||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
if ( !shapes1.empty()
|
if ( !shapes1.empty()
|
||||||
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj shapes2.empty()
|
&& shapes2.empty()
|
||||||
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj shapes1.count(shape1->this.broadPhaseID) == 0
|
&& shapes1.count(shape1.this.broadPhaseID) == 0
|
||||||
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj shapes1.count(shape2->this.broadPhaseID) == 0) {
|
&& shapes1.count(shape2.this.broadPhaseID) == 0) {
|
||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
if ( !shapes2.empty()
|
if ( !shapes2.empty()
|
||||||
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj shapes1.empty()
|
&& shapes1.empty()
|
||||||
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj shapes2.count(shape1->this.broadPhaseID) == 0
|
&& shapes2.count(shape1.this.broadPhaseID) == 0
|
||||||
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj shapes2.count(shape2->this.broadPhaseID) == 0) {
|
&& shapes2.count(shape2.this.broadPhaseID) == 0) {
|
||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
// For each contact manifold set of the overlapping pair
|
// For each contact manifold set of the overlapping pair
|
||||||
ContactManifoldSet manifoldSet = pair->getContactManifoldSet();
|
ContactManifoldSet manifoldSet = pair.getContactManifoldSet();
|
||||||
for (int j=0; j<manifoldSet.getNbContactManifolds(); j++) {
|
for (int j=0; j<manifoldSet.getNbContactManifolds(); j++) {
|
||||||
ContactManifold* manifold = manifoldSet.getContactManifold(j);
|
ContactManifold* manifold = manifoldSet.getContactManifold(j);
|
||||||
// For each contact manifold of the manifold set
|
// For each contact manifold of the manifold set
|
||||||
for (int i=0; i<manifold->getNbContactPoints(); i++) {
|
for (int i=0; i<manifold.getNbContactPoints(); i++) {
|
||||||
ContactPoint* contactPoint = manifold->getContactPoint(i);
|
ContactPoint* contactPoint = manifold.getContactPoint(i);
|
||||||
// Create the contact info object for the contact
|
// Create the contact info object for the contact
|
||||||
ContactPointInfo contactInfo(manifold->getShape1(), manifold->getShape2(),
|
ContactPointInfo contactInfo(manifold.getShape1(), manifold.getShape2(),
|
||||||
manifold->getShape1()->getCollisionShape(),
|
manifold.getShape1().getCollisionShape(),
|
||||||
manifold->getShape2()->getCollisionShape(),
|
manifold.getShape2().getCollisionShape(),
|
||||||
contactPoint->getNormal(),
|
contactPoint.getNormal(),
|
||||||
contactPoint->getPenetrationDepth(),
|
contactPoint.getPenetrationDepth(),
|
||||||
contactPoint->getLocalPointOnBody1(),
|
contactPoint.getLocalPointOnBody1(),
|
||||||
contactPoint->getLocalPointOnBody2());
|
contactPoint.getLocalPointOnBody2());
|
||||||
// Notify the collision callback about this new contact
|
// Notify the collision callback about this new contact
|
||||||
if (callback != null) {
|
if (callback != null) {
|
||||||
callback->notifyContact(contactInfo);
|
callback.notifyContact(contactInfo);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@ -119,35 +119,35 @@ void CollisionDetection::computeNarrowPhase() {
|
|||||||
// Clear the set of overlapping pairs in narrow-phase contact
|
// Clear the set of overlapping pairs in narrow-phase contact
|
||||||
this.contactOverlappingPairs.clear();
|
this.contactOverlappingPairs.clear();
|
||||||
// For each possible collision pair of bodies
|
// For each possible collision pair of bodies
|
||||||
etk::Map<overlappingpairid, OverlappingPair*>::Iterator it;
|
Map<overlappingpairid, OverlappingPair*>::Iterator it;
|
||||||
for (it = this.overlappingPairs.begin(); it != this.overlappingPairs.end(); ) {
|
for (it = this.overlappingPairs.begin(); it != this.overlappingPairs.end(); ) {
|
||||||
OverlappingPair* pair = it->second;
|
OverlappingPair* pair = it.second;
|
||||||
ProxyShape* shape1 = pair->getShape1();
|
ProxyShape* shape1 = pair.getShape1();
|
||||||
ProxyShape* shape2 = pair->getShape2();
|
ProxyShape* shape2 = pair.getShape2();
|
||||||
assert(shape1->this.broadPhaseID != shape2->this.broadPhaseID);
|
assert(shape1.this.broadPhaseID != shape2.this.broadPhaseID);
|
||||||
// Check if the collision filtering allows collision between the two shapes and
|
// Check if the collision filtering allows collision between the two shapes and
|
||||||
// that the two shapes are still overlapping. Otherwise, we destroy the
|
// that the two shapes are still overlapping. Otherwise, we destroy the
|
||||||
// overlapping pair
|
// overlapping pair
|
||||||
if (((shape1->getCollideWithMaskBits() shape2->getCollisionCategoryBits()) == 0 ||
|
if (((shape1.getCollideWithMaskBits() shape2.getCollisionCategoryBits()) == 0 ||
|
||||||
(shape1->getCollisionCategoryBits() shape2->getCollideWithMaskBits()) == 0) ||
|
(shape1.getCollisionCategoryBits() shape2.getCollideWithMaskBits()) == 0) ||
|
||||||
!this.broadPhaseAlgorithm.testOverlappingShapes(shape1, shape2)) {
|
!this.broadPhaseAlgorithm.testOverlappingShapes(shape1, shape2)) {
|
||||||
// TODO : Remove all the contact manifold of the overlapping pair from the contact manifolds list of the two bodies involved
|
// TODO : Remove all the contact manifold of the overlapping pair from the contact manifolds list of the two bodies involved
|
||||||
// Destroy the overlapping pair
|
// Destroy the overlapping pair
|
||||||
ETKDELETE(OverlappingPair, it->second);
|
ETKDELETE(OverlappingPair, it.second);
|
||||||
it->second = null;
|
it.second = null;
|
||||||
it = this.overlappingPairs.erase(it);
|
it = this.overlappingPairs.erase(it);
|
||||||
continue;
|
continue;
|
||||||
} else {
|
} else {
|
||||||
++it;
|
++it;
|
||||||
}
|
}
|
||||||
CollisionBody* body1 = shape1->getBody();
|
CollisionBody* body1 = shape1.getBody();
|
||||||
CollisionBody* body2 = shape2->getBody();
|
CollisionBody* body2 = shape2.getBody();
|
||||||
// Update the contact cache of the overlapping pair
|
// Update the contact cache of the overlapping pair
|
||||||
pair->update();
|
pair.update();
|
||||||
// Check that at least one body is awake and not static
|
// Check that at least one body is awake and not static
|
||||||
boolean isBody1Active = !body1->isSleeping() hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj body1->getType() != STATIC;
|
boolean isBody1Active = !body1.isSleeping() && body1.getType() != STATIC;
|
||||||
boolean isBody2Active = !body2->isSleeping() hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj body2->getType() != STATIC;
|
boolean isBody2Active = !body2.isSleeping() && body2.getType() != STATIC;
|
||||||
if (!isBody1Active hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj !isBody2Active) {
|
if (!isBody1Active && !isBody2Active) {
|
||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
// Check if the bodies are in the set of bodies that cannot collide between each other
|
// Check if the bodies are in the set of bodies that cannot collide between each other
|
||||||
@ -156,85 +156,85 @@ void CollisionDetection::computeNarrowPhase() {
|
|||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
// Select the narrow phase algorithm to use according to the two collision shapes
|
// Select the narrow phase algorithm to use according to the two collision shapes
|
||||||
CollisionShapeType shape1Type = shape1->getCollisionShape()->getType();
|
CollisionShapeType shape1Type = shape1.getCollisionShape().getType();
|
||||||
CollisionShapeType shape2Type = shape2->getCollisionShape()->getType();
|
CollisionShapeType shape2Type = shape2.getCollisionShape().getType();
|
||||||
NarrowPhaseAlgorithm* narrowPhaseAlgorithm = this.collisionMatrix[shape1Type][shape2Type];
|
NarrowPhaseAlgorithm* narrowPhaseAlgorithm = this.collisionMatrix[shape1Type][shape2Type];
|
||||||
// If there is no collision algorithm between those two kinds of shapes
|
// If there is no collision algorithm between those two kinds of shapes
|
||||||
if (narrowPhaseAlgorithm == null) {
|
if (narrowPhaseAlgorithm == null) {
|
||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
// Notify the narrow-phase algorithm about the overlapping pair we are going to test
|
// Notify the narrow-phase algorithm about the overlapping pair we are going to test
|
||||||
narrowPhaseAlgorithm->setCurrentOverlappingPair(pair);
|
narrowPhaseAlgorithm.setCurrentOverlappingPair(pair);
|
||||||
// Create the CollisionShapeInfo objects
|
// Create the CollisionShapeInfo objects
|
||||||
CollisionShapeInfo shape1Info(shape1, shape1->getCollisionShape(), shape1->getLocalToWorldTransform(),
|
CollisionShapeInfo shape1Info(shape1, shape1.getCollisionShape(), shape1.getLocalToWorldTransform(),
|
||||||
pair, shape1->getCachedCollisionData());
|
pair, shape1.getCachedCollisionData());
|
||||||
CollisionShapeInfo shape2Info(shape2, shape2->getCollisionShape(), shape2->getLocalToWorldTransform(),
|
CollisionShapeInfo shape2Info(shape2, shape2.getCollisionShape(), shape2.getLocalToWorldTransform(),
|
||||||
pair, shape2->getCachedCollisionData());
|
pair, shape2.getCachedCollisionData());
|
||||||
|
|
||||||
// Use the narrow-phase collision detection algorithm to check
|
// Use the narrow-phase collision detection algorithm to check
|
||||||
// if there really is a collision. If a collision occurs, the
|
// if there really is a collision. If a collision occurs, the
|
||||||
// notifyContact() callback method will be called.
|
// notifyContact() callback method will be called.
|
||||||
narrowPhaseAlgorithm->testCollision(shape1Info, shape2Info, this);
|
narrowPhaseAlgorithm.testCollision(shape1Info, shape2Info, this);
|
||||||
}
|
}
|
||||||
// Add all the contact manifolds (between colliding bodies) to the bodies
|
// Add all the contact manifolds (between colliding bodies) to the bodies
|
||||||
addAllContactManifoldsToBodies();
|
addAllContactManifoldsToBodies();
|
||||||
}
|
}
|
||||||
|
|
||||||
void CollisionDetection::computeNarrowPhaseBetweenShapes(CollisionCallback* callback, etk::Set<int> shapes1, etk::Set<int> shapes2) {
|
void CollisionDetection::computeNarrowPhaseBetweenShapes(CollisionCallback* callback, Set<int> shapes1, Set<int> shapes2) {
|
||||||
this.contactOverlappingPairs.clear();
|
this.contactOverlappingPairs.clear();
|
||||||
// For each possible collision pair of bodies
|
// For each possible collision pair of bodies
|
||||||
etk::Map<overlappingpairid, OverlappingPair*>::Iterator it;
|
Map<overlappingpairid, OverlappingPair*>::Iterator it;
|
||||||
for (it = this.overlappingPairs.begin(); it != this.overlappingPairs.end(); ) {
|
for (it = this.overlappingPairs.begin(); it != this.overlappingPairs.end(); ) {
|
||||||
OverlappingPair* pair = it->second;
|
OverlappingPair* pair = it.second;
|
||||||
ProxyShape* shape1 = pair->getShape1();
|
ProxyShape* shape1 = pair.getShape1();
|
||||||
ProxyShape* shape2 = pair->getShape2();
|
ProxyShape* shape2 = pair.getShape2();
|
||||||
assert(shape1->this.broadPhaseID != shape2->this.broadPhaseID);
|
assert(shape1.this.broadPhaseID != shape2.this.broadPhaseID);
|
||||||
// If both shapes1 and shapes2 sets are non-empty, we check that
|
// If both shapes1 and shapes2 sets are non-empty, we check that
|
||||||
// shape1 is among on set and shape2 is among the other one
|
// shape1 is among on set and shape2 is among the other one
|
||||||
if ( !shapes1.empty()
|
if ( !shapes1.empty()
|
||||||
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj !shapes2.empty()
|
&& !shapes2.empty()
|
||||||
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj ( shapes1.count(shape1->this.broadPhaseID) == 0
|
&& ( shapes1.count(shape1.this.broadPhaseID) == 0
|
||||||
|| shapes2.count(shape2->this.broadPhaseID) == 0 )
|
|| shapes2.count(shape2.this.broadPhaseID) == 0 )
|
||||||
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj ( shapes1.count(shape2->this.broadPhaseID) == 0
|
&& ( shapes1.count(shape2.this.broadPhaseID) == 0
|
||||||
|| shapes2.count(shape1->this.broadPhaseID) == 0 ) ) {
|
|| shapes2.count(shape1.this.broadPhaseID) == 0 ) ) {
|
||||||
++it;
|
++it;
|
||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
if ( !shapes1.empty()
|
if ( !shapes1.empty()
|
||||||
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj shapes2.empty()
|
&& shapes2.empty()
|
||||||
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj shapes1.count(shape1->this.broadPhaseID) == 0
|
&& shapes1.count(shape1.this.broadPhaseID) == 0
|
||||||
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj shapes1.count(shape2->this.broadPhaseID) == 0) {
|
&& shapes1.count(shape2.this.broadPhaseID) == 0) {
|
||||||
++it;
|
++it;
|
||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
if ( !shapes2.empty()
|
if ( !shapes2.empty()
|
||||||
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj shapes1.empty()
|
&& shapes1.empty()
|
||||||
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj shapes2.count(shape1->this.broadPhaseID) == 0
|
&& shapes2.count(shape1.this.broadPhaseID) == 0
|
||||||
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj shapes2.count(shape2->this.broadPhaseID) == 0) {
|
&& shapes2.count(shape2.this.broadPhaseID) == 0) {
|
||||||
++it;
|
++it;
|
||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
// Check if the collision filtering allows collision between the two shapes and
|
// Check if the collision filtering allows collision between the two shapes and
|
||||||
// that the two shapes are still overlapping. Otherwise, we destroy the
|
// that the two shapes are still overlapping. Otherwise, we destroy the
|
||||||
// overlapping pair
|
// overlapping pair
|
||||||
if ( ( (shape1->getCollideWithMaskBits() shape2->getCollisionCategoryBits()) == 0
|
if ( ( (shape1.getCollideWithMaskBits() shape2.getCollisionCategoryBits()) == 0
|
||||||
|| (shape1->getCollisionCategoryBits() shape2->getCollideWithMaskBits()) == 0 )
|
|| (shape1.getCollisionCategoryBits() shape2.getCollideWithMaskBits()) == 0 )
|
||||||
|| !this.broadPhaseAlgorithm.testOverlappingShapes(shape1, shape2) ) {
|
|| !this.broadPhaseAlgorithm.testOverlappingShapes(shape1, shape2) ) {
|
||||||
// TODO : Remove all the contact manifold of the overlapping pair from the contact manifolds list of the two bodies involved
|
// TODO : Remove all the contact manifold of the overlapping pair from the contact manifolds list of the two bodies involved
|
||||||
// Destroy the overlapping pair
|
// Destroy the overlapping pair
|
||||||
ETKDELETE(OverlappingPair, it->second);
|
ETKDELETE(OverlappingPair, it.second);
|
||||||
it->second = null;
|
it.second = null;
|
||||||
it = this.overlappingPairs.erase(it);
|
it = this.overlappingPairs.erase(it);
|
||||||
continue;
|
continue;
|
||||||
} else {
|
} else {
|
||||||
++it;
|
++it;
|
||||||
}
|
}
|
||||||
CollisionBody* body1 = shape1->getBody();
|
CollisionBody* body1 = shape1.getBody();
|
||||||
CollisionBody* body2 = shape2->getBody();
|
CollisionBody* body2 = shape2.getBody();
|
||||||
// Update the contact cache of the overlapping pair
|
// Update the contact cache of the overlapping pair
|
||||||
pair->update();
|
pair.update();
|
||||||
// Check if the two bodies are allowed to collide, otherwise, we do not test for collision
|
// Check if the two bodies are allowed to collide, otherwise, we do not test for collision
|
||||||
if (body1->getType() != DYNAMIC hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj body2->getType() != DYNAMIC) {
|
if (body1.getType() != DYNAMIC && body2.getType() != DYNAMIC) {
|
||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
longpair bodiesIndex = OverlappingPair::computeBodiesIndexPair(body1, body2);
|
longpair bodiesIndex = OverlappingPair::computeBodiesIndexPair(body1, body2);
|
||||||
@ -242,48 +242,48 @@ void CollisionDetection::computeNarrowPhaseBetweenShapes(CollisionCallback* call
|
|||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
// Check if the two bodies are sleeping, if so, we do no test collision between them
|
// Check if the two bodies are sleeping, if so, we do no test collision between them
|
||||||
if (body1->isSleeping() hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj body2->isSleeping()) {
|
if (body1.isSleeping() && body2.isSleeping()) {
|
||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
// Select the narrow phase algorithm to use according to the two collision shapes
|
// Select the narrow phase algorithm to use according to the two collision shapes
|
||||||
CollisionShapeType shape1Type = shape1->getCollisionShape()->getType();
|
CollisionShapeType shape1Type = shape1.getCollisionShape().getType();
|
||||||
CollisionShapeType shape2Type = shape2->getCollisionShape()->getType();
|
CollisionShapeType shape2Type = shape2.getCollisionShape().getType();
|
||||||
NarrowPhaseAlgorithm* narrowPhaseAlgorithm = this.collisionMatrix[shape1Type][shape2Type];
|
NarrowPhaseAlgorithm* narrowPhaseAlgorithm = this.collisionMatrix[shape1Type][shape2Type];
|
||||||
// If there is no collision algorithm between those two kinds of shapes
|
// If there is no collision algorithm between those two kinds of shapes
|
||||||
if (narrowPhaseAlgorithm == null) {
|
if (narrowPhaseAlgorithm == null) {
|
||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
// Notify the narrow-phase algorithm about the overlapping pair we are going to test
|
// Notify the narrow-phase algorithm about the overlapping pair we are going to test
|
||||||
narrowPhaseAlgorithm->setCurrentOverlappingPair(pair);
|
narrowPhaseAlgorithm.setCurrentOverlappingPair(pair);
|
||||||
// Create the CollisionShapeInfo objects
|
// Create the CollisionShapeInfo objects
|
||||||
CollisionShapeInfo shape1Info(shape1,
|
CollisionShapeInfo shape1Info(shape1,
|
||||||
shape1->getCollisionShape(),
|
shape1.getCollisionShape(),
|
||||||
shape1->getLocalToWorldTransform(),
|
shape1.getLocalToWorldTransform(),
|
||||||
pair,
|
pair,
|
||||||
shape1->getCachedCollisionData());
|
shape1.getCachedCollisionData());
|
||||||
CollisionShapeInfo shape2Info(shape2,
|
CollisionShapeInfo shape2Info(shape2,
|
||||||
shape2->getCollisionShape(),
|
shape2.getCollisionShape(),
|
||||||
shape2->getLocalToWorldTransform(),
|
shape2.getLocalToWorldTransform(),
|
||||||
pair,
|
pair,
|
||||||
shape2->getCachedCollisionData());
|
shape2.getCachedCollisionData());
|
||||||
TestCollisionBetweenShapesCallback narrowPhaseCallback(callback);
|
TestCollisionBetweenShapesCallback narrowPhaseCallback(callback);
|
||||||
// Use the narrow-phase collision detection algorithm to check
|
// Use the narrow-phase collision detection algorithm to check
|
||||||
// if there really is a collision
|
// if there really is a collision
|
||||||
narrowPhaseAlgorithm->testCollision(shape1Info, shape2Info, narrowPhaseCallback);
|
narrowPhaseAlgorithm.testCollision(shape1Info, shape2Info, narrowPhaseCallback);
|
||||||
}
|
}
|
||||||
// Add all the contact manifolds (between colliding bodies) to the bodies
|
// Add all the contact manifolds (between colliding bodies) to the bodies
|
||||||
addAllContactManifoldsToBodies();
|
addAllContactManifoldsToBodies();
|
||||||
}
|
}
|
||||||
|
|
||||||
void CollisionDetection::broadPhaseNotifyOverlappingPair(ProxyShape* shape1, ProxyShape* shape2) {
|
void CollisionDetection::broadPhaseNotifyOverlappingPair(ProxyShape* shape1, ProxyShape* shape2) {
|
||||||
assert(shape1->this.broadPhaseID != shape2->this.broadPhaseID);
|
assert(shape1.this.broadPhaseID != shape2.this.broadPhaseID);
|
||||||
// If the two proxy collision shapes are from the same body, skip it
|
// If the two proxy collision shapes are from the same body, skip it
|
||||||
if (shape1->getBody()->getID() == shape2->getBody()->getID()) {
|
if (shape1.getBody().getID() == shape2.getBody().getID()) {
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
// Check if the collision filtering allows collision between the two shapes
|
// Check if the collision filtering allows collision between the two shapes
|
||||||
if ( (shape1->getCollideWithMaskBits() shape2->getCollisionCategoryBits()) == 0
|
if ( (shape1.getCollideWithMaskBits() shape2.getCollisionCategoryBits()) == 0
|
||||||
|| (shape1->getCollisionCategoryBits() shape2->getCollideWithMaskBits()) == 0) {
|
|| (shape1.getCollisionCategoryBits() shape2.getCollideWithMaskBits()) == 0) {
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
// Compute the overlapping pair ID
|
// Compute the overlapping pair ID
|
||||||
@ -291,27 +291,27 @@ void CollisionDetection::broadPhaseNotifyOverlappingPair(ProxyShape* shape1, Pro
|
|||||||
// Check if the overlapping pair already exists
|
// Check if the overlapping pair already exists
|
||||||
if (this.overlappingPairs.find(pairID) != this.overlappingPairs.end()) return;
|
if (this.overlappingPairs.find(pairID) != this.overlappingPairs.end()) return;
|
||||||
// Compute the maximum number of contact manifolds for this pair
|
// Compute the maximum number of contact manifolds for this pair
|
||||||
int nbMaxManifolds = CollisionShape::computeNbMaxContactManifolds(shape1->getCollisionShape()->getType(),
|
int nbMaxManifolds = CollisionShape::computeNbMaxContactManifolds(shape1.getCollisionShape().getType(),
|
||||||
shape2->getCollisionShape()->getType());
|
shape2.getCollisionShape().getType());
|
||||||
// Create the overlapping pair and add it into the set of overlapping pairs
|
// Create the overlapping pair and add it into the set of overlapping pairs
|
||||||
OverlappingPair* newPair = ETKNEW(OverlappingPair, shape1, shape2, nbMaxManifolds);
|
OverlappingPair* newPair = ETKNEW(OverlappingPair, shape1, shape2, nbMaxManifolds);
|
||||||
assert(newPair != null);
|
assert(newPair != null);
|
||||||
this.overlappingPairs.set(pairID, newPair);
|
this.overlappingPairs.set(pairID, newPair);
|
||||||
// Wake up the two bodies
|
// Wake up the two bodies
|
||||||
shape1->getBody()->setIsSleeping(false);
|
shape1.getBody().setIsSleeping(false);
|
||||||
shape2->getBody()->setIsSleeping(false);
|
shape2.getBody().setIsSleeping(false);
|
||||||
}
|
}
|
||||||
|
|
||||||
void CollisionDetection::removeProxyCollisionShape(ProxyShape* proxyShape) {
|
void CollisionDetection::removeProxyCollisionShape(ProxyShape* proxyShape) {
|
||||||
// Remove all the overlapping pairs involving this proxy shape
|
// Remove all the overlapping pairs involving this proxy shape
|
||||||
etk::Map<overlappingpairid, OverlappingPair*>::Iterator it;
|
Map<overlappingpairid, OverlappingPair*>::Iterator it;
|
||||||
for (it = this.overlappingPairs.begin(); it != this.overlappingPairs.end(); ) {
|
for (it = this.overlappingPairs.begin(); it != this.overlappingPairs.end(); ) {
|
||||||
if (it->second->getShape1()->this.broadPhaseID == proxyShape->this.broadPhaseID||
|
if (it.second.getShape1().this.broadPhaseID == proxyShape.this.broadPhaseID||
|
||||||
it->second->getShape2()->this.broadPhaseID == proxyShape->this.broadPhaseID) {
|
it.second.getShape2().this.broadPhaseID == proxyShape.this.broadPhaseID) {
|
||||||
// TODO : Remove all the contact manifold of the overlapping pair from the contact manifolds list of the two bodies involved
|
// TODO : Remove all the contact manifold of the overlapping pair from the contact manifolds list of the two bodies involved
|
||||||
// Destroy the overlapping pair
|
// Destroy the overlapping pair
|
||||||
ETKDELETE(OverlappingPair, it->second);
|
ETKDELETE(OverlappingPair, it.second);
|
||||||
it->second = null;
|
it.second = null;
|
||||||
it = this.overlappingPairs.erase(it);
|
it = this.overlappingPairs.erase(it);
|
||||||
} else {
|
} else {
|
||||||
++it;
|
++it;
|
||||||
@ -323,17 +323,17 @@ void CollisionDetection::removeProxyCollisionShape(ProxyShape* proxyShape) {
|
|||||||
|
|
||||||
void CollisionDetection::notifyContact(OverlappingPair* overlappingPair, ContactPointInfo contactInfo) {
|
void CollisionDetection::notifyContact(OverlappingPair* overlappingPair, ContactPointInfo contactInfo) {
|
||||||
// If it is the first contact since the pairs are overlapping
|
// If it is the first contact since the pairs are overlapping
|
||||||
if (overlappingPair->getNbContactPoints() == 0) {
|
if (overlappingPair.getNbContactPoints() == 0) {
|
||||||
// Trigger a callback event
|
// Trigger a callback event
|
||||||
if (this.world->this.eventListener != NULL) {
|
if (this.world.this.eventListener != NULL) {
|
||||||
this.world->this.eventListener->beginContact(contactInfo);
|
this.world.this.eventListener.beginContact(contactInfo);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
// Create a new contact
|
// Create a new contact
|
||||||
createContact(overlappingPair, contactInfo);
|
createContact(overlappingPair, contactInfo);
|
||||||
// Trigger a callback event for the new contact
|
// Trigger a callback event for the new contact
|
||||||
if (this.world->this.eventListener != NULL) {
|
if (this.world.this.eventListener != NULL) {
|
||||||
this.world->this.eventListener->newContact(contactInfo);
|
this.world.this.eventListener.newContact(contactInfo);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -341,46 +341,46 @@ void CollisionDetection::createContact(OverlappingPair* overlappingPair, Contac
|
|||||||
// Create a new contact
|
// Create a new contact
|
||||||
ContactPoint* contact = ETKNEW(ContactPoint, contactInfo);
|
ContactPoint* contact = ETKNEW(ContactPoint, contactInfo);
|
||||||
// Add the contact to the contact manifold set of the corresponding overlapping pair
|
// Add the contact to the contact manifold set of the corresponding overlapping pair
|
||||||
overlappingPair->addContact(contact);
|
overlappingPair.addContact(contact);
|
||||||
// Add the overlapping pair into the set of pairs in contact during narrow-phase
|
// Add the overlapping pair into the set of pairs in contact during narrow-phase
|
||||||
overlappingpairid pairId = OverlappingPair::computeID(overlappingPair->getShape1(),
|
overlappingpairid pairId = OverlappingPair::computeID(overlappingPair.getShape1(),
|
||||||
overlappingPair->getShape2());
|
overlappingPair.getShape2());
|
||||||
this.contactOverlappingPairs.set(pairId, overlappingPair);
|
this.contactOverlappingPairs.set(pairId, overlappingPair);
|
||||||
}
|
}
|
||||||
|
|
||||||
void CollisionDetection::addAllContactManifoldsToBodies() {
|
void CollisionDetection::addAllContactManifoldsToBodies() {
|
||||||
// For each overlapping pairs in contact during the narrow-phase
|
// For each overlapping pairs in contact during the narrow-phase
|
||||||
etk::Map<overlappingpairid, OverlappingPair*>::Iterator it;
|
Map<overlappingpairid, OverlappingPair*>::Iterator it;
|
||||||
for (it = this.contactOverlappingPairs.begin(); it != this.contactOverlappingPairs.end(); ++it) {
|
for (it = this.contactOverlappingPairs.begin(); it != this.contactOverlappingPairs.end(); ++it) {
|
||||||
// Add all the contact manifolds of the pair into the list of contact manifolds
|
// Add all the contact manifolds of the pair into the list of contact manifolds
|
||||||
// of the two bodies involved in the contact
|
// of the two bodies involved in the contact
|
||||||
addContactManifoldToBody(it->second);
|
addContactManifoldToBody(it.second);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void CollisionDetection::addContactManifoldToBody(OverlappingPair* pair) {
|
void CollisionDetection::addContactManifoldToBody(OverlappingPair* pair) {
|
||||||
assert(pair != null);
|
assert(pair != null);
|
||||||
CollisionBody* body1 = pair->getShape1()->getBody();
|
CollisionBody* body1 = pair.getShape1().getBody();
|
||||||
CollisionBody* body2 = pair->getShape2()->getBody();
|
CollisionBody* body2 = pair.getShape2().getBody();
|
||||||
ContactManifoldSet manifoldSet = pair->getContactManifoldSet();
|
ContactManifoldSet manifoldSet = pair.getContactManifoldSet();
|
||||||
// For each contact manifold in the set of manifolds in the pair
|
// For each contact manifold in the set of manifolds in the pair
|
||||||
for (int i=0; i<manifoldSet.getNbContactManifolds(); i++) {
|
for (int i=0; i<manifoldSet.getNbContactManifolds(); i++) {
|
||||||
ContactManifold* contactManifold = manifoldSet.getContactManifold(i);
|
ContactManifold* contactManifold = manifoldSet.getContactManifold(i);
|
||||||
assert(contactManifold->getNbContactPoints() > 0);
|
assert(contactManifold.getNbContactPoints() > 0);
|
||||||
// Add the contact manifold at the beginning of the linked
|
// Add the contact manifold at the beginning of the linked
|
||||||
// list of contact manifolds of the first body
|
// list of contact manifolds of the first body
|
||||||
body1->this.contactManifoldsList = ETKNEW(ContactManifoldListElement, contactManifold, body1->this.contactManifoldsList);;
|
body1.this.contactManifoldsList = ETKNEW(ContactManifoldListElement, contactManifold, body1.this.contactManifoldsList);;
|
||||||
// Add the contact manifold at the beginning of the linked
|
// Add the contact manifold at the beginning of the linked
|
||||||
// list of the contact manifolds of the second body
|
// list of the contact manifolds of the second body
|
||||||
body2->this.contactManifoldsList = ETKNEW(ContactManifoldListElement, contactManifold, body2->this.contactManifoldsList);;
|
body2.this.contactManifoldsList = ETKNEW(ContactManifoldListElement, contactManifold, body2.this.contactManifoldsList);;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void CollisionDetection::clearContactPoints() {
|
void CollisionDetection::clearContactPoints() {
|
||||||
// For each overlapping pair
|
// For each overlapping pair
|
||||||
etk::Map<overlappingpairid, OverlappingPair*>::Iterator it;
|
Map<overlappingpairid, OverlappingPair*>::Iterator it;
|
||||||
for (it = this.overlappingPairs.begin(); it != this.overlappingPairs.end(); ++it) {
|
for (it = this.overlappingPairs.begin(); it != this.overlappingPairs.end(); ++it) {
|
||||||
it->second->clearContactPoints();
|
it.second.clearContactPoints();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -388,17 +388,17 @@ void CollisionDetection::fillInCollisionMatrix() {
|
|||||||
// For each possible type of collision shape
|
// For each possible type of collision shape
|
||||||
for (int i=0; i<NBCOLLISIONSHAPETYPES; i++) {
|
for (int i=0; i<NBCOLLISIONSHAPETYPES; i++) {
|
||||||
for (int j=0; j<NBCOLLISIONSHAPETYPES; j++) {
|
for (int j=0; j<NBCOLLISIONSHAPETYPES; j++) {
|
||||||
this.collisionMatrix[i][j] = this.collisionDispatch->selectAlgorithm(i, j);
|
this.collisionMatrix[i][j] = this.collisionDispatch.selectAlgorithm(i, j);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
EventListener* CollisionDetection::getWorldEventListener() {
|
EventListener* CollisionDetection::getWorldEventListener() {
|
||||||
return this.world->this.eventListener;
|
return this.world.this.eventListener;
|
||||||
}
|
}
|
||||||
|
|
||||||
void TestCollisionBetweenShapesCallback::notifyContact(OverlappingPair* overlappingPair, ContactPointInfo contactInfo) {
|
void TestCollisionBetweenShapesCallback::notifyContact(OverlappingPair* overlappingPair, ContactPointInfo contactInfo) {
|
||||||
this.collisionCallback->notifyContact(contactInfo);
|
this.collisionCallback.notifyContact(contactInfo);
|
||||||
}
|
}
|
||||||
|
|
||||||
NarrowPhaseAlgorithm* CollisionDetection::getCollisionAlgorithm(CollisionShapeType shape1Type, CollisionShapeType shape2Type) {
|
NarrowPhaseAlgorithm* CollisionDetection::getCollisionAlgorithm(CollisionShapeType shape1Type, CollisionShapeType shape2Type) {
|
||||||
@ -407,7 +407,7 @@ NarrowPhaseAlgorithm* CollisionDetection::getCollisionAlgorithm(CollisionShapeTy
|
|||||||
|
|
||||||
void CollisionDetection::setCollisionDispatch(CollisionDispatch* collisionDispatch) {
|
void CollisionDetection::setCollisionDispatch(CollisionDispatch* collisionDispatch) {
|
||||||
this.collisionDispatch = collisionDispatch;
|
this.collisionDispatch = collisionDispatch;
|
||||||
this.collisionDispatch->init(this);
|
this.collisionDispatch.init(this);
|
||||||
// Fill-in the collision matrix with the new algorithms to use
|
// Fill-in the collision matrix with the new algorithms to use
|
||||||
fillInCollisionMatrix();
|
fillInCollisionMatrix();
|
||||||
}
|
}
|
||||||
@ -427,14 +427,14 @@ void CollisionDetection::removeNoCollisionPair(CollisionBody* body1, CollisionBo
|
|||||||
}
|
}
|
||||||
|
|
||||||
void CollisionDetection::askForBroadPhaseCollisionCheck(ProxyShape* shape) {
|
void CollisionDetection::askForBroadPhaseCollisionCheck(ProxyShape* shape) {
|
||||||
this.broadPhaseAlgorithm.addMovedCollisionShape(shape->this.broadPhaseID);
|
this.broadPhaseAlgorithm.addMovedCollisionShape(shape.this.broadPhaseID);
|
||||||
}
|
}
|
||||||
|
|
||||||
void CollisionDetection::updateProxyCollisionShape(ProxyShape* shape, AABB aabb, vec3 displacement, boolean forceReinsert) {
|
void CollisionDetection::updateProxyCollisionShape(ProxyShape* shape, AABB aabb, Vector3f displacement, boolean forceReinsert) {
|
||||||
this.broadPhaseAlgorithm.updateProxyCollisionShape(shape, aabb, displacement);
|
this.broadPhaseAlgorithm.updateProxyCollisionShape(shape, aabb, displacement);
|
||||||
}
|
}
|
||||||
|
|
||||||
void CollisionDetection::raycast(RaycastCallback* raycastCallback, Ray ray, unsigned short raycastWithCategoryMaskBits) {
|
void CollisionDetection::raycast(RaycastCallback* raycastCallback, Ray ray, int raycastWithCategoryMaskBits) {
|
||||||
PROFILE("CollisionDetection::raycast()");
|
PROFILE("CollisionDetection::raycast()");
|
||||||
RaycastTest rayCastTest(raycastCallback);
|
RaycastTest rayCastTest(raycastCallback);
|
||||||
// Ask the broad-phase algorithm to call the testRaycastAgainstShape()
|
// Ask the broad-phase algorithm to call the testRaycastAgainstShape()
|
||||||
@ -444,8 +444,8 @@ void CollisionDetection::raycast(RaycastCallback* raycastCallback, Ray ray, uns
|
|||||||
|
|
||||||
boolean CollisionDetection::testAABBOverlap( ProxyShape* shape1, ProxyShape* shape2) {
|
boolean CollisionDetection::testAABBOverlap( ProxyShape* shape1, ProxyShape* shape2) {
|
||||||
// If one of the shape's body is not active, we return no overlap
|
// If one of the shape's body is not active, we return no overlap
|
||||||
if ( !shape1->getBody()->isActive()
|
if ( !shape1.getBody().isActive()
|
||||||
|| !shape2->getBody()->isActive()) {
|
|| !shape2.getBody().isActive()) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
return this.broadPhaseAlgorithm.testOverlappingShapes(shape1, shape2);
|
return this.broadPhaseAlgorithm.testOverlappingShapes(shape1, shape2);
|
||||||
|
@ -1,32 +1,9 @@
|
|||||||
/** @file
|
package org.atriaSoft.ephysics.collision;
|
||||||
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
|
|
||||||
* @author Daniel CHAPPUIS
|
|
||||||
* @author Edouard DUPIN
|
|
||||||
* @copyright 2010-2016, Daniel Chappuis
|
|
||||||
* @copyright 2017, Edouard DUPIN
|
|
||||||
* @license MPL v2.0 (see license file)
|
|
||||||
*/
|
|
||||||
#pragma once
|
|
||||||
|
|
||||||
#include <ephysics/body/CollisionBody.hpp>
|
|
||||||
#include <ephysics/collision/broadphase/BroadPhaseAlgorithm.hpp>
|
|
||||||
#include <ephysics/engine/OverlappingPair.hpp>
|
|
||||||
#include <ephysics/engine/EventListener.hpp>
|
|
||||||
#include <ephysics/collision/narrowphase/DefaultCollisionDispatch.hpp>
|
|
||||||
#include <ephysics/raint/ContactPoint.hpp>
|
|
||||||
#include <etk/Vector.hpp>
|
|
||||||
#include <etk/Map.hpp>
|
|
||||||
#include <etk/Set.hpp>
|
|
||||||
|
|
||||||
namespace ephysics {
|
|
||||||
|
|
||||||
class BroadPhaseAlgorithm;
|
|
||||||
class CollisionWorld;
|
|
||||||
class CollisionCallback;
|
|
||||||
|
|
||||||
class TestCollisionBetweenShapesCallback : public NarrowPhaseCallback {
|
class TestCollisionBetweenShapesCallback extends NarrowPhaseCallback {
|
||||||
private:
|
private:
|
||||||
CollisionCallback* this.collisionCallback; //!<
|
CollisionCallback* collisionCallback; //!<
|
||||||
public:
|
public:
|
||||||
// Constructor
|
// Constructor
|
||||||
TestCollisionBetweenShapesCallback(CollisionCallback* callback):
|
TestCollisionBetweenShapesCallback(CollisionCallback* callback):
|
||||||
@ -34,7 +11,7 @@ namespace ephysics {
|
|||||||
|
|
||||||
}
|
}
|
||||||
// Called by a narrow-phase collision algorithm when a new contact has been found
|
// Called by a narrow-phase collision algorithm when a new contact has been found
|
||||||
virtual void notifyContact(OverlappingPair* overlappingPair,
|
void notifyContact(OverlappingPair* overlappingPair,
|
||||||
ContactPointInfo contactInfo);
|
ContactPointInfo contactInfo);
|
||||||
};
|
};
|
||||||
|
|
||||||
@ -44,23 +21,19 @@ namespace ephysics {
|
|||||||
* collide and then we run a narrow-phase algorithm to compute the
|
* collide and then we run a narrow-phase algorithm to compute the
|
||||||
* collision contacts between bodies.
|
* collision contacts between bodies.
|
||||||
*/
|
*/
|
||||||
class CollisionDetection : public NarrowPhaseCallback {
|
class CollisionDetection extends NarrowPhaseCallback {
|
||||||
private :
|
private :
|
||||||
CollisionDispatch* this.collisionDispatch; //!< Collision Detection Dispatch configuration
|
CollisionDispatch* collisionDispatch; //!< Collision Detection Dispatch configuration
|
||||||
DefaultCollisionDispatch this.defaultCollisionDispatch; //!< Default collision dispatch configuration
|
DefaultCollisionDispatch defaultCollisionDispatch; //!< Default collision dispatch configuration
|
||||||
NarrowPhaseAlgorithm* this.collisionMatrix[NBCOLLISIONSHAPETYPES][NBCOLLISIONSHAPETYPES]; //!< Collision detection matrix (algorithms to use)
|
NarrowPhaseAlgorithm* collisionMatrix[NBCOLLISIONSHAPETYPES][NBCOLLISIONSHAPETYPES]; //!< Collision detection matrix (algorithms to use)
|
||||||
CollisionWorld* this.world; //!< Pointer to the physics world
|
CollisionWorld* world; //!< Pointer to the physics world
|
||||||
etk::Map<overlappingpairid, OverlappingPair*> this.overlappingPairs; //!< Broad-phase overlapping pairs
|
Map<overlappingpairid, OverlappingPair*> overlappingPairs; //!< Broad-phase overlapping pairs
|
||||||
etk::Map<overlappingpairid, OverlappingPair*> this.contactOverlappingPairs; //!< Overlapping pairs in contact (during the current Narrow-phase collision detection)
|
Map<overlappingpairid, OverlappingPair*> contactOverlappingPairs; //!< Overlapping pairs in contact (during the current Narrow-phase collision detection)
|
||||||
BroadPhaseAlgorithm this.broadPhaseAlgorithm; //!< Broad-phase algorithm
|
BroadPhaseAlgorithm broadPhaseAlgorithm; //!< Broad-phase algorithm
|
||||||
// TODO : Delete this
|
// TODO : Delete this
|
||||||
GJKAlgorithm this.narrowPhaseGJKAlgorithm; //!< Narrow-phase GJK algorithm
|
GJKAlgorithm narrowPhaseGJKAlgorithm; //!< Narrow-phase GJK algorithm
|
||||||
etk::Set<longpair> this.noCollisionPairs; //!< Set of pair of bodies that cannot collide between each other
|
Set<longpair> noCollisionPairs; //!< Set of pair of bodies that cannot collide between each other
|
||||||
boolean this.isCollisionShapesAdded; //!< True if some collision shapes have been added previously
|
boolean isCollisionShapesAdded; //!< True if some collision shapes have been added previously
|
||||||
/// Private copy-ructor
|
|
||||||
CollisionDetection( CollisionDetection collisionDetection);
|
|
||||||
/// Private assignment operator
|
|
||||||
CollisionDetection operator=( CollisionDetection collisionDetection);
|
|
||||||
/// Compute the broad-phase collision detection
|
/// Compute the broad-phase collision detection
|
||||||
void computeBroadPhase();
|
void computeBroadPhase();
|
||||||
/// Compute the narrow-phase collision detection
|
/// Compute the narrow-phase collision detection
|
||||||
@ -77,8 +50,6 @@ namespace ephysics {
|
|||||||
public :
|
public :
|
||||||
/// Constructor
|
/// Constructor
|
||||||
CollisionDetection(CollisionWorld* world);
|
CollisionDetection(CollisionWorld* world);
|
||||||
/// Destructor
|
|
||||||
~CollisionDetection();
|
|
||||||
/// Set the collision dispatch configuration
|
/// Set the collision dispatch configuration
|
||||||
void setCollisionDispatch(CollisionDispatch* collisionDispatch);
|
void setCollisionDispatch(CollisionDispatch* collisionDispatch);
|
||||||
/// Return the Narrow-phase collision detection algorithm to use between two types of shapes
|
/// Return the Narrow-phase collision detection algorithm to use between two types of shapes
|
||||||
@ -91,7 +62,7 @@ namespace ephysics {
|
|||||||
/// Update a proxy collision shape (that has moved for instance)
|
/// Update a proxy collision shape (that has moved for instance)
|
||||||
void updateProxyCollisionShape(ProxyShape* shape,
|
void updateProxyCollisionShape(ProxyShape* shape,
|
||||||
AABB aabb,
|
AABB aabb,
|
||||||
vec3 displacement = vec3(0, 0, 0),
|
Vector3f displacement = Vector3f(0, 0, 0),
|
||||||
boolean forceReinsert = false);
|
boolean forceReinsert = false);
|
||||||
/// Add a pair of bodies that cannot collide with each other
|
/// Add a pair of bodies that cannot collide with each other
|
||||||
void addNoCollisionPair(CollisionBody* body1, CollisionBody* body2);
|
void addNoCollisionPair(CollisionBody* body1, CollisionBody* body2);
|
||||||
@ -105,16 +76,16 @@ namespace ephysics {
|
|||||||
void computeCollisionDetection();
|
void computeCollisionDetection();
|
||||||
/// Compute the collision detection
|
/// Compute the collision detection
|
||||||
void testCollisionBetweenShapes(CollisionCallback* callback,
|
void testCollisionBetweenShapes(CollisionCallback* callback,
|
||||||
etk::Set<int> shapes1,
|
Set<int> shapes1,
|
||||||
etk::Set<int> shapes2);
|
Set<int> shapes2);
|
||||||
/// Report collision between two sets of shapes
|
/// Report collision between two sets of shapes
|
||||||
void reportCollisionBetweenShapes(CollisionCallback* callback,
|
void reportCollisionBetweenShapes(CollisionCallback* callback,
|
||||||
etk::Set<int> shapes1,
|
Set<int> shapes1,
|
||||||
etk::Set<int> shapes2) ;
|
Set<int> shapes2) ;
|
||||||
/// Ray casting method
|
/// Ray casting method
|
||||||
void raycast(RaycastCallback* raycastCallback,
|
void raycast(RaycastCallback* raycastCallback,
|
||||||
Ray ray,
|
Ray ray,
|
||||||
unsigned short raycastWithCategoryMaskBits) ;
|
int raycastWithCategoryMaskBits) ;
|
||||||
/// Test if the AABBs of two bodies overlap
|
/// Test if the AABBs of two bodies overlap
|
||||||
boolean testAABBOverlap( CollisionBody* body1,
|
boolean testAABBOverlap( CollisionBody* body1,
|
||||||
CollisionBody* body2) ;
|
CollisionBody* body2) ;
|
||||||
@ -126,14 +97,14 @@ namespace ephysics {
|
|||||||
void broadPhaseNotifyOverlappingPair(ProxyShape* shape1, ProxyShape* shape2);
|
void broadPhaseNotifyOverlappingPair(ProxyShape* shape1, ProxyShape* shape2);
|
||||||
/// Compute the narrow-phase collision detection
|
/// Compute the narrow-phase collision detection
|
||||||
void computeNarrowPhaseBetweenShapes(CollisionCallback* callback,
|
void computeNarrowPhaseBetweenShapes(CollisionCallback* callback,
|
||||||
etk::Set<int> shapes1,
|
Set<int> shapes1,
|
||||||
etk::Set<int> shapes2);
|
Set<int> shapes2);
|
||||||
/// Return a pointer to the world
|
/// Return a pointer to the world
|
||||||
CollisionWorld* getWorld();
|
CollisionWorld* getWorld();
|
||||||
/// Return the world event listener
|
/// Return the world event listener
|
||||||
EventListener* getWorldEventListener();
|
EventListener* getWorldEventListener();
|
||||||
/// Called by a narrow-phase collision algorithm when a new contact has been found
|
/// Called by a narrow-phase collision algorithm when a new contact has been found
|
||||||
virtual void notifyContact(OverlappingPair* overlappingPair, ContactPointInfo contactInfo) override;
|
void notifyContact(OverlappingPair* overlappingPair, ContactPointInfo contactInfo) ;
|
||||||
/// Create a new contact
|
/// Create a new contact
|
||||||
void createContact(OverlappingPair* overlappingPair, ContactPointInfo contactInfo);
|
void createContact(OverlappingPair* overlappingPair, ContactPointInfo contactInfo);
|
||||||
friend class DynamicsWorld;
|
friend class DynamicsWorld;
|
@ -11,7 +11,6 @@
|
|||||||
#include <ephysics/collision/shapes/CollisionShape.hpp>
|
#include <ephysics/collision/shapes/CollisionShape.hpp>
|
||||||
|
|
||||||
namespace ephysics {
|
namespace ephysics {
|
||||||
class OverlappingPair;
|
|
||||||
/**
|
/**
|
||||||
* @brief It regroups different things about a collision shape. This is
|
* @brief It regroups different things about a collision shape. This is
|
||||||
* used to pass information about a collision shape to a collision algorithm.
|
* used to pass information about a collision shape to a collision algorithm.
|
||||||
@ -21,12 +20,12 @@ namespace ephysics {
|
|||||||
OverlappingPair* overlappingPair; //!< Broadphase overlapping pair
|
OverlappingPair* overlappingPair; //!< Broadphase overlapping pair
|
||||||
ProxyShape* proxyShape; //!< Proxy shape
|
ProxyShape* proxyShape; //!< Proxy shape
|
||||||
CollisionShape* collisionShape; //!< Pointer to the collision shape
|
CollisionShape* collisionShape; //!< Pointer to the collision shape
|
||||||
etk::Transform3D shapeToWorldTransform; //!< etk::Transform3D that maps from collision shape local-space to world-space
|
Transform3D shapeToWorldTransform; //!< Transform3D that maps from collision shape local-space to world-space
|
||||||
void** cachedCollisionData; //!< Cached collision data of the proxy shape
|
void** cachedCollisionData; //!< Cached collision data of the proxy shape
|
||||||
/// Constructor
|
/// Constructor
|
||||||
CollisionShapeInfo(ProxyShape* proxyCollisionShape,
|
CollisionShapeInfo(ProxyShape* proxyCollisionShape,
|
||||||
CollisionShape* shape,
|
CollisionShape* shape,
|
||||||
etk::Transform3D shapeLocalToWorldTransform,
|
Transform3D shapeLocalToWorldTransform,
|
||||||
OverlappingPair* pair,
|
OverlappingPair* pair,
|
||||||
void** cachedData):
|
void** cachedData):
|
||||||
overlappingPair(pair),
|
overlappingPair(pair),
|
||||||
|
@ -16,9 +16,7 @@
|
|||||||
namespace ephysics {
|
namespace ephysics {
|
||||||
|
|
||||||
int MAXCONTACTPOINTSINMANIFOLD = 4; //!< Maximum number of contacts in the manifold
|
int MAXCONTACTPOINTSINMANIFOLD = 4; //!< Maximum number of contacts in the manifold
|
||||||
|
|
||||||
class ContactManifold;
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @brief This structure represents a single element of a linked list of contact manifolds
|
* @brief This structure represents a single element of a linked list of contact manifolds
|
||||||
*/
|
*/
|
||||||
@ -54,25 +52,19 @@ namespace ephysics {
|
|||||||
ContactManifold(ProxyShape* shape1,
|
ContactManifold(ProxyShape* shape1,
|
||||||
ProxyShape* shape2,
|
ProxyShape* shape2,
|
||||||
int16t normalDirectionId);
|
int16t normalDirectionId);
|
||||||
/// Destructor
|
|
||||||
~ContactManifold();
|
|
||||||
/// DELETE copy-ructor
|
|
||||||
ContactManifold( ContactManifold contactManifold) = delete;
|
|
||||||
/// DELETE assignment operator
|
|
||||||
ContactManifold operator=( ContactManifold contactManifold) = delete;
|
|
||||||
private:
|
private:
|
||||||
ProxyShape* this.shape1; //!< Pointer to the first proxy shape of the contact
|
ProxyShape* shape1; //!< Pointer to the first proxy shape of the contact
|
||||||
ProxyShape* this.shape2; //!< Pointer to the second proxy shape of the contact
|
ProxyShape* shape2; //!< Pointer to the second proxy shape of the contact
|
||||||
ContactPoint* this.contactPoints[MAXCONTACTPOINTSINMANIFOLD]; //!< Contact points in the manifold
|
ContactPoint* contactPoints[MAXCONTACTPOINTSINMANIFOLD]; //!< Contact points in the manifold
|
||||||
int16t this.normalDirectionId; //!< Normal direction Id (Unique Id representing the normal direction)
|
int16t normalDirectionId; //!< Normal direction Id (Unique Id representing the normal direction)
|
||||||
int this.nbContactPoints; //!< Number of contacts in the cache
|
int nbContactPoints; //!< Number of contacts in the cache
|
||||||
vec3 this.frictionVector1; //!< First friction vector of the contact manifold
|
Vector3f frictionVector1; //!< First friction vector of the contact manifold
|
||||||
vec3 this.frictionvec2; //!< Second friction vector of the contact manifold
|
Vector3f frictionvec2; //!< Second friction vector of the contact manifold
|
||||||
float this.frictionImpulse1; //!< First friction raint accumulated impulse
|
float frictionImpulse1; //!< First friction raint accumulated impulse
|
||||||
float this.frictionImpulse2; //!< Second friction raint accumulated impulse
|
float frictionImpulse2; //!< Second friction raint accumulated impulse
|
||||||
float this.frictionTwistImpulse; //!< Twist friction raint accumulated impulse
|
float frictionTwistImpulse; //!< Twist friction raint accumulated impulse
|
||||||
vec3 this.rollingResistanceImpulse; //!< Accumulated rolling resistance impulse
|
Vector3f rollingResistanceImpulse; //!< Accumulated rolling resistance impulse
|
||||||
boolean this.isAlreadyInIsland; //!< True if the contact manifold has already been added into an island
|
boolean isAlreadyInIsland; //!< True if the contact manifold has already been added into an island
|
||||||
/// Return the index of maximum area
|
/// Return the index of maximum area
|
||||||
int getMaxArea(float area0, float area1, float area2, float area3) ;
|
int getMaxArea(float area0, float area1, float area2, float area3) ;
|
||||||
/**
|
/**
|
||||||
@ -94,7 +86,7 @@ namespace ephysics {
|
|||||||
* this is only a guess that is faster to compute. This idea comes from the Bullet Physics library
|
* this is only a guess that is faster to compute. This idea comes from the Bullet Physics library
|
||||||
* by Erwin Coumans (http://wwww.bulletphysics.org).
|
* by Erwin Coumans (http://wwww.bulletphysics.org).
|
||||||
*/
|
*/
|
||||||
int getIndexToRemove(int indexMaxPenetration, vec3 newPoint) ;
|
int getIndexToRemove(int indexMaxPenetration, Vector3f newPoint) ;
|
||||||
/// Remove a contact point from the manifold
|
/// Remove a contact point from the manifold
|
||||||
void removeContactPoint(int index);
|
void removeContactPoint(int index);
|
||||||
/// Return true if the contact manifold has already been added into an island
|
/// Return true if the contact manifold has already been added into an island
|
||||||
@ -121,20 +113,20 @@ namespace ephysics {
|
|||||||
* the contacts with a too large distance between the contact points in the plane orthogonal to the
|
* the contacts with a too large distance between the contact points in the plane orthogonal to the
|
||||||
* contact normal.
|
* contact normal.
|
||||||
*/
|
*/
|
||||||
void update( etk::Transform3D transform1,
|
void update( Transform3D transform1,
|
||||||
etk::Transform3D transform2);
|
Transform3D transform2);
|
||||||
/// Clear the contact manifold
|
/// Clear the contact manifold
|
||||||
void clear();
|
void clear();
|
||||||
/// Return the number of contact points in the manifold
|
/// Return the number of contact points in the manifold
|
||||||
int getNbContactPoints() ;
|
int getNbContactPoints() ;
|
||||||
/// Return the first friction vector at the center of the contact manifold
|
/// Return the first friction vector at the center of the contact manifold
|
||||||
vec3 getFrictionVector1() ;
|
Vector3f getFrictionVector1() ;
|
||||||
/// set the first friction vector at the center of the contact manifold
|
/// set the first friction vector at the center of the contact manifold
|
||||||
void setFrictionVector1( vec3 frictionVector1);
|
void setFrictionVector1( Vector3f frictionVector1);
|
||||||
/// Return the second friction vector at the center of the contact manifold
|
/// Return the second friction vector at the center of the contact manifold
|
||||||
vec3 getFrictionvec2() ;
|
Vector3f getFrictionvec2() ;
|
||||||
/// set the second friction vector at the center of the contact manifold
|
/// set the second friction vector at the center of the contact manifold
|
||||||
void setFrictionvec2( vec3 frictionvec2);
|
void setFrictionvec2( Vector3f frictionvec2);
|
||||||
/// Return the first friction accumulated impulse
|
/// Return the first friction accumulated impulse
|
||||||
float getFrictionImpulse1() ;
|
float getFrictionImpulse1() ;
|
||||||
/// Set the first friction accumulated impulse
|
/// Set the first friction accumulated impulse
|
||||||
@ -148,16 +140,13 @@ namespace ephysics {
|
|||||||
/// Set the friction twist accumulated impulse
|
/// Set the friction twist accumulated impulse
|
||||||
void setFrictionTwistImpulse(float frictionTwistImpulse);
|
void setFrictionTwistImpulse(float frictionTwistImpulse);
|
||||||
/// Set the accumulated rolling resistance impulse
|
/// Set the accumulated rolling resistance impulse
|
||||||
void setRollingResistanceImpulse( vec3 rollingResistanceImpulse);
|
void setRollingResistanceImpulse( Vector3f rollingResistanceImpulse);
|
||||||
/// Return a contact point of the manifold
|
/// Return a contact point of the manifold
|
||||||
ContactPoint* getContactPoint(int index) ;
|
ContactPoint* getContactPoint(int index) ;
|
||||||
/// Return the normalized averaged normal vector
|
/// Return the normalized averaged normal vector
|
||||||
vec3 getAverageContactNormal() ;
|
Vector3f getAverageContactNormal() ;
|
||||||
/// Return the largest depth of all the contact points
|
/// Return the largest depth of all the contact points
|
||||||
float getLargestContactDepth() ;
|
float getLargestContactDepth();
|
||||||
friend class DynamicsWorld;
|
|
||||||
friend class Island;
|
|
||||||
friend class CollisionBody;
|
|
||||||
};
|
};
|
||||||
|
|
||||||
}
|
}
|
||||||
|
@ -1,18 +1,8 @@
|
|||||||
/** @file
|
package org.atriaSoft.ephysics.collision;
|
||||||
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
|
|
||||||
* @author Daniel CHAPPUIS
|
|
||||||
* @author Edouard DUPIN
|
|
||||||
* @copyright 2010-2016, Daniel Chappuis
|
|
||||||
* @copyright 2017, Edouard DUPIN
|
|
||||||
* @license MPL v2.0 (see license file)
|
|
||||||
*/
|
|
||||||
#include <ephysics/collision/ContactManifold.hpp>
|
|
||||||
|
|
||||||
using namespace ephysics;
|
|
||||||
|
|
||||||
ContactManifold::ContactManifold(ProxyShape* shape1,
|
ContactManifold::ContactManifold(ProxyShape* shape1,
|
||||||
ProxyShape* shape2,
|
ProxyShape* shape2,
|
||||||
short normalDirectionId):
|
int normalDirectionId):
|
||||||
this.shape1(shape1),
|
this.shape1(shape1),
|
||||||
this.shape2(shape2),
|
this.shape2(shape2),
|
||||||
this.normalDirectionId(normalDirectionId),
|
this.normalDirectionId(normalDirectionId),
|
||||||
@ -33,7 +23,7 @@ void ContactManifold::addContactPoint(ContactPoint* contact) {
|
|||||||
for (int i=0; i<this.nbContactPoints; i++) {
|
for (int i=0; i<this.nbContactPoints; i++) {
|
||||||
// Check if the new point point does not correspond to a same contact point
|
// Check if the new point point does not correspond to a same contact point
|
||||||
// already in the manifold.
|
// already in the manifold.
|
||||||
float distance = (this.contactPoints[i]->getWorldPointOnBody1() - contact->getWorldPointOnBody1()).length2();
|
float distance = (this.contactPoints[i].getWorldPointOnBody1() - contact.getWorldPointOnBody1()).length2();
|
||||||
if (distance <= PERSISTENTCONTACTDISTTHRESHOLD*PERSISTENTCONTACTDISTTHRESHOLD) {
|
if (distance <= PERSISTENTCONTACTDISTTHRESHOLD*PERSISTENTCONTACTDISTTHRESHOLD) {
|
||||||
// Delete the new contact
|
// Delete the new contact
|
||||||
ETKDELETE(ContactPoint, contact);
|
ETKDELETE(ContactPoint, contact);
|
||||||
@ -44,7 +34,7 @@ void ContactManifold::addContactPoint(ContactPoint* contact) {
|
|||||||
// If the contact manifold is full
|
// If the contact manifold is full
|
||||||
if (this.nbContactPoints == MAXCONTACTPOINTSINMANIFOLD) {
|
if (this.nbContactPoints == MAXCONTACTPOINTSINMANIFOLD) {
|
||||||
int indexMaxPenetration = getIndexOfDeepestPenetration(contact);
|
int indexMaxPenetration = getIndexOfDeepestPenetration(contact);
|
||||||
int indexToRemove = getIndexToRemove(indexMaxPenetration, contact->getLocalPointOnBody1());
|
int indexToRemove = getIndexToRemove(indexMaxPenetration, contact.getLocalPointOnBody1());
|
||||||
removeContactPoint(indexToRemove);
|
removeContactPoint(indexToRemove);
|
||||||
}
|
}
|
||||||
// Add the new contact point in the manifold
|
// Add the new contact point in the manifold
|
||||||
@ -67,30 +57,30 @@ void ContactManifold::removeContactPoint(int index) {
|
|||||||
this.nbContactPoints--;
|
this.nbContactPoints--;
|
||||||
}
|
}
|
||||||
|
|
||||||
void ContactManifold::update( etk::Transform3D transform1, etk::Transform3D transform2) {
|
void ContactManifold::update( Transform3D transform1, Transform3D transform2) {
|
||||||
if (this.nbContactPoints == 0) {
|
if (this.nbContactPoints == 0) {
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
// Update the world coordinates and penetration depth of the contact points in the manifold
|
// Update the world coordinates and penetration depth of the contact points in the manifold
|
||||||
for (int i=0; i<this.nbContactPoints; i++) {
|
for (int i=0; i<this.nbContactPoints; i++) {
|
||||||
this.contactPoints[i]->setWorldPointOnBody1(transform1 * this.contactPoints[i]->getLocalPointOnBody1());
|
this.contactPoints[i].setWorldPointOnBody1(transform1 * this.contactPoints[i].getLocalPointOnBody1());
|
||||||
this.contactPoints[i]->setWorldPointOnBody2(transform2 * this.contactPoints[i]->getLocalPointOnBody2());
|
this.contactPoints[i].setWorldPointOnBody2(transform2 * this.contactPoints[i].getLocalPointOnBody2());
|
||||||
this.contactPoints[i]->setPenetrationDepth((this.contactPoints[i]->getWorldPointOnBody1() - this.contactPoints[i]->getWorldPointOnBody2()).dot(this.contactPoints[i]->getNormal()));
|
this.contactPoints[i].setPenetrationDepth((this.contactPoints[i].getWorldPointOnBody1() - this.contactPoints[i].getWorldPointOnBody2()).dot(this.contactPoints[i].getNormal()));
|
||||||
}
|
}
|
||||||
float squarePersistentContactThreshold = PERSISTENTCONTACTDISTTHRESHOLD * PERSISTENTCONTACTDISTTHRESHOLD;
|
float squarePersistentContactThreshold = PERSISTENTCONTACTDISTTHRESHOLD * PERSISTENTCONTACTDISTTHRESHOLD;
|
||||||
// Remove the contact points that don't represent very well the contact manifold
|
// Remove the contact points that don't represent very well the contact manifold
|
||||||
for (int i=staticcast<int>(this.nbContactPoints)-1; i>=0; i--) {
|
for (int i=staticcast<int>(this.nbContactPoints)-1; i>=0; i--) {
|
||||||
assert(i < staticcast<int>(this.nbContactPoints));
|
assert(i < staticcast<int>(this.nbContactPoints));
|
||||||
// Compute the distance between contact points in the normal direction
|
// Compute the distance between contact points in the normal direction
|
||||||
float distanceNormal = -this.contactPoints[i]->getPenetrationDepth();
|
float distanceNormal = -this.contactPoints[i].getPenetrationDepth();
|
||||||
// If the contacts points are too far from each other in the normal direction
|
// If the contacts points are too far from each other in the normal direction
|
||||||
if (distanceNormal > squarePersistentContactThreshold) {
|
if (distanceNormal > squarePersistentContactThreshold) {
|
||||||
removeContactPoint(i);
|
removeContactPoint(i);
|
||||||
} else {
|
} else {
|
||||||
// Compute the distance of the two contact points in the plane
|
// Compute the distance of the two contact points in the plane
|
||||||
// orthogonal to the contact normal
|
// orthogonal to the contact normal
|
||||||
vec3 projOfPoint1 = this.contactPoints[i]->getWorldPointOnBody1() + this.contactPoints[i]->getNormal() * distanceNormal;
|
Vector3f projOfPoint1 = this.contactPoints[i].getWorldPointOnBody1() + this.contactPoints[i].getNormal() * distanceNormal;
|
||||||
vec3 projDifference = this.contactPoints[i]->getWorldPointOnBody2() - projOfPoint1;
|
Vector3f projDifference = this.contactPoints[i].getWorldPointOnBody2() - projOfPoint1;
|
||||||
// If the orthogonal distance is larger than the valid distance
|
// If the orthogonal distance is larger than the valid distance
|
||||||
// threshold, we remove the contact
|
// threshold, we remove the contact
|
||||||
if (projDifference.length2() > squarePersistentContactThreshold) {
|
if (projDifference.length2() > squarePersistentContactThreshold) {
|
||||||
@ -103,12 +93,12 @@ void ContactManifold::update( etk::Transform3D transform1, etk::Transform3D tra
|
|||||||
int ContactManifold::getIndexOfDeepestPenetration(ContactPoint* newContact) {
|
int ContactManifold::getIndexOfDeepestPenetration(ContactPoint* newContact) {
|
||||||
assert(this.nbContactPoints == MAXCONTACTPOINTSINMANIFOLD);
|
assert(this.nbContactPoints == MAXCONTACTPOINTSINMANIFOLD);
|
||||||
int indexMaxPenetrationDepth = -1;
|
int indexMaxPenetrationDepth = -1;
|
||||||
float maxPenetrationDepth = newContact->getPenetrationDepth();
|
float maxPenetrationDepth = newContact.getPenetrationDepth();
|
||||||
// For each contact in the cache
|
// For each contact in the cache
|
||||||
for (int i=0; i<this.nbContactPoints; i++) {
|
for (int i=0; i<this.nbContactPoints; i++) {
|
||||||
// If the current contact has a larger penetration depth
|
// If the current contact has a larger penetration depth
|
||||||
if (this.contactPoints[i]->getPenetrationDepth() > maxPenetrationDepth) {
|
if (this.contactPoints[i].getPenetrationDepth() > maxPenetrationDepth) {
|
||||||
maxPenetrationDepth = this.contactPoints[i]->getPenetrationDepth();
|
maxPenetrationDepth = this.contactPoints[i].getPenetrationDepth();
|
||||||
indexMaxPenetrationDepth = i;
|
indexMaxPenetrationDepth = i;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@ -116,7 +106,7 @@ int ContactManifold::getIndexOfDeepestPenetration(ContactPoint* newContact) {
|
|||||||
return indexMaxPenetrationDepth;
|
return indexMaxPenetrationDepth;
|
||||||
}
|
}
|
||||||
|
|
||||||
int ContactManifold::getIndexToRemove(int indexMaxPenetration, vec3 newPoint) {
|
int ContactManifold::getIndexToRemove(int indexMaxPenetration, Vector3f newPoint) {
|
||||||
assert(this.nbContactPoints == MAXCONTACTPOINTSINMANIFOLD);
|
assert(this.nbContactPoints == MAXCONTACTPOINTSINMANIFOLD);
|
||||||
float area0 = 0.0f; // Area with contact 1,2,3 and newPoint
|
float area0 = 0.0f; // Area with contact 1,2,3 and newPoint
|
||||||
float area1 = 0.0f; // Area with contact 0,2,3 and newPoint
|
float area1 = 0.0f; // Area with contact 0,2,3 and newPoint
|
||||||
@ -124,30 +114,30 @@ int ContactManifold::getIndexToRemove(int indexMaxPenetration, vec3 newPoint)
|
|||||||
float area3 = 0.0f; // Area with contact 0,1,2 and newPoint
|
float area3 = 0.0f; // Area with contact 0,1,2 and newPoint
|
||||||
if (indexMaxPenetration != 0) {
|
if (indexMaxPenetration != 0) {
|
||||||
// Compute the area
|
// Compute the area
|
||||||
vec3 vector1 = newPoint - this.contactPoints[1]->getLocalPointOnBody1();
|
Vector3f vector1 = newPoint - this.contactPoints[1].getLocalPointOnBody1();
|
||||||
vec3 vector2 = this.contactPoints[3]->getLocalPointOnBody1() - this.contactPoints[2]->getLocalPointOnBody1();
|
Vector3f vector2 = this.contactPoints[3].getLocalPointOnBody1() - this.contactPoints[2].getLocalPointOnBody1();
|
||||||
vec3 crossProduct = vector1.cross(vector2);
|
Vector3f crossProduct = vector1.cross(vector2);
|
||||||
area0 = crossProduct.length2();
|
area0 = crossProduct.length2();
|
||||||
}
|
}
|
||||||
if (indexMaxPenetration != 1) {
|
if (indexMaxPenetration != 1) {
|
||||||
// Compute the area
|
// Compute the area
|
||||||
vec3 vector1 = newPoint - this.contactPoints[0]->getLocalPointOnBody1();
|
Vector3f vector1 = newPoint - this.contactPoints[0].getLocalPointOnBody1();
|
||||||
vec3 vector2 = this.contactPoints[3]->getLocalPointOnBody1() - this.contactPoints[2]->getLocalPointOnBody1();
|
Vector3f vector2 = this.contactPoints[3].getLocalPointOnBody1() - this.contactPoints[2].getLocalPointOnBody1();
|
||||||
vec3 crossProduct = vector1.cross(vector2);
|
Vector3f crossProduct = vector1.cross(vector2);
|
||||||
area1 = crossProduct.length2();
|
area1 = crossProduct.length2();
|
||||||
}
|
}
|
||||||
if (indexMaxPenetration != 2) {
|
if (indexMaxPenetration != 2) {
|
||||||
// Compute the area
|
// Compute the area
|
||||||
vec3 vector1 = newPoint - this.contactPoints[0]->getLocalPointOnBody1();
|
Vector3f vector1 = newPoint - this.contactPoints[0].getLocalPointOnBody1();
|
||||||
vec3 vector2 = this.contactPoints[3]->getLocalPointOnBody1() - this.contactPoints[1]->getLocalPointOnBody1();
|
Vector3f vector2 = this.contactPoints[3].getLocalPointOnBody1() - this.contactPoints[1].getLocalPointOnBody1();
|
||||||
vec3 crossProduct = vector1.cross(vector2);
|
Vector3f crossProduct = vector1.cross(vector2);
|
||||||
area2 = crossProduct.length2();
|
area2 = crossProduct.length2();
|
||||||
}
|
}
|
||||||
if (indexMaxPenetration != 3) {
|
if (indexMaxPenetration != 3) {
|
||||||
// Compute the area
|
// Compute the area
|
||||||
vec3 vector1 = newPoint - this.contactPoints[0]->getLocalPointOnBody1();
|
Vector3f vector1 = newPoint - this.contactPoints[0].getLocalPointOnBody1();
|
||||||
vec3 vector2 = this.contactPoints[2]->getLocalPointOnBody1() - this.contactPoints[1]->getLocalPointOnBody1();
|
Vector3f vector2 = this.contactPoints[2].getLocalPointOnBody1() - this.contactPoints[1].getLocalPointOnBody1();
|
||||||
vec3 crossProduct = vector1.cross(vector2);
|
Vector3f crossProduct = vector1.cross(vector2);
|
||||||
area3 = crossProduct.length2();
|
area3 = crossProduct.length2();
|
||||||
}
|
}
|
||||||
// Return the index of the contact to remove
|
// Return the index of the contact to remove
|
||||||
@ -204,12 +194,12 @@ ProxyShape* ContactManifold::getShape2() {
|
|||||||
|
|
||||||
// Return a pointer to the first body of the contact manifold
|
// Return a pointer to the first body of the contact manifold
|
||||||
CollisionBody* ContactManifold::getBody1() {
|
CollisionBody* ContactManifold::getBody1() {
|
||||||
return this.shape1->getBody();
|
return this.shape1.getBody();
|
||||||
}
|
}
|
||||||
|
|
||||||
// Return a pointer to the second body of the contact manifold
|
// Return a pointer to the second body of the contact manifold
|
||||||
CollisionBody* ContactManifold::getBody2() {
|
CollisionBody* ContactManifold::getBody2() {
|
||||||
return this.shape2->getBody();
|
return this.shape2.getBody();
|
||||||
}
|
}
|
||||||
|
|
||||||
// Return the normal direction Id
|
// Return the normal direction Id
|
||||||
@ -223,22 +213,22 @@ int ContactManifold::getNbContactPoints() {
|
|||||||
}
|
}
|
||||||
|
|
||||||
// Return the first friction vector at the center of the contact manifold
|
// Return the first friction vector at the center of the contact manifold
|
||||||
vec3 ContactManifold::getFrictionVector1() {
|
Vector3f ContactManifold::getFrictionVector1() {
|
||||||
return this.frictionVector1;
|
return this.frictionVector1;
|
||||||
}
|
}
|
||||||
|
|
||||||
// set the first friction vector at the center of the contact manifold
|
// set the first friction vector at the center of the contact manifold
|
||||||
void ContactManifold::setFrictionVector1( vec3 frictionVector1) {
|
void ContactManifold::setFrictionVector1( Vector3f frictionVector1) {
|
||||||
this.frictionVector1 = frictionVector1;
|
this.frictionVector1 = frictionVector1;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Return the second friction vector at the center of the contact manifold
|
// Return the second friction vector at the center of the contact manifold
|
||||||
vec3 ContactManifold::getFrictionvec2() {
|
Vector3f ContactManifold::getFrictionvec2() {
|
||||||
return this.frictionvec2;
|
return this.frictionvec2;
|
||||||
}
|
}
|
||||||
|
|
||||||
// set the second friction vector at the center of the contact manifold
|
// set the second friction vector at the center of the contact manifold
|
||||||
void ContactManifold::setFrictionvec2( vec3 frictionvec2) {
|
void ContactManifold::setFrictionvec2( Vector3f frictionvec2) {
|
||||||
this.frictionvec2 = frictionvec2;
|
this.frictionvec2 = frictionvec2;
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -273,7 +263,7 @@ void ContactManifold::setFrictionTwistImpulse(float frictionTwistImpulse) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
// Set the accumulated rolling resistance impulse
|
// Set the accumulated rolling resistance impulse
|
||||||
void ContactManifold::setRollingResistanceImpulse( vec3 rollingResistanceImpulse) {
|
void ContactManifold::setRollingResistanceImpulse( Vector3f rollingResistanceImpulse) {
|
||||||
this.rollingResistanceImpulse = rollingResistanceImpulse;
|
this.rollingResistanceImpulse = rollingResistanceImpulse;
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -289,10 +279,10 @@ boolean ContactManifold::isAlreadyInIsland() {
|
|||||||
}
|
}
|
||||||
|
|
||||||
// Return the normalized averaged normal vector
|
// Return the normalized averaged normal vector
|
||||||
vec3 ContactManifold::getAverageContactNormal() {
|
Vector3f ContactManifold::getAverageContactNormal() {
|
||||||
vec3 averageNormal;
|
Vector3f averageNormal;
|
||||||
for (int i=0; i<this.nbContactPoints; i++) {
|
for (int i=0; i<this.nbContactPoints; i++) {
|
||||||
averageNormal += this.contactPoints[i]->getNormal();
|
averageNormal += this.contactPoints[i].getNormal();
|
||||||
}
|
}
|
||||||
return averageNormal.safeNormalized();
|
return averageNormal.safeNormalized();
|
||||||
}
|
}
|
||||||
@ -301,7 +291,7 @@ vec3 ContactManifold::getAverageContactNormal() {
|
|||||||
float ContactManifold::getLargestContactDepth() {
|
float ContactManifold::getLargestContactDepth() {
|
||||||
float largestDepth = 0.0f;
|
float largestDepth = 0.0f;
|
||||||
for (int i=0; i<this.nbContactPoints; i++) {
|
for (int i=0; i<this.nbContactPoints; i++) {
|
||||||
float depth = this.contactPoints[i]->getPenetrationDepth();
|
float depth = this.contactPoints[i].getPenetrationDepth();
|
||||||
if (depth > largestDepth) {
|
if (depth > largestDepth) {
|
||||||
largestDepth = depth;
|
largestDepth = depth;
|
||||||
}
|
}
|
@ -26,14 +26,14 @@ ContactManifoldSet::~ContactManifoldSet() {
|
|||||||
|
|
||||||
void ContactManifoldSet::addContactPoint(ContactPoint* contact) {
|
void ContactManifoldSet::addContactPoint(ContactPoint* contact) {
|
||||||
// Compute an Id corresponding to the normal direction (using a cubemap)
|
// Compute an Id corresponding to the normal direction (using a cubemap)
|
||||||
int16t normalDirectionId = computeCubemapNormalId(contact->getNormal());
|
int16t normalDirectionId = computeCubemapNormalId(contact.getNormal());
|
||||||
// If there is no contact manifold yet
|
// If there is no contact manifold yet
|
||||||
if (this.nbManifolds == 0) {
|
if (this.nbManifolds == 0) {
|
||||||
createManifold(normalDirectionId);
|
createManifold(normalDirectionId);
|
||||||
this.manifolds[0]->addContactPoint(contact);
|
this.manifolds[0].addContactPoint(contact);
|
||||||
assert(this.manifolds[this.nbManifolds-1]->getNbContactPoints() > 0);
|
assert(this.manifolds[this.nbManifolds-1].getNbContactPoints() > 0);
|
||||||
for (int i=0; i<this.nbManifolds; i++) {
|
for (int i=0; i<this.nbManifolds; i++) {
|
||||||
assert(this.manifolds[i]->getNbContactPoints() > 0);
|
assert(this.manifolds[i].getNbContactPoints() > 0);
|
||||||
}
|
}
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
@ -45,17 +45,17 @@ void ContactManifoldSet::addContactPoint(ContactPoint* contact) {
|
|||||||
// If a similar manifold has been found
|
// If a similar manifold has been found
|
||||||
if (similarManifoldIndex != -1) {
|
if (similarManifoldIndex != -1) {
|
||||||
// Add the contact point to that similar manifold
|
// Add the contact point to that similar manifold
|
||||||
this.manifolds[similarManifoldIndex]->addContactPoint(contact);
|
this.manifolds[similarManifoldIndex].addContactPoint(contact);
|
||||||
assert(this.manifolds[similarManifoldIndex]->getNbContactPoints() > 0);
|
assert(this.manifolds[similarManifoldIndex].getNbContactPoints() > 0);
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
// If the maximum number of manifold has not been reached yet
|
// If the maximum number of manifold has not been reached yet
|
||||||
if (this.nbManifolds < this.nbMaxManifolds) {
|
if (this.nbManifolds < this.nbMaxManifolds) {
|
||||||
// Create a new manifold for the contact point
|
// Create a new manifold for the contact point
|
||||||
createManifold(normalDirectionId);
|
createManifold(normalDirectionId);
|
||||||
this.manifolds[this.nbManifolds-1]->addContactPoint(contact);
|
this.manifolds[this.nbManifolds-1].addContactPoint(contact);
|
||||||
for (int i=0; i<this.nbManifolds; i++) {
|
for (int i=0; i<this.nbManifolds; i++) {
|
||||||
assert(this.manifolds[i]->getNbContactPoints() > 0);
|
assert(this.manifolds[i].getNbContactPoints() > 0);
|
||||||
}
|
}
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
@ -63,10 +63,10 @@ void ContactManifoldSet::addContactPoint(ContactPoint* contact) {
|
|||||||
// manifolds condidates. We need to remove one. We choose to keep the manifolds
|
// manifolds condidates. We need to remove one. We choose to keep the manifolds
|
||||||
// with the largest contact depth among their points
|
// with the largest contact depth among their points
|
||||||
int smallestDepthIndex = -1;
|
int smallestDepthIndex = -1;
|
||||||
float minDepth = contact->getPenetrationDepth();
|
float minDepth = contact.getPenetrationDepth();
|
||||||
assert(this.nbManifolds == this.nbMaxManifolds);
|
assert(this.nbManifolds == this.nbMaxManifolds);
|
||||||
for (int i=0; i<this.nbManifolds; i++) {
|
for (int i=0; i<this.nbManifolds; i++) {
|
||||||
float depth = this.manifolds[i]->getLargestContactDepth();
|
float depth = this.manifolds[i].getLargestContactDepth();
|
||||||
if (depth < minDepth) {
|
if (depth < minDepth) {
|
||||||
minDepth = depth;
|
minDepth = depth;
|
||||||
smallestDepthIndex = i;
|
smallestDepthIndex = i;
|
||||||
@ -80,15 +80,15 @@ void ContactManifoldSet::addContactPoint(ContactPoint* contact) {
|
|||||||
contact = null;
|
contact = null;
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
assert(smallestDepthIndex >= 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj smallestDepthIndex < this.nbManifolds);
|
assert(smallestDepthIndex >= 0 && smallestDepthIndex < this.nbManifolds);
|
||||||
// Here we need to replace an existing manifold with a new one (that contains
|
// Here we need to replace an existing manifold with a new one (that contains
|
||||||
// the new contact point)
|
// the new contact point)
|
||||||
removeManifold(smallestDepthIndex);
|
removeManifold(smallestDepthIndex);
|
||||||
createManifold(normalDirectionId);
|
createManifold(normalDirectionId);
|
||||||
this.manifolds[this.nbManifolds-1]->addContactPoint(contact);
|
this.manifolds[this.nbManifolds-1].addContactPoint(contact);
|
||||||
assert(this.manifolds[this.nbManifolds-1]->getNbContactPoints() > 0);
|
assert(this.manifolds[this.nbManifolds-1].getNbContactPoints() > 0);
|
||||||
for (int i=0; i<this.nbManifolds; i++) {
|
for (int i=0; i<this.nbManifolds; i++) {
|
||||||
assert(this.manifolds[i]->getNbContactPoints() > 0);
|
assert(this.manifolds[i].getNbContactPoints() > 0);
|
||||||
}
|
}
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
@ -96,24 +96,24 @@ void ContactManifoldSet::addContactPoint(ContactPoint* contact) {
|
|||||||
int ContactManifoldSet::selectManifoldWithSimilarNormal(int16t normalDirectionId) {
|
int ContactManifoldSet::selectManifoldWithSimilarNormal(int16t normalDirectionId) {
|
||||||
// Return the Id of the manifold with the same normal direction id (if exists)
|
// Return the Id of the manifold with the same normal direction id (if exists)
|
||||||
for (int i=0; i<this.nbManifolds; i++) {
|
for (int i=0; i<this.nbManifolds; i++) {
|
||||||
if (normalDirectionId == this.manifolds[i]->getNormalDirectionId()) {
|
if (normalDirectionId == this.manifolds[i].getNormalDirectionId()) {
|
||||||
return i;
|
return i;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
return -1;
|
return -1;
|
||||||
}
|
}
|
||||||
|
|
||||||
int16t ContactManifoldSet::computeCubemapNormalId( vec3 normal) {
|
int16t ContactManifoldSet::computeCubemapNormalId( Vector3f normal) {
|
||||||
assert(normal.length2() > FLTEPSILON);
|
assert(normal.length2() > FLTEPSILON);
|
||||||
int faceNo;
|
int faceNo;
|
||||||
float u, v;
|
float u, v;
|
||||||
float max = max3(fabs(normal.x()), fabs(normal.y()), fabs(normal.z()));
|
float max = max3(fabs(normal.x()), fabs(normal.y()), fabs(normal.z()));
|
||||||
vec3 normalScaled = normal / max;
|
Vector3f normalScaled = normal / max;
|
||||||
if (normalScaled.x() >= normalScaled.y() hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj normalScaled.x() >= normalScaled.z()) {
|
if (normalScaled.x() >= normalScaled.y() && normalScaled.x() >= normalScaled.z()) {
|
||||||
faceNo = normalScaled.x() > 0 ? 0 : 1;
|
faceNo = normalScaled.x() > 0 ? 0 : 1;
|
||||||
u = normalScaled.y();
|
u = normalScaled.y();
|
||||||
v = normalScaled.z();
|
v = normalScaled.z();
|
||||||
} else if (normalScaled.y() >= normalScaled.x() hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj normalScaled.y() >= normalScaled.z()) {
|
} else if (normalScaled.y() >= normalScaled.x() && normalScaled.y() >= normalScaled.z()) {
|
||||||
faceNo = normalScaled.y() > 0 ? 2 : 3;
|
faceNo = normalScaled.y() > 0 ? 2 : 3;
|
||||||
u = normalScaled.x();
|
u = normalScaled.x();
|
||||||
v = normalScaled.z();
|
v = normalScaled.z();
|
||||||
@ -137,10 +137,10 @@ int16t ContactManifoldSet::computeCubemapNormalId( vec3 normal) {
|
|||||||
void ContactManifoldSet::update() {
|
void ContactManifoldSet::update() {
|
||||||
for (int i=this.nbManifolds-1; i>=0; i--) {
|
for (int i=this.nbManifolds-1; i>=0; i--) {
|
||||||
// Update the contact manifold
|
// Update the contact manifold
|
||||||
this.manifolds[i]->update(this.shape1->getBody()->getTransform() * this.shape1->getLocalToBodyTransform(),
|
this.manifolds[i].update(this.shape1.getBody().getTransform() * this.shape1.getLocalToBodyTransform(),
|
||||||
this.shape2->getBody()->getTransform() * this.shape2->getLocalToBodyTransform());
|
this.shape2.getBody().getTransform() * this.shape2.getLocalToBodyTransform());
|
||||||
// Remove the contact manifold if has no contact points anymore
|
// Remove the contact manifold if has no contact points anymore
|
||||||
if (this.manifolds[i]->getNbContactPoints() == 0) {
|
if (this.manifolds[i].getNbContactPoints() == 0) {
|
||||||
removeManifold(i);
|
removeManifold(i);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@ -161,7 +161,7 @@ void ContactManifoldSet::createManifold(int16t normalDirectionId) {
|
|||||||
|
|
||||||
void ContactManifoldSet::removeManifold(int index) {
|
void ContactManifoldSet::removeManifold(int index) {
|
||||||
assert(this.nbManifolds > 0);
|
assert(this.nbManifolds > 0);
|
||||||
assert(index >= 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj index < this.nbManifolds);
|
assert(index >= 0 && index < this.nbManifolds);
|
||||||
// Delete the new contact
|
// Delete the new contact
|
||||||
ETKDELETE(ContactManifold, this.manifolds[index]);
|
ETKDELETE(ContactManifold, this.manifolds[index]);
|
||||||
this.manifolds[index] = null;
|
this.manifolds[index] = null;
|
||||||
@ -184,14 +184,14 @@ int ContactManifoldSet::getNbContactManifolds() {
|
|||||||
}
|
}
|
||||||
|
|
||||||
ContactManifold* ContactManifoldSet::getContactManifold(int index) {
|
ContactManifold* ContactManifoldSet::getContactManifold(int index) {
|
||||||
assert(index >= 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj index < this.nbManifolds);
|
assert(index >= 0 && index < this.nbManifolds);
|
||||||
return this.manifolds[index];
|
return this.manifolds[index];
|
||||||
}
|
}
|
||||||
|
|
||||||
int ContactManifoldSet::getTotalNbContactPoints() {
|
int ContactManifoldSet::getTotalNbContactPoints() {
|
||||||
int nbPoints = 0;
|
int nbPoints = 0;
|
||||||
for (int i=0; i<this.nbManifolds; i++) {
|
for (int i=0; i<this.nbManifolds; i++) {
|
||||||
nbPoints += this.manifolds[i]->getNbContactPoints();
|
nbPoints += this.manifolds[i].getNbContactPoints();
|
||||||
}
|
}
|
||||||
return nbPoints;
|
return nbPoints;
|
||||||
}
|
}
|
||||||
|
@ -1,16 +1,5 @@
|
|||||||
/** @file
|
package org.atriaSoft.ephysics.collision;
|
||||||
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
|
|
||||||
* @author Daniel CHAPPUIS
|
|
||||||
* @author Edouard DUPIN
|
|
||||||
* @copyright 2010-2016, Daniel Chappuis
|
|
||||||
* @copyright 2017, Edouard DUPIN
|
|
||||||
* @license MPL v2.0 (see license file)
|
|
||||||
*/
|
|
||||||
#pragma once
|
|
||||||
|
|
||||||
#include <ephysics/collision/ContactManifold.hpp>
|
|
||||||
|
|
||||||
namespace ephysics {
|
|
||||||
int MAXMANIFOLDSINCONTACTMANIFOLDSET = 3; // Maximum number of contact manifolds in the set
|
int MAXMANIFOLDSINCONTACTMANIFOLDSET = 3; // Maximum number of contact manifolds in the set
|
||||||
int CONTACTCUBEMAPFACENBSUBDIVISIONS = 3; // N Number for the N x N subdivisions of the cubemap
|
int CONTACTCUBEMAPFACENBSUBDIVISIONS = 3; // N Number for the N x N subdivisions of the cubemap
|
||||||
/**
|
/**
|
||||||
@ -21,13 +10,13 @@ namespace ephysics {
|
|||||||
*/
|
*/
|
||||||
class ContactManifoldSet {
|
class ContactManifoldSet {
|
||||||
private:
|
private:
|
||||||
int this.nbMaxManifolds; //!< Maximum number of contact manifolds in the set
|
int nbMaxManifolds; //!< Maximum number of contact manifolds in the set
|
||||||
int this.nbManifolds; //!< Current number of contact manifolds in the set
|
int nbManifolds; //!< Current number of contact manifolds in the set
|
||||||
ProxyShape* this.shape1; //!< Pointer to the first proxy shape of the contact
|
ProxyShape* shape1; //!< Pointer to the first proxy shape of the contact
|
||||||
ProxyShape* this.shape2; //!< Pointer to the second proxy shape of the contact
|
ProxyShape* shape2; //!< Pointer to the second proxy shape of the contact
|
||||||
ContactManifold* this.manifolds[MAXMANIFOLDSINCONTACTMANIFOLDSET]; //!< Contact manifolds of the set
|
ContactManifold* manifolds[MAXMANIFOLDSINCONTACTMANIFOLDSET]; //!< Contact manifolds of the set
|
||||||
/// Create a new contact manifold and add it to the set
|
/// Create a new contact manifold and add it to the set
|
||||||
void createManifold(short normalDirectionId);
|
void createManifold(int normalDirectionId);
|
||||||
/// Remove a contact manifold from the set
|
/// Remove a contact manifold from the set
|
||||||
void removeManifold(int index);
|
void removeManifold(int index);
|
||||||
// Return the index of the contact manifold with a similar average normal.
|
// Return the index of the contact manifold with a similar average normal.
|
||||||
@ -35,14 +24,12 @@ namespace ephysics {
|
|||||||
// Map the normal vector into a cubemap face bucket (a face contains 4x4 buckets)
|
// Map the normal vector into a cubemap face bucket (a face contains 4x4 buckets)
|
||||||
// Each face of the cube is divided into 4x4 buckets. This method maps the
|
// Each face of the cube is divided into 4x4 buckets. This method maps the
|
||||||
// normal vector into of the of the bucket and returns a unique Id for the bucket
|
// normal vector into of the of the bucket and returns a unique Id for the bucket
|
||||||
int16t computeCubemapNormalId( vec3 normal) ;
|
int16t computeCubemapNormalId( Vector3f normal) ;
|
||||||
public:
|
public:
|
||||||
/// Constructor
|
/// Constructor
|
||||||
ContactManifoldSet(ProxyShape* shape1,
|
ContactManifoldSet(ProxyShape* shape1,
|
||||||
ProxyShape* shape2,
|
ProxyShape* shape2,
|
||||||
int nbMaxManifolds);
|
int nbMaxManifolds);
|
||||||
/// Destructor
|
|
||||||
~ContactManifoldSet();
|
|
||||||
/// Return the first proxy shape
|
/// Return the first proxy shape
|
||||||
ProxyShape* getShape1() ;
|
ProxyShape* getShape1() ;
|
||||||
/// Return the second proxy shape
|
/// Return the second proxy shape
|
@ -14,10 +14,10 @@ using namespace ephysics;
|
|||||||
/**
|
/**
|
||||||
* @param body Pointer to the parent body
|
* @param body Pointer to the parent body
|
||||||
* @param shape Pointer to the collision shape
|
* @param shape Pointer to the collision shape
|
||||||
* @param transform etk::Transform3Dation from collision shape local-space to body local-space
|
* @param transform Transform3Dation from collision shape local-space to body local-space
|
||||||
* @param mass Mass of the collision shape (in kilograms)
|
* @param mass Mass of the collision shape (in kilograms)
|
||||||
*/
|
*/
|
||||||
ProxyShape::ProxyShape(CollisionBody* body, CollisionShape* shape, etk::Transform3D transform, float mass)
|
ProxyShape::ProxyShape(CollisionBody* body, CollisionShape* shape, Transform3D transform, float mass)
|
||||||
:this.body(body), this.collisionShape(shape), this.localToBodyTransform(transform), this.mass(mass),
|
:this.body(body), this.collisionShape(shape), this.localToBodyTransform(transform), this.mass(mass),
|
||||||
this.next(NULL), this.broadPhaseID(-1), this.cachedCollisionData(NULL), this.userData(NULL),
|
this.next(NULL), this.broadPhaseID(-1), this.cachedCollisionData(NULL), this.userData(NULL),
|
||||||
this.collisionCategoryBits(0x0001), this.collideWithMaskBits(0xFFFF) {
|
this.collisionCategoryBits(0x0001), this.collideWithMaskBits(0xFFFF) {
|
||||||
@ -37,10 +37,10 @@ ProxyShape::~ProxyShape() {
|
|||||||
* @param worldPoint Point to test in world-space coordinates
|
* @param worldPoint Point to test in world-space coordinates
|
||||||
* @return True if the point is inside the collision shape
|
* @return True if the point is inside the collision shape
|
||||||
*/
|
*/
|
||||||
boolean ProxyShape::testPointInside( vec3 worldPoint) {
|
boolean ProxyShape::testPointInside( Vector3f worldPoint) {
|
||||||
etk::Transform3D localToWorld = this.body->getTransform() * this.localToBodyTransform;
|
Transform3D localToWorld = this.body.getTransform() * this.localToBodyTransform;
|
||||||
vec3 localPoint = localToWorld.getInverse() * worldPoint;
|
Vector3f localPoint = localToWorld.getInverse() * worldPoint;
|
||||||
return this.collisionShape->testPointInside(localPoint, this);
|
return this.collisionShape.testPointInside(localPoint, this);
|
||||||
}
|
}
|
||||||
|
|
||||||
// Raycast method with feedback information
|
// Raycast method with feedback information
|
||||||
@ -53,16 +53,16 @@ boolean ProxyShape::testPointInside( vec3 worldPoint) {
|
|||||||
boolean ProxyShape::raycast( Ray ray, RaycastInfo raycastInfo) {
|
boolean ProxyShape::raycast( Ray ray, RaycastInfo raycastInfo) {
|
||||||
|
|
||||||
// If the corresponding body is not active, it cannot be hit by rays
|
// If the corresponding body is not active, it cannot be hit by rays
|
||||||
if (!this.body->isActive()) return false;
|
if (!this.body.isActive()) return false;
|
||||||
|
|
||||||
// Convert the ray into the local-space of the collision shape
|
// Convert the ray into the local-space of the collision shape
|
||||||
etk::Transform3D localToWorldTransform = getLocalToWorldTransform();
|
Transform3D localToWorldTransform = getLocalToWorldTransform();
|
||||||
etk::Transform3D worldToLocalTransform = localToWorldTransform.getInverse();
|
Transform3D worldToLocalTransform = localToWorldTransform.getInverse();
|
||||||
Ray rayLocal(worldToLocalTransform * ray.point1,
|
Ray rayLocal(worldToLocalTransform * ray.point1,
|
||||||
worldToLocalTransform * ray.point2,
|
worldToLocalTransform * ray.point2,
|
||||||
ray.maxFraction);
|
ray.maxFraction);
|
||||||
|
|
||||||
boolean isHit = this.collisionShape->raycast(rayLocal, raycastInfo, this);
|
boolean isHit = this.collisionShape.raycast(rayLocal, raycastInfo, this);
|
||||||
if (isHit == true) {
|
if (isHit == true) {
|
||||||
// Convert the raycast info into world-space
|
// Convert the raycast info into world-space
|
||||||
raycastInfo.worldPoint = localToWorldTransform * raycastInfo.worldPoint;
|
raycastInfo.worldPoint = localToWorldTransform * raycastInfo.worldPoint;
|
||||||
@ -122,19 +122,19 @@ void ProxyShape::setUserData(void* userData) {
|
|||||||
* @return The transformation that transforms the local-space of the collision shape
|
* @return The transformation that transforms the local-space of the collision shape
|
||||||
* to the local-space of the parent body
|
* to the local-space of the parent body
|
||||||
*/
|
*/
|
||||||
etk::Transform3D ProxyShape::getLocalToBodyTransform() {
|
Transform3D ProxyShape::getLocalToBodyTransform() {
|
||||||
return this.localToBodyTransform;
|
return this.localToBodyTransform;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Set the local to parent body transform
|
// Set the local to parent body transform
|
||||||
void ProxyShape::setLocalToBodyTransform( etk::Transform3D transform) {
|
void ProxyShape::setLocalToBodyTransform( Transform3D transform) {
|
||||||
|
|
||||||
this.localToBodyTransform = transform;
|
this.localToBodyTransform = transform;
|
||||||
|
|
||||||
this.body->setIsSleeping(false);
|
this.body.setIsSleeping(false);
|
||||||
|
|
||||||
// Notify the body that the proxy shape has to be updated in the broad-phase
|
// Notify the body that the proxy shape has to be updated in the broad-phase
|
||||||
this.body->updateProxyShapeInBroadPhase(this, true);
|
this.body.updateProxyShapeInBroadPhase(this, true);
|
||||||
}
|
}
|
||||||
|
|
||||||
// Return the local to world transform
|
// Return the local to world transform
|
||||||
@ -142,8 +142,8 @@ void ProxyShape::setLocalToBodyTransform( etk::Transform3D transform) {
|
|||||||
* @return The transformation that transforms the local-space of the collision
|
* @return The transformation that transforms the local-space of the collision
|
||||||
* shape to the world-space
|
* shape to the world-space
|
||||||
*/
|
*/
|
||||||
etk::Transform3D ProxyShape::getLocalToWorldTransform() {
|
Transform3D ProxyShape::getLocalToWorldTransform() {
|
||||||
return this.body->this.transform * this.localToBodyTransform;
|
return this.body.this.transform * this.localToBodyTransform;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Return the next proxy shape in the linked list of proxy shapes
|
// Return the next proxy shape in the linked list of proxy shapes
|
||||||
@ -166,7 +166,7 @@ ProxyShape* ProxyShape::getNext() {
|
|||||||
/**
|
/**
|
||||||
* @return The collision category bits mask of the proxy shape
|
* @return The collision category bits mask of the proxy shape
|
||||||
*/
|
*/
|
||||||
unsigned short ProxyShape::getCollisionCategoryBits() {
|
int ProxyShape::getCollisionCategoryBits() {
|
||||||
return this.collisionCategoryBits;
|
return this.collisionCategoryBits;
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -174,7 +174,7 @@ unsigned short ProxyShape::getCollisionCategoryBits() {
|
|||||||
/**
|
/**
|
||||||
* @param collisionCategoryBits The collision category bits mask of the proxy shape
|
* @param collisionCategoryBits The collision category bits mask of the proxy shape
|
||||||
*/
|
*/
|
||||||
void ProxyShape::setCollisionCategoryBits(unsigned short collisionCategoryBits) {
|
void ProxyShape::setCollisionCategoryBits(int collisionCategoryBits) {
|
||||||
this.collisionCategoryBits = collisionCategoryBits;
|
this.collisionCategoryBits = collisionCategoryBits;
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -182,7 +182,7 @@ void ProxyShape::setCollisionCategoryBits(unsigned short collisionCategoryBits)
|
|||||||
/**
|
/**
|
||||||
* @return The bits mask that specifies with which collision category this shape will collide
|
* @return The bits mask that specifies with which collision category this shape will collide
|
||||||
*/
|
*/
|
||||||
unsigned short ProxyShape::getCollideWithMaskBits() {
|
int ProxyShape::getCollideWithMaskBits() {
|
||||||
return this.collideWithMaskBits;
|
return this.collideWithMaskBits;
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -190,7 +190,7 @@ unsigned short ProxyShape::getCollideWithMaskBits() {
|
|||||||
/**
|
/**
|
||||||
* @param collideWithMaskBits The bits mask that specifies with which collision category this shape will collide
|
* @param collideWithMaskBits The bits mask that specifies with which collision category this shape will collide
|
||||||
*/
|
*/
|
||||||
void ProxyShape::setCollideWithMaskBits(unsigned short collideWithMaskBits) {
|
void ProxyShape::setCollideWithMaskBits(int collideWithMaskBits) {
|
||||||
this.collideWithMaskBits = collideWithMaskBits;
|
this.collideWithMaskBits = collideWithMaskBits;
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -198,21 +198,21 @@ void ProxyShape::setCollideWithMaskBits(unsigned short collideWithMaskBits) {
|
|||||||
/**
|
/**
|
||||||
* @return The local scaling vector
|
* @return The local scaling vector
|
||||||
*/
|
*/
|
||||||
vec3 ProxyShape::getLocalScaling() {
|
Vector3f ProxyShape::getLocalScaling() {
|
||||||
return this.collisionShape->getScaling();
|
return this.collisionShape.getScaling();
|
||||||
}
|
}
|
||||||
|
|
||||||
// Set the local scaling vector of the collision shape
|
// Set the local scaling vector of the collision shape
|
||||||
/**
|
/**
|
||||||
* @param scaling The new local scaling vector
|
* @param scaling The new local scaling vector
|
||||||
*/
|
*/
|
||||||
void ProxyShape::setLocalScaling( vec3 scaling) {
|
void ProxyShape::setLocalScaling( Vector3f scaling) {
|
||||||
|
|
||||||
// Set the local scaling of the collision shape
|
// Set the local scaling of the collision shape
|
||||||
this.collisionShape->setLocalScaling(scaling);
|
this.collisionShape.setLocalScaling(scaling);
|
||||||
|
|
||||||
this.body->setIsSleeping(false);
|
this.body.setIsSleeping(false);
|
||||||
|
|
||||||
// Notify the body that the proxy shape has to be updated in the broad-phase
|
// Notify the body that the proxy shape has to be updated in the broad-phase
|
||||||
this.body->updateProxyShapeInBroadPhase(this, true);
|
this.body.updateProxyShapeInBroadPhase(this, true);
|
||||||
}
|
}
|
||||||
|
@ -1,17 +1,5 @@
|
|||||||
/** @file
|
package org.atriaSoft.ephysics.collision;
|
||||||
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
|
|
||||||
* @author Daniel CHAPPUIS
|
|
||||||
* @author Edouard DUPIN
|
|
||||||
* @copyright 2010-2016, Daniel Chappuis
|
|
||||||
* @copyright 2017, Edouard DUPIN
|
|
||||||
* @license MPL v2.0 (see license file)
|
|
||||||
*/
|
|
||||||
#pragma once
|
|
||||||
|
|
||||||
#include <ephysics/body/CollisionBody.hpp>
|
|
||||||
#include <ephysics/collision/shapes/CollisionShape.hpp>
|
|
||||||
|
|
||||||
namespace ephysics {
|
|
||||||
/**
|
/**
|
||||||
* @breif The CollisionShape instances are supposed to be unique for memory optimization. For instance,
|
* @breif The CollisionShape instances are supposed to be unique for memory optimization. For instance,
|
||||||
* consider two rigid bodies with the same sphere collision shape. In this situation, we will have
|
* consider two rigid bodies with the same sphere collision shape. In this situation, we will have
|
||||||
@ -23,14 +11,14 @@ namespace ephysics {
|
|||||||
*/
|
*/
|
||||||
class ProxyShape {
|
class ProxyShape {
|
||||||
protected:
|
protected:
|
||||||
CollisionBody* this.body; //!< Pointer to the parent body
|
CollisionBody* body; //!< Pointer to the parent body
|
||||||
CollisionShape* this.collisionShape; //!< Internal collision shape
|
CollisionShape* collisionShape; //!< Internal collision shape
|
||||||
etk::Transform3D this.localToBodyTransform; //!< Local-space to parent body-space transform (does not change over time)
|
Transform3D localToBodyTransform; //!< Local-space to parent body-space transform (does not change over time)
|
||||||
float this.mass; //!< Mass (in kilogramms) of the corresponding collision shape
|
float mass; //!< Mass (in kilogramms) of the corresponding collision shape
|
||||||
ProxyShape* this.next; //!< Pointer to the next proxy shape of the body (linked list)
|
ProxyShape* next; //!< Pointer to the next proxy shape of the body (linked list)
|
||||||
int this.broadPhaseID; //!< Broad-phase ID (node ID in the dynamic AABB tree)
|
int broadPhaseID; //!< Broad-phase ID (node ID in the dynamic AABB tree)
|
||||||
void* this.cachedCollisionData; //!< Cached collision data
|
void* cachedCollisionData; //!< Cached collision data
|
||||||
void* this.userData; //!< Pointer to user data
|
void* userData; //!< Pointer to user data
|
||||||
/**
|
/**
|
||||||
* @brief Bits used to define the collision category of this shape.
|
* @brief Bits used to define the collision category of this shape.
|
||||||
* You can set a single bit to one to define a category value for this
|
* You can set a single bit to one to define a category value for this
|
||||||
@ -39,29 +27,21 @@ namespace ephysics {
|
|||||||
* categories of shapes collide with each other and do not collide with
|
* categories of shapes collide with each other and do not collide with
|
||||||
* other categories.
|
* other categories.
|
||||||
*/
|
*/
|
||||||
unsigned short this.collisionCategoryBits;
|
int collisionCategoryBits;
|
||||||
/**
|
/**
|
||||||
* @brief Bits mask used to state which collision categories this shape can
|
* @brief Bits mask used to state which collision categories this shape can
|
||||||
* collide with. This value is 0xFFFF by default. It means that this
|
* collide with. This value is 0xFFFF by default. It means that this
|
||||||
* proxy shape will collide with every collision categories by default.
|
* proxy shape will collide with every collision categories by default.
|
||||||
*/
|
*/
|
||||||
unsigned short this.collideWithMaskBits;
|
int collideWithMaskBits;
|
||||||
/// Private copy-ructor
|
|
||||||
ProxyShape( ProxyShape) = delete;
|
|
||||||
/// Private assignment operator
|
|
||||||
ProxyShape operator=( ProxyShape) = delete;
|
|
||||||
public:
|
public:
|
||||||
/// Constructor
|
/// Constructor
|
||||||
ProxyShape(CollisionBody* body,
|
ProxyShape(CollisionBody* body,
|
||||||
CollisionShape* shape,
|
CollisionShape* shape,
|
||||||
etk::Transform3D transform,
|
Transform3D transform,
|
||||||
float mass);
|
float mass);
|
||||||
|
|
||||||
/// Destructor
|
|
||||||
virtual ~ProxyShape();
|
|
||||||
|
|
||||||
/// Return the collision shape
|
/// Return the collision shape
|
||||||
CollisionShape* getCollisionShape() ;
|
CollisionShape* getCollisionShape() ;
|
||||||
|
|
||||||
/// Return the parent body
|
/// Return the parent body
|
||||||
CollisionBody* getBody() ;
|
CollisionBody* getBody() ;
|
||||||
@ -76,31 +56,31 @@ namespace ephysics {
|
|||||||
void setUserData(void* userData);
|
void setUserData(void* userData);
|
||||||
|
|
||||||
/// Return the local to parent body transform
|
/// Return the local to parent body transform
|
||||||
etk::Transform3D getLocalToBodyTransform() ;
|
Transform3D getLocalToBodyTransform() ;
|
||||||
|
|
||||||
/// Set the local to parent body transform
|
/// Set the local to parent body transform
|
||||||
void setLocalToBodyTransform( etk::Transform3D transform);
|
void setLocalToBodyTransform( Transform3D transform);
|
||||||
|
|
||||||
/// Return the local to world transform
|
/// Return the local to world transform
|
||||||
etk::Transform3D getLocalToWorldTransform() ;
|
Transform3D getLocalToWorldTransform() ;
|
||||||
|
|
||||||
/// Return true if a point is inside the collision shape
|
/// Return true if a point is inside the collision shape
|
||||||
boolean testPointInside( vec3 worldPoint);
|
boolean testPointInside( Vector3f worldPoint);
|
||||||
|
|
||||||
/// Raycast method with feedback information
|
/// Raycast method with feedback information
|
||||||
boolean raycast( Ray ray, RaycastInfo raycastInfo);
|
boolean raycast( Ray ray, RaycastInfo raycastInfo);
|
||||||
|
|
||||||
/// Return the collision bits mask
|
/// Return the collision bits mask
|
||||||
unsigned short getCollideWithMaskBits() ;
|
int getCollideWithMaskBits() ;
|
||||||
|
|
||||||
/// Set the collision bits mask
|
/// Set the collision bits mask
|
||||||
void setCollideWithMaskBits(unsigned short collideWithMaskBits);
|
void setCollideWithMaskBits(int collideWithMaskBits);
|
||||||
|
|
||||||
/// Return the collision category bits
|
/// Return the collision category bits
|
||||||
unsigned short getCollisionCategoryBits() ;
|
int getCollisionCategoryBits() ;
|
||||||
|
|
||||||
/// Set the collision category bits
|
/// Set the collision category bits
|
||||||
void setCollisionCategoryBits(unsigned short collisionCategoryBits);
|
void setCollisionCategoryBits(int collisionCategoryBits);
|
||||||
|
|
||||||
/// Return the next proxy shape in the linked list of proxy shapes
|
/// Return the next proxy shape in the linked list of proxy shapes
|
||||||
ProxyShape* getNext();
|
ProxyShape* getNext();
|
||||||
@ -112,10 +92,10 @@ namespace ephysics {
|
|||||||
void** getCachedCollisionData();
|
void** getCachedCollisionData();
|
||||||
|
|
||||||
/// Return the local scaling vector of the collision shape
|
/// Return the local scaling vector of the collision shape
|
||||||
vec3 getLocalScaling() ;
|
Vector3f getLocalScaling() ;
|
||||||
|
|
||||||
/// Set the local scaling vector of the collision shape
|
/// Set the local scaling vector of the collision shape
|
||||||
virtual void setLocalScaling( vec3 scaling);
|
void setLocalScaling( Vector3f scaling);
|
||||||
|
|
||||||
friend class OverlappingPair;
|
friend class OverlappingPair;
|
||||||
friend class CollisionBody;
|
friend class CollisionBody;
|
@ -16,14 +16,14 @@ float RaycastTest::raycastAgainstShape(ProxyShape* shape, Ray ray) {
|
|||||||
|
|
||||||
// Ray casting test against the collision shape
|
// Ray casting test against the collision shape
|
||||||
RaycastInfo raycastInfo;
|
RaycastInfo raycastInfo;
|
||||||
boolean isHit = shape->raycast(ray, raycastInfo);
|
boolean isHit = shape.raycast(ray, raycastInfo);
|
||||||
|
|
||||||
// If the ray hit the collision shape
|
// If the ray hit the collision shape
|
||||||
if (isHit) {
|
if (isHit) {
|
||||||
|
|
||||||
// Report the hit to the user and return the
|
// Report the hit to the user and return the
|
||||||
// user hit fraction value
|
// user hit fraction value
|
||||||
return userCallback->notifyRaycastHit(raycastInfo);
|
return userCallback.notifyRaycastHit(raycastInfo);
|
||||||
}
|
}
|
||||||
|
|
||||||
return ray.maxFraction;
|
return ray.maxFraction;
|
||||||
|
@ -1,20 +1,5 @@
|
|||||||
/** @file
|
package org.atriaSoft.ephysics.collision;
|
||||||
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
|
|
||||||
* @author Daniel CHAPPUIS
|
|
||||||
* @author Edouard DUPIN
|
|
||||||
* @copyright 2010-2016, Daniel Chappuis
|
|
||||||
* @copyright 2017, Edouard DUPIN
|
|
||||||
* @license MPL v2.0 (see license file)
|
|
||||||
*/
|
|
||||||
#pragma once
|
|
||||||
|
|
||||||
#include <etk/math/Vector3D.hpp>
|
|
||||||
#include <ephysics/mathematics/Ray.hpp>
|
|
||||||
|
|
||||||
namespace ephysics {
|
|
||||||
class CollisionBody;
|
|
||||||
class ProxyShape;
|
|
||||||
class CollisionShape;
|
|
||||||
/**
|
/**
|
||||||
* @brief It contains the information about a raycast hit.
|
* @brief It contains the information about a raycast hit.
|
||||||
*/
|
*/
|
||||||
@ -25,8 +10,8 @@ namespace ephysics {
|
|||||||
/// Private assignment operator
|
/// Private assignment operator
|
||||||
RaycastInfo operator=( RaycastInfo) = delete;
|
RaycastInfo operator=( RaycastInfo) = delete;
|
||||||
public:
|
public:
|
||||||
vec3 worldPoint; //!< Hit point in world-space coordinates
|
Vector3f worldPoint; //!< Hit point in world-space coordinates
|
||||||
vec3 worldNormal; //!< Surface normal at hit point in world-space coordinates
|
Vector3f worldNormal; //!< Surface normal at hit point in world-space coordinates
|
||||||
float hitFraction; //!< Fraction distance of the hit point between point1 and point2 of the ray. The hit point "p" is such that p = point1 + hitFraction * (point2 - point1)
|
float hitFraction; //!< Fraction distance of the hit point between point1 and point2 of the ray. The hit point "p" is such that p = point1 + hitFraction * (point2 - point1)
|
||||||
int meshSubpart; //!< Mesh subpart index that has been hit (only used for triangles mesh and -1 otherwise)
|
int meshSubpart; //!< Mesh subpart index that has been hit (only used for triangles mesh and -1 otherwise)
|
||||||
int triangleIndex; //!< Hit triangle index (only used for triangles mesh and -1 otherwise)
|
int triangleIndex; //!< Hit triangle index (only used for triangles mesh and -1 otherwise)
|
||||||
@ -40,9 +25,6 @@ namespace ephysics {
|
|||||||
proxyShape(null) {
|
proxyShape(null) {
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
/// Destructor
|
|
||||||
virtual ~RaycastInfo() = default;
|
|
||||||
};
|
};
|
||||||
|
|
||||||
/**
|
/**
|
||||||
@ -53,8 +35,6 @@ namespace ephysics {
|
|||||||
*/
|
*/
|
||||||
class RaycastCallback {
|
class RaycastCallback {
|
||||||
public:
|
public:
|
||||||
/// Destructor
|
|
||||||
virtual ~RaycastCallback() = default;
|
|
||||||
/**
|
/**
|
||||||
* @brief This method will be called for each ProxyShape that is hit by the
|
* @brief This method will be called for each ProxyShape that is hit by the
|
||||||
* ray. You cannot make any assumptions about the order of the
|
* ray. You cannot make any assumptions about the order of the
|
||||||
@ -70,7 +50,7 @@ namespace ephysics {
|
|||||||
* @param[in] raycastInfo Information about the raycast hit
|
* @param[in] raycastInfo Information about the raycast hit
|
||||||
* @return Value that controls the continuation of the ray after a hit
|
* @return Value that controls the continuation of the ray after a hit
|
||||||
*/
|
*/
|
||||||
virtual float notifyRaycastHit( RaycastInfo raycastInfo)=0;
|
float notifyRaycastHit( RaycastInfo raycastInfo)=0;
|
||||||
};
|
};
|
||||||
|
|
||||||
struct RaycastTest {
|
struct RaycastTest {
|
@ -1,16 +1,5 @@
|
|||||||
/** @file
|
package org.atriaSoft.ephysics.collision;
|
||||||
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
|
|
||||||
* @author Daniel CHAPPUIS
|
|
||||||
* @author Edouard DUPIN
|
|
||||||
* @copyright 2010-2016, Daniel Chappuis
|
|
||||||
* @copyright 2017, Edouard DUPIN
|
|
||||||
* @license MPL v2.0 (see license file)
|
|
||||||
*/
|
|
||||||
#pragma once
|
|
||||||
#include <etk/Vector.hpp>
|
|
||||||
#include <ephysics/collision/TriangleVertexArray.hpp>
|
|
||||||
|
|
||||||
namespace ephysics {
|
|
||||||
/**
|
/**
|
||||||
* @brief Represents a mesh made of triangles. A TriangleMesh contains
|
* @brief Represents a mesh made of triangles. A TriangleMesh contains
|
||||||
* one or several parts. Each part is a set of triangles represented in a
|
* one or several parts. Each part is a set of triangles represented in a
|
||||||
@ -20,16 +9,12 @@ namespace ephysics {
|
|||||||
*/
|
*/
|
||||||
class TriangleMesh {
|
class TriangleMesh {
|
||||||
protected:
|
protected:
|
||||||
etk::Vector<TriangleVertexArray*> this.triangleArrays; //!< All the triangle arrays of the mesh (one triangle array per part)
|
Vector<TriangleVertexArray*> triangleArrays; //!< All the triangle arrays of the mesh (one triangle array per part)
|
||||||
public:
|
public:
|
||||||
/**
|
/**
|
||||||
* @brief Constructor
|
* @brief Constructor
|
||||||
*/
|
*/
|
||||||
TriangleMesh();
|
TriangleMesh();
|
||||||
/**
|
|
||||||
* @brief Virtualisation of Destructor
|
|
||||||
*/
|
|
||||||
virtual ~TriangleMesh() = default;
|
|
||||||
/**
|
/**
|
||||||
* @brief Add a subpart of the mesh
|
* @brief Add a subpart of the mesh
|
||||||
*/
|
*/
|
@ -9,25 +9,25 @@
|
|||||||
#include <ephysics/collision/TriangleVertexArray.hpp>
|
#include <ephysics/collision/TriangleVertexArray.hpp>
|
||||||
|
|
||||||
|
|
||||||
ephysics::TriangleVertexArray::TriangleVertexArray( etk::Vector<vec3> vertices, etk::Vector<int> triangles):
|
ephysics::TriangleVertexArray::TriangleVertexArray( Vector<Vector3f> vertices, Vector<int> triangles):
|
||||||
this.vertices(vertices),
|
this.vertices(vertices),
|
||||||
this.triangles(triangles) {
|
this.triangles(triangles) {
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
sizet ephysics::TriangleVertexArray::getNbVertices() {
|
long ephysics::TriangleVertexArray::getNbVertices() {
|
||||||
return this.vertices.size();
|
return this.vertices.size();
|
||||||
}
|
}
|
||||||
|
|
||||||
sizet ephysics::TriangleVertexArray::getNbTriangles() {
|
long ephysics::TriangleVertexArray::getNbTriangles() {
|
||||||
return this.triangles.size()/3;
|
return this.triangles.size()/3;
|
||||||
}
|
}
|
||||||
|
|
||||||
etk::Vector<vec3> ephysics::TriangleVertexArray::getVertices() {
|
Vector<Vector3f> ephysics::TriangleVertexArray::getVertices() {
|
||||||
return this.vertices;
|
return this.vertices;
|
||||||
}
|
}
|
||||||
|
|
||||||
etk::Vector<int> ephysics::TriangleVertexArray::getIndices() {
|
Vector<int> ephysics::TriangleVertexArray::getIndices() {
|
||||||
return this.triangles;
|
return this.triangles;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -1,21 +1,9 @@
|
|||||||
/** @file
|
package org.atriaSoft.ephysics.collision;
|
||||||
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
|
|
||||||
* @author Daniel CHAPPUIS
|
|
||||||
* @author Edouard DUPIN
|
|
||||||
* @copyright 2010-2016, Daniel Chappuis
|
|
||||||
* @copyright 2017, Edouard DUPIN
|
|
||||||
* @license MPL v2.0 (see license file)
|
|
||||||
*/
|
|
||||||
#pragma once
|
|
||||||
|
|
||||||
#include <ephysics/configuration.hpp>
|
|
||||||
#include <etk/math/Vector3D.hpp>
|
|
||||||
|
|
||||||
namespace ephysics {
|
|
||||||
class Triangle {
|
class Triangle {
|
||||||
public:
|
public:
|
||||||
vec3 value[3];
|
Vector3f value[3];
|
||||||
vec3 operator[] (sizet id) {
|
Vector3f operator[] (long id) {
|
||||||
return value[id];
|
return value[id];
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
@ -30,36 +18,36 @@ namespace ephysics {
|
|||||||
*/
|
*/
|
||||||
class TriangleVertexArray {
|
class TriangleVertexArray {
|
||||||
protected:
|
protected:
|
||||||
etk::Vector<vec3> this.vertices; //!< Vertice list
|
Vector<Vector3f> vertices; //!< Vertice list
|
||||||
etk::Vector<int> this.triangles; //!< List of triangle (3 pos for each triangle)
|
Vector<int> triangles; //!< List of triangle (3 pos for each triangle)
|
||||||
public:
|
public:
|
||||||
/**
|
/**
|
||||||
* @brief Constructor
|
* @brief Constructor
|
||||||
* @param[in] vertices List Of all vertices
|
* @param[in] vertices List Of all vertices
|
||||||
* @param[in] triangles List of all linked points
|
* @param[in] triangles List of all linked points
|
||||||
*/
|
*/
|
||||||
TriangleVertexArray( etk::Vector<vec3> vertices,
|
TriangleVertexArray( Vector<Vector3f> vertices,
|
||||||
etk::Vector<int> triangles);
|
Vector<int> triangles);
|
||||||
/**
|
/**
|
||||||
* @brief Get the number of vertices
|
* @brief Get the number of vertices
|
||||||
* @return Number of vertices
|
* @return Number of vertices
|
||||||
*/
|
*/
|
||||||
sizet getNbVertices() ;
|
long getNbVertices() ;
|
||||||
/**
|
/**
|
||||||
* @brief Get the number of triangle
|
* @brief Get the number of triangle
|
||||||
* @return Number of triangles
|
* @return Number of triangles
|
||||||
*/
|
*/
|
||||||
sizet getNbTriangles() ;
|
long getNbTriangles() ;
|
||||||
/**
|
/**
|
||||||
* @brief Get The table of the vertices
|
* @brief Get The table of the vertices
|
||||||
* @return reference on the vertices
|
* @return reference on the vertices
|
||||||
*/
|
*/
|
||||||
etk::Vector<vec3> getVertices() ;
|
Vector<Vector3f> getVertices() ;
|
||||||
/**
|
/**
|
||||||
* @brief Get The table of the triangle indice
|
* @brief Get The table of the triangle indice
|
||||||
* @return reference on the triangle indice
|
* @return reference on the triangle indice
|
||||||
*/
|
*/
|
||||||
etk::Vector<int> getIndices() ;
|
Vector<int> getIndices() ;
|
||||||
/**
|
/**
|
||||||
* @brief Get a triangle at the specific ID
|
* @brief Get a triangle at the specific ID
|
||||||
* @return Buffer of 3 points
|
* @return Buffer of 3 points
|
@ -41,7 +41,7 @@ void BroadPhaseAlgorithm::removeMovedCollisionShape(int broadPhaseID) {
|
|||||||
|
|
||||||
// If less than the quarter of allocated elements of the non-static shapes IDs array
|
// If less than the quarter of allocated elements of the non-static shapes IDs array
|
||||||
// are used, we release some allocated memory
|
// are used, we release some allocated memory
|
||||||
if ((this.numberMovedShapes - this.numberNonUsedMovedShapes) < this.numberAllocatedMovedShapes / 4 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj
|
if ((this.numberMovedShapes - this.numberNonUsedMovedShapes) < this.numberAllocatedMovedShapes / 4 &&
|
||||||
this.numberAllocatedMovedShapes > 8) {
|
this.numberAllocatedMovedShapes > 8) {
|
||||||
|
|
||||||
this.numberAllocatedMovedShapes /= 2;
|
this.numberAllocatedMovedShapes /= 2;
|
||||||
@ -75,14 +75,14 @@ void BroadPhaseAlgorithm::addProxyCollisionShape(ProxyShape* proxyShape, AABB a
|
|||||||
// Add the collision shape into the dynamic AABB tree and get its broad-phase ID
|
// Add the collision shape into the dynamic AABB tree and get its broad-phase ID
|
||||||
int nodeId = this.dynamicAABBTree.addObject(aabb, proxyShape);
|
int nodeId = this.dynamicAABBTree.addObject(aabb, proxyShape);
|
||||||
// Set the broad-phase ID of the proxy shape
|
// Set the broad-phase ID of the proxy shape
|
||||||
proxyShape->this.broadPhaseID = nodeId;
|
proxyShape.this.broadPhaseID = nodeId;
|
||||||
// Add the collision shape into the array of bodies that have moved (or have been created)
|
// Add the collision shape into the array of bodies that have moved (or have been created)
|
||||||
// during the last simulation step
|
// during the last simulation step
|
||||||
addMovedCollisionShape(proxyShape->this.broadPhaseID);
|
addMovedCollisionShape(proxyShape.this.broadPhaseID);
|
||||||
}
|
}
|
||||||
|
|
||||||
void BroadPhaseAlgorithm::removeProxyCollisionShape(ProxyShape* proxyShape) {
|
void BroadPhaseAlgorithm::removeProxyCollisionShape(ProxyShape* proxyShape) {
|
||||||
int broadPhaseID = proxyShape->this.broadPhaseID;
|
int broadPhaseID = proxyShape.this.broadPhaseID;
|
||||||
// Remove the collision shape from the dynamic AABB tree
|
// Remove the collision shape from the dynamic AABB tree
|
||||||
this.dynamicAABBTree.removeObject(broadPhaseID);
|
this.dynamicAABBTree.removeObject(broadPhaseID);
|
||||||
// Remove the collision shape into the array of shapes that have moved (or have been created)
|
// Remove the collision shape into the array of shapes that have moved (or have been created)
|
||||||
@ -92,9 +92,9 @@ void BroadPhaseAlgorithm::removeProxyCollisionShape(ProxyShape* proxyShape) {
|
|||||||
|
|
||||||
void BroadPhaseAlgorithm::updateProxyCollisionShape(ProxyShape* proxyShape,
|
void BroadPhaseAlgorithm::updateProxyCollisionShape(ProxyShape* proxyShape,
|
||||||
AABB aabb,
|
AABB aabb,
|
||||||
vec3 displacement,
|
Vector3f displacement,
|
||||||
boolean forceReinsert) {
|
boolean forceReinsert) {
|
||||||
int broadPhaseID = proxyShape->this.broadPhaseID;
|
int broadPhaseID = proxyShape.this.broadPhaseID;
|
||||||
assert(broadPhaseID >= 0);
|
assert(broadPhaseID >= 0);
|
||||||
// Update the dynamic AABB tree according to the movement of the collision shape
|
// Update the dynamic AABB tree according to the movement of the collision shape
|
||||||
boolean hasBeenReInserted = this.dynamicAABBTree.updateObject(broadPhaseID, aabb, displacement, forceReinsert);
|
boolean hasBeenReInserted = this.dynamicAABBTree.updateObject(broadPhaseID, aabb, displacement, forceReinsert);
|
||||||
@ -107,7 +107,7 @@ void BroadPhaseAlgorithm::updateProxyCollisionShape(ProxyShape* proxyShape,
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
static boolean sortFunction( etk::Pair<int,int> pair1, etk::Pair<int,int> pair2) {
|
static boolean sortFunction( Pair<int,int> pair1, Pair<int,int> pair2) {
|
||||||
if (pair1.first < pair2.first) {
|
if (pair1.first < pair2.first) {
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
@ -137,19 +137,19 @@ void BroadPhaseAlgorithm::computeOverlappingPairs() {
|
|||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
// Add the new potential pair into the array of potential overlapping pairs
|
// Add the new potential pair into the array of potential overlapping pairs
|
||||||
this.potentialPairs.pushBack(etk::makePair(etk::min(it, nodeId), etk::max(it, nodeId) ));
|
this.potentialPairs.pushBack(makePair(min(it, nodeId), max(it, nodeId) ));
|
||||||
});
|
});
|
||||||
}
|
}
|
||||||
// Reset the array of collision shapes that have move (or have been created) during the last simulation step
|
// Reset the array of collision shapes that have move (or have been created) during the last simulation step
|
||||||
this.movedShapes.clear();
|
this.movedShapes.clear();
|
||||||
// Sort the array of potential overlapping pairs in order to remove duplicate pairs
|
// Sort the array of potential overlapping pairs in order to remove duplicate pairs
|
||||||
etk::algorithm::quickSort(this.potentialPairs, sortFunction);
|
algorithm::quickSort(this.potentialPairs, sortFunction);
|
||||||
// Check all the potential overlapping pairs avoiding duplicates to report unique
|
// Check all the potential overlapping pairs avoiding duplicates to report unique
|
||||||
// overlapping pairs
|
// overlapping pairs
|
||||||
int iii=0;
|
int iii=0;
|
||||||
while (iii < this.potentialPairs.size()) {
|
while (iii < this.potentialPairs.size()) {
|
||||||
// Get a potential overlapping pair
|
// Get a potential overlapping pair
|
||||||
etk::Pair<int,int> pair = (this.potentialPairs[iii]);
|
Pair<int,int> pair = (this.potentialPairs[iii]);
|
||||||
++iii;
|
++iii;
|
||||||
// Get the two collision shapes of the pair
|
// Get the two collision shapes of the pair
|
||||||
ProxyShape* shape1 = staticcast<ProxyShape*>(this.dynamicAABBTree.getNodeDataPointer(pair.first));
|
ProxyShape* shape1 = staticcast<ProxyShape*>(this.dynamicAABBTree.getNodeDataPointer(pair.first));
|
||||||
@ -159,7 +159,7 @@ void BroadPhaseAlgorithm::computeOverlappingPairs() {
|
|||||||
// Skip the duplicate overlapping pairs
|
// Skip the duplicate overlapping pairs
|
||||||
while (iii < this.potentialPairs.size()) {
|
while (iii < this.potentialPairs.size()) {
|
||||||
// Get the next pair
|
// Get the next pair
|
||||||
etk::Pair<int,int> nextPair = this.potentialPairs[iii];
|
Pair<int,int> nextPair = this.potentialPairs[iii];
|
||||||
// If the next pair is different from the previous one, we stop skipping pairs
|
// If the next pair is different from the previous one, we stop skipping pairs
|
||||||
if ( nextPair.first != pair.first
|
if ( nextPair.first != pair.first
|
||||||
|| nextPair.second != pair.second) {
|
|| nextPair.second != pair.second) {
|
||||||
@ -175,7 +175,7 @@ float BroadPhaseRaycastCallback::operator()(int nodeId, Ray ray) {
|
|||||||
// Get the proxy shape from the node
|
// Get the proxy shape from the node
|
||||||
ProxyShape* proxyShape = staticcast<ProxyShape*>(this.dynamicAABBTree.getNodeDataPointer(nodeId));
|
ProxyShape* proxyShape = staticcast<ProxyShape*>(this.dynamicAABBTree.getNodeDataPointer(nodeId));
|
||||||
// Check if the raycast filtering mask allows raycast against this shape
|
// Check if the raycast filtering mask allows raycast against this shape
|
||||||
if ((this.raycastWithCategoryMaskBits proxyShape->getCollisionCategoryBits()) != 0) {
|
if ((this.raycastWithCategoryMaskBits proxyShape.getCollisionCategoryBits()) != 0) {
|
||||||
// Ask the collision detection to perform a ray cast test against
|
// Ask the collision detection to perform a ray cast test against
|
||||||
// the proxy shape of this node because the ray is overlapping
|
// the proxy shape of this node because the ray is overlapping
|
||||||
// with the shape in the broad-phase
|
// with the shape in the broad-phase
|
||||||
@ -187,15 +187,15 @@ float BroadPhaseRaycastCallback::operator()(int nodeId, Ray ray) {
|
|||||||
boolean BroadPhaseAlgorithm::testOverlappingShapes( ProxyShape* shape1,
|
boolean BroadPhaseAlgorithm::testOverlappingShapes( ProxyShape* shape1,
|
||||||
ProxyShape* shape2) {
|
ProxyShape* shape2) {
|
||||||
// Get the two AABBs of the collision shapes
|
// Get the two AABBs of the collision shapes
|
||||||
AABB aabb1 = this.dynamicAABBTree.getFatAABB(shape1->this.broadPhaseID);
|
AABB aabb1 = this.dynamicAABBTree.getFatAABB(shape1.this.broadPhaseID);
|
||||||
AABB aabb2 = this.dynamicAABBTree.getFatAABB(shape2->this.broadPhaseID);
|
AABB aabb2 = this.dynamicAABBTree.getFatAABB(shape2.this.broadPhaseID);
|
||||||
// Check if the two AABBs are overlapping
|
// Check if the two AABBs are overlapping
|
||||||
return aabb1.testCollision(aabb2);
|
return aabb1.testCollision(aabb2);
|
||||||
}
|
}
|
||||||
|
|
||||||
void BroadPhaseAlgorithm::raycast( Ray ray,
|
void BroadPhaseAlgorithm::raycast( Ray ray,
|
||||||
RaycastTest raycastTest,
|
RaycastTest raycastTest,
|
||||||
unsigned short raycastWithCategoryMaskBits) {
|
int raycastWithCategoryMaskBits) {
|
||||||
PROFILE("BroadPhaseAlgorithm::raycast()");
|
PROFILE("BroadPhaseAlgorithm::raycast()");
|
||||||
BroadPhaseRaycastCallback broadPhaseRaycastCallback(this.dynamicAABBTree, raycastWithCategoryMaskBits, raycastTest);
|
BroadPhaseRaycastCallback broadPhaseRaycastCallback(this.dynamicAABBTree, raycastWithCategoryMaskBits, raycastTest);
|
||||||
this.dynamicAABBTree.raycast(ray, broadPhaseRaycastCallback);
|
this.dynamicAABBTree.raycast(ray, broadPhaseRaycastCallback);
|
||||||
|
@ -1,23 +1,5 @@
|
|||||||
/** @file
|
package org.atriaSoft.ephysics.collision.broadphase;
|
||||||
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
|
|
||||||
* @author Daniel CHAPPUIS
|
|
||||||
* @author Edouard DUPIN
|
|
||||||
* @copyright 2010-2016, Daniel Chappuis
|
|
||||||
* @copyright 2017, Edouard DUPIN
|
|
||||||
* @license MPL v2.0 (see license file)
|
|
||||||
*/
|
|
||||||
#pragma once
|
|
||||||
|
|
||||||
#include <etk/Vector.hpp>
|
|
||||||
#include <ephysics/body/CollisionBody.hpp>
|
|
||||||
#include <ephysics/collision/ProxyShape.hpp>
|
|
||||||
#include <ephysics/collision/broadphase/DynamicAABBTree.hpp>
|
|
||||||
#include <ephysics/engine/Profiler.hpp>
|
|
||||||
|
|
||||||
namespace ephysics {
|
|
||||||
|
|
||||||
class CollisionDetection;
|
|
||||||
class BroadPhaseAlgorithm;
|
|
||||||
|
|
||||||
// TODO : remove this as callback ... DynamicAABBTreeOverlapCallback {
|
// TODO : remove this as callback ... DynamicAABBTreeOverlapCallback {
|
||||||
/**
|
/**
|
||||||
@ -27,12 +9,12 @@ namespace ephysics {
|
|||||||
class BroadPhaseRaycastCallback {
|
class BroadPhaseRaycastCallback {
|
||||||
private :
|
private :
|
||||||
DynamicAABBTree this.dynamicAABBTree;
|
DynamicAABBTree this.dynamicAABBTree;
|
||||||
unsigned short this.raycastWithCategoryMaskBits;
|
int this.raycastWithCategoryMaskBits;
|
||||||
RaycastTest this.raycastTest;
|
RaycastTest this.raycastTest;
|
||||||
public:
|
public:
|
||||||
// Constructor
|
// Constructor
|
||||||
BroadPhaseRaycastCallback( DynamicAABBTree dynamicAABBTree,
|
BroadPhaseRaycastCallback( DynamicAABBTree dynamicAABBTree,
|
||||||
unsigned short raycastWithCategoryMaskBits,
|
int raycastWithCategoryMaskBits,
|
||||||
RaycastTest raycastTest):
|
RaycastTest raycastTest):
|
||||||
this.dynamicAABBTree(dynamicAABBTree),
|
this.dynamicAABBTree(dynamicAABBTree),
|
||||||
this.raycastWithCategoryMaskBits(raycastWithCategoryMaskBits),
|
this.raycastWithCategoryMaskBits(raycastWithCategoryMaskBits),
|
||||||
@ -53,8 +35,8 @@ namespace ephysics {
|
|||||||
class BroadPhaseAlgorithm {
|
class BroadPhaseAlgorithm {
|
||||||
protected :
|
protected :
|
||||||
DynamicAABBTree this.dynamicAABBTree; //!< Dynamic AABB tree
|
DynamicAABBTree this.dynamicAABBTree; //!< Dynamic AABB tree
|
||||||
etk::Vector<int> this.movedShapes; //!< Array with the broad-phase IDs of all collision shapes that have moved (or have been created) during the last simulation step. Those are the shapes that need to be tested for overlapping in the next simulation step.
|
Vector<int> this.movedShapes; //!< Array with the broad-phase IDs of all collision shapes that have moved (or have been created) during the last simulation step. Those are the shapes that need to be tested for overlapping in the next simulation step.
|
||||||
etk::Vector<etk::Pair<int,int>> this.potentialPairs; //!< Temporary array of potential overlapping pairs (with potential duplicates)
|
Vector<Pair<int,int>> this.potentialPairs; //!< Temporary array of potential overlapping pairs (with potential duplicates)
|
||||||
CollisionDetection this.collisionDetection; //!< Reference to the collision detection object
|
CollisionDetection this.collisionDetection; //!< Reference to the collision detection object
|
||||||
/// Private copy-ructor
|
/// Private copy-ructor
|
||||||
BroadPhaseAlgorithm( BroadPhaseAlgorithm obj);
|
BroadPhaseAlgorithm( BroadPhaseAlgorithm obj);
|
||||||
@ -64,7 +46,7 @@ namespace ephysics {
|
|||||||
/// Constructor
|
/// Constructor
|
||||||
BroadPhaseAlgorithm(CollisionDetection collisionDetection);
|
BroadPhaseAlgorithm(CollisionDetection collisionDetection);
|
||||||
/// Destructor
|
/// Destructor
|
||||||
virtual ~BroadPhaseAlgorithm();
|
~BroadPhaseAlgorithm();
|
||||||
/// Add a proxy collision shape into the broad-phase collision detection
|
/// Add a proxy collision shape into the broad-phase collision detection
|
||||||
void addProxyCollisionShape(ProxyShape* proxyShape, AABB aabb);
|
void addProxyCollisionShape(ProxyShape* proxyShape, AABB aabb);
|
||||||
/// Remove a proxy collision shape from the broad-phase collision detection
|
/// Remove a proxy collision shape from the broad-phase collision detection
|
||||||
@ -72,7 +54,7 @@ namespace ephysics {
|
|||||||
/// Notify the broad-phase that a collision shape has moved and need to be updated
|
/// Notify the broad-phase that a collision shape has moved and need to be updated
|
||||||
void updateProxyCollisionShape(ProxyShape* proxyShape,
|
void updateProxyCollisionShape(ProxyShape* proxyShape,
|
||||||
AABB aabb,
|
AABB aabb,
|
||||||
vec3 displacement,
|
Vector3f displacement,
|
||||||
boolean forceReinsert = false);
|
boolean forceReinsert = false);
|
||||||
/// Add a collision shape in the array of shapes that have moved in the last simulation step
|
/// Add a collision shape in the array of shapes that have moved in the last simulation step
|
||||||
/// and that need to be tested again for broad-phase overlapping.
|
/// and that need to be tested again for broad-phase overlapping.
|
||||||
@ -87,7 +69,7 @@ namespace ephysics {
|
|||||||
/// Ray casting method
|
/// Ray casting method
|
||||||
void raycast( Ray ray,
|
void raycast( Ray ray,
|
||||||
RaycastTest raycastTest,
|
RaycastTest raycastTest,
|
||||||
unsigned short raycastWithCategoryMaskBits) ;
|
int raycastWithCategoryMaskBits) ;
|
||||||
};
|
};
|
||||||
|
|
||||||
}
|
}
|
@ -85,7 +85,7 @@ int DynamicAABBTree::allocateNode() {
|
|||||||
// Release a node
|
// Release a node
|
||||||
void DynamicAABBTree::releaseNode(int nodeID) {
|
void DynamicAABBTree::releaseNode(int nodeID) {
|
||||||
assert(this.numberNodes > 0);
|
assert(this.numberNodes > 0);
|
||||||
assert(nodeID >= 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj nodeID < this.numberAllocatedNodes);
|
assert(nodeID >= 0 && nodeID < this.numberAllocatedNodes);
|
||||||
assert(this.nodes[nodeID].height >= 0);
|
assert(this.nodes[nodeID].height >= 0);
|
||||||
this.nodes[nodeID].nextNodeID = this.freeNodeID;
|
this.nodes[nodeID].nextNodeID = this.freeNodeID;
|
||||||
this.nodes[nodeID].height = -1;
|
this.nodes[nodeID].height = -1;
|
||||||
@ -98,7 +98,7 @@ int DynamicAABBTree::addObjectInternal( AABB aabb) {
|
|||||||
// Get the next available node (or allocate new ones if necessary)
|
// Get the next available node (or allocate new ones if necessary)
|
||||||
int nodeID = allocateNode();
|
int nodeID = allocateNode();
|
||||||
// Create the fat aabb to use in the tree
|
// Create the fat aabb to use in the tree
|
||||||
vec3 gap(this.extraAABBGap, this.extraAABBGap, this.extraAABBGap);
|
Vector3f gap(this.extraAABBGap, this.extraAABBGap, this.extraAABBGap);
|
||||||
this.nodes[nodeID].aabb.setMin(aabb.getMin() - gap);
|
this.nodes[nodeID].aabb.setMin(aabb.getMin() - gap);
|
||||||
this.nodes[nodeID].aabb.setMax(aabb.getMax() + gap);
|
this.nodes[nodeID].aabb.setMax(aabb.getMax() + gap);
|
||||||
// Set the height of the node in the tree
|
// Set the height of the node in the tree
|
||||||
@ -113,7 +113,7 @@ int DynamicAABBTree::addObjectInternal( AABB aabb) {
|
|||||||
|
|
||||||
// Remove an object from the tree
|
// Remove an object from the tree
|
||||||
void DynamicAABBTree::removeObject(int nodeID) {
|
void DynamicAABBTree::removeObject(int nodeID) {
|
||||||
assert(nodeID >= 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj nodeID < this.numberAllocatedNodes);
|
assert(nodeID >= 0 && nodeID < this.numberAllocatedNodes);
|
||||||
assert(this.nodes[nodeID].isLeaf());
|
assert(this.nodes[nodeID].isLeaf());
|
||||||
// Remove the node from the tree
|
// Remove the node from the tree
|
||||||
removeLeafNode(nodeID);
|
removeLeafNode(nodeID);
|
||||||
@ -128,43 +128,43 @@ void DynamicAABBTree::removeObject(int nodeID) {
|
|||||||
/// argument is the linear velocity of the AABB multiplied by the elapsed time between two
|
/// argument is the linear velocity of the AABB multiplied by the elapsed time between two
|
||||||
/// frames. If the "forceReinsert" parameter is true, we force a removal and reinsertion of the node
|
/// frames. If the "forceReinsert" parameter is true, we force a removal and reinsertion of the node
|
||||||
/// (this can be useful if the shape AABB has become much smaller than the previous one for instance).
|
/// (this can be useful if the shape AABB has become much smaller than the previous one for instance).
|
||||||
boolean DynamicAABBTree::updateObject(int nodeID, AABB newAABB, vec3 displacement, bool forceReinsert) {
|
boolean DynamicAABBTree::updateObject(int nodeID, AABB newAABB, Vector3f displacement, bool forceReinsert) {
|
||||||
PROFILE("DynamicAABBTree::updateObject()");
|
PROFILE("DynamicAABBTree::updateObject()");
|
||||||
assert(nodeID >= 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj nodeID < this.numberAllocatedNodes);
|
assert(nodeID >= 0 && nodeID < this.numberAllocatedNodes);
|
||||||
assert(this.nodes[nodeID].isLeaf());
|
assert(this.nodes[nodeID].isLeaf());
|
||||||
assert(this.nodes[nodeID].height >= 0);
|
assert(this.nodes[nodeID].height >= 0);
|
||||||
Log.verbose(" compare : " << this.nodes[nodeID].aabb.this.minCoordinates << " " << this.nodes[nodeID].aabb.this.maxCoordinates);
|
Log.verbose(" compare : " + this.nodes[nodeID].aabb.minCoordinates + " " + this.nodes[nodeID].aabb.maxCoordinates);
|
||||||
Log.verbose(" : " << newAABB.this.minCoordinates << " " << newAABB.this.maxCoordinates);
|
Log.verbose(" : " + newAABB.minCoordinates + " " + newAABB.maxCoordinates);
|
||||||
// If the new AABB is still inside the fat AABB of the node
|
// If the new AABB is still inside the fat AABB of the node
|
||||||
if ( forceReinsert == false
|
if ( forceReinsert == false
|
||||||
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj this.nodes[nodeID].aabb.contains(newAABB)) {
|
&& this.nodes[nodeID].aabb.contains(newAABB)) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
// If the new AABB is outside the fat AABB, we remove the corresponding node
|
// If the new AABB is outside the fat AABB, we remove the corresponding node
|
||||||
removeLeafNode(nodeID);
|
removeLeafNode(nodeID);
|
||||||
// Compute the fat AABB by inflating the AABB with a ant gap
|
// Compute the fat AABB by inflating the AABB with a ant gap
|
||||||
this.nodes[nodeID].aabb = newAABB;
|
this.nodes[nodeID].aabb = newAABB;
|
||||||
vec3 gap(this.extraAABBGap, this.extraAABBGap, this.extraAABBGap);
|
Vector3f gap(this.extraAABBGap, this.extraAABBGap, this.extraAABBGap);
|
||||||
this.nodes[nodeID].aabb.this.minCoordinates -= gap;
|
this.nodes[nodeID].aabb.minCoordinates -= gap;
|
||||||
this.nodes[nodeID].aabb.this.maxCoordinates += gap;
|
this.nodes[nodeID].aabb.maxCoordinates += gap;
|
||||||
// Inflate the fat AABB in direction of the linear motion of the AABB
|
// Inflate the fat AABB in direction of the linear motion of the AABB
|
||||||
if (displacement.x() < 0.0f) {
|
if (displacement.x() < 0.0f) {
|
||||||
this.nodes[nodeID].aabb.this.minCoordinates.setX(this.nodes[nodeID].aabb.this.minCoordinates.x() + DYNAMICTREEAABBLINGAPMULTIPLIER *displacement.x());
|
this.nodes[nodeID].aabb.minCoordinates.setX(this.nodes[nodeID].aabb.minCoordinates.x() + DYNAMICTREEAABBLINGAPMULTIPLIER *displacement.x());
|
||||||
} else {
|
} else {
|
||||||
this.nodes[nodeID].aabb.this.maxCoordinates.setX(this.nodes[nodeID].aabb.this.maxCoordinates.x() + DYNAMICTREEAABBLINGAPMULTIPLIER *displacement.x());
|
this.nodes[nodeID].aabb.maxCoordinates.setX(this.nodes[nodeID].aabb.maxCoordinates.x() + DYNAMICTREEAABBLINGAPMULTIPLIER *displacement.x());
|
||||||
}
|
}
|
||||||
if (displacement.y() < 0.0f) {
|
if (displacement.y() < 0.0f) {
|
||||||
this.nodes[nodeID].aabb.this.minCoordinates.setY(this.nodes[nodeID].aabb.this.minCoordinates.y() + DYNAMICTREEAABBLINGAPMULTIPLIER *displacement.y());
|
this.nodes[nodeID].aabb.minCoordinates.setY(this.nodes[nodeID].aabb.minCoordinates.y() + DYNAMICTREEAABBLINGAPMULTIPLIER *displacement.y());
|
||||||
} else {
|
} else {
|
||||||
this.nodes[nodeID].aabb.this.maxCoordinates.setY(this.nodes[nodeID].aabb.this.maxCoordinates.y() + DYNAMICTREEAABBLINGAPMULTIPLIER *displacement.y());
|
this.nodes[nodeID].aabb.maxCoordinates.setY(this.nodes[nodeID].aabb.maxCoordinates.y() + DYNAMICTREEAABBLINGAPMULTIPLIER *displacement.y());
|
||||||
}
|
}
|
||||||
if (displacement.z() < 0.0f) {
|
if (displacement.z() < 0.0f) {
|
||||||
this.nodes[nodeID].aabb.this.minCoordinates.setZ(this.nodes[nodeID].aabb.this.minCoordinates.z() + DYNAMICTREEAABBLINGAPMULTIPLIER *displacement.z());
|
this.nodes[nodeID].aabb.minCoordinates.setZ(this.nodes[nodeID].aabb.minCoordinates.z() + DYNAMICTREEAABBLINGAPMULTIPLIER *displacement.z());
|
||||||
} else {
|
} else {
|
||||||
this.nodes[nodeID].aabb.this.maxCoordinates.setZ(this.nodes[nodeID].aabb.this.maxCoordinates.z() + DYNAMICTREEAABBLINGAPMULTIPLIER *displacement.z());
|
this.nodes[nodeID].aabb.maxCoordinates.setZ(this.nodes[nodeID].aabb.maxCoordinates.z() + DYNAMICTREEAABBLINGAPMULTIPLIER *displacement.z());
|
||||||
}
|
}
|
||||||
Log.error(" compare : " << this.nodes[nodeID].aabb.this.minCoordinates << " " << this.nodes[nodeID].aabb.this.maxCoordinates);
|
Log.error(" compare : " + this.nodes[nodeID].aabb.minCoordinates + " " + this.nodes[nodeID].aabb.maxCoordinates);
|
||||||
Log.error(" : " << newAABB.this.minCoordinates << " " << newAABB.this.maxCoordinates);
|
Log.error(" : " + newAABB.minCoordinates + " " + newAABB.maxCoordinates);
|
||||||
if (this.nodes[nodeID].aabb.contains(newAABB) == false) {
|
if (this.nodes[nodeID].aabb.contains(newAABB) == false) {
|
||||||
//Log.critical("ERROR");
|
//Log.critical("ERROR");
|
||||||
}
|
}
|
||||||
@ -222,7 +222,7 @@ void DynamicAABBTree::insertLeafNode(int nodeID) {
|
|||||||
}
|
}
|
||||||
// If the cost of making the current node a sibbling of the new node is smaller than
|
// If the cost of making the current node a sibbling of the new node is smaller than
|
||||||
// the cost of going down into the left or right child
|
// the cost of going down into the left or right child
|
||||||
if (costS < costLeft hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj costS < costRight) {
|
if (costS < costLeft && costS < costRight) {
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
// It is cheaper to go down into a child of the current node, choose the best child
|
// It is cheaper to go down into a child of the current node, choose the best child
|
||||||
@ -273,7 +273,7 @@ void DynamicAABBTree::insertLeafNode(int nodeID) {
|
|||||||
assert(leftChild != TreeNode::NULLTREENODE);
|
assert(leftChild != TreeNode::NULLTREENODE);
|
||||||
assert(rightChild != TreeNode::NULLTREENODE);
|
assert(rightChild != TreeNode::NULLTREENODE);
|
||||||
// Recompute the height of the node in the tree
|
// Recompute the height of the node in the tree
|
||||||
this.nodes[currentNodeID].height = etk::max(this.nodes[leftChild].height,
|
this.nodes[currentNodeID].height = max(this.nodes[leftChild].height,
|
||||||
this.nodes[rightChild].height) + 1;
|
this.nodes[rightChild].height) + 1;
|
||||||
assert(this.nodes[currentNodeID].height > 0);
|
assert(this.nodes[currentNodeID].height > 0);
|
||||||
// Recompute the AABB of the node
|
// Recompute the AABB of the node
|
||||||
@ -285,7 +285,7 @@ void DynamicAABBTree::insertLeafNode(int nodeID) {
|
|||||||
|
|
||||||
// Remove a leaf node from the tree
|
// Remove a leaf node from the tree
|
||||||
void DynamicAABBTree::removeLeafNode(int nodeID) {
|
void DynamicAABBTree::removeLeafNode(int nodeID) {
|
||||||
assert(nodeID >= 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj nodeID < this.numberAllocatedNodes);
|
assert(nodeID >= 0 && nodeID < this.numberAllocatedNodes);
|
||||||
assert(this.nodes[nodeID].isLeaf());
|
assert(this.nodes[nodeID].isLeaf());
|
||||||
// If we are removing the root node (root node is a leaf in this case)
|
// If we are removing the root node (root node is a leaf in this case)
|
||||||
if (this.rootNodeID == nodeID) {
|
if (this.rootNodeID == nodeID) {
|
||||||
@ -324,7 +324,7 @@ void DynamicAABBTree::removeLeafNode(int nodeID) {
|
|||||||
// Recompute the AABB and the height of the current node
|
// Recompute the AABB and the height of the current node
|
||||||
this.nodes[currentNodeID].aabb.mergeTwoAABBs(this.nodes[leftChildID].aabb,
|
this.nodes[currentNodeID].aabb.mergeTwoAABBs(this.nodes[leftChildID].aabb,
|
||||||
this.nodes[rightChildID].aabb);
|
this.nodes[rightChildID].aabb);
|
||||||
this.nodes[currentNodeID].height = etk::max(this.nodes[leftChildID].height,
|
this.nodes[currentNodeID].height = max(this.nodes[leftChildID].height,
|
||||||
this.nodes[rightChildID].height) + 1;
|
this.nodes[rightChildID].height) + 1;
|
||||||
assert(this.nodes[currentNodeID].height > 0);
|
assert(this.nodes[currentNodeID].height > 0);
|
||||||
currentNodeID = this.nodes[currentNodeID].parentID;
|
currentNodeID = this.nodes[currentNodeID].parentID;
|
||||||
@ -344,123 +344,123 @@ int DynamicAABBTree::balanceSubTreeAtNode(int nodeID) {
|
|||||||
assert(nodeID != TreeNode::NULLTREENODE);
|
assert(nodeID != TreeNode::NULLTREENODE);
|
||||||
TreeNode* nodeA = this.nodes + nodeID;
|
TreeNode* nodeA = this.nodes + nodeID;
|
||||||
// If the node is a leaf or the height of A's sub-tree is less than 2
|
// If the node is a leaf or the height of A's sub-tree is less than 2
|
||||||
if (nodeA->isLeaf() || nodeA->height < 2) {
|
if (nodeA.isLeaf() || nodeA.height < 2) {
|
||||||
// Do not perform any rotation
|
// Do not perform any rotation
|
||||||
return nodeID;
|
return nodeID;
|
||||||
}
|
}
|
||||||
// Get the two children nodes
|
// Get the two children nodes
|
||||||
int nodeBID = nodeA->children[0];
|
int nodeBID = nodeA.children[0];
|
||||||
int nodeCID = nodeA->children[1];
|
int nodeCID = nodeA.children[1];
|
||||||
assert(nodeBID >= 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj nodeBID < this.numberAllocatedNodes);
|
assert(nodeBID >= 0 && nodeBID < this.numberAllocatedNodes);
|
||||||
assert(nodeCID >= 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj nodeCID < this.numberAllocatedNodes);
|
assert(nodeCID >= 0 && nodeCID < this.numberAllocatedNodes);
|
||||||
TreeNode* nodeB = this.nodes + nodeBID;
|
TreeNode* nodeB = this.nodes + nodeBID;
|
||||||
TreeNode* nodeC = this.nodes + nodeCID;
|
TreeNode* nodeC = this.nodes + nodeCID;
|
||||||
// Compute the factor of the left and right sub-trees
|
// Compute the factor of the left and right sub-trees
|
||||||
int balanceFactor = nodeC->height - nodeB->height;
|
int balanceFactor = nodeC.height - nodeB.height;
|
||||||
// If the right node C is 2 higher than left node B
|
// If the right node C is 2 higher than left node B
|
||||||
if (balanceFactor > 1) {
|
if (balanceFactor > 1) {
|
||||||
assert(!nodeC->isLeaf());
|
assert(!nodeC.isLeaf());
|
||||||
int nodeFID = nodeC->children[0];
|
int nodeFID = nodeC.children[0];
|
||||||
int nodeGID = nodeC->children[1];
|
int nodeGID = nodeC.children[1];
|
||||||
assert(nodeFID >= 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj nodeFID < this.numberAllocatedNodes);
|
assert(nodeFID >= 0 && nodeFID < this.numberAllocatedNodes);
|
||||||
assert(nodeGID >= 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj nodeGID < this.numberAllocatedNodes);
|
assert(nodeGID >= 0 && nodeGID < this.numberAllocatedNodes);
|
||||||
TreeNode* nodeF = this.nodes + nodeFID;
|
TreeNode* nodeF = this.nodes + nodeFID;
|
||||||
TreeNode* nodeG = this.nodes + nodeGID;
|
TreeNode* nodeG = this.nodes + nodeGID;
|
||||||
nodeC->children[0] = nodeID;
|
nodeC.children[0] = nodeID;
|
||||||
nodeC->parentID = nodeA->parentID;
|
nodeC.parentID = nodeA.parentID;
|
||||||
nodeA->parentID = nodeCID;
|
nodeA.parentID = nodeCID;
|
||||||
if (nodeC->parentID != TreeNode::NULLTREENODE) {
|
if (nodeC.parentID != TreeNode::NULLTREENODE) {
|
||||||
if (this.nodes[nodeC->parentID].children[0] == nodeID) {
|
if (this.nodes[nodeC.parentID].children[0] == nodeID) {
|
||||||
this.nodes[nodeC->parentID].children[0] = nodeCID;
|
this.nodes[nodeC.parentID].children[0] = nodeCID;
|
||||||
} else {
|
} else {
|
||||||
assert(this.nodes[nodeC->parentID].children[1] == nodeID);
|
assert(this.nodes[nodeC.parentID].children[1] == nodeID);
|
||||||
this.nodes[nodeC->parentID].children[1] = nodeCID;
|
this.nodes[nodeC.parentID].children[1] = nodeCID;
|
||||||
}
|
}
|
||||||
} else {
|
} else {
|
||||||
this.rootNodeID = nodeCID;
|
this.rootNodeID = nodeCID;
|
||||||
}
|
}
|
||||||
assert(!nodeC->isLeaf());
|
assert(!nodeC.isLeaf());
|
||||||
assert(!nodeA->isLeaf());
|
assert(!nodeA.isLeaf());
|
||||||
// If the right node C was higher than left node B because of the F node
|
// If the right node C was higher than left node B because of the F node
|
||||||
if (nodeF->height > nodeG->height) {
|
if (nodeF.height > nodeG.height) {
|
||||||
nodeC->children[1] = nodeFID;
|
nodeC.children[1] = nodeFID;
|
||||||
nodeA->children[1] = nodeGID;
|
nodeA.children[1] = nodeGID;
|
||||||
nodeG->parentID = nodeID;
|
nodeG.parentID = nodeID;
|
||||||
// Recompute the AABB of node A and C
|
// Recompute the AABB of node A and C
|
||||||
nodeA->aabb.mergeTwoAABBs(nodeB->aabb, nodeG->aabb);
|
nodeA.aabb.mergeTwoAABBs(nodeB.aabb, nodeG.aabb);
|
||||||
nodeC->aabb.mergeTwoAABBs(nodeA->aabb, nodeF->aabb);
|
nodeC.aabb.mergeTwoAABBs(nodeA.aabb, nodeF.aabb);
|
||||||
// Recompute the height of node A and C
|
// Recompute the height of node A and C
|
||||||
nodeA->height = etk::max(nodeB->height, nodeG->height) + 1;
|
nodeA.height = max(nodeB.height, nodeG.height) + 1;
|
||||||
nodeC->height = etk::max(nodeA->height, nodeF->height) + 1;
|
nodeC.height = max(nodeA.height, nodeF.height) + 1;
|
||||||
assert(nodeA->height > 0);
|
assert(nodeA.height > 0);
|
||||||
assert(nodeC->height > 0);
|
assert(nodeC.height > 0);
|
||||||
} else {
|
} else {
|
||||||
// If the right node C was higher than left node B because of node G
|
// If the right node C was higher than left node B because of node G
|
||||||
nodeC->children[1] = nodeGID;
|
nodeC.children[1] = nodeGID;
|
||||||
nodeA->children[1] = nodeFID;
|
nodeA.children[1] = nodeFID;
|
||||||
nodeF->parentID = nodeID;
|
nodeF.parentID = nodeID;
|
||||||
// Recompute the AABB of node A and C
|
// Recompute the AABB of node A and C
|
||||||
nodeA->aabb.mergeTwoAABBs(nodeB->aabb, nodeF->aabb);
|
nodeA.aabb.mergeTwoAABBs(nodeB.aabb, nodeF.aabb);
|
||||||
nodeC->aabb.mergeTwoAABBs(nodeA->aabb, nodeG->aabb);
|
nodeC.aabb.mergeTwoAABBs(nodeA.aabb, nodeG.aabb);
|
||||||
// Recompute the height of node A and C
|
// Recompute the height of node A and C
|
||||||
nodeA->height = etk::max(nodeB->height, nodeF->height) + 1;
|
nodeA.height = max(nodeB.height, nodeF.height) + 1;
|
||||||
nodeC->height = etk::max(nodeA->height, nodeG->height) + 1;
|
nodeC.height = max(nodeA.height, nodeG.height) + 1;
|
||||||
assert(nodeA->height > 0);
|
assert(nodeA.height > 0);
|
||||||
assert(nodeC->height > 0);
|
assert(nodeC.height > 0);
|
||||||
}
|
}
|
||||||
// Return the new root of the sub-tree
|
// Return the new root of the sub-tree
|
||||||
return nodeCID;
|
return nodeCID;
|
||||||
}
|
}
|
||||||
// If the left node B is 2 higher than right node C
|
// If the left node B is 2 higher than right node C
|
||||||
if (balanceFactor < -1) {
|
if (balanceFactor < -1) {
|
||||||
assert(!nodeB->isLeaf());
|
assert(!nodeB.isLeaf());
|
||||||
int nodeFID = nodeB->children[0];
|
int nodeFID = nodeB.children[0];
|
||||||
int nodeGID = nodeB->children[1];
|
int nodeGID = nodeB.children[1];
|
||||||
assert(nodeFID >= 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj nodeFID < this.numberAllocatedNodes);
|
assert(nodeFID >= 0 && nodeFID < this.numberAllocatedNodes);
|
||||||
assert(nodeGID >= 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj nodeGID < this.numberAllocatedNodes);
|
assert(nodeGID >= 0 && nodeGID < this.numberAllocatedNodes);
|
||||||
TreeNode* nodeF = this.nodes + nodeFID;
|
TreeNode* nodeF = this.nodes + nodeFID;
|
||||||
TreeNode* nodeG = this.nodes + nodeGID;
|
TreeNode* nodeG = this.nodes + nodeGID;
|
||||||
nodeB->children[0] = nodeID;
|
nodeB.children[0] = nodeID;
|
||||||
nodeB->parentID = nodeA->parentID;
|
nodeB.parentID = nodeA.parentID;
|
||||||
nodeA->parentID = nodeBID;
|
nodeA.parentID = nodeBID;
|
||||||
if (nodeB->parentID != TreeNode::NULLTREENODE) {
|
if (nodeB.parentID != TreeNode::NULLTREENODE) {
|
||||||
if (this.nodes[nodeB->parentID].children[0] == nodeID) {
|
if (this.nodes[nodeB.parentID].children[0] == nodeID) {
|
||||||
this.nodes[nodeB->parentID].children[0] = nodeBID;
|
this.nodes[nodeB.parentID].children[0] = nodeBID;
|
||||||
} else {
|
} else {
|
||||||
assert(this.nodes[nodeB->parentID].children[1] == nodeID);
|
assert(this.nodes[nodeB.parentID].children[1] == nodeID);
|
||||||
this.nodes[nodeB->parentID].children[1] = nodeBID;
|
this.nodes[nodeB.parentID].children[1] = nodeBID;
|
||||||
}
|
}
|
||||||
} else {
|
} else {
|
||||||
this.rootNodeID = nodeBID;
|
this.rootNodeID = nodeBID;
|
||||||
}
|
}
|
||||||
assert(!nodeB->isLeaf());
|
assert(!nodeB.isLeaf());
|
||||||
assert(!nodeA->isLeaf());
|
assert(!nodeA.isLeaf());
|
||||||
// If the left node B was higher than right node C because of the F node
|
// If the left node B was higher than right node C because of the F node
|
||||||
if (nodeF->height > nodeG->height) {
|
if (nodeF.height > nodeG.height) {
|
||||||
nodeB->children[1] = nodeFID;
|
nodeB.children[1] = nodeFID;
|
||||||
nodeA->children[0] = nodeGID;
|
nodeA.children[0] = nodeGID;
|
||||||
nodeG->parentID = nodeID;
|
nodeG.parentID = nodeID;
|
||||||
// Recompute the AABB of node A and B
|
// Recompute the AABB of node A and B
|
||||||
nodeA->aabb.mergeTwoAABBs(nodeC->aabb, nodeG->aabb);
|
nodeA.aabb.mergeTwoAABBs(nodeC.aabb, nodeG.aabb);
|
||||||
nodeB->aabb.mergeTwoAABBs(nodeA->aabb, nodeF->aabb);
|
nodeB.aabb.mergeTwoAABBs(nodeA.aabb, nodeF.aabb);
|
||||||
// Recompute the height of node A and B
|
// Recompute the height of node A and B
|
||||||
nodeA->height = etk::max(nodeC->height, nodeG->height) + 1;
|
nodeA.height = max(nodeC.height, nodeG.height) + 1;
|
||||||
nodeB->height = etk::max(nodeA->height, nodeF->height) + 1;
|
nodeB.height = max(nodeA.height, nodeF.height) + 1;
|
||||||
assert(nodeA->height > 0);
|
assert(nodeA.height > 0);
|
||||||
assert(nodeB->height > 0);
|
assert(nodeB.height > 0);
|
||||||
} else {
|
} else {
|
||||||
// If the left node B was higher than right node C because of node G
|
// If the left node B was higher than right node C because of node G
|
||||||
nodeB->children[1] = nodeGID;
|
nodeB.children[1] = nodeGID;
|
||||||
nodeA->children[0] = nodeFID;
|
nodeA.children[0] = nodeFID;
|
||||||
nodeF->parentID = nodeID;
|
nodeF.parentID = nodeID;
|
||||||
// Recompute the AABB of node A and B
|
// Recompute the AABB of node A and B
|
||||||
nodeA->aabb.mergeTwoAABBs(nodeC->aabb, nodeF->aabb);
|
nodeA.aabb.mergeTwoAABBs(nodeC.aabb, nodeF.aabb);
|
||||||
nodeB->aabb.mergeTwoAABBs(nodeA->aabb, nodeG->aabb);
|
nodeB.aabb.mergeTwoAABBs(nodeA.aabb, nodeG.aabb);
|
||||||
// Recompute the height of node A and B
|
// Recompute the height of node A and B
|
||||||
nodeA->height = etk::max(nodeC->height, nodeF->height) + 1;
|
nodeA.height = max(nodeC.height, nodeF.height) + 1;
|
||||||
nodeB->height = etk::max(nodeA->height, nodeG->height) + 1;
|
nodeB.height = max(nodeA.height, nodeG.height) + 1;
|
||||||
assert(nodeA->height > 0);
|
assert(nodeA.height > 0);
|
||||||
assert(nodeB->height > 0);
|
assert(nodeB.height > 0);
|
||||||
}
|
}
|
||||||
// Return the new root of the sub-tree
|
// Return the new root of the sub-tree
|
||||||
return nodeBID;
|
return nodeBID;
|
||||||
@ -470,7 +470,7 @@ int DynamicAABBTree::balanceSubTreeAtNode(int nodeID) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
/// Report all shapes overlapping with the AABB given in parameter.
|
/// Report all shapes overlapping with the AABB given in parameter.
|
||||||
void DynamicAABBTree::reportAllShapesOverlappingWithAABB( AABB aabb, etk::Function<void(int nodeId)> callback) {
|
void DynamicAABBTree::reportAllShapesOverlappingWithAABB( AABB aabb, Function<void(int nodeId)> callback) {
|
||||||
if (callback == null) {
|
if (callback == null) {
|
||||||
Log.error("call with null callback");
|
Log.error("call with null callback");
|
||||||
return;
|
return;
|
||||||
@ -489,23 +489,23 @@ void DynamicAABBTree::reportAllShapesOverlappingWithAABB( AABB aabb, etk::Functi
|
|||||||
// Get the corresponding node
|
// Get the corresponding node
|
||||||
TreeNode* nodeToVisit = this.nodes + nodeIDToVisit;
|
TreeNode* nodeToVisit = this.nodes + nodeIDToVisit;
|
||||||
// If the AABB in parameter overlaps with the AABB of the node to visit
|
// If the AABB in parameter overlaps with the AABB of the node to visit
|
||||||
if (aabb.testCollision(nodeToVisit->aabb)) {
|
if (aabb.testCollision(nodeToVisit.aabb)) {
|
||||||
// If the node is a leaf
|
// If the node is a leaf
|
||||||
if (nodeToVisit->isLeaf()) {
|
if (nodeToVisit.isLeaf()) {
|
||||||
// Notify the broad-phase about a new potential overlapping pair
|
// Notify the broad-phase about a new potential overlapping pair
|
||||||
callback(nodeIDToVisit);
|
callback(nodeIDToVisit);
|
||||||
} else {
|
} else {
|
||||||
// If the node is not a leaf
|
// If the node is not a leaf
|
||||||
// We need to visit its children
|
// We need to visit its children
|
||||||
stack.push(nodeToVisit->children[0]);
|
stack.push(nodeToVisit.children[0]);
|
||||||
stack.push(nodeToVisit->children[1]);
|
stack.push(nodeToVisit.children[1]);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// Ray casting method
|
// Ray casting method
|
||||||
void DynamicAABBTree::raycast( ephysics::Ray ray, etk::Function<float(int nodeId, ephysics::Ray ray)> callback) {
|
void DynamicAABBTree::raycast( ephysics::Ray ray, Function<float(int nodeId, ephysics::Ray ray)> callback) {
|
||||||
PROFILE("DynamicAABBTree::raycast()");
|
PROFILE("DynamicAABBTree::raycast()");
|
||||||
if (callback == null) {
|
if (callback == null) {
|
||||||
Log.error("call with null callback");
|
Log.error("call with null callback");
|
||||||
@ -527,11 +527,11 @@ void DynamicAABBTree::raycast( ephysics::Ray ray, etk::Function<float(int nodeId
|
|||||||
TreeNode* node = this.nodes + nodeID;
|
TreeNode* node = this.nodes + nodeID;
|
||||||
Ray rayTemp(ray.point1, ray.point2, maxFraction);
|
Ray rayTemp(ray.point1, ray.point2, maxFraction);
|
||||||
// Test if the ray intersects with the current node AABB
|
// Test if the ray intersects with the current node AABB
|
||||||
if (node->aabb.testRayIntersect(rayTemp) == false) {
|
if (node.aabb.testRayIntersect(rayTemp) == false) {
|
||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
// If the node is a leaf of the tree
|
// If the node is a leaf of the tree
|
||||||
if (node->isLeaf()) {
|
if (node.isLeaf()) {
|
||||||
// Call the callback that will raycast again the broad-phase shape
|
// Call the callback that will raycast again the broad-phase shape
|
||||||
float hitFraction = callback(nodeID, rayTemp);
|
float hitFraction = callback(nodeID, rayTemp);
|
||||||
// If the user returned a hitFraction of zero, it means that
|
// If the user returned a hitFraction of zero, it means that
|
||||||
@ -551,8 +551,8 @@ void DynamicAABBTree::raycast( ephysics::Ray ray, etk::Function<float(int nodeId
|
|||||||
// the raycasting as if the proxy shape did not exist
|
// the raycasting as if the proxy shape did not exist
|
||||||
} else { // If the node has children
|
} else { // If the node has children
|
||||||
// Push its children in the stack of nodes to explore
|
// Push its children in the stack of nodes to explore
|
||||||
stack.push(node->children[0]);
|
stack.push(node.children[0]);
|
||||||
stack.push(node->children[1]);
|
stack.push(node.children[1]);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@ -564,20 +564,20 @@ boolean TreeNode::isLeaf() {
|
|||||||
|
|
||||||
// Return the fat AABB corresponding to a given node ID
|
// Return the fat AABB corresponding to a given node ID
|
||||||
AABB DynamicAABBTree::getFatAABB(int nodeID) {
|
AABB DynamicAABBTree::getFatAABB(int nodeID) {
|
||||||
assert(nodeID >= 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj nodeID < this.numberAllocatedNodes);
|
assert(nodeID >= 0 && nodeID < this.numberAllocatedNodes);
|
||||||
return this.nodes[nodeID].aabb;
|
return this.nodes[nodeID].aabb;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Return the pointer to the data array of a given leaf node of the tree
|
// Return the pointer to the data array of a given leaf node of the tree
|
||||||
int* DynamicAABBTree::getNodeDataInt(int nodeID) {
|
int* DynamicAABBTree::getNodeDataInt(int nodeID) {
|
||||||
assert(nodeID >= 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj nodeID < this.numberAllocatedNodes);
|
assert(nodeID >= 0 && nodeID < this.numberAllocatedNodes);
|
||||||
assert(this.nodes[nodeID].isLeaf());
|
assert(this.nodes[nodeID].isLeaf());
|
||||||
return this.nodes[nodeID].dataInt;
|
return this.nodes[nodeID].dataInt;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Return the pointer to the data pointer of a given leaf node of the tree
|
// Return the pointer to the data pointer of a given leaf node of the tree
|
||||||
void* DynamicAABBTree::getNodeDataPointer(int nodeID) {
|
void* DynamicAABBTree::getNodeDataPointer(int nodeID) {
|
||||||
assert(nodeID >= 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj nodeID < this.numberAllocatedNodes);
|
assert(nodeID >= 0 && nodeID < this.numberAllocatedNodes);
|
||||||
assert(this.nodes[nodeID].isLeaf());
|
assert(this.nodes[nodeID].isLeaf());
|
||||||
return this.nodes[nodeID].dataPointer;
|
return this.nodes[nodeID].dataPointer;
|
||||||
}
|
}
|
||||||
@ -615,7 +615,7 @@ void DynamicAABBTree::check() {
|
|||||||
int freeNodeID = this.freeNodeID;
|
int freeNodeID = this.freeNodeID;
|
||||||
// Check the free nodes
|
// Check the free nodes
|
||||||
while(freeNodeID != TreeNode::NULLTREENODE) {
|
while(freeNodeID != TreeNode::NULLTREENODE) {
|
||||||
assert(0 <= freeNodeID hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj freeNodeID < this.numberAllocatedNodes);
|
assert(0 <= freeNodeID && freeNodeID < this.numberAllocatedNodes);
|
||||||
freeNodeID = this.nodes[freeNodeID].nextNodeID;
|
freeNodeID = this.nodes[freeNodeID].nextNodeID;
|
||||||
nbFreeNodes++;
|
nbFreeNodes++;
|
||||||
}
|
}
|
||||||
@ -633,26 +633,26 @@ void DynamicAABBTree::checkNode(int nodeID) {
|
|||||||
}
|
}
|
||||||
// Get the children nodes
|
// Get the children nodes
|
||||||
TreeNode* pNode = this.nodes + nodeID;
|
TreeNode* pNode = this.nodes + nodeID;
|
||||||
assert(!pNode->isLeaf());
|
assert(!pNode.isLeaf());
|
||||||
int leftChild = pNode->children[0];
|
int leftChild = pNode.children[0];
|
||||||
int rightChild = pNode->children[1];
|
int rightChild = pNode.children[1];
|
||||||
assert(pNode->height >= 0);
|
assert(pNode.height >= 0);
|
||||||
assert(pNode->aabb.getVolume() > 0);
|
assert(pNode.aabb.getVolume() > 0);
|
||||||
// If the current node is a leaf
|
// If the current node is a leaf
|
||||||
if (pNode->isLeaf()) {
|
if (pNode.isLeaf()) {
|
||||||
// Check that there are no children
|
// Check that there are no children
|
||||||
assert(leftChild == TreeNode::NULLTREENODE);
|
assert(leftChild == TreeNode::NULLTREENODE);
|
||||||
assert(rightChild == TreeNode::NULLTREENODE);
|
assert(rightChild == TreeNode::NULLTREENODE);
|
||||||
assert(pNode->height == 0);
|
assert(pNode.height == 0);
|
||||||
} else {
|
} else {
|
||||||
// Check that the children node IDs are valid
|
// Check that the children node IDs are valid
|
||||||
assert(0 <= leftChild hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj leftChild < this.numberAllocatedNodes);
|
assert(0 <= leftChild && leftChild < this.numberAllocatedNodes);
|
||||||
assert(0 <= rightChild hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj rightChild < this.numberAllocatedNodes);
|
assert(0 <= rightChild && rightChild < this.numberAllocatedNodes);
|
||||||
// Check that the children nodes have the correct parent node
|
// Check that the children nodes have the correct parent node
|
||||||
assert(this.nodes[leftChild].parentID == nodeID);
|
assert(this.nodes[leftChild].parentID == nodeID);
|
||||||
assert(this.nodes[rightChild].parentID == nodeID);
|
assert(this.nodes[rightChild].parentID == nodeID);
|
||||||
// Check the height of node
|
// Check the height of node
|
||||||
int height = 1 + etk::max(this.nodes[leftChild].height, this.nodes[rightChild].height);
|
int height = 1 + max(this.nodes[leftChild].height, this.nodes[rightChild].height);
|
||||||
assert(this.nodes[nodeID].height == height);
|
assert(this.nodes[nodeID].height == height);
|
||||||
// Check the AABB of the node
|
// Check the AABB of the node
|
||||||
AABB aabb;
|
AABB aabb;
|
||||||
@ -672,17 +672,17 @@ int DynamicAABBTree::computeHeight() {
|
|||||||
|
|
||||||
// Compute the height of a given node in the tree
|
// Compute the height of a given node in the tree
|
||||||
int DynamicAABBTree::computeHeight(int nodeID) {
|
int DynamicAABBTree::computeHeight(int nodeID) {
|
||||||
assert(nodeID >= 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj nodeID < this.numberAllocatedNodes);
|
assert(nodeID >= 0 && nodeID < this.numberAllocatedNodes);
|
||||||
TreeNode* node = this.nodes + nodeID;
|
TreeNode* node = this.nodes + nodeID;
|
||||||
// If the node is a leaf, its height is zero
|
// If the node is a leaf, its height is zero
|
||||||
if (node->isLeaf()) {
|
if (node.isLeaf()) {
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
// Compute the height of the left and right sub-tree
|
// Compute the height of the left and right sub-tree
|
||||||
int leftHeight = computeHeight(node->children[0]);
|
int leftHeight = computeHeight(node.children[0]);
|
||||||
int rightHeight = computeHeight(node->children[1]);
|
int rightHeight = computeHeight(node.children[1]);
|
||||||
// Return the height of the node
|
// Return the height of the node
|
||||||
return 1 + etk::max(leftHeight, rightHeight);
|
return 1 + max(leftHeight, rightHeight);
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
@ -1,19 +1,5 @@
|
|||||||
/** @file
|
package org.atriaSoft.ephysics.collision.broadphase;
|
||||||
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
|
|
||||||
* @author Daniel CHAPPUIS
|
|
||||||
* @author Edouard DUPIN
|
|
||||||
* @copyright 2010-2016, Daniel Chappuis
|
|
||||||
* @copyright 2017, Edouard DUPIN
|
|
||||||
* @license MPL v2.0 (see license file)
|
|
||||||
*/
|
|
||||||
#pragma once
|
|
||||||
|
|
||||||
#include <ephysics/configuration.hpp>
|
|
||||||
#include <ephysics/collision/shapes/AABB.hpp>
|
|
||||||
#include <ephysics/body/CollisionBody.hpp>
|
|
||||||
#include <etk/Function.hpp>
|
|
||||||
|
|
||||||
namespace ephysics {
|
|
||||||
// TODO: to replace this, create a Tree<T> template (multiple child) or TreeRedBlack<T>
|
// TODO: to replace this, create a Tree<T> template (multiple child) or TreeRedBlack<T>
|
||||||
/**
|
/**
|
||||||
* @brief It represents a node of the dynamic AABB tree.
|
* @brief It represents a node of the dynamic AABB tree.
|
||||||
@ -85,7 +71,7 @@ namespace ephysics {
|
|||||||
/// Constructor
|
/// Constructor
|
||||||
DynamicAABBTree(float extraAABBGap = 0.0f);
|
DynamicAABBTree(float extraAABBGap = 0.0f);
|
||||||
/// Destructor
|
/// Destructor
|
||||||
virtual ~DynamicAABBTree();
|
~DynamicAABBTree();
|
||||||
/// Add an object into the tree (where node data are two integers)
|
/// Add an object into the tree (where node data are two integers)
|
||||||
int addObject( AABB aabb, int data1, int data2);
|
int addObject( AABB aabb, int data1, int data2);
|
||||||
/// Add an object into the tree (where node data is a pointer)
|
/// Add an object into the tree (where node data is a pointer)
|
||||||
@ -93,7 +79,7 @@ namespace ephysics {
|
|||||||
/// Remove an object from the tree
|
/// Remove an object from the tree
|
||||||
void removeObject(int nodeID);
|
void removeObject(int nodeID);
|
||||||
/// Update the dynamic tree after an object has moved.
|
/// Update the dynamic tree after an object has moved.
|
||||||
boolean updateObject(int nodeID, AABB newAABB, vec3 displacement, bool forceReinsert = false);
|
boolean updateObject(int nodeID, AABB newAABB, Vector3f displacement, bool forceReinsert = false);
|
||||||
/// Return the fat AABB corresponding to a given node ID
|
/// Return the fat AABB corresponding to a given node ID
|
||||||
AABB getFatAABB(int nodeID) ;
|
AABB getFatAABB(int nodeID) ;
|
||||||
/// Return the pointer to the data array of a given leaf node of the tree
|
/// Return the pointer to the data array of a given leaf node of the tree
|
||||||
@ -101,9 +87,9 @@ namespace ephysics {
|
|||||||
/// Return the data pointer of a given leaf node of the tree
|
/// Return the data pointer of a given leaf node of the tree
|
||||||
void* getNodeDataPointer(int nodeID) ;
|
void* getNodeDataPointer(int nodeID) ;
|
||||||
/// Report all shapes overlapping with the AABB given in parameter.
|
/// Report all shapes overlapping with the AABB given in parameter.
|
||||||
void reportAllShapesOverlappingWithAABB( AABB aabb, etk::Function<void(int nodeId)> callback) ;
|
void reportAllShapesOverlappingWithAABB( AABB aabb, Function<void(int nodeId)> callback) ;
|
||||||
/// Ray casting method
|
/// Ray casting method
|
||||||
void raycast( Ray ray, etk::Function<float(int nodeId, ephysics::Ray ray)> callback) ;
|
void raycast( Ray ray, Function<float(int nodeId, ephysics::Ray ray)> callback) ;
|
||||||
/// Compute the height of the tree
|
/// Compute the height of the tree
|
||||||
int computeHeight();
|
int computeHeight();
|
||||||
/// Return the root AABB of the tree
|
/// Return the root AABB of the tree
|
@ -1,37 +0,0 @@
|
|||||||
/** @file
|
|
||||||
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
|
|
||||||
* @author Daniel CHAPPUIS
|
|
||||||
* @author Edouard DUPIN
|
|
||||||
* @copyright 2010-2016, Daniel Chappuis
|
|
||||||
* @copyright 2017, Edouard DUPIN
|
|
||||||
* @license MPL v2.0 (see license file)
|
|
||||||
*/
|
|
||||||
#pragma once
|
|
||||||
|
|
||||||
#include <ephysics/collision/narrowphase/NarrowPhaseAlgorithm.hpp>
|
|
||||||
|
|
||||||
namespace ephysics {
|
|
||||||
/**
|
|
||||||
* @biref Abstract base class for dispatching the narrow-phase
|
|
||||||
* collision detection algorithm. Collision dispatching decides which collision
|
|
||||||
* algorithm to use given two types of proxy collision shapes.
|
|
||||||
*/
|
|
||||||
class CollisionDispatch {
|
|
||||||
public:
|
|
||||||
/// Constructor
|
|
||||||
CollisionDispatch() {}
|
|
||||||
/// Destructor
|
|
||||||
virtual ~CollisionDispatch() = default;
|
|
||||||
/// Initialize the collision dispatch configuration
|
|
||||||
virtual void init(CollisionDetection* collisionDetection) {
|
|
||||||
// Nothing to do ...
|
|
||||||
}
|
|
||||||
/// Select and return the narrow-phase collision detection algorithm to
|
|
||||||
/// use between two types of collision shapes.
|
|
||||||
virtual NarrowPhaseAlgorithm* selectAlgorithm(int shape1Type,
|
|
||||||
int shape2Type) = 0;
|
|
||||||
};
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
@ -0,0 +1,21 @@
|
|||||||
|
package org.atriaSoft.ephysics.collision.narrowphase;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @biref Abstract base class for dispatching the narrow-phase
|
||||||
|
* collision detection algorithm. Collision dispatching decides which collision
|
||||||
|
* algorithm to use given two types of proxy collision shapes.
|
||||||
|
*/
|
||||||
|
class CollisionDispatch {
|
||||||
|
public:
|
||||||
|
/// Initialize the collision dispatch configuration
|
||||||
|
void init(CollisionDetection* collisionDetection) {
|
||||||
|
// Nothing to do ...
|
||||||
|
}
|
||||||
|
/// Select and return the narrow-phase collision detection algorithm to
|
||||||
|
/// use between two types of collision shapes.
|
||||||
|
NarrowPhaseAlgorithm* selectAlgorithm(int shape1Type, int shape2Type) = 0;
|
||||||
|
};
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
|
@ -27,7 +27,7 @@ void ConcaveVsConvexAlgorithm::testCollision( CollisionShapeInfo shape1Info,
|
|||||||
ConvexShape* convexShape;
|
ConvexShape* convexShape;
|
||||||
ConcaveShape* concaveShape;
|
ConcaveShape* concaveShape;
|
||||||
// Collision shape 1 is convex, collision shape 2 is concave
|
// Collision shape 1 is convex, collision shape 2 is concave
|
||||||
if (shape1Info.collisionShape->isConvex()) {
|
if (shape1Info.collisionShape.isConvex()) {
|
||||||
convexProxyShape = shape1Info.proxyShape;
|
convexProxyShape = shape1Info.proxyShape;
|
||||||
convexShape = staticcast< ConvexShape*>(shape1Info.collisionShape);
|
convexShape = staticcast< ConvexShape*>(shape1Info.collisionShape);
|
||||||
concaveProxyShape = shape2Info.proxyShape;
|
concaveProxyShape = shape2Info.proxyShape;
|
||||||
@ -48,48 +48,48 @@ void ConcaveVsConvexAlgorithm::testCollision( CollisionShapeInfo shape1Info,
|
|||||||
convexVsTriangleCallback.setOverlappingPair(shape1Info.overlappingPair);
|
convexVsTriangleCallback.setOverlappingPair(shape1Info.overlappingPair);
|
||||||
// Compute the convex shape AABB in the local-space of the convex shape
|
// Compute the convex shape AABB in the local-space of the convex shape
|
||||||
AABB aabb;
|
AABB aabb;
|
||||||
convexShape->computeAABB(aabb, convexProxyShape->getLocalToWorldTransform());
|
convexShape.computeAABB(aabb, convexProxyShape.getLocalToWorldTransform());
|
||||||
// If smooth mesh collision is enabled for the concave mesh
|
// If smooth mesh collision is enabled for the concave mesh
|
||||||
if (concaveShape->getIsSmoothMeshCollisionEnabled()) {
|
if (concaveShape.getIsSmoothMeshCollisionEnabled()) {
|
||||||
etk::Vector<SmoothMeshContactInfo> contactPoints;
|
Vector<SmoothMeshContactInfo> contactPoints;
|
||||||
SmoothCollisionNarrowPhaseCallback smoothNarrowPhaseCallback(contactPoints);
|
SmoothCollisionNarrowPhaseCallback smoothNarrowPhaseCallback(contactPoints);
|
||||||
convexVsTriangleCallback.setNarrowPhaseCallback(smoothNarrowPhaseCallback);
|
convexVsTriangleCallback.setNarrowPhaseCallback(smoothNarrowPhaseCallback);
|
||||||
// Call the convex vs triangle callback for each triangle of the concave shape
|
// Call the convex vs triangle callback for each triangle of the concave shape
|
||||||
concaveShape->testAllTriangles(convexVsTriangleCallback, aabb);
|
concaveShape.testAllTriangles(convexVsTriangleCallback, aabb);
|
||||||
// Run the smooth mesh collision algorithm
|
// Run the smooth mesh collision algorithm
|
||||||
processSmoothMeshCollision(shape1Info.overlappingPair, contactPoints, callback);
|
processSmoothMeshCollision(shape1Info.overlappingPair, contactPoints, callback);
|
||||||
} else {
|
} else {
|
||||||
convexVsTriangleCallback.setNarrowPhaseCallback(callback);
|
convexVsTriangleCallback.setNarrowPhaseCallback(callback);
|
||||||
// Call the convex vs triangle callback for each triangle of the concave shape
|
// Call the convex vs triangle callback for each triangle of the concave shape
|
||||||
concaveShape->testAllTriangles(convexVsTriangleCallback, aabb);
|
concaveShape.testAllTriangles(convexVsTriangleCallback, aabb);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void ConvexVsTriangleCallback::testTriangle( vec3* trianglePoints) {
|
void ConvexVsTriangleCallback::testTriangle( Vector3f* trianglePoints) {
|
||||||
// Create a triangle collision shape
|
// Create a triangle collision shape
|
||||||
float margin = this.concaveShape->getTriangleMargin();
|
float margin = this.concaveShape.getTriangleMargin();
|
||||||
TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], margin);
|
TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], margin);
|
||||||
// Select the collision algorithm to use between the triangle and the convex shape
|
// Select the collision algorithm to use between the triangle and the convex shape
|
||||||
NarrowPhaseAlgorithm* algo = this.collisionDetection->getCollisionAlgorithm(triangleShape.getType(), this.convexShape->getType());
|
NarrowPhaseAlgorithm* algo = this.collisionDetection.getCollisionAlgorithm(triangleShape.getType(), this.convexShape.getType());
|
||||||
// If there is no collision algorithm between those two kinds of shapes
|
// If there is no collision algorithm between those two kinds of shapes
|
||||||
if (algo == null) {
|
if (algo == null) {
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
// Notify the narrow-phase algorithm about the overlapping pair we are going to test
|
// Notify the narrow-phase algorithm about the overlapping pair we are going to test
|
||||||
algo->setCurrentOverlappingPair(this.overlappingPair);
|
algo.setCurrentOverlappingPair(this.overlappingPair);
|
||||||
// Create the CollisionShapeInfo objects
|
// Create the CollisionShapeInfo objects
|
||||||
CollisionShapeInfo shapeConvexInfo(this.convexProxyShape,
|
CollisionShapeInfo shapeConvexInfo(this.convexProxyShape,
|
||||||
this.convexShape,
|
this.convexShape,
|
||||||
this.convexProxyShape->getLocalToWorldTransform(),
|
this.convexProxyShape.getLocalToWorldTransform(),
|
||||||
this.overlappingPair,
|
this.overlappingPair,
|
||||||
this.convexProxyShape->getCachedCollisionData());
|
this.convexProxyShape.getCachedCollisionData());
|
||||||
CollisionShapeInfo shapeConcaveInfo(this.concaveProxyShape,
|
CollisionShapeInfo shapeConcaveInfo(this.concaveProxyShape,
|
||||||
triangleShape,
|
triangleShape,
|
||||||
this.concaveProxyShape->getLocalToWorldTransform(),
|
this.concaveProxyShape.getLocalToWorldTransform(),
|
||||||
this.overlappingPair,
|
this.overlappingPair,
|
||||||
this.concaveProxyShape->getCachedCollisionData());
|
this.concaveProxyShape.getCachedCollisionData());
|
||||||
// Use the collision algorithm to test collision between the triangle and the other convex shape
|
// Use the collision algorithm to test collision between the triangle and the other convex shape
|
||||||
algo->testCollision(shapeConvexInfo, shapeConcaveInfo, this.narrowPhaseCallback);
|
algo.testCollision(shapeConvexInfo, shapeConcaveInfo, this.narrowPhaseCallback);
|
||||||
}
|
}
|
||||||
|
|
||||||
static boolean sortFunction( SmoothMeshContactInfo contact1, SmoothMeshContactInfo contact2) {
|
static boolean sortFunction( SmoothMeshContactInfo contact1, SmoothMeshContactInfo contact2) {
|
||||||
@ -97,17 +97,17 @@ static boolean sortFunction( SmoothMeshContactInfo contact1, SmoothMeshContactI
|
|||||||
}
|
}
|
||||||
|
|
||||||
void ConcaveVsConvexAlgorithm::processSmoothMeshCollision(OverlappingPair* overlappingPair,
|
void ConcaveVsConvexAlgorithm::processSmoothMeshCollision(OverlappingPair* overlappingPair,
|
||||||
etk::Vector<SmoothMeshContactInfo> contactPoints,
|
Vector<SmoothMeshContactInfo> contactPoints,
|
||||||
NarrowPhaseCallback* callback) {
|
NarrowPhaseCallback* callback) {
|
||||||
// Set with the triangle vertices already processed to void further contacts with same triangle
|
// Set with the triangle vertices already processed to void further contacts with same triangle
|
||||||
etk::Vector<etk::Pair<int, vec3>> processTriangleVertices;
|
Vector<Pair<int, Vector3f>> processTriangleVertices;
|
||||||
// Sort the list of narrow-phase contacts according to their penetration depth
|
// Sort the list of narrow-phase contacts according to their penetration depth
|
||||||
etk::algorithm::quickSort(contactPoints, sortFunction);
|
algorithm::quickSort(contactPoints, sortFunction);
|
||||||
// For each contact point (from smaller penetration depth to larger)
|
// For each contact point (from smaller penetration depth to larger)
|
||||||
etk::Vector<SmoothMeshContactInfo>::Iterator it;
|
Vector<SmoothMeshContactInfo>::Iterator it;
|
||||||
for (it = contactPoints.begin(); it != contactPoints.end(); ++it) {
|
for (it = contactPoints.begin(); it != contactPoints.end(); ++it) {
|
||||||
SmoothMeshContactInfo info = *it;
|
SmoothMeshContactInfo info = *it;
|
||||||
vec3 contactPoint = info.isFirstShapeTriangle ? info.contactInfo.localPoint1 : info.contactInfo.localPoint2;
|
Vector3f contactPoint = info.isFirstShapeTriangle ? info.contactInfo.localPoint1 : info.contactInfo.localPoint2;
|
||||||
// Compute the barycentric coordinates of the point in the triangle
|
// Compute the barycentric coordinates of the point in the triangle
|
||||||
float u, v, w;
|
float u, v, w;
|
||||||
computeBarycentricCoordinatesInTriangle(info.triangleVertices[0],
|
computeBarycentricCoordinatesInTriangle(info.triangleVertices[0],
|
||||||
@ -129,21 +129,21 @@ void ConcaveVsConvexAlgorithm::processSmoothMeshCollision(OverlappingPair* overl
|
|||||||
}
|
}
|
||||||
// If it is a vertex contact
|
// If it is a vertex contact
|
||||||
if (nbZeros == 2) {
|
if (nbZeros == 2) {
|
||||||
vec3 contactVertex = !isUZero ? info.triangleVertices[0] : (!isVZero ? info.triangleVertices[1] : info.triangleVertices[2]);
|
Vector3f contactVertex = !isUZero ? info.triangleVertices[0] : (!isVZero ? info.triangleVertices[1] : info.triangleVertices[2]);
|
||||||
// Check that this triangle vertex has not been processed yet
|
// Check that this triangle vertex has not been processed yet
|
||||||
if (!hasVertexBeenProcessed(processTriangleVertices, contactVertex)) {
|
if (!hasVertexBeenProcessed(processTriangleVertices, contactVertex)) {
|
||||||
// Keep the contact as it is and report it
|
// Keep the contact as it is and report it
|
||||||
callback->notifyContact(overlappingPair, info.contactInfo);
|
callback.notifyContact(overlappingPair, info.contactInfo);
|
||||||
}
|
}
|
||||||
} else if (nbZeros == 1) {
|
} else if (nbZeros == 1) {
|
||||||
// If it is an edge contact
|
// If it is an edge contact
|
||||||
vec3 contactVertex1 = isUZero ? info.triangleVertices[1] : (isVZero ? info.triangleVertices[0] : info.triangleVertices[0]);
|
Vector3f contactVertex1 = isUZero ? info.triangleVertices[1] : (isVZero ? info.triangleVertices[0] : info.triangleVertices[0]);
|
||||||
vec3 contactVertex2 = isUZero ? info.triangleVertices[2] : (isVZero ? info.triangleVertices[2] : info.triangleVertices[1]);
|
Vector3f contactVertex2 = isUZero ? info.triangleVertices[2] : (isVZero ? info.triangleVertices[2] : info.triangleVertices[1]);
|
||||||
// Check that this triangle edge has not been processed yet
|
// Check that this triangle edge has not been processed yet
|
||||||
if (!hasVertexBeenProcessed(processTriangleVertices, contactVertex1) hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj
|
if (!hasVertexBeenProcessed(processTriangleVertices, contactVertex1) &&
|
||||||
!hasVertexBeenProcessed(processTriangleVertices, contactVertex2)) {
|
!hasVertexBeenProcessed(processTriangleVertices, contactVertex2)) {
|
||||||
// Keep the contact as it is and report it
|
// Keep the contact as it is and report it
|
||||||
callback->notifyContact(overlappingPair, info.contactInfo);
|
callback.notifyContact(overlappingPair, info.contactInfo);
|
||||||
}
|
}
|
||||||
} else {
|
} else {
|
||||||
// If it is a face contact
|
// If it is a face contact
|
||||||
@ -151,19 +151,19 @@ void ConcaveVsConvexAlgorithm::processSmoothMeshCollision(OverlappingPair* overl
|
|||||||
ProxyShape* firstShape;
|
ProxyShape* firstShape;
|
||||||
ProxyShape* secondShape;
|
ProxyShape* secondShape;
|
||||||
if (info.isFirstShapeTriangle) {
|
if (info.isFirstShapeTriangle) {
|
||||||
firstShape = overlappingPair->getShape1();
|
firstShape = overlappingPair.getShape1();
|
||||||
secondShape = overlappingPair->getShape2();
|
secondShape = overlappingPair.getShape2();
|
||||||
} else {
|
} else {
|
||||||
firstShape = overlappingPair->getShape2();
|
firstShape = overlappingPair.getShape2();
|
||||||
secondShape = overlappingPair->getShape1();
|
secondShape = overlappingPair.getShape1();
|
||||||
}
|
}
|
||||||
// We use the triangle normal as the contact normal
|
// We use the triangle normal as the contact normal
|
||||||
vec3 a = info.triangleVertices[1] - info.triangleVertices[0];
|
Vector3f a = info.triangleVertices[1] - info.triangleVertices[0];
|
||||||
vec3 b = info.triangleVertices[2] - info.triangleVertices[0];
|
Vector3f b = info.triangleVertices[2] - info.triangleVertices[0];
|
||||||
vec3 localNormal = a.cross(b);
|
Vector3f localNormal = a.cross(b);
|
||||||
newContactInfo.normal = firstShape->getLocalToWorldTransform().getOrientation() * localNormal;
|
newContactInfo.normal = firstShape.getLocalToWorldTransform().getOrientation() * localNormal;
|
||||||
vec3 firstLocalPoint = info.isFirstShapeTriangle ? info.contactInfo.localPoint1 : info.contactInfo.localPoint2;
|
Vector3f firstLocalPoint = info.isFirstShapeTriangle ? info.contactInfo.localPoint1 : info.contactInfo.localPoint2;
|
||||||
vec3 firstWorldPoint = firstShape->getLocalToWorldTransform() * firstLocalPoint;
|
Vector3f firstWorldPoint = firstShape.getLocalToWorldTransform() * firstLocalPoint;
|
||||||
newContactInfo.normal.normalize();
|
newContactInfo.normal.normalize();
|
||||||
if (newContactInfo.normal.dot(info.contactInfo.normal) < 0) {
|
if (newContactInfo.normal.dot(info.contactInfo.normal) < 0) {
|
||||||
newContactInfo.normal = -newContactInfo.normal;
|
newContactInfo.normal = -newContactInfo.normal;
|
||||||
@ -171,16 +171,16 @@ void ConcaveVsConvexAlgorithm::processSmoothMeshCollision(OverlappingPair* overl
|
|||||||
// We recompute the contact point on the second body with the new normal as described in
|
// We recompute the contact point on the second body with the new normal as described in
|
||||||
// the Smooth Mesh Contacts with GJK of the Game Physics Pearls book (from Gino van Den Bergen and
|
// the Smooth Mesh Contacts with GJK of the Game Physics Pearls book (from Gino van Den Bergen and
|
||||||
// Dirk Gregorius) to avoid adding torque
|
// Dirk Gregorius) to avoid adding torque
|
||||||
etk::Transform3D worldToLocalSecondPoint = secondShape->getLocalToWorldTransform().getInverse();
|
Transform3D worldToLocalSecondPoint = secondShape.getLocalToWorldTransform().getInverse();
|
||||||
if (info.isFirstShapeTriangle) {
|
if (info.isFirstShapeTriangle) {
|
||||||
vec3 newSecondWorldPoint = firstWorldPoint + newContactInfo.normal;
|
Vector3f newSecondWorldPoint = firstWorldPoint + newContactInfo.normal;
|
||||||
newContactInfo.localPoint2 = worldToLocalSecondPoint * newSecondWorldPoint;
|
newContactInfo.localPoint2 = worldToLocalSecondPoint * newSecondWorldPoint;
|
||||||
} else {
|
} else {
|
||||||
vec3 newSecondWorldPoint = firstWorldPoint - newContactInfo.normal;
|
Vector3f newSecondWorldPoint = firstWorldPoint - newContactInfo.normal;
|
||||||
newContactInfo.localPoint1 = worldToLocalSecondPoint * newSecondWorldPoint;
|
newContactInfo.localPoint1 = worldToLocalSecondPoint * newSecondWorldPoint;
|
||||||
}
|
}
|
||||||
// Report the contact
|
// Report the contact
|
||||||
callback->notifyContact(overlappingPair, newContactInfo);
|
callback.notifyContact(overlappingPair, newContactInfo);
|
||||||
}
|
}
|
||||||
// Add the three vertices of the triangle to the set of processed
|
// Add the three vertices of the triangle to the set of processed
|
||||||
// triangle vertices
|
// triangle vertices
|
||||||
@ -190,14 +190,14 @@ void ConcaveVsConvexAlgorithm::processSmoothMeshCollision(OverlappingPair* overl
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
boolean ConcaveVsConvexAlgorithm::hasVertexBeenProcessed( etk::Vector<etk::Pair<int, vec3>> processTriangleVertices, vec3 vertex) {
|
boolean ConcaveVsConvexAlgorithm::hasVertexBeenProcessed( Vector<Pair<int, Vector3f>> processTriangleVertices, Vector3f vertex) {
|
||||||
/* TODO : etk::Vector<etk::Pair<int, vec3>> was an unordered map ... ==> stupid idee... I replace code because I do not have enouth time to do something good...
|
/* TODO : Vector<Pair<int, Vector3f>> was an unordered map ... ==> stupid idee... I replace code because I do not have enouth time to do something good...
|
||||||
int key = int(vertex.x() * vertex.y() * vertex.z());
|
int key = int(vertex.x() * vertex.y() * vertex.z());
|
||||||
auto range = processTriangleVertices.equalrange(key);
|
auto range = processTriangleVertices.equalrange(key);
|
||||||
for (auto it = range.first; it != range.second; ++it) {
|
for (auto it = range.first; it != range.second; ++it) {
|
||||||
if ( vertex.x() == it->second.x()
|
if ( vertex.x() == it.second.x()
|
||||||
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj vertex.y() == it->second.y()
|
&& vertex.y() == it.second.y()
|
||||||
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj vertex.z() == it->second.z()) {
|
&& vertex.z() == it.second.z()) {
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@ -206,8 +206,8 @@ boolean ConcaveVsConvexAlgorithm::hasVertexBeenProcessed( etk::Vector<etk::Pair<
|
|||||||
// TODO : This is not really the same ...
|
// TODO : This is not really the same ...
|
||||||
for (auto it: processTriangleVertices) {
|
for (auto it: processTriangleVertices) {
|
||||||
if ( vertex.x() == it.second.x()
|
if ( vertex.x() == it.second.x()
|
||||||
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj vertex.y() == it.second.y()
|
&& vertex.y() == it.second.y()
|
||||||
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj vertex.z() == it.second.z()) {
|
&& vertex.z() == it.second.z()) {
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@ -216,22 +216,22 @@ boolean ConcaveVsConvexAlgorithm::hasVertexBeenProcessed( etk::Vector<etk::Pair<
|
|||||||
|
|
||||||
void SmoothCollisionNarrowPhaseCallback::notifyContact(OverlappingPair* overlappingPair,
|
void SmoothCollisionNarrowPhaseCallback::notifyContact(OverlappingPair* overlappingPair,
|
||||||
ContactPointInfo contactInfo) {
|
ContactPointInfo contactInfo) {
|
||||||
vec3 triangleVertices[3];
|
Vector3f triangleVertices[3];
|
||||||
boolean isFirstShapeTriangle;
|
boolean isFirstShapeTriangle;
|
||||||
// If the collision shape 1 is the triangle
|
// If the collision shape 1 is the triangle
|
||||||
if (contactInfo.collisionShape1->getType() == TRIANGLE) {
|
if (contactInfo.collisionShape1.getType() == TRIANGLE) {
|
||||||
assert(contactInfo.collisionShape2->getType() != TRIANGLE);
|
assert(contactInfo.collisionShape2.getType() != TRIANGLE);
|
||||||
TriangleShape* triangleShape = staticcast< TriangleShape*>(contactInfo.collisionShape1);
|
TriangleShape* triangleShape = staticcast< TriangleShape*>(contactInfo.collisionShape1);
|
||||||
triangleVertices[0] = triangleShape->getVertex(0);
|
triangleVertices[0] = triangleShape.getVertex(0);
|
||||||
triangleVertices[1] = triangleShape->getVertex(1);
|
triangleVertices[1] = triangleShape.getVertex(1);
|
||||||
triangleVertices[2] = triangleShape->getVertex(2);
|
triangleVertices[2] = triangleShape.getVertex(2);
|
||||||
isFirstShapeTriangle = true;
|
isFirstShapeTriangle = true;
|
||||||
} else { // If the collision shape 2 is the triangle
|
} else { // If the collision shape 2 is the triangle
|
||||||
assert(contactInfo.collisionShape2->getType() == TRIANGLE);
|
assert(contactInfo.collisionShape2.getType() == TRIANGLE);
|
||||||
TriangleShape* triangleShape = staticcast< TriangleShape*>(contactInfo.collisionShape2);
|
TriangleShape* triangleShape = staticcast< TriangleShape*>(contactInfo.collisionShape2);
|
||||||
triangleVertices[0] = triangleShape->getVertex(0);
|
triangleVertices[0] = triangleShape.getVertex(0);
|
||||||
triangleVertices[1] = triangleShape->getVertex(1);
|
triangleVertices[1] = triangleShape.getVertex(1);
|
||||||
triangleVertices[2] = triangleShape->getVertex(2);
|
triangleVertices[2] = triangleShape.getVertex(2);
|
||||||
isFirstShapeTriangle = false;
|
isFirstShapeTriangle = false;
|
||||||
}
|
}
|
||||||
SmoothMeshContactInfo smoothContactInfo(contactInfo, isFirstShapeTriangle, triangleVertices[0], triangleVertices[1], triangleVertices[2]);
|
SmoothMeshContactInfo smoothContactInfo(contactInfo, isFirstShapeTriangle, triangleVertices[0], triangleVertices[1], triangleVertices[2]);
|
||||||
|
@ -1,25 +1,12 @@
|
|||||||
/** @file
|
package org.atriaSoft.ephysics.collision.narrowphase;
|
||||||
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
|
|
||||||
* @author Daniel CHAPPUIS
|
|
||||||
* @author Edouard DUPIN
|
|
||||||
* @copyright 2010-2016, Daniel Chappuis
|
|
||||||
* @copyright 2017, Edouard DUPIN
|
|
||||||
* @license MPL v2.0 (see license file)
|
|
||||||
*/
|
|
||||||
#pragma once
|
|
||||||
|
|
||||||
#include <ephysics/collision/narrowphase/NarrowPhaseAlgorithm.hpp>
|
|
||||||
#include <ephysics/collision/shapes/ConvexShape.hpp>
|
|
||||||
#include <ephysics/collision/shapes/ConcaveShape.hpp>
|
|
||||||
|
|
||||||
namespace ephysics {
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @brief This class is used to encapsulate a callback method for
|
* @brief This class is used to encapsulate a callback method for
|
||||||
* collision detection between the triangle of a concave mesh shape
|
* collision detection between the triangle of a concave mesh shape
|
||||||
* and a convex shape.
|
* and a convex shape.
|
||||||
*/
|
*/
|
||||||
class ConvexVsTriangleCallback : public TriangleCallback {
|
class ConvexVsTriangleCallback extends TriangleCallback {
|
||||||
protected:
|
protected:
|
||||||
CollisionDetection* this.collisionDetection; //!< Pointer to the collision detection object
|
CollisionDetection* this.collisionDetection; //!< Pointer to the collision detection object
|
||||||
NarrowPhaseCallback* this.narrowPhaseCallback; //!< Narrow-phase collision callback
|
NarrowPhaseCallback* this.narrowPhaseCallback; //!< Narrow-phase collision callback
|
||||||
@ -57,7 +44,7 @@ namespace ephysics {
|
|||||||
this.concaveProxyShape = concaveProxyShape;
|
this.concaveProxyShape = concaveProxyShape;
|
||||||
}
|
}
|
||||||
/// Test collision between a triangle and the convex mesh shape
|
/// Test collision between a triangle and the convex mesh shape
|
||||||
virtual void testTriangle( vec3* trianglePoints);
|
void testTriangle( Vector3f* trianglePoints);
|
||||||
};
|
};
|
||||||
|
|
||||||
/**
|
/**
|
||||||
@ -68,13 +55,13 @@ namespace ephysics {
|
|||||||
public:
|
public:
|
||||||
ContactPointInfo contactInfo;
|
ContactPointInfo contactInfo;
|
||||||
boolean isFirstShapeTriangle;
|
boolean isFirstShapeTriangle;
|
||||||
vec3 triangleVertices[3];
|
Vector3f triangleVertices[3];
|
||||||
/// Constructor
|
/// Constructor
|
||||||
SmoothMeshContactInfo( ContactPointInfo contact,
|
SmoothMeshContactInfo( ContactPointInfo contact,
|
||||||
boolean firstShapeTriangle,
|
boolean firstShapeTriangle,
|
||||||
vec3 trianglePoint1,
|
Vector3f trianglePoint1,
|
||||||
vec3 trianglePoint2,
|
Vector3f trianglePoint2,
|
||||||
vec3 trianglePoint3):
|
Vector3f trianglePoint3):
|
||||||
contactInfo(contact) {
|
contactInfo(contact) {
|
||||||
isFirstShapeTriangle = firstShapeTriangle;
|
isFirstShapeTriangle = firstShapeTriangle;
|
||||||
triangleVertices[0] = trianglePoint1;
|
triangleVertices[0] = trianglePoint1;
|
||||||
@ -82,7 +69,7 @@ namespace ephysics {
|
|||||||
triangleVertices[2] = trianglePoint3;
|
triangleVertices[2] = trianglePoint3;
|
||||||
}
|
}
|
||||||
SmoothMeshContactInfo() {
|
SmoothMeshContactInfo() {
|
||||||
// TODO: add it for etk::Vector
|
// TODO: add it for Vector
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
@ -99,17 +86,17 @@ namespace ephysics {
|
|||||||
* of the concave triangle mesh to temporary store them in order to be used in
|
* of the concave triangle mesh to temporary store them in order to be used in
|
||||||
* the smooth mesh collision algorithm if this one is enabled.
|
* the smooth mesh collision algorithm if this one is enabled.
|
||||||
*/
|
*/
|
||||||
class SmoothCollisionNarrowPhaseCallback : public NarrowPhaseCallback {
|
class SmoothCollisionNarrowPhaseCallback extends NarrowPhaseCallback {
|
||||||
private:
|
private:
|
||||||
etk::Vector<SmoothMeshContactInfo> this.contactPoints;
|
Vector<SmoothMeshContactInfo> this.contactPoints;
|
||||||
public:
|
public:
|
||||||
// Constructor
|
// Constructor
|
||||||
SmoothCollisionNarrowPhaseCallback(etk::Vector<SmoothMeshContactInfo> contactPoints):
|
SmoothCollisionNarrowPhaseCallback(Vector<SmoothMeshContactInfo> contactPoints):
|
||||||
this.contactPoints(contactPoints) {
|
this.contactPoints(contactPoints) {
|
||||||
|
|
||||||
}
|
}
|
||||||
/// Called by a narrow-phase collision algorithm when a new contact has been found
|
/// Called by a narrow-phase collision algorithm when a new contact has been found
|
||||||
virtual void notifyContact(OverlappingPair* overlappingPair, ContactPointInfo contactInfo);
|
void notifyContact(OverlappingPair* overlappingPair, ContactPointInfo contactInfo);
|
||||||
};
|
};
|
||||||
|
|
||||||
/**
|
/**
|
||||||
@ -118,7 +105,7 @@ namespace ephysics {
|
|||||||
* to use the GJK collision detection algorithm to compute the collision between
|
* to use the GJK collision detection algorithm to compute the collision between
|
||||||
* the convex shape and each of the triangles in the concave shape.
|
* the convex shape and each of the triangles in the concave shape.
|
||||||
*/
|
*/
|
||||||
class ConcaveVsConvexAlgorithm : public NarrowPhaseAlgorithm {
|
class ConcaveVsConvexAlgorithm extends NarrowPhaseAlgorithm {
|
||||||
protected :
|
protected :
|
||||||
/// Private copy-ructor
|
/// Private copy-ructor
|
||||||
ConcaveVsConvexAlgorithm( ConcaveVsConvexAlgorithm algorithm);
|
ConcaveVsConvexAlgorithm( ConcaveVsConvexAlgorithm algorithm);
|
||||||
@ -126,20 +113,20 @@ namespace ephysics {
|
|||||||
ConcaveVsConvexAlgorithm operator=( ConcaveVsConvexAlgorithm algorithm);
|
ConcaveVsConvexAlgorithm operator=( ConcaveVsConvexAlgorithm algorithm);
|
||||||
/// Process the concave triangle mesh collision using the smooth mesh collision algorithm
|
/// Process the concave triangle mesh collision using the smooth mesh collision algorithm
|
||||||
void processSmoothMeshCollision(OverlappingPair* overlappingPair,
|
void processSmoothMeshCollision(OverlappingPair* overlappingPair,
|
||||||
etk::Vector<SmoothMeshContactInfo> contactPoints,
|
Vector<SmoothMeshContactInfo> contactPoints,
|
||||||
NarrowPhaseCallback* narrowPhaseCallback);
|
NarrowPhaseCallback* narrowPhaseCallback);
|
||||||
/// Add a triangle vertex into the set of processed triangles
|
/// Add a triangle vertex into the set of processed triangles
|
||||||
void addProcessedVertex(etk::Vector<etk::Pair<int, vec3>> processTriangleVertices, vec3 vertex) {
|
void addProcessedVertex(Vector<Pair<int, Vector3f>> processTriangleVertices, Vector3f vertex) {
|
||||||
processTriangleVertices.pushBack(etk::makePair(int(vertex.x() * vertex.y() * vertex.z()), vertex));
|
processTriangleVertices.pushBack(makePair(int(vertex.x() * vertex.y() * vertex.z()), vertex));
|
||||||
}
|
}
|
||||||
/// Return true if the vertex is in the set of already processed vertices
|
/// Return true if the vertex is in the set of already processed vertices
|
||||||
boolean hasVertexBeenProcessed( etk::Vector<etk::Pair<int, vec3>> processTriangleVertices,
|
boolean hasVertexBeenProcessed( Vector<Pair<int, Vector3f>> processTriangleVertices,
|
||||||
vec3 vertex) ;
|
Vector3f vertex) ;
|
||||||
public :
|
public :
|
||||||
/// Constructor
|
/// Constructor
|
||||||
ConcaveVsConvexAlgorithm();
|
ConcaveVsConvexAlgorithm();
|
||||||
/// Compute a contact info if the two bounding volume collide
|
/// Compute a contact info if the two bounding volume collide
|
||||||
virtual void testCollision( CollisionShapeInfo shape1Info,
|
void testCollision( CollisionShapeInfo shape1Info,
|
||||||
CollisionShapeInfo shape2Info,
|
CollisionShapeInfo shape2Info,
|
||||||
NarrowPhaseCallback* narrowPhaseCallback);
|
NarrowPhaseCallback* narrowPhaseCallback);
|
||||||
};
|
};
|
@ -30,15 +30,15 @@ NarrowPhaseAlgorithm* DefaultCollisionDispatch::selectAlgorithm(int type1, int t
|
|||||||
CollisionShapeType shape1Type = staticcast<CollisionShapeType>(type1);
|
CollisionShapeType shape1Type = staticcast<CollisionShapeType>(type1);
|
||||||
CollisionShapeType shape2Type = staticcast<CollisionShapeType>(type2);
|
CollisionShapeType shape2Type = staticcast<CollisionShapeType>(type2);
|
||||||
// Sphere vs Sphere algorithm
|
// Sphere vs Sphere algorithm
|
||||||
if (shape1Type == SPHERE hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj shape2Type == SPHERE) {
|
if (shape1Type == SPHERE && shape2Type == SPHERE) {
|
||||||
return this.sphereVsSphereAlgorithm;
|
return this.sphereVsSphereAlgorithm;
|
||||||
} else if ( ( !CollisionShape::isConvex(shape1Type)
|
} else if ( ( !CollisionShape::isConvex(shape1Type)
|
||||||
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj CollisionShape::isConvex(shape2Type) )
|
&& CollisionShape::isConvex(shape2Type) )
|
||||||
|| ( !CollisionShape::isConvex(shape2Type)
|
|| ( !CollisionShape::isConvex(shape2Type)
|
||||||
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj CollisionShape::isConvex(shape1Type) ) ) {
|
&& CollisionShape::isConvex(shape1Type) ) ) {
|
||||||
// Concave vs Convex algorithm
|
// Concave vs Convex algorithm
|
||||||
return this.concaveVsConvexAlgorithm;
|
return this.concaveVsConvexAlgorithm;
|
||||||
} else if (CollisionShape::isConvex(shape1Type) hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj CollisionShape::isConvex(shape2Type)) {
|
} else if (CollisionShape::isConvex(shape1Type) && CollisionShape::isConvex(shape2Type)) {
|
||||||
// Convex vs Convex algorithm (GJK algorithm)
|
// Convex vs Convex algorithm (GJK algorithm)
|
||||||
return this.GJKAlgorithm;
|
return this.GJKAlgorithm;
|
||||||
} else {
|
} else {
|
||||||
|
@ -1,37 +0,0 @@
|
|||||||
/** @file
|
|
||||||
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
|
|
||||||
* @author Daniel CHAPPUIS
|
|
||||||
* @author Edouard DUPIN
|
|
||||||
* @copyright 2010-2016, Daniel Chappuis
|
|
||||||
* @copyright 2017, Edouard DUPIN
|
|
||||||
* @license MPL v2.0 (see license file)
|
|
||||||
*/
|
|
||||||
#pragma once
|
|
||||||
|
|
||||||
#include <ephysics/collision/narrowphase/CollisionDispatch.hpp>
|
|
||||||
#include <ephysics/collision/narrowphase/ConcaveVsConvexAlgorithm.hpp>
|
|
||||||
#include <ephysics/collision/narrowphase/SphereVsSphereAlgorithm.hpp>
|
|
||||||
#include <ephysics/collision/narrowphase/GJK/GJKAlgorithm.hpp>
|
|
||||||
|
|
||||||
namespace ephysics {
|
|
||||||
/**
|
|
||||||
* @brief This is the default collision dispatch configuration use in ephysics.
|
|
||||||
* Collision dispatching decides which collision
|
|
||||||
* algorithm to use given two types of proxy collision shapes.
|
|
||||||
*/
|
|
||||||
class DefaultCollisionDispatch : public CollisionDispatch {
|
|
||||||
protected:
|
|
||||||
SphereVsSphereAlgorithm this.sphereVsSphereAlgorithm; //!< Sphere vs Sphere collision algorithm
|
|
||||||
ConcaveVsConvexAlgorithm this.concaveVsConvexAlgorithm; //!< Concave vs Convex collision algorithm
|
|
||||||
GJKAlgorithm this.GJKAlgorithm; //!< GJK Algorithm
|
|
||||||
public:
|
|
||||||
/**
|
|
||||||
* @brief Constructor
|
|
||||||
*/
|
|
||||||
DefaultCollisionDispatch();
|
|
||||||
void init(CollisionDetection* collisionDetection) override;
|
|
||||||
NarrowPhaseAlgorithm* selectAlgorithm(int type1, int type2) override;
|
|
||||||
};
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
@ -0,0 +1,23 @@
|
|||||||
|
package org.atriaSoft.ephysics.collision.narrowphase;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief This is the default collision dispatch configuration use in ephysics.
|
||||||
|
* Collision dispatching decides which collision
|
||||||
|
* algorithm to use given two types of proxy collision shapes.
|
||||||
|
*/
|
||||||
|
class DefaultCollisionDispatch extends CollisionDispatch {
|
||||||
|
protected:
|
||||||
|
SphereVsSphereAlgorithm this.sphereVsSphereAlgorithm; //!< Sphere vs Sphere collision algorithm
|
||||||
|
ConcaveVsConvexAlgorithm this.concaveVsConvexAlgorithm; //!< Concave vs Convex collision algorithm
|
||||||
|
GJKAlgorithm this.GJKAlgorithm; //!< GJK Algorithm
|
||||||
|
public:
|
||||||
|
/**
|
||||||
|
* @brief Constructor
|
||||||
|
*/
|
||||||
|
DefaultCollisionDispatch();
|
||||||
|
void init(CollisionDetection* collisionDetection) ;
|
||||||
|
NarrowPhaseAlgorithm* selectAlgorithm(int type1, int type2) ;
|
||||||
|
};
|
||||||
|
}
|
||||||
|
|
||||||
|
|
@ -21,24 +21,24 @@ EPAAlgorithm::~EPAAlgorithm() {
|
|||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
int EPAAlgorithm::isOriginInTetrahedron( vec3 p1, vec3 p2, vec3 p3, vec3 p4) {
|
int EPAAlgorithm::isOriginInTetrahedron( Vector3f p1, Vector3f p2, Vector3f p3, Vector3f p4) {
|
||||||
// Check vertex 1
|
// Check vertex 1
|
||||||
vec3 normal1 = (p2-p1).cross(p3-p1);
|
Vector3f normal1 = (p2-p1).cross(p3-p1);
|
||||||
if ((normal1.dot(p1) > 0.0) == (normal1.dot(p4) > 0.0)) {
|
if ((normal1.dot(p1) > 0.0) == (normal1.dot(p4) > 0.0)) {
|
||||||
return 4;
|
return 4;
|
||||||
}
|
}
|
||||||
// Check vertex 2
|
// Check vertex 2
|
||||||
vec3 normal2 = (p4-p2).cross(p3-p2);
|
Vector3f normal2 = (p4-p2).cross(p3-p2);
|
||||||
if ((normal2.dot(p2) > 0.0) == (normal2.dot(p1) > 0.0)) {
|
if ((normal2.dot(p2) > 0.0) == (normal2.dot(p1) > 0.0)) {
|
||||||
return 1;
|
return 1;
|
||||||
}
|
}
|
||||||
// Check vertex 3
|
// Check vertex 3
|
||||||
vec3 normal3 = (p4-p3).cross(p1-p3);
|
Vector3f normal3 = (p4-p3).cross(p1-p3);
|
||||||
if ((normal3.dot(p3) > 0.0) == (normal3.dot(p2) > 0.0)) {
|
if ((normal3.dot(p3) > 0.0) == (normal3.dot(p2) > 0.0)) {
|
||||||
return 2;
|
return 2;
|
||||||
}
|
}
|
||||||
// Check vertex 4
|
// Check vertex 4
|
||||||
vec3 normal4 = (p2-p4).cross(p1-p4);
|
Vector3f normal4 = (p2-p4).cross(p1-p4);
|
||||||
if ((normal4.dot(p4) > 0.0) == (normal4.dot(p3) > 0.0)) {
|
if ((normal4.dot(p4) > 0.0) == (normal4.dot(p3) > 0.0)) {
|
||||||
return 3;
|
return 3;
|
||||||
}
|
}
|
||||||
@ -48,32 +48,32 @@ int EPAAlgorithm::isOriginInTetrahedron( vec3 p1, vec3 p2, vec3 p3, vec3 p4)
|
|||||||
|
|
||||||
void EPAAlgorithm::computePenetrationDepthAndContactPoints( Simplex simplex,
|
void EPAAlgorithm::computePenetrationDepthAndContactPoints( Simplex simplex,
|
||||||
CollisionShapeInfo shape1Info,
|
CollisionShapeInfo shape1Info,
|
||||||
etk::Transform3D transform1,
|
Transform3D transform1,
|
||||||
CollisionShapeInfo shape2Info,
|
CollisionShapeInfo shape2Info,
|
||||||
etk::Transform3D transform2,
|
Transform3D transform2,
|
||||||
vec3 vector,
|
Vector3f vector,
|
||||||
NarrowPhaseCallback* narrowPhaseCallback) {
|
NarrowPhaseCallback* narrowPhaseCallback) {
|
||||||
PROFILE("EPAAlgorithm::computePenetrationDepthAndContactPoints()");
|
PROFILE("EPAAlgorithm::computePenetrationDepthAndContactPoints()");
|
||||||
assert(shape1Info.collisionShape->isConvex());
|
assert(shape1Info.collisionShape.isConvex());
|
||||||
assert(shape2Info.collisionShape->isConvex());
|
assert(shape2Info.collisionShape.isConvex());
|
||||||
ConvexShape* shape1 = staticcast< ConvexShape*>(shape1Info.collisionShape);
|
ConvexShape* shape1 = staticcast< ConvexShape*>(shape1Info.collisionShape);
|
||||||
ConvexShape* shape2 = staticcast< ConvexShape*>(shape2Info.collisionShape);
|
ConvexShape* shape2 = staticcast< ConvexShape*>(shape2Info.collisionShape);
|
||||||
void** shape1CachedCollisionData = shape1Info.cachedCollisionData;
|
void** shape1CachedCollisionData = shape1Info.cachedCollisionData;
|
||||||
void** shape2CachedCollisionData = shape2Info.cachedCollisionData;
|
void** shape2CachedCollisionData = shape2Info.cachedCollisionData;
|
||||||
vec3 suppPointsA[MAXSUPPORTPOINTS]; // Support points of object A in local coordinates
|
Vector3f suppPointsA[MAXSUPPORTPOINTS]; // Support points of object A in local coordinates
|
||||||
vec3 suppPointsB[MAXSUPPORTPOINTS]; // Support points of object B in local coordinates
|
Vector3f suppPointsB[MAXSUPPORTPOINTS]; // Support points of object B in local coordinates
|
||||||
vec3 points[MAXSUPPORTPOINTS]; // Current points
|
Vector3f points[MAXSUPPORTPOINTS]; // Current points
|
||||||
TrianglesStore triangleStore; // Store the triangles
|
TrianglesStore triangleStore; // Store the triangles
|
||||||
etk::Set<TriangleEPA*> triangleHeap; // list of face candidate of the EPA algorithm sorted lower square dist to upper square dist
|
Set<TriangleEPA*> triangleHeap; // list of face candidate of the EPA algorithm sorted lower square dist to upper square dist
|
||||||
triangleHeap.setComparator([](TriangleEPA * face1, TriangleEPA * face2) {
|
triangleHeap.setComparator([](TriangleEPA * face1, TriangleEPA * face2) {
|
||||||
return (face1->getDistSquare() < face2->getDistSquare());
|
return (face1.getDistSquare() < face2.getDistSquare());
|
||||||
});
|
});
|
||||||
// etk::Transform3D a point from local space of body 2 to local
|
// Transform3D a point from local space of body 2 to local
|
||||||
// space of body 1 (the GJK algorithm is done in local space of body 1)
|
// space of body 1 (the GJK algorithm is done in local space of body 1)
|
||||||
etk::Transform3D body2Tobody1 = transform1.getInverse() * transform2;
|
Transform3D body2Tobody1 = transform1.getInverse() * transform2;
|
||||||
// Matrix that transform a direction from local
|
// Matrix that transform a direction from local
|
||||||
// space of body 1 into local space of body 2
|
// space of body 1 into local space of body 2
|
||||||
etk::Quaternion rotateToBody2 = transform2.getOrientation().getInverse() * transform1.getOrientation();
|
Quaternion rotateToBody2 = transform2.getOrientation().getInverse() * transform1.getOrientation();
|
||||||
// Get the simplex computed previously by the GJK algorithm
|
// Get the simplex computed previously by the GJK algorithm
|
||||||
int nbVertices = simplex.getSimplex(suppPointsA, suppPointsB, points);
|
int nbVertices = simplex.getSimplex(suppPointsA, suppPointsB, points);
|
||||||
// Compute the tolerance
|
// Compute the tolerance
|
||||||
@ -98,32 +98,32 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints( Simplex simplex,
|
|||||||
// ruct the polytope are the three support points in those three directions
|
// ruct the polytope are the three support points in those three directions
|
||||||
// v1, v2 and v3.
|
// v1, v2 and v3.
|
||||||
// Direction of the segment
|
// Direction of the segment
|
||||||
vec3 d = (points[1] - points[0]).safeNormalized();
|
Vector3f d = (points[1] - points[0]).safeNormalized();
|
||||||
// Choose the coordinate axis from the minimal absolute component of the vector d
|
// Choose the coordinate axis from the minimal absolute component of the vector d
|
||||||
int minAxis = d.absolute().getMinAxis();
|
int minAxis = d.absolute().getMinAxis();
|
||||||
// Compute sin(60)
|
// Compute sin(60)
|
||||||
float sin60 = float(sqrt(3.0)) * 0.5f;
|
float sin60 = float(sqrt(3.0)) * 0.5f;
|
||||||
// Create a rotation quaternion to rotate the vector v1 to get the vectors
|
// Create a rotation quaternion to rotate the vector v1 to get the vectors
|
||||||
// v2 and v3
|
// v2 and v3
|
||||||
etk::Quaternion rotationQuat(d.x() * sin60, d.y() * sin60, d.z() * sin60, 0.5);
|
Quaternion rotationQuat(d.x() * sin60, d.y() * sin60, d.z() * sin60, 0.5);
|
||||||
// Compute the vector v1, v2, v3
|
// Compute the vector v1, v2, v3
|
||||||
vec3 v1 = d.cross(vec3(minAxis == 0, minAxis == 1, minAxis == 2));
|
Vector3f v1 = d.cross(Vector3f(minAxis == 0, minAxis == 1, minAxis == 2));
|
||||||
vec3 v2 = rotationQuat * v1;
|
Vector3f v2 = rotationQuat * v1;
|
||||||
vec3 v3 = rotationQuat * v2;
|
Vector3f v3 = rotationQuat * v2;
|
||||||
// Compute the support point in the direction of v1
|
// Compute the support point in the direction of v1
|
||||||
suppPointsA[2] = shape1->getLocalSupportPointWithMargin(v1, shape1CachedCollisionData);
|
suppPointsA[2] = shape1.getLocalSupportPointWithMargin(v1, shape1CachedCollisionData);
|
||||||
suppPointsB[2] = body2Tobody1 *
|
suppPointsB[2] = body2Tobody1 *
|
||||||
shape2->getLocalSupportPointWithMargin(rotateToBody2 * (-v1), shape2CachedCollisionData);
|
shape2.getLocalSupportPointWithMargin(rotateToBody2 * (-v1), shape2CachedCollisionData);
|
||||||
points[2] = suppPointsA[2] - suppPointsB[2];
|
points[2] = suppPointsA[2] - suppPointsB[2];
|
||||||
// Compute the support point in the direction of v2
|
// Compute the support point in the direction of v2
|
||||||
suppPointsA[3] = shape1->getLocalSupportPointWithMargin(v2, shape1CachedCollisionData);
|
suppPointsA[3] = shape1.getLocalSupportPointWithMargin(v2, shape1CachedCollisionData);
|
||||||
suppPointsB[3] = body2Tobody1 *
|
suppPointsB[3] = body2Tobody1 *
|
||||||
shape2->getLocalSupportPointWithMargin(rotateToBody2 * (-v2), shape2CachedCollisionData);
|
shape2.getLocalSupportPointWithMargin(rotateToBody2 * (-v2), shape2CachedCollisionData);
|
||||||
points[3] = suppPointsA[3] - suppPointsB[3];
|
points[3] = suppPointsA[3] - suppPointsB[3];
|
||||||
// Compute the support point in the direction of v3
|
// Compute the support point in the direction of v3
|
||||||
suppPointsA[4] = shape1->getLocalSupportPointWithMargin(v3, shape1CachedCollisionData);
|
suppPointsA[4] = shape1.getLocalSupportPointWithMargin(v3, shape1CachedCollisionData);
|
||||||
suppPointsB[4] = body2Tobody1 *
|
suppPointsB[4] = body2Tobody1 *
|
||||||
shape2->getLocalSupportPointWithMargin(rotateToBody2 * (-v3), shape2CachedCollisionData);
|
shape2.getLocalSupportPointWithMargin(rotateToBody2 * (-v3), shape2CachedCollisionData);
|
||||||
points[4] = suppPointsA[4] - suppPointsB[4];
|
points[4] = suppPointsA[4] - suppPointsB[4];
|
||||||
// Now we have an hexahedron (two tetrahedron glued together). We can simply keep the
|
// Now we have an hexahedron (two tetrahedron glued together). We can simply keep the
|
||||||
// tetrahedron that contains the origin in order that the initial polytope of the
|
// tetrahedron that contains the origin in order that the initial polytope of the
|
||||||
@ -166,9 +166,9 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints( Simplex simplex,
|
|||||||
TriangleEPA* face2 = triangleStore.newTriangle(points, 0, 2, 3);
|
TriangleEPA* face2 = triangleStore.newTriangle(points, 0, 2, 3);
|
||||||
TriangleEPA* face3 = triangleStore.newTriangle(points, 1, 3, 2);
|
TriangleEPA* face3 = triangleStore.newTriangle(points, 1, 3, 2);
|
||||||
// If the ructed tetrahedron is not correct
|
// If the ructed tetrahedron is not correct
|
||||||
if (!((face0 != NULL) hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj (face1 != NULL) hjkhjkhjkhkj (face2 != NULL) hjkhjkhjkhkj (face3 != NULL)
|
if (!((face0 != NULL) && (face1 != NULL) hjkhjkhjkhkj (face2 != NULL) hjkhjkhjkhkj (face3 != NULL)
|
||||||
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj face0->getDistSquare() > 0.0 hjkhjkhjkhkj face1->getDistSquare() > 0.0
|
&& face0.getDistSquare() > 0.0 hjkhjkhjkhkj face1.getDistSquare() > 0.0
|
||||||
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj face2->getDistSquare() > 0.0 hjkhjkhjkhkj face3->getDistSquare() > 0.0)) {
|
&& face2.getDistSquare() > 0.0 hjkhjkhjkhkj face3.getDistSquare() > 0.0)) {
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
// Associate the edges of neighbouring triangle faces
|
// Associate the edges of neighbouring triangle faces
|
||||||
@ -203,17 +203,17 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints( Simplex simplex,
|
|||||||
// where "n" is the normal of the triangle. Then, we use only the
|
// where "n" is the normal of the triangle. Then, we use only the
|
||||||
// tetrahedron that contains the origin.
|
// tetrahedron that contains the origin.
|
||||||
// Compute the normal of the triangle
|
// Compute the normal of the triangle
|
||||||
vec3 v1 = points[1] - points[0];
|
Vector3f v1 = points[1] - points[0];
|
||||||
vec3 v2 = points[2] - points[0];
|
Vector3f v2 = points[2] - points[0];
|
||||||
vec3 n = v1.cross(v2);
|
Vector3f n = v1.cross(v2);
|
||||||
// Compute the two new vertices to obtain a hexahedron
|
// Compute the two new vertices to obtain a hexahedron
|
||||||
suppPointsA[3] = shape1->getLocalSupportPointWithMargin(n, shape1CachedCollisionData);
|
suppPointsA[3] = shape1.getLocalSupportPointWithMargin(n, shape1CachedCollisionData);
|
||||||
suppPointsB[3] = body2Tobody1 *
|
suppPointsB[3] = body2Tobody1 *
|
||||||
shape2->getLocalSupportPointWithMargin(rotateToBody2 * (-n), shape2CachedCollisionData);
|
shape2.getLocalSupportPointWithMargin(rotateToBody2 * (-n), shape2CachedCollisionData);
|
||||||
points[3] = suppPointsA[3] - suppPointsB[3];
|
points[3] = suppPointsA[3] - suppPointsB[3];
|
||||||
suppPointsA[4] = shape1->getLocalSupportPointWithMargin(-n, shape1CachedCollisionData);
|
suppPointsA[4] = shape1.getLocalSupportPointWithMargin(-n, shape1CachedCollisionData);
|
||||||
suppPointsB[4] = body2Tobody1 *
|
suppPointsB[4] = body2Tobody1 *
|
||||||
shape2->getLocalSupportPointWithMargin(rotateToBody2 * n, shape2CachedCollisionData);
|
shape2.getLocalSupportPointWithMargin(rotateToBody2 * n, shape2CachedCollisionData);
|
||||||
points[4] = suppPointsA[4] - suppPointsB[4];
|
points[4] = suppPointsA[4] - suppPointsB[4];
|
||||||
TriangleEPA* face0 = null;
|
TriangleEPA* face0 = null;
|
||||||
TriangleEPA* face1 = null;
|
TriangleEPA* face1 = null;
|
||||||
@ -245,13 +245,13 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints( Simplex simplex,
|
|||||||
}
|
}
|
||||||
// If the ructed tetrahedron is not correct
|
// If the ructed tetrahedron is not correct
|
||||||
if (!( face0 != null
|
if (!( face0 != null
|
||||||
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj face1 != null
|
&& face1 != null
|
||||||
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj face2 != null
|
&& face2 != null
|
||||||
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj face3 != null
|
&& face3 != null
|
||||||
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj face0->getDistSquare() > 0.0
|
&& face0.getDistSquare() > 0.0
|
||||||
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj face1->getDistSquare() > 0.0
|
&& face1.getDistSquare() > 0.0
|
||||||
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj face2->getDistSquare() > 0.0
|
&& face2.getDistSquare() > 0.0
|
||||||
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj face3->getDistSquare() > 0.0) ) {
|
&& face3.getDistSquare() > 0.0) ) {
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
// Associate the edges of neighbouring triangle faces
|
// Associate the edges of neighbouring triangle faces
|
||||||
@ -281,11 +281,11 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints( Simplex simplex,
|
|||||||
triangle = triangleHeap[0];
|
triangle = triangleHeap[0];
|
||||||
triangleHeap.popFront();
|
triangleHeap.popFront();
|
||||||
Log.info("rm from heap:");
|
Log.info("rm from heap:");
|
||||||
for (sizet iii=0; iii<triangleHeap.size(); ++iii) {
|
for (long iii=0; iii<triangleHeap.size(); ++iii) {
|
||||||
Log.info(" [" << iii << "] " << triangleHeap[iii]->getDistSquare());
|
Log.info(" [" + iii + "] " + triangleHeap[iii].getDistSquare());
|
||||||
}
|
}
|
||||||
// If the candidate face in the heap is not obsolete
|
// If the candidate face in the heap is not obsolete
|
||||||
if (!triangle->getIsObsolete()) {
|
if (!triangle.getIsObsolete()) {
|
||||||
// If we have reached the maximum number of support points
|
// If we have reached the maximum number of support points
|
||||||
if (nbVertices == MAXSUPPORTPOINTS) {
|
if (nbVertices == MAXSUPPORTPOINTS) {
|
||||||
assert(false);
|
assert(false);
|
||||||
@ -293,28 +293,28 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints( Simplex simplex,
|
|||||||
}
|
}
|
||||||
// Compute the support point of the Minkowski
|
// Compute the support point of the Minkowski
|
||||||
// difference (A-B) in the closest point direction
|
// difference (A-B) in the closest point direction
|
||||||
suppPointsA[nbVertices] = shape1->getLocalSupportPointWithMargin(triangle->getClosestPoint(), shape1CachedCollisionData);
|
suppPointsA[nbVertices] = shape1.getLocalSupportPointWithMargin(triangle.getClosestPoint(), shape1CachedCollisionData);
|
||||||
suppPointsB[nbVertices] = body2Tobody1 * shape2->getLocalSupportPointWithMargin(rotateToBody2 * (-triangle->getClosestPoint()), shape2CachedCollisionData);
|
suppPointsB[nbVertices] = body2Tobody1 * shape2.getLocalSupportPointWithMargin(rotateToBody2 * (-triangle.getClosestPoint()), shape2CachedCollisionData);
|
||||||
points[nbVertices] = suppPointsA[nbVertices] - suppPointsB[nbVertices];
|
points[nbVertices] = suppPointsA[nbVertices] - suppPointsB[nbVertices];
|
||||||
int indexNewVertex = nbVertices;
|
int indexNewVertex = nbVertices;
|
||||||
nbVertices++;
|
nbVertices++;
|
||||||
// Update the upper bound of the penetration depth
|
// Update the upper bound of the penetration depth
|
||||||
float wDotv = points[indexNewVertex].dot(triangle->getClosestPoint());
|
float wDotv = points[indexNewVertex].dot(triangle.getClosestPoint());
|
||||||
Log.info(" point=" << points[indexNewVertex]);
|
Log.info(" point=" + points[indexNewVertex]);
|
||||||
Log.info("close point=" << triangle->getClosestPoint());
|
Log.info("close point=" + triangle.getClosestPoint());
|
||||||
Log.info(" ==>" << wDotv);
|
Log.info(" ==>" + wDotv);
|
||||||
if (wDotv < 0.0) {
|
if (wDotv < 0.0) {
|
||||||
Log.error("depth penetration error " << wDotv);
|
Log.error("depth penetration error " + wDotv);
|
||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
EPHYASSERT(wDotv >= 0.0, "depth penetration error " << wDotv);
|
EPHYASSERT(wDotv >= 0.0, "depth penetration error " + wDotv);
|
||||||
float wDotVSquare = wDotv * wDotv / triangle->getDistSquare();
|
float wDotVSquare = wDotv * wDotv / triangle.getDistSquare();
|
||||||
if (wDotVSquare < upperBoundSquarePenDepth) {
|
if (wDotVSquare < upperBoundSquarePenDepth) {
|
||||||
upperBoundSquarePenDepth = wDotVSquare;
|
upperBoundSquarePenDepth = wDotVSquare;
|
||||||
}
|
}
|
||||||
// Compute the error
|
// Compute the error
|
||||||
float error = wDotv - triangle->getDistSquare();
|
float error = wDotv - triangle.getDistSquare();
|
||||||
if (error <= etk::max(tolerance, RELERRORSQUARE * wDotv) ||
|
if (error <= max(tolerance, RELERRORSQUARE * wDotv) ||
|
||||||
points[indexNewVertex] == points[(*triangle)[0]] ||
|
points[indexNewVertex] == points[(*triangle)[0]] ||
|
||||||
points[indexNewVertex] == points[(*triangle)[1]] ||
|
points[indexNewVertex] == points[(*triangle)[1]] ||
|
||||||
points[indexNewVertex] == points[(*triangle)[2]]) {
|
points[indexNewVertex] == points[(*triangle)[2]]) {
|
||||||
@ -323,8 +323,8 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints( Simplex simplex,
|
|||||||
// Now, we compute the silhouette cast by the new vertex. The current triangle
|
// Now, we compute the silhouette cast by the new vertex. The current triangle
|
||||||
// face will not be in the convex hull. We start the local recursive silhouette
|
// face will not be in the convex hull. We start the local recursive silhouette
|
||||||
// algorithm from the current triangle face.
|
// algorithm from the current triangle face.
|
||||||
sizet i = triangleStore.getNbTriangles();
|
long i = triangleStore.getNbTriangles();
|
||||||
if (!triangle->computeSilhouette(points, indexNewVertex, triangleStore)) {
|
if (!triangle.computeSilhouette(points, indexNewVertex, triangleStore)) {
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
// Add all the new triangle faces computed with the silhouette algorithm
|
// Add all the new triangle faces computed with the silhouette algorithm
|
||||||
@ -336,12 +336,12 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints( Simplex simplex,
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
} while( triangleHeap.size() > 0
|
} while( triangleHeap.size() > 0
|
||||||
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj triangleHeap[0]->getDistSquare() <= upperBoundSquarePenDepth);
|
&& triangleHeap[0].getDistSquare() <= upperBoundSquarePenDepth);
|
||||||
// Compute the contact info
|
// Compute the contact info
|
||||||
vector = transform1.getOrientation() * triangle->getClosestPoint();
|
vector = transform1.getOrientation() * triangle.getClosestPoint();
|
||||||
vec3 pALocal = triangle->computeClosestPointOfObject(suppPointsA);
|
Vector3f pALocal = triangle.computeClosestPointOfObject(suppPointsA);
|
||||||
vec3 pBLocal = body2Tobody1.getInverse() * triangle->computeClosestPointOfObject(suppPointsB);
|
Vector3f pBLocal = body2Tobody1.getInverse() * triangle.computeClosestPointOfObject(suppPointsB);
|
||||||
vec3 normal = vector.safeNormalized();
|
Vector3f normal = vector.safeNormalized();
|
||||||
float penetrationDepth = vector.length();
|
float penetrationDepth = vector.length();
|
||||||
EPHYASSERT(penetrationDepth >= 0.0, "penetration depth <0");
|
EPHYASSERT(penetrationDepth >= 0.0, "penetration depth <0");
|
||||||
if (normal.length2() < FLTEPSILON) {
|
if (normal.length2() < FLTEPSILON) {
|
||||||
@ -349,5 +349,5 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints( Simplex simplex,
|
|||||||
}
|
}
|
||||||
// Create the contact info object
|
// Create the contact info object
|
||||||
ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape, shape2Info.collisionShape, normal, penetrationDepth, pALocal, pBLocal);
|
ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape, shape2Info.collisionShape, normal, penetrationDepth, pALocal, pBLocal);
|
||||||
narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo);
|
narrowPhaseCallback.notifyContact(shape1Info.overlappingPair, contactInfo);
|
||||||
}
|
}
|
||||||
|
@ -1,23 +1,5 @@
|
|||||||
/** @file
|
package org.atriaSoft.ephysics.collision.narrowphase.EPA;
|
||||||
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
|
|
||||||
* @author Daniel CHAPPUIS
|
|
||||||
* @author Edouard DUPIN
|
|
||||||
* @copyright 2010-2016, Daniel Chappuis
|
|
||||||
* @copyright 2017, Edouard DUPIN
|
|
||||||
* @license MPL v2.0 (see license file)
|
|
||||||
*/
|
|
||||||
#pragma once
|
|
||||||
#include <ephysics/collision/narrowphase/GJK/Simplex.hpp>
|
|
||||||
#include <ephysics/collision/shapes/CollisionShape.hpp>
|
|
||||||
#include <ephysics/collision/CollisionShapeInfo.hpp>
|
|
||||||
#include <ephysics/raint/ContactPoint.hpp>
|
|
||||||
#include <ephysics/collision/narrowphase/NarrowPhaseAlgorithm.hpp>
|
|
||||||
#include <ephysics/mathematics/mathematics.hpp>
|
|
||||||
#include <ephysics/collision/narrowphase/EPA/TriangleEPA.hpp>
|
|
||||||
#include <ephysics/debug.hpp>
|
|
||||||
#include <etk/Set.hpp>
|
|
||||||
|
|
||||||
namespace ephysics {
|
|
||||||
/// Maximum number of support points of the polytope
|
/// Maximum number of support points of the polytope
|
||||||
int MAXSUPPORTPOINTS = 100;
|
int MAXSUPPORTPOINTS = 100;
|
||||||
/// Maximum number of facets of the polytope
|
/// Maximum number of facets of the polytope
|
||||||
@ -44,31 +26,29 @@ namespace ephysics {
|
|||||||
EPAAlgorithm operator=( EPAAlgorithm algorithm);
|
EPAAlgorithm operator=( EPAAlgorithm algorithm);
|
||||||
/// Add a triangle face in the candidate triangle heap
|
/// Add a triangle face in the candidate triangle heap
|
||||||
void addFaceCandidate(TriangleEPA* triangle,
|
void addFaceCandidate(TriangleEPA* triangle,
|
||||||
etk::Set<TriangleEPA*> heap,
|
Set<TriangleEPA*> heap,
|
||||||
float upperBoundSquarePenDepth) {
|
float upperBoundSquarePenDepth) {
|
||||||
// If the closest point of the affine hull of triangle
|
// If the closest point of the affine hull of triangle
|
||||||
// points is internal to the triangle and if the distance
|
// points is internal to the triangle and if the distance
|
||||||
// of the closest point from the origin is at most the
|
// of the closest point from the origin is at most the
|
||||||
// penetration depth upper bound
|
// penetration depth upper bound
|
||||||
if ( triangle->isClosestPointInternalToTriangle()
|
if ( triangle.isClosestPointInternalToTriangle()
|
||||||
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj triangle->getDistSquare() <= upperBoundSquarePenDepth) {
|
&& triangle.getDistSquare() <= upperBoundSquarePenDepth) {
|
||||||
// Add the triangle face to the list of candidates
|
// Add the triangle face to the list of candidates
|
||||||
heap.add(triangle);
|
heap.add(triangle);
|
||||||
Log.info("add in heap:");
|
Log.info("add in heap:");
|
||||||
for (sizet iii=0; iii<heap.size(); ++iii) {
|
for (long iii=0; iii<heap.size(); ++iii) {
|
||||||
Log.info(" [" << iii << "] " << heap[iii]->getDistSquare());
|
Log.info(" [" + iii + "] " + heap[iii].getDistSquare());
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
// Decide if the origin is in the tetrahedron.
|
// Decide if the origin is in the tetrahedron.
|
||||||
/// Return 0 if the origin is in the tetrahedron and return the number (1,2,3 or 4) of
|
/// Return 0 if the origin is in the tetrahedron and return the number (1,2,3 or 4) of
|
||||||
/// the vertex that is wrong if the origin is not in the tetrahedron
|
/// the vertex that is wrong if the origin is not in the tetrahedron
|
||||||
int isOriginInTetrahedron( vec3 p1, vec3 p2, vec3 p3, vec3 p4) ;
|
int isOriginInTetrahedron( Vector3f p1, Vector3f p2, Vector3f p3, Vector3f p4) ;
|
||||||
public:
|
public:
|
||||||
/// Constructor
|
/// Constructor
|
||||||
EPAAlgorithm();
|
EPAAlgorithm();
|
||||||
/// Destructor
|
|
||||||
~EPAAlgorithm();
|
|
||||||
/// Initalize the algorithm
|
/// Initalize the algorithm
|
||||||
void init() {
|
void init() {
|
||||||
|
|
||||||
@ -81,10 +61,10 @@ namespace ephysics {
|
|||||||
/// the correct penetration depth
|
/// the correct penetration depth
|
||||||
void computePenetrationDepthAndContactPoints( Simplex simplex,
|
void computePenetrationDepthAndContactPoints( Simplex simplex,
|
||||||
CollisionShapeInfo shape1Info,
|
CollisionShapeInfo shape1Info,
|
||||||
etk::Transform3D transform1,
|
Transform3D transform1,
|
||||||
CollisionShapeInfo shape2Info,
|
CollisionShapeInfo shape2Info,
|
||||||
etk::Transform3D transform2,
|
Transform3D transform2,
|
||||||
vec3 v,
|
Vector3f v,
|
||||||
NarrowPhaseCallback* narrowPhaseCallback);
|
NarrowPhaseCallback* narrowPhaseCallback);
|
||||||
};
|
};
|
||||||
}
|
}
|
@ -21,19 +21,19 @@ EdgeEPA::EdgeEPA() {
|
|||||||
EdgeEPA::EdgeEPA(TriangleEPA* ownerTriangle, int index):
|
EdgeEPA::EdgeEPA(TriangleEPA* ownerTriangle, int index):
|
||||||
this.ownerTriangle(ownerTriangle),
|
this.ownerTriangle(ownerTriangle),
|
||||||
this.index(index) {
|
this.index(index) {
|
||||||
assert(index >= 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj index < 3);
|
assert(index >= 0 && index < 3);
|
||||||
}
|
}
|
||||||
|
|
||||||
EdgeEPA::EdgeEPA( EdgeEPA obj):
|
EdgeEPA::EdgeEPA( EdgeEPA obj):
|
||||||
this.ownerTriangle(obj.this.ownerTriangle),
|
this.ownerTriangle(obj.ownerTriangle),
|
||||||
this.index(obj.this.index) {
|
this.index(obj.index) {
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
EdgeEPA::EdgeEPA(EdgeEPAhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj obj):
|
EdgeEPA::EdgeEPA(EdgeEPA&& obj):
|
||||||
this.ownerTriangle(null) {
|
this.ownerTriangle(null) {
|
||||||
etk::swap(this.ownerTriangle, obj.this.ownerTriangle);
|
swap(this.ownerTriangle, obj.ownerTriangle);
|
||||||
etk::swap(this.index, obj.this.index);
|
swap(this.index, obj.index);
|
||||||
}
|
}
|
||||||
|
|
||||||
int EdgeEPA::getSourceVertexIndex() {
|
int EdgeEPA::getSourceVertexIndex() {
|
||||||
@ -44,12 +44,12 @@ int EdgeEPA::getTargetVertexIndex() {
|
|||||||
return (*this.ownerTriangle)[indexOfNextCounterClockwiseEdge(this.index)];
|
return (*this.ownerTriangle)[indexOfNextCounterClockwiseEdge(this.index)];
|
||||||
}
|
}
|
||||||
|
|
||||||
boolean EdgeEPA::computeSilhouette( vec3* vertices, int indexNewVertex,
|
boolean EdgeEPA::computeSilhouette( Vector3f* vertices, int indexNewVertex,
|
||||||
TrianglesStore triangleStore) {
|
TrianglesStore triangleStore) {
|
||||||
// If the edge has not already been visited
|
// If the edge has not already been visited
|
||||||
if (!this.ownerTriangle->getIsObsolete()) {
|
if (!this.ownerTriangle.getIsObsolete()) {
|
||||||
// If the triangle of this edge is not visible from the given point
|
// If the triangle of this edge is not visible from the given point
|
||||||
if (!this.ownerTriangle->isVisibleFromVertex(vertices, indexNewVertex)) {
|
if (!this.ownerTriangle.isVisibleFromVertex(vertices, indexNewVertex)) {
|
||||||
TriangleEPA* triangle = triangleStore.newTriangle(vertices, indexNewVertex,
|
TriangleEPA* triangle = triangleStore.newTriangle(vertices, indexNewVertex,
|
||||||
getTargetVertexIndex(),
|
getTargetVertexIndex(),
|
||||||
getSourceVertexIndex());
|
getSourceVertexIndex());
|
||||||
@ -61,12 +61,12 @@ boolean EdgeEPA::computeSilhouette( vec3* vertices, int indexNewVertex,
|
|||||||
return false;
|
return false;
|
||||||
} else {
|
} else {
|
||||||
// The current triangle is visible and therefore obsolete
|
// The current triangle is visible and therefore obsolete
|
||||||
this.ownerTriangle->setIsObsolete(true);
|
this.ownerTriangle.setIsObsolete(true);
|
||||||
int backup = triangleStore.getNbTriangles();
|
int backup = triangleStore.getNbTriangles();
|
||||||
if(!this.ownerTriangle->getAdjacentEdge(indexOfNextCounterClockwiseEdge(this->this.index)).computeSilhouette(vertices,
|
if(!this.ownerTriangle.getAdjacentEdge(indexOfNextCounterClockwiseEdge(this.this.index)).computeSilhouette(vertices,
|
||||||
indexNewVertex,
|
indexNewVertex,
|
||||||
triangleStore)) {
|
triangleStore)) {
|
||||||
this.ownerTriangle->setIsObsolete(false);
|
this.ownerTriangle.setIsObsolete(false);
|
||||||
TriangleEPA* triangle = triangleStore.newTriangle(vertices, indexNewVertex,
|
TriangleEPA* triangle = triangleStore.newTriangle(vertices, indexNewVertex,
|
||||||
getTargetVertexIndex(),
|
getTargetVertexIndex(),
|
||||||
getSourceVertexIndex());
|
getSourceVertexIndex());
|
||||||
@ -76,10 +76,10 @@ boolean EdgeEPA::computeSilhouette( vec3* vertices, int indexNewVertex,
|
|||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
return false;
|
return false;
|
||||||
} else if (!this.ownerTriangle->getAdjacentEdge(indexOfPreviousCounterClockwiseEdge(this->this.index)).computeSilhouette(vertices,
|
} else if (!this.ownerTriangle.getAdjacentEdge(indexOfPreviousCounterClockwiseEdge(this.this.index)).computeSilhouette(vertices,
|
||||||
indexNewVertex,
|
indexNewVertex,
|
||||||
triangleStore)) {
|
triangleStore)) {
|
||||||
this.ownerTriangle->setIsObsolete(false);
|
this.ownerTriangle.setIsObsolete(false);
|
||||||
triangleStore.resize(backup);
|
triangleStore.resize(backup);
|
||||||
TriangleEPA* triangle = triangleStore.newTriangle(vertices, indexNewVertex,
|
TriangleEPA* triangle = triangleStore.newTriangle(vertices, indexNewVertex,
|
||||||
getTargetVertexIndex(),
|
getTargetVertexIndex(),
|
||||||
|
@ -1,17 +1,5 @@
|
|||||||
/** @file
|
package org.atriaSoft.ephysics.collision.narrowphase.EPA;
|
||||||
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
|
|
||||||
* @author Daniel CHAPPUIS
|
|
||||||
* @author Edouard DUPIN
|
|
||||||
* @copyright 2010-2016, Daniel Chappuis
|
|
||||||
* @copyright 2017, Edouard DUPIN
|
|
||||||
* @license MPL v2.0 (see license file)
|
|
||||||
*/
|
|
||||||
#pragma once
|
|
||||||
#include <ephysics/mathematics/mathematics.hpp>
|
|
||||||
|
|
||||||
namespace ephysics {
|
|
||||||
class TriangleEPA;
|
|
||||||
class TrianglesStore;
|
|
||||||
/**
|
/**
|
||||||
* @brief Class EdgeEPA
|
* @brief Class EdgeEPA
|
||||||
* This class represents an edge of the current polytope in the EPA algorithm.
|
* This class represents an edge of the current polytope in the EPA algorithm.
|
||||||
@ -31,7 +19,7 @@ class EdgeEPA {
|
|||||||
/// Copy-ructor
|
/// Copy-ructor
|
||||||
EdgeEPA( EdgeEPA obj);
|
EdgeEPA( EdgeEPA obj);
|
||||||
/// Move-ructor
|
/// Move-ructor
|
||||||
EdgeEPA(EdgeEPAhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj obj);
|
EdgeEPA(EdgeEPA obj);
|
||||||
/// Return the pointer to the owner triangle
|
/// Return the pointer to the owner triangle
|
||||||
TriangleEPA* getOwnerTriangle() {
|
TriangleEPA* getOwnerTriangle() {
|
||||||
return this.ownerTriangle;
|
return this.ownerTriangle;
|
||||||
@ -45,17 +33,17 @@ class EdgeEPA {
|
|||||||
/// Return the index of the target vertex of the edge
|
/// Return the index of the target vertex of the edge
|
||||||
int getTargetVertexIndex() ;
|
int getTargetVertexIndex() ;
|
||||||
/// Execute the recursive silhouette algorithm from this edge
|
/// Execute the recursive silhouette algorithm from this edge
|
||||||
boolean computeSilhouette( vec3* vertices, int index, TrianglesStore triangleStore);
|
boolean computeSilhouette( Vector3f* vertices, int index, TrianglesStore triangleStore);
|
||||||
/// Assignment operator
|
/// Assignment operator
|
||||||
EdgeEPA operator=( EdgeEPA obj) {
|
EdgeEPA operator=( EdgeEPA obj) {
|
||||||
this.ownerTriangle = obj.this.ownerTriangle;
|
this.ownerTriangle = obj.ownerTriangle;
|
||||||
this.index = obj.this.index;
|
this.index = obj.index;
|
||||||
return *this;
|
return *this;
|
||||||
}
|
}
|
||||||
/// Move operator
|
/// Move operator
|
||||||
EdgeEPA operator=(EdgeEPAhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj obj) {
|
EdgeEPA operator=(EdgeEPA obj) {
|
||||||
etk::swap(this.ownerTriangle, obj.this.ownerTriangle);
|
swap(this.ownerTriangle, obj.ownerTriangle);
|
||||||
etk::swap(this.index, obj.this.index);
|
swap(this.index, obj.index);
|
||||||
return *this;
|
return *this;
|
||||||
}
|
}
|
||||||
};
|
};
|
@ -34,10 +34,10 @@ TriangleEPA::~TriangleEPA() {
|
|||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
boolean TriangleEPA::computeClosestPoint( vec3* vertices) {
|
boolean TriangleEPA::computeClosestPoint( Vector3f* vertices) {
|
||||||
vec3 p0 = vertices[this.indicesVertices[0]];
|
Vector3f p0 = vertices[this.indicesVertices[0]];
|
||||||
vec3 v1 = vertices[this.indicesVertices[1]] - p0;
|
Vector3f v1 = vertices[this.indicesVertices[1]] - p0;
|
||||||
vec3 v2 = vertices[this.indicesVertices[2]] - p0;
|
Vector3f v2 = vertices[this.indicesVertices[2]] - p0;
|
||||||
float v1Dotv1 = v1.dot(v1);
|
float v1Dotv1 = v1.dot(v1);
|
||||||
float v1Dotv2 = v1.dot(v2);
|
float v1Dotv2 = v1.dot(v2);
|
||||||
float v2Dotv2 = v2.dot(v2);
|
float v2Dotv2 = v2.dot(v2);
|
||||||
@ -61,9 +61,9 @@ boolean TriangleEPA::computeClosestPoint( vec3* vertices) {
|
|||||||
|
|
||||||
boolean ephysics::link( EdgeEPA edge0, EdgeEPA edge1) {
|
boolean ephysics::link( EdgeEPA edge0, EdgeEPA edge1) {
|
||||||
if ( edge0.getSourceVertexIndex() == edge1.getTargetVertexIndex()
|
if ( edge0.getSourceVertexIndex() == edge1.getTargetVertexIndex()
|
||||||
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj edge0.getTargetVertexIndex() == edge1.getSourceVertexIndex() ) {
|
&& edge0.getTargetVertexIndex() == edge1.getSourceVertexIndex() ) {
|
||||||
edge0.getOwnerTriangle()->this.adjacentEdges[edge0.getIndex()] = edge1;
|
edge0.getOwnerTriangle().this.adjacentEdges[edge0.getIndex()] = edge1;
|
||||||
edge1.getOwnerTriangle()->this.adjacentEdges[edge1.getIndex()] = edge0;
|
edge1.getOwnerTriangle().this.adjacentEdges[edge1.getIndex()] = edge0;
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
return false;
|
return false;
|
||||||
@ -71,20 +71,20 @@ boolean ephysics::link( EdgeEPA edge0, EdgeEPA edge1) {
|
|||||||
|
|
||||||
void ephysics::halfLink( EdgeEPA edge0, EdgeEPA edge1) {
|
void ephysics::halfLink( EdgeEPA edge0, EdgeEPA edge1) {
|
||||||
assert( edge0.getSourceVertexIndex() == edge1.getTargetVertexIndex()
|
assert( edge0.getSourceVertexIndex() == edge1.getTargetVertexIndex()
|
||||||
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj edge0.getTargetVertexIndex() == edge1.getSourceVertexIndex());
|
&& edge0.getTargetVertexIndex() == edge1.getSourceVertexIndex());
|
||||||
edge0.getOwnerTriangle()->this.adjacentEdges[edge0.getIndex()] = edge1;
|
edge0.getOwnerTriangle().this.adjacentEdges[edge0.getIndex()] = edge1;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
boolean TriangleEPA::computeSilhouette( vec3* vertices, int indexNewVertex,
|
boolean TriangleEPA::computeSilhouette( Vector3f* vertices, int indexNewVertex,
|
||||||
TrianglesStore triangleStore) {
|
TrianglesStore triangleStore) {
|
||||||
int first = triangleStore.getNbTriangles();
|
int first = triangleStore.getNbTriangles();
|
||||||
// Mark the current triangle as obsolete because it
|
// Mark the current triangle as obsolete because it
|
||||||
setIsObsolete(true);
|
setIsObsolete(true);
|
||||||
// Execute recursively the silhouette algorithm for the adjacent edges of neighboring
|
// Execute recursively the silhouette algorithm for the adjacent edges of neighboring
|
||||||
// triangles of the current triangle
|
// triangles of the current triangle
|
||||||
boolean result = this.adjacentEdges[0].computeSilhouette(vertices, indexNewVertex, triangleStore) hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj
|
boolean result = this.adjacentEdges[0].computeSilhouette(vertices, indexNewVertex, triangleStore) &&
|
||||||
this.adjacentEdges[1].computeSilhouette(vertices, indexNewVertex, triangleStore) hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj
|
this.adjacentEdges[1].computeSilhouette(vertices, indexNewVertex, triangleStore) &&
|
||||||
this.adjacentEdges[2].computeSilhouette(vertices, indexNewVertex, triangleStore);
|
this.adjacentEdges[2].computeSilhouette(vertices, indexNewVertex, triangleStore);
|
||||||
if (result) {
|
if (result) {
|
||||||
int i,j;
|
int i,j;
|
||||||
@ -92,7 +92,7 @@ boolean TriangleEPA::computeSilhouette( vec3* vertices, int indexNewVertex,
|
|||||||
for (i=first, j=triangleStore.getNbTriangles()-1;
|
for (i=first, j=triangleStore.getNbTriangles()-1;
|
||||||
i != triangleStore.getNbTriangles(); j = i++) {
|
i != triangleStore.getNbTriangles(); j = i++) {
|
||||||
TriangleEPA* triangle = triangleStore[i];
|
TriangleEPA* triangle = triangleStore[i];
|
||||||
halfLink(triangle->getAdjacentEdge(1), EdgeEPA(triangle, 1));
|
halfLink(triangle.getAdjacentEdge(1), EdgeEPA(triangle, 1));
|
||||||
if (!link(EdgeEPA(triangle, 0), EdgeEPA(triangleStore[j], 2))) {
|
if (!link(EdgeEPA(triangle, 0), EdgeEPA(triangleStore[j], 2))) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
@ -1,16 +1,5 @@
|
|||||||
/** @file
|
package org.atriaSoft.ephysics.collision.narrowphase.EPA;
|
||||||
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
|
|
||||||
* @author Daniel CHAPPUIS
|
|
||||||
* @author Edouard DUPIN
|
|
||||||
* @copyright 2010-2016, Daniel Chappuis
|
|
||||||
* @copyright 2017, Edouard DUPIN
|
|
||||||
* @license MPL v2.0 (see license file)
|
|
||||||
*/
|
|
||||||
#pragma once
|
|
||||||
#include <ephysics/mathematics/mathematics.hpp>
|
|
||||||
#include <ephysics/configuration.hpp>
|
|
||||||
#include <ephysics/collision/narrowphase/EPA/EdgeEPA.hpp>
|
|
||||||
namespace ephysics {
|
|
||||||
boolean link( EdgeEPA edge0, EdgeEPA edge1);
|
boolean link( EdgeEPA edge0, EdgeEPA edge1);
|
||||||
void halfLink( EdgeEPA edge0, EdgeEPA edge1);
|
void halfLink( EdgeEPA edge0, EdgeEPA edge1);
|
||||||
/**
|
/**
|
||||||
@ -23,40 +12,40 @@ namespace ephysics {
|
|||||||
EdgeEPA this.adjacentEdges[3]; //!< Three adjacent edges of the triangle (edges of other triangles)
|
EdgeEPA this.adjacentEdges[3]; //!< Three adjacent edges of the triangle (edges of other triangles)
|
||||||
boolean this.isObsolete; //!< True if the triangle face is visible from the new support point
|
boolean this.isObsolete; //!< True if the triangle face is visible from the new support point
|
||||||
float this.determinant; //!< Determinant
|
float this.determinant; //!< Determinant
|
||||||
vec3 this.closestPoint; //!< Point v closest to the origin on the affine hull of the triangle
|
Vector3f this.closestPoint; //!< Point v closest to the origin on the affine hull of the triangle
|
||||||
float this.lambda1; //!< Lambda1 value such that v = lambda0 * y0 + lambda1 * y1 + lambda2 * y2
|
float this.lambda1; //!< Lambda1 value such that v = lambda0 * y0 + lambda1 * y1 + lambda2 * y2
|
||||||
float this.lambda2; //!< Lambda1 value such that v = lambda0 * y0 + lambda1 * y1 + lambda2 * y2
|
float this.lambda2; //!< Lambda1 value such that v = lambda0 * y0 + lambda1 * y1 + lambda2 * y2
|
||||||
float this.distSquare; //!< Square distance of the point closest point v to the origin
|
float this.distSquare; //!< Square distance of the point closest point v to the origin
|
||||||
public:
|
public:
|
||||||
/// Private copy-ructor
|
/// Private copy-ructor
|
||||||
TriangleEPA( TriangleEPA triangle) {
|
TriangleEPA( TriangleEPA triangle) {
|
||||||
this.indicesVertices[0] = triangle.this.indicesVertices[0];
|
this.indicesVertices[0] = triangle.indicesVertices[0];
|
||||||
this.indicesVertices[1] = triangle.this.indicesVertices[1];
|
this.indicesVertices[1] = triangle.indicesVertices[1];
|
||||||
this.indicesVertices[2] = triangle.this.indicesVertices[2];
|
this.indicesVertices[2] = triangle.indicesVertices[2];
|
||||||
this.adjacentEdges[0] = triangle.this.adjacentEdges[0];
|
this.adjacentEdges[0] = triangle.adjacentEdges[0];
|
||||||
this.adjacentEdges[1] = triangle.this.adjacentEdges[1];
|
this.adjacentEdges[1] = triangle.adjacentEdges[1];
|
||||||
this.adjacentEdges[2] = triangle.this.adjacentEdges[2];
|
this.adjacentEdges[2] = triangle.adjacentEdges[2];
|
||||||
this.isObsolete = triangle.this.isObsolete;
|
this.isObsolete = triangle.isObsolete;
|
||||||
this.determinant = triangle.this.determinant;
|
this.determinant = triangle.determinant;
|
||||||
this.closestPoint = triangle.this.closestPoint;
|
this.closestPoint = triangle.closestPoint;
|
||||||
this.lambda1 = triangle.this.lambda1;
|
this.lambda1 = triangle.lambda1;
|
||||||
this.lambda2 = triangle.this.lambda2;
|
this.lambda2 = triangle.lambda2;
|
||||||
this.distSquare = triangle.this.distSquare;
|
this.distSquare = triangle.distSquare;
|
||||||
}
|
}
|
||||||
/// Private assignment operator
|
/// Private assignment operator
|
||||||
TriangleEPA operator=( TriangleEPA triangle) {
|
TriangleEPA operator=( TriangleEPA triangle) {
|
||||||
this.indicesVertices[0] = triangle.this.indicesVertices[0];
|
this.indicesVertices[0] = triangle.indicesVertices[0];
|
||||||
this.indicesVertices[1] = triangle.this.indicesVertices[1];
|
this.indicesVertices[1] = triangle.indicesVertices[1];
|
||||||
this.indicesVertices[2] = triangle.this.indicesVertices[2];
|
this.indicesVertices[2] = triangle.indicesVertices[2];
|
||||||
this.adjacentEdges[0] = triangle.this.adjacentEdges[0];
|
this.adjacentEdges[0] = triangle.adjacentEdges[0];
|
||||||
this.adjacentEdges[1] = triangle.this.adjacentEdges[1];
|
this.adjacentEdges[1] = triangle.adjacentEdges[1];
|
||||||
this.adjacentEdges[2] = triangle.this.adjacentEdges[2];
|
this.adjacentEdges[2] = triangle.adjacentEdges[2];
|
||||||
this.isObsolete = triangle.this.isObsolete;
|
this.isObsolete = triangle.isObsolete;
|
||||||
this.determinant = triangle.this.determinant;
|
this.determinant = triangle.determinant;
|
||||||
this.closestPoint = triangle.this.closestPoint;
|
this.closestPoint = triangle.closestPoint;
|
||||||
this.lambda1 = triangle.this.lambda1;
|
this.lambda1 = triangle.lambda1;
|
||||||
this.lambda2 = triangle.this.lambda2;
|
this.lambda2 = triangle.lambda2;
|
||||||
this.distSquare = triangle.this.distSquare;
|
this.distSquare = triangle.distSquare;
|
||||||
return *this;
|
return *this;
|
||||||
}
|
}
|
||||||
/// Constructor
|
/// Constructor
|
||||||
@ -65,16 +54,14 @@ namespace ephysics {
|
|||||||
TriangleEPA(int v1, int v2, int v3);
|
TriangleEPA(int v1, int v2, int v3);
|
||||||
/// Constructor
|
/// Constructor
|
||||||
void set(int v1, int v2, int v3);
|
void set(int v1, int v2, int v3);
|
||||||
/// Destructor
|
|
||||||
~TriangleEPA();
|
|
||||||
/// Return an adjacent edge of the triangle
|
/// Return an adjacent edge of the triangle
|
||||||
EdgeEPA getAdjacentEdge(int index) {
|
EdgeEPA getAdjacentEdge(int index) {
|
||||||
assert(index >= 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj index < 3);
|
assert(index >= 0 && index < 3);
|
||||||
return this.adjacentEdges[index];
|
return this.adjacentEdges[index];
|
||||||
}
|
}
|
||||||
/// Set an adjacent edge of the triangle
|
/// Set an adjacent edge of the triangle
|
||||||
void setAdjacentEdge(int index, EdgeEPA edge) {
|
void setAdjacentEdge(int index, EdgeEPA edge) {
|
||||||
assert(index >=0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj index < 3);
|
assert(index >=0 && index < 3);
|
||||||
this.adjacentEdges[index] = edge;
|
this.adjacentEdges[index] = edge;
|
||||||
}
|
}
|
||||||
/// Return the square distance of the closest point to origin
|
/// Return the square distance of the closest point to origin
|
||||||
@ -90,23 +77,23 @@ namespace ephysics {
|
|||||||
return this.isObsolete;
|
return this.isObsolete;
|
||||||
}
|
}
|
||||||
/// Return the point closest to the origin
|
/// Return the point closest to the origin
|
||||||
vec3 getClosestPoint() {
|
Vector3f getClosestPoint() {
|
||||||
return this.closestPoint;
|
return this.closestPoint;
|
||||||
}
|
}
|
||||||
// Return true if the closest point on affine hull is inside the triangle
|
// Return true if the closest point on affine hull is inside the triangle
|
||||||
boolean isClosestPointInternalToTriangle() {
|
boolean isClosestPointInternalToTriangle() {
|
||||||
return (this.lambda1 >= 0.0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj this.lambda2 >= 0.0 hjkhjkhjkhkj (this.lambda1 + this.lambda2) <= this.determinant);
|
return (this.lambda1 >= 0.0 && this.lambda2 >= 0.0 hjkhjkhjkhkj (this.lambda1 + this.lambda2) <= this.determinant);
|
||||||
}
|
}
|
||||||
/// Return true if the triangle is visible from a given vertex
|
/// Return true if the triangle is visible from a given vertex
|
||||||
boolean isVisibleFromVertex( vec3* vertices, int index) {
|
boolean isVisibleFromVertex( Vector3f* vertices, int index) {
|
||||||
vec3 closestToVert = vertices[index] - this.closestPoint;
|
Vector3f closestToVert = vertices[index] - this.closestPoint;
|
||||||
return (this.closestPoint.dot(closestToVert) > 0.0);
|
return (this.closestPoint.dot(closestToVert) > 0.0);
|
||||||
}
|
}
|
||||||
/// Compute the point v closest to the origin of this triangle
|
/// Compute the point v closest to the origin of this triangle
|
||||||
boolean computeClosestPoint( vec3* vertices);
|
boolean computeClosestPoint( Vector3f* vertices);
|
||||||
/// Compute the point of an object closest to the origin
|
/// Compute the point of an object closest to the origin
|
||||||
vec3 computeClosestPointOfObject( vec3* supportPointsOfObject) {
|
Vector3f computeClosestPointOfObject( Vector3f* supportPointsOfObject) {
|
||||||
vec3 p0 = supportPointsOfObject[this.indicesVertices[0]];
|
Vector3f p0 = supportPointsOfObject[this.indicesVertices[0]];
|
||||||
return p0 + 1.0f/this.determinant * (this.lambda1 * (supportPointsOfObject[this.indicesVertices[1]] - p0) +
|
return p0 + 1.0f/this.determinant * (this.lambda1 * (supportPointsOfObject[this.indicesVertices[1]] - p0) +
|
||||||
this.lambda2 * (supportPointsOfObject[this.indicesVertices[2]] - p0));
|
this.lambda2 * (supportPointsOfObject[this.indicesVertices[2]] - p0));
|
||||||
}
|
}
|
||||||
@ -121,10 +108,10 @@ namespace ephysics {
|
|||||||
/// face from the new vertex, computes the silhouette and create the new faces from the new vertex in
|
/// face from the new vertex, computes the silhouette and create the new faces from the new vertex in
|
||||||
/// order that we always have a convex polytope. The faces visible from the new vertex are set
|
/// order that we always have a convex polytope. The faces visible from the new vertex are set
|
||||||
/// obselete and will not be considered as being a candidate face in the future.
|
/// obselete and will not be considered as being a candidate face in the future.
|
||||||
boolean computeSilhouette( vec3* vertices, int index, TrianglesStore triangleStore);
|
boolean computeSilhouette( Vector3f* vertices, int index, TrianglesStore triangleStore);
|
||||||
/// Access operator
|
/// Access operator
|
||||||
int operator[](int pos) {
|
int operator[](int pos) {
|
||||||
assert(pos >= 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj pos <3);
|
assert(pos >= 0 && pos <3);
|
||||||
return this.indicesVertices[pos];
|
return this.indicesVertices[pos];
|
||||||
}
|
}
|
||||||
/// Link an edge with another one. It means that the current edge of a triangle will
|
/// Link an edge with another one. It means that the current edge of a triangle will
|
@ -1,28 +1,12 @@
|
|||||||
/** @file
|
package org.atriaSoft.ephysics.collision.narrowphase.EPA;
|
||||||
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
|
|
||||||
* @author Daniel CHAPPUIS
|
|
||||||
* @author Edouard DUPIN
|
|
||||||
* @copyright 2010-2016, Daniel Chappuis
|
|
||||||
* @copyright 2017, Edouard DUPIN
|
|
||||||
* @license MPL v2.0 (see license file)
|
|
||||||
*/
|
|
||||||
#pragma once
|
|
||||||
#include <ephysics/collision/narrowphase/EPA/TriangleEPA.hpp>
|
|
||||||
#include <ephysics/debug.hpp>
|
|
||||||
#include <etk/Array.hpp>
|
|
||||||
|
|
||||||
namespace ephysics {
|
|
||||||
int MAXTRIANGLES = 200; // Maximum number of triangles
|
int MAXTRIANGLES = 200; // Maximum number of triangles
|
||||||
/**
|
/**
|
||||||
* @brief This class stores several triangles of the polytope in the EPA algorithm.
|
* @brief This class stores several triangles of the polytope in the EPA algorithm.
|
||||||
*/
|
*/
|
||||||
class TrianglesStore {
|
class TrianglesStore {
|
||||||
private:
|
private:
|
||||||
etk::Array<TriangleEPA, MAXTRIANGLES> this.triangles; //!< Triangles
|
Array<TriangleEPA, MAXTRIANGLES> this.triangles; //!< Triangles
|
||||||
/// Private copy-ructor
|
|
||||||
TrianglesStore( TrianglesStore triangleStore) = delete;
|
|
||||||
/// Private assignment operator
|
|
||||||
TrianglesStore operator=( TrianglesStore triangleStore) = delete;
|
|
||||||
public:
|
public:
|
||||||
/// Constructor
|
/// Constructor
|
||||||
TrianglesStore() = default;
|
TrianglesStore() = default;
|
||||||
@ -31,13 +15,13 @@ namespace ephysics {
|
|||||||
this.triangles.clear();
|
this.triangles.clear();
|
||||||
}
|
}
|
||||||
/// Return the number of triangles
|
/// Return the number of triangles
|
||||||
sizet getNbTriangles() {
|
long getNbTriangles() {
|
||||||
return this.triangles.size();
|
return this.triangles.size();
|
||||||
}
|
}
|
||||||
/// Set the number of triangles
|
/// Set the number of triangles
|
||||||
void resize(int backup) {
|
void resize(int backup) {
|
||||||
if (backup > this.triangles.size()) {
|
if (backup > this.triangles.size()) {
|
||||||
Log.error("RESIZE BIGGER : " << backup << " > " << this.triangles.size());
|
Log.error("RESIZE BIGGER : " + backup + " > " + this.triangles.size());
|
||||||
}
|
}
|
||||||
this.triangles.resize(backup);
|
this.triangles.resize(backup);
|
||||||
}
|
}
|
||||||
@ -46,14 +30,14 @@ namespace ephysics {
|
|||||||
return this.triangles.back();
|
return this.triangles.back();
|
||||||
}
|
}
|
||||||
/// Create a new triangle
|
/// Create a new triangle
|
||||||
TriangleEPA* newTriangle( vec3* vertices, int v0, int v1, int v2) {
|
TriangleEPA* newTriangle( Vector3f* vertices, int v0, int v1, int v2) {
|
||||||
// If we have not reached the maximum number of triangles
|
// If we have not reached the maximum number of triangles
|
||||||
if (this.triangles.size() < MAXTRIANGLES) {
|
if (this.triangles.size() < MAXTRIANGLES) {
|
||||||
TriangleEPA tmp(v0, v1, v2);
|
TriangleEPA tmp(v0, v1, v2);
|
||||||
if (!tmp.computeClosestPoint(vertices)) {
|
if (!tmp.computeClosestPoint(vertices)) {
|
||||||
return null;
|
return null;
|
||||||
}
|
}
|
||||||
this.triangles.pushBack(etk::move(tmp));
|
this.triangles.pushBack(move(tmp));
|
||||||
return this.triangles.back();
|
return this.triangles.back();
|
||||||
}
|
}
|
||||||
// We are at the limit (internal)
|
// We are at the limit (internal)
|
@ -26,50 +26,50 @@ void GJKAlgorithm::testCollision( CollisionShapeInfo shape1Info,
|
|||||||
CollisionShapeInfo shape2Info,
|
CollisionShapeInfo shape2Info,
|
||||||
NarrowPhaseCallback* narrowPhaseCallback) {
|
NarrowPhaseCallback* narrowPhaseCallback) {
|
||||||
PROFILE("GJKAlgorithm::testCollision()");
|
PROFILE("GJKAlgorithm::testCollision()");
|
||||||
vec3 suppA; // Support point of object A
|
Vector3f suppA; // Support point of object A
|
||||||
vec3 suppB; // Support point of object B
|
Vector3f suppB; // Support point of object B
|
||||||
vec3 w; // Support point of Minkowski difference A-B
|
Vector3f w; // Support point of Minkowski difference A-B
|
||||||
vec3 pA; // Closest point of object A
|
Vector3f pA; // Closest point of object A
|
||||||
vec3 pB; // Closest point of object B
|
Vector3f pB; // Closest point of object B
|
||||||
float vDotw;
|
float vDotw;
|
||||||
float prevDistSquare;
|
float prevDistSquare;
|
||||||
assert(shape1Info.collisionShape->isConvex());
|
assert(shape1Info.collisionShape.isConvex());
|
||||||
assert(shape2Info.collisionShape->isConvex());
|
assert(shape2Info.collisionShape.isConvex());
|
||||||
ConvexShape* shape1 = staticcast< ConvexShape*>(shape1Info.collisionShape);
|
ConvexShape* shape1 = staticcast< ConvexShape*>(shape1Info.collisionShape);
|
||||||
ConvexShape* shape2 = staticcast< ConvexShape*>(shape2Info.collisionShape);
|
ConvexShape* shape2 = staticcast< ConvexShape*>(shape2Info.collisionShape);
|
||||||
void** shape1CachedCollisionData = shape1Info.cachedCollisionData;
|
void** shape1CachedCollisionData = shape1Info.cachedCollisionData;
|
||||||
void** shape2CachedCollisionData = shape2Info.cachedCollisionData;
|
void** shape2CachedCollisionData = shape2Info.cachedCollisionData;
|
||||||
// Get the local-space to world-space transforms
|
// Get the local-space to world-space transforms
|
||||||
etk::Transform3D transform1 = shape1Info.shapeToWorldTransform;
|
Transform3D transform1 = shape1Info.shapeToWorldTransform;
|
||||||
etk::Transform3D transform2 = shape2Info.shapeToWorldTransform;
|
Transform3D transform2 = shape2Info.shapeToWorldTransform;
|
||||||
// etk::Transform3D a point from local space of body 2 to local
|
// Transform3D a point from local space of body 2 to local
|
||||||
// space of body 1 (the GJK algorithm is done in local space of body 1)
|
// space of body 1 (the GJK algorithm is done in local space of body 1)
|
||||||
etk::Transform3D body2Tobody1 = transform1.getInverse() * transform2;
|
Transform3D body2Tobody1 = transform1.getInverse() * transform2;
|
||||||
// Matrix that transform a direction from local
|
// Matrix that transform a direction from local
|
||||||
// space of body 1 into local space of body 2
|
// space of body 1 into local space of body 2
|
||||||
etk::Matrix3x3 rotateToBody2 = transform2.getOrientation().getMatrix().getTranspose() *
|
Matrix3f rotateToBody2 = transform2.getOrientation().getMatrix().getTranspose() *
|
||||||
transform1.getOrientation().getMatrix();
|
transform1.getOrientation().getMatrix();
|
||||||
// Initialize the margin (sum of margins of both objects)
|
// Initialize the margin (sum of margins of both objects)
|
||||||
float margin = shape1->getMargin() + shape2->getMargin();
|
float margin = shape1.getMargin() + shape2.getMargin();
|
||||||
float marginSquare = margin * margin;
|
float marginSquare = margin * margin;
|
||||||
assert(margin > 0.0);
|
assert(margin > 0.0);
|
||||||
// Create a simplex set
|
// Create a simplex set
|
||||||
Simplex simplex;
|
Simplex simplex;
|
||||||
// Get the previous point V (last cached separating axis)
|
// Get the previous point V (last cached separating axis)
|
||||||
vec3 v = this.currentOverlappingPair->getCachedSeparatingAxis();
|
Vector3f v = this.currentOverlappingPair.getCachedSeparatingAxis();
|
||||||
// Initialize the upper bound for the square distance
|
// Initialize the upper bound for the square distance
|
||||||
float distSquare = FLTMAX;
|
float distSquare = FLTMAX;
|
||||||
do {
|
do {
|
||||||
// Compute the support points for original objects (without margins) A and B
|
// Compute the support points for original objects (without margins) A and B
|
||||||
suppA = shape1->getLocalSupportPointWithoutMargin(-v, shape1CachedCollisionData);
|
suppA = shape1.getLocalSupportPointWithoutMargin(-v, shape1CachedCollisionData);
|
||||||
suppB = body2Tobody1 * shape2->getLocalSupportPointWithoutMargin(rotateToBody2 * v, shape2CachedCollisionData);
|
suppB = body2Tobody1 * shape2.getLocalSupportPointWithoutMargin(rotateToBody2 * v, shape2CachedCollisionData);
|
||||||
// Compute the support point for the Minkowski difference A-B
|
// Compute the support point for the Minkowski difference A-B
|
||||||
w = suppA - suppB;
|
w = suppA - suppB;
|
||||||
vDotw = v.dot(w);
|
vDotw = v.dot(w);
|
||||||
// If the enlarge objects (with margins) do not intersect
|
// If the enlarge objects (with margins) do not intersect
|
||||||
if (vDotw > 0.0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj vDotw * vDotw > distSquare * marginSquare) {
|
if (vDotw > 0.0 && vDotw * vDotw > distSquare * marginSquare) {
|
||||||
// Cache the current separating axis for frame coherence
|
// Cache the current separating axis for frame coherence
|
||||||
this.currentOverlappingPair->setCachedSeparatingAxis(v);
|
this.currentOverlappingPair.setCachedSeparatingAxis(v);
|
||||||
// No intersection, we return
|
// No intersection, we return
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
@ -81,17 +81,17 @@ void GJKAlgorithm::testCollision( CollisionShapeInfo shape1Info,
|
|||||||
// object with the margins
|
// object with the margins
|
||||||
float dist = sqrt(distSquare);
|
float dist = sqrt(distSquare);
|
||||||
assert(dist > 0.0);
|
assert(dist > 0.0);
|
||||||
pA = (pA - (shape1->getMargin() / dist) * v);
|
pA = (pA - (shape1.getMargin() / dist) * v);
|
||||||
pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v);
|
pB = body2Tobody1.getInverse() * (pB + (shape2.getMargin() / dist) * v);
|
||||||
// Compute the contact info
|
// Compute the contact info
|
||||||
vec3 normal = transform1.getOrientation() * (-v.safeNormalized());
|
Vector3f normal = transform1.getOrientation() * (-v.safeNormalized());
|
||||||
float penetrationDepth = margin - dist;
|
float penetrationDepth = margin - dist;
|
||||||
// Reject the contact if the penetration depth is negative (due too numerical errors)
|
// Reject the contact if the penetration depth is negative (due too numerical errors)
|
||||||
if (penetrationDepth <= 0.0) return;
|
if (penetrationDepth <= 0.0) return;
|
||||||
// Create the contact info object
|
// Create the contact info object
|
||||||
ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape,
|
ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape,
|
||||||
shape2Info.collisionShape, normal, penetrationDepth, pA, pB);
|
shape2Info.collisionShape, normal, penetrationDepth, pA, pB);
|
||||||
narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo);
|
narrowPhaseCallback.notifyContact(shape1Info.overlappingPair, contactInfo);
|
||||||
// There is an intersection, therefore we return
|
// There is an intersection, therefore we return
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
@ -105,10 +105,10 @@ void GJKAlgorithm::testCollision( CollisionShapeInfo shape1Info,
|
|||||||
// object with the margins
|
// object with the margins
|
||||||
float dist = sqrt(distSquare);
|
float dist = sqrt(distSquare);
|
||||||
assert(dist > 0.0);
|
assert(dist > 0.0);
|
||||||
pA = (pA - (shape1->getMargin() / dist) * v);
|
pA = (pA - (shape1.getMargin() / dist) * v);
|
||||||
pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v);
|
pB = body2Tobody1.getInverse() * (pB + (shape2.getMargin() / dist) * v);
|
||||||
// Compute the contact info
|
// Compute the contact info
|
||||||
vec3 normal = transform1.getOrientation() * (-v.safeNormalized());
|
Vector3f normal = transform1.getOrientation() * (-v.safeNormalized());
|
||||||
float penetrationDepth = margin - dist;
|
float penetrationDepth = margin - dist;
|
||||||
|
|
||||||
// Reject the contact if the penetration depth is negative (due too numerical errors)
|
// Reject the contact if the penetration depth is negative (due too numerical errors)
|
||||||
@ -117,7 +117,7 @@ void GJKAlgorithm::testCollision( CollisionShapeInfo shape1Info,
|
|||||||
// Create the contact info object
|
// Create the contact info object
|
||||||
ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape,
|
ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape,
|
||||||
shape2Info.collisionShape, normal, penetrationDepth, pA, pB);
|
shape2Info.collisionShape, normal, penetrationDepth, pA, pB);
|
||||||
narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo);
|
narrowPhaseCallback.notifyContact(shape1Info.overlappingPair, contactInfo);
|
||||||
// There is an intersection, therefore we return
|
// There is an intersection, therefore we return
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
@ -130,10 +130,10 @@ void GJKAlgorithm::testCollision( CollisionShapeInfo shape1Info,
|
|||||||
// object with the margins
|
// object with the margins
|
||||||
float dist = sqrt(distSquare);
|
float dist = sqrt(distSquare);
|
||||||
assert(dist > 0.0);
|
assert(dist > 0.0);
|
||||||
pA = (pA - (shape1->getMargin() / dist) * v);
|
pA = (pA - (shape1.getMargin() / dist) * v);
|
||||||
pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v);
|
pB = body2Tobody1.getInverse() * (pB + (shape2.getMargin() / dist) * v);
|
||||||
// Compute the contact info
|
// Compute the contact info
|
||||||
vec3 normal = transform1.getOrientation() * (-v.safeNormalized());
|
Vector3f normal = transform1.getOrientation() * (-v.safeNormalized());
|
||||||
float penetrationDepth = margin - dist;
|
float penetrationDepth = margin - dist;
|
||||||
|
|
||||||
// Reject the contact if the penetration depth is negative (due too numerical errors)
|
// Reject the contact if the penetration depth is negative (due too numerical errors)
|
||||||
@ -142,7 +142,7 @@ void GJKAlgorithm::testCollision( CollisionShapeInfo shape1Info,
|
|||||||
// Create the contact info object
|
// Create the contact info object
|
||||||
ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape,
|
ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape,
|
||||||
shape2Info.collisionShape, normal, penetrationDepth, pA, pB);
|
shape2Info.collisionShape, normal, penetrationDepth, pA, pB);
|
||||||
narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo);
|
narrowPhaseCallback.notifyContact(shape1Info.overlappingPair, contactInfo);
|
||||||
// There is an intersection, therefore we return
|
// There is an intersection, therefore we return
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
@ -161,10 +161,10 @@ void GJKAlgorithm::testCollision( CollisionShapeInfo shape1Info,
|
|||||||
// object with the margins
|
// object with the margins
|
||||||
float dist = sqrt(distSquare);
|
float dist = sqrt(distSquare);
|
||||||
assert(dist > 0.0);
|
assert(dist > 0.0);
|
||||||
pA = (pA - (shape1->getMargin() / dist) * v);
|
pA = (pA - (shape1.getMargin() / dist) * v);
|
||||||
pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v);
|
pB = body2Tobody1.getInverse() * (pB + (shape2.getMargin() / dist) * v);
|
||||||
// Compute the contact info
|
// Compute the contact info
|
||||||
vec3 normal = transform1.getOrientation() * (-v.safeNormalized());
|
Vector3f normal = transform1.getOrientation() * (-v.safeNormalized());
|
||||||
float penetrationDepth = margin - dist;
|
float penetrationDepth = margin - dist;
|
||||||
|
|
||||||
// Reject the contact if the penetration depth is negative (due too numerical errors)
|
// Reject the contact if the penetration depth is negative (due too numerical errors)
|
||||||
@ -173,11 +173,11 @@ void GJKAlgorithm::testCollision( CollisionShapeInfo shape1Info,
|
|||||||
// Create the contact info object
|
// Create the contact info object
|
||||||
ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape,
|
ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape,
|
||||||
shape2Info.collisionShape, normal, penetrationDepth, pA, pB);
|
shape2Info.collisionShape, normal, penetrationDepth, pA, pB);
|
||||||
narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo);
|
narrowPhaseCallback.notifyContact(shape1Info.overlappingPair, contactInfo);
|
||||||
// There is an intersection, therefore we return
|
// There is an intersection, therefore we return
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
} while(!simplex.isFull() hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj distSquare > FLTEPSILON *
|
} while(!simplex.isFull() && distSquare > FLTEPSILON *
|
||||||
simplex.getMaxLengthSquareOfAPoint());
|
simplex.getMaxLengthSquareOfAPoint());
|
||||||
// The objects (without margins) intersect. Therefore, we run the GJK algorithm
|
// The objects (without margins) intersect. Therefore, we run the GJK algorithm
|
||||||
// again but on the enlarged objects to compute a simplex polytope that contains
|
// again but on the enlarged objects to compute a simplex polytope that contains
|
||||||
@ -188,35 +188,35 @@ void GJKAlgorithm::testCollision( CollisionShapeInfo shape1Info,
|
|||||||
}
|
}
|
||||||
|
|
||||||
void GJKAlgorithm::computePenetrationDepthForEnlargedObjects( CollisionShapeInfo shape1Info,
|
void GJKAlgorithm::computePenetrationDepthForEnlargedObjects( CollisionShapeInfo shape1Info,
|
||||||
etk::Transform3D transform1,
|
Transform3D transform1,
|
||||||
CollisionShapeInfo shape2Info,
|
CollisionShapeInfo shape2Info,
|
||||||
etk::Transform3D transform2,
|
Transform3D transform2,
|
||||||
NarrowPhaseCallback* narrowPhaseCallback,
|
NarrowPhaseCallback* narrowPhaseCallback,
|
||||||
vec3 v) {
|
Vector3f v) {
|
||||||
PROFILE("GJKAlgorithm::computePenetrationDepthForEnlargedObjects()");
|
PROFILE("GJKAlgorithm::computePenetrationDepthForEnlargedObjects()");
|
||||||
Simplex simplex;
|
Simplex simplex;
|
||||||
vec3 suppA;
|
Vector3f suppA;
|
||||||
vec3 suppB;
|
Vector3f suppB;
|
||||||
vec3 w;
|
Vector3f w;
|
||||||
float vDotw;
|
float vDotw;
|
||||||
float distSquare = FLTMAX;
|
float distSquare = FLTMAX;
|
||||||
float prevDistSquare;
|
float prevDistSquare;
|
||||||
assert(shape1Info.collisionShape->isConvex());
|
assert(shape1Info.collisionShape.isConvex());
|
||||||
assert(shape2Info.collisionShape->isConvex());
|
assert(shape2Info.collisionShape.isConvex());
|
||||||
ConvexShape* shape1 = staticcast< ConvexShape*>(shape1Info.collisionShape);
|
ConvexShape* shape1 = staticcast< ConvexShape*>(shape1Info.collisionShape);
|
||||||
ConvexShape* shape2 = staticcast< ConvexShape*>(shape2Info.collisionShape);
|
ConvexShape* shape2 = staticcast< ConvexShape*>(shape2Info.collisionShape);
|
||||||
void** shape1CachedCollisionData = shape1Info.cachedCollisionData;
|
void** shape1CachedCollisionData = shape1Info.cachedCollisionData;
|
||||||
void** shape2CachedCollisionData = shape2Info.cachedCollisionData;
|
void** shape2CachedCollisionData = shape2Info.cachedCollisionData;
|
||||||
// etk::Transform3D a point from local space of body 2 to local space
|
// Transform3D a point from local space of body 2 to local space
|
||||||
// of body 1 (the GJK algorithm is done in local space of body 1)
|
// of body 1 (the GJK algorithm is done in local space of body 1)
|
||||||
etk::Transform3D body2ToBody1 = transform1.getInverse() * transform2;
|
Transform3D body2ToBody1 = transform1.getInverse() * transform2;
|
||||||
// Matrix that transform a direction from local space of body 1 into local space of body 2
|
// Matrix that transform a direction from local space of body 1 into local space of body 2
|
||||||
etk::Matrix3x3 rotateToBody2 = transform2.getOrientation().getMatrix().getTranspose() *
|
Matrix3f rotateToBody2 = transform2.getOrientation().getMatrix().getTranspose() *
|
||||||
transform1.getOrientation().getMatrix();
|
transform1.getOrientation().getMatrix();
|
||||||
do {
|
do {
|
||||||
// Compute the support points for the enlarged object A and B
|
// Compute the support points for the enlarged object A and B
|
||||||
suppA = shape1->getLocalSupportPointWithMargin(-v, shape1CachedCollisionData);
|
suppA = shape1.getLocalSupportPointWithMargin(-v, shape1CachedCollisionData);
|
||||||
suppB = body2ToBody1 * shape2->getLocalSupportPointWithMargin(rotateToBody2 * v, shape2CachedCollisionData);
|
suppB = body2ToBody1 * shape2.getLocalSupportPointWithMargin(rotateToBody2 * v, shape2CachedCollisionData);
|
||||||
// Compute the support point for the Minkowski difference A-B
|
// Compute the support point for the Minkowski difference A-B
|
||||||
w = suppA - suppB;
|
w = suppA - suppB;
|
||||||
vDotw = v.dot(w);
|
vDotw = v.dot(w);
|
||||||
@ -239,7 +239,7 @@ void GJKAlgorithm::computePenetrationDepthForEnlargedObjects( CollisionShapeInfo
|
|||||||
if (prevDistSquare - distSquare <= FLTEPSILON * prevDistSquare) {
|
if (prevDistSquare - distSquare <= FLTEPSILON * prevDistSquare) {
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
} while(!simplex.isFull() hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj distSquare > FLTEPSILON *
|
} while(!simplex.isFull() && distSquare > FLTEPSILON *
|
||||||
simplex.getMaxLengthSquareOfAPoint());
|
simplex.getMaxLengthSquareOfAPoint());
|
||||||
// Give the simplex computed with GJK algorithm to the EPA algorithm
|
// Give the simplex computed with GJK algorithm to the EPA algorithm
|
||||||
// which will compute the correct penetration depth and contact points
|
// which will compute the correct penetration depth and contact points
|
||||||
@ -249,24 +249,24 @@ void GJKAlgorithm::computePenetrationDepthForEnlargedObjects( CollisionShapeInfo
|
|||||||
v, narrowPhaseCallback);
|
v, narrowPhaseCallback);
|
||||||
}
|
}
|
||||||
|
|
||||||
boolean GJKAlgorithm::testPointInside( vec3 localPoint, ProxyShape* proxyShape) {
|
boolean GJKAlgorithm::testPointInside( Vector3f localPoint, ProxyShape* proxyShape) {
|
||||||
vec3 suppA; // Support point of object A
|
Vector3f suppA; // Support point of object A
|
||||||
vec3 w; // Support point of Minkowski difference A-B
|
Vector3f w; // Support point of Minkowski difference A-B
|
||||||
float prevDistSquare;
|
float prevDistSquare;
|
||||||
assert(proxyShape->getCollisionShape()->isConvex());
|
assert(proxyShape.getCollisionShape().isConvex());
|
||||||
ConvexShape* shape = staticcast< ConvexShape*>(proxyShape->getCollisionShape());
|
ConvexShape* shape = staticcast< ConvexShape*>(proxyShape.getCollisionShape());
|
||||||
void** shapeCachedCollisionData = proxyShape->getCachedCollisionData();
|
void** shapeCachedCollisionData = proxyShape.getCachedCollisionData();
|
||||||
// Support point of object B (object B is a single point)
|
// Support point of object B (object B is a single point)
|
||||||
vec3 suppB(localPoint);
|
Vector3f suppB(localPoint);
|
||||||
// Create a simplex set
|
// Create a simplex set
|
||||||
Simplex simplex;
|
Simplex simplex;
|
||||||
// Initial supporting direction
|
// Initial supporting direction
|
||||||
vec3 v(1, 1, 1);
|
Vector3f v(1, 1, 1);
|
||||||
// Initialize the upper bound for the square distance
|
// Initialize the upper bound for the square distance
|
||||||
float distSquare = FLTMAX;
|
float distSquare = FLTMAX;
|
||||||
do {
|
do {
|
||||||
// Compute the support points for original objects (without margins) A and B
|
// Compute the support points for original objects (without margins) A and B
|
||||||
suppA = shape->getLocalSupportPointWithoutMargin(-v, shapeCachedCollisionData);
|
suppA = shape.getLocalSupportPointWithoutMargin(-v, shapeCachedCollisionData);
|
||||||
// Compute the support point for the Minkowski difference A-B
|
// Compute the support point for the Minkowski difference A-B
|
||||||
w = suppA - suppB;
|
w = suppA - suppB;
|
||||||
// Add the new support point to the simplex
|
// Add the new support point to the simplex
|
||||||
@ -288,38 +288,38 @@ boolean GJKAlgorithm::testPointInside( vec3 localPoint, ProxyShape* proxyShape)
|
|||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
} while( !simplex.isFull()
|
} while( !simplex.isFull()
|
||||||
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj distSquare > FLTEPSILON * simplex.getMaxLengthSquareOfAPoint());
|
&& distSquare > FLTEPSILON * simplex.getMaxLengthSquareOfAPoint());
|
||||||
// The point is inside the collision shape
|
// The point is inside the collision shape
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
boolean GJKAlgorithm::raycast( Ray ray, ProxyShape* proxyShape, RaycastInfo raycastInfo) {
|
boolean GJKAlgorithm::raycast( Ray ray, ProxyShape* proxyShape, RaycastInfo raycastInfo) {
|
||||||
assert(proxyShape->getCollisionShape()->isConvex());
|
assert(proxyShape.getCollisionShape().isConvex());
|
||||||
ConvexShape* shape = staticcast< ConvexShape*>(proxyShape->getCollisionShape());
|
ConvexShape* shape = staticcast< ConvexShape*>(proxyShape.getCollisionShape());
|
||||||
void** shapeCachedCollisionData = proxyShape->getCachedCollisionData();
|
void** shapeCachedCollisionData = proxyShape.getCachedCollisionData();
|
||||||
vec3 suppA; // Current lower bound point on the ray (starting at ray's origin)
|
Vector3f suppA; // Current lower bound point on the ray (starting at ray's origin)
|
||||||
vec3 suppB; // Support point on the collision shape
|
Vector3f suppB; // Support point on the collision shape
|
||||||
float machineEpsilonSquare = FLTEPSILON * FLTEPSILON;
|
float machineEpsilonSquare = FLTEPSILON * FLTEPSILON;
|
||||||
float epsilon = float(0.0001);
|
float epsilon = float(0.0001);
|
||||||
// Convert the ray origin and direction into the local-space of the collision shape
|
// Convert the ray origin and direction into the local-space of the collision shape
|
||||||
vec3 rayDirection = ray.point2 - ray.point1;
|
Vector3f rayDirection = ray.point2 - ray.point1;
|
||||||
// If the points of the segment are two close, return no hit
|
// If the points of the segment are two close, return no hit
|
||||||
if (rayDirection.length2() < machineEpsilonSquare) return false;
|
if (rayDirection.length2() < machineEpsilonSquare) return false;
|
||||||
vec3 w;
|
Vector3f w;
|
||||||
// Create a simplex set
|
// Create a simplex set
|
||||||
Simplex simplex;
|
Simplex simplex;
|
||||||
vec3 n(0.0f, float(0.0), float(0.0));
|
Vector3f n(0.0f, float(0.0), float(0.0));
|
||||||
float lambda = 0.0f;
|
float lambda = 0.0f;
|
||||||
suppA = ray.point1; // Current lower bound point on the ray (starting at ray's origin)
|
suppA = ray.point1; // Current lower bound point on the ray (starting at ray's origin)
|
||||||
suppB = shape->getLocalSupportPointWithoutMargin(rayDirection, shapeCachedCollisionData);
|
suppB = shape.getLocalSupportPointWithoutMargin(rayDirection, shapeCachedCollisionData);
|
||||||
vec3 v = suppA - suppB;
|
Vector3f v = suppA - suppB;
|
||||||
float vDotW, vDotR;
|
float vDotW, vDotR;
|
||||||
float distSquare = v.length2();
|
float distSquare = v.length2();
|
||||||
int nbIterations = 0;
|
int nbIterations = 0;
|
||||||
// GJK Algorithm loop
|
// GJK Algorithm loop
|
||||||
while (distSquare > epsilon hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj nbIterations < MAXITERATIONSGJKRAYCAST) {
|
while (distSquare > epsilon && nbIterations < MAXITERATIONSGJKRAYCAST) {
|
||||||
// Compute the support points
|
// Compute the support points
|
||||||
suppB = shape->getLocalSupportPointWithoutMargin(v, shapeCachedCollisionData);
|
suppB = shape.getLocalSupportPointWithoutMargin(v, shapeCachedCollisionData);
|
||||||
w = suppA - suppB;
|
w = suppA - suppB;
|
||||||
vDotW = v.dot(w);
|
vDotW = v.dot(w);
|
||||||
if (vDotW > float(0)) {
|
if (vDotW > float(0)) {
|
||||||
@ -353,18 +353,18 @@ boolean GJKAlgorithm::raycast( Ray ray, ProxyShape* proxyShape, RaycastInfo rayc
|
|||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
// Compute the closet points of both objects (without the margins)
|
// Compute the closet points of both objects (without the margins)
|
||||||
vec3 pointA;
|
Vector3f pointA;
|
||||||
vec3 pointB;
|
Vector3f pointB;
|
||||||
simplex.computeClosestPointsOfAandB(pointA, pointB);
|
simplex.computeClosestPointsOfAandB(pointA, pointB);
|
||||||
// A raycast hit has been found, we fill in the raycast info
|
// A raycast hit has been found, we fill in the raycast info
|
||||||
raycastInfo.hitFraction = lambda;
|
raycastInfo.hitFraction = lambda;
|
||||||
raycastInfo.worldPoint = pointB;
|
raycastInfo.worldPoint = pointB;
|
||||||
raycastInfo.body = proxyShape->getBody();
|
raycastInfo.body = proxyShape.getBody();
|
||||||
raycastInfo.proxyShape = proxyShape;
|
raycastInfo.proxyShape = proxyShape;
|
||||||
if (n.length2() >= machineEpsilonSquare) { // The normal vector is valid
|
if (n.length2() >= machineEpsilonSquare) { // The normal vector is valid
|
||||||
raycastInfo.worldNormal = n;
|
raycastInfo.worldNormal = n;
|
||||||
} else { // Degenerated normal vector, we return a zero normal vector
|
} else { // Degenerated normal vector, we return a zero normal vector
|
||||||
raycastInfo.worldNormal = vec3(float(0), float(0), float(0));
|
raycastInfo.worldNormal = Vector3f(float(0), float(0), float(0));
|
||||||
}
|
}
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
@ -1,19 +1,5 @@
|
|||||||
/** @file
|
package org.atriaSoft.ephysics.collision.narrowphase.GJK;
|
||||||
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
|
|
||||||
* @author Daniel CHAPPUIS
|
|
||||||
* @author Edouard DUPIN
|
|
||||||
* @copyright 2010-2016, Daniel Chappuis
|
|
||||||
* @copyright 2017, Edouard DUPIN
|
|
||||||
* @license MPL v2.0 (see license file)
|
|
||||||
*/
|
|
||||||
#pragma once
|
|
||||||
|
|
||||||
#include <ephysics/collision/narrowphase/NarrowPhaseAlgorithm.hpp>
|
|
||||||
#include <ephysics/raint/ContactPoint.hpp>
|
|
||||||
#include <ephysics/collision/shapes/ConvexShape.hpp>
|
|
||||||
#include <ephysics/collision/narrowphase/EPA/EPAAlgorithm.hpp>
|
|
||||||
|
|
||||||
namespace ephysics {
|
|
||||||
float RELERROR = float(1.0e-3);
|
float RELERROR = float(1.0e-3);
|
||||||
float RELERRORSQUARE = RELERROR * RELERROR;
|
float RELERRORSQUARE = RELERROR * RELERROR;
|
||||||
int MAXITERATIONSGJKRAYCAST = 32;
|
int MAXITERATIONSGJKRAYCAST = 32;
|
||||||
@ -32,7 +18,7 @@ namespace ephysics {
|
|||||||
* Polytope Algorithm) to compute the correct penetration depth between the
|
* Polytope Algorithm) to compute the correct penetration depth between the
|
||||||
* enlarged objects.
|
* enlarged objects.
|
||||||
*/
|
*/
|
||||||
class GJKAlgorithm : public NarrowPhaseAlgorithm {
|
class GJKAlgorithm extends NarrowPhaseAlgorithm {
|
||||||
private :
|
private :
|
||||||
EPAAlgorithm this.algoEPA; //!< EPA Algorithm
|
EPAAlgorithm this.algoEPA; //!< EPA Algorithm
|
||||||
/// Private copy-ructor
|
/// Private copy-ructor
|
||||||
@ -45,18 +31,16 @@ namespace ephysics {
|
|||||||
/// a polytope must exist. Then, we give that polytope to the EPA algorithm to
|
/// a polytope must exist. Then, we give that polytope to the EPA algorithm to
|
||||||
/// compute the correct penetration depth and contact points of the enlarged objects.
|
/// compute the correct penetration depth and contact points of the enlarged objects.
|
||||||
void computePenetrationDepthForEnlargedObjects( CollisionShapeInfo shape1Info,
|
void computePenetrationDepthForEnlargedObjects( CollisionShapeInfo shape1Info,
|
||||||
etk::Transform3D transform1,
|
Transform3D transform1,
|
||||||
CollisionShapeInfo shape2Info,
|
CollisionShapeInfo shape2Info,
|
||||||
etk::Transform3D transform2,
|
Transform3D transform2,
|
||||||
NarrowPhaseCallback* narrowPhaseCallback,
|
NarrowPhaseCallback* narrowPhaseCallback,
|
||||||
vec3 v);
|
Vector3f v);
|
||||||
public :
|
public :
|
||||||
/// Constructor
|
/// Constructor
|
||||||
GJKAlgorithm();
|
GJKAlgorithm();
|
||||||
/// Destructor
|
|
||||||
~GJKAlgorithm();
|
|
||||||
/// Initalize the algorithm
|
/// Initalize the algorithm
|
||||||
virtual void init(CollisionDetection* collisionDetection) {
|
void init(CollisionDetection* collisionDetection) {
|
||||||
NarrowPhaseAlgorithm::init(collisionDetection);
|
NarrowPhaseAlgorithm::init(collisionDetection);
|
||||||
this.algoEPA.init();
|
this.algoEPA.init();
|
||||||
};
|
};
|
||||||
@ -69,11 +53,11 @@ namespace ephysics {
|
|||||||
/// algorithm on the enlarged object to obtain a simplex polytope that contains the
|
/// algorithm on the enlarged object to obtain a simplex polytope that contains the
|
||||||
/// origin, they we give that simplex polytope to the EPA algorithm which will compute
|
/// origin, they we give that simplex polytope to the EPA algorithm which will compute
|
||||||
/// the correct penetration depth and contact points between the enlarged objects.
|
/// the correct penetration depth and contact points between the enlarged objects.
|
||||||
virtual void testCollision( CollisionShapeInfo shape1Info,
|
void testCollision( CollisionShapeInfo shape1Info,
|
||||||
CollisionShapeInfo shape2Info,
|
CollisionShapeInfo shape2Info,
|
||||||
NarrowPhaseCallback* narrowPhaseCallback);
|
NarrowPhaseCallback* narrowPhaseCallback);
|
||||||
/// Use the GJK Algorithm to find if a point is inside a convex collision shape
|
/// Use the GJK Algorithm to find if a point is inside a convex collision shape
|
||||||
boolean testPointInside( vec3 localPoint, ProxyShape* proxyShape);
|
boolean testPointInside( Vector3f localPoint, ProxyShape* proxyShape);
|
||||||
/// Ray casting algorithm agains a convex collision shape using the GJK Algorithm
|
/// Ray casting algorithm agains a convex collision shape using the GJK Algorithm
|
||||||
/// This method implements the GJK ray casting algorithm described by Gino Van Den Bergen in
|
/// This method implements the GJK ray casting algorithm described by Gino Van Den Bergen in
|
||||||
/// "Ray Casting against General Convex Objects with Application to Continuous Collision Detection".
|
/// "Ray Casting against General Convex Objects with Application to Continuous Collision Detection".
|
@ -18,7 +18,7 @@ Simplex::Simplex() : mBitsCurrentSimplex(0x0), mAllBits(0x0) {
|
|||||||
/// suppPointA : support point of object A in a direction -v
|
/// suppPointA : support point of object A in a direction -v
|
||||||
/// suppPointB : support point of object B in a direction v
|
/// suppPointB : support point of object B in a direction v
|
||||||
/// point : support point of object (A-B) => point = suppPointA - suppPointB
|
/// point : support point of object (A-B) => point = suppPointA - suppPointB
|
||||||
void Simplex::addPoint( vec3 point, vec3 suppPointA, vec3 suppPointB) {
|
void Simplex::addPoint( Vector3f point, Vector3f suppPointA, Vector3f suppPointB) {
|
||||||
assert(!isFull());
|
assert(!isFull());
|
||||||
|
|
||||||
mLastFound = 0;
|
mLastFound = 0;
|
||||||
@ -28,7 +28,7 @@ void Simplex::addPoint( vec3 point, vec3 suppPointA, vec3 suppPointB) {
|
|||||||
// the current simplex
|
// the current simplex
|
||||||
while (overlap(mBitsCurrentSimplex, mLastFoundBit)) {
|
while (overlap(mBitsCurrentSimplex, mLastFoundBit)) {
|
||||||
mLastFound++;
|
mLastFound++;
|
||||||
mLastFoundBit <<= 1;
|
mLastFoundBit += 1;
|
||||||
}
|
}
|
||||||
|
|
||||||
assert(mLastFound < 4);
|
assert(mLastFound < 4);
|
||||||
@ -50,14 +50,14 @@ void Simplex::addPoint( vec3 point, vec3 suppPointA, vec3 suppPointB) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
// Return true if the point is in the simplex
|
// Return true if the point is in the simplex
|
||||||
boolean Simplex::isPointInSimplex( vec3 point) {
|
boolean Simplex::isPointInSimplex( Vector3f point) {
|
||||||
int i;
|
int i;
|
||||||
Bits bit;
|
Bits bit;
|
||||||
|
|
||||||
// For each four possible points in the simplex
|
// For each four possible points in the simplex
|
||||||
for (i=0, bit = 0x1; i<4; i++, bit <<= 1) {
|
for (i=0, bit = 0x1; i<4; i++, bit += 1) {
|
||||||
// Check if the current point is in the simplex
|
// Check if the current point is in the simplex
|
||||||
if (overlap(mAllBits, bit) hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj point == mPoints[i]) return true;
|
if (overlap(mAllBits, bit) && point == mPoints[i]) return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
return false;
|
return false;
|
||||||
@ -69,7 +69,7 @@ void Simplex::updateCache() {
|
|||||||
Bits bit;
|
Bits bit;
|
||||||
|
|
||||||
// For each of the four possible points of the simplex
|
// For each of the four possible points of the simplex
|
||||||
for (i=0, bit = 0x1; i<4; i++, bit <<= 1) {
|
for (i=0, bit = 0x1; i<4; i++, bit += 1) {
|
||||||
// If the current points is in the simplex
|
// If the current points is in the simplex
|
||||||
if (overlap(mBitsCurrentSimplex, bit)) {
|
if (overlap(mBitsCurrentSimplex, bit)) {
|
||||||
|
|
||||||
@ -86,22 +86,22 @@ void Simplex::updateCache() {
|
|||||||
}
|
}
|
||||||
|
|
||||||
// Return the points of the simplex
|
// Return the points of the simplex
|
||||||
int Simplex::getSimplex(vec3* suppPointsA, vec3* suppPointsB,
|
int Simplex::getSimplex(Vector3f* suppPointsA, Vector3f* suppPointsB,
|
||||||
vec3* points) {
|
Vector3f* points) {
|
||||||
int nbVertices = 0;
|
int nbVertices = 0;
|
||||||
int i;
|
int i;
|
||||||
Bits bit;
|
Bits bit;
|
||||||
|
|
||||||
// For each four point in the possible simplex
|
// For each four point in the possible simplex
|
||||||
for (i=0, bit=0x1; i<4; i++, bit <<=1) {
|
for (i=0, bit=0x1; i<4; i++, bit +=1) {
|
||||||
|
|
||||||
// If the current point is in the simplex
|
// If the current point is in the simplex
|
||||||
if (overlap(mBitsCurrentSimplex, bit)) {
|
if (overlap(mBitsCurrentSimplex, bit)) {
|
||||||
|
|
||||||
// Store the points
|
// Store the points
|
||||||
suppPointsA[nbVertices] = this->mSuppPointsA[nbVertices];
|
suppPointsA[nbVertices] = this.mSuppPointsA[nbVertices];
|
||||||
suppPointsB[nbVertices] = this->mSuppPointsB[nbVertices];
|
suppPointsB[nbVertices] = this.mSuppPointsB[nbVertices];
|
||||||
points[nbVertices] = this->mPoints[nbVertices];
|
points[nbVertices] = this.mPoints[nbVertices];
|
||||||
|
|
||||||
nbVertices++;
|
nbVertices++;
|
||||||
}
|
}
|
||||||
@ -121,7 +121,7 @@ void Simplex::computeDeterminants() {
|
|||||||
Bits bitI;
|
Bits bitI;
|
||||||
|
|
||||||
// For each possible four points in the simplex set
|
// For each possible four points in the simplex set
|
||||||
for (i=0, bitI = 0x1; i<4; i++, bitI <<= 1) {
|
for (i=0, bitI = 0x1; i<4; i++, bitI += 1) {
|
||||||
|
|
||||||
// If the current point is in the simplex
|
// If the current point is in the simplex
|
||||||
if (overlap(mBitsCurrentSimplex, bitI)) {
|
if (overlap(mBitsCurrentSimplex, bitI)) {
|
||||||
@ -134,7 +134,7 @@ void Simplex::computeDeterminants() {
|
|||||||
int j;
|
int j;
|
||||||
Bits bitJ;
|
Bits bitJ;
|
||||||
|
|
||||||
for (j=0, bitJ = 0x1; j<i; j++, bitJ <<= 1) {
|
for (j=0, bitJ = 0x1; j<i; j++, bitJ += 1) {
|
||||||
if (overlap(mBitsCurrentSimplex, bitJ)) {
|
if (overlap(mBitsCurrentSimplex, bitJ)) {
|
||||||
int k;
|
int k;
|
||||||
Bits bit3 = bitJ | bit2;
|
Bits bit3 = bitJ | bit2;
|
||||||
@ -202,8 +202,8 @@ boolean Simplex::isProperSubset(Bits subset) {
|
|||||||
Bits bit;
|
Bits bit;
|
||||||
|
|
||||||
// For each four point of the possible simplex set
|
// For each four point of the possible simplex set
|
||||||
for (i=0, bit=0x1; i<4; i++, bit <<=1) {
|
for (i=0, bit=0x1; i<4; i++, bit +=1) {
|
||||||
if (overlap(subset, bit) hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj mDet[subset][i] <= 0.0) {
|
if (overlap(subset, bit) && mDet[subset][i] <= 0.0) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@ -220,7 +220,7 @@ boolean Simplex::isAffinelyDependent() {
|
|||||||
Bits bit;
|
Bits bit;
|
||||||
|
|
||||||
// For each four point of the possible simplex set
|
// For each four point of the possible simplex set
|
||||||
for (i=0, bit=0x1; i<4; i++, bit <<= 1) {
|
for (i=0, bit=0x1; i<4; i++, bit += 1) {
|
||||||
if (overlap(mAllBits, bit)) {
|
if (overlap(mAllBits, bit)) {
|
||||||
sum += mDet[mAllBits][i];
|
sum += mDet[mAllBits][i];
|
||||||
}
|
}
|
||||||
@ -238,7 +238,7 @@ boolean Simplex::isValidSubset(Bits subset) {
|
|||||||
Bits bit;
|
Bits bit;
|
||||||
|
|
||||||
// For each four point in the possible simplex set
|
// For each four point in the possible simplex set
|
||||||
for (i=0, bit=0x1; i<4; i++, bit <<= 1) {
|
for (i=0, bit=0x1; i<4; i++, bit += 1) {
|
||||||
if (overlap(mAllBits, bit)) {
|
if (overlap(mAllBits, bit)) {
|
||||||
// If the current point is in the subset
|
// If the current point is in the subset
|
||||||
if (overlap(subset, bit)) {
|
if (overlap(subset, bit)) {
|
||||||
@ -265,7 +265,7 @@ boolean Simplex::isValidSubset(Bits subset) {
|
|||||||
/// pA = sum(lambdai * ai) where "ai" are the support points of object A
|
/// pA = sum(lambdai * ai) where "ai" are the support points of object A
|
||||||
/// pB = sum(lambdai * bi) where "bi" are the support points of object B
|
/// pB = sum(lambdai * bi) where "bi" are the support points of object B
|
||||||
/// with lambdai = deltaXi / deltaX
|
/// with lambdai = deltaXi / deltaX
|
||||||
void Simplex::computeClosestPointsOfAandB(vec3 pA, vec3 pB) {
|
void Simplex::computeClosestPointsOfAandB(Vector3f pA, Vector3f pB) {
|
||||||
float deltaX = 0.0;
|
float deltaX = 0.0;
|
||||||
pA.setValue(0.0, 0.0, 0.0);
|
pA.setValue(0.0, 0.0, 0.0);
|
||||||
pB.setValue(0.0, 0.0, 0.0);
|
pB.setValue(0.0, 0.0, 0.0);
|
||||||
@ -273,7 +273,7 @@ void Simplex::computeClosestPointsOfAandB(vec3 pA, vec3 pB) {
|
|||||||
Bits bit;
|
Bits bit;
|
||||||
|
|
||||||
// For each four points in the possible simplex set
|
// For each four points in the possible simplex set
|
||||||
for (i=0, bit=0x1; i<4; i++, bit <<= 1) {
|
for (i=0, bit=0x1; i<4; i++, bit += 1) {
|
||||||
// If the current point is part of the simplex
|
// If the current point is part of the simplex
|
||||||
if (overlap(mBitsCurrentSimplex, bit)) {
|
if (overlap(mBitsCurrentSimplex, bit)) {
|
||||||
deltaX += mDet[mBitsCurrentSimplex][i];
|
deltaX += mDet[mBitsCurrentSimplex][i];
|
||||||
@ -292,14 +292,14 @@ void Simplex::computeClosestPointsOfAandB(vec3 pA, vec3 pB) {
|
|||||||
/// This method executes the Jonhnson's algorithm for computing the point
|
/// This method executes the Jonhnson's algorithm for computing the point
|
||||||
/// "v" of simplex that is closest to the origin. The method returns true
|
/// "v" of simplex that is closest to the origin. The method returns true
|
||||||
/// if a closest point has been found.
|
/// if a closest point has been found.
|
||||||
boolean Simplex::computeClosestPoint(vec3 v) {
|
boolean Simplex::computeClosestPoint(Vector3f v) {
|
||||||
Bits subset;
|
Bits subset;
|
||||||
|
|
||||||
// For each possible simplex set
|
// For each possible simplex set
|
||||||
for (subset=mBitsCurrentSimplex; subset != 0x0; subset--) {
|
for (subset=mBitsCurrentSimplex; subset != 0x0; subset--) {
|
||||||
// If the simplex is a subset of the current simplex and is valid for the Johnson's
|
// If the simplex is a subset of the current simplex and is valid for the Johnson's
|
||||||
// algorithm test
|
// algorithm test
|
||||||
if (isSubset(subset, mBitsCurrentSimplex) hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj isValidSubset(subset | mLastFoundBit)) {
|
if (isSubset(subset, mBitsCurrentSimplex) && isValidSubset(subset | mLastFoundBit)) {
|
||||||
mBitsCurrentSimplex = subset | mLastFoundBit; // Add the last added point to the current simplex
|
mBitsCurrentSimplex = subset | mLastFoundBit; // Add the last added point to the current simplex
|
||||||
v = computeClosestPointForSubset(mBitsCurrentSimplex); // Compute the closest point in the simplex
|
v = computeClosestPointForSubset(mBitsCurrentSimplex); // Compute the closest point in the simplex
|
||||||
return true;
|
return true;
|
||||||
@ -319,13 +319,13 @@ boolean Simplex::computeClosestPoint(vec3 v) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
// Backup the closest point
|
// Backup the closest point
|
||||||
void Simplex::backupClosestPointInSimplex(vec3 v) {
|
void Simplex::backupClosestPointInSimplex(Vector3f v) {
|
||||||
float minDistSquare = FLTMAX;
|
float minDistSquare = FLTMAX;
|
||||||
Bits bit;
|
Bits bit;
|
||||||
|
|
||||||
for (bit = mAllBits; bit != 0x0; bit--) {
|
for (bit = mAllBits; bit != 0x0; bit--) {
|
||||||
if (isSubset(bit, mAllBits) hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj isProperSubset(bit)) {
|
if (isSubset(bit, mAllBits) && isProperSubset(bit)) {
|
||||||
vec3 u = computeClosestPointForSubset(bit);
|
Vector3f u = computeClosestPointForSubset(bit);
|
||||||
float distSquare = u.dot(u);
|
float distSquare = u.dot(u);
|
||||||
if (distSquare < minDistSquare) {
|
if (distSquare < minDistSquare) {
|
||||||
minDistSquare = distSquare;
|
minDistSquare = distSquare;
|
||||||
@ -338,15 +338,15 @@ void Simplex::backupClosestPointInSimplex(vec3 v) {
|
|||||||
|
|
||||||
// Return the closest point "v" in the convex hull of the points in the subset
|
// Return the closest point "v" in the convex hull of the points in the subset
|
||||||
// represented by the bits "subset"
|
// represented by the bits "subset"
|
||||||
vec3 Simplex::computeClosestPointForSubset(Bits subset) {
|
Vector3f Simplex::computeClosestPointForSubset(Bits subset) {
|
||||||
vec3 v(0.0, 0.0, 0.0); // Closet point v = sum(lambdai * points[i])
|
Vector3f v(0.0, 0.0, 0.0); // Closet point v = sum(lambdai * points[i])
|
||||||
mMaxLengthSquare = 0.0;
|
mMaxLengthSquare = 0.0;
|
||||||
float deltaX = 0.0; // deltaX = sum of all det[subset][i]
|
float deltaX = 0.0; // deltaX = sum of all det[subset][i]
|
||||||
int i;
|
int i;
|
||||||
Bits bit;
|
Bits bit;
|
||||||
|
|
||||||
// For each four point in the possible simplex set
|
// For each four point in the possible simplex set
|
||||||
for (i=0, bit=0x1; i<4; i++, bit <<= 1) {
|
for (i=0, bit=0x1; i<4; i++, bit += 1) {
|
||||||
// If the current point is in the subset
|
// If the current point is in the subset
|
||||||
if (overlap(subset, bit)) {
|
if (overlap(subset, bit)) {
|
||||||
// deltaX = sum of all det[subset][i]
|
// deltaX = sum of all det[subset][i]
|
||||||
|
@ -1,22 +1,4 @@
|
|||||||
/** @file
|
package org.atriaSoft.ephysics.collision.narrowphase.GJK;
|
||||||
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
|
|
||||||
* @author Daniel CHAPPUIS
|
|
||||||
* @author Edouard DUPIN
|
|
||||||
* @copyright 2010-2016, Daniel Chappuis
|
|
||||||
* @copyright 2017, Edouard DUPIN
|
|
||||||
* @license MPL v2.0 (see license file)
|
|
||||||
*/
|
|
||||||
#pragma once
|
|
||||||
|
|
||||||
// Libraries
|
|
||||||
#include <ephysics/mathematics/mathematics.hpp>
|
|
||||||
#include <etk/Vector.hpp>
|
|
||||||
|
|
||||||
/// ReactPhysics3D namespace
|
|
||||||
namespace ephysics {
|
|
||||||
|
|
||||||
// Type definitions
|
|
||||||
typedef int Bits;
|
|
||||||
|
|
||||||
// Class Simplex
|
// Class Simplex
|
||||||
/**
|
/**
|
||||||
@ -34,7 +16,7 @@ class Simplex {
|
|||||||
// -------------------- Attributes -------------------- //
|
// -------------------- Attributes -------------------- //
|
||||||
|
|
||||||
/// Current points
|
/// Current points
|
||||||
vec3 mPoints[4];
|
Vector3f mPoints[4];
|
||||||
|
|
||||||
/// pointsLengthSquare[i] = (points[i].length)^2
|
/// pointsLengthSquare[i] = (points[i].length)^2
|
||||||
float mPointsLengthSquare[4];
|
float mPointsLengthSquare[4];
|
||||||
@ -43,13 +25,13 @@ class Simplex {
|
|||||||
float mMaxLengthSquare;
|
float mMaxLengthSquare;
|
||||||
|
|
||||||
/// Support points of object A in local coordinates
|
/// Support points of object A in local coordinates
|
||||||
vec3 mSuppPointsA[4];
|
Vector3f mSuppPointsA[4];
|
||||||
|
|
||||||
/// Support points of object B in local coordinates
|
/// Support points of object B in local coordinates
|
||||||
vec3 mSuppPointsB[4];
|
Vector3f mSuppPointsB[4];
|
||||||
|
|
||||||
/// diff[i][j] contains points[i] - points[j]
|
/// diff[i][j] contains points[i] - points[j]
|
||||||
vec3 mDiffLength[4][4];
|
Vector3f mDiffLength[4][4];
|
||||||
|
|
||||||
/// Cached determinant values
|
/// Cached determinant values
|
||||||
float mDet[16][4];
|
float mDet[16][4];
|
||||||
@ -59,16 +41,16 @@ class Simplex {
|
|||||||
|
|
||||||
/// 4 bits that identify the current points of the simplex
|
/// 4 bits that identify the current points of the simplex
|
||||||
/// For instance, 0101 means that points[1] and points[3] are in the simplex
|
/// For instance, 0101 means that points[1] and points[3] are in the simplex
|
||||||
Bits mBitsCurrentSimplex;
|
int mBitsCurrentSimplex;
|
||||||
|
|
||||||
/// Number between 1 and 4 that identify the last found support point
|
/// Number between 1 and 4 that identify the last found support point
|
||||||
Bits mLastFound;
|
int mLastFound;
|
||||||
|
|
||||||
/// Position of the last found support point (lastFoundBit = 0x1 << lastFound)
|
/// Position of the last found support point (lastFoundBit = 0x1 + lastFound)
|
||||||
Bits mLastFoundBit;
|
int mLastFoundBit;
|
||||||
|
|
||||||
/// allBits = bitsCurrentSimplex | lastFoundBit;
|
/// allBits = bitsCurrentSimplex | lastFoundBit;
|
||||||
Bits mAllBits;
|
int mAllBits;
|
||||||
|
|
||||||
// -------------------- Methods -------------------- //
|
// -------------------- Methods -------------------- //
|
||||||
|
|
||||||
@ -97,7 +79,7 @@ class Simplex {
|
|||||||
void computeDeterminants();
|
void computeDeterminants();
|
||||||
|
|
||||||
/// Return the closest point "v" in the convex hull of a subset of points
|
/// Return the closest point "v" in the convex hull of a subset of points
|
||||||
vec3 computeClosestPointForSubset(Bits subset);
|
Vector3f computeClosestPointForSubset(Bits subset);
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|
||||||
@ -113,29 +95,29 @@ class Simplex {
|
|||||||
boolean isEmpty() ;
|
boolean isEmpty() ;
|
||||||
|
|
||||||
/// Return the points of the simplex
|
/// Return the points of the simplex
|
||||||
int getSimplex(vec3* mSuppPointsA, vec3* mSuppPointsB,
|
int getSimplex(Vector3f* mSuppPointsA, Vector3f* mSuppPointsB,
|
||||||
vec3* mPoints) ;
|
Vector3f* mPoints) ;
|
||||||
|
|
||||||
/// Return the maximum squared length of a point
|
/// Return the maximum squared length of a point
|
||||||
float getMaxLengthSquareOfAPoint() ;
|
float getMaxLengthSquareOfAPoint() ;
|
||||||
|
|
||||||
/// Add a new support point of (A-B) into the simplex.
|
/// Add a new support point of (A-B) into the simplex.
|
||||||
void addPoint( vec3 point, vec3 suppPointA, vec3 suppPointB);
|
void addPoint( Vector3f point, Vector3f suppPointA, Vector3f suppPointB);
|
||||||
|
|
||||||
/// Return true if the point is in the simplex
|
/// Return true if the point is in the simplex
|
||||||
boolean isPointInSimplex( vec3 point) ;
|
boolean isPointInSimplex( Vector3f point) ;
|
||||||
|
|
||||||
/// Return true if the set is affinely dependent
|
/// Return true if the set is affinely dependent
|
||||||
boolean isAffinelyDependent() ;
|
boolean isAffinelyDependent() ;
|
||||||
|
|
||||||
/// Backup the closest point
|
/// Backup the closest point
|
||||||
void backupClosestPointInSimplex(vec3 point);
|
void backupClosestPointInSimplex(Vector3f point);
|
||||||
|
|
||||||
/// Compute the closest points "pA" and "pB" of object A and B.
|
/// Compute the closest points "pA" and "pB" of object A and B.
|
||||||
void computeClosestPointsOfAandB(vec3 pA, vec3 pB) ;
|
void computeClosestPointsOfAandB(Vector3f pA, Vector3f pB) ;
|
||||||
|
|
||||||
/// Compute the closest point to the origin of the current simplex.
|
/// Compute the closest point to the origin of the current simplex.
|
||||||
boolean computeClosestPoint(vec3 v);
|
boolean computeClosestPoint(Vector3f v);
|
||||||
};
|
};
|
||||||
|
|
||||||
// Return true if some bits of "a" overlap with bits of "b"
|
// Return true if some bits of "a" overlap with bits of "b"
|
@ -1,21 +1,5 @@
|
|||||||
/** @file
|
package org.atriaSoft.ephysics.collision.narrowphase;
|
||||||
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
|
|
||||||
* @author Daniel CHAPPUIS
|
|
||||||
* @author Edouard DUPIN
|
|
||||||
* @copyright 2010-2016, Daniel Chappuis
|
|
||||||
* @copyright 2017, Edouard DUPIN
|
|
||||||
* @license MPL v2.0 (see license file)
|
|
||||||
*/
|
|
||||||
#pragma once
|
|
||||||
|
|
||||||
#include <ephysics/body/Body.hpp>
|
|
||||||
#include <ephysics/raint/ContactPoint.hpp>
|
|
||||||
#include <ephysics/engine/OverlappingPair.hpp>
|
|
||||||
#include <ephysics/collision/CollisionShapeInfo.hpp>
|
|
||||||
|
|
||||||
namespace ephysics {
|
|
||||||
|
|
||||||
class CollisionDetection;
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @brief It is the base class for a narrow-phase collision
|
* @brief It is the base class for a narrow-phase collision
|
||||||
@ -23,9 +7,8 @@ namespace ephysics {
|
|||||||
*/
|
*/
|
||||||
class NarrowPhaseCallback {
|
class NarrowPhaseCallback {
|
||||||
public:
|
public:
|
||||||
virtual ~NarrowPhaseCallback() = default;
|
|
||||||
/// Called by a narrow-phase collision algorithm when a new contact has been found
|
/// Called by a narrow-phase collision algorithm when a new contact has been found
|
||||||
virtual void notifyContact(OverlappingPair* overlappingPair,
|
void notifyContact(OverlappingPair* overlappingPair,
|
||||||
ContactPointInfo contactInfo) = 0;
|
ContactPointInfo contactInfo) = 0;
|
||||||
|
|
||||||
};
|
};
|
||||||
@ -39,21 +22,15 @@ namespace ephysics {
|
|||||||
protected :
|
protected :
|
||||||
CollisionDetection* this.collisionDetection; //!< Pointer to the collision detection object
|
CollisionDetection* this.collisionDetection; //!< Pointer to the collision detection object
|
||||||
OverlappingPair* this.currentOverlappingPair; //!< Overlapping pair of the bodies currently tested for collision
|
OverlappingPair* this.currentOverlappingPair; //!< Overlapping pair of the bodies currently tested for collision
|
||||||
/// Private copy-ructor
|
|
||||||
NarrowPhaseAlgorithm( NarrowPhaseAlgorithm algorithm) = delete;
|
|
||||||
/// Private assignment operator
|
|
||||||
NarrowPhaseAlgorithm operator=( NarrowPhaseAlgorithm algorithm) = delete;
|
|
||||||
public :
|
public :
|
||||||
/// Constructor
|
/// Constructor
|
||||||
NarrowPhaseAlgorithm();
|
NarrowPhaseAlgorithm();
|
||||||
/// Destructor
|
|
||||||
virtual ~NarrowPhaseAlgorithm() = default;
|
|
||||||
/// Initalize the algorithm
|
/// Initalize the algorithm
|
||||||
virtual void init(CollisionDetection* collisionDetection);
|
void init(CollisionDetection* collisionDetection);
|
||||||
/// Set the current overlapping pair of bodies
|
/// Set the current overlapping pair of bodies
|
||||||
void setCurrentOverlappingPair(OverlappingPair* overlappingPair);
|
void setCurrentOverlappingPair(OverlappingPair* overlappingPair);
|
||||||
/// Compute a contact info if the two bounding volume collide
|
/// Compute a contact info if the two bounding volume collide
|
||||||
virtual void testCollision( CollisionShapeInfo shape1Info,
|
void testCollision( CollisionShapeInfo shape1Info,
|
||||||
CollisionShapeInfo shape2Info,
|
CollisionShapeInfo shape2Info,
|
||||||
NarrowPhaseCallback* narrowPhaseCallback) = 0;
|
NarrowPhaseCallback* narrowPhaseCallback) = 0;
|
||||||
};
|
};
|
@ -21,20 +21,20 @@ void ephysics::SphereVsSphereAlgorithm::testCollision( ephysics::CollisionShapeI
|
|||||||
ephysics::SphereShape* sphereShape1 = staticcast< ephysics::SphereShape*>(shape1Info.collisionShape);
|
ephysics::SphereShape* sphereShape1 = staticcast< ephysics::SphereShape*>(shape1Info.collisionShape);
|
||||||
ephysics::SphereShape* sphereShape2 = staticcast< ephysics::SphereShape*>(shape2Info.collisionShape);
|
ephysics::SphereShape* sphereShape2 = staticcast< ephysics::SphereShape*>(shape2Info.collisionShape);
|
||||||
// Get the local-space to world-space transforms
|
// Get the local-space to world-space transforms
|
||||||
etk::Transform3D transform1 = shape1Info.shapeToWorldTransform;
|
Transform3D transform1 = shape1Info.shapeToWorldTransform;
|
||||||
etk::Transform3D transform2 = shape2Info.shapeToWorldTransform;
|
Transform3D transform2 = shape2Info.shapeToWorldTransform;
|
||||||
// Compute the distance between the centers
|
// Compute the distance between the centers
|
||||||
vec3 vectorBetweenCenters = transform2.getPosition() - transform1.getPosition();
|
Vector3f vectorBetweenCenters = transform2.getPosition() - transform1.getPosition();
|
||||||
float squaredDistanceBetweenCenters = vectorBetweenCenters.length2();
|
float squaredDistanceBetweenCenters = vectorBetweenCenters.length2();
|
||||||
// Compute the sum of the radius
|
// Compute the sum of the radius
|
||||||
float sumRadius = sphereShape1->getRadius() + sphereShape2->getRadius();
|
float sumRadius = sphereShape1.getRadius() + sphereShape2.getRadius();
|
||||||
// If the sphere collision shapes intersect
|
// If the sphere collision shapes intersect
|
||||||
if (squaredDistanceBetweenCenters <= sumRadius * sumRadius) {
|
if (squaredDistanceBetweenCenters <= sumRadius * sumRadius) {
|
||||||
vec3 centerSphere2InBody1LocalSpace = transform1.getInverse() * transform2.getPosition();
|
Vector3f centerSphere2InBody1LocalSpace = transform1.getInverse() * transform2.getPosition();
|
||||||
vec3 centerSphere1InBody2LocalSpace = transform2.getInverse() * transform1.getPosition();
|
Vector3f centerSphere1InBody2LocalSpace = transform2.getInverse() * transform1.getPosition();
|
||||||
vec3 intersectionOnBody1 = sphereShape1->getRadius() * centerSphere2InBody1LocalSpace.safeNormalized();
|
Vector3f intersectionOnBody1 = sphereShape1.getRadius() * centerSphere2InBody1LocalSpace.safeNormalized();
|
||||||
vec3 intersectionOnBody2 = sphereShape2->getRadius() * centerSphere1InBody2LocalSpace.safeNormalized();
|
Vector3f intersectionOnBody2 = sphereShape2.getRadius() * centerSphere1InBody2LocalSpace.safeNormalized();
|
||||||
float penetrationDepth = sumRadius - etk::sqrt(squaredDistanceBetweenCenters);
|
float penetrationDepth = sumRadius - sqrt(squaredDistanceBetweenCenters);
|
||||||
|
|
||||||
// Create the contact info object
|
// Create the contact info object
|
||||||
ephysics::ContactPointInfo contactInfo(shape1Info.proxyShape,
|
ephysics::ContactPointInfo contactInfo(shape1Info.proxyShape,
|
||||||
@ -46,6 +46,6 @@ void ephysics::SphereVsSphereAlgorithm::testCollision( ephysics::CollisionShapeI
|
|||||||
intersectionOnBody1,
|
intersectionOnBody1,
|
||||||
intersectionOnBody2);
|
intersectionOnBody2);
|
||||||
// Notify about the new contact
|
// Notify about the new contact
|
||||||
narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo);
|
narrowPhaseCallback.notifyContact(shape1Info.overlappingPair, contactInfo);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -1,42 +0,0 @@
|
|||||||
/** @file
|
|
||||||
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
|
|
||||||
* @author Daniel CHAPPUIS
|
|
||||||
* @author Edouard DUPIN
|
|
||||||
* @copyright 2010-2016, Daniel Chappuis
|
|
||||||
* @copyright 2017, Edouard DUPIN
|
|
||||||
* @license MPL v2.0 (see license file)
|
|
||||||
*/
|
|
||||||
#pragma once
|
|
||||||
|
|
||||||
#include <ephysics/body/Body.hpp>
|
|
||||||
#include <ephysics/raint/ContactPoint.hpp>
|
|
||||||
#include <ephysics/collision/narrowphase/NarrowPhaseAlgorithm.hpp>
|
|
||||||
|
|
||||||
namespace ephysics {
|
|
||||||
/**
|
|
||||||
* @brief It is used to compute the narrow-phase collision detection
|
|
||||||
* between two sphere collision shapes.
|
|
||||||
*/
|
|
||||||
class SphereVsSphereAlgorithm : public NarrowPhaseAlgorithm {
|
|
||||||
protected :
|
|
||||||
SphereVsSphereAlgorithm( SphereVsSphereAlgorithm) = delete;
|
|
||||||
SphereVsSphereAlgorithm operator=( SphereVsSphereAlgorithm) = delete;
|
|
||||||
public :
|
|
||||||
/**
|
|
||||||
* @brief Constructor
|
|
||||||
*/
|
|
||||||
SphereVsSphereAlgorithm();
|
|
||||||
/**
|
|
||||||
* @brief Destructor
|
|
||||||
*/
|
|
||||||
virtual ~SphereVsSphereAlgorithm() = default;
|
|
||||||
/**
|
|
||||||
* @brief Compute a contact info if the two bounding volume collide
|
|
||||||
*/
|
|
||||||
virtual void testCollision( CollisionShapeInfo shape1Info,
|
|
||||||
CollisionShapeInfo shape2Info,
|
|
||||||
NarrowPhaseCallback* narrowPhaseCallback);
|
|
||||||
};
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
@ -0,0 +1,29 @@
|
|||||||
|
package org.atriaSoft.ephysics.collision.narrowphase;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief It is used to compute the narrow-phase collision detection
|
||||||
|
* between two sphere collision shapes.
|
||||||
|
*/
|
||||||
|
class SphereVsSphereAlgorithm extends NarrowPhaseAlgorithm {
|
||||||
|
protected :
|
||||||
|
SphereVsSphereAlgorithm( SphereVsSphereAlgorithm) = delete;
|
||||||
|
SphereVsSphereAlgorithm operator=( SphereVsSphereAlgorithm) = delete;
|
||||||
|
public :
|
||||||
|
/**
|
||||||
|
* @brief Constructor
|
||||||
|
*/
|
||||||
|
SphereVsSphereAlgorithm();
|
||||||
|
/**
|
||||||
|
* @brief Destructor
|
||||||
|
*/
|
||||||
|
~SphereVsSphereAlgorithm() = default;
|
||||||
|
/**
|
||||||
|
* @brief Compute a contact info if the two bounding volume collide
|
||||||
|
*/
|
||||||
|
void testCollision( CollisionShapeInfo shape1Info,
|
||||||
|
CollisionShapeInfo shape2Info,
|
||||||
|
NarrowPhaseCallback* narrowPhaseCallback);
|
||||||
|
};
|
||||||
|
}
|
||||||
|
|
||||||
|
|
@ -20,50 +20,50 @@ AABB::AABB():
|
|||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
AABB::AABB( vec3 minCoordinates, vec3 maxCoordinates):
|
AABB::AABB( Vector3f minCoordinates, Vector3f maxCoordinates):
|
||||||
this.minCoordinates(minCoordinates),
|
this.minCoordinates(minCoordinates),
|
||||||
this.maxCoordinates(maxCoordinates) {
|
this.maxCoordinates(maxCoordinates) {
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
AABB::AABB( AABB aabb):
|
AABB::AABB( AABB aabb):
|
||||||
this.minCoordinates(aabb.this.minCoordinates),
|
this.minCoordinates(aabb.minCoordinates),
|
||||||
this.maxCoordinates(aabb.this.maxCoordinates) {
|
this.maxCoordinates(aabb.maxCoordinates) {
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void AABB::mergeWithAABB( AABB aabb) {
|
void AABB::mergeWithAABB( AABB aabb) {
|
||||||
this.minCoordinates.setX(etk::min(this.minCoordinates.x(), aabb.this.minCoordinates.x()));
|
this.minCoordinates.setX(min(this.minCoordinates.x(), aabb.minCoordinates.x()));
|
||||||
this.minCoordinates.setY(etk::min(this.minCoordinates.y(), aabb.this.minCoordinates.y()));
|
this.minCoordinates.setY(min(this.minCoordinates.y(), aabb.minCoordinates.y()));
|
||||||
this.minCoordinates.setZ(etk::min(this.minCoordinates.z(), aabb.this.minCoordinates.z()));
|
this.minCoordinates.setZ(min(this.minCoordinates.z(), aabb.minCoordinates.z()));
|
||||||
this.maxCoordinates.setX(etk::max(this.maxCoordinates.x(), aabb.this.maxCoordinates.x()));
|
this.maxCoordinates.setX(max(this.maxCoordinates.x(), aabb.maxCoordinates.x()));
|
||||||
this.maxCoordinates.setY(etk::max(this.maxCoordinates.y(), aabb.this.maxCoordinates.y()));
|
this.maxCoordinates.setY(max(this.maxCoordinates.y(), aabb.maxCoordinates.y()));
|
||||||
this.maxCoordinates.setZ(etk::max(this.maxCoordinates.z(), aabb.this.maxCoordinates.z()));
|
this.maxCoordinates.setZ(max(this.maxCoordinates.z(), aabb.maxCoordinates.z()));
|
||||||
}
|
}
|
||||||
|
|
||||||
void AABB::mergeTwoAABBs( AABB aabb1, AABB aabb2) {
|
void AABB::mergeTwoAABBs( AABB aabb1, AABB aabb2) {
|
||||||
this.minCoordinates.setX(etk::min(aabb1.this.minCoordinates.x(), aabb2.this.minCoordinates.x()));
|
this.minCoordinates.setX(min(aabb1.minCoordinates.x(), aabb2.minCoordinates.x()));
|
||||||
this.minCoordinates.setY(etk::min(aabb1.this.minCoordinates.y(), aabb2.this.minCoordinates.y()));
|
this.minCoordinates.setY(min(aabb1.minCoordinates.y(), aabb2.minCoordinates.y()));
|
||||||
this.minCoordinates.setZ(etk::min(aabb1.this.minCoordinates.z(), aabb2.this.minCoordinates.z()));
|
this.minCoordinates.setZ(min(aabb1.minCoordinates.z(), aabb2.minCoordinates.z()));
|
||||||
this.maxCoordinates.setX(etk::max(aabb1.this.maxCoordinates.x(), aabb2.this.maxCoordinates.x()));
|
this.maxCoordinates.setX(max(aabb1.maxCoordinates.x(), aabb2.maxCoordinates.x()));
|
||||||
this.maxCoordinates.setY(etk::max(aabb1.this.maxCoordinates.y(), aabb2.this.maxCoordinates.y()));
|
this.maxCoordinates.setY(max(aabb1.maxCoordinates.y(), aabb2.maxCoordinates.y()));
|
||||||
this.maxCoordinates.setZ(etk::max(aabb1.this.maxCoordinates.z(), aabb2.this.maxCoordinates.z()));
|
this.maxCoordinates.setZ(max(aabb1.maxCoordinates.z(), aabb2.maxCoordinates.z()));
|
||||||
}
|
}
|
||||||
|
|
||||||
boolean AABB::contains( AABB aabb) {
|
boolean AABB::contains( AABB aabb) {
|
||||||
boolean isInside = true;
|
boolean isInside = true;
|
||||||
isInside = isInside hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj this.minCoordinates.x() <= aabb.this.minCoordinates.x();
|
isInside = isInside && this.minCoordinates.x() <= aabb.minCoordinates.x();
|
||||||
isInside = isInside hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj this.minCoordinates.y() <= aabb.this.minCoordinates.y();
|
isInside = isInside && this.minCoordinates.y() <= aabb.minCoordinates.y();
|
||||||
isInside = isInside hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj this.minCoordinates.z() <= aabb.this.minCoordinates.z();
|
isInside = isInside && this.minCoordinates.z() <= aabb.minCoordinates.z();
|
||||||
isInside = isInside hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj this.maxCoordinates.x() >= aabb.this.maxCoordinates.x();
|
isInside = isInside && this.maxCoordinates.x() >= aabb.maxCoordinates.x();
|
||||||
isInside = isInside hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj this.maxCoordinates.y() >= aabb.this.maxCoordinates.y();
|
isInside = isInside && this.maxCoordinates.y() >= aabb.maxCoordinates.y();
|
||||||
isInside = isInside hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj this.maxCoordinates.z() >= aabb.this.maxCoordinates.z();
|
isInside = isInside && this.maxCoordinates.z() >= aabb.maxCoordinates.z();
|
||||||
return isInside;
|
return isInside;
|
||||||
}
|
}
|
||||||
|
|
||||||
AABB AABB::createAABBForTriangle( vec3* trianglePoints) {
|
AABB AABB::createAABBForTriangle( Vector3f* trianglePoints) {
|
||||||
vec3 minCoords(trianglePoints[0].x(), trianglePoints[0].y(), trianglePoints[0].z());
|
Vector3f minCoords(trianglePoints[0].x(), trianglePoints[0].y(), trianglePoints[0].z());
|
||||||
vec3 maxCoords(trianglePoints[0].x(), trianglePoints[0].y(), trianglePoints[0].z());
|
Vector3f maxCoords(trianglePoints[0].x(), trianglePoints[0].y(), trianglePoints[0].z());
|
||||||
if (trianglePoints[1].x() < minCoords.x()) {
|
if (trianglePoints[1].x() < minCoords.x()) {
|
||||||
minCoords.setX(trianglePoints[1].x());
|
minCoords.setX(trianglePoints[1].x());
|
||||||
}
|
}
|
||||||
@ -104,21 +104,21 @@ AABB AABB::createAABBForTriangle( vec3* trianglePoints) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
boolean AABB::testRayIntersect( Ray ray) {
|
boolean AABB::testRayIntersect( Ray ray) {
|
||||||
vec3 point2 = ray.point1 + ray.maxFraction * (ray.point2 - ray.point1);
|
Vector3f point2 = ray.point1 + ray.maxFraction * (ray.point2 - ray.point1);
|
||||||
vec3 e = this.maxCoordinates - this.minCoordinates;
|
Vector3f e = this.maxCoordinates - this.minCoordinates;
|
||||||
vec3 d = point2 - ray.point1;
|
Vector3f d = point2 - ray.point1;
|
||||||
vec3 m = ray.point1 + point2 - this.minCoordinates - this.maxCoordinates;
|
Vector3f m = ray.point1 + point2 - this.minCoordinates - this.maxCoordinates;
|
||||||
// Test if the AABB face normals are separating axis
|
// Test if the AABB face normals are separating axis
|
||||||
float adx = etk::abs(d.x());
|
float adx = abs(d.x());
|
||||||
if (etk::abs(m.x()) > e.x() + adx) {
|
if (abs(m.x()) > e.x() + adx) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
float ady = etk::abs(d.y());
|
float ady = abs(d.y());
|
||||||
if (etk::abs(m.y()) > e.y() + ady) {
|
if (abs(m.y()) > e.y() + ady) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
float adz = etk::abs(d.z());
|
float adz = abs(d.z());
|
||||||
if (etk::abs(m.z()) > e.z() + adz) {
|
if (abs(m.z()) > e.z() + adz) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
// Add in an epsilon term to counteract arithmetic errors when segment is
|
// Add in an epsilon term to counteract arithmetic errors when segment is
|
||||||
@ -129,50 +129,50 @@ boolean AABB::testRayIntersect( Ray ray) {
|
|||||||
adz += epsilon;
|
adz += epsilon;
|
||||||
// Test if the cross products between face normals and ray direction are
|
// Test if the cross products between face normals and ray direction are
|
||||||
// separating axis
|
// separating axis
|
||||||
if (etk::abs(m.y() * d.z() - m.z() * d.y()) > e.y() * adz + e.z() * ady) {
|
if (abs(m.y() * d.z() - m.z() * d.y()) > e.y() * adz + e.z() * ady) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
if (etk::abs(m.z() * d.x() - m.x() * d.z()) > e.x() * adz + e.z() * adx) {
|
if (abs(m.z() * d.x() - m.x() * d.z()) > e.x() * adz + e.z() * adx) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
if (etk::abs(m.x() * d.y() - m.y() * d.x()) > e.x() * ady + e.y() * adx) {
|
if (abs(m.x() * d.y() - m.y() * d.x()) > e.x() * ady + e.y() * adx) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
// No separating axis has been found
|
// No separating axis has been found
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
vec3 AABB::getExtent() {
|
Vector3f AABB::getExtent() {
|
||||||
return this.maxCoordinates - this.minCoordinates;
|
return this.maxCoordinates - this.minCoordinates;
|
||||||
}
|
}
|
||||||
|
|
||||||
void AABB::inflate(float dx, float dy, float dz) {
|
void AABB::inflate(float dx, float dy, float dz) {
|
||||||
this.maxCoordinates += vec3(dx, dy, dz);
|
this.maxCoordinates += Vector3f(dx, dy, dz);
|
||||||
this.minCoordinates -= vec3(dx, dy, dz);
|
this.minCoordinates -= Vector3f(dx, dy, dz);
|
||||||
}
|
}
|
||||||
|
|
||||||
boolean AABB::testCollision( AABB aabb) {
|
boolean AABB::testCollision( AABB aabb) {
|
||||||
if ( this.maxCoordinates.x() < aabb.this.minCoordinates.x()
|
if ( this.maxCoordinates.x() < aabb.minCoordinates.x()
|
||||||
|| aabb.this.maxCoordinates.x() < this.minCoordinates.x()) {
|
|| aabb.maxCoordinates.x() < this.minCoordinates.x()) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
if ( this.maxCoordinates.y() < aabb.this.minCoordinates.y()
|
if ( this.maxCoordinates.y() < aabb.minCoordinates.y()
|
||||||
|| aabb.this.maxCoordinates.y() < this.minCoordinates.y()) {
|
|| aabb.maxCoordinates.y() < this.minCoordinates.y()) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
if ( this.maxCoordinates.z() < aabb.this.minCoordinates.z()
|
if ( this.maxCoordinates.z() < aabb.minCoordinates.z()
|
||||||
|| aabb.this.maxCoordinates.z() < this.minCoordinates.z()) {
|
|| aabb.maxCoordinates.z() < this.minCoordinates.z()) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
float AABB::getVolume() {
|
float AABB::getVolume() {
|
||||||
vec3 diff = this.maxCoordinates - this.minCoordinates;
|
Vector3f diff = this.maxCoordinates - this.minCoordinates;
|
||||||
return (diff.x() * diff.y() * diff.z());
|
return (diff.x() * diff.y() * diff.z());
|
||||||
}
|
}
|
||||||
|
|
||||||
boolean AABB::testCollisionTriangleAABB( vec3* trianglePoints) {
|
boolean AABB::testCollisionTriangleAABB( Vector3f* trianglePoints) {
|
||||||
if (min3(trianglePoints[0].x(), trianglePoints[1].x(), trianglePoints[2].x()) > this.maxCoordinates.x()) {
|
if (min3(trianglePoints[0].x(), trianglePoints[1].x(), trianglePoints[2].x()) > this.maxCoordinates.x()) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
@ -194,16 +194,16 @@ boolean AABB::testCollisionTriangleAABB( vec3* trianglePoints) {
|
|||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
boolean AABB::contains( vec3 point) {
|
boolean AABB::contains( Vector3f point) {
|
||||||
return point.x() >= this.minCoordinates.x() - FLTEPSILON hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj point.x() <= this.maxCoordinates.x() + FLTEPSILON
|
return point.x() >= this.minCoordinates.x() - FLTEPSILON && point.x() <= this.maxCoordinates.x() + FLTEPSILON
|
||||||
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj point.y() >= this.minCoordinates.y() - FLTEPSILON hjkhjkhjkhkj point.y() <= this.maxCoordinates.y() + FLTEPSILON
|
&& point.y() >= this.minCoordinates.y() - FLTEPSILON hjkhjkhjkhkj point.y() <= this.maxCoordinates.y() + FLTEPSILON
|
||||||
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj point.z() >= this.minCoordinates.z() - FLTEPSILON hjkhjkhjkhkj point.z() <= this.maxCoordinates.z() + FLTEPSILON;
|
&& point.z() >= this.minCoordinates.z() - FLTEPSILON hjkhjkhjkhkj point.z() <= this.maxCoordinates.z() + FLTEPSILON;
|
||||||
}
|
}
|
||||||
|
|
||||||
AABB AABB::operator=( AABB aabb) {
|
AABB AABB::operator=( AABB aabb) {
|
||||||
if (this != aabb) {
|
if (this != aabb) {
|
||||||
this.minCoordinates = aabb.this.minCoordinates;
|
this.minCoordinates = aabb.minCoordinates;
|
||||||
this.maxCoordinates = aabb.this.maxCoordinates;
|
this.maxCoordinates = aabb.maxCoordinates;
|
||||||
}
|
}
|
||||||
return *this;
|
return *this;
|
||||||
}
|
}
|
||||||
|
@ -1,14 +1,5 @@
|
|||||||
/** @file
|
package org.atriaSoft.ephysics.collision.shapes;
|
||||||
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
|
|
||||||
* @author Daniel CHAPPUIS
|
|
||||||
* @author Edouard DUPIN
|
|
||||||
* @copyright 2010-2016, Daniel Chappuis
|
|
||||||
* @copyright 2017, Edouard DUPIN
|
|
||||||
* @license MPL v2.0 (see license file)
|
|
||||||
*/
|
|
||||||
#pragma once
|
|
||||||
#include <ephysics/mathematics/mathematics.hpp>
|
|
||||||
namespace ephysics {
|
|
||||||
/**
|
/**
|
||||||
* @brief Represents a bounding volume of type "Axis Aligned
|
* @brief Represents a bounding volume of type "Axis Aligned
|
||||||
* Bounding Box". It's a box where all the edges are always aligned
|
* Bounding Box". It's a box where all the edges are always aligned
|
||||||
@ -18,9 +9,9 @@ namespace ephysics {
|
|||||||
class AABB {
|
class AABB {
|
||||||
private :
|
private :
|
||||||
/// Minimum world coordinates of the AABB on the x,y and z axis
|
/// Minimum world coordinates of the AABB on the x,y and z axis
|
||||||
vec3 this.minCoordinates;
|
Vector3f minCoordinates;
|
||||||
/// Maximum world coordinates of the AABB on the x,y and z axis
|
/// Maximum world coordinates of the AABB on the x,y and z axis
|
||||||
vec3 this.maxCoordinates;
|
Vector3f maxCoordinates;
|
||||||
public :
|
public :
|
||||||
/**
|
/**
|
||||||
* @brief default contructor
|
* @brief default contructor
|
||||||
@ -31,7 +22,7 @@ namespace ephysics {
|
|||||||
* @param[in] minCoordinates Minimum coordinates
|
* @param[in] minCoordinates Minimum coordinates
|
||||||
* @param[in] maxCoordinates Maximum coordinates
|
* @param[in] maxCoordinates Maximum coordinates
|
||||||
*/
|
*/
|
||||||
AABB( vec3 minCoordinates, vec3 maxCoordinates);
|
AABB( Vector3f minCoordinates, Vector3f maxCoordinates);
|
||||||
/**
|
/**
|
||||||
* @brief Copy-contructor
|
* @brief Copy-contructor
|
||||||
* @param[in] aabb the object to copy
|
* @param[in] aabb the object to copy
|
||||||
@ -41,42 +32,42 @@ namespace ephysics {
|
|||||||
* @brief Get the center point of the AABB box
|
* @brief Get the center point of the AABB box
|
||||||
* @return The 3D position of the center
|
* @return The 3D position of the center
|
||||||
*/
|
*/
|
||||||
vec3 getCenter() {
|
Vector3f getCenter() {
|
||||||
return (this.minCoordinates + this.maxCoordinates) * 0.5f;
|
return (this.minCoordinates + this.maxCoordinates) * 0.5f;
|
||||||
}
|
}
|
||||||
/**
|
/**
|
||||||
* @brief Get the minimum coordinates of the AABB
|
* @brief Get the minimum coordinates of the AABB
|
||||||
* @return The 3d minimum coordonates
|
* @return The 3d minimum coordonates
|
||||||
*/
|
*/
|
||||||
vec3 getMin() {
|
Vector3f getMin() {
|
||||||
return this.minCoordinates;
|
return this.minCoordinates;
|
||||||
}
|
}
|
||||||
/**
|
/**
|
||||||
* @brief Set the minimum coordinates of the AABB
|
* @brief Set the minimum coordinates of the AABB
|
||||||
* @param[in] min The 3d minimum coordonates
|
* @param[in] min The 3d minimum coordonates
|
||||||
*/
|
*/
|
||||||
void setMin( vec3 min) {
|
void setMin( Vector3f min) {
|
||||||
this.minCoordinates = min;
|
this.minCoordinates = min;
|
||||||
}
|
}
|
||||||
/**
|
/**
|
||||||
* @brief Return the maximum coordinates of the AABB
|
* @brief Return the maximum coordinates of the AABB
|
||||||
* @return The 3d maximum coordonates
|
* @return The 3d maximum coordonates
|
||||||
*/
|
*/
|
||||||
vec3 getMax() {
|
Vector3f getMax() {
|
||||||
return this.maxCoordinates;
|
return this.maxCoordinates;
|
||||||
}
|
}
|
||||||
/**
|
/**
|
||||||
* @brief Set the maximum coordinates of the AABB
|
* @brief Set the maximum coordinates of the AABB
|
||||||
* @param[in] max The 3d maximum coordonates
|
* @param[in] max The 3d maximum coordonates
|
||||||
*/
|
*/
|
||||||
void setMax( vec3 max) {
|
void setMax( Vector3f max) {
|
||||||
this.maxCoordinates = max;
|
this.maxCoordinates = max;
|
||||||
}
|
}
|
||||||
/**
|
/**
|
||||||
* @brief Get the size of the AABB in the three dimension x, y and z
|
* @brief Get the size of the AABB in the three dimension x, y and z
|
||||||
* @return the AABB 3D size
|
* @return the AABB 3D size
|
||||||
*/
|
*/
|
||||||
vec3 getExtent() ;
|
Vector3f getExtent() ;
|
||||||
/**
|
/**
|
||||||
* @brief Inflate each side of the AABB by a given size
|
* @brief Inflate each side of the AABB by a given size
|
||||||
* @param[in] dx Inflate X size
|
* @param[in] dx Inflate X size
|
||||||
@ -119,13 +110,13 @@ namespace ephysics {
|
|||||||
* @param[in] point Point to check.
|
* @param[in] point Point to check.
|
||||||
* @return true The point in contained inside
|
* @return true The point in contained inside
|
||||||
*/
|
*/
|
||||||
boolean contains( vec3 point) ;
|
boolean contains( Vector3f point) ;
|
||||||
/**
|
/**
|
||||||
* @brief check if the AABB of a triangle intersects the AABB
|
* @brief check if the AABB of a triangle intersects the AABB
|
||||||
* @param[in] trianglePoints List of 3 point od a triangle
|
* @param[in] trianglePoints List of 3 point od a triangle
|
||||||
* @return true The triangle is contained in the Box
|
* @return true The triangle is contained in the Box
|
||||||
*/
|
*/
|
||||||
boolean testCollisionTriangleAABB( vec3* trianglePoints) ;
|
boolean testCollisionTriangleAABB( Vector3f* trianglePoints) ;
|
||||||
/**
|
/**
|
||||||
* @brief check if the ray intersects the AABB
|
* @brief check if the ray intersects the AABB
|
||||||
* This method use the line vs AABB raycasting technique described in
|
* This method use the line vs AABB raycasting technique described in
|
||||||
@ -139,15 +130,14 @@ namespace ephysics {
|
|||||||
* @param[in] trianglePoints List of 3 point od a triangle
|
* @param[in] trianglePoints List of 3 point od a triangle
|
||||||
* @return An AABB box
|
* @return An AABB box
|
||||||
*/
|
*/
|
||||||
static AABB createAABBForTriangle( vec3* trianglePoints);
|
static AABB createAABBForTriangle( Vector3f* trianglePoints);
|
||||||
/**
|
/**
|
||||||
* @brief Assignment operator
|
* @brief Assignment operator
|
||||||
* @param[in] aabb The other box to compare
|
* @param[in] aabb The other box to compare
|
||||||
* @return reference on this
|
* @return reference on this
|
||||||
*/
|
*/
|
||||||
AABB operator=( AABB aabb);
|
AABB operator=( AABB aabb);
|
||||||
friend class DynamicAABBTree;
|
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
}
|
}
|
@ -14,17 +14,17 @@
|
|||||||
|
|
||||||
using namespace ephysics;
|
using namespace ephysics;
|
||||||
|
|
||||||
BoxShape::BoxShape( vec3 extent, float margin):
|
BoxShape::BoxShape( Vector3f extent, float margin):
|
||||||
ConvexShape(BOX, margin),
|
ConvexShape(BOX, margin),
|
||||||
this.extent(extent - vec3(margin, margin, margin)) {
|
this.extent(extent - Vector3f(margin, margin, margin)) {
|
||||||
assert(extent.x() > 0.0f hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj extent.x() > margin);
|
assert(extent.x() > 0.0f && extent.x() > margin);
|
||||||
assert(extent.y() > 0.0f hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj extent.y() > margin);
|
assert(extent.y() > 0.0f && extent.y() > margin);
|
||||||
assert(extent.z() > 0.0f hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj extent.z() > margin);
|
assert(extent.z() > 0.0f && extent.z() > margin);
|
||||||
}
|
}
|
||||||
|
|
||||||
void BoxShape::computeLocalInertiaTensor(etk::Matrix3x3 tensor, float mass) {
|
void BoxShape::computeLocalInertiaTensor(Matrix3f tensor, float mass) {
|
||||||
float factor = (1.0f / float(3.0)) * mass;
|
float factor = (1.0f / float(3.0)) * mass;
|
||||||
vec3 realExtent = this.extent + vec3(this.margin, this.margin, this.margin);
|
Vector3f realExtent = this.extent + Vector3f(this.margin, this.margin, this.margin);
|
||||||
float xSquare = realExtent.x() * realExtent.x();
|
float xSquare = realExtent.x() * realExtent.x();
|
||||||
float ySquare = realExtent.y() * realExtent.y();
|
float ySquare = realExtent.y() * realExtent.y();
|
||||||
float zSquare = realExtent.z() * realExtent.z();
|
float zSquare = realExtent.z() * realExtent.z();
|
||||||
@ -34,15 +34,15 @@ void BoxShape::computeLocalInertiaTensor(etk::Matrix3x3 tensor, float mass) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
boolean BoxShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape) {
|
boolean BoxShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape) {
|
||||||
vec3 rayDirection = ray.point2 - ray.point1;
|
Vector3f rayDirection = ray.point2 - ray.point1;
|
||||||
float tMin = FLTMIN;
|
float tMin = FLTMIN;
|
||||||
float tMax = FLTMAX;
|
float tMax = FLTMAX;
|
||||||
vec3 normalDirection(0,0,0);
|
Vector3f normalDirection(0,0,0);
|
||||||
vec3 currentNormal(0,0,0);
|
Vector3f currentNormal(0,0,0);
|
||||||
// For each of the three slabs
|
// For each of the three slabs
|
||||||
for (int iii=0; iii<3; ++iii) {
|
for (int iii=0; iii<3; ++iii) {
|
||||||
// If ray is parallel to the slab
|
// If ray is parallel to the slab
|
||||||
if (etk::abs(rayDirection[iii]) < FLTEPSILON) {
|
if (abs(rayDirection[iii]) < FLTEPSILON) {
|
||||||
// If the ray's origin is not inside the slab, there is no hit
|
// If the ray's origin is not inside the slab, there is no hit
|
||||||
if (ray.point1[iii] > this.extent[iii] || ray.point1[iii] < -this.extent[iii]) {
|
if (ray.point1[iii] > this.extent[iii] || ray.point1[iii] < -this.extent[iii]) {
|
||||||
return false;
|
return false;
|
||||||
@ -58,7 +58,7 @@ boolean BoxShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxySh
|
|||||||
// Swap t1 and t2 if need so that t1 is intersection with near plane and
|
// Swap t1 and t2 if need so that t1 is intersection with near plane and
|
||||||
// t2 with far plane
|
// t2 with far plane
|
||||||
if (t1 > t2) {
|
if (t1 > t2) {
|
||||||
etk::swap(t1, t2);
|
swap(t1, t2);
|
||||||
currentNormal = -currentNormal;
|
currentNormal = -currentNormal;
|
||||||
}
|
}
|
||||||
// Compute the intersection of the of slab intersection interval with previous slabs
|
// Compute the intersection of the of slab intersection interval with previous slabs
|
||||||
@ -66,7 +66,7 @@ boolean BoxShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxySh
|
|||||||
tMin = t1;
|
tMin = t1;
|
||||||
normalDirection = currentNormal;
|
normalDirection = currentNormal;
|
||||||
}
|
}
|
||||||
tMax = etk::min(tMax, t2);
|
tMax = min(tMax, t2);
|
||||||
// If tMin is larger than the maximum raycasting fraction, we return no hit
|
// If tMin is larger than the maximum raycasting fraction, we return no hit
|
||||||
if (tMin > ray.maxFraction) {
|
if (tMin > ray.maxFraction) {
|
||||||
return false;
|
return false;
|
||||||
@ -82,12 +82,12 @@ boolean BoxShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxySh
|
|||||||
|| tMin > ray.maxFraction) {
|
|| tMin > ray.maxFraction) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
if (normalDirection == vec3(0,0,0)) {
|
if (normalDirection == Vector3f(0,0,0)) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
// The ray intersects the three slabs, we compute the hit point
|
// The ray intersects the three slabs, we compute the hit point
|
||||||
vec3 localHitPoint = ray.point1 + tMin * rayDirection;
|
Vector3f localHitPoint = ray.point1 + tMin * rayDirection;
|
||||||
raycastInfo.body = proxyShape->getBody();
|
raycastInfo.body = proxyShape.getBody();
|
||||||
raycastInfo.proxyShape = proxyShape;
|
raycastInfo.proxyShape = proxyShape;
|
||||||
raycastInfo.hitFraction = tMin;
|
raycastInfo.hitFraction = tMin;
|
||||||
raycastInfo.worldPoint = localHitPoint;
|
raycastInfo.worldPoint = localHitPoint;
|
||||||
@ -95,39 +95,39 @@ boolean BoxShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxySh
|
|||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
vec3 BoxShape::getExtent() {
|
Vector3f BoxShape::getExtent() {
|
||||||
return this.extent + vec3(this.margin, this.margin, this.margin);
|
return this.extent + Vector3f(this.margin, this.margin, this.margin);
|
||||||
}
|
}
|
||||||
|
|
||||||
void BoxShape::setLocalScaling( vec3 scaling) {
|
void BoxShape::setLocalScaling( Vector3f scaling) {
|
||||||
this.extent = (this.extent / this.scaling) * scaling;
|
this.extent = (this.extent / this.scaling) * scaling;
|
||||||
CollisionShape::setLocalScaling(scaling);
|
CollisionShape::setLocalScaling(scaling);
|
||||||
}
|
}
|
||||||
|
|
||||||
void BoxShape::getLocalBounds(vec3 min, vec3 max) {
|
void BoxShape::getLocalBounds(Vector3f min, Vector3f max) {
|
||||||
// Maximum bounds
|
// Maximum bounds
|
||||||
max = this.extent + vec3(this.margin, this.margin, this.margin);
|
max = this.extent + Vector3f(this.margin, this.margin, this.margin);
|
||||||
// Minimum bounds
|
// Minimum bounds
|
||||||
min = -max;
|
min = -max;
|
||||||
}
|
}
|
||||||
|
|
||||||
sizet BoxShape::getSizeInBytes() {
|
long BoxShape::getSizeInBytes() {
|
||||||
return sizeof(BoxShape);
|
return sizeof(BoxShape);
|
||||||
}
|
}
|
||||||
|
|
||||||
vec3 BoxShape::getLocalSupportPointWithoutMargin( vec3 direction,
|
Vector3f BoxShape::getLocalSupportPointWithoutMargin( Vector3f direction,
|
||||||
void** cachedCollisionData) {
|
void** cachedCollisionData) {
|
||||||
return vec3(direction.x() < 0.0 ? -this.extent.x() : this.extent.x(),
|
return Vector3f(direction.x() < 0.0 ? -this.extent.x() : this.extent.x(),
|
||||||
direction.y() < 0.0 ? -this.extent.y() : this.extent.y(),
|
direction.y() < 0.0 ? -this.extent.y() : this.extent.y(),
|
||||||
direction.z() < 0.0 ? -this.extent.z() : this.extent.z());
|
direction.z() < 0.0 ? -this.extent.z() : this.extent.z());
|
||||||
}
|
}
|
||||||
|
|
||||||
boolean BoxShape::testPointInside( vec3 localPoint, ProxyShape* proxyShape) {
|
boolean BoxShape::testPointInside( Vector3f localPoint, ProxyShape* proxyShape) {
|
||||||
return ( localPoint.x() < this.extent[0]
|
return ( localPoint.x() < this.extent[0]
|
||||||
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj localPoint.x() > -this.extent[0]
|
&& localPoint.x() > -this.extent[0]
|
||||||
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj localPoint.y() < this.extent[1]
|
&& localPoint.y() < this.extent[1]
|
||||||
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj localPoint.y() > -this.extent[1]
|
&& localPoint.y() > -this.extent[1]
|
||||||
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj localPoint.z() < this.extent[2]
|
&& localPoint.z() < this.extent[2]
|
||||||
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj localPoint.z() > -this.extent[2]);
|
&& localPoint.z() > -this.extent[2]);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -1,58 +0,0 @@
|
|||||||
/** @file
|
|
||||||
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
|
|
||||||
* @author Daniel CHAPPUIS
|
|
||||||
* @author Edouard DUPIN
|
|
||||||
* @copyright 2010-2016, Daniel Chappuis
|
|
||||||
* @copyright 2017, Edouard DUPIN
|
|
||||||
* @license MPL v2.0 (see license file)
|
|
||||||
*/
|
|
||||||
#pragma once
|
|
||||||
|
|
||||||
#include <ephysics/collision/shapes/ConvexShape.hpp>
|
|
||||||
#include <ephysics/body/CollisionBody.hpp>
|
|
||||||
#include <ephysics/mathematics/mathematics.hpp>
|
|
||||||
|
|
||||||
namespace ephysics {
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief It represents a 3D box shape. Those axis are unit length.
|
|
||||||
* The three extents are half-widths of the box along the three
|
|
||||||
* axis x, y, z local axis. The "transform" of the corresponding
|
|
||||||
* rigid body will give an orientation and a position to the box. This
|
|
||||||
* collision shape uses an extra margin distance around it for collision
|
|
||||||
* detection purpose. The default margin is 4cm (if your units are meters,
|
|
||||||
* which is recommended). In case, you want to simulate small objects
|
|
||||||
* (smaller than the margin distance), you might want to reduce the margin by
|
|
||||||
* specifying your own margin distance using the "margin" parameter in the
|
|
||||||
* ructor of the box shape. Otherwise, it is recommended to use the
|
|
||||||
* default margin distance by not using the "margin" parameter in the ructor.
|
|
||||||
*/
|
|
||||||
class BoxShape : public ConvexShape {
|
|
||||||
public:
|
|
||||||
/**
|
|
||||||
* @brief Constructor
|
|
||||||
* @param extent The vector with the three extents of the box (in meters)
|
|
||||||
* @param margin The collision margin (in meters) around the collision shape
|
|
||||||
*/
|
|
||||||
BoxShape( vec3 extent, float margin = OBJECTMARGIN);
|
|
||||||
/// DELETE copy-ructor
|
|
||||||
BoxShape( BoxShape shape) = delete;
|
|
||||||
/// DELETE assignment operator
|
|
||||||
BoxShape operator=( BoxShape shape) = delete;
|
|
||||||
/**
|
|
||||||
* @brief Return the extents of the box
|
|
||||||
* @return The vector with the three extents of the box shape (in meters)
|
|
||||||
*/
|
|
||||||
vec3 getExtent() ;
|
|
||||||
void setLocalScaling( vec3 scaling) override;
|
|
||||||
void getLocalBounds(vec3 min, vec3 max) override;
|
|
||||||
void computeLocalInertiaTensor(etk::Matrix3x3 tensor, float mass) override;
|
|
||||||
protected:
|
|
||||||
vec3 this.extent; //!< Extent sizes of the box in the x, y and z direction
|
|
||||||
vec3 getLocalSupportPointWithoutMargin( vec3 direction, void** cachedCollisionData) override;
|
|
||||||
boolean testPointInside( vec3 localPoint, ProxyShape* proxyShape) override;
|
|
||||||
boolean raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape) override;
|
|
||||||
sizet getSizeInBytes() override;
|
|
||||||
};
|
|
||||||
|
|
||||||
}
|
|
41
src/org/atriaSoft/ephysics/collision/shapes/BoxShape.java
Normal file
41
src/org/atriaSoft/ephysics/collision/shapes/BoxShape.java
Normal file
@ -0,0 +1,41 @@
|
|||||||
|
package org.atriaSoft.ephysics.collision.shapes;
|
||||||
|
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief It represents a 3D box shape. Those axis are unit length.
|
||||||
|
* The three extents are half-widths of the box along the three
|
||||||
|
* axis x, y, z local axis. The "transform" of the corresponding
|
||||||
|
* rigid body will give an orientation and a position to the box. This
|
||||||
|
* collision shape uses an extra margin distance around it for collision
|
||||||
|
* detection purpose. The default margin is 4cm (if your units are meters,
|
||||||
|
* which is recommended). In case, you want to simulate small objects
|
||||||
|
* (smaller than the margin distance), you might want to reduce the margin by
|
||||||
|
* specifying your own margin distance using the "margin" parameter in the
|
||||||
|
* ructor of the box shape. Otherwise, it is recommended to use the
|
||||||
|
* default margin distance by not using the "margin" parameter in the ructor.
|
||||||
|
*/
|
||||||
|
class BoxShape extends ConvexShape {
|
||||||
|
public:
|
||||||
|
/**
|
||||||
|
* @brief Constructor
|
||||||
|
* @param extent The vector with the three extents of the box (in meters)
|
||||||
|
* @param margin The collision margin (in meters) around the collision shape
|
||||||
|
*/
|
||||||
|
BoxShape( Vector3f extent, float margin = OBJECTMARGIN);
|
||||||
|
/**
|
||||||
|
* @brief Return the extents of the box
|
||||||
|
* @return The vector with the three extents of the box shape (in meters)
|
||||||
|
*/
|
||||||
|
Vector3f getExtent();
|
||||||
|
void setLocalScaling( Vector3f scaling);
|
||||||
|
void getLocalBounds(Vector3f min, Vector3f max);
|
||||||
|
void computeLocalInertiaTensor(Matrix3f tensor, float mass);
|
||||||
|
protected:
|
||||||
|
Vector3f extent; //!< Extent sizes of the box in the x, y and z direction
|
||||||
|
Vector3f getLocalSupportPointWithoutMargin( Vector3f direction, void** cachedCollisionData);
|
||||||
|
boolean testPointInside( Vector3f localPoint, ProxyShape* proxyShape);
|
||||||
|
boolean raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape);
|
||||||
|
long getSizeInBytes();
|
||||||
|
};
|
||||||
|
|
||||||
|
}
|
@ -19,7 +19,7 @@ CapsuleShape::CapsuleShape(float radius, float height):
|
|||||||
assert(height > 0.0f);
|
assert(height > 0.0f);
|
||||||
}
|
}
|
||||||
|
|
||||||
void CapsuleShape::computeLocalInertiaTensor(etk::Matrix3x3 tensor, float mass) {
|
void CapsuleShape::computeLocalInertiaTensor(Matrix3f tensor, float mass) {
|
||||||
// The inertia tensor formula for a capsule can be found in : Game Engine Gems, Volume 1
|
// The inertia tensor formula for a capsule can be found in : Game Engine Gems, Volume 1
|
||||||
float height = this.halfHeight + this.halfHeight;
|
float height = this.halfHeight + this.halfHeight;
|
||||||
float radiusSquare = this.margin * this.margin;
|
float radiusSquare = this.margin * this.margin;
|
||||||
@ -37,39 +37,39 @@ void CapsuleShape::computeLocalInertiaTensor(etk::Matrix3x3 tensor, float mass)
|
|||||||
0.0, 0.0, IxxAndzz);
|
0.0, 0.0, IxxAndzz);
|
||||||
}
|
}
|
||||||
|
|
||||||
boolean CapsuleShape::testPointInside( vec3 localPoint, ProxyShape* proxyShape) {
|
boolean CapsuleShape::testPointInside( Vector3f localPoint, ProxyShape* proxyShape) {
|
||||||
float diffYCenterSphere1 = localPoint.y() - this.halfHeight;
|
float diffYCenterSphere1 = localPoint.y() - this.halfHeight;
|
||||||
float diffYCenterSphere2 = localPoint.y() + this.halfHeight;
|
float diffYCenterSphere2 = localPoint.y() + this.halfHeight;
|
||||||
float xSquare = localPoint.x() * localPoint.x();
|
float xSquare = localPoint.x() * localPoint.x();
|
||||||
float zSquare = localPoint.z() * localPoint.z();
|
float zSquare = localPoint.z() * localPoint.z();
|
||||||
float squareRadius = this.margin * this.margin;
|
float squareRadius = this.margin * this.margin;
|
||||||
// Return true if the point is inside the cylinder or one of the two spheres of the capsule
|
// Return true if the point is inside the cylinder or one of the two spheres of the capsule
|
||||||
return ((xSquare + zSquare) < squareRadius hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj
|
return ((xSquare + zSquare) < squareRadius &&
|
||||||
localPoint.y() < this.halfHeight hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj localPoint.y() > -this.halfHeight) ||
|
localPoint.y() < this.halfHeight && localPoint.y() > -this.halfHeight) ||
|
||||||
(xSquare + zSquare + diffYCenterSphere1 * diffYCenterSphere1) < squareRadius ||
|
(xSquare + zSquare + diffYCenterSphere1 * diffYCenterSphere1) < squareRadius ||
|
||||||
(xSquare + zSquare + diffYCenterSphere2 * diffYCenterSphere2) < squareRadius;
|
(xSquare + zSquare + diffYCenterSphere2 * diffYCenterSphere2) < squareRadius;
|
||||||
}
|
}
|
||||||
|
|
||||||
boolean CapsuleShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape) {
|
boolean CapsuleShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape) {
|
||||||
vec3 n = ray.point2 - ray.point1;
|
Vector3f n = ray.point2 - ray.point1;
|
||||||
float epsilon = float(0.01);
|
float epsilon = float(0.01);
|
||||||
vec3 p(float(0), -this.halfHeight, float(0));
|
Vector3f p(float(0), -this.halfHeight, float(0));
|
||||||
vec3 q(float(0), this.halfHeight, float(0));
|
Vector3f q(float(0), this.halfHeight, float(0));
|
||||||
vec3 d = q - p;
|
Vector3f d = q - p;
|
||||||
vec3 m = ray.point1 - p;
|
Vector3f m = ray.point1 - p;
|
||||||
float t;
|
float t;
|
||||||
float mDotD = m.dot(d);
|
float mDotD = m.dot(d);
|
||||||
float nDotD = n.dot(d);
|
float nDotD = n.dot(d);
|
||||||
float dDotD = d.dot(d);
|
float dDotD = d.dot(d);
|
||||||
// Test if the segment is outside the cylinder
|
// Test if the segment is outside the cylinder
|
||||||
float vec1DotD = (ray.point1 - vec3(0.0f, -this.halfHeight - this.margin, float(0.0))).dot(d);
|
float vec1DotD = (ray.point1 - Vector3f(0.0f, -this.halfHeight - this.margin, float(0.0))).dot(d);
|
||||||
if ( vec1DotD < 0.0f
|
if ( vec1DotD < 0.0f
|
||||||
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj vec1DotD + nDotD < float(0.0)) {
|
&& vec1DotD + nDotD < float(0.0)) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
float ddotDExtraCaps = float(2.0) * this.margin * d.y();
|
float ddotDExtraCaps = float(2.0) * this.margin * d.y();
|
||||||
if ( vec1DotD > dDotD + ddotDExtraCaps
|
if ( vec1DotD > dDotD + ddotDExtraCaps
|
||||||
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj vec1DotD + nDotD > dDotD + ddotDExtraCaps) {
|
&& vec1DotD + nDotD > dDotD + ddotDExtraCaps) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
float nDotN = n.dot(n);
|
float nDotN = n.dot(n);
|
||||||
@ -78,7 +78,7 @@ boolean CapsuleShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* pro
|
|||||||
float k = m.dot(m) - this.margin * this.margin;
|
float k = m.dot(m) - this.margin * this.margin;
|
||||||
float c = dDotD * k - mDotD * mDotD;
|
float c = dDotD * k - mDotD * mDotD;
|
||||||
// If the ray is parallel to the capsule axis
|
// If the ray is parallel to the capsule axis
|
||||||
if (etk::abs(a) < epsilon) {
|
if (abs(a) < epsilon) {
|
||||||
// If the origin is outside the surface of the capusle's cylinder, we return no hit
|
// If the origin is outside the surface of the capusle's cylinder, we return no hit
|
||||||
if (c > 0.0f) {
|
if (c > 0.0f) {
|
||||||
return false;
|
return false;
|
||||||
@ -87,28 +87,28 @@ boolean CapsuleShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* pro
|
|||||||
// If the ray intersects with the "p" endcap of the capsule
|
// If the ray intersects with the "p" endcap of the capsule
|
||||||
if (mDotD < 0.0f) {
|
if (mDotD < 0.0f) {
|
||||||
// Check intersection between the ray and the "p" sphere endcap of the capsule
|
// Check intersection between the ray and the "p" sphere endcap of the capsule
|
||||||
vec3 hitLocalPoint;
|
Vector3f hitLocalPoint;
|
||||||
float hitFraction;
|
float hitFraction;
|
||||||
if (raycastWithSphereEndCap(ray.point1, ray.point2, p, ray.maxFraction, hitLocalPoint, hitFraction)) {
|
if (raycastWithSphereEndCap(ray.point1, ray.point2, p, ray.maxFraction, hitLocalPoint, hitFraction)) {
|
||||||
raycastInfo.body = proxyShape->getBody();
|
raycastInfo.body = proxyShape.getBody();
|
||||||
raycastInfo.proxyShape = proxyShape;
|
raycastInfo.proxyShape = proxyShape;
|
||||||
raycastInfo.hitFraction = hitFraction;
|
raycastInfo.hitFraction = hitFraction;
|
||||||
raycastInfo.worldPoint = hitLocalPoint;
|
raycastInfo.worldPoint = hitLocalPoint;
|
||||||
vec3 normalDirection = hitLocalPoint - p;
|
Vector3f normalDirection = hitLocalPoint - p;
|
||||||
raycastInfo.worldNormal = normalDirection;
|
raycastInfo.worldNormal = normalDirection;
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
return false;
|
return false;
|
||||||
} else if (mDotD > dDotD) { // If the ray intersects with the "q" endcap of the cylinder
|
} else if (mDotD > dDotD) { // If the ray intersects with the "q" endcap of the cylinder
|
||||||
// Check intersection between the ray and the "q" sphere endcap of the capsule
|
// Check intersection between the ray and the "q" sphere endcap of the capsule
|
||||||
vec3 hitLocalPoint;
|
Vector3f hitLocalPoint;
|
||||||
float hitFraction;
|
float hitFraction;
|
||||||
if (raycastWithSphereEndCap(ray.point1, ray.point2, q, ray.maxFraction, hitLocalPoint, hitFraction)) {
|
if (raycastWithSphereEndCap(ray.point1, ray.point2, q, ray.maxFraction, hitLocalPoint, hitFraction)) {
|
||||||
raycastInfo.body = proxyShape->getBody();
|
raycastInfo.body = proxyShape.getBody();
|
||||||
raycastInfo.proxyShape = proxyShape;
|
raycastInfo.proxyShape = proxyShape;
|
||||||
raycastInfo.hitFraction = hitFraction;
|
raycastInfo.hitFraction = hitFraction;
|
||||||
raycastInfo.worldPoint = hitLocalPoint;
|
raycastInfo.worldPoint = hitLocalPoint;
|
||||||
vec3 normalDirection = hitLocalPoint - q;
|
Vector3f normalDirection = hitLocalPoint - q;
|
||||||
raycastInfo.worldNormal = normalDirection;
|
raycastInfo.worldNormal = normalDirection;
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
@ -125,33 +125,33 @@ boolean CapsuleShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* pro
|
|||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
// Compute the smallest root (first intersection along the ray)
|
// Compute the smallest root (first intersection along the ray)
|
||||||
float t0 = t = (-b - etk::sqrt(discriminant)) / a;
|
float t0 = t = (-b - sqrt(discriminant)) / a;
|
||||||
// If the intersection is outside the finite cylinder of the capsule on "p" endcap side
|
// If the intersection is outside the finite cylinder of the capsule on "p" endcap side
|
||||||
float value = mDotD + t * nDotD;
|
float value = mDotD + t * nDotD;
|
||||||
if (value < 0.0f) {
|
if (value < 0.0f) {
|
||||||
// Check intersection between the ray and the "p" sphere endcap of the capsule
|
// Check intersection between the ray and the "p" sphere endcap of the capsule
|
||||||
vec3 hitLocalPoint;
|
Vector3f hitLocalPoint;
|
||||||
float hitFraction;
|
float hitFraction;
|
||||||
if (raycastWithSphereEndCap(ray.point1, ray.point2, p, ray.maxFraction, hitLocalPoint, hitFraction)) {
|
if (raycastWithSphereEndCap(ray.point1, ray.point2, p, ray.maxFraction, hitLocalPoint, hitFraction)) {
|
||||||
raycastInfo.body = proxyShape->getBody();
|
raycastInfo.body = proxyShape.getBody();
|
||||||
raycastInfo.proxyShape = proxyShape;
|
raycastInfo.proxyShape = proxyShape;
|
||||||
raycastInfo.hitFraction = hitFraction;
|
raycastInfo.hitFraction = hitFraction;
|
||||||
raycastInfo.worldPoint = hitLocalPoint;
|
raycastInfo.worldPoint = hitLocalPoint;
|
||||||
vec3 normalDirection = hitLocalPoint - p;
|
Vector3f normalDirection = hitLocalPoint - p;
|
||||||
raycastInfo.worldNormal = normalDirection;
|
raycastInfo.worldNormal = normalDirection;
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
return false;
|
return false;
|
||||||
} else if (value > dDotD) { // If the intersection is outside the finite cylinder on the "q" side
|
} else if (value > dDotD) { // If the intersection is outside the finite cylinder on the "q" side
|
||||||
// Check intersection between the ray and the "q" sphere endcap of the capsule
|
// Check intersection between the ray and the "q" sphere endcap of the capsule
|
||||||
vec3 hitLocalPoint;
|
Vector3f hitLocalPoint;
|
||||||
float hitFraction;
|
float hitFraction;
|
||||||
if (raycastWithSphereEndCap(ray.point1, ray.point2, q, ray.maxFraction, hitLocalPoint, hitFraction)) {
|
if (raycastWithSphereEndCap(ray.point1, ray.point2, q, ray.maxFraction, hitLocalPoint, hitFraction)) {
|
||||||
raycastInfo.body = proxyShape->getBody();
|
raycastInfo.body = proxyShape.getBody();
|
||||||
raycastInfo.proxyShape = proxyShape;
|
raycastInfo.proxyShape = proxyShape;
|
||||||
raycastInfo.hitFraction = hitFraction;
|
raycastInfo.hitFraction = hitFraction;
|
||||||
raycastInfo.worldPoint = hitLocalPoint;
|
raycastInfo.worldPoint = hitLocalPoint;
|
||||||
vec3 normalDirection = hitLocalPoint - q;
|
Vector3f normalDirection = hitLocalPoint - q;
|
||||||
raycastInfo.worldNormal = normalDirection;
|
raycastInfo.worldNormal = normalDirection;
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
@ -164,31 +164,31 @@ boolean CapsuleShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* pro
|
|||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
// Compute the hit information
|
// Compute the hit information
|
||||||
vec3 localHitPoint = ray.point1 + t * n;
|
Vector3f localHitPoint = ray.point1 + t * n;
|
||||||
raycastInfo.body = proxyShape->getBody();
|
raycastInfo.body = proxyShape.getBody();
|
||||||
raycastInfo.proxyShape = proxyShape;
|
raycastInfo.proxyShape = proxyShape;
|
||||||
raycastInfo.hitFraction = t;
|
raycastInfo.hitFraction = t;
|
||||||
raycastInfo.worldPoint = localHitPoint;
|
raycastInfo.worldPoint = localHitPoint;
|
||||||
vec3 v = localHitPoint - p;
|
Vector3f v = localHitPoint - p;
|
||||||
vec3 w = (v.dot(d) / d.length2()) * d;
|
Vector3f w = (v.dot(d) / d.length2()) * d;
|
||||||
vec3 normalDirection = (localHitPoint - (p + w)).safeNormalized();
|
Vector3f normalDirection = (localHitPoint - (p + w)).safeNormalized();
|
||||||
raycastInfo.worldNormal = normalDirection;
|
raycastInfo.worldNormal = normalDirection;
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
boolean CapsuleShape::raycastWithSphereEndCap( vec3 point1,
|
boolean CapsuleShape::raycastWithSphereEndCap( Vector3f point1,
|
||||||
vec3 point2,
|
Vector3f point2,
|
||||||
vec3 sphereCenter,
|
Vector3f sphereCenter,
|
||||||
float maxFraction,
|
float maxFraction,
|
||||||
vec3 hitLocalPoint,
|
Vector3f hitLocalPoint,
|
||||||
float hitFraction) {
|
float hitFraction) {
|
||||||
vec3 m = point1 - sphereCenter;
|
Vector3f m = point1 - sphereCenter;
|
||||||
float c = m.dot(m) - this.margin * this.margin;
|
float c = m.dot(m) - this.margin * this.margin;
|
||||||
// If the origin of the ray is inside the sphere, we return no intersection
|
// If the origin of the ray is inside the sphere, we return no intersection
|
||||||
if (c < 0.0f) {
|
if (c < 0.0f) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
vec3 rayDirection = point2 - point1;
|
Vector3f rayDirection = point2 - point1;
|
||||||
float b = m.dot(rayDirection);
|
float b = m.dot(rayDirection);
|
||||||
// If the origin of the ray is outside the sphere and the ray
|
// If the origin of the ray is outside the sphere and the ray
|
||||||
// is pointing away from the sphere, there is no intersection
|
// is pointing away from the sphere, there is no intersection
|
||||||
@ -204,7 +204,7 @@ boolean CapsuleShape::raycastWithSphereEndCap( vec3 point1,
|
|||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
// Compute the solution "t" closest to the origin
|
// Compute the solution "t" closest to the origin
|
||||||
float t = -b - etk::sqrt(discriminant);
|
float t = -b - sqrt(discriminant);
|
||||||
assert(t >= 0.0f);
|
assert(t >= 0.0f);
|
||||||
// If the hit point is withing the segment ray fraction
|
// If the hit point is withing the segment ray fraction
|
||||||
if (t < maxFraction * raySquareLength) {
|
if (t < maxFraction * raySquareLength) {
|
||||||
@ -225,17 +225,17 @@ float CapsuleShape::getHeight() {
|
|||||||
return this.halfHeight + this.halfHeight;
|
return this.halfHeight + this.halfHeight;
|
||||||
}
|
}
|
||||||
|
|
||||||
void CapsuleShape::setLocalScaling( vec3 scaling) {
|
void CapsuleShape::setLocalScaling( Vector3f scaling) {
|
||||||
this.halfHeight = (this.halfHeight / this.scaling.y()) * scaling.y();
|
this.halfHeight = (this.halfHeight / this.scaling.y()) * scaling.y();
|
||||||
this.margin = (this.margin / this.scaling.x()) * scaling.x();
|
this.margin = (this.margin / this.scaling.x()) * scaling.x();
|
||||||
CollisionShape::setLocalScaling(scaling);
|
CollisionShape::setLocalScaling(scaling);
|
||||||
}
|
}
|
||||||
|
|
||||||
sizet CapsuleShape::getSizeInBytes() {
|
long CapsuleShape::getSizeInBytes() {
|
||||||
return sizeof(CapsuleShape);
|
return sizeof(CapsuleShape);
|
||||||
}
|
}
|
||||||
|
|
||||||
void CapsuleShape::getLocalBounds(vec3 min, vec3 max) {
|
void CapsuleShape::getLocalBounds(Vector3f min, Vector3f max) {
|
||||||
// Maximum bounds
|
// Maximum bounds
|
||||||
max.setX(this.margin);
|
max.setX(this.margin);
|
||||||
max.setY(this.halfHeight + this.margin);
|
max.setY(this.halfHeight + this.margin);
|
||||||
@ -246,7 +246,7 @@ void CapsuleShape::getLocalBounds(vec3 min, vec3 max) {
|
|||||||
min.setZ(min.x());
|
min.setZ(min.x());
|
||||||
}
|
}
|
||||||
|
|
||||||
vec3 CapsuleShape::getLocalSupportPointWithoutMargin( vec3 direction,
|
Vector3f CapsuleShape::getLocalSupportPointWithoutMargin( Vector3f direction,
|
||||||
void** cachedCollisionData) {
|
void** cachedCollisionData) {
|
||||||
// Support point top sphere
|
// Support point top sphere
|
||||||
float dotProductTop = this.halfHeight * direction.y();
|
float dotProductTop = this.halfHeight * direction.y();
|
||||||
@ -254,7 +254,7 @@ vec3 CapsuleShape::getLocalSupportPointWithoutMargin( vec3 direction,
|
|||||||
float dotProductBottom = -this.halfHeight * direction.y();
|
float dotProductBottom = -this.halfHeight * direction.y();
|
||||||
// Return the point with the maximum dot product
|
// Return the point with the maximum dot product
|
||||||
if (dotProductTop > dotProductBottom) {
|
if (dotProductTop > dotProductBottom) {
|
||||||
return vec3(0, this.halfHeight, 0);
|
return Vector3f(0, this.halfHeight, 0);
|
||||||
}
|
}
|
||||||
return vec3(0, -this.halfHeight, 0);
|
return Vector3f(0, -this.halfHeight, 0);
|
||||||
}
|
}
|
||||||
|
@ -1,18 +1,5 @@
|
|||||||
/** @file
|
package org.atriaSoft.ephysics.collision.shapes;
|
||||||
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
|
|
||||||
* @author Daniel CHAPPUIS
|
|
||||||
* @author Edouard DUPIN
|
|
||||||
* @copyright 2010-2016, Daniel Chappuis
|
|
||||||
* @copyright 2017, Edouard DUPIN
|
|
||||||
* @license MPL v2.0 (see license file)
|
|
||||||
*/
|
|
||||||
#pragma once
|
|
||||||
|
|
||||||
#include <ephysics/collision/shapes/ConvexShape.hpp>
|
|
||||||
#include <ephysics/body/CollisionBody.hpp>
|
|
||||||
#include <ephysics/mathematics/mathematics.hpp>
|
|
||||||
|
|
||||||
namespace ephysics {
|
|
||||||
/**
|
/**
|
||||||
* @brief It represents a capsule collision shape that is defined around the Y axis.
|
* @brief It represents a capsule collision shape that is defined around the Y axis.
|
||||||
* A capsule shape can be seen as the convex hull of two spheres.
|
* A capsule shape can be seen as the convex hull of two spheres.
|
||||||
@ -22,7 +9,7 @@ namespace ephysics {
|
|||||||
* and height of the shape. Therefore, no need to specify an object margin for a
|
* and height of the shape. Therefore, no need to specify an object margin for a
|
||||||
* capsule shape.
|
* capsule shape.
|
||||||
*/
|
*/
|
||||||
class CapsuleShape : public ConvexShape {
|
class CapsuleShape extends ConvexShape {
|
||||||
public :
|
public :
|
||||||
/**
|
/**
|
||||||
* @brief Constructor
|
* @brief Constructor
|
||||||
@ -44,23 +31,23 @@ namespace ephysics {
|
|||||||
* @return The height of the capsule shape (in meters)
|
* @return The height of the capsule shape (in meters)
|
||||||
*/
|
*/
|
||||||
float getHeight() ;
|
float getHeight() ;
|
||||||
void setLocalScaling( vec3 scaling) override;
|
void setLocalScaling( Vector3f scaling) ;
|
||||||
void getLocalBounds(vec3 min, vec3 max) override;
|
void getLocalBounds(Vector3f min, Vector3f max) ;
|
||||||
void computeLocalInertiaTensor(etk::Matrix3x3 tensor, float mass) override;
|
void computeLocalInertiaTensor(Matrix3f tensor, float mass) ;
|
||||||
protected:
|
protected:
|
||||||
float this.halfHeight; //!< Half height of the capsule (height = distance between the centers of the two spheres)
|
float halfHeight; //!< Half height of the capsule (height = distance between the centers of the two spheres)
|
||||||
vec3 getLocalSupportPointWithoutMargin( vec3 direction, void** cachedCollisionData) override;
|
Vector3f getLocalSupportPointWithoutMargin( Vector3f direction, void** cachedCollisionData) ;
|
||||||
boolean testPointInside( vec3 localPoint, ProxyShape* proxyShape) override;
|
boolean testPointInside( Vector3f localPoint, ProxyShape* proxyShape) ;
|
||||||
boolean raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape) override;
|
boolean raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape) ;
|
||||||
/**
|
/**
|
||||||
* @brief Raycasting method between a ray one of the two spheres end cap of the capsule
|
* @brief Raycasting method between a ray one of the two spheres end cap of the capsule
|
||||||
*/
|
*/
|
||||||
boolean raycastWithSphereEndCap( vec3 point1,
|
boolean raycastWithSphereEndCap( Vector3f point1,
|
||||||
vec3 point2,
|
Vector3f point2,
|
||||||
vec3 sphereCenter,
|
Vector3f sphereCenter,
|
||||||
float maxFraction,
|
float maxFraction,
|
||||||
vec3 hitLocalPoint,
|
Vector3f hitLocalPoint,
|
||||||
float hitFraction) ;
|
float hitFraction) ;
|
||||||
sizet getSizeInBytes() override;
|
long getSizeInBytes() ;
|
||||||
};
|
};
|
||||||
}
|
}
|
@ -19,23 +19,23 @@ CollisionShape::CollisionShape(CollisionShapeType type) :
|
|||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void CollisionShape::computeAABB(AABB aabb, etk::Transform3D transform) {
|
void CollisionShape::computeAABB(AABB aabb, Transform3D transform) {
|
||||||
PROFILE("CollisionShape::computeAABB()");
|
PROFILE("CollisionShape::computeAABB()");
|
||||||
// Get the local bounds in x,y and z direction
|
// Get the local bounds in x,y and z direction
|
||||||
vec3 minBounds(0,0,0);
|
Vector3f minBounds(0,0,0);
|
||||||
vec3 maxBounds(0,0,0);
|
Vector3f maxBounds(0,0,0);
|
||||||
getLocalBounds(minBounds, maxBounds);
|
getLocalBounds(minBounds, maxBounds);
|
||||||
// Rotate the local bounds according to the orientation of the body
|
// Rotate the local bounds according to the orientation of the body
|
||||||
etk::Matrix3x3 worldAxis = transform.getOrientation().getMatrix().getAbsolute();
|
Matrix3f worldAxis = transform.getOrientation().getMatrix().getAbsolute();
|
||||||
vec3 worldMinBounds(worldAxis.getColumn(0).dot(minBounds),
|
Vector3f worldMinBounds(worldAxis.getColumn(0).dot(minBounds),
|
||||||
worldAxis.getColumn(1).dot(minBounds),
|
worldAxis.getColumn(1).dot(minBounds),
|
||||||
worldAxis.getColumn(2).dot(minBounds));
|
worldAxis.getColumn(2).dot(minBounds));
|
||||||
vec3 worldMaxBounds(worldAxis.getColumn(0).dot(maxBounds),
|
Vector3f worldMaxBounds(worldAxis.getColumn(0).dot(maxBounds),
|
||||||
worldAxis.getColumn(1).dot(maxBounds),
|
worldAxis.getColumn(1).dot(maxBounds),
|
||||||
worldAxis.getColumn(2).dot(maxBounds));
|
worldAxis.getColumn(2).dot(maxBounds));
|
||||||
// Compute the minimum and maximum coordinates of the rotated extents
|
// Compute the minimum and maximum coordinates of the rotated extents
|
||||||
vec3 minCoordinates = transform.getPosition() + worldMinBounds;
|
Vector3f minCoordinates = transform.getPosition() + worldMinBounds;
|
||||||
vec3 maxCoordinates = transform.getPosition() + worldMaxBounds;
|
Vector3f maxCoordinates = transform.getPosition() + worldMaxBounds;
|
||||||
// Update the AABB with the new minimum and maximum coordinates
|
// Update the AABB with the new minimum and maximum coordinates
|
||||||
aabb.setMin(minCoordinates);
|
aabb.setMin(minCoordinates);
|
||||||
aabb.setMax(maxCoordinates);
|
aabb.setMax(maxCoordinates);
|
||||||
@ -44,7 +44,7 @@ void CollisionShape::computeAABB(AABB aabb, etk::Transform3D transform) {
|
|||||||
int CollisionShape::computeNbMaxContactManifolds(CollisionShapeType shapeType1,
|
int CollisionShape::computeNbMaxContactManifolds(CollisionShapeType shapeType1,
|
||||||
CollisionShapeType shapeType2) {
|
CollisionShapeType shapeType2) {
|
||||||
// If both shapes are convex
|
// If both shapes are convex
|
||||||
if (isConvex(shapeType1) hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj isConvex(shapeType2)) {
|
if (isConvex(shapeType1) && isConvex(shapeType2)) {
|
||||||
return NBMAXCONTACTMANIFOLDSCONVEXSHAPE;
|
return NBMAXCONTACTMANIFOLDSCONVEXSHAPE;
|
||||||
}
|
}
|
||||||
// If there is at least one concave shape
|
// If there is at least one concave shape
|
||||||
|
@ -1,21 +1,5 @@
|
|||||||
/** @file
|
package org.atriaSoft.ephysics.collision.shapes;
|
||||||
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
|
|
||||||
* @author Daniel CHAPPUIS
|
|
||||||
* @author Edouard DUPIN
|
|
||||||
* @copyright 2010-2016, Daniel Chappuis
|
|
||||||
* @copyright 2017, Edouard DUPIN
|
|
||||||
* @license MPL v2.0 (see license file)
|
|
||||||
*/
|
|
||||||
#pragma once
|
|
||||||
|
|
||||||
#include <etk/typeInfo.hpp>
|
|
||||||
#include <etk/math/Vector3D.hpp>
|
|
||||||
#include <etk/math/Matrix3x3.hpp>
|
|
||||||
#include <ephysics/mathematics/Ray.hpp>
|
|
||||||
#include <ephysics/collision/shapes/AABB.hpp>
|
|
||||||
#include <ephysics/collision/RaycastInfo.hpp>
|
|
||||||
|
|
||||||
namespace ephysics {
|
|
||||||
enum CollisionShapeType {TRIANGLE, BOX, SPHERE, CONE, CYLINDER,
|
enum CollisionShapeType {TRIANGLE, BOX, SPHERE, CONE, CYLINDER,
|
||||||
CAPSULE, CONVEXMESH, CONCAVEMESH, HEIGHTFIELD};
|
CAPSULE, CONVEXMESH, CONCAVEMESH, HEIGHTFIELD};
|
||||||
int NBCOLLISIONSHAPETYPES = 9;
|
int NBCOLLISIONSHAPETYPES = 9;
|
||||||
@ -31,12 +15,6 @@ class CollisionShape {
|
|||||||
public :
|
public :
|
||||||
/// Constructor
|
/// Constructor
|
||||||
CollisionShape(CollisionShapeType type);
|
CollisionShape(CollisionShapeType type);
|
||||||
/// DELETE copy-ructor
|
|
||||||
CollisionShape( CollisionShape shape) = delete;
|
|
||||||
/// DELETE assignment operator
|
|
||||||
CollisionShape operator=( CollisionShape shape) = delete;
|
|
||||||
/// Virtualize destructor
|
|
||||||
virtual ~CollisionShape() {};
|
|
||||||
/**
|
/**
|
||||||
* @brief Get the type of the collision shapes
|
* @brief Get the type of the collision shapes
|
||||||
* @return The type of the collision shape (box, sphere, cylinder, ...)
|
* @return The type of the collision shape (box, sphere, cylinder, ...)
|
||||||
@ -49,22 +27,22 @@ class CollisionShape {
|
|||||||
* @return true If the collision shape is convex
|
* @return true If the collision shape is convex
|
||||||
* @return false If it is concave
|
* @return false If it is concave
|
||||||
*/
|
*/
|
||||||
virtual boolean isConvex() = 0;
|
boolean isConvex() = 0;
|
||||||
/**
|
/**
|
||||||
* @brief Get the local bounds of the shape in x, y and z directions.
|
* @brief Get the local bounds of the shape in x, y and z directions.
|
||||||
* This method is used to compute the AABB of the box
|
* This method is used to compute the AABB of the box
|
||||||
* @param min The minimum bounds of the shape in local-space coordinates
|
* @param min The minimum bounds of the shape in local-space coordinates
|
||||||
* @param max The maximum bounds of the shape in local-space coordinates
|
* @param max The maximum bounds of the shape in local-space coordinates
|
||||||
*/
|
*/
|
||||||
virtual void getLocalBounds(vec3 min, vec3 max) =0;
|
void getLocalBounds(Vector3f min, Vector3f max) =0;
|
||||||
/// Return the scaling vector of the collision shape
|
/// Return the scaling vector of the collision shape
|
||||||
vec3 getScaling() {
|
Vector3f getScaling() {
|
||||||
return this.scaling;
|
return this.scaling;
|
||||||
}
|
}
|
||||||
/**
|
/**
|
||||||
* @brief Set the scaling vector of the collision shape
|
* @brief Set the scaling vector of the collision shape
|
||||||
*/
|
*/
|
||||||
virtual void setLocalScaling( vec3 scaling) {
|
void setLocalScaling( Vector3f scaling) {
|
||||||
this.scaling = scaling;
|
this.scaling = scaling;
|
||||||
}
|
}
|
||||||
/**
|
/**
|
||||||
@ -72,13 +50,13 @@ class CollisionShape {
|
|||||||
* @param[out] tensor The 3x3 inertia tensor matrix of the shape in local-space coordinates
|
* @param[out] tensor The 3x3 inertia tensor matrix of the shape in local-space coordinates
|
||||||
* @param[in] mass Mass to use to compute the inertia tensor of the collision shape
|
* @param[in] mass Mass to use to compute the inertia tensor of the collision shape
|
||||||
*/
|
*/
|
||||||
virtual void computeLocalInertiaTensor(etk::Matrix3x3 tensor, float mass) =0;
|
void computeLocalInertiaTensor(Matrix3f tensor, float mass) =0;
|
||||||
/**
|
/**
|
||||||
* @brief Update the AABB of a body using its collision shape
|
* @brief Update the AABB of a body using its collision shape
|
||||||
* @param[out] aabb The axis-aligned bounding box (AABB) of the collision shape computed in world-space coordinates
|
* @param[out] aabb The axis-aligned bounding box (AABB) of the collision shape computed in world-space coordinates
|
||||||
* @param[in] transform etk::Transform3D used to compute the AABB of the collision shape
|
* @param[in] transform Transform3D used to compute the AABB of the collision shape
|
||||||
*/
|
*/
|
||||||
virtual void computeAABB(AABB aabb, etk::Transform3D transform) ;
|
void computeAABB(AABB aabb, Transform3D transform) ;
|
||||||
/**
|
/**
|
||||||
* @brief Check if the shape is convex
|
* @brief Check if the shape is convex
|
||||||
* @param[in] shapeType shape type
|
* @param[in] shapeType shape type
|
||||||
@ -87,7 +65,7 @@ class CollisionShape {
|
|||||||
*/
|
*/
|
||||||
static boolean isConvex(CollisionShapeType shapeType) {
|
static boolean isConvex(CollisionShapeType shapeType) {
|
||||||
return shapeType != CONCAVEMESH
|
return shapeType != CONCAVEMESH
|
||||||
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj shapeType != HEIGHTFIELD;
|
&& shapeType != HEIGHTFIELD;
|
||||||
}
|
}
|
||||||
/**
|
/**
|
||||||
* @brief Get the maximum number of contact
|
* @brief Get the maximum number of contact
|
||||||
@ -95,17 +73,15 @@ class CollisionShape {
|
|||||||
*/
|
*/
|
||||||
static int computeNbMaxContactManifolds(CollisionShapeType shapeType1,
|
static int computeNbMaxContactManifolds(CollisionShapeType shapeType1,
|
||||||
CollisionShapeType shapeType2);
|
CollisionShapeType shapeType2);
|
||||||
friend class ProxyShape;
|
|
||||||
friend class CollisionWorld;
|
|
||||||
protected :
|
protected :
|
||||||
CollisionShapeType this.type; //!< Type of the collision shape
|
CollisionShapeType type; //!< Type of the collision shape
|
||||||
vec3 this.scaling; //!< Scaling vector of the collision shape
|
Vector3f scaling; //!< Scaling vector of the collision shape
|
||||||
/// Return true if a point is inside the collision shape
|
/// Return true if a point is inside the collision shape
|
||||||
virtual boolean testPointInside( vec3 worldPoint, ProxyShape* proxyShape) = 0;
|
boolean testPointInside( Vector3f worldPoint, ProxyShape* proxyShape) = 0;
|
||||||
/// Raycast method with feedback information
|
/// Raycast method with feedback information
|
||||||
virtual boolean raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape) = 0;
|
boolean raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape) = 0;
|
||||||
/// Return the number of bytes used by the collision shape
|
/// Return the number of bytes used by the collision shape
|
||||||
virtual sizet getSizeInBytes() = 0;
|
long getSizeInBytes() = 0;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
@ -21,13 +21,13 @@ ConcaveMeshShape::ConcaveMeshShape(TriangleMesh* triangleMesh):
|
|||||||
void ConcaveMeshShape::initBVHTree() {
|
void ConcaveMeshShape::initBVHTree() {
|
||||||
// TODO : Try to randomly add the triangles into the tree to obtain a better tree
|
// TODO : Try to randomly add the triangles into the tree to obtain a better tree
|
||||||
// For each sub-part of the mesh
|
// For each sub-part of the mesh
|
||||||
for (int subPart=0; subPart<this.triangleMesh->getNbSubparts(); subPart++) {
|
for (int subPart=0; subPart<this.triangleMesh.getNbSubparts(); subPart++) {
|
||||||
// Get the triangle vertex array of the current sub-part
|
// Get the triangle vertex array of the current sub-part
|
||||||
TriangleVertexArray* triangleVertexArray = this.triangleMesh->getSubpart(subPart);
|
TriangleVertexArray* triangleVertexArray = this.triangleMesh.getSubpart(subPart);
|
||||||
// For each triangle of the concave mesh
|
// For each triangle of the concave mesh
|
||||||
for (sizet iii=0; iii<triangleVertexArray->getNbTriangles(); ++iii) {
|
for (long iii=0; iii<triangleVertexArray.getNbTriangles(); ++iii) {
|
||||||
ephysics::Triangle trianglePoints = triangleVertexArray->getTriangle(iii);
|
ephysics::Triangle trianglePoints = triangleVertexArray.getTriangle(iii);
|
||||||
vec3 trianglePoints2[3];
|
Vector3f trianglePoints2[3];
|
||||||
trianglePoints2[0] = trianglePoints[0];
|
trianglePoints2[0] = trianglePoints[0];
|
||||||
trianglePoints2[1] = trianglePoints[1];
|
trianglePoints2[1] = trianglePoints[1];
|
||||||
trianglePoints2[2] = trianglePoints[2];
|
trianglePoints2[2] = trianglePoints[2];
|
||||||
@ -40,14 +40,14 @@ void ConcaveMeshShape::initBVHTree() {
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void ConcaveMeshShape::getTriangleVerticesWithIndexPointer(int subPart, int triangleIndex, vec3* outTriangleVertices) {
|
void ConcaveMeshShape::getTriangleVerticesWithIndexPointer(int subPart, int triangleIndex, Vector3f* outTriangleVertices) {
|
||||||
EPHYASSERT(outTriangleVertices != null, "Input check error");
|
EPHYASSERT(outTriangleVertices != null, "Input check error");
|
||||||
// Get the triangle vertex array of the current sub-part
|
// Get the triangle vertex array of the current sub-part
|
||||||
TriangleVertexArray* triangleVertexArray = this.triangleMesh->getSubpart(subPart);
|
TriangleVertexArray* triangleVertexArray = this.triangleMesh.getSubpart(subPart);
|
||||||
if (triangleVertexArray == null) {
|
if (triangleVertexArray == null) {
|
||||||
Log.error("get null ...");
|
Log.error("get null ...");
|
||||||
}
|
}
|
||||||
ephysics::Triangle trianglePoints = triangleVertexArray->getTriangle(triangleIndex);
|
ephysics::Triangle trianglePoints = triangleVertexArray.getTriangle(triangleIndex);
|
||||||
outTriangleVertices[0] = trianglePoints[0] * this.scaling;
|
outTriangleVertices[0] = trianglePoints[0] * this.scaling;
|
||||||
outTriangleVertices[1] = trianglePoints[1] * this.scaling;
|
outTriangleVertices[1] = trianglePoints[1] * this.scaling;
|
||||||
outTriangleVertices[2] = trianglePoints[2] * this.scaling;
|
outTriangleVertices[2] = trianglePoints[2] * this.scaling;
|
||||||
@ -60,7 +60,7 @@ void ConcaveMeshShape::testAllTriangles(TriangleCallback callback, AABB localAA
|
|||||||
// Get the node data (triangle index and mesh subpart index)
|
// Get the node data (triangle index and mesh subpart index)
|
||||||
int* data = this.dynamicAABBTree.getNodeDataInt(nodeId);
|
int* data = this.dynamicAABBTree.getNodeDataInt(nodeId);
|
||||||
// Get the triangle vertices for this node from the concave mesh shape
|
// Get the triangle vertices for this node from the concave mesh shape
|
||||||
vec3 trianglePoints[3];
|
Vector3f trianglePoints[3];
|
||||||
getTriangleVerticesWithIndexPointer(data[0], data[1], trianglePoints);
|
getTriangleVerticesWithIndexPointer(data[0], data[1], trianglePoints);
|
||||||
// Call the callback to test narrow-phase collision with this triangle
|
// Call the callback to test narrow-phase collision with this triangle
|
||||||
callback.testTriangle(trianglePoints);
|
callback.testTriangle(trianglePoints);
|
||||||
@ -86,13 +86,13 @@ float ConcaveMeshRaycastCallback::operator()(int nodeId, Ray ray) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
void ConcaveMeshRaycastCallback::raycastTriangles() {
|
void ConcaveMeshRaycastCallback::raycastTriangles() {
|
||||||
etk::Vector<int>::Iterator it;
|
Vector<int>::Iterator it;
|
||||||
float smallestHitFraction = this.ray.maxFraction;
|
float smallestHitFraction = this.ray.maxFraction;
|
||||||
for (it = this.hitAABBNodes.begin(); it != this.hitAABBNodes.end(); ++it) {
|
for (it = this.hitAABBNodes.begin(); it != this.hitAABBNodes.end(); ++it) {
|
||||||
// Get the node data (triangle index and mesh subpart index)
|
// Get the node data (triangle index and mesh subpart index)
|
||||||
int* data = this.dynamicAABBTree.getNodeDataInt(*it);
|
int* data = this.dynamicAABBTree.getNodeDataInt(*it);
|
||||||
// Get the triangle vertices for this node from the concave mesh shape
|
// Get the triangle vertices for this node from the concave mesh shape
|
||||||
vec3 trianglePoints[3];
|
Vector3f trianglePoints[3];
|
||||||
this.concaveMeshShape.getTriangleVerticesWithIndexPointer(data[0], data[1], trianglePoints);
|
this.concaveMeshShape.getTriangleVerticesWithIndexPointer(data[0], data[1], trianglePoints);
|
||||||
// Create a triangle collision shape
|
// Create a triangle collision shape
|
||||||
float margin = this.concaveMeshShape.getTriangleMargin();
|
float margin = this.concaveMeshShape.getTriangleMargin();
|
||||||
@ -102,7 +102,7 @@ void ConcaveMeshRaycastCallback::raycastTriangles() {
|
|||||||
RaycastInfo raycastInfo;
|
RaycastInfo raycastInfo;
|
||||||
boolean isTriangleHit = triangleShape.raycast(this.ray, raycastInfo, this.proxyShape);
|
boolean isTriangleHit = triangleShape.raycast(this.ray, raycastInfo, this.proxyShape);
|
||||||
// If the ray hit the collision shape
|
// If the ray hit the collision shape
|
||||||
if (isTriangleHit hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj raycastInfo.hitFraction <= smallestHitFraction) {
|
if (isTriangleHit && raycastInfo.hitFraction <= smallestHitFraction) {
|
||||||
assert(raycastInfo.hitFraction >= 0.0f);
|
assert(raycastInfo.hitFraction >= 0.0f);
|
||||||
this.raycastInfo.body = raycastInfo.body;
|
this.raycastInfo.body = raycastInfo.body;
|
||||||
this.raycastInfo.proxyShape = raycastInfo.proxyShape;
|
this.raycastInfo.proxyShape = raycastInfo.proxyShape;
|
||||||
@ -117,24 +117,24 @@ void ConcaveMeshRaycastCallback::raycastTriangles() {
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
sizet ConcaveMeshShape::getSizeInBytes() {
|
long ConcaveMeshShape::getSizeInBytes() {
|
||||||
return sizeof(ConcaveMeshShape);
|
return sizeof(ConcaveMeshShape);
|
||||||
}
|
}
|
||||||
|
|
||||||
void ConcaveMeshShape::getLocalBounds(vec3 min, vec3 max) {
|
void ConcaveMeshShape::getLocalBounds(Vector3f min, Vector3f max) {
|
||||||
// Get the AABB of the whole tree
|
// Get the AABB of the whole tree
|
||||||
AABB treeAABB = this.dynamicAABBTree.getRootAABB();
|
AABB treeAABB = this.dynamicAABBTree.getRootAABB();
|
||||||
min = treeAABB.getMin();
|
min = treeAABB.getMin();
|
||||||
max = treeAABB.getMax();
|
max = treeAABB.getMax();
|
||||||
}
|
}
|
||||||
|
|
||||||
void ConcaveMeshShape::setLocalScaling( vec3 scaling) {
|
void ConcaveMeshShape::setLocalScaling( Vector3f scaling) {
|
||||||
CollisionShape::setLocalScaling(scaling);
|
CollisionShape::setLocalScaling(scaling);
|
||||||
this.dynamicAABBTree.reset();
|
this.dynamicAABBTree.reset();
|
||||||
initBVHTree();
|
initBVHTree();
|
||||||
}
|
}
|
||||||
|
|
||||||
void ConcaveMeshShape::computeLocalInertiaTensor(etk::Matrix3x3 tensor, float mass) {
|
void ConcaveMeshShape::computeLocalInertiaTensor(Matrix3f tensor, float mass) {
|
||||||
// Default inertia tensor
|
// Default inertia tensor
|
||||||
// Note that this is not very realistic for a concave triangle mesh.
|
// Note that this is not very realistic for a concave triangle mesh.
|
||||||
// However, in most cases, it will only be used static bodies and therefore,
|
// However, in most cases, it will only be used static bodies and therefore,
|
||||||
|
@ -1,85 +0,0 @@
|
|||||||
/** @file
|
|
||||||
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
|
|
||||||
* @author Daniel CHAPPUIS
|
|
||||||
* @author Edouard DUPIN
|
|
||||||
* @copyright 2010-2016, Daniel Chappuis
|
|
||||||
* @copyright 2017, Edouard DUPIN
|
|
||||||
* @license MPL v2.0 (see license file)
|
|
||||||
*/
|
|
||||||
#pragma once
|
|
||||||
|
|
||||||
#include <ephysics/collision/shapes/ConcaveShape.hpp>
|
|
||||||
#include <ephysics/collision/broadphase/DynamicAABBTree.hpp>
|
|
||||||
#include <ephysics/collision/TriangleMesh.hpp>
|
|
||||||
#include <ephysics/collision/shapes/TriangleShape.hpp>
|
|
||||||
#include <ephysics/engine/Profiler.hpp>
|
|
||||||
|
|
||||||
namespace ephysics {
|
|
||||||
class ConcaveMeshShape;
|
|
||||||
class ConcaveMeshRaycastCallback {
|
|
||||||
private:
|
|
||||||
etk::Vector<int> this.hitAABBNodes;
|
|
||||||
DynamicAABBTree this.dynamicAABBTree;
|
|
||||||
ConcaveMeshShape this.concaveMeshShape;
|
|
||||||
ProxyShape* this.proxyShape;
|
|
||||||
RaycastInfo this.raycastInfo;
|
|
||||||
Ray this.ray;
|
|
||||||
boolean this.isHit;
|
|
||||||
public:
|
|
||||||
// Constructor
|
|
||||||
ConcaveMeshRaycastCallback( DynamicAABBTree dynamicAABBTree,
|
|
||||||
ConcaveMeshShape concaveMeshShape,
|
|
||||||
ProxyShape* proxyShape,
|
|
||||||
RaycastInfo raycastInfo,
|
|
||||||
Ray ray):
|
|
||||||
this.dynamicAABBTree(dynamicAABBTree),
|
|
||||||
this.concaveMeshShape(concaveMeshShape),
|
|
||||||
this.proxyShape(proxyShape),
|
|
||||||
this.raycastInfo(raycastInfo),
|
|
||||||
this.ray(ray),
|
|
||||||
this.isHit(false) {
|
|
||||||
|
|
||||||
}
|
|
||||||
/// Collect all the AABB nodes that are hit by the ray in the Dynamic AABB Tree
|
|
||||||
float operator()(int nodeId, ephysics::Ray ray);
|
|
||||||
/// Raycast all collision shapes that have been collected
|
|
||||||
void raycastTriangles();
|
|
||||||
/// Return true if a raycast hit has been found
|
|
||||||
boolean getIsHit() {
|
|
||||||
return this.isHit;
|
|
||||||
}
|
|
||||||
};
|
|
||||||
/**
|
|
||||||
* @brief Represents a static concave mesh shape. Note that collision detection
|
|
||||||
* with a concave mesh shape can be very expensive. You should use only use
|
|
||||||
* this shape for a static mesh.
|
|
||||||
*/
|
|
||||||
class ConcaveMeshShape : public ConcaveShape {
|
|
||||||
public:
|
|
||||||
/// Constructor
|
|
||||||
ConcaveMeshShape(TriangleMesh* triangleMesh);
|
|
||||||
/// DELETE copy-ructor
|
|
||||||
ConcaveMeshShape( ConcaveMeshShape shape) = delete;
|
|
||||||
/// DELETE assignment operator
|
|
||||||
ConcaveMeshShape operator=( ConcaveMeshShape shape) = delete;
|
|
||||||
virtual void getLocalBounds(vec3 min, vec3 max) override;
|
|
||||||
virtual void setLocalScaling( vec3 scaling) override;
|
|
||||||
virtual void computeLocalInertiaTensor(etk::Matrix3x3 tensor, float mass) override;
|
|
||||||
virtual void testAllTriangles(TriangleCallback callback, AABB localAABB) override;
|
|
||||||
friend class ConvexTriangleAABBOverlapCallback;
|
|
||||||
friend class ConcaveMeshRaycastCallback;
|
|
||||||
protected:
|
|
||||||
TriangleMesh* this.triangleMesh; //!< Triangle mesh
|
|
||||||
DynamicAABBTree this.dynamicAABBTree; //!< Dynamic AABB tree to accelerate collision with the triangles
|
|
||||||
virtual boolean raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape) override;
|
|
||||||
virtual sizet getSizeInBytes() override;
|
|
||||||
/// Insert all the triangles into the dynamic AABB tree
|
|
||||||
void initBVHTree();
|
|
||||||
/// Return the three vertices coordinates (in the array outTriangleVertices) of a triangle
|
|
||||||
/// given the start vertex index pointer of the triangle.
|
|
||||||
void getTriangleVerticesWithIndexPointer(int subPart,
|
|
||||||
int triangleIndex,
|
|
||||||
vec3* outTriangleVertices) ;
|
|
||||||
};
|
|
||||||
|
|
||||||
}
|
|
@ -0,0 +1,63 @@
|
|||||||
|
package org.atriaSoft.ephysics.collision.shapes;
|
||||||
|
|
||||||
|
class ConcaveMeshRaycastCallback {
|
||||||
|
private:
|
||||||
|
Vector<int> hitAABBNodes;
|
||||||
|
DynamicAABBTree dynamicAABBTree;
|
||||||
|
ConcaveMeshShape concaveMeshShape;
|
||||||
|
ProxyShape* proxyShape;
|
||||||
|
RaycastInfo raycastInfo;
|
||||||
|
Ray ray;
|
||||||
|
boolean isHit;
|
||||||
|
public:
|
||||||
|
// Constructor
|
||||||
|
ConcaveMeshRaycastCallback( DynamicAABBTree dynamicAABBTree,
|
||||||
|
ConcaveMeshShape concaveMeshShape,
|
||||||
|
ProxyShape* proxyShape,
|
||||||
|
RaycastInfo raycastInfo,
|
||||||
|
Ray ray):
|
||||||
|
this.dynamicAABBTree(dynamicAABBTree),
|
||||||
|
this.concaveMeshShape(concaveMeshShape),
|
||||||
|
this.proxyShape(proxyShape),
|
||||||
|
this.raycastInfo(raycastInfo),
|
||||||
|
this.ray(ray),
|
||||||
|
this.isHit(false) {
|
||||||
|
|
||||||
|
}
|
||||||
|
/// Collect all the AABB nodes that are hit by the ray in the Dynamic AABB Tree
|
||||||
|
float operator()(int nodeId, ephysics::Ray ray);
|
||||||
|
/// Raycast all collision shapes that have been collected
|
||||||
|
void raycastTriangles();
|
||||||
|
/// Return true if a raycast hit has been found
|
||||||
|
boolean getIsHit() {
|
||||||
|
return this.isHit;
|
||||||
|
}
|
||||||
|
};
|
||||||
|
/**
|
||||||
|
* @brief Represents a static concave mesh shape. Note that collision detection
|
||||||
|
* with a concave mesh shape can be very expensive. You should use only use
|
||||||
|
* this shape for a static mesh.
|
||||||
|
*/
|
||||||
|
class ConcaveMeshShape extends ConcaveShape {
|
||||||
|
public:
|
||||||
|
/// Constructor
|
||||||
|
ConcaveMeshShape(TriangleMesh* triangleMesh);
|
||||||
|
void getLocalBounds(Vector3f min, Vector3f max);
|
||||||
|
void setLocalScaling( Vector3f scaling);
|
||||||
|
void computeLocalInertiaTensor(Matrix3f tensor, float mass);
|
||||||
|
void testAllTriangles(TriangleCallback callback, AABB localAABB);
|
||||||
|
protected:
|
||||||
|
TriangleMesh* triangleMesh; //!< Triangle mesh
|
||||||
|
DynamicAABBTree dynamicAABBTree; //!< Dynamic AABB tree to accelerate collision with the triangles
|
||||||
|
boolean raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape);
|
||||||
|
long getSizeInBytes();
|
||||||
|
/// Insert all the triangles into the dynamic AABB tree
|
||||||
|
void initBVHTree();
|
||||||
|
/// Return the three vertices coordinates (in the array outTriangleVertices) of a triangle
|
||||||
|
/// given the start vertex index pointer of the triangle.
|
||||||
|
void getTriangleVerticesWithIndexPointer(int subPart,
|
||||||
|
int triangleIndex,
|
||||||
|
Vector3f* outTriangleVertices) ;
|
||||||
|
};
|
||||||
|
|
||||||
|
}
|
@ -29,7 +29,7 @@ boolean ConcaveShape::isConvex() {
|
|||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
boolean ConcaveShape::testPointInside( vec3 localPoint, ProxyShape* proxyShape) {
|
boolean ConcaveShape::testPointInside( Vector3f localPoint, ProxyShape* proxyShape) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -1,33 +1,19 @@
|
|||||||
/** @file
|
package org.atriaSoft.ephysics.collision.shapes;
|
||||||
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
|
|
||||||
* @author Daniel CHAPPUIS
|
|
||||||
* @author Edouard DUPIN
|
|
||||||
* @copyright 2010-2016, Daniel Chappuis
|
|
||||||
* @copyright 2017, Edouard DUPIN
|
|
||||||
* @license MPL v2.0 (see license file)
|
|
||||||
*/
|
|
||||||
#pragma once
|
|
||||||
|
|
||||||
#include <ephysics/collision/shapes/CollisionShape.hpp>
|
|
||||||
#include <ephysics/collision/shapes/TriangleShape.hpp>
|
|
||||||
|
|
||||||
namespace ephysics {
|
|
||||||
/**
|
/**
|
||||||
* @brief It is used to encapsulate a callback method for
|
* @brief It is used to encapsulate a callback method for
|
||||||
* a single triangle of a ConcaveMesh.
|
* a single triangle of a ConcaveMesh.
|
||||||
*/
|
*/
|
||||||
class TriangleCallback {
|
class TriangleCallback {
|
||||||
public:
|
/// Report a triangle
|
||||||
virtual ~TriangleCallback() = default;
|
public void testTriangle( Vector3f* trianglePoints)=0;
|
||||||
/// Report a triangle
|
|
||||||
virtual void testTriangle( vec3* trianglePoints)=0;
|
|
||||||
};
|
};
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @brief This abstract class represents a concave collision shape associated with a
|
* @brief This abstract class represents a concave collision shape associated with a
|
||||||
* body that is used during the narrow-phase collision detection.
|
* body that is used during the narrow-phase collision detection.
|
||||||
*/
|
*/
|
||||||
class ConcaveShape : public CollisionShape {
|
class ConcaveShape extends CollisionShape {
|
||||||
public :
|
public :
|
||||||
/// Constructor
|
/// Constructor
|
||||||
ConcaveShape(CollisionShapeType type);
|
ConcaveShape(CollisionShapeType type);
|
||||||
@ -36,10 +22,10 @@ namespace ephysics {
|
|||||||
/// DELETE assignment operator
|
/// DELETE assignment operator
|
||||||
ConcaveShape operator=( ConcaveShape shape) = delete;
|
ConcaveShape operator=( ConcaveShape shape) = delete;
|
||||||
protected :
|
protected :
|
||||||
boolean this.isSmoothMeshCollisionEnabled; //!< True if the smooth mesh collision algorithm is enabled
|
boolean isSmoothMeshCollisionEnabled; //!< True if the smooth mesh collision algorithm is enabled
|
||||||
float this.triangleMargin; //!< Margin use for collision detection for each triangle
|
float triangleMargin; //!< Margin use for collision detection for each triangle
|
||||||
TriangleRaycastSide this.raycastTestType; //!< Raycast test type for the triangle (front, back, front-back)
|
TriangleRaycastSide raycastTestType; //!< Raycast test type for the triangle (front, back, front-back)
|
||||||
boolean testPointInside( vec3 localPoint, ProxyShape* proxyShape) override;
|
boolean testPointInside( Vector3f localPoint, ProxyShape* proxyShape) ;
|
||||||
public:
|
public:
|
||||||
/// Return the triangle margin
|
/// Return the triangle margin
|
||||||
float getTriangleMargin() ;
|
float getTriangleMargin() ;
|
||||||
@ -51,9 +37,9 @@ namespace ephysics {
|
|||||||
*/
|
*/
|
||||||
void setRaycastTestType(TriangleRaycastSide testType);
|
void setRaycastTestType(TriangleRaycastSide testType);
|
||||||
/// Return true if the collision shape is convex, false if it is concave
|
/// Return true if the collision shape is convex, false if it is concave
|
||||||
virtual boolean isConvex() override;
|
boolean isConvex() ;
|
||||||
/// Use a callback method on all triangles of the concave shape inside a given AABB
|
/// Use a callback method on all triangles of the concave shape inside a given AABB
|
||||||
virtual void testAllTriangles(TriangleCallback callback, AABB localAABB) =0;
|
void testAllTriangles(TriangleCallback callback, AABB localAABB) =0;
|
||||||
/// Return true if the smooth mesh collision is enabled
|
/// Return true if the smooth mesh collision is enabled
|
||||||
boolean getIsSmoothMeshCollisionEnabled() ;
|
boolean getIsSmoothMeshCollisionEnabled() ;
|
||||||
/**
|
/**
|
@ -22,49 +22,49 @@ ConeShape::ConeShape(float radius, float height, float margin):
|
|||||||
this.sinTheta = this.radius / (sqrt(this.radius * this.radius + height * height));
|
this.sinTheta = this.radius / (sqrt(this.radius * this.radius + height * height));
|
||||||
}
|
}
|
||||||
|
|
||||||
vec3 ConeShape::getLocalSupportPointWithoutMargin( vec3 direction, void** cachedCollisionData) {
|
Vector3f ConeShape::getLocalSupportPointWithoutMargin( Vector3f direction, void** cachedCollisionData) {
|
||||||
vec3 v = direction;
|
Vector3f v = direction;
|
||||||
float sinThetaTimesLengthV = this.sinTheta * v.length();
|
float sinThetaTimesLengthV = this.sinTheta * v.length();
|
||||||
vec3 supportPoint;
|
Vector3f supportPoint;
|
||||||
if (v.y() > sinThetaTimesLengthV) {
|
if (v.y() > sinThetaTimesLengthV) {
|
||||||
supportPoint = vec3(0.0, this.halfHeight, 0.0);
|
supportPoint = Vector3f(0.0, this.halfHeight, 0.0);
|
||||||
} else {
|
} else {
|
||||||
float projectedLength = sqrt(v.x() * v.x() + v.z() * v.z());
|
float projectedLength = sqrt(v.x() * v.x() + v.z() * v.z());
|
||||||
if (projectedLength > FLTEPSILON) {
|
if (projectedLength > FLTEPSILON) {
|
||||||
float d = this.radius / projectedLength;
|
float d = this.radius / projectedLength;
|
||||||
supportPoint = vec3(v.x() * d, -this.halfHeight, v.z() * d);
|
supportPoint = Vector3f(v.x() * d, -this.halfHeight, v.z() * d);
|
||||||
} else {
|
} else {
|
||||||
supportPoint = vec3(0.0, -this.halfHeight, 0.0);
|
supportPoint = Vector3f(0.0, -this.halfHeight, 0.0);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
return supportPoint;
|
return supportPoint;
|
||||||
}
|
}
|
||||||
|
|
||||||
boolean ConeShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape) {
|
boolean ConeShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape) {
|
||||||
vec3 r = ray.point2 - ray.point1;
|
Vector3f r = ray.point2 - ray.point1;
|
||||||
float epsilon = float(0.00001);
|
float epsilon = float(0.00001);
|
||||||
vec3 V(0, this.halfHeight, 0);
|
Vector3f V(0, this.halfHeight, 0);
|
||||||
vec3 centerBase(0, -this.halfHeight, 0);
|
Vector3f centerBase(0, -this.halfHeight, 0);
|
||||||
vec3 axis(0, float(-1.0), 0);
|
Vector3f axis(0, float(-1.0), 0);
|
||||||
float heightSquare = float(4.0) * this.halfHeight * this.halfHeight;
|
float heightSquare = float(4.0) * this.halfHeight * this.halfHeight;
|
||||||
float cosThetaSquare = heightSquare / (heightSquare + this.radius * this.radius);
|
float cosThetaSquare = heightSquare / (heightSquare + this.radius * this.radius);
|
||||||
float factor = 1.0f - cosThetaSquare;
|
float factor = 1.0f - cosThetaSquare;
|
||||||
vec3 delta = ray.point1 - V;
|
Vector3f delta = ray.point1 - V;
|
||||||
float c0 = -cosThetaSquare * delta.x() * delta.x() + factor * delta.y() * delta.y() - cosThetaSquare * delta.z() * delta.z();
|
float c0 = -cosThetaSquare * delta.x() * delta.x() + factor * delta.y() * delta.y() - cosThetaSquare * delta.z() * delta.z();
|
||||||
float c1 = -cosThetaSquare * delta.x() * r.x() + factor * delta.y() * r.y() - cosThetaSquare * delta.z() * r.z();
|
float c1 = -cosThetaSquare * delta.x() * r.x() + factor * delta.y() * r.y() - cosThetaSquare * delta.z() * r.z();
|
||||||
float c2 = -cosThetaSquare * r.x() * r.x() + factor * r.y() * r.y() - cosThetaSquare * r.z() * r.z();
|
float c2 = -cosThetaSquare * r.x() * r.x() + factor * r.y() * r.y() - cosThetaSquare * r.z() * r.z();
|
||||||
float tHit[] = {float(-1.0), float(-1.0), float(-1.0)};
|
float tHit[] = {float(-1.0), float(-1.0), float(-1.0)};
|
||||||
vec3 localHitPoint[3];
|
Vector3f localHitPoint[3];
|
||||||
vec3 localNormal[3];
|
Vector3f localNormal[3];
|
||||||
// If c2 is different from zero
|
// If c2 is different from zero
|
||||||
if (etk::abs(c2) > FLTEPSILON) {
|
if (abs(c2) > FLTEPSILON) {
|
||||||
float gamma = c1 * c1 - c0 * c2;
|
float gamma = c1 * c1 - c0 * c2;
|
||||||
// If there is no real roots in the quadratic equation
|
// If there is no real roots in the quadratic equation
|
||||||
if (gamma < 0.0f) {
|
if (gamma < 0.0f) {
|
||||||
return false;
|
return false;
|
||||||
} else if (gamma > 0.0f) { // The equation has two real roots
|
} else if (gamma > 0.0f) { // The equation has two real roots
|
||||||
// Compute two intersections
|
// Compute two intersections
|
||||||
float sqrRoot = etk::sqrt(gamma);
|
float sqrRoot = sqrt(gamma);
|
||||||
tHit[0] = (-c1 - sqrRoot) / c2;
|
tHit[0] = (-c1 - sqrRoot) / c2;
|
||||||
tHit[1] = (-c1 + sqrRoot) / c2;
|
tHit[1] = (-c1 + sqrRoot) / c2;
|
||||||
} else { // If the equation has a single real root
|
} else { // If the equation has a single real root
|
||||||
@ -73,7 +73,7 @@ boolean ConeShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyS
|
|||||||
}
|
}
|
||||||
} else {
|
} else {
|
||||||
// If c2 == 0
|
// If c2 == 0
|
||||||
if (etk::abs(c1) > FLTEPSILON) {
|
if (abs(c1) > FLTEPSILON) {
|
||||||
// If c2 = 0 and c1 != 0
|
// If c2 = 0 and c1 != 0
|
||||||
tHit[0] = -c0 / (float(2.0) * c1);
|
tHit[0] = -c0 / (float(2.0) * c1);
|
||||||
} else {
|
} else {
|
||||||
@ -140,14 +140,14 @@ boolean ConeShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyS
|
|||||||
float value1 = (localHitPoint[hitIndex].x() * localHitPoint[hitIndex].x() + localHitPoint[hitIndex].z() * localHitPoint[hitIndex].z());
|
float value1 = (localHitPoint[hitIndex].x() * localHitPoint[hitIndex].x() + localHitPoint[hitIndex].z() * localHitPoint[hitIndex].z());
|
||||||
float rOverH = this.radius / h;
|
float rOverH = this.radius / h;
|
||||||
float value2 = 1.0f + rOverH * rOverH;
|
float value2 = 1.0f + rOverH * rOverH;
|
||||||
float factor = 1.0f / etk::sqrt(value1 * value2);
|
float factor = 1.0f / sqrt(value1 * value2);
|
||||||
float x = localHitPoint[hitIndex].x() * factor;
|
float x = localHitPoint[hitIndex].x() * factor;
|
||||||
float z = localHitPoint[hitIndex].z() * factor;
|
float z = localHitPoint[hitIndex].z() * factor;
|
||||||
localNormal[hitIndex].setX(x);
|
localNormal[hitIndex].setX(x);
|
||||||
localNormal[hitIndex].setY(etk::sqrt(x * x + z * z) * rOverH);
|
localNormal[hitIndex].setY(sqrt(x * x + z * z) * rOverH);
|
||||||
localNormal[hitIndex].setZ(z);
|
localNormal[hitIndex].setZ(z);
|
||||||
}
|
}
|
||||||
raycastInfo.body = proxyShape->getBody();
|
raycastInfo.body = proxyShape.getBody();
|
||||||
raycastInfo.proxyShape = proxyShape;
|
raycastInfo.proxyShape = proxyShape;
|
||||||
raycastInfo.hitFraction = t;
|
raycastInfo.hitFraction = t;
|
||||||
raycastInfo.worldPoint = localHitPoint[hitIndex];
|
raycastInfo.worldPoint = localHitPoint[hitIndex];
|
||||||
@ -163,17 +163,17 @@ float ConeShape::getHeight() {
|
|||||||
return float(2.0) * this.halfHeight;
|
return float(2.0) * this.halfHeight;
|
||||||
}
|
}
|
||||||
|
|
||||||
void ConeShape::setLocalScaling( vec3 scaling) {
|
void ConeShape::setLocalScaling( Vector3f scaling) {
|
||||||
this.halfHeight = (this.halfHeight / this.scaling.y()) * scaling.y();
|
this.halfHeight = (this.halfHeight / this.scaling.y()) * scaling.y();
|
||||||
this.radius = (this.radius / this.scaling.x()) * scaling.x();
|
this.radius = (this.radius / this.scaling.x()) * scaling.x();
|
||||||
CollisionShape::setLocalScaling(scaling);
|
CollisionShape::setLocalScaling(scaling);
|
||||||
}
|
}
|
||||||
|
|
||||||
sizet ConeShape::getSizeInBytes() {
|
long ConeShape::getSizeInBytes() {
|
||||||
return sizeof(ConeShape);
|
return sizeof(ConeShape);
|
||||||
}
|
}
|
||||||
|
|
||||||
void ConeShape::getLocalBounds(vec3 min, vec3 max) {
|
void ConeShape::getLocalBounds(Vector3f min, Vector3f max) {
|
||||||
// Maximum bounds
|
// Maximum bounds
|
||||||
max.setX(this.radius + this.margin);
|
max.setX(this.radius + this.margin);
|
||||||
max.setY(this.halfHeight + this.margin);
|
max.setY(this.halfHeight + this.margin);
|
||||||
@ -184,7 +184,7 @@ void ConeShape::getLocalBounds(vec3 min, vec3 max) {
|
|||||||
min.setZ(min.x());
|
min.setZ(min.x());
|
||||||
}
|
}
|
||||||
|
|
||||||
void ConeShape::computeLocalInertiaTensor(etk::Matrix3x3 tensor, float mass) {
|
void ConeShape::computeLocalInertiaTensor(Matrix3f tensor, float mass) {
|
||||||
float rSquare = this.radius * this.radius;
|
float rSquare = this.radius * this.radius;
|
||||||
float diagXZ = float(0.15) * mass * (rSquare + this.halfHeight);
|
float diagXZ = float(0.15) * mass * (rSquare + this.halfHeight);
|
||||||
tensor.setValue(diagXZ, 0.0, 0.0,
|
tensor.setValue(diagXZ, 0.0, 0.0,
|
||||||
@ -192,11 +192,11 @@ void ConeShape::computeLocalInertiaTensor(etk::Matrix3x3 tensor, float mass) {
|
|||||||
0.0, 0.0, 0.0, diagXZ);
|
0.0, 0.0, 0.0, diagXZ);
|
||||||
}
|
}
|
||||||
|
|
||||||
boolean ConeShape::testPointInside( vec3 localPoint, ProxyShape* proxyShape) {
|
boolean ConeShape::testPointInside( Vector3f localPoint, ProxyShape* proxyShape) {
|
||||||
float radiusHeight = this.radius
|
float radiusHeight = this.radius
|
||||||
* (-localPoint.y() + this.halfHeight)
|
* (-localPoint.y() + this.halfHeight)
|
||||||
/ (this.halfHeight * float(2.0));
|
/ (this.halfHeight * float(2.0));
|
||||||
return ( localPoint.y() < this.halfHeight
|
return ( localPoint.y() < this.halfHeight
|
||||||
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj localPoint.y() > -this.halfHeight)
|
&& localPoint.y() > -this.halfHeight)
|
||||||
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj (localPoint.x() * localPoint.x() + localPoint.z() * localPoint.z() < radiusHeight *radiusHeight);
|
&& (localPoint.x() * localPoint.x() + localPoint.z() * localPoint.z() < radiusHeight *radiusHeight);
|
||||||
}
|
}
|
||||||
|
@ -1,18 +1,5 @@
|
|||||||
/** @file
|
package org.atriaSoft.ephysics.collision.shapes;
|
||||||
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
|
|
||||||
* @author Daniel CHAPPUIS
|
|
||||||
* @author Edouard DUPIN
|
|
||||||
* @copyright 2010-2016, Daniel Chappuis
|
|
||||||
* @copyright 2017, Edouard DUPIN
|
|
||||||
* @license MPL v2.0 (see license file)
|
|
||||||
*/
|
|
||||||
#pragma once
|
|
||||||
|
|
||||||
#include <ephysics/collision/shapes/ConvexShape.hpp>
|
|
||||||
#include <ephysics/body/CollisionBody.hpp>
|
|
||||||
#include <ephysics/mathematics/mathematics.hpp>
|
|
||||||
|
|
||||||
namespace ephysics {
|
|
||||||
/**
|
/**
|
||||||
* @brief This class represents a cone collision shape centered at the
|
* @brief This class represents a cone collision shape centered at the
|
||||||
* origin and alligned with the Y axis. The cone is defined
|
* origin and alligned with the Y axis. The cone is defined
|
||||||
@ -27,7 +14,7 @@ namespace ephysics {
|
|||||||
* ructor of the cone shape. Otherwise, it is recommended to use the
|
* ructor of the cone shape. Otherwise, it is recommended to use the
|
||||||
* default margin distance by not using the "margin" parameter in the ructor.
|
* default margin distance by not using the "margin" parameter in the ructor.
|
||||||
*/
|
*/
|
||||||
class ConeShape : public ConvexShape {
|
class ConeShape extends ConvexShape {
|
||||||
public :
|
public :
|
||||||
/**
|
/**
|
||||||
* @brief Constructor
|
* @brief Constructor
|
||||||
@ -36,32 +23,28 @@ namespace ephysics {
|
|||||||
* @param margin Collision margin (in meters) around the collision shape
|
* @param margin Collision margin (in meters) around the collision shape
|
||||||
*/
|
*/
|
||||||
ConeShape(float radius, float height, float margin = OBJECTMARGIN);
|
ConeShape(float radius, float height, float margin = OBJECTMARGIN);
|
||||||
/// DELETE copy-ructor
|
|
||||||
ConeShape( ConeShape shape) = delete;
|
|
||||||
/// DELETE assignment operator
|
|
||||||
ConeShape operator=( ConeShape shape) = delete;
|
|
||||||
protected :
|
protected :
|
||||||
float this.radius; //!< Radius of the base
|
float radius; //!< Radius of the base
|
||||||
float this.halfHeight; //!< Half height of the cone
|
float halfHeight; //!< Half height of the cone
|
||||||
float this.sinTheta; //!< sine of the semi angle at the apex point
|
float sinTheta; //!< sine of the semi angle at the apex point
|
||||||
virtual vec3 getLocalSupportPointWithoutMargin( vec3 direction, void** cachedCollisionData) override;
|
Vector3f getLocalSupportPointWithoutMargin( Vector3f direction, void** cachedCollisionData) ;
|
||||||
boolean testPointInside( vec3 localPoint, ProxyShape* proxyShape) override;
|
boolean testPointInside( Vector3f localPoint, ProxyShape* proxyShape) ;
|
||||||
boolean raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape) override;
|
boolean raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape) ;
|
||||||
sizet getSizeInBytes() override;
|
long getSizeInBytes() ;
|
||||||
public:
|
public:
|
||||||
/**
|
/**
|
||||||
* @brief Return the radius
|
* @brief Return the radius
|
||||||
* @return Radius of the cone (in meters)
|
* @return Radius of the cone (in meters)
|
||||||
*/
|
*/
|
||||||
float getRadius() ;
|
float getRadius();
|
||||||
/**
|
/**
|
||||||
* @brief Return the height
|
* @brief Return the height
|
||||||
* @return Height of the cone (in meters)
|
* @return Height of the cone (in meters)
|
||||||
*/
|
*/
|
||||||
float getHeight() ;
|
float getHeight();
|
||||||
|
|
||||||
void setLocalScaling( vec3 scaling) override;
|
void setLocalScaling( Vector3f scaling) ;
|
||||||
void getLocalBounds(vec3 min, vec3 max) override;
|
void getLocalBounds(Vector3f min, Vector3f max) ;
|
||||||
void computeLocalInertiaTensor(etk::Matrix3x3 tensor, float mass) override;
|
void computeLocalInertiaTensor(Matrix3f tensor, float mass) ;
|
||||||
};
|
};
|
||||||
}
|
}
|
@ -26,7 +26,7 @@ ConvexMeshShape::ConvexMeshShape( float* arrayVertices,
|
|||||||
// Copy all the vertices into the internal array
|
// Copy all the vertices into the internal array
|
||||||
for (int iii=0; iii<this.numberVertices; iii++) {
|
for (int iii=0; iii<this.numberVertices; iii++) {
|
||||||
float* newPoint = ( float*) vertexPointer;
|
float* newPoint = ( float*) vertexPointer;
|
||||||
this.vertices.pushBack(vec3(newPoint[0], newPoint[1], newPoint[2]));
|
this.vertices.pushBack(Vector3f(newPoint[0], newPoint[1], newPoint[2]));
|
||||||
vertexPointer += stride;
|
vertexPointer += stride;
|
||||||
}
|
}
|
||||||
// Recalculate the bounds of the mesh
|
// Recalculate the bounds of the mesh
|
||||||
@ -41,17 +41,17 @@ ConvexMeshShape::ConvexMeshShape(TriangleVertexArray* triangleVertexArray,
|
|||||||
this.maxBounds(0, 0, 0),
|
this.maxBounds(0, 0, 0),
|
||||||
this.isEdgesInformationUsed(isEdgesInformationUsed) {
|
this.isEdgesInformationUsed(isEdgesInformationUsed) {
|
||||||
// For each vertex of the mesh
|
// For each vertex of the mesh
|
||||||
for (auto it: triangleVertexArray->getVertices()) {
|
for (auto it: triangleVertexArray.getVertices()) {
|
||||||
this.vertices.pushBack(it*this.scaling);
|
this.vertices.pushBack(it*this.scaling);
|
||||||
}
|
}
|
||||||
// If we need to use the edges information of the mesh
|
// If we need to use the edges information of the mesh
|
||||||
if (this.isEdgesInformationUsed) {
|
if (this.isEdgesInformationUsed) {
|
||||||
// For each triangle of the mesh
|
// For each triangle of the mesh
|
||||||
for (sizet iii=0; iii<triangleVertexArray->getNbTriangles(); iii++) {
|
for (long iii=0; iii<triangleVertexArray.getNbTriangles(); iii++) {
|
||||||
int vertexIndex[3] = {0, 0, 0};
|
int vertexIndex[3] = {0, 0, 0};
|
||||||
vertexIndex[0] = triangleVertexArray->getIndices()[iii*3];
|
vertexIndex[0] = triangleVertexArray.getIndices()[iii*3];
|
||||||
vertexIndex[1] = triangleVertexArray->getIndices()[iii*3+1];
|
vertexIndex[1] = triangleVertexArray.getIndices()[iii*3+1];
|
||||||
vertexIndex[2] = triangleVertexArray->getIndices()[iii*3+2];
|
vertexIndex[2] = triangleVertexArray.getIndices()[iii*3+2];
|
||||||
// Add information about the edges
|
// Add information about the edges
|
||||||
addEdge(vertexIndex[0], vertexIndex[1]);
|
addEdge(vertexIndex[0], vertexIndex[1]);
|
||||||
addEdge(vertexIndex[0], vertexIndex[2]);
|
addEdge(vertexIndex[0], vertexIndex[2]);
|
||||||
@ -71,7 +71,7 @@ ConvexMeshShape::ConvexMeshShape(float margin):
|
|||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
vec3 ConvexMeshShape::getLocalSupportPointWithoutMargin( vec3 direction,
|
Vector3f ConvexMeshShape::getLocalSupportPointWithoutMargin( Vector3f direction,
|
||||||
void** cachedCollisionData) {
|
void** cachedCollisionData) {
|
||||||
assert(this.numberVertices == this.vertices.size());
|
assert(this.numberVertices == this.vertices.size());
|
||||||
assert(cachedCollisionData != null);
|
assert(cachedCollisionData != null);
|
||||||
@ -91,9 +91,9 @@ vec3 ConvexMeshShape::getLocalSupportPointWithoutMargin( vec3 direction,
|
|||||||
isOptimal = true;
|
isOptimal = true;
|
||||||
assert(this.edgesAdjacencyList[maxVertex].size() > 0);
|
assert(this.edgesAdjacencyList[maxVertex].size() > 0);
|
||||||
// For all neighbors of the current vertex
|
// For all neighbors of the current vertex
|
||||||
etk::Set<int>::Iterator it;
|
Set<int>::Iterator it;
|
||||||
etk::Set<int>::Iterator itBegin = this.edgesAdjacencyList[maxVertex].begin();
|
Set<int>::Iterator itBegin = this.edgesAdjacencyList[maxVertex].begin();
|
||||||
etk::Set<int>::Iterator itEnd = this.edgesAdjacencyList[maxVertex].end();
|
Set<int>::Iterator itEnd = this.edgesAdjacencyList[maxVertex].end();
|
||||||
for (it = itBegin; it != itEnd; ++it) {
|
for (it = itBegin; it != itEnd; ++it) {
|
||||||
// Compute the dot product
|
// Compute the dot product
|
||||||
float dotProduct = direction.dot(this.vertices[*it]);
|
float dotProduct = direction.dot(this.vertices[*it]);
|
||||||
@ -160,32 +160,32 @@ void ConvexMeshShape::recalculateBounds() {
|
|||||||
this.maxBounds = this.maxBounds * this.scaling;
|
this.maxBounds = this.maxBounds * this.scaling;
|
||||||
this.minBounds = this.minBounds * this.scaling;
|
this.minBounds = this.minBounds * this.scaling;
|
||||||
// Add the object margin to the bounds
|
// Add the object margin to the bounds
|
||||||
this.maxBounds += vec3(this.margin, this.margin, this.margin);
|
this.maxBounds += Vector3f(this.margin, this.margin, this.margin);
|
||||||
this.minBounds -= vec3(this.margin, this.margin, this.margin);
|
this.minBounds -= Vector3f(this.margin, this.margin, this.margin);
|
||||||
}
|
}
|
||||||
|
|
||||||
boolean ConvexMeshShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape) {
|
boolean ConvexMeshShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape) {
|
||||||
return proxyShape->this.body->this.world.this.collisionDetection.this.narrowPhaseGJKAlgorithm.raycast(ray, proxyShape, raycastInfo);
|
return proxyShape.this.body.this.world.collisionDetection.narrowPhaseGJKAlgorithm.raycast(ray, proxyShape, raycastInfo);
|
||||||
}
|
}
|
||||||
|
|
||||||
void ConvexMeshShape::setLocalScaling( vec3 scaling) {
|
void ConvexMeshShape::setLocalScaling( Vector3f scaling) {
|
||||||
ConvexShape::setLocalScaling(scaling);
|
ConvexShape::setLocalScaling(scaling);
|
||||||
recalculateBounds();
|
recalculateBounds();
|
||||||
}
|
}
|
||||||
|
|
||||||
sizet ConvexMeshShape::getSizeInBytes() {
|
long ConvexMeshShape::getSizeInBytes() {
|
||||||
return sizeof(ConvexMeshShape);
|
return sizeof(ConvexMeshShape);
|
||||||
}
|
}
|
||||||
|
|
||||||
void ConvexMeshShape::getLocalBounds(vec3 min, vec3 max) {
|
void ConvexMeshShape::getLocalBounds(Vector3f min, Vector3f max) {
|
||||||
min = this.minBounds;
|
min = this.minBounds;
|
||||||
max = this.maxBounds;
|
max = this.maxBounds;
|
||||||
}
|
}
|
||||||
|
|
||||||
void ConvexMeshShape::computeLocalInertiaTensor(etk::Matrix3x3 tensor, float mass) {
|
void ConvexMeshShape::computeLocalInertiaTensor(Matrix3f tensor, float mass) {
|
||||||
float factor = (1.0f / float(3.0)) * mass;
|
float factor = (1.0f / float(3.0)) * mass;
|
||||||
vec3 realExtent = 0.5f * (this.maxBounds - this.minBounds);
|
Vector3f realExtent = 0.5f * (this.maxBounds - this.minBounds);
|
||||||
assert(realExtent.x() > 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj realExtent.y() > 0 hjkhjkhjkhkj realExtent.z() > 0);
|
assert(realExtent.x() > 0 && realExtent.y() > 0 hjkhjkhjkhkj realExtent.z() > 0);
|
||||||
float xSquare = realExtent.x() * realExtent.x();
|
float xSquare = realExtent.x() * realExtent.x();
|
||||||
float ySquare = realExtent.y() * realExtent.y();
|
float ySquare = realExtent.y() * realExtent.y();
|
||||||
float zSquare = realExtent.z() * realExtent.z();
|
float zSquare = realExtent.z() * realExtent.z();
|
||||||
@ -194,7 +194,7 @@ void ConvexMeshShape::computeLocalInertiaTensor(etk::Matrix3x3 tensor, float mas
|
|||||||
0.0, 0.0, factor * (xSquare + ySquare));
|
0.0, 0.0, factor * (xSquare + ySquare));
|
||||||
}
|
}
|
||||||
|
|
||||||
void ConvexMeshShape::addVertex( vec3 vertex) {
|
void ConvexMeshShape::addVertex( Vector3f vertex) {
|
||||||
// Add the vertex in to vertices array
|
// Add the vertex in to vertices array
|
||||||
this.vertices.pushBack(vertex);
|
this.vertices.pushBack(vertex);
|
||||||
this.numberVertices++;
|
this.numberVertices++;
|
||||||
@ -222,11 +222,11 @@ void ConvexMeshShape::addVertex( vec3 vertex) {
|
|||||||
void ConvexMeshShape::addEdge(int v1, int v2) {
|
void ConvexMeshShape::addEdge(int v1, int v2) {
|
||||||
// If the entry for vertex v1 does not exist in the adjacency list
|
// If the entry for vertex v1 does not exist in the adjacency list
|
||||||
if (this.edgesAdjacencyList.count(v1) == 0) {
|
if (this.edgesAdjacencyList.count(v1) == 0) {
|
||||||
this.edgesAdjacencyList.add(v1, etk::Set<int>());
|
this.edgesAdjacencyList.add(v1, Set<int>());
|
||||||
}
|
}
|
||||||
// If the entry for vertex v2 does not exist in the adjacency list
|
// If the entry for vertex v2 does not exist in the adjacency list
|
||||||
if (this.edgesAdjacencyList.count(v2) == 0) {
|
if (this.edgesAdjacencyList.count(v2) == 0) {
|
||||||
this.edgesAdjacencyList.add(v2, etk::Set<int>());
|
this.edgesAdjacencyList.add(v2, Set<int>());
|
||||||
}
|
}
|
||||||
// Add the edge in the adjacency list
|
// Add the edge in the adjacency list
|
||||||
this.edgesAdjacencyList[v1].add(v2);
|
this.edgesAdjacencyList[v1].add(v2);
|
||||||
@ -241,8 +241,8 @@ void ConvexMeshShape::setIsEdgesInformationUsed(boolean isEdgesUsed) {
|
|||||||
this.isEdgesInformationUsed = isEdgesUsed;
|
this.isEdgesInformationUsed = isEdgesUsed;
|
||||||
}
|
}
|
||||||
|
|
||||||
boolean ConvexMeshShape::testPointInside( vec3 localPoint,
|
boolean ConvexMeshShape::testPointInside( Vector3f localPoint,
|
||||||
ProxyShape* proxyShape) {
|
ProxyShape* proxyShape) {
|
||||||
// Use the GJK algorithm to test if the point is inside the convex mesh
|
// Use the GJK algorithm to test if the point is inside the convex mesh
|
||||||
return proxyShape->this.body->this.world.this.collisionDetection.this.narrowPhaseGJKAlgorithm.testPointInside(localPoint, proxyShape);
|
return proxyShape.this.body.this.world.collisionDetection.narrowPhaseGJKAlgorithm.testPointInside(localPoint, proxyShape);
|
||||||
}
|
}
|
@ -1,23 +1,5 @@
|
|||||||
/** @file
|
package org.atriaSoft.ephysics.collision.shapes;
|
||||||
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
|
|
||||||
* @author Daniel CHAPPUIS
|
|
||||||
* @author Edouard DUPIN
|
|
||||||
* @copyright 2010-2016, Daniel Chappuis
|
|
||||||
* @copyright 2017, Edouard DUPIN
|
|
||||||
* @license MPL v2.0 (see license file)
|
|
||||||
*/
|
|
||||||
#pragma once
|
|
||||||
|
|
||||||
#include <ephysics/collision/shapes/ConvexShape.hpp>
|
|
||||||
#include <ephysics/engine/CollisionWorld.hpp>
|
|
||||||
#include <ephysics/mathematics/mathematics.hpp>
|
|
||||||
#include <ephysics/collision/TriangleMesh.hpp>
|
|
||||||
#include <ephysics/collision/narrowphase/GJK/GJKAlgorithm.hpp>
|
|
||||||
#include <etk/Vector.hpp>
|
|
||||||
#include <etk/Map.hpp>
|
|
||||||
|
|
||||||
namespace ephysics {
|
|
||||||
class CollisionWorld;
|
|
||||||
/**
|
/**
|
||||||
* @brief It represents a convex mesh shape. In order to create a convex mesh shape, you
|
* @brief It represents a convex mesh shape. In order to create a convex mesh shape, you
|
||||||
* need to indicate the local-space position of the mesh vertices. You do it either by
|
* need to indicate the local-space position of the mesh vertices. You do it either by
|
||||||
@ -33,25 +15,21 @@ namespace ephysics {
|
|||||||
* with the addEdge() method. Then, you must use the setIsEdgesInformationUsed(true) method
|
* with the addEdge() method. Then, you must use the setIsEdgesInformationUsed(true) method
|
||||||
* in order to use the edges information for collision detection.
|
* in order to use the edges information for collision detection.
|
||||||
*/
|
*/
|
||||||
class ConvexMeshShape : public ConvexShape {
|
class ConvexMeshShape extends ConvexShape {
|
||||||
protected :
|
protected :
|
||||||
etk::Vector<vec3> this.vertices; //!< Array with the vertices of the mesh
|
Vector<Vector3f> vertices; //!< Array with the vertices of the mesh
|
||||||
int this.numberVertices; //!< Number of vertices in the mesh
|
int numberVertices; //!< Number of vertices in the mesh
|
||||||
vec3 this.minBounds; //!< Mesh minimum bounds in the three local x, y and z directions
|
Vector3f minBounds; //!< Mesh minimum bounds in the three local x, y and z directions
|
||||||
vec3 this.maxBounds; //!< Mesh maximum bounds in the three local x, y and z directions
|
Vector3f maxBounds; //!< Mesh maximum bounds in the three local x, y and z directions
|
||||||
boolean this.isEdgesInformationUsed; //!< True if the shape contains the edges of the convex mesh in order to make the collision detection faster
|
boolean isEdgesInformationUsed; //!< True if the shape contains the edges of the convex mesh in order to make the collision detection faster
|
||||||
etk::Map<int, etk::Set<int> > this.edgesAdjacencyList; //!< Adjacency list representing the edges of the mesh
|
Map<int, Set<int> > edgesAdjacencyList; //!< Adjacency list representing the edges of the mesh
|
||||||
/// Private copy-ructor
|
|
||||||
ConvexMeshShape( ConvexMeshShape shape);
|
|
||||||
/// Private assignment operator
|
|
||||||
ConvexMeshShape operator=( ConvexMeshShape shape);
|
|
||||||
/// Recompute the bounds of the mesh
|
/// Recompute the bounds of the mesh
|
||||||
void recalculateBounds();
|
void recalculateBounds();
|
||||||
void setLocalScaling( vec3 scaling) override;
|
void setLocalScaling( Vector3f scaling) ;
|
||||||
vec3 getLocalSupportPointWithoutMargin( vec3 direction, void** cachedCollisionData) override;
|
Vector3f getLocalSupportPointWithoutMargin( Vector3f direction, void** cachedCollisionData) ;
|
||||||
boolean testPointInside( vec3 localPoint, ProxyShape* proxyShape) override;
|
boolean testPointInside( Vector3f localPoint, ProxyShape* proxyShape) ;
|
||||||
boolean raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape) override;
|
boolean raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape) ;
|
||||||
sizet getSizeInBytes() override;
|
long getSizeInBytes() ;
|
||||||
public :
|
public :
|
||||||
/**
|
/**
|
||||||
* @brief Constructor to initialize with an array of 3D vertices.
|
* @brief Constructor to initialize with an array of 3D vertices.
|
||||||
@ -81,13 +59,13 @@ namespace ephysics {
|
|||||||
*/
|
*/
|
||||||
ConvexMeshShape(float margin = OBJECTMARGIN);
|
ConvexMeshShape(float margin = OBJECTMARGIN);
|
||||||
public:
|
public:
|
||||||
void getLocalBounds(vec3 min, vec3 max) override;
|
void getLocalBounds(Vector3f min, Vector3f max) ;
|
||||||
void computeLocalInertiaTensor(etk::Matrix3x3 tensor, float mass) override;
|
void computeLocalInertiaTensor(Matrix3f tensor, float mass) ;
|
||||||
/**
|
/**
|
||||||
* @brief Add a vertex into the convex mesh
|
* @brief Add a vertex into the convex mesh
|
||||||
* @param vertex Vertex to be added
|
* @param vertex Vertex to be added
|
||||||
*/
|
*/
|
||||||
void addVertex( vec3 vertex);
|
void addVertex( Vector3f vertex);
|
||||||
/**
|
/**
|
||||||
* @brief Add an edge into the convex mesh by specifying the two vertex indices of the edge.
|
* @brief Add an edge into the convex mesh by specifying the two vertex indices of the edge.
|
||||||
* @note that the vertex indices start at zero and need to correspond to the order of
|
* @note that the vertex indices start at zero and need to correspond to the order of
|
@ -18,12 +18,12 @@ ephysics::ConvexShape::~ConvexShape() {
|
|||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
vec3 ephysics::ConvexShape::getLocalSupportPointWithMargin( vec3 direction, void** cachedCollisionData) {
|
Vector3f ephysics::ConvexShape::getLocalSupportPointWithMargin( Vector3f direction, void** cachedCollisionData) {
|
||||||
// Get the support point without margin
|
// Get the support point without margin
|
||||||
vec3 supportPoint = getLocalSupportPointWithoutMargin(direction, cachedCollisionData);
|
Vector3f supportPoint = getLocalSupportPointWithoutMargin(direction, cachedCollisionData);
|
||||||
if (this.margin != 0.0f) {
|
if (this.margin != 0.0f) {
|
||||||
// Add the margin to the support point
|
// Add the margin to the support point
|
||||||
vec3 unitVec(0.0, -1.0, 0.0);
|
Vector3f unitVec(0.0, -1.0, 0.0);
|
||||||
if (direction.length2() > FLTEPSILON * FLTEPSILON) {
|
if (direction.length2() > FLTEPSILON * FLTEPSILON) {
|
||||||
unitVec = direction.safeNormalized();
|
unitVec = direction.safeNormalized();
|
||||||
}
|
}
|
||||||
|
@ -1,51 +0,0 @@
|
|||||||
/** @file
|
|
||||||
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
|
|
||||||
* @author Daniel CHAPPUIS
|
|
||||||
* @author Edouard DUPIN
|
|
||||||
* @copyright 2010-2016, Daniel Chappuis
|
|
||||||
* @copyright 2017, Edouard DUPIN
|
|
||||||
* @license MPL v2.0 (see license file)
|
|
||||||
*/
|
|
||||||
#pragma once
|
|
||||||
|
|
||||||
#include <ephysics/collision/shapes/CollisionShape.hpp>
|
|
||||||
namespace ephysics {
|
|
||||||
/**
|
|
||||||
* @brief It represents a convex collision shape associated with a
|
|
||||||
* body that is used during the narrow-phase collision detection.
|
|
||||||
*/
|
|
||||||
class ConvexShape: public CollisionShape {
|
|
||||||
protected:
|
|
||||||
float this.margin; //!< Margin used for the GJK collision detection algorithm
|
|
||||||
/// Private copy-ructor
|
|
||||||
ConvexShape( ConvexShape shape) = delete;
|
|
||||||
/// Private assignment operator
|
|
||||||
ConvexShape operator=( ConvexShape shape) = delete;
|
|
||||||
// Return a local support point in a given direction with the object margin
|
|
||||||
virtual vec3 getLocalSupportPointWithMargin( vec3 direction, void** cachedCollisionData) ;
|
|
||||||
/// Return a local support point in a given direction without the object margin
|
|
||||||
virtual vec3 getLocalSupportPointWithoutMargin( vec3 direction, void** cachedCollisionData) =0;
|
|
||||||
boolean testPointInside( vec3 worldPoint, ProxyShape* proxyShape) override = 0;
|
|
||||||
public:
|
|
||||||
/// Constructor
|
|
||||||
ConvexShape(CollisionShapeType type, float margin);
|
|
||||||
/// Destructor
|
|
||||||
virtual ~ConvexShape();
|
|
||||||
public:
|
|
||||||
/**
|
|
||||||
* @brief Get the current object margin
|
|
||||||
* @return The margin (in meters) around the collision shape
|
|
||||||
*/
|
|
||||||
float getMargin() {
|
|
||||||
return this.margin;
|
|
||||||
}
|
|
||||||
virtual boolean isConvex() override {
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
friend class GJKAlgorithm;
|
|
||||||
friend class EPAAlgorithm;
|
|
||||||
};
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
33
src/org/atriaSoft/ephysics/collision/shapes/ConvexShape.java
Normal file
33
src/org/atriaSoft/ephysics/collision/shapes/ConvexShape.java
Normal file
@ -0,0 +1,33 @@
|
|||||||
|
package org.atriaSoft.ephysics.collision.shapes;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief It represents a convex collision shape associated with a
|
||||||
|
* body that is used during the narrow-phase collision detection.
|
||||||
|
*/
|
||||||
|
class ConvexShape: public CollisionShape {
|
||||||
|
protected:
|
||||||
|
float margin; //!< Margin used for the GJK collision detection algorithm
|
||||||
|
// Return a local support point in a given direction with the object margin
|
||||||
|
Vector3f getLocalSupportPointWithMargin( Vector3f direction, void** cachedCollisionData) ;
|
||||||
|
/// Return a local support point in a given direction without the object margin
|
||||||
|
Vector3f getLocalSupportPointWithoutMargin( Vector3f direction, void** cachedCollisionData) =0;
|
||||||
|
boolean testPointInside( Vector3f worldPoint, ProxyShape* proxyShape) = 0;
|
||||||
|
public:
|
||||||
|
/// Constructor
|
||||||
|
ConvexShape(CollisionShapeType type, float margin);
|
||||||
|
public:
|
||||||
|
/**
|
||||||
|
* @brief Get the current object margin
|
||||||
|
* @return The margin (in meters) around the collision shape
|
||||||
|
*/
|
||||||
|
float getMargin() {
|
||||||
|
return this.margin;
|
||||||
|
}
|
||||||
|
boolean isConvex() {
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
|
@ -20,11 +20,11 @@ CylinderShape::CylinderShape(float radius,
|
|||||||
assert(height > 0.0f);
|
assert(height > 0.0f);
|
||||||
}
|
}
|
||||||
|
|
||||||
vec3 CylinderShape::getLocalSupportPointWithoutMargin( vec3 direction,
|
Vector3f CylinderShape::getLocalSupportPointWithoutMargin( Vector3f direction,
|
||||||
void** cachedCollisionData) {
|
void** cachedCollisionData) {
|
||||||
vec3 supportPoint(0.0, 0.0, 0.0);
|
Vector3f supportPoint(0.0, 0.0, 0.0);
|
||||||
float uDotv = direction.y();
|
float uDotv = direction.y();
|
||||||
vec3 w(direction.x(), 0.0, direction.z());
|
Vector3f w(direction.x(), 0.0, direction.z());
|
||||||
float lengthW = sqrt(direction.x() * direction.x() + direction.z() * direction.z());
|
float lengthW = sqrt(direction.x() * direction.x() + direction.z() * direction.z());
|
||||||
if (lengthW > FLTEPSILON) {
|
if (lengthW > FLTEPSILON) {
|
||||||
if (uDotv < 0.0) {
|
if (uDotv < 0.0) {
|
||||||
@ -44,21 +44,21 @@ vec3 CylinderShape::getLocalSupportPointWithoutMargin( vec3 direction,
|
|||||||
}
|
}
|
||||||
|
|
||||||
boolean CylinderShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape) {
|
boolean CylinderShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape) {
|
||||||
vec3 n = ray.point2 - ray.point1;
|
Vector3f n = ray.point2 - ray.point1;
|
||||||
float epsilon = float(0.01);
|
float epsilon = float(0.01);
|
||||||
vec3 p(float(0), -this.halfHeight, float(0));
|
Vector3f p(float(0), -this.halfHeight, float(0));
|
||||||
vec3 q(float(0), this.halfHeight, float(0));
|
Vector3f q(float(0), this.halfHeight, float(0));
|
||||||
vec3 d = q - p;
|
Vector3f d = q - p;
|
||||||
vec3 m = ray.point1 - p;
|
Vector3f m = ray.point1 - p;
|
||||||
float t;
|
float t;
|
||||||
float mDotD = m.dot(d);
|
float mDotD = m.dot(d);
|
||||||
float nDotD = n.dot(d);
|
float nDotD = n.dot(d);
|
||||||
float dDotD = d.dot(d);
|
float dDotD = d.dot(d);
|
||||||
// Test if the segment is outside the cylinder
|
// Test if the segment is outside the cylinder
|
||||||
if (mDotD < 0.0f hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj mDotD + nDotD < float(0.0)) {
|
if (mDotD < 0.0f && mDotD + nDotD < float(0.0)) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
if (mDotD > dDotD hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj mDotD + nDotD > dDotD) {
|
if (mDotD > dDotD && mDotD + nDotD > dDotD) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
float nDotN = n.dot(n);
|
float nDotN = n.dot(n);
|
||||||
@ -67,7 +67,7 @@ boolean CylinderShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* pr
|
|||||||
float k = m.dot(m) - this.radius * this.radius;
|
float k = m.dot(m) - this.radius * this.radius;
|
||||||
float c = dDotD * k - mDotD * mDotD;
|
float c = dDotD * k - mDotD * mDotD;
|
||||||
// If the ray is parallel to the cylinder axis
|
// If the ray is parallel to the cylinder axis
|
||||||
if (etk::abs(a) < epsilon) {
|
if (abs(a) < epsilon) {
|
||||||
// If the origin is outside the surface of the cylinder, we return no hit
|
// If the origin is outside the surface of the cylinder, we return no hit
|
||||||
if (c > 0.0f) {
|
if (c > 0.0f) {
|
||||||
return false;
|
return false;
|
||||||
@ -82,12 +82,12 @@ boolean CylinderShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* pr
|
|||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
// Compute the hit information
|
// Compute the hit information
|
||||||
vec3 localHitPoint = ray.point1 + t * n;
|
Vector3f localHitPoint = ray.point1 + t * n;
|
||||||
raycastInfo.body = proxyShape->getBody();
|
raycastInfo.body = proxyShape.getBody();
|
||||||
raycastInfo.proxyShape = proxyShape;
|
raycastInfo.proxyShape = proxyShape;
|
||||||
raycastInfo.hitFraction = t;
|
raycastInfo.hitFraction = t;
|
||||||
raycastInfo.worldPoint = localHitPoint;
|
raycastInfo.worldPoint = localHitPoint;
|
||||||
vec3 normalDirection(0, float(-1), 0);
|
Vector3f normalDirection(0, float(-1), 0);
|
||||||
raycastInfo.worldNormal = normalDirection;
|
raycastInfo.worldNormal = normalDirection;
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
@ -100,12 +100,12 @@ boolean CylinderShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* pr
|
|||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
// Compute the hit information
|
// Compute the hit information
|
||||||
vec3 localHitPoint = ray.point1 + t * n;
|
Vector3f localHitPoint = ray.point1 + t * n;
|
||||||
raycastInfo.body = proxyShape->getBody();
|
raycastInfo.body = proxyShape.getBody();
|
||||||
raycastInfo.proxyShape = proxyShape;
|
raycastInfo.proxyShape = proxyShape;
|
||||||
raycastInfo.hitFraction = t;
|
raycastInfo.hitFraction = t;
|
||||||
raycastInfo.worldPoint = localHitPoint;
|
raycastInfo.worldPoint = localHitPoint;
|
||||||
vec3 normalDirection(0, 1.0f, 0);
|
Vector3f normalDirection(0, 1.0f, 0);
|
||||||
raycastInfo.worldNormal = normalDirection;
|
raycastInfo.worldNormal = normalDirection;
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
@ -119,7 +119,7 @@ boolean CylinderShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* pr
|
|||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
// Compute the smallest root (first intersection along the ray)
|
// Compute the smallest root (first intersection along the ray)
|
||||||
float t0 = t = (-b - etk::sqrt(discriminant)) / a;
|
float t0 = t = (-b - sqrt(discriminant)) / a;
|
||||||
// If the intersection is outside the cylinder on "p" endcap side
|
// If the intersection is outside the cylinder on "p" endcap side
|
||||||
float value = mDotD + t * nDotD;
|
float value = mDotD + t * nDotD;
|
||||||
if (value < 0.0f) {
|
if (value < 0.0f) {
|
||||||
@ -139,12 +139,12 @@ boolean CylinderShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* pr
|
|||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
// Compute the hit information
|
// Compute the hit information
|
||||||
vec3 localHitPoint = ray.point1 + t * n;
|
Vector3f localHitPoint = ray.point1 + t * n;
|
||||||
raycastInfo.body = proxyShape->getBody();
|
raycastInfo.body = proxyShape.getBody();
|
||||||
raycastInfo.proxyShape = proxyShape;
|
raycastInfo.proxyShape = proxyShape;
|
||||||
raycastInfo.hitFraction = t;
|
raycastInfo.hitFraction = t;
|
||||||
raycastInfo.worldPoint = localHitPoint;
|
raycastInfo.worldPoint = localHitPoint;
|
||||||
vec3 normalDirection(0, float(-1.0), 0);
|
Vector3f normalDirection(0, float(-1.0), 0);
|
||||||
raycastInfo.worldNormal = normalDirection;
|
raycastInfo.worldNormal = normalDirection;
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
@ -166,12 +166,12 @@ boolean CylinderShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* pr
|
|||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
// Compute the hit information
|
// Compute the hit information
|
||||||
vec3 localHitPoint = ray.point1 + t * n;
|
Vector3f localHitPoint = ray.point1 + t * n;
|
||||||
raycastInfo.body = proxyShape->getBody();
|
raycastInfo.body = proxyShape.getBody();
|
||||||
raycastInfo.proxyShape = proxyShape;
|
raycastInfo.proxyShape = proxyShape;
|
||||||
raycastInfo.hitFraction = t;
|
raycastInfo.hitFraction = t;
|
||||||
raycastInfo.worldPoint = localHitPoint;
|
raycastInfo.worldPoint = localHitPoint;
|
||||||
vec3 normalDirection(0, 1.0f, 0);
|
Vector3f normalDirection(0, 1.0f, 0);
|
||||||
raycastInfo.worldNormal = normalDirection;
|
raycastInfo.worldNormal = normalDirection;
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
@ -182,14 +182,14 @@ boolean CylinderShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* pr
|
|||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
// Compute the hit information
|
// Compute the hit information
|
||||||
vec3 localHitPoint = ray.point1 + t * n;
|
Vector3f localHitPoint = ray.point1 + t * n;
|
||||||
raycastInfo.body = proxyShape->getBody();
|
raycastInfo.body = proxyShape.getBody();
|
||||||
raycastInfo.proxyShape = proxyShape;
|
raycastInfo.proxyShape = proxyShape;
|
||||||
raycastInfo.hitFraction = t;
|
raycastInfo.hitFraction = t;
|
||||||
raycastInfo.worldPoint = localHitPoint;
|
raycastInfo.worldPoint = localHitPoint;
|
||||||
vec3 v = localHitPoint - p;
|
Vector3f v = localHitPoint - p;
|
||||||
vec3 w = (v.dot(d) / d.length2()) * d;
|
Vector3f w = (v.dot(d) / d.length2()) * d;
|
||||||
vec3 normalDirection = (localHitPoint - (p + w));
|
Vector3f normalDirection = (localHitPoint - (p + w));
|
||||||
raycastInfo.worldNormal = normalDirection;
|
raycastInfo.worldNormal = normalDirection;
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
@ -202,17 +202,17 @@ float CylinderShape::getHeight() {
|
|||||||
return this.halfHeight + this.halfHeight;
|
return this.halfHeight + this.halfHeight;
|
||||||
}
|
}
|
||||||
|
|
||||||
void CylinderShape::setLocalScaling( vec3 scaling) {
|
void CylinderShape::setLocalScaling( Vector3f scaling) {
|
||||||
this.halfHeight = (this.halfHeight / this.scaling.y()) * scaling.y();
|
this.halfHeight = (this.halfHeight / this.scaling.y()) * scaling.y();
|
||||||
this.radius = (this.radius / this.scaling.x()) * scaling.x();
|
this.radius = (this.radius / this.scaling.x()) * scaling.x();
|
||||||
CollisionShape::setLocalScaling(scaling);
|
CollisionShape::setLocalScaling(scaling);
|
||||||
}
|
}
|
||||||
|
|
||||||
sizet CylinderShape::getSizeInBytes() {
|
long CylinderShape::getSizeInBytes() {
|
||||||
return sizeof(CylinderShape);
|
return sizeof(CylinderShape);
|
||||||
}
|
}
|
||||||
|
|
||||||
void CylinderShape::getLocalBounds(vec3 min, vec3 max) {
|
void CylinderShape::getLocalBounds(Vector3f min, Vector3f max) {
|
||||||
// Maximum bounds
|
// Maximum bounds
|
||||||
max.setX(this.radius + this.margin);
|
max.setX(this.radius + this.margin);
|
||||||
max.setY(this.halfHeight + this.margin);
|
max.setY(this.halfHeight + this.margin);
|
||||||
@ -223,7 +223,7 @@ void CylinderShape::getLocalBounds(vec3 min, vec3 max) {
|
|||||||
min.setZ(min.x());
|
min.setZ(min.x());
|
||||||
}
|
}
|
||||||
|
|
||||||
void CylinderShape::computeLocalInertiaTensor(etk::Matrix3x3 tensor, float mass) {
|
void CylinderShape::computeLocalInertiaTensor(Matrix3f tensor, float mass) {
|
||||||
float height = float(2.0) * this.halfHeight;
|
float height = float(2.0) * this.halfHeight;
|
||||||
float diag = (1.0f / float(12.0)) * mass * (3 * this.radius * this.radius + height * height);
|
float diag = (1.0f / float(12.0)) * mass * (3 * this.radius * this.radius + height * height);
|
||||||
tensor.setValue(diag, 0.0, 0.0, 0.0,
|
tensor.setValue(diag, 0.0, 0.0, 0.0,
|
||||||
@ -231,8 +231,8 @@ void CylinderShape::computeLocalInertiaTensor(etk::Matrix3x3 tensor, float mass)
|
|||||||
0.0, 0.0, diag);
|
0.0, 0.0, diag);
|
||||||
}
|
}
|
||||||
|
|
||||||
boolean CylinderShape::testPointInside( vec3 localPoint, ProxyShape* proxyShape) {
|
boolean CylinderShape::testPointInside( Vector3f localPoint, ProxyShape* proxyShape) {
|
||||||
return ( (localPoint.x() * localPoint.x() + localPoint.z() * localPoint.z()) < this.radius * this.radius
|
return ( (localPoint.x() * localPoint.x() + localPoint.z() * localPoint.z()) < this.radius * this.radius
|
||||||
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj localPoint.y() < this.halfHeight
|
&& localPoint.y() < this.halfHeight
|
||||||
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj localPoint.y() > -this.halfHeight);
|
&& localPoint.y() > -this.halfHeight);
|
||||||
}
|
}
|
||||||
|
@ -1,18 +1,5 @@
|
|||||||
/** @file
|
package org.atriaSoft.ephysics.collision.shapes;
|
||||||
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
|
|
||||||
* @author Daniel CHAPPUIS
|
|
||||||
* @author Edouard DUPIN
|
|
||||||
* @copyright 2010-2016, Daniel Chappuis
|
|
||||||
* @copyright 2017, Edouard DUPIN
|
|
||||||
* @license MPL v2.0 (see license file)
|
|
||||||
*/
|
|
||||||
#pragma once
|
|
||||||
|
|
||||||
#include <ephysics/collision/shapes/ConvexShape.hpp>
|
|
||||||
#include <ephysics/body/CollisionBody.hpp>
|
|
||||||
#include <ephysics/mathematics/mathematics.hpp>
|
|
||||||
|
|
||||||
namespace ephysics {
|
|
||||||
/**
|
/**
|
||||||
* @brief It represents a cylinder collision shape around the Y axis
|
* @brief It represents a cylinder collision shape around the Y axis
|
||||||
* and centered at the origin. The cylinder is defined by its height
|
* and centered at the origin. The cylinder is defined by its height
|
||||||
@ -28,16 +15,12 @@ namespace ephysics {
|
|||||||
*/
|
*/
|
||||||
class CylinderShape: public ConvexShape {
|
class CylinderShape: public ConvexShape {
|
||||||
protected:
|
protected:
|
||||||
float this.radius; //!< Radius of the base
|
float radius; //!< Radius of the base
|
||||||
float this.halfHeight; //!< Half height of the cylinder
|
float halfHeight; //!< Half height of the cylinder
|
||||||
/// DELETED copy-ructor
|
Vector3f getLocalSupportPointWithoutMargin( Vector3f direction, void** cachedCollisionData) ;
|
||||||
CylinderShape( CylinderShape) = delete;
|
boolean testPointInside( Vector3f localPoint, ProxyShape* proxyShape) ;
|
||||||
/// DELETED assignment operator
|
boolean raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape) ;
|
||||||
CylinderShape operator=( CylinderShape) = delete;
|
long getSizeInBytes() ;
|
||||||
vec3 getLocalSupportPointWithoutMargin( vec3 direction, void** cachedCollisionData) override;
|
|
||||||
boolean testPointInside( vec3 localPoint, ProxyShape* proxyShape) override;
|
|
||||||
boolean raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape) override;
|
|
||||||
sizet getSizeInBytes() override;
|
|
||||||
public:
|
public:
|
||||||
/**
|
/**
|
||||||
* @brief Contructor
|
* @brief Contructor
|
||||||
@ -56,9 +39,9 @@ namespace ephysics {
|
|||||||
* @return Height of the cylinder (in meters)
|
* @return Height of the cylinder (in meters)
|
||||||
*/
|
*/
|
||||||
float getHeight() ;
|
float getHeight() ;
|
||||||
void setLocalScaling( vec3 scaling) override;
|
void setLocalScaling( Vector3f scaling) ;
|
||||||
void getLocalBounds(vec3 min, vec3 max) override;
|
void getLocalBounds(Vector3f min, Vector3f max) ;
|
||||||
void computeLocalInertiaTensor(etk::Matrix3x3 tensor, float mass) override;
|
void computeLocalInertiaTensor(Matrix3f tensor, float mass) ;
|
||||||
};
|
};
|
||||||
|
|
||||||
}
|
}
|
@ -40,25 +40,25 @@ HeightFieldShape::HeightFieldShape(int nbGridColumns,
|
|||||||
assert(halfHeight >= 0);
|
assert(halfHeight >= 0);
|
||||||
// Compute the local AABB of the height field
|
// Compute the local AABB of the height field
|
||||||
if (this.upAxis == 0) {
|
if (this.upAxis == 0) {
|
||||||
this.AABB.setMin(vec3(-halfHeight, -this.width * 0.5f, -this.length * float(0.5)));
|
this.AABB.setMin(Vector3f(-halfHeight, -this.width * 0.5f, -this.length * float(0.5)));
|
||||||
this.AABB.setMax(vec3(halfHeight, this.width * 0.5f, this.length* float(0.5)));
|
this.AABB.setMax(Vector3f(halfHeight, this.width * 0.5f, this.length* float(0.5)));
|
||||||
} else if (this.upAxis == 1) {
|
} else if (this.upAxis == 1) {
|
||||||
this.AABB.setMin(vec3(-this.width * 0.5f, -halfHeight, -this.length * float(0.5)));
|
this.AABB.setMin(Vector3f(-this.width * 0.5f, -halfHeight, -this.length * float(0.5)));
|
||||||
this.AABB.setMax(vec3(this.width * 0.5f, halfHeight, this.length * float(0.5)));
|
this.AABB.setMax(Vector3f(this.width * 0.5f, halfHeight, this.length * float(0.5)));
|
||||||
} else if (this.upAxis == 2) {
|
} else if (this.upAxis == 2) {
|
||||||
this.AABB.setMin(vec3(-this.width * 0.5f, -this.length * float(0.5), -halfHeight));
|
this.AABB.setMin(Vector3f(-this.width * 0.5f, -this.length * float(0.5), -halfHeight));
|
||||||
this.AABB.setMax(vec3(this.width * 0.5f, this.length * float(0.5), halfHeight));
|
this.AABB.setMax(Vector3f(this.width * 0.5f, this.length * float(0.5), halfHeight));
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void HeightFieldShape::getLocalBounds(vec3 min, vec3 max) {
|
void HeightFieldShape::getLocalBounds(Vector3f min, Vector3f max) {
|
||||||
min = this.AABB.getMin() * this.scaling;
|
min = this.AABB.getMin() * this.scaling;
|
||||||
max = this.AABB.getMax() * this.scaling;
|
max = this.AABB.getMax() * this.scaling;
|
||||||
}
|
}
|
||||||
|
|
||||||
void HeightFieldShape::testAllTriangles(TriangleCallback callback, AABB localAABB) {
|
void HeightFieldShape::testAllTriangles(TriangleCallback callback, AABB localAABB) {
|
||||||
// Compute the non-scaled AABB
|
// Compute the non-scaled AABB
|
||||||
vec3 inverseScaling(1.0f / this.scaling.x(), 1.0f / this.scaling.y(), float(1.0) / this.scaling.z());
|
Vector3f inverseScaling(1.0f / this.scaling.x(), 1.0f / this.scaling.y(), float(1.0) / this.scaling.z());
|
||||||
AABB aabb(localAABB.getMin() * inverseScaling, localAABB.getMax() * inverseScaling);
|
AABB aabb(localAABB.getMin() * inverseScaling, localAABB.getMax() * inverseScaling);
|
||||||
// Compute the integer grid coordinates inside the area we need to test for collision
|
// Compute the integer grid coordinates inside the area we need to test for collision
|
||||||
int minGridCoords[3];
|
int minGridCoords[3];
|
||||||
@ -89,20 +89,20 @@ void HeightFieldShape::testAllTriangles(TriangleCallback callback, AABB localAA
|
|||||||
jMax = clamp(maxGridCoords[1], 0, this.numberRows - 1);
|
jMax = clamp(maxGridCoords[1], 0, this.numberRows - 1);
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
assert(iMin >= 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj iMin < this.numberColumns);
|
assert(iMin >= 0 && iMin < this.numberColumns);
|
||||||
assert(iMax >= 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj iMax < this.numberColumns);
|
assert(iMax >= 0 && iMax < this.numberColumns);
|
||||||
assert(jMin >= 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj jMin < this.numberRows);
|
assert(jMin >= 0 && jMin < this.numberRows);
|
||||||
assert(jMax >= 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj jMax < this.numberRows);
|
assert(jMax >= 0 && jMax < this.numberRows);
|
||||||
// For each sub-grid points (except the last ones one each dimension)
|
// For each sub-grid points (except the last ones one each dimension)
|
||||||
for (int i = iMin; i < iMax; i++) {
|
for (int i = iMin; i < iMax; i++) {
|
||||||
for (int j = jMin; j < jMax; j++) {
|
for (int j = jMin; j < jMax; j++) {
|
||||||
// Compute the four point of the current quad
|
// Compute the four point of the current quad
|
||||||
vec3 p1 = getVertexAt(i, j);
|
Vector3f p1 = getVertexAt(i, j);
|
||||||
vec3 p2 = getVertexAt(i, j + 1);
|
Vector3f p2 = getVertexAt(i, j + 1);
|
||||||
vec3 p3 = getVertexAt(i + 1, j);
|
Vector3f p3 = getVertexAt(i + 1, j);
|
||||||
vec3 p4 = getVertexAt(i + 1, j + 1);
|
Vector3f p4 = getVertexAt(i + 1, j + 1);
|
||||||
// Generate the first triangle for the current grid rectangle
|
// Generate the first triangle for the current grid rectangle
|
||||||
vec3 trianglePoints[3] = {p1, p2, p3};
|
Vector3f trianglePoints[3] = {p1, p2, p3};
|
||||||
// Test collision against the first triangle
|
// Test collision against the first triangle
|
||||||
callback.testTriangle(trianglePoints);
|
callback.testTriangle(trianglePoints);
|
||||||
// Generate the second triangle for the current grid rectangle
|
// Generate the second triangle for the current grid rectangle
|
||||||
@ -117,14 +117,14 @@ void HeightFieldShape::testAllTriangles(TriangleCallback callback, AABB localAA
|
|||||||
|
|
||||||
void HeightFieldShape::computeMinMaxGridCoordinates(int* minCoords, int* maxCoords, AABB aabbToCollide) {
|
void HeightFieldShape::computeMinMaxGridCoordinates(int* minCoords, int* maxCoords, AABB aabbToCollide) {
|
||||||
// Clamp the min/max coords of the AABB to collide inside the height field AABB
|
// Clamp the min/max coords of the AABB to collide inside the height field AABB
|
||||||
vec3 minPoint = etk::max(aabbToCollide.getMin(), this.AABB.getMin());
|
Vector3f minPoint = max(aabbToCollide.getMin(), this.AABB.getMin());
|
||||||
minPoint = etk::min(minPoint, this.AABB.getMax());
|
minPoint = min(minPoint, this.AABB.getMax());
|
||||||
vec3 maxPoint = etk::min(aabbToCollide.getMax(), this.AABB.getMax());
|
Vector3f maxPoint = min(aabbToCollide.getMax(), this.AABB.getMax());
|
||||||
maxPoint = etk::max(maxPoint, this.AABB.getMin());
|
maxPoint = max(maxPoint, this.AABB.getMin());
|
||||||
// Translate the min/max points such that the we compute grid points from [0 ... mNbWidthGridPoints]
|
// Translate the min/max points such that the we compute grid points from [0 ... mNbWidthGridPoints]
|
||||||
// and from [0 ... mNbLengthGridPoints] because the AABB coordinates range are [-mWdith/2 ... this.width/2]
|
// and from [0 ... mNbLengthGridPoints] because the AABB coordinates range are [-mWdith/2 ... this.width/2]
|
||||||
// and [-this.length/2 ... this.length/2]
|
// and [-this.length/2 ... this.length/2]
|
||||||
vec3 translateVec = this.AABB.getExtent() * 0.5f;
|
Vector3f translateVec = this.AABB.getExtent() * 0.5f;
|
||||||
minPoint += translateVec;
|
minPoint += translateVec;
|
||||||
maxPoint += translateVec;
|
maxPoint += translateVec;
|
||||||
// Convert the floating min/max coords of the AABB into closest integer
|
// Convert the floating min/max coords of the AABB into closest integer
|
||||||
@ -144,27 +144,27 @@ boolean HeightFieldShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape*
|
|||||||
PROFILE("HeightFieldShape::raycast()");
|
PROFILE("HeightFieldShape::raycast()");
|
||||||
TriangleOverlapCallback triangleCallback(ray, proxyShape, raycastInfo, *this);
|
TriangleOverlapCallback triangleCallback(ray, proxyShape, raycastInfo, *this);
|
||||||
// Compute the AABB for the ray
|
// Compute the AABB for the ray
|
||||||
vec3 rayEnd = ray.point1 + ray.maxFraction * (ray.point2 - ray.point1);
|
Vector3f rayEnd = ray.point1 + ray.maxFraction * (ray.point2 - ray.point1);
|
||||||
AABB rayAABB(etk::min(ray.point1, rayEnd), etk::max(ray.point1, rayEnd));
|
AABB rayAABB(min(ray.point1, rayEnd), max(ray.point1, rayEnd));
|
||||||
testAllTriangles(triangleCallback, rayAABB);
|
testAllTriangles(triangleCallback, rayAABB);
|
||||||
return triangleCallback.getIsHit();
|
return triangleCallback.getIsHit();
|
||||||
}
|
}
|
||||||
|
|
||||||
vec3 HeightFieldShape::getVertexAt(int xxx, int yyy) {
|
Vector3f HeightFieldShape::getVertexAt(int xxx, int yyy) {
|
||||||
// Get the height value
|
// Get the height value
|
||||||
float height = getHeightAt(xxx, yyy);
|
float height = getHeightAt(xxx, yyy);
|
||||||
// Height values origin
|
// Height values origin
|
||||||
float heightOrigin = -(this.maxHeight - this.minHeight) * 0.5f - this.minHeight;
|
float heightOrigin = -(this.maxHeight - this.minHeight) * 0.5f - this.minHeight;
|
||||||
vec3 vertex;
|
Vector3f vertex;
|
||||||
switch (this.upAxis) {
|
switch (this.upAxis) {
|
||||||
case 0:
|
case 0:
|
||||||
vertex = vec3(heightOrigin + height, -this.width * 0.5f + xxx, -this.length * float(0.5) + yyy);
|
vertex = Vector3f(heightOrigin + height, -this.width * 0.5f + xxx, -this.length * float(0.5) + yyy);
|
||||||
break;
|
break;
|
||||||
case 1:
|
case 1:
|
||||||
vertex = vec3(-this.width * 0.5f + xxx, heightOrigin + height, -this.length * float(0.5) + yyy);
|
vertex = Vector3f(-this.width * 0.5f + xxx, heightOrigin + height, -this.length * float(0.5) + yyy);
|
||||||
break;
|
break;
|
||||||
case 2:
|
case 2:
|
||||||
vertex = vec3(-this.width * 0.5f + xxx, -this.length * float(0.5) + yyy, heightOrigin + height);
|
vertex = Vector3f(-this.width * 0.5f + xxx, -this.length * float(0.5) + yyy, heightOrigin + height);
|
||||||
break;
|
break;
|
||||||
default:
|
default:
|
||||||
assert(false);
|
assert(false);
|
||||||
@ -173,7 +173,7 @@ vec3 HeightFieldShape::getVertexAt(int xxx, int yyy) {
|
|||||||
return vertex * this.scaling;
|
return vertex * this.scaling;
|
||||||
}
|
}
|
||||||
|
|
||||||
void TriangleOverlapCallback::testTriangle( vec3* trianglePoints) {
|
void TriangleOverlapCallback::testTriangle( Vector3f* trianglePoints) {
|
||||||
// Create a triangle collision shape
|
// Create a triangle collision shape
|
||||||
float margin = this.heightFieldShape.getTriangleMargin();
|
float margin = this.heightFieldShape.getTriangleMargin();
|
||||||
TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], margin);
|
TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], margin);
|
||||||
@ -183,7 +183,7 @@ void TriangleOverlapCallback::testTriangle( vec3* trianglePoints) {
|
|||||||
boolean isTriangleHit = triangleShape.raycast(this.ray, raycastInfo, this.proxyShape);
|
boolean isTriangleHit = triangleShape.raycast(this.ray, raycastInfo, this.proxyShape);
|
||||||
// If the ray hit the collision shape
|
// If the ray hit the collision shape
|
||||||
if ( isTriangleHit
|
if ( isTriangleHit
|
||||||
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj raycastInfo.hitFraction <= this.smallestHitFraction) {
|
&& raycastInfo.hitFraction <= this.smallestHitFraction) {
|
||||||
assert(raycastInfo.hitFraction >= 0.0f);
|
assert(raycastInfo.hitFraction >= 0.0f);
|
||||||
this.raycastInfo.body = raycastInfo.body;
|
this.raycastInfo.body = raycastInfo.body;
|
||||||
this.raycastInfo.proxyShape = raycastInfo.proxyShape;
|
this.raycastInfo.proxyShape = raycastInfo.proxyShape;
|
||||||
@ -209,11 +209,11 @@ HeightFieldShape::HeightDataType HeightFieldShape::getHeightDataType() {
|
|||||||
return this.heightDataType;
|
return this.heightDataType;
|
||||||
}
|
}
|
||||||
|
|
||||||
sizet HeightFieldShape::getSizeInBytes() {
|
long HeightFieldShape::getSizeInBytes() {
|
||||||
return sizeof(HeightFieldShape);
|
return sizeof(HeightFieldShape);
|
||||||
}
|
}
|
||||||
|
|
||||||
void HeightFieldShape::setLocalScaling( vec3 scaling) {
|
void HeightFieldShape::setLocalScaling( Vector3f scaling) {
|
||||||
CollisionShape::setLocalScaling(scaling);
|
CollisionShape::setLocalScaling(scaling);
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -235,7 +235,7 @@ int HeightFieldShape::computeIntegerGridValue(float value) {
|
|||||||
return (value < 0.0f) ? value - 0.5f : value + 0.5f;
|
return (value < 0.0f) ? value - 0.5f : value + 0.5f;
|
||||||
}
|
}
|
||||||
|
|
||||||
void HeightFieldShape::computeLocalInertiaTensor(etk::Matrix3x3 tensor, float mass) {
|
void HeightFieldShape::computeLocalInertiaTensor(Matrix3f tensor, float mass) {
|
||||||
// Default inertia tensor
|
// Default inertia tensor
|
||||||
// Note that this is not very realistic for a concave triangle mesh.
|
// Note that this is not very realistic for a concave triangle mesh.
|
||||||
// However, in most cases, it will only be used static bodies and therefore,
|
// However, in most cases, it will only be used static bodies and therefore,
|
||||||
|
@ -1,30 +1,17 @@
|
|||||||
/** @file
|
package org.atriaSoft.ephysics.collision.shapes;
|
||||||
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
|
|
||||||
* @author Daniel CHAPPUIS
|
|
||||||
* @author Edouard DUPIN
|
|
||||||
* @copyright 2010-2016, Daniel Chappuis
|
|
||||||
* @copyright 2017, Edouard DUPIN
|
|
||||||
* @license MPL v2.0 (see license file)
|
|
||||||
*/
|
|
||||||
#pragma once
|
|
||||||
|
|
||||||
#include <ephysics/collision/shapes/ConcaveShape.hpp>
|
|
||||||
#include <ephysics/collision/shapes/TriangleShape.hpp>
|
|
||||||
#include <ephysics/engine/Profiler.hpp>
|
|
||||||
|
|
||||||
namespace ephysics {
|
|
||||||
class HeightFieldShape;
|
class HeightFieldShape;
|
||||||
/**
|
/**
|
||||||
* @brief This class is used for testing AABB and triangle overlap for raycasting
|
* @brief This class is used for testing AABB and triangle overlap for raycasting
|
||||||
*/
|
*/
|
||||||
class TriangleOverlapCallback : public TriangleCallback {
|
class TriangleOverlapCallback extends TriangleCallback {
|
||||||
protected:
|
protected:
|
||||||
Ray this.ray;
|
Ray ray;
|
||||||
ProxyShape* this.proxyShape;
|
ProxyShape* proxyShape;
|
||||||
RaycastInfo this.raycastInfo;
|
RaycastInfo raycastInfo;
|
||||||
boolean this.isHit;
|
boolean isHit;
|
||||||
float this.smallestHitFraction;
|
float smallestHitFraction;
|
||||||
HeightFieldShape this.heightFieldShape;
|
HeightFieldShape heightFieldShape;
|
||||||
public:
|
public:
|
||||||
TriangleOverlapCallback( Ray ray,
|
TriangleOverlapCallback( Ray ray,
|
||||||
ProxyShape* proxyShape,
|
ProxyShape* proxyShape,
|
||||||
@ -41,7 +28,7 @@ namespace ephysics {
|
|||||||
return this.isHit;
|
return this.isHit;
|
||||||
}
|
}
|
||||||
/// Raycast test between a ray and a triangle of the heightfield
|
/// Raycast test between a ray and a triangle of the heightfield
|
||||||
virtual void testTriangle( vec3* trianglePoints);
|
void testTriangle( Vector3f* trianglePoints);
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
@ -55,7 +42,7 @@ namespace ephysics {
|
|||||||
* that for instance, if the minimum height value is -200 and the maximum value is 400, the final
|
* that for instance, if the minimum height value is -200 and the maximum value is 400, the final
|
||||||
* minimum height of the field in the simulation will be -300 and the maximum height will be 300.
|
* minimum height of the field in the simulation will be -300 and the maximum height will be 300.
|
||||||
*/
|
*/
|
||||||
class HeightFieldShape : public ConcaveShape {
|
class HeightFieldShape extends ConcaveShape {
|
||||||
public:
|
public:
|
||||||
/**
|
/**
|
||||||
* @brief Data type for the height data of the height field
|
* @brief Data type for the height data of the height field
|
||||||
@ -66,32 +53,28 @@ namespace ephysics {
|
|||||||
HEIGHTINTTYPE
|
HEIGHTINTTYPE
|
||||||
};
|
};
|
||||||
protected:
|
protected:
|
||||||
int this.numberColumns; //!< Number of columns in the grid of the height field
|
int numberColumns; //!< Number of columns in the grid of the height field
|
||||||
int this.numberRows; //!< Number of rows in the grid of the height field
|
int numberRows; //!< Number of rows in the grid of the height field
|
||||||
float this.width; //!< Height field width
|
float width; //!< Height field width
|
||||||
float this.length; //!< Height field length
|
float length; //!< Height field length
|
||||||
float this.minHeight; //!< Minimum height of the height field
|
float minHeight; //!< Minimum height of the height field
|
||||||
float this.maxHeight; //!< Maximum height of the height field
|
float maxHeight; //!< Maximum height of the height field
|
||||||
int this.upAxis; //!< Up axis direction (0 => x, 1 => y, 2 => z)
|
int upAxis; //!< Up axis direction (0 => x, 1 => y, 2 => z)
|
||||||
float this.integerHeightScale; //!< Height values scale for height field with integer height values
|
float integerHeightScale; //!< Height values scale for height field with integer height values
|
||||||
HeightDataType this.heightDataType; //!< Data type of the height values
|
HeightDataType heightDataType; //!< Data type of the height values
|
||||||
void* this.heightFieldData; //!< Array of data with all the height values of the height field
|
void* heightFieldData; //!< Array of data with all the height values of the height field
|
||||||
AABB this.AABB; //!< Local AABB of the height field (without scaling)
|
AABB AABB; //!< Local AABB of the height field (without scaling)
|
||||||
/// DELETED copy-ructor
|
boolean raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape) ;
|
||||||
HeightFieldShape( HeightFieldShape) = delete;
|
long getSizeInBytes() ;
|
||||||
/// DELETED assignment operator
|
|
||||||
HeightFieldShape operator=( HeightFieldShape) = delete;
|
|
||||||
boolean raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape) override;
|
|
||||||
sizet getSizeInBytes() override;
|
|
||||||
/// Insert all the triangles into the dynamic AABB tree
|
/// Insert all the triangles into the dynamic AABB tree
|
||||||
void initBVHTree();
|
void initBVHTree();
|
||||||
/// Return the three vertices coordinates (in the array outTriangleVertices) of a triangle
|
/// Return the three vertices coordinates (in the array outTriangleVertices) of a triangle
|
||||||
/// given the start vertex index pointer of the triangle.
|
/// given the start vertex index pointer of the triangle.
|
||||||
void getTriangleVerticesWithIndexPointer(int subPart,
|
void getTriangleVerticesWithIndexPointer(int subPart,
|
||||||
int triangleIndex,
|
int triangleIndex,
|
||||||
vec3* outTriangleVertices) ;
|
Vector3f* outTriangleVertices) ;
|
||||||
/// Return the vertex (local-coordinates) of the height field at a given (x,y) position
|
/// Return the vertex (local-coordinates) of the height field at a given (x,y) position
|
||||||
vec3 getVertexAt(int x, int y) ;
|
Vector3f getVertexAt(int x, int y) ;
|
||||||
/// Return the height of a given (x,y) point in the height field
|
/// Return the height of a given (x,y) point in the height field
|
||||||
float getHeightAt(int x, int y) ;
|
float getHeightAt(int x, int y) ;
|
||||||
/// Return the closest inside integer grid value of a given floating grid value
|
/// Return the closest inside integer grid value of a given floating grid value
|
||||||
@ -123,10 +106,10 @@ namespace ephysics {
|
|||||||
int getNbColumns() ;
|
int getNbColumns() ;
|
||||||
/// Return the type of height value in the height field
|
/// Return the type of height value in the height field
|
||||||
HeightDataType getHeightDataType() ;
|
HeightDataType getHeightDataType() ;
|
||||||
void getLocalBounds(vec3 min, vec3 max) override;
|
void getLocalBounds(Vector3f min, Vector3f max) ;
|
||||||
void setLocalScaling( vec3 scaling) override;
|
void setLocalScaling( Vector3f scaling) ;
|
||||||
void computeLocalInertiaTensor(etk::Matrix3x3 tensor, float mass) override;
|
void computeLocalInertiaTensor(Matrix3f tensor, float mass) ;
|
||||||
virtual void testAllTriangles(TriangleCallback callback, AABB localAABB) override;
|
void testAllTriangles(TriangleCallback callback, AABB localAABB) ;
|
||||||
friend class ConvexTriangleAABBOverlapCallback;
|
friend class ConvexTriangleAABBOverlapCallback;
|
||||||
friend class ConcaveMeshRaycastCallback;
|
friend class ConcaveMeshRaycastCallback;
|
||||||
};
|
};
|
@ -18,27 +18,27 @@ SphereShape::SphereShape(float radius):
|
|||||||
assert(radius > 0.0f);
|
assert(radius > 0.0f);
|
||||||
}
|
}
|
||||||
|
|
||||||
void SphereShape::setLocalScaling( vec3 scaling) {
|
void SphereShape::setLocalScaling( Vector3f scaling) {
|
||||||
this.margin = (this.margin / this.scaling.x()) * scaling.x();
|
this.margin = (this.margin / this.scaling.x()) * scaling.x();
|
||||||
CollisionShape::setLocalScaling(scaling);
|
CollisionShape::setLocalScaling(scaling);
|
||||||
}
|
}
|
||||||
|
|
||||||
void SphereShape::computeLocalInertiaTensor(etk::Matrix3x3 tensor, float mass) {
|
void SphereShape::computeLocalInertiaTensor(Matrix3f tensor, float mass) {
|
||||||
float diag = 0.4f * mass * this.margin * this.margin;
|
float diag = 0.4f * mass * this.margin * this.margin;
|
||||||
tensor.setValue(diag, 0.0f, 0.0f,
|
tensor.setValue(diag, 0.0f, 0.0f,
|
||||||
0.0f, diag, 0.0f,
|
0.0f, diag, 0.0f,
|
||||||
0.0f, 0.0f, diag);
|
0.0f, 0.0f, diag);
|
||||||
}
|
}
|
||||||
|
|
||||||
void SphereShape::computeAABB(AABB aabb, etk::Transform3D transform) {
|
void SphereShape::computeAABB(AABB aabb, Transform3D transform) {
|
||||||
// Get the local extents in x,y and z direction
|
// Get the local extents in x,y and z direction
|
||||||
vec3 extents(this.margin, this.margin, this.margin);
|
Vector3f extents(this.margin, this.margin, this.margin);
|
||||||
// Update the AABB with the new minimum and maximum coordinates
|
// Update the AABB with the new minimum and maximum coordinates
|
||||||
aabb.setMin(transform.getPosition() - extents);
|
aabb.setMin(transform.getPosition() - extents);
|
||||||
aabb.setMax(transform.getPosition() + extents);
|
aabb.setMax(transform.getPosition() + extents);
|
||||||
}
|
}
|
||||||
|
|
||||||
void SphereShape::getLocalBounds(vec3 min, vec3 max) {
|
void SphereShape::getLocalBounds(Vector3f min, Vector3f max) {
|
||||||
// Maximum bounds
|
// Maximum bounds
|
||||||
max.setX(this.margin);
|
max.setX(this.margin);
|
||||||
max.setY(this.margin);
|
max.setY(this.margin);
|
||||||
@ -50,13 +50,13 @@ void SphereShape::getLocalBounds(vec3 min, vec3 max) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
boolean SphereShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape) {
|
boolean SphereShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape) {
|
||||||
vec3 m = ray.point1;
|
Vector3f m = ray.point1;
|
||||||
float c = m.dot(m) - this.margin * this.margin;
|
float c = m.dot(m) - this.margin * this.margin;
|
||||||
// If the origin of the ray is inside the sphere, we return no intersection
|
// If the origin of the ray is inside the sphere, we return no intersection
|
||||||
if (c < 0.0f) {
|
if (c < 0.0f) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
vec3 rayDirection = ray.point2 - ray.point1;
|
Vector3f rayDirection = ray.point2 - ray.point1;
|
||||||
float b = m.dot(rayDirection);
|
float b = m.dot(rayDirection);
|
||||||
// If the origin of the ray is outside the sphere and the ray
|
// If the origin of the ray is outside the sphere and the ray
|
||||||
// is pointing away from the sphere, there is no intersection
|
// is pointing away from the sphere, there is no intersection
|
||||||
@ -72,13 +72,13 @@ boolean SphereShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* prox
|
|||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
// Compute the solution "t" closest to the origin
|
// Compute the solution "t" closest to the origin
|
||||||
float t = -b - etk::sqrt(discriminant);
|
float t = -b - sqrt(discriminant);
|
||||||
assert(t >= 0.0f);
|
assert(t >= 0.0f);
|
||||||
// If the hit point is withing the segment ray fraction
|
// If the hit point is withing the segment ray fraction
|
||||||
if (t < ray.maxFraction * raySquareLength) {
|
if (t < ray.maxFraction * raySquareLength) {
|
||||||
// Compute the intersection information
|
// Compute the intersection information
|
||||||
t /= raySquareLength;
|
t /= raySquareLength;
|
||||||
raycastInfo.body = proxyShape->getBody();
|
raycastInfo.body = proxyShape.getBody();
|
||||||
raycastInfo.proxyShape = proxyShape;
|
raycastInfo.proxyShape = proxyShape;
|
||||||
raycastInfo.hitFraction = t;
|
raycastInfo.hitFraction = t;
|
||||||
raycastInfo.worldPoint = ray.point1 + t * rayDirection;
|
raycastInfo.worldPoint = ray.point1 + t * rayDirection;
|
||||||
|
@ -1,57 +0,0 @@
|
|||||||
/** @file
|
|
||||||
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
|
|
||||||
* @author Daniel CHAPPUIS
|
|
||||||
* @author Edouard DUPIN
|
|
||||||
* @copyright 2010-2016, Daniel Chappuis
|
|
||||||
* @copyright 2017, Edouard DUPIN
|
|
||||||
* @license MPL v2.0 (see license file)
|
|
||||||
*/
|
|
||||||
#pragma once
|
|
||||||
|
|
||||||
#include <ephysics/collision/shapes/ConvexShape.hpp>
|
|
||||||
#include <ephysics/body/CollisionBody.hpp>
|
|
||||||
#include <ephysics/mathematics/mathematics.hpp>
|
|
||||||
|
|
||||||
namespace ephysics {
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief Represents a sphere collision shape that is centered
|
|
||||||
* at the origin and defined by its radius. This collision shape does not
|
|
||||||
* have an explicit object margin distance. The margin is implicitly the
|
|
||||||
* radius of the sphere. Therefore, no need to specify an object margin
|
|
||||||
* for a sphere shape.
|
|
||||||
*/
|
|
||||||
class SphereShape : public ConvexShape {
|
|
||||||
protected :
|
|
||||||
SphereShape( SphereShape shape);
|
|
||||||
SphereShape operator=( SphereShape shape) = delete;
|
|
||||||
vec3 getLocalSupportPointWithoutMargin( vec3 direction, void** cachedCollisionData) override {
|
|
||||||
return vec3(0.0, 0.0, 0.0);
|
|
||||||
}
|
|
||||||
boolean testPointInside( vec3 localPoint, ProxyShape* proxyShape) override {
|
|
||||||
return (localPoint.length2() < this.margin * this.margin);
|
|
||||||
}
|
|
||||||
boolean raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape) override;
|
|
||||||
sizet getSizeInBytes() override {
|
|
||||||
return sizeof(SphereShape);
|
|
||||||
}
|
|
||||||
public :
|
|
||||||
/**
|
|
||||||
* @brief Constructor
|
|
||||||
* @param[in] radius Radius of the sphere (in meters)
|
|
||||||
*/
|
|
||||||
SphereShape(float radius);
|
|
||||||
/**
|
|
||||||
* @brief Get the radius of the sphere
|
|
||||||
* @return Radius of the sphere (in meters)
|
|
||||||
*/
|
|
||||||
float getRadius() {
|
|
||||||
return this.margin;
|
|
||||||
}
|
|
||||||
void setLocalScaling( vec3 scaling) override;
|
|
||||||
void getLocalBounds(vec3 min, vec3 max) override;
|
|
||||||
void computeLocalInertiaTensor(etk::Matrix3x3 tensor, float mass) override;
|
|
||||||
void computeAABB(AABB aabb, etk::Transform3D transform) override;
|
|
||||||
};
|
|
||||||
|
|
||||||
}
|
|
42
src/org/atriaSoft/ephysics/collision/shapes/SphereShape.java
Normal file
42
src/org/atriaSoft/ephysics/collision/shapes/SphereShape.java
Normal file
@ -0,0 +1,42 @@
|
|||||||
|
package org.atriaSoft.ephysics.collision.shapes;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Represents a sphere collision shape that is centered
|
||||||
|
* at the origin and defined by its radius. This collision shape does not
|
||||||
|
* have an explicit object margin distance. The margin is implicitly the
|
||||||
|
* radius of the sphere. Therefore, no need to specify an object margin
|
||||||
|
* for a sphere shape.
|
||||||
|
*/
|
||||||
|
class SphereShape extends ConvexShape {
|
||||||
|
protected :
|
||||||
|
SphereShape( SphereShape shape);
|
||||||
|
Vector3f getLocalSupportPointWithoutMargin( Vector3f direction, void** cachedCollisionData) {
|
||||||
|
return Vector3f(0.0, 0.0, 0.0);
|
||||||
|
}
|
||||||
|
boolean testPointInside( Vector3f localPoint, ProxyShape* proxyShape) {
|
||||||
|
return (localPoint.length2() < this.margin * this.margin);
|
||||||
|
}
|
||||||
|
boolean raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape) ;
|
||||||
|
long getSizeInBytes() {
|
||||||
|
return sizeof(SphereShape);
|
||||||
|
}
|
||||||
|
public :
|
||||||
|
/**
|
||||||
|
* @brief Constructor
|
||||||
|
* @param[in] radius Radius of the sphere (in meters)
|
||||||
|
*/
|
||||||
|
SphereShape(float radius);
|
||||||
|
/**
|
||||||
|
* @brief Get the radius of the sphere
|
||||||
|
* @return Radius of the sphere (in meters)
|
||||||
|
*/
|
||||||
|
float getRadius() {
|
||||||
|
return this.margin;
|
||||||
|
}
|
||||||
|
void setLocalScaling( Vector3f scaling) ;
|
||||||
|
void getLocalBounds(Vector3f min, Vector3f max) ;
|
||||||
|
void computeLocalInertiaTensor(Matrix3f tensor, float mass) ;
|
||||||
|
void computeAABB(AABB aabb, Transform3D transform) ;
|
||||||
|
};
|
||||||
|
|
||||||
|
}
|
@ -14,7 +14,7 @@
|
|||||||
// TODO: REMOVE this...
|
// TODO: REMOVE this...
|
||||||
using namespace ephysics;
|
using namespace ephysics;
|
||||||
|
|
||||||
TriangleShape::TriangleShape( vec3 point1, vec3 point2, vec3 point3, float margin)
|
TriangleShape::TriangleShape( Vector3f point1, Vector3f point2, Vector3f point3, float margin)
|
||||||
: ConvexShape(TRIANGLE, margin) {
|
: ConvexShape(TRIANGLE, margin) {
|
||||||
this.points[0] = point1;
|
this.points[0] = point1;
|
||||||
this.points[1] = point2;
|
this.points[1] = point2;
|
||||||
@ -24,13 +24,13 @@ TriangleShape::TriangleShape( vec3 point1, vec3 point2, vec3 point3, float mar
|
|||||||
|
|
||||||
boolean TriangleShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape) {
|
boolean TriangleShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape) {
|
||||||
PROFILE("TriangleShape::raycast()");
|
PROFILE("TriangleShape::raycast()");
|
||||||
vec3 pq = ray.point2 - ray.point1;
|
Vector3f pq = ray.point2 - ray.point1;
|
||||||
vec3 pa = this.points[0] - ray.point1;
|
Vector3f pa = this.points[0] - ray.point1;
|
||||||
vec3 pb = this.points[1] - ray.point1;
|
Vector3f pb = this.points[1] - ray.point1;
|
||||||
vec3 pc = this.points[2] - ray.point1;
|
Vector3f pc = this.points[2] - ray.point1;
|
||||||
// Test if the line PQ is inside the eges BC, CA and AB. We use the triple
|
// Test if the line PQ is inside the eges BC, CA and AB. We use the triple
|
||||||
// product for this test.
|
// product for this test.
|
||||||
vec3 m = pq.cross(pc);
|
Vector3f m = pq.cross(pc);
|
||||||
float u = pb.dot(m);
|
float u = pb.dot(m);
|
||||||
if (this.raycastTestType == FRONT) {
|
if (this.raycastTestType == FRONT) {
|
||||||
if (u < 0.0f) {
|
if (u < 0.0f) {
|
||||||
@ -71,8 +71,8 @@ boolean TriangleShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* pr
|
|||||||
}
|
}
|
||||||
// If the line PQ is in the triangle plane (case where u=v=w=0)
|
// If the line PQ is in the triangle plane (case where u=v=w=0)
|
||||||
if ( approxEqual(u, 0)
|
if ( approxEqual(u, 0)
|
||||||
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj approxEqual(v, 0)
|
&& approxEqual(v, 0)
|
||||||
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj approxEqual(w, 0)) {
|
&& approxEqual(w, 0)) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
// Compute the barycentric coordinates (u, v, w) to determine the
|
// Compute the barycentric coordinates (u, v, w) to determine the
|
||||||
@ -82,17 +82,17 @@ boolean TriangleShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* pr
|
|||||||
v *= denom;
|
v *= denom;
|
||||||
w *= denom;
|
w *= denom;
|
||||||
// Compute the local hit point using the barycentric coordinates
|
// Compute the local hit point using the barycentric coordinates
|
||||||
vec3 localHitPoint = u * this.points[0] + v * this.points[1] + w * this.points[2];
|
Vector3f localHitPoint = u * this.points[0] + v * this.points[1] + w * this.points[2];
|
||||||
float hitFraction = (localHitPoint - ray.point1).length() / pq.length();
|
float hitFraction = (localHitPoint - ray.point1).length() / pq.length();
|
||||||
if ( hitFraction < 0.0f
|
if ( hitFraction < 0.0f
|
||||||
|| hitFraction > ray.maxFraction) {
|
|| hitFraction > ray.maxFraction) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
vec3 localHitNormal = (this.points[1] - this.points[0]).cross(this.points[2] - this.points[0]);
|
Vector3f localHitNormal = (this.points[1] - this.points[0]).cross(this.points[2] - this.points[0]);
|
||||||
if (localHitNormal.dot(pq) > 0.0f) {
|
if (localHitNormal.dot(pq) > 0.0f) {
|
||||||
localHitNormal = -localHitNormal;
|
localHitNormal = -localHitNormal;
|
||||||
}
|
}
|
||||||
raycastInfo.body = proxyShape->getBody();
|
raycastInfo.body = proxyShape.getBody();
|
||||||
raycastInfo.proxyShape = proxyShape;
|
raycastInfo.proxyShape = proxyShape;
|
||||||
raycastInfo.worldPoint = localHitPoint;
|
raycastInfo.worldPoint = localHitPoint;
|
||||||
raycastInfo.hitFraction = hitFraction;
|
raycastInfo.hitFraction = hitFraction;
|
||||||
@ -100,49 +100,49 @@ boolean TriangleShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* pr
|
|||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
sizet TriangleShape::getSizeInBytes() {
|
long TriangleShape::getSizeInBytes() {
|
||||||
return sizeof(TriangleShape);
|
return sizeof(TriangleShape);
|
||||||
}
|
}
|
||||||
|
|
||||||
vec3 TriangleShape::getLocalSupportPointWithoutMargin( vec3 direction,
|
Vector3f TriangleShape::getLocalSupportPointWithoutMargin( Vector3f direction,
|
||||||
void** cachedCollisionData) {
|
void** cachedCollisionData) {
|
||||||
vec3 dotProducts(direction.dot(this.points[0]), direction.dot(this.points[1]), direction.dot(this.points[2]));
|
Vector3f dotProducts(direction.dot(this.points[0]), direction.dot(this.points[1]), direction.dot(this.points[2]));
|
||||||
return this.points[dotProducts.getMaxAxis()];
|
return this.points[dotProducts.getMaxAxis()];
|
||||||
}
|
}
|
||||||
|
|
||||||
void TriangleShape::getLocalBounds(vec3 min, vec3 max) {
|
void TriangleShape::getLocalBounds(Vector3f min, Vector3f max) {
|
||||||
vec3 xAxis(this.points[0].x(), this.points[1].x(), this.points[2].x());
|
Vector3f xAxis(this.points[0].x(), this.points[1].x(), this.points[2].x());
|
||||||
vec3 yAxis(this.points[0].y(), this.points[1].y(), this.points[2].y());
|
Vector3f yAxis(this.points[0].y(), this.points[1].y(), this.points[2].y());
|
||||||
vec3 zAxis(this.points[0].z(), this.points[1].z(), this.points[2].z());
|
Vector3f zAxis(this.points[0].z(), this.points[1].z(), this.points[2].z());
|
||||||
min.setValue(xAxis.getMin(), yAxis.getMin(), zAxis.getMin());
|
min.setValue(xAxis.getMin(), yAxis.getMin(), zAxis.getMin());
|
||||||
max.setValue(xAxis.getMax(), yAxis.getMax(), zAxis.getMax());
|
max.setValue(xAxis.getMax(), yAxis.getMax(), zAxis.getMax());
|
||||||
min -= vec3(this.margin, this.margin, this.margin);
|
min -= Vector3f(this.margin, this.margin, this.margin);
|
||||||
max += vec3(this.margin, this.margin, this.margin);
|
max += Vector3f(this.margin, this.margin, this.margin);
|
||||||
}
|
}
|
||||||
|
|
||||||
void TriangleShape::setLocalScaling( vec3 scaling) {
|
void TriangleShape::setLocalScaling( Vector3f scaling) {
|
||||||
this.points[0] = (this.points[0] / this.scaling) * scaling;
|
this.points[0] = (this.points[0] / this.scaling) * scaling;
|
||||||
this.points[1] = (this.points[1] / this.scaling) * scaling;
|
this.points[1] = (this.points[1] / this.scaling) * scaling;
|
||||||
this.points[2] = (this.points[2] / this.scaling) * scaling;
|
this.points[2] = (this.points[2] / this.scaling) * scaling;
|
||||||
CollisionShape::setLocalScaling(scaling);
|
CollisionShape::setLocalScaling(scaling);
|
||||||
}
|
}
|
||||||
|
|
||||||
void TriangleShape::computeLocalInertiaTensor(etk::Matrix3x3 tensor, float mass) {
|
void TriangleShape::computeLocalInertiaTensor(Matrix3f tensor, float mass) {
|
||||||
tensor.setZero();
|
tensor.setZero();
|
||||||
}
|
}
|
||||||
|
|
||||||
void TriangleShape::computeAABB(AABB aabb, etk::Transform3D transform) {
|
void TriangleShape::computeAABB(AABB aabb, Transform3D transform) {
|
||||||
vec3 worldPoint1 = transform * this.points[0];
|
Vector3f worldPoint1 = transform * this.points[0];
|
||||||
vec3 worldPoint2 = transform * this.points[1];
|
Vector3f worldPoint2 = transform * this.points[1];
|
||||||
vec3 worldPoint3 = transform * this.points[2];
|
Vector3f worldPoint3 = transform * this.points[2];
|
||||||
vec3 xAxis(worldPoint1.x(), worldPoint2.x(), worldPoint3.x());
|
Vector3f xAxis(worldPoint1.x(), worldPoint2.x(), worldPoint3.x());
|
||||||
vec3 yAxis(worldPoint1.y(), worldPoint2.y(), worldPoint3.y());
|
Vector3f yAxis(worldPoint1.y(), worldPoint2.y(), worldPoint3.y());
|
||||||
vec3 zAxis(worldPoint1.z(), worldPoint2.z(), worldPoint3.z());
|
Vector3f zAxis(worldPoint1.z(), worldPoint2.z(), worldPoint3.z());
|
||||||
aabb.setMin(vec3(xAxis.getMin(), yAxis.getMin(), zAxis.getMin()));
|
aabb.setMin(Vector3f(xAxis.getMin(), yAxis.getMin(), zAxis.getMin()));
|
||||||
aabb.setMax(vec3(xAxis.getMax(), yAxis.getMax(), zAxis.getMax()));
|
aabb.setMax(Vector3f(xAxis.getMax(), yAxis.getMax(), zAxis.getMax()));
|
||||||
}
|
}
|
||||||
|
|
||||||
boolean TriangleShape::testPointInside( vec3 localPoint, ProxyShape* proxyShape) {
|
boolean TriangleShape::testPointInside( Vector3f localPoint, ProxyShape* proxyShape) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -155,9 +155,9 @@ void TriangleShape::setRaycastTestType(TriangleRaycastSide testType) {
|
|||||||
this.raycastTestType = testType;
|
this.raycastTestType = testType;
|
||||||
}
|
}
|
||||||
|
|
||||||
vec3 TriangleShape::getVertex(int index) {
|
Vector3f TriangleShape::getVertex(int index) {
|
||||||
assert( index >= 0
|
assert( index >= 0
|
||||||
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj index < 3);
|
&& index < 3);
|
||||||
return this.points[index];
|
return this.points[index];
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -1,17 +1,5 @@
|
|||||||
/** @file
|
package org.atriaSoft.ephysics.collision.shapes;
|
||||||
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
|
|
||||||
* @author Daniel CHAPPUIS
|
|
||||||
* @author Edouard DUPIN
|
|
||||||
* @copyright 2010-2016, Daniel Chappuis
|
|
||||||
* @copyright 2017, Edouard DUPIN
|
|
||||||
* @license MPL v2.0 (see license file)
|
|
||||||
*/
|
|
||||||
#pragma once
|
|
||||||
|
|
||||||
#include <ephysics/mathematics/mathematics.hpp>
|
|
||||||
#include <ephysics/collision/shapes/ConvexShape.hpp>
|
|
||||||
|
|
||||||
namespace ephysics {
|
|
||||||
/**
|
/**
|
||||||
* @brief Raycast test side for the triangle
|
* @brief Raycast test side for the triangle
|
||||||
*/
|
*/
|
||||||
@ -27,16 +15,16 @@ namespace ephysics {
|
|||||||
*/
|
*/
|
||||||
class TriangleShape: public ConvexShape {
|
class TriangleShape: public ConvexShape {
|
||||||
protected:
|
protected:
|
||||||
vec3 this.points[3]; //!< Three points of the triangle
|
Vector3f this.points[3]; //!< Three points of the triangle
|
||||||
TriangleRaycastSide this.raycastTestType; //!< Raycast test type for the triangle (front, back, front-back)
|
TriangleRaycastSide raycastTestType; //!< Raycast test type for the triangle (front, back, front-back)
|
||||||
/// Private copy-ructor
|
/// Private copy-ructor
|
||||||
TriangleShape( TriangleShape shape);
|
TriangleShape( TriangleShape shape);
|
||||||
/// Private assignment operator
|
/// Private assignment operator
|
||||||
TriangleShape operator=( TriangleShape shape);
|
TriangleShape operator=( TriangleShape shape);
|
||||||
vec3 getLocalSupportPointWithoutMargin( vec3 direction, void** cachedCollisionData) override;
|
Vector3f getLocalSupportPointWithoutMargin( Vector3f direction, void** cachedCollisionData) ;
|
||||||
boolean testPointInside( vec3 localPoint, ProxyShape* proxyShape) override;
|
boolean testPointInside( Vector3f localPoint, ProxyShape* proxyShape) ;
|
||||||
boolean raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape) override;
|
boolean raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape) ;
|
||||||
sizet getSizeInBytes() override;
|
long getSizeInBytes() ;
|
||||||
public:
|
public:
|
||||||
/**
|
/**
|
||||||
* @brief Constructor
|
* @brief Constructor
|
||||||
@ -45,14 +33,14 @@ namespace ephysics {
|
|||||||
* @param point3 Third point of the triangle
|
* @param point3 Third point of the triangle
|
||||||
* @param margin The collision margin (in meters) around the collision shape
|
* @param margin The collision margin (in meters) around the collision shape
|
||||||
*/
|
*/
|
||||||
TriangleShape( vec3 point1,
|
TriangleShape( Vector3f point1,
|
||||||
vec3 point2,
|
Vector3f point2,
|
||||||
vec3 point3,
|
Vector3f point3,
|
||||||
float margin = OBJECTMARGIN);
|
float margin = OBJECTMARGIN);
|
||||||
void getLocalBounds(vec3 min, vec3 max) override;
|
void getLocalBounds(Vector3f min, Vector3f max) ;
|
||||||
void setLocalScaling( vec3 scaling) override;
|
void setLocalScaling( Vector3f scaling) ;
|
||||||
void computeLocalInertiaTensor(etk::Matrix3x3 tensor, float mass) override;
|
void computeLocalInertiaTensor(Matrix3f tensor, float mass) ;
|
||||||
void computeAABB(AABB aabb, etk::Transform3D transform) override;
|
void computeAABB(AABB aabb, Transform3D transform) ;
|
||||||
/// Return the raycast test type (front, back, front-back)
|
/// Return the raycast test type (front, back, front-back)
|
||||||
TriangleRaycastSide getRaycastTestType() ;
|
TriangleRaycastSide getRaycastTestType() ;
|
||||||
/**
|
/**
|
||||||
@ -64,7 +52,7 @@ namespace ephysics {
|
|||||||
* @brief Return the coordinates of a given vertex of the triangle
|
* @brief Return the coordinates of a given vertex of the triangle
|
||||||
* @param[in] index Index (0 to 2) of a vertex of the triangle
|
* @param[in] index Index (0 to 2) of a vertex of the triangle
|
||||||
*/
|
*/
|
||||||
vec3 getVertex(int index) ;
|
Vector3f getVertex(int index) ;
|
||||||
friend class ConcaveMeshRaycastCallback;
|
friend class ConcaveMeshRaycastCallback;
|
||||||
friend class TriangleOverlapCallback;
|
friend class TriangleOverlapCallback;
|
||||||
};
|
};
|
@ -1,99 +0,0 @@
|
|||||||
/** @file
|
|
||||||
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
|
|
||||||
* @author Daniel CHAPPUIS
|
|
||||||
* @author Edouard DUPIN
|
|
||||||
* @copyright 2010-2016, Daniel Chappuis
|
|
||||||
* @copyright 2017, Edouard DUPIN
|
|
||||||
* @license MPL v2.0 (see license file)
|
|
||||||
*/
|
|
||||||
#pragma once
|
|
||||||
|
|
||||||
// Libraries
|
|
||||||
#include <etk/types.hpp>
|
|
||||||
#include <etk/Pair.hpp>
|
|
||||||
|
|
||||||
/// Namespace ephysics
|
|
||||||
namespace ephysics {
|
|
||||||
// ------------------- Type definitions ------------------- //
|
|
||||||
typedef long long;
|
|
||||||
typedef etk::Pair<long, long> longpair;
|
|
||||||
|
|
||||||
// ------------------- Enumerations ------------------- //
|
|
||||||
|
|
||||||
/// Position correction technique used in the raint solver (for joints).
|
|
||||||
/// BAUMGARTEJOINTS : Faster but can be innacurate in some situations.
|
|
||||||
/// NONLINEARGAUSSSEIDEL : Slower but more precise. This is the option used by default.
|
|
||||||
enum JointsPositionCorrectionTechnique {BAUMGARTEJOINTS, NONLINEARGAUSSSEIDEL};
|
|
||||||
|
|
||||||
/// Position correction technique used in the contact solver (for contacts)
|
|
||||||
/// BAUMGARTECONTACTS : Faster but can be innacurate and can lead to unexpected bounciness
|
|
||||||
/// in some situations (due to error correction factor being added to
|
|
||||||
/// the bodies momentum).
|
|
||||||
/// SPLITIMPULSES : A bit slower but the error correction factor is not added to the
|
|
||||||
/// bodies momentum. This is the option used by default.
|
|
||||||
enum ContactsPositionCorrectionTechnique {BAUMGARTECONTACTS, SPLITIMPULSES};
|
|
||||||
|
|
||||||
// ------------------- Constants ------------------- //
|
|
||||||
/// Pi ant
|
|
||||||
float PI = float(3.14159265);
|
|
||||||
|
|
||||||
/// 2*Pi ant
|
|
||||||
float PITIMES2 = float(6.28318530);
|
|
||||||
|
|
||||||
/// Default friction coefficient for a rigid body
|
|
||||||
float DEFAULTFRICTIONCOEFFICIENT = float(0.3);
|
|
||||||
|
|
||||||
/// Default bounciness factor for a rigid body
|
|
||||||
float DEFAULTBOUNCINESS = 0.5f;
|
|
||||||
|
|
||||||
/// Default rolling resistance
|
|
||||||
float DEFAULTROLLINGRESISTANCE = 0.0f;
|
|
||||||
|
|
||||||
/// True if the spleeping technique is enabled
|
|
||||||
boolean SPLEEPINGENABLED = true;
|
|
||||||
|
|
||||||
/// Object margin for collision detection in meters (for the GJK-EPA Algorithm)
|
|
||||||
float OBJECTMARGIN = float(0.04);
|
|
||||||
|
|
||||||
/// Distance threshold for two contact points for a valid persistent contact (in meters)
|
|
||||||
float PERSISTENTCONTACTDISTTHRESHOLD = float(0.03);
|
|
||||||
|
|
||||||
/// Velocity threshold for contact velocity restitution
|
|
||||||
float RESTITUTIONVELOCITYTHRESHOLD = 1.0f;
|
|
||||||
|
|
||||||
/// Number of iterations when solving the velocity raints of the Sequential Impulse technique
|
|
||||||
int DEFAULTVELOCITYSOLVERNBITERATIONS = 10;
|
|
||||||
|
|
||||||
/// Number of iterations when solving the position raints of the Sequential Impulse technique
|
|
||||||
int DEFAULTPOSITIONSOLVERNBITERATIONS = 5;
|
|
||||||
|
|
||||||
/// Time (in seconds) that a body must stay still to be considered sleeping
|
|
||||||
float DEFAULTTIMEBEFORESLEEP = 1.0f;
|
|
||||||
|
|
||||||
/// A body with a linear velocity smaller than the sleep linear velocity (in m/s)
|
|
||||||
/// might enter sleeping mode.
|
|
||||||
float DEFAULTSLEEPLINEARVELOCITY = float(0.02);
|
|
||||||
|
|
||||||
/// A body with angular velocity smaller than the sleep angular velocity (in rad/s)
|
|
||||||
/// might enter sleeping mode
|
|
||||||
float DEFAULTSLEEPANGULARVELOCITY = float(3.0 * (PI / 180.0));
|
|
||||||
|
|
||||||
/// In the broad-phase collision detection (dynamic AABB tree), the AABBs are
|
|
||||||
/// inflated with a ant gap to allow the collision shape to move a little bit
|
|
||||||
/// without triggering a large modification of the tree which can be costly
|
|
||||||
float DYNAMICTREEAABBGAP = float(0.1);
|
|
||||||
|
|
||||||
/// In the broad-phase collision detection (dynamic AABB tree), the AABBs are
|
|
||||||
/// also inflated in direction of the linear motion of the body by mutliplying the
|
|
||||||
/// followin ant with the linear velocity and the elapsed time between two frames.
|
|
||||||
float DYNAMICTREEAABBLINGAPMULTIPLIER = float(1.7);
|
|
||||||
|
|
||||||
/// Maximum number of contact manifolds in an overlapping pair that involves two
|
|
||||||
/// convex collision shapes.
|
|
||||||
int NBMAXCONTACTMANIFOLDSCONVEXSHAPE = 1;
|
|
||||||
|
|
||||||
/// Maximum number of contact manifolds in an overlapping pair that involves at
|
|
||||||
/// least one concave collision shape.
|
|
||||||
int NBMAXCONTACTMANIFOLDSCONCAVESHAPE = 3;
|
|
||||||
|
|
||||||
}
|
|
@ -17,41 +17,41 @@ using namespace ephysics;
|
|||||||
|
|
||||||
// Constructor
|
// Constructor
|
||||||
BallAndSocketJoint::BallAndSocketJoint( BallAndSocketJointInfo jointInfo)
|
BallAndSocketJoint::BallAndSocketJoint( BallAndSocketJointInfo jointInfo)
|
||||||
: Joint(jointInfo), this.impulse(vec3(0, 0, 0)) {
|
: Joint(jointInfo), this.impulse(Vector3f(0, 0, 0)) {
|
||||||
|
|
||||||
// Compute the local-space anchor point for each body
|
// Compute the local-space anchor point for each body
|
||||||
this.localAnchorPointBody1 = this.body1->getTransform().getInverse() * jointInfo.this.anchorPointWorldSpace;
|
this.localAnchorPointBody1 = this.body1.getTransform().getInverse() * jointInfo.anchorPointWorldSpace;
|
||||||
this.localAnchorPointBody2 = this.body2->getTransform().getInverse() * jointInfo.this.anchorPointWorldSpace;
|
this.localAnchorPointBody2 = this.body2.getTransform().getInverse() * jointInfo.anchorPointWorldSpace;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Initialize before solving the raint
|
// Initialize before solving the raint
|
||||||
void BallAndSocketJoint::initBeforeSolve( ConstraintSolverData raintSolverData) {
|
void BallAndSocketJoint::initBeforeSolve( ConstraintSolverData raintSolverData) {
|
||||||
|
|
||||||
// Initialize the bodies index in the velocity array
|
// Initialize the bodies index in the velocity array
|
||||||
this.indexBody1 = raintSolverData.mapBodyToConstrainedVelocityIndex.find(this.body1)->second;
|
this.indexBody1 = raintSolverData.mapBodyToConstrainedVelocityIndex.find(this.body1).second;
|
||||||
this.indexBody2 = raintSolverData.mapBodyToConstrainedVelocityIndex.find(this.body2)->second;
|
this.indexBody2 = raintSolverData.mapBodyToConstrainedVelocityIndex.find(this.body2).second;
|
||||||
|
|
||||||
// Get the bodies center of mass and orientations
|
// Get the bodies center of mass and orientations
|
||||||
vec3 x1 = this.body1->this.centerOfMassWorld;
|
Vector3f x1 = this.body1.this.centerOfMassWorld;
|
||||||
vec3 x2 = this.body2->this.centerOfMassWorld;
|
Vector3f x2 = this.body2.this.centerOfMassWorld;
|
||||||
etk::Quaternion orientationBody1 = this.body1->getTransform().getOrientation();
|
Quaternion orientationBody1 = this.body1.getTransform().getOrientation();
|
||||||
etk::Quaternion orientationBody2 = this.body2->getTransform().getOrientation();
|
Quaternion orientationBody2 = this.body2.getTransform().getOrientation();
|
||||||
|
|
||||||
// Get the inertia tensor of bodies
|
// Get the inertia tensor of bodies
|
||||||
this.i1 = this.body1->getInertiaTensorInverseWorld();
|
this.i1 = this.body1.getInertiaTensorInverseWorld();
|
||||||
this.i2 = this.body2->getInertiaTensorInverseWorld();
|
this.i2 = this.body2.getInertiaTensorInverseWorld();
|
||||||
|
|
||||||
// Compute the vector from body center to the anchor point in world-space
|
// Compute the vector from body center to the anchor point in world-space
|
||||||
this.r1World = orientationBody1 * this.localAnchorPointBody1;
|
this.r1World = orientationBody1 * this.localAnchorPointBody1;
|
||||||
this.r2World = orientationBody2 * this.localAnchorPointBody2;
|
this.r2World = orientationBody2 * this.localAnchorPointBody2;
|
||||||
|
|
||||||
// Compute the corresponding skew-symmetric matrices
|
// Compute the corresponding skew-symmetric matrices
|
||||||
etk::Matrix3x3 skewSymmetricMatrixU1= etk::Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(this.r1World);
|
Matrix3f skewSymmetricMatrixU1= Matrix3f::computeSkewSymmetricMatrixForCrossProduct(this.r1World);
|
||||||
etk::Matrix3x3 skewSymmetricMatrixU2= etk::Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(this.r2World);
|
Matrix3f skewSymmetricMatrixU2= Matrix3f::computeSkewSymmetricMatrixForCrossProduct(this.r2World);
|
||||||
|
|
||||||
// Compute the matrix K=JM^-1J^t (3x3 matrix)
|
// Compute the matrix K=JM^-1J^t (3x3 matrix)
|
||||||
float inverseMassBodies = this.body1->this.massInverse + this.body2->this.massInverse;
|
float inverseMassBodies = this.body1.this.massInverse + this.body2.this.massInverse;
|
||||||
etk::Matrix3x3 massMatrix = etk::Matrix3x3(inverseMassBodies, 0, 0,
|
Matrix3f massMatrix = Matrix3f(inverseMassBodies, 0, 0,
|
||||||
0, inverseMassBodies, 0,
|
0, inverseMassBodies, 0,
|
||||||
0, 0, inverseMassBodies) +
|
0, 0, inverseMassBodies) +
|
||||||
skewSymmetricMatrixU1 * this.i1 * skewSymmetricMatrixU1.getTranspose() +
|
skewSymmetricMatrixU1 * this.i1 * skewSymmetricMatrixU1.getTranspose() +
|
||||||
@ -59,7 +59,7 @@ void BallAndSocketJoint::initBeforeSolve( ConstraintSolverData raintSolverData)
|
|||||||
|
|
||||||
// Compute the inverse mass matrix K^-1
|
// Compute the inverse mass matrix K^-1
|
||||||
this.inverseMassMatrix.setZero();
|
this.inverseMassMatrix.setZero();
|
||||||
if (this.body1->getType() == DYNAMIC || this.body2->getType() == DYNAMIC) {
|
if (this.body1.getType() == DYNAMIC || this.body2.getType() == DYNAMIC) {
|
||||||
this.inverseMassMatrix = massMatrix.getInverse();
|
this.inverseMassMatrix = massMatrix.getInverse();
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -82,24 +82,24 @@ void BallAndSocketJoint::initBeforeSolve( ConstraintSolverData raintSolverData)
|
|||||||
void BallAndSocketJoint::warmstart( ConstraintSolverData raintSolverData) {
|
void BallAndSocketJoint::warmstart( ConstraintSolverData raintSolverData) {
|
||||||
|
|
||||||
// Get the velocities
|
// Get the velocities
|
||||||
vec3 v1 = raintSolverData.linearVelocities[this.indexBody1];
|
Vector3f v1 = raintSolverData.linearVelocities[this.indexBody1];
|
||||||
vec3 v2 = raintSolverData.linearVelocities[this.indexBody2];
|
Vector3f v2 = raintSolverData.linearVelocities[this.indexBody2];
|
||||||
vec3 w1 = raintSolverData.angularVelocities[this.indexBody1];
|
Vector3f w1 = raintSolverData.angularVelocities[this.indexBody1];
|
||||||
vec3 w2 = raintSolverData.angularVelocities[this.indexBody2];
|
Vector3f w2 = raintSolverData.angularVelocities[this.indexBody2];
|
||||||
|
|
||||||
// Compute the impulse P=J^T * lambda for the body 1
|
// Compute the impulse P=J^T * lambda for the body 1
|
||||||
vec3 linearImpulseBody1 = -this.impulse;
|
Vector3f linearImpulseBody1 = -this.impulse;
|
||||||
vec3 angularImpulseBody1 = this.impulse.cross(this.r1World);
|
Vector3f angularImpulseBody1 = this.impulse.cross(this.r1World);
|
||||||
|
|
||||||
// Apply the impulse to the body 1
|
// Apply the impulse to the body 1
|
||||||
v1 += this.body1->this.massInverse * linearImpulseBody1;
|
v1 += this.body1.this.massInverse * linearImpulseBody1;
|
||||||
w1 += this.i1 * angularImpulseBody1;
|
w1 += this.i1 * angularImpulseBody1;
|
||||||
|
|
||||||
// Compute the impulse P=J^T * lambda for the body 2
|
// Compute the impulse P=J^T * lambda for the body 2
|
||||||
vec3 angularImpulseBody2 = -this.impulse.cross(this.r2World);
|
Vector3f angularImpulseBody2 = -this.impulse.cross(this.r2World);
|
||||||
|
|
||||||
// Apply the impulse to the body to the body 2
|
// Apply the impulse to the body to the body 2
|
||||||
v2 += this.body2->this.massInverse * this.impulse;
|
v2 += this.body2.this.massInverse * this.impulse;
|
||||||
w2 += this.i2 * angularImpulseBody2;
|
w2 += this.i2 * angularImpulseBody2;
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -107,31 +107,31 @@ void BallAndSocketJoint::warmstart( ConstraintSolverData raintSolverData) {
|
|||||||
void BallAndSocketJoint::solveVelocityConstraint( ConstraintSolverData raintSolverData) {
|
void BallAndSocketJoint::solveVelocityConstraint( ConstraintSolverData raintSolverData) {
|
||||||
|
|
||||||
// Get the velocities
|
// Get the velocities
|
||||||
vec3 v1 = raintSolverData.linearVelocities[this.indexBody1];
|
Vector3f v1 = raintSolverData.linearVelocities[this.indexBody1];
|
||||||
vec3 v2 = raintSolverData.linearVelocities[this.indexBody2];
|
Vector3f v2 = raintSolverData.linearVelocities[this.indexBody2];
|
||||||
vec3 w1 = raintSolverData.angularVelocities[this.indexBody1];
|
Vector3f w1 = raintSolverData.angularVelocities[this.indexBody1];
|
||||||
vec3 w2 = raintSolverData.angularVelocities[this.indexBody2];
|
Vector3f w2 = raintSolverData.angularVelocities[this.indexBody2];
|
||||||
|
|
||||||
// Compute J*v
|
// Compute J*v
|
||||||
vec3 Jv = v2 + w2.cross(this.r2World) - v1 - w1.cross(this.r1World);
|
Vector3f Jv = v2 + w2.cross(this.r2World) - v1 - w1.cross(this.r1World);
|
||||||
|
|
||||||
// Compute the Lagrange multiplier lambda
|
// Compute the Lagrange multiplier lambda
|
||||||
vec3 deltaLambda = this.inverseMassMatrix * (-Jv - this.biasVector);
|
Vector3f deltaLambda = this.inverseMassMatrix * (-Jv - this.biasVector);
|
||||||
this.impulse += deltaLambda;
|
this.impulse += deltaLambda;
|
||||||
|
|
||||||
// Compute the impulse P=J^T * lambda for the body 1
|
// Compute the impulse P=J^T * lambda for the body 1
|
||||||
vec3 linearImpulseBody1 = -deltaLambda;
|
Vector3f linearImpulseBody1 = -deltaLambda;
|
||||||
vec3 angularImpulseBody1 = deltaLambda.cross(this.r1World);
|
Vector3f angularImpulseBody1 = deltaLambda.cross(this.r1World);
|
||||||
|
|
||||||
// Apply the impulse to the body 1
|
// Apply the impulse to the body 1
|
||||||
v1 += this.body1->this.massInverse * linearImpulseBody1;
|
v1 += this.body1.this.massInverse * linearImpulseBody1;
|
||||||
w1 += this.i1 * angularImpulseBody1;
|
w1 += this.i1 * angularImpulseBody1;
|
||||||
|
|
||||||
// Compute the impulse P=J^T * lambda for the body 2
|
// Compute the impulse P=J^T * lambda for the body 2
|
||||||
vec3 angularImpulseBody2 = -deltaLambda.cross(this.r2World);
|
Vector3f angularImpulseBody2 = -deltaLambda.cross(this.r2World);
|
||||||
|
|
||||||
// Apply the impulse to the body 2
|
// Apply the impulse to the body 2
|
||||||
v2 += this.body2->this.massInverse * deltaLambda;
|
v2 += this.body2.this.massInverse * deltaLambda;
|
||||||
w2 += this.i2 * angularImpulseBody2;
|
w2 += this.i2 * angularImpulseBody2;
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -143,70 +143,70 @@ void BallAndSocketJoint::solvePositionConstraint( ConstraintSolverData raintSolv
|
|||||||
if (this.positionCorrectionTechnique != NONLINEARGAUSSSEIDEL) return;
|
if (this.positionCorrectionTechnique != NONLINEARGAUSSSEIDEL) return;
|
||||||
|
|
||||||
// Get the bodies center of mass and orientations
|
// Get the bodies center of mass and orientations
|
||||||
vec3 x1 = raintSolverData.positions[this.indexBody1];
|
Vector3f x1 = raintSolverData.positions[this.indexBody1];
|
||||||
vec3 x2 = raintSolverData.positions[this.indexBody2];
|
Vector3f x2 = raintSolverData.positions[this.indexBody2];
|
||||||
etk::Quaternion q1 = raintSolverData.orientations[this.indexBody1];
|
Quaternion q1 = raintSolverData.orientations[this.indexBody1];
|
||||||
etk::Quaternion q2 = raintSolverData.orientations[this.indexBody2];
|
Quaternion q2 = raintSolverData.orientations[this.indexBody2];
|
||||||
|
|
||||||
// Get the inverse mass and inverse inertia tensors of the bodies
|
// Get the inverse mass and inverse inertia tensors of the bodies
|
||||||
float inverseMassBody1 = this.body1->this.massInverse;
|
float inverseMassBody1 = this.body1.this.massInverse;
|
||||||
float inverseMassBody2 = this.body2->this.massInverse;
|
float inverseMassBody2 = this.body2.this.massInverse;
|
||||||
|
|
||||||
// Recompute the inverse inertia tensors
|
// Recompute the inverse inertia tensors
|
||||||
this.i1 = this.body1->getInertiaTensorInverseWorld();
|
this.i1 = this.body1.getInertiaTensorInverseWorld();
|
||||||
this.i2 = this.body2->getInertiaTensorInverseWorld();
|
this.i2 = this.body2.getInertiaTensorInverseWorld();
|
||||||
|
|
||||||
// Compute the vector from body center to the anchor point in world-space
|
// Compute the vector from body center to the anchor point in world-space
|
||||||
this.r1World = q1 * this.localAnchorPointBody1;
|
this.r1World = q1 * this.localAnchorPointBody1;
|
||||||
this.r2World = q2 * this.localAnchorPointBody2;
|
this.r2World = q2 * this.localAnchorPointBody2;
|
||||||
|
|
||||||
// Compute the corresponding skew-symmetric matrices
|
// Compute the corresponding skew-symmetric matrices
|
||||||
etk::Matrix3x3 skewSymmetricMatrixU1= etk::Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(this.r1World);
|
Matrix3f skewSymmetricMatrixU1= Matrix3f::computeSkewSymmetricMatrixForCrossProduct(this.r1World);
|
||||||
etk::Matrix3x3 skewSymmetricMatrixU2= etk::Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(this.r2World);
|
Matrix3f skewSymmetricMatrixU2= Matrix3f::computeSkewSymmetricMatrixForCrossProduct(this.r2World);
|
||||||
|
|
||||||
// Recompute the inverse mass matrix K=J^TM^-1J of of the 3 translation raints
|
// Recompute the inverse mass matrix K=J^TM^-1J of of the 3 translation raints
|
||||||
float inverseMassBodies = inverseMassBody1 + inverseMassBody2;
|
float inverseMassBodies = inverseMassBody1 + inverseMassBody2;
|
||||||
etk::Matrix3x3 massMatrix = etk::Matrix3x3(inverseMassBodies, 0, 0,
|
Matrix3f massMatrix = Matrix3f(inverseMassBodies, 0, 0,
|
||||||
0, inverseMassBodies, 0,
|
0, inverseMassBodies, 0,
|
||||||
0, 0, inverseMassBodies) +
|
0, 0, inverseMassBodies) +
|
||||||
skewSymmetricMatrixU1 * this.i1 * skewSymmetricMatrixU1.getTranspose() +
|
skewSymmetricMatrixU1 * this.i1 * skewSymmetricMatrixU1.getTranspose() +
|
||||||
skewSymmetricMatrixU2 * this.i2 * skewSymmetricMatrixU2.getTranspose();
|
skewSymmetricMatrixU2 * this.i2 * skewSymmetricMatrixU2.getTranspose();
|
||||||
this.inverseMassMatrix.setZero();
|
this.inverseMassMatrix.setZero();
|
||||||
if (this.body1->getType() == DYNAMIC || this.body2->getType() == DYNAMIC) {
|
if (this.body1.getType() == DYNAMIC || this.body2.getType() == DYNAMIC) {
|
||||||
this.inverseMassMatrix = massMatrix.getInverse();
|
this.inverseMassMatrix = massMatrix.getInverse();
|
||||||
}
|
}
|
||||||
|
|
||||||
// Compute the raint error (value of the C(x) function)
|
// Compute the raint error (value of the C(x) function)
|
||||||
vec3 raintError = (x2 + this.r2World - x1 - this.r1World);
|
Vector3f raintError = (x2 + this.r2World - x1 - this.r1World);
|
||||||
|
|
||||||
// Compute the Lagrange multiplier lambda
|
// Compute the Lagrange multiplier lambda
|
||||||
// TODO : Do not solve the system by computing the inverse each time and multiplying with the
|
// TODO : Do not solve the system by computing the inverse each time and multiplying with the
|
||||||
// right-hand side vector but instead use a method to directly solve the linear system.
|
// right-hand side vector but instead use a method to directly solve the linear system.
|
||||||
vec3 lambda = this.inverseMassMatrix * (-raintError);
|
Vector3f lambda = this.inverseMassMatrix * (-raintError);
|
||||||
|
|
||||||
// Compute the impulse of body 1
|
// Compute the impulse of body 1
|
||||||
vec3 linearImpulseBody1 = -lambda;
|
Vector3f linearImpulseBody1 = -lambda;
|
||||||
vec3 angularImpulseBody1 = lambda.cross(this.r1World);
|
Vector3f angularImpulseBody1 = lambda.cross(this.r1World);
|
||||||
|
|
||||||
// Compute the pseudo velocity of body 1
|
// Compute the pseudo velocity of body 1
|
||||||
vec3 v1 = inverseMassBody1 * linearImpulseBody1;
|
Vector3f v1 = inverseMassBody1 * linearImpulseBody1;
|
||||||
vec3 w1 = this.i1 * angularImpulseBody1;
|
Vector3f w1 = this.i1 * angularImpulseBody1;
|
||||||
|
|
||||||
// Update the body center of mass and orientation of body 1
|
// Update the body center of mass and orientation of body 1
|
||||||
x1 += v1;
|
x1 += v1;
|
||||||
q1 += etk::Quaternion(0, w1) * q1 * 0.5f;
|
q1 += Quaternion(0, w1) * q1 * 0.5f;
|
||||||
q1.normalize();
|
q1.normalize();
|
||||||
|
|
||||||
// Compute the impulse of body 2
|
// Compute the impulse of body 2
|
||||||
vec3 angularImpulseBody2 = -lambda.cross(this.r2World);
|
Vector3f angularImpulseBody2 = -lambda.cross(this.r2World);
|
||||||
|
|
||||||
// Compute the pseudo velocity of body 2
|
// Compute the pseudo velocity of body 2
|
||||||
vec3 v2 = inverseMassBody2 * lambda;
|
Vector3f v2 = inverseMassBody2 * lambda;
|
||||||
vec3 w2 = this.i2 * angularImpulseBody2;
|
Vector3f w2 = this.i2 * angularImpulseBody2;
|
||||||
|
|
||||||
// Update the body position/orientation of body 2
|
// Update the body position/orientation of body 2
|
||||||
x2 += v2;
|
x2 += v2;
|
||||||
q2 += etk::Quaternion(0, w2) * q2 * 0.5f;
|
q2 += Quaternion(0, w2) * q2 * 0.5f;
|
||||||
q2.normalize();
|
q2.normalize();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -1,68 +0,0 @@
|
|||||||
/** @file
|
|
||||||
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
|
|
||||||
* @author Daniel CHAPPUIS
|
|
||||||
* @author Edouard DUPIN
|
|
||||||
* @copyright 2010-2016, Daniel Chappuis
|
|
||||||
* @copyright 2017, Edouard DUPIN
|
|
||||||
* @license MPL v2.0 (see license file)
|
|
||||||
*/
|
|
||||||
#pragma once
|
|
||||||
|
|
||||||
#include <ephysics/raint/Joint.hpp>
|
|
||||||
#include <ephysics/mathematics/mathematics.hpp>
|
|
||||||
|
|
||||||
namespace ephysics {
|
|
||||||
/**
|
|
||||||
* @brief It is used to gather the information needed to create a ball-and-socket
|
|
||||||
* joint. This structure will be used to create the actual ball-and-socket joint.
|
|
||||||
*/
|
|
||||||
struct BallAndSocketJointInfo : public JointInfo {
|
|
||||||
public :
|
|
||||||
vec3 this.anchorPointWorldSpace; //!< Anchor point (in world-space coordinates)
|
|
||||||
/**
|
|
||||||
* @brief Constructor
|
|
||||||
* @param rigidBody1 Pointer to the first body of the joint
|
|
||||||
* @param rigidBody2 Pointer to the second body of the joint
|
|
||||||
* @param initAnchorPointWorldSpace The anchor point in world-space coordinates
|
|
||||||
*/
|
|
||||||
BallAndSocketJointInfo(RigidBody* rigidBody1,
|
|
||||||
RigidBody* rigidBody2,
|
|
||||||
vec3 initAnchorPointWorldSpace):
|
|
||||||
JointInfo(rigidBody1, rigidBody2, BALLSOCKETJOINT),
|
|
||||||
this.anchorPointWorldSpace(initAnchorPointWorldSpace) {
|
|
||||||
|
|
||||||
}
|
|
||||||
};
|
|
||||||
/**
|
|
||||||
* @brief Represents a ball-and-socket joint that allows arbitrary rotation
|
|
||||||
* between two bodies. This joint has three degrees of freedom. It can be used to
|
|
||||||
* create a chain of bodies for instance.
|
|
||||||
*/
|
|
||||||
class BallAndSocketJoint : public Joint {
|
|
||||||
private:
|
|
||||||
static float BETA; //!< Beta value for the bias factor of position correction
|
|
||||||
vec3 this.localAnchorPointBody1; //!< Anchor point of body 1 (in local-space coordinates of body 1)
|
|
||||||
vec3 this.localAnchorPointBody2; //!< Anchor point of body 2 (in local-space coordinates of body 2)
|
|
||||||
vec3 this.r1World; //!< Vector from center of body 2 to anchor point in world-space
|
|
||||||
vec3 this.r2World; //!< Vector from center of body 2 to anchor point in world-space
|
|
||||||
etk::Matrix3x3 this.i1; //!< Inertia tensor of body 1 (in world-space coordinates)
|
|
||||||
etk::Matrix3x3 this.i2; //!< Inertia tensor of body 2 (in world-space coordinates)
|
|
||||||
vec3 this.biasVector; //!< Bias vector for the raint
|
|
||||||
etk::Matrix3x3 this.inverseMassMatrix; //!< Inverse mass matrix K=JM^-1J^-t of the raint
|
|
||||||
vec3 this.impulse; //!< Accumulated impulse
|
|
||||||
/// Private copy-ructor
|
|
||||||
BallAndSocketJoint( BallAndSocketJoint raint);
|
|
||||||
/// Private assignment operator
|
|
||||||
BallAndSocketJoint operator=( BallAndSocketJoint raint);
|
|
||||||
sizet getSizeInBytes() override {
|
|
||||||
return sizeof(BallAndSocketJoint);
|
|
||||||
}
|
|
||||||
void initBeforeSolve( ConstraintSolverData raintSolverData) override;
|
|
||||||
void warmstart( ConstraintSolverData raintSolverData) override;
|
|
||||||
void solveVelocityConstraint( ConstraintSolverData raintSolverData) override;
|
|
||||||
void solvePositionConstraint( ConstraintSolverData raintSolverData) override;
|
|
||||||
public:
|
|
||||||
/// Constructor
|
|
||||||
BallAndSocketJoint( BallAndSocketJointInfo jointInfo);
|
|
||||||
};
|
|
||||||
}
|
|
@ -0,0 +1,52 @@
|
|||||||
|
package org.atriaSoft.ephysics.constraint;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief It is used to gather the information needed to create a ball-and-socket
|
||||||
|
* joint. This structure will be used to create the actual ball-and-socket joint.
|
||||||
|
*/
|
||||||
|
struct BallAndSocketJointInfo extends JointInfo {
|
||||||
|
public :
|
||||||
|
Vector3f anchorPointWorldSpace; //!< Anchor point (in world-space coordinates)
|
||||||
|
/**
|
||||||
|
* @brief Constructor
|
||||||
|
* @param rigidBody1 Pointer to the first body of the joint
|
||||||
|
* @param rigidBody2 Pointer to the second body of the joint
|
||||||
|
* @param initAnchorPointWorldSpace The anchor point in world-space coordinates
|
||||||
|
*/
|
||||||
|
BallAndSocketJointInfo(RigidBody* rigidBody1,
|
||||||
|
RigidBody* rigidBody2,
|
||||||
|
Vector3f initAnchorPointWorldSpace):
|
||||||
|
JointInfo(rigidBody1, rigidBody2, BALLSOCKETJOINT),
|
||||||
|
this.anchorPointWorldSpace(initAnchorPointWorldSpace) {
|
||||||
|
|
||||||
|
}
|
||||||
|
};
|
||||||
|
/**
|
||||||
|
* @brief Represents a ball-and-socket joint that allows arbitrary rotation
|
||||||
|
* between two bodies. This joint has three degrees of freedom. It can be used to
|
||||||
|
* create a chain of bodies for instance.
|
||||||
|
*/
|
||||||
|
class BallAndSocketJoint extends Joint {
|
||||||
|
private:
|
||||||
|
static float BETA; //!< Beta value for the bias factor of position correction
|
||||||
|
Vector3f localAnchorPointBody1; //!< Anchor point of body 1 (in local-space coordinates of body 1)
|
||||||
|
Vector3f localAnchorPointBody2; //!< Anchor point of body 2 (in local-space coordinates of body 2)
|
||||||
|
Vector3f r1World; //!< Vector from center of body 2 to anchor point in world-space
|
||||||
|
Vector3f r2World; //!< Vector from center of body 2 to anchor point in world-space
|
||||||
|
Matrix3f i1; //!< Inertia tensor of body 1 (in world-space coordinates)
|
||||||
|
Matrix3f i2; //!< Inertia tensor of body 2 (in world-space coordinates)
|
||||||
|
Vector3f biasVector; //!< Bias vector for the raint
|
||||||
|
Matrix3f inverseMassMatrix; //!< Inverse mass matrix K=JM^-1J^-t of the raint
|
||||||
|
Vector3f impulse; //!< Accumulated impulse
|
||||||
|
long getSizeInBytes() {
|
||||||
|
return sizeof(BallAndSocketJoint);
|
||||||
|
}
|
||||||
|
void initBeforeSolve( ConstraintSolverData raintSolverData) ;
|
||||||
|
void warmstart( ConstraintSolverData raintSolverData) ;
|
||||||
|
void solveVelocityConstraint( ConstraintSolverData raintSolverData) ;
|
||||||
|
void solvePositionConstraint( ConstraintSolverData raintSolverData) ;
|
||||||
|
public:
|
||||||
|
/// Constructor
|
||||||
|
BallAndSocketJoint( BallAndSocketJointInfo jointInfo);
|
||||||
|
};
|
||||||
|
}
|
@ -14,22 +14,22 @@ using namespace std;
|
|||||||
|
|
||||||
// Constructor
|
// Constructor
|
||||||
ContactPoint::ContactPoint( ContactPointInfo contactInfo):
|
ContactPoint::ContactPoint( ContactPointInfo contactInfo):
|
||||||
this.body1(contactInfo.shape1->getBody()),
|
this.body1(contactInfo.shape1.getBody()),
|
||||||
this.body2(contactInfo.shape2->getBody()),
|
this.body2(contactInfo.shape2.getBody()),
|
||||||
this.normal(contactInfo.normal),
|
this.normal(contactInfo.normal),
|
||||||
this.penetrationDepth(contactInfo.penetrationDepth),
|
this.penetrationDepth(contactInfo.penetrationDepth),
|
||||||
this.localPointOnBody1(contactInfo.localPoint1),
|
this.localPointOnBody1(contactInfo.localPoint1),
|
||||||
this.localPointOnBody2(contactInfo.localPoint2),
|
this.localPointOnBody2(contactInfo.localPoint2),
|
||||||
this.worldPointOnBody1(contactInfo.shape1->getBody()->getTransform() *
|
this.worldPointOnBody1(contactInfo.shape1.getBody().getTransform() *
|
||||||
contactInfo.shape1->getLocalToBodyTransform() *
|
contactInfo.shape1.getLocalToBodyTransform() *
|
||||||
contactInfo.localPoint1),
|
contactInfo.localPoint1),
|
||||||
this.worldPointOnBody2(contactInfo.shape2->getBody()->getTransform() *
|
this.worldPointOnBody2(contactInfo.shape2.getBody().getTransform() *
|
||||||
contactInfo.shape2->getLocalToBodyTransform() *
|
contactInfo.shape2.getLocalToBodyTransform() *
|
||||||
contactInfo.localPoint2),
|
contactInfo.localPoint2),
|
||||||
this.isRestingContact(false) {
|
this.isRestingContact(false) {
|
||||||
|
|
||||||
this.frictionVectors[0] = vec3(0, 0, 0);
|
this.frictionVectors[0] = Vector3f(0, 0, 0);
|
||||||
this.frictionVectors[1] = vec3(0, 0, 0);
|
this.frictionVectors[1] = Vector3f(0, 0, 0);
|
||||||
|
|
||||||
assert(this.penetrationDepth >= 0.0);
|
assert(this.penetrationDepth >= 0.0);
|
||||||
|
|
||||||
@ -51,32 +51,32 @@ CollisionBody* ContactPoint::getBody2() {
|
|||||||
}
|
}
|
||||||
|
|
||||||
// Return the normal vector of the contact
|
// Return the normal vector of the contact
|
||||||
vec3 ContactPoint::getNormal() {
|
Vector3f ContactPoint::getNormal() {
|
||||||
return this.normal;
|
return this.normal;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Set the penetration depth of the contact
|
// Set the penetration depth of the contact
|
||||||
void ContactPoint::setPenetrationDepth(float penetrationDepth) {
|
void ContactPoint::setPenetrationDepth(float penetrationDepth) {
|
||||||
this->this.penetrationDepth = penetrationDepth;
|
this.this.penetrationDepth = penetrationDepth;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Return the contact point on body 1
|
// Return the contact point on body 1
|
||||||
vec3 ContactPoint::getLocalPointOnBody1() {
|
Vector3f ContactPoint::getLocalPointOnBody1() {
|
||||||
return this.localPointOnBody1;
|
return this.localPointOnBody1;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Return the contact point on body 2
|
// Return the contact point on body 2
|
||||||
vec3 ContactPoint::getLocalPointOnBody2() {
|
Vector3f ContactPoint::getLocalPointOnBody2() {
|
||||||
return this.localPointOnBody2;
|
return this.localPointOnBody2;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Return the contact world point on body 1
|
// Return the contact world point on body 1
|
||||||
vec3 ContactPoint::getWorldPointOnBody1() {
|
Vector3f ContactPoint::getWorldPointOnBody1() {
|
||||||
return this.worldPointOnBody1;
|
return this.worldPointOnBody1;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Return the contact world point on body 2
|
// Return the contact world point on body 2
|
||||||
vec3 ContactPoint::getWorldPointOnBody2() {
|
Vector3f ContactPoint::getWorldPointOnBody2() {
|
||||||
return this.worldPointOnBody2;
|
return this.worldPointOnBody2;
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -96,7 +96,7 @@ float ContactPoint::getFrictionImpulse2() {
|
|||||||
}
|
}
|
||||||
|
|
||||||
// Return the cached rolling resistance impulse
|
// Return the cached rolling resistance impulse
|
||||||
vec3 ContactPoint::getRollingResistanceImpulse() {
|
Vector3f ContactPoint::getRollingResistanceImpulse() {
|
||||||
return this.rollingResistanceImpulse;
|
return this.rollingResistanceImpulse;
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -116,17 +116,17 @@ void ContactPoint::setFrictionImpulse2(float impulse) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
// Set the cached rolling resistance impulse
|
// Set the cached rolling resistance impulse
|
||||||
void ContactPoint::setRollingResistanceImpulse( vec3 impulse) {
|
void ContactPoint::setRollingResistanceImpulse( Vector3f impulse) {
|
||||||
this.rollingResistanceImpulse = impulse;
|
this.rollingResistanceImpulse = impulse;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Set the contact world point on body 1
|
// Set the contact world point on body 1
|
||||||
void ContactPoint::setWorldPointOnBody1( vec3 worldPoint) {
|
void ContactPoint::setWorldPointOnBody1( Vector3f worldPoint) {
|
||||||
this.worldPointOnBody1 = worldPoint;
|
this.worldPointOnBody1 = worldPoint;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Set the contact world point on body 2
|
// Set the contact world point on body 2
|
||||||
void ContactPoint::setWorldPointOnBody2( vec3 worldPoint) {
|
void ContactPoint::setWorldPointOnBody2( Vector3f worldPoint) {
|
||||||
this.worldPointOnBody2 = worldPoint;
|
this.worldPointOnBody2 = worldPoint;
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -141,22 +141,22 @@ void ContactPoint::setIsRestingContact(boolean isRestingContact) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
// Get the first friction vector
|
// Get the first friction vector
|
||||||
vec3 ContactPoint::getFrictionVector1() {
|
Vector3f ContactPoint::getFrictionVector1() {
|
||||||
return this.frictionVectors[0];
|
return this.frictionVectors[0];
|
||||||
}
|
}
|
||||||
|
|
||||||
// Set the first friction vector
|
// Set the first friction vector
|
||||||
void ContactPoint::setFrictionVector1( vec3 frictionVector1) {
|
void ContactPoint::setFrictionVector1( Vector3f frictionVector1) {
|
||||||
this.frictionVectors[0] = frictionVector1;
|
this.frictionVectors[0] = frictionVector1;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Get the second friction vector
|
// Get the second friction vector
|
||||||
vec3 ContactPoint::getFrictionvec2() {
|
Vector3f ContactPoint::getFrictionvec2() {
|
||||||
return this.frictionVectors[1];
|
return this.frictionVectors[1];
|
||||||
}
|
}
|
||||||
|
|
||||||
// Set the second friction vector
|
// Set the second friction vector
|
||||||
void ContactPoint::setFrictionvec2( vec3 frictionvec2) {
|
void ContactPoint::setFrictionvec2( Vector3f frictionvec2) {
|
||||||
this.frictionVectors[1] = frictionvec2;
|
this.frictionVectors[1] = frictionvec2;
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -166,6 +166,6 @@ float ContactPoint::getPenetrationDepth() {
|
|||||||
}
|
}
|
||||||
|
|
||||||
// Return the number of bytes used by the contact point
|
// Return the number of bytes used by the contact point
|
||||||
sizet ContactPoint::getSizeInBytes() {
|
long ContactPoint::getSizeInBytes() {
|
||||||
return sizeof(ContactPoint);
|
return sizeof(ContactPoint);
|
||||||
}
|
}
|
||||||
|
@ -1,20 +1,4 @@
|
|||||||
/** @file
|
package org.atriaSoft.ephysics.constraint;
|
||||||
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
|
|
||||||
* @author Daniel CHAPPUIS
|
|
||||||
* @author Edouard DUPIN
|
|
||||||
* @copyright 2010-2016, Daniel Chappuis
|
|
||||||
* @copyright 2017, Edouard DUPIN
|
|
||||||
* @license MPL v2.0 (see license file)
|
|
||||||
*/
|
|
||||||
#pragma once
|
|
||||||
|
|
||||||
#include <ephysics/body/CollisionBody.hpp>
|
|
||||||
#include <ephysics/collision/CollisionShapeInfo.hpp>
|
|
||||||
#include <ephysics/configuration.hpp>
|
|
||||||
#include <ephysics/mathematics/mathematics.hpp>
|
|
||||||
#include <ephysics/configuration.hpp>
|
|
||||||
|
|
||||||
namespace ephysics {
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @brief This structure contains informations about a collision contact
|
* @brief This structure contains informations about a collision contact
|
||||||
@ -28,18 +12,18 @@ namespace ephysics {
|
|||||||
ProxyShape* shape2; //!< Second proxy shape of the contact
|
ProxyShape* shape2; //!< Second proxy shape of the contact
|
||||||
CollisionShape* collisionShape1; //!< First collision shape
|
CollisionShape* collisionShape1; //!< First collision shape
|
||||||
CollisionShape* collisionShape2; //!< Second collision shape
|
CollisionShape* collisionShape2; //!< Second collision shape
|
||||||
vec3 normal; //!< Normalized normal vector of the collision contact in world space
|
Vector3f normal; //!< Normalized normal vector of the collision contact in world space
|
||||||
float penetrationDepth; //!< Penetration depth of the contact
|
float penetrationDepth; //!< Penetration depth of the contact
|
||||||
vec3 localPoint1; //!< Contact point of body 1 in local space of body 1
|
Vector3f localPoint1; //!< Contact point of body 1 in local space of body 1
|
||||||
vec3 localPoint2; //!< Contact point of body 2 in local space of body 2
|
Vector3f localPoint2; //!< Contact point of body 2 in local space of body 2
|
||||||
ContactPointInfo(ProxyShape* proxyShape1,
|
ContactPointInfo(ProxyShape* proxyShape1,
|
||||||
ProxyShape* proxyShape2,
|
ProxyShape* proxyShape2,
|
||||||
CollisionShape* collShape1,
|
CollisionShape* collShape1,
|
||||||
CollisionShape* collShape2,
|
CollisionShape* collShape2,
|
||||||
vec3 normal,
|
Vector3f normal,
|
||||||
float penetrationDepth,
|
float penetrationDepth,
|
||||||
vec3 localPoint1,
|
Vector3f localPoint1,
|
||||||
vec3 localPoint2):
|
Vector3f localPoint2):
|
||||||
shape1(proxyShape1),
|
shape1(proxyShape1),
|
||||||
shape2(proxyShape2),
|
shape2(proxyShape2),
|
||||||
collisionShape1(collShape1),
|
collisionShape1(collShape1),
|
||||||
@ -55,7 +39,7 @@ namespace ephysics {
|
|||||||
shape2(null),
|
shape2(null),
|
||||||
collisionShape1(null),
|
collisionShape1(null),
|
||||||
collisionShape2(null) {
|
collisionShape2(null) {
|
||||||
// TODO: add it for etk::Vector
|
// TODO: add it for Vector
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
@ -65,45 +49,39 @@ namespace ephysics {
|
|||||||
*/
|
*/
|
||||||
class ContactPoint {
|
class ContactPoint {
|
||||||
private :
|
private :
|
||||||
CollisionBody* this.body1; //!< First rigid body of the contact
|
CollisionBody* body1; //!< First rigid body of the contact
|
||||||
CollisionBody* this.body2; //!< Second rigid body of the contact
|
CollisionBody* body2; //!< Second rigid body of the contact
|
||||||
vec3 this.normal; //!< Normalized normal vector of the contact (from body1 toward body2) in world space
|
Vector3f normal; //!< Normalized normal vector of the contact (from body1 toward body2) in world space
|
||||||
float this.penetrationDepth; //!< Penetration depth
|
float penetrationDepth; //!< Penetration depth
|
||||||
vec3 this.localPointOnBody1; //!< Contact point on body 1 in local space of body 1
|
Vector3f localPointOnBody1; //!< Contact point on body 1 in local space of body 1
|
||||||
vec3 this.localPointOnBody2; //!< Contact point on body 2 in local space of body 2
|
Vector3f localPointOnBody2; //!< Contact point on body 2 in local space of body 2
|
||||||
vec3 this.worldPointOnBody1; //!< Contact point on body 1 in world space
|
Vector3f worldPointOnBody1; //!< Contact point on body 1 in world space
|
||||||
vec3 this.worldPointOnBody2; //!< Contact point on body 2 in world space
|
Vector3f worldPointOnBody2; //!< Contact point on body 2 in world space
|
||||||
boolean this.isRestingContact; //!< True if the contact is a resting contact (exists for more than one time step)
|
boolean isRestingContact; //!< True if the contact is a resting contact (exists for more than one time step)
|
||||||
vec3 this.frictionVectors[2]; //!< Two orthogonal vectors that span the tangential friction plane
|
Vector3f frictionVectors[2]; //!< Two orthogonal vectors that span the tangential friction plane
|
||||||
float this.penetrationImpulse; //!< Cached penetration impulse
|
float penetrationImpulse; //!< Cached penetration impulse
|
||||||
float this.frictionImpulse1; //!< Cached first friction impulse
|
float frictionImpulse1; //!< Cached first friction impulse
|
||||||
float this.frictionImpulse2; //!< Cached second friction impulse
|
float frictionImpulse2; //!< Cached second friction impulse
|
||||||
vec3 this.rollingResistanceImpulse; //!< Cached rolling resistance impulse
|
Vector3f rollingResistanceImpulse; //!< Cached rolling resistance impulse
|
||||||
/// Private copy-ructor
|
|
||||||
ContactPoint( ContactPoint contact) = delete;
|
|
||||||
/// Private assignment operator
|
|
||||||
ContactPoint operator=( ContactPoint contact) = delete;
|
|
||||||
public :
|
public :
|
||||||
/// Constructor
|
/// Constructor
|
||||||
ContactPoint( ContactPointInfo contactInfo);
|
ContactPoint( ContactPointInfo contactInfo);
|
||||||
/// Destructor
|
|
||||||
~ContactPoint();
|
|
||||||
/// Return the reference to the body 1
|
/// Return the reference to the body 1
|
||||||
CollisionBody* getBody1() ;
|
CollisionBody* getBody1() ;
|
||||||
/// Return the reference to the body 2
|
/// Return the reference to the body 2
|
||||||
CollisionBody* getBody2() ;
|
CollisionBody* getBody2() ;
|
||||||
/// Return the normal vector of the contact
|
/// Return the normal vector of the contact
|
||||||
vec3 getNormal() ;
|
Vector3f getNormal() ;
|
||||||
/// Set the penetration depth of the contact
|
/// Set the penetration depth of the contact
|
||||||
void setPenetrationDepth(float penetrationDepth);
|
void setPenetrationDepth(float penetrationDepth);
|
||||||
/// Return the contact local point on body 1
|
/// Return the contact local point on body 1
|
||||||
vec3 getLocalPointOnBody1() ;
|
Vector3f getLocalPointOnBody1() ;
|
||||||
/// Return the contact local point on body 2
|
/// Return the contact local point on body 2
|
||||||
vec3 getLocalPointOnBody2() ;
|
Vector3f getLocalPointOnBody2() ;
|
||||||
/// Return the contact world point on body 1
|
/// Return the contact world point on body 1
|
||||||
vec3 getWorldPointOnBody1() ;
|
Vector3f getWorldPointOnBody1() ;
|
||||||
/// Return the contact world point on body 2
|
/// Return the contact world point on body 2
|
||||||
vec3 getWorldPointOnBody2() ;
|
Vector3f getWorldPointOnBody2() ;
|
||||||
/// Return the cached penetration impulse
|
/// Return the cached penetration impulse
|
||||||
float getPenetrationImpulse() ;
|
float getPenetrationImpulse() ;
|
||||||
/// Return the cached first friction impulse
|
/// Return the cached first friction impulse
|
||||||
@ -111,7 +89,7 @@ namespace ephysics {
|
|||||||
/// Return the cached second friction impulse
|
/// Return the cached second friction impulse
|
||||||
float getFrictionImpulse2() ;
|
float getFrictionImpulse2() ;
|
||||||
/// Return the cached rolling resistance impulse
|
/// Return the cached rolling resistance impulse
|
||||||
vec3 getRollingResistanceImpulse() ;
|
Vector3f getRollingResistanceImpulse() ;
|
||||||
/// Set the cached penetration impulse
|
/// Set the cached penetration impulse
|
||||||
void setPenetrationImpulse(float impulse);
|
void setPenetrationImpulse(float impulse);
|
||||||
/// Set the first cached friction impulse
|
/// Set the first cached friction impulse
|
||||||
@ -119,27 +97,27 @@ namespace ephysics {
|
|||||||
/// Set the second cached friction impulse
|
/// Set the second cached friction impulse
|
||||||
void setFrictionImpulse2(float impulse);
|
void setFrictionImpulse2(float impulse);
|
||||||
/// Set the cached rolling resistance impulse
|
/// Set the cached rolling resistance impulse
|
||||||
void setRollingResistanceImpulse( vec3 impulse);
|
void setRollingResistanceImpulse( Vector3f impulse);
|
||||||
/// Set the contact world point on body 1
|
/// Set the contact world point on body 1
|
||||||
void setWorldPointOnBody1( vec3 worldPoint);
|
void setWorldPointOnBody1( Vector3f worldPoint);
|
||||||
/// Set the contact world point on body 2
|
/// Set the contact world point on body 2
|
||||||
void setWorldPointOnBody2( vec3 worldPoint);
|
void setWorldPointOnBody2( Vector3f worldPoint);
|
||||||
/// Return true if the contact is a resting contact
|
/// Return true if the contact is a resting contact
|
||||||
boolean getIsRestingContact() ;
|
boolean getIsRestingContact() ;
|
||||||
/// Set the this.isRestingContact variable
|
/// Set the this.isRestingContact variable
|
||||||
void setIsRestingContact(boolean isRestingContact);
|
void setIsRestingContact(boolean isRestingContact);
|
||||||
/// Get the first friction vector
|
/// Get the first friction vector
|
||||||
vec3 getFrictionVector1() ;
|
Vector3f getFrictionVector1() ;
|
||||||
/// Set the first friction vector
|
/// Set the first friction vector
|
||||||
void setFrictionVector1( vec3 frictionVector1);
|
void setFrictionVector1( Vector3f frictionVector1);
|
||||||
/// Get the second friction vector
|
/// Get the second friction vector
|
||||||
vec3 getFrictionvec2() ;
|
Vector3f getFrictionvec2() ;
|
||||||
/// Set the second friction vector
|
/// Set the second friction vector
|
||||||
void setFrictionvec2( vec3 frictionvec2);
|
void setFrictionvec2( Vector3f frictionvec2);
|
||||||
/// Return the penetration depth
|
/// Return the penetration depth
|
||||||
float getPenetrationDepth() ;
|
float getPenetrationDepth() ;
|
||||||
/// Return the number of bytes used by the contact point
|
/// Return the number of bytes used by the contact point
|
||||||
sizet getSizeInBytes() ;
|
long getSizeInBytes() ;
|
||||||
};
|
};
|
||||||
|
|
||||||
}
|
}
|
@ -19,10 +19,10 @@ FixedJoint::FixedJoint( FixedJointInfo jointInfo)
|
|||||||
: Joint(jointInfo), this.impulseTranslation(0, 0, 0), this.impulseRotation(0, 0, 0) {
|
: Joint(jointInfo), this.impulseTranslation(0, 0, 0), this.impulseRotation(0, 0, 0) {
|
||||||
|
|
||||||
// Compute the local-space anchor point for each body
|
// Compute the local-space anchor point for each body
|
||||||
etk::Transform3D transform1 = this.body1->getTransform();
|
Transform3D transform1 = this.body1.getTransform();
|
||||||
etk::Transform3D transform2 = this.body2->getTransform();
|
Transform3D transform2 = this.body2.getTransform();
|
||||||
this.localAnchorPointBody1 = transform1.getInverse() * jointInfo.this.anchorPointWorldSpace;
|
this.localAnchorPointBody1 = transform1.getInverse() * jointInfo.anchorPointWorldSpace;
|
||||||
this.localAnchorPointBody2 = transform2.getInverse() * jointInfo.this.anchorPointWorldSpace;
|
this.localAnchorPointBody2 = transform2.getInverse() * jointInfo.anchorPointWorldSpace;
|
||||||
|
|
||||||
// Compute the inverse of the initial orientation difference between the two bodies
|
// Compute the inverse of the initial orientation difference between the two bodies
|
||||||
this.initOrientationDifferenceInv = transform2.getOrientation() *
|
this.initOrientationDifferenceInv = transform2.getOrientation() *
|
||||||
@ -40,30 +40,30 @@ FixedJoint::~FixedJoint() {
|
|||||||
void FixedJoint::initBeforeSolve( ConstraintSolverData raintSolverData) {
|
void FixedJoint::initBeforeSolve( ConstraintSolverData raintSolverData) {
|
||||||
|
|
||||||
// Initialize the bodies index in the velocity array
|
// Initialize the bodies index in the velocity array
|
||||||
this.indexBody1 = raintSolverData.mapBodyToConstrainedVelocityIndex.find(this.body1)->second;
|
this.indexBody1 = raintSolverData.mapBodyToConstrainedVelocityIndex.find(this.body1).second;
|
||||||
this.indexBody2 = raintSolverData.mapBodyToConstrainedVelocityIndex.find(this.body2)->second;
|
this.indexBody2 = raintSolverData.mapBodyToConstrainedVelocityIndex.find(this.body2).second;
|
||||||
|
|
||||||
// Get the bodies positions and orientations
|
// Get the bodies positions and orientations
|
||||||
vec3 x1 = this.body1->this.centerOfMassWorld;
|
Vector3f x1 = this.body1.this.centerOfMassWorld;
|
||||||
vec3 x2 = this.body2->this.centerOfMassWorld;
|
Vector3f x2 = this.body2.this.centerOfMassWorld;
|
||||||
etk::Quaternion orientationBody1 = this.body1->getTransform().getOrientation();
|
Quaternion orientationBody1 = this.body1.getTransform().getOrientation();
|
||||||
etk::Quaternion orientationBody2 = this.body2->getTransform().getOrientation();
|
Quaternion orientationBody2 = this.body2.getTransform().getOrientation();
|
||||||
|
|
||||||
// Get the inertia tensor of bodies
|
// Get the inertia tensor of bodies
|
||||||
this.i1 = this.body1->getInertiaTensorInverseWorld();
|
this.i1 = this.body1.getInertiaTensorInverseWorld();
|
||||||
this.i2 = this.body2->getInertiaTensorInverseWorld();
|
this.i2 = this.body2.getInertiaTensorInverseWorld();
|
||||||
|
|
||||||
// Compute the vector from body center to the anchor point in world-space
|
// Compute the vector from body center to the anchor point in world-space
|
||||||
this.r1World = orientationBody1 * this.localAnchorPointBody1;
|
this.r1World = orientationBody1 * this.localAnchorPointBody1;
|
||||||
this.r2World = orientationBody2 * this.localAnchorPointBody2;
|
this.r2World = orientationBody2 * this.localAnchorPointBody2;
|
||||||
|
|
||||||
// Compute the corresponding skew-symmetric matrices
|
// Compute the corresponding skew-symmetric matrices
|
||||||
etk::Matrix3x3 skewSymmetricMatrixU1= etk::Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(this.r1World);
|
Matrix3f skewSymmetricMatrixU1= Matrix3f::computeSkewSymmetricMatrixForCrossProduct(this.r1World);
|
||||||
etk::Matrix3x3 skewSymmetricMatrixU2= etk::Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(this.r2World);
|
Matrix3f skewSymmetricMatrixU2= Matrix3f::computeSkewSymmetricMatrixForCrossProduct(this.r2World);
|
||||||
|
|
||||||
// Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation raints
|
// Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation raints
|
||||||
float inverseMassBodies = this.body1->this.massInverse + this.body2->this.massInverse;
|
float inverseMassBodies = this.body1.this.massInverse + this.body2.this.massInverse;
|
||||||
etk::Matrix3x3 massMatrix = etk::Matrix3x3(inverseMassBodies, 0, 0,
|
Matrix3f massMatrix = Matrix3f(inverseMassBodies, 0, 0,
|
||||||
0, inverseMassBodies, 0,
|
0, inverseMassBodies, 0,
|
||||||
0, 0, inverseMassBodies)
|
0, 0, inverseMassBodies)
|
||||||
+ skewSymmetricMatrixU1 * this.i1 * skewSymmetricMatrixU1.getTranspose()
|
+ skewSymmetricMatrixU1 * this.i1 * skewSymmetricMatrixU1.getTranspose()
|
||||||
@ -71,7 +71,7 @@ void FixedJoint::initBeforeSolve( ConstraintSolverData raintSolverData) {
|
|||||||
|
|
||||||
// Compute the inverse mass matrix K^-1 for the 3 translation raints
|
// Compute the inverse mass matrix K^-1 for the 3 translation raints
|
||||||
this.inverseMassMatrixTranslation.setZero();
|
this.inverseMassMatrixTranslation.setZero();
|
||||||
if (this.body1->getType() == DYNAMIC || this.body2->getType() == DYNAMIC) {
|
if (this.body1.getType() == DYNAMIC || this.body2.getType() == DYNAMIC) {
|
||||||
this.inverseMassMatrixTranslation = massMatrix.getInverse();
|
this.inverseMassMatrixTranslation = massMatrix.getInverse();
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -85,16 +85,16 @@ void FixedJoint::initBeforeSolve( ConstraintSolverData raintSolverData) {
|
|||||||
// Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation
|
// Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation
|
||||||
// contraints (3x3 matrix)
|
// contraints (3x3 matrix)
|
||||||
this.inverseMassMatrixRotation = this.i1 + this.i2;
|
this.inverseMassMatrixRotation = this.i1 + this.i2;
|
||||||
if (this.body1->getType() == DYNAMIC || this.body2->getType() == DYNAMIC) {
|
if (this.body1.getType() == DYNAMIC || this.body2.getType() == DYNAMIC) {
|
||||||
this.inverseMassMatrixRotation = this.inverseMassMatrixRotation.getInverse();
|
this.inverseMassMatrixRotation = this.inverseMassMatrixRotation.getInverse();
|
||||||
}
|
}
|
||||||
|
|
||||||
// Compute the bias "b" for the 3 rotation raints
|
// Compute the bias "b" for the 3 rotation raints
|
||||||
this.biasRotation.setZero();
|
this.biasRotation.setZero();
|
||||||
if (this.positionCorrectionTechnique == BAUMGARTEJOINTS) {
|
if (this.positionCorrectionTechnique == BAUMGARTEJOINTS) {
|
||||||
etk::Quaternion currentOrientationDifference = orientationBody2 * orientationBody1.getInverse();
|
Quaternion currentOrientationDifference = orientationBody2 * orientationBody1.getInverse();
|
||||||
currentOrientationDifference.normalize();
|
currentOrientationDifference.normalize();
|
||||||
etk::Quaternion qError = currentOrientationDifference * this.initOrientationDifferenceInv;
|
Quaternion qError = currentOrientationDifference * this.initOrientationDifferenceInv;
|
||||||
this.biasRotation = biasFactor * float(2.0) * qError.getVectorV();
|
this.biasRotation = biasFactor * float(2.0) * qError.getVectorV();
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -111,18 +111,18 @@ void FixedJoint::initBeforeSolve( ConstraintSolverData raintSolverData) {
|
|||||||
void FixedJoint::warmstart( ConstraintSolverData raintSolverData) {
|
void FixedJoint::warmstart( ConstraintSolverData raintSolverData) {
|
||||||
|
|
||||||
// Get the velocities
|
// Get the velocities
|
||||||
vec3 v1 = raintSolverData.linearVelocities[this.indexBody1];
|
Vector3f v1 = raintSolverData.linearVelocities[this.indexBody1];
|
||||||
vec3 v2 = raintSolverData.linearVelocities[this.indexBody2];
|
Vector3f v2 = raintSolverData.linearVelocities[this.indexBody2];
|
||||||
vec3 w1 = raintSolverData.angularVelocities[this.indexBody1];
|
Vector3f w1 = raintSolverData.angularVelocities[this.indexBody1];
|
||||||
vec3 w2 = raintSolverData.angularVelocities[this.indexBody2];
|
Vector3f w2 = raintSolverData.angularVelocities[this.indexBody2];
|
||||||
|
|
||||||
// Get the inverse mass of the bodies
|
// Get the inverse mass of the bodies
|
||||||
float inverseMassBody1 = this.body1->this.massInverse;
|
float inverseMassBody1 = this.body1.this.massInverse;
|
||||||
float inverseMassBody2 = this.body2->this.massInverse;
|
float inverseMassBody2 = this.body2.this.massInverse;
|
||||||
|
|
||||||
// Compute the impulse P=J^T * lambda for the 3 translation raints for body 1
|
// Compute the impulse P=J^T * lambda for the 3 translation raints for body 1
|
||||||
vec3 linearImpulseBody1 = -this.impulseTranslation;
|
Vector3f linearImpulseBody1 = -this.impulseTranslation;
|
||||||
vec3 angularImpulseBody1 = this.impulseTranslation.cross(this.r1World);
|
Vector3f angularImpulseBody1 = this.impulseTranslation.cross(this.r1World);
|
||||||
|
|
||||||
// Compute the impulse P=J^T * lambda for the 3 rotation raints for body 1
|
// Compute the impulse P=J^T * lambda for the 3 rotation raints for body 1
|
||||||
angularImpulseBody1 += -this.impulseRotation;
|
angularImpulseBody1 += -this.impulseRotation;
|
||||||
@ -132,7 +132,7 @@ void FixedJoint::warmstart( ConstraintSolverData raintSolverData) {
|
|||||||
w1 += this.i1 * angularImpulseBody1;
|
w1 += this.i1 * angularImpulseBody1;
|
||||||
|
|
||||||
// Compute the impulse P=J^T * lambda for the 3 translation raints for body 2
|
// Compute the impulse P=J^T * lambda for the 3 translation raints for body 2
|
||||||
vec3 angularImpulseBody2 = -this.impulseTranslation.cross(this.r2World);
|
Vector3f angularImpulseBody2 = -this.impulseTranslation.cross(this.r2World);
|
||||||
|
|
||||||
// Compute the impulse P=J^T * lambda for the 3 rotation raints for body 2
|
// Compute the impulse P=J^T * lambda for the 3 rotation raints for body 2
|
||||||
angularImpulseBody2 += this.impulseRotation;
|
angularImpulseBody2 += this.impulseRotation;
|
||||||
@ -146,35 +146,35 @@ void FixedJoint::warmstart( ConstraintSolverData raintSolverData) {
|
|||||||
void FixedJoint::solveVelocityConstraint( ConstraintSolverData raintSolverData) {
|
void FixedJoint::solveVelocityConstraint( ConstraintSolverData raintSolverData) {
|
||||||
|
|
||||||
// Get the velocities
|
// Get the velocities
|
||||||
vec3 v1 = raintSolverData.linearVelocities[this.indexBody1];
|
Vector3f v1 = raintSolverData.linearVelocities[this.indexBody1];
|
||||||
vec3 v2 = raintSolverData.linearVelocities[this.indexBody2];
|
Vector3f v2 = raintSolverData.linearVelocities[this.indexBody2];
|
||||||
vec3 w1 = raintSolverData.angularVelocities[this.indexBody1];
|
Vector3f w1 = raintSolverData.angularVelocities[this.indexBody1];
|
||||||
vec3 w2 = raintSolverData.angularVelocities[this.indexBody2];
|
Vector3f w2 = raintSolverData.angularVelocities[this.indexBody2];
|
||||||
|
|
||||||
// Get the inverse mass of the bodies
|
// Get the inverse mass of the bodies
|
||||||
float inverseMassBody1 = this.body1->this.massInverse;
|
float inverseMassBody1 = this.body1.this.massInverse;
|
||||||
float inverseMassBody2 = this.body2->this.massInverse;
|
float inverseMassBody2 = this.body2.this.massInverse;
|
||||||
|
|
||||||
// --------------- Translation Constraints --------------- //
|
// --------------- Translation Constraints --------------- //
|
||||||
|
|
||||||
// Compute J*v for the 3 translation raints
|
// Compute J*v for the 3 translation raints
|
||||||
vec3 JvTranslation = v2 + w2.cross(this.r2World) - v1 - w1.cross(this.r1World);
|
Vector3f JvTranslation = v2 + w2.cross(this.r2World) - v1 - w1.cross(this.r1World);
|
||||||
|
|
||||||
// Compute the Lagrange multiplier lambda
|
// Compute the Lagrange multiplier lambda
|
||||||
vec3 deltaLambda = this.inverseMassMatrixTranslation *
|
Vector3f deltaLambda = this.inverseMassMatrixTranslation *
|
||||||
(-JvTranslation - this.biasTranslation);
|
(-JvTranslation - this.biasTranslation);
|
||||||
this.impulseTranslation += deltaLambda;
|
this.impulseTranslation += deltaLambda;
|
||||||
|
|
||||||
// Compute the impulse P=J^T * lambda for body 1
|
// Compute the impulse P=J^T * lambda for body 1
|
||||||
vec3 linearImpulseBody1 = -deltaLambda;
|
Vector3f linearImpulseBody1 = -deltaLambda;
|
||||||
vec3 angularImpulseBody1 = deltaLambda.cross(this.r1World);
|
Vector3f angularImpulseBody1 = deltaLambda.cross(this.r1World);
|
||||||
|
|
||||||
// Apply the impulse to the body 1
|
// Apply the impulse to the body 1
|
||||||
v1 += inverseMassBody1 * linearImpulseBody1;
|
v1 += inverseMassBody1 * linearImpulseBody1;
|
||||||
w1 += this.i1 * angularImpulseBody1;
|
w1 += this.i1 * angularImpulseBody1;
|
||||||
|
|
||||||
// Compute the impulse P=J^T * lambda for body 2
|
// Compute the impulse P=J^T * lambda for body 2
|
||||||
vec3 angularImpulseBody2 = -deltaLambda.cross(this.r2World);
|
Vector3f angularImpulseBody2 = -deltaLambda.cross(this.r2World);
|
||||||
|
|
||||||
// Apply the impulse to the body 2
|
// Apply the impulse to the body 2
|
||||||
v2 += inverseMassBody2 * deltaLambda;
|
v2 += inverseMassBody2 * deltaLambda;
|
||||||
@ -183,10 +183,10 @@ void FixedJoint::solveVelocityConstraint( ConstraintSolverData raintSolverData)
|
|||||||
// --------------- Rotation Constraints --------------- //
|
// --------------- Rotation Constraints --------------- //
|
||||||
|
|
||||||
// Compute J*v for the 3 rotation raints
|
// Compute J*v for the 3 rotation raints
|
||||||
vec3 JvRotation = w2 - w1;
|
Vector3f JvRotation = w2 - w1;
|
||||||
|
|
||||||
// Compute the Lagrange multiplier lambda for the 3 rotation raints
|
// Compute the Lagrange multiplier lambda for the 3 rotation raints
|
||||||
vec3 deltaLambda2 = this.inverseMassMatrixRotation * (-JvRotation - this.biasRotation);
|
Vector3f deltaLambda2 = this.inverseMassMatrixRotation * (-JvRotation - this.biasRotation);
|
||||||
this.impulseRotation += deltaLambda2;
|
this.impulseRotation += deltaLambda2;
|
||||||
|
|
||||||
// Compute the impulse P=J^T * lambda for the 3 rotation raints for body 1
|
// Compute the impulse P=J^T * lambda for the 3 rotation raints for body 1
|
||||||
@ -207,70 +207,70 @@ void FixedJoint::solvePositionConstraint( ConstraintSolverData raintSolverData)
|
|||||||
if (this.positionCorrectionTechnique != NONLINEARGAUSSSEIDEL) return;
|
if (this.positionCorrectionTechnique != NONLINEARGAUSSSEIDEL) return;
|
||||||
|
|
||||||
// Get the bodies positions and orientations
|
// Get the bodies positions and orientations
|
||||||
vec3 x1 = raintSolverData.positions[this.indexBody1];
|
Vector3f x1 = raintSolverData.positions[this.indexBody1];
|
||||||
vec3 x2 = raintSolverData.positions[this.indexBody2];
|
Vector3f x2 = raintSolverData.positions[this.indexBody2];
|
||||||
etk::Quaternion q1 = raintSolverData.orientations[this.indexBody1];
|
Quaternion q1 = raintSolverData.orientations[this.indexBody1];
|
||||||
etk::Quaternion q2 = raintSolverData.orientations[this.indexBody2];
|
Quaternion q2 = raintSolverData.orientations[this.indexBody2];
|
||||||
|
|
||||||
// Get the inverse mass and inverse inertia tensors of the bodies
|
// Get the inverse mass and inverse inertia tensors of the bodies
|
||||||
float inverseMassBody1 = this.body1->this.massInverse;
|
float inverseMassBody1 = this.body1.this.massInverse;
|
||||||
float inverseMassBody2 = this.body2->this.massInverse;
|
float inverseMassBody2 = this.body2.this.massInverse;
|
||||||
|
|
||||||
// Recompute the inverse inertia tensors
|
// Recompute the inverse inertia tensors
|
||||||
this.i1 = this.body1->getInertiaTensorInverseWorld();
|
this.i1 = this.body1.getInertiaTensorInverseWorld();
|
||||||
this.i2 = this.body2->getInertiaTensorInverseWorld();
|
this.i2 = this.body2.getInertiaTensorInverseWorld();
|
||||||
|
|
||||||
// Compute the vector from body center to the anchor point in world-space
|
// Compute the vector from body center to the anchor point in world-space
|
||||||
this.r1World = q1 * this.localAnchorPointBody1;
|
this.r1World = q1 * this.localAnchorPointBody1;
|
||||||
this.r2World = q2 * this.localAnchorPointBody2;
|
this.r2World = q2 * this.localAnchorPointBody2;
|
||||||
|
|
||||||
// Compute the corresponding skew-symmetric matrices
|
// Compute the corresponding skew-symmetric matrices
|
||||||
etk::Matrix3x3 skewSymmetricMatrixU1= etk::Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(this.r1World);
|
Matrix3f skewSymmetricMatrixU1= Matrix3f::computeSkewSymmetricMatrixForCrossProduct(this.r1World);
|
||||||
etk::Matrix3x3 skewSymmetricMatrixU2= etk::Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(this.r2World);
|
Matrix3f skewSymmetricMatrixU2= Matrix3f::computeSkewSymmetricMatrixForCrossProduct(this.r2World);
|
||||||
|
|
||||||
// --------------- Translation Constraints --------------- //
|
// --------------- Translation Constraints --------------- //
|
||||||
|
|
||||||
// Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation raints
|
// Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation raints
|
||||||
float inverseMassBodies = this.body1->this.massInverse + this.body2->this.massInverse;
|
float inverseMassBodies = this.body1.this.massInverse + this.body2.this.massInverse;
|
||||||
etk::Matrix3x3 massMatrix = etk::Matrix3x3(inverseMassBodies, 0, 0,
|
Matrix3f massMatrix = Matrix3f(inverseMassBodies, 0, 0,
|
||||||
0, inverseMassBodies, 0,
|
0, inverseMassBodies, 0,
|
||||||
0, 0, inverseMassBodies)
|
0, 0, inverseMassBodies)
|
||||||
+ skewSymmetricMatrixU1 * this.i1 * skewSymmetricMatrixU1.getTranspose()
|
+ skewSymmetricMatrixU1 * this.i1 * skewSymmetricMatrixU1.getTranspose()
|
||||||
+ skewSymmetricMatrixU2 * this.i2 * skewSymmetricMatrixU2.getTranspose();
|
+ skewSymmetricMatrixU2 * this.i2 * skewSymmetricMatrixU2.getTranspose();
|
||||||
this.inverseMassMatrixTranslation.setZero();
|
this.inverseMassMatrixTranslation.setZero();
|
||||||
if (this.body1->getType() == DYNAMIC || this.body2->getType() == DYNAMIC) {
|
if (this.body1.getType() == DYNAMIC || this.body2.getType() == DYNAMIC) {
|
||||||
this.inverseMassMatrixTranslation = massMatrix.getInverse();
|
this.inverseMassMatrixTranslation = massMatrix.getInverse();
|
||||||
}
|
}
|
||||||
|
|
||||||
// Compute position error for the 3 translation raints
|
// Compute position error for the 3 translation raints
|
||||||
vec3 errorTranslation = x2 + this.r2World - x1 - this.r1World;
|
Vector3f errorTranslation = x2 + this.r2World - x1 - this.r1World;
|
||||||
|
|
||||||
// Compute the Lagrange multiplier lambda
|
// Compute the Lagrange multiplier lambda
|
||||||
vec3 lambdaTranslation = this.inverseMassMatrixTranslation * (-errorTranslation);
|
Vector3f lambdaTranslation = this.inverseMassMatrixTranslation * (-errorTranslation);
|
||||||
|
|
||||||
// Compute the impulse of body 1
|
// Compute the impulse of body 1
|
||||||
vec3 linearImpulseBody1 = -lambdaTranslation;
|
Vector3f linearImpulseBody1 = -lambdaTranslation;
|
||||||
vec3 angularImpulseBody1 = lambdaTranslation.cross(this.r1World);
|
Vector3f angularImpulseBody1 = lambdaTranslation.cross(this.r1World);
|
||||||
|
|
||||||
// Compute the pseudo velocity of body 1
|
// Compute the pseudo velocity of body 1
|
||||||
vec3 v1 = inverseMassBody1 * linearImpulseBody1;
|
Vector3f v1 = inverseMassBody1 * linearImpulseBody1;
|
||||||
vec3 w1 = this.i1 * angularImpulseBody1;
|
Vector3f w1 = this.i1 * angularImpulseBody1;
|
||||||
|
|
||||||
// Update the body position/orientation of body 1
|
// Update the body position/orientation of body 1
|
||||||
x1 += v1;
|
x1 += v1;
|
||||||
q1 += etk::Quaternion(0, w1) * q1 * 0.5f;
|
q1 += Quaternion(0, w1) * q1 * 0.5f;
|
||||||
q1.normalize();
|
q1.normalize();
|
||||||
|
|
||||||
// Compute the impulse of body 2
|
// Compute the impulse of body 2
|
||||||
vec3 angularImpulseBody2 = -lambdaTranslation.cross(this.r2World);
|
Vector3f angularImpulseBody2 = -lambdaTranslation.cross(this.r2World);
|
||||||
|
|
||||||
// Compute the pseudo velocity of body 2
|
// Compute the pseudo velocity of body 2
|
||||||
vec3 v2 = inverseMassBody2 * lambdaTranslation;
|
Vector3f v2 = inverseMassBody2 * lambdaTranslation;
|
||||||
vec3 w2 = this.i2 * angularImpulseBody2;
|
Vector3f w2 = this.i2 * angularImpulseBody2;
|
||||||
|
|
||||||
// Update the body position/orientation of body 2
|
// Update the body position/orientation of body 2
|
||||||
x2 += v2;
|
x2 += v2;
|
||||||
q2 += etk::Quaternion(0, w2) * q2 * 0.5f;
|
q2 += Quaternion(0, w2) * q2 * 0.5f;
|
||||||
q2.normalize();
|
q2.normalize();
|
||||||
|
|
||||||
// --------------- Rotation Constraints --------------- //
|
// --------------- Rotation Constraints --------------- //
|
||||||
@ -278,18 +278,18 @@ void FixedJoint::solvePositionConstraint( ConstraintSolverData raintSolverData)
|
|||||||
// Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation
|
// Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation
|
||||||
// contraints (3x3 matrix)
|
// contraints (3x3 matrix)
|
||||||
this.inverseMassMatrixRotation = this.i1 + this.i2;
|
this.inverseMassMatrixRotation = this.i1 + this.i2;
|
||||||
if (this.body1->getType() == DYNAMIC || this.body2->getType() == DYNAMIC) {
|
if (this.body1.getType() == DYNAMIC || this.body2.getType() == DYNAMIC) {
|
||||||
this.inverseMassMatrixRotation = this.inverseMassMatrixRotation.getInverse();
|
this.inverseMassMatrixRotation = this.inverseMassMatrixRotation.getInverse();
|
||||||
}
|
}
|
||||||
|
|
||||||
// Compute the position error for the 3 rotation raints
|
// Compute the position error for the 3 rotation raints
|
||||||
etk::Quaternion currentOrientationDifference = q2 * q1.getInverse();
|
Quaternion currentOrientationDifference = q2 * q1.getInverse();
|
||||||
currentOrientationDifference.normalize();
|
currentOrientationDifference.normalize();
|
||||||
etk::Quaternion qError = currentOrientationDifference * this.initOrientationDifferenceInv;
|
Quaternion qError = currentOrientationDifference * this.initOrientationDifferenceInv;
|
||||||
vec3 errorRotation = float(2.0) * qError.getVectorV();
|
Vector3f errorRotation = float(2.0) * qError.getVectorV();
|
||||||
|
|
||||||
// Compute the Lagrange multiplier lambda for the 3 rotation raints
|
// Compute the Lagrange multiplier lambda for the 3 rotation raints
|
||||||
vec3 lambdaRotation = this.inverseMassMatrixRotation * (-errorRotation);
|
Vector3f lambdaRotation = this.inverseMassMatrixRotation * (-errorRotation);
|
||||||
|
|
||||||
// Compute the impulse P=J^T * lambda for the 3 rotation raints of body 1
|
// Compute the impulse P=J^T * lambda for the 3 rotation raints of body 1
|
||||||
angularImpulseBody1 = -lambdaRotation;
|
angularImpulseBody1 = -lambdaRotation;
|
||||||
@ -298,14 +298,14 @@ void FixedJoint::solvePositionConstraint( ConstraintSolverData raintSolverData)
|
|||||||
w1 = this.i1 * angularImpulseBody1;
|
w1 = this.i1 * angularImpulseBody1;
|
||||||
|
|
||||||
// Update the body position/orientation of body 1
|
// Update the body position/orientation of body 1
|
||||||
q1 += etk::Quaternion(0, w1) * q1 * 0.5f;
|
q1 += Quaternion(0, w1) * q1 * 0.5f;
|
||||||
q1.normalize();
|
q1.normalize();
|
||||||
|
|
||||||
// Compute the pseudo velocity of body 2
|
// Compute the pseudo velocity of body 2
|
||||||
w2 = this.i2 * lambdaRotation;
|
w2 = this.i2 * lambdaRotation;
|
||||||
|
|
||||||
// Update the body position/orientation of body 2
|
// Update the body position/orientation of body 2
|
||||||
q2 += etk::Quaternion(0, w2) * q2 * 0.5f;
|
q2 += Quaternion(0, w2) * q2 * 0.5f;
|
||||||
q2.normalize();
|
q2.normalize();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -1,78 +0,0 @@
|
|||||||
/** @file
|
|
||||||
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
|
|
||||||
* @author Daniel CHAPPUIS
|
|
||||||
* @author Edouard DUPIN
|
|
||||||
* @copyright 2010-2016, Daniel Chappuis
|
|
||||||
* @copyright 2017, Edouard DUPIN
|
|
||||||
* @license MPL v2.0 (see license file)
|
|
||||||
*/
|
|
||||||
#pragma once
|
|
||||||
|
|
||||||
// Libraries
|
|
||||||
#include <ephysics/raint/Joint.hpp>
|
|
||||||
#include <ephysics/mathematics/mathematics.hpp>
|
|
||||||
|
|
||||||
namespace ephysics {
|
|
||||||
|
|
||||||
/**
|
|
||||||
* This structure is used to gather the information needed to create a fixed
|
|
||||||
* joint. This structure will be used to create the actual fixed joint.
|
|
||||||
*/
|
|
||||||
struct FixedJointInfo : public JointInfo {
|
|
||||||
public :
|
|
||||||
vec3 this.anchorPointWorldSpace; //!< Anchor point (in world-space coordinates)
|
|
||||||
/**
|
|
||||||
* @breif Contructor
|
|
||||||
* @param rigidBody1 The first body of the joint
|
|
||||||
* @param rigidBody2 The second body of the joint
|
|
||||||
* @param initAnchorPointWorldSpace The initial anchor point of the joint in world-space coordinates
|
|
||||||
*/
|
|
||||||
FixedJointInfo(RigidBody* rigidBody1,
|
|
||||||
RigidBody* rigidBody2,
|
|
||||||
vec3 initAnchorPointWorldSpace):
|
|
||||||
JointInfo(rigidBody1, rigidBody2, FIXEDJOINT),
|
|
||||||
this.anchorPointWorldSpace(initAnchorPointWorldSpace){
|
|
||||||
|
|
||||||
}
|
|
||||||
};
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @breif It represents a fixed joint that is used to forbid any translation or rotation
|
|
||||||
* between two bodies.
|
|
||||||
*/
|
|
||||||
class FixedJoint : public Joint {
|
|
||||||
private:
|
|
||||||
static float BETA; //!< Beta value for the bias factor of position correction
|
|
||||||
vec3 this.localAnchorPointBody1; //!< Anchor point of body 1 (in local-space coordinates of body 1)
|
|
||||||
vec3 this.localAnchorPointBody2; //!< Anchor point of body 2 (in local-space coordinates of body 2)
|
|
||||||
vec3 this.r1World; //!< Vector from center of body 2 to anchor point in world-space
|
|
||||||
vec3 this.r2World; //!< Vector from center of body 2 to anchor point in world-space
|
|
||||||
etk::Matrix3x3 this.i1; //!< Inertia tensor of body 1 (in world-space coordinates)
|
|
||||||
etk::Matrix3x3 this.i2; //!< Inertia tensor of body 2 (in world-space coordinates)
|
|
||||||
vec3 this.impulseTranslation; //!< Accumulated impulse for the 3 translation raints
|
|
||||||
vec3 this.impulseRotation; //!< Accumulate impulse for the 3 rotation raints
|
|
||||||
etk::Matrix3x3 this.inverseMassMatrixTranslation; //!< Inverse mass matrix K=JM^-1J^-t of the 3 translation raints (3x3 matrix)
|
|
||||||
etk::Matrix3x3 this.inverseMassMatrixRotation; //!< Inverse mass matrix K=JM^-1J^-t of the 3 rotation raints (3x3 matrix)
|
|
||||||
vec3 this.biasTranslation; //!< Bias vector for the 3 translation raints
|
|
||||||
vec3 this.biasRotation; //!< Bias vector for the 3 rotation raints
|
|
||||||
etk::Quaternion this.initOrientationDifferenceInv; //!< Inverse of the initial orientation difference between the two bodies
|
|
||||||
/// Private copy-ructor
|
|
||||||
FixedJoint( FixedJoint raint) = delete;
|
|
||||||
/// Private assignment operator
|
|
||||||
FixedJoint operator=( FixedJoint raint) = delete;
|
|
||||||
|
|
||||||
sizet getSizeInBytes() override {
|
|
||||||
return sizeof(FixedJoint);
|
|
||||||
}
|
|
||||||
void initBeforeSolve( ConstraintSolverData raintSolverData) override;
|
|
||||||
void warmstart( ConstraintSolverData raintSolverData) override;
|
|
||||||
void solveVelocityConstraint( ConstraintSolverData raintSolverData) override;
|
|
||||||
void solvePositionConstraint( ConstraintSolverData raintSolverData) override;
|
|
||||||
public:
|
|
||||||
/// Constructor
|
|
||||||
FixedJoint( FixedJointInfo jointInfo);
|
|
||||||
/// Destructor
|
|
||||||
virtual ~FixedJoint();
|
|
||||||
};
|
|
||||||
}
|
|
||||||
|
|
58
src/org/atriaSoft/ephysics/constraint/FixedJoint.java
Normal file
58
src/org/atriaSoft/ephysics/constraint/FixedJoint.java
Normal file
@ -0,0 +1,58 @@
|
|||||||
|
package org.atriaSoft.ephysics.constraint;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* This structure is used to gather the information needed to create a fixed
|
||||||
|
* joint. This structure will be used to create the actual fixed joint.
|
||||||
|
*/
|
||||||
|
struct FixedJointInfo extends JointInfo {
|
||||||
|
public :
|
||||||
|
Vector3f anchorPointWorldSpace; //!< Anchor point (in world-space coordinates)
|
||||||
|
/**
|
||||||
|
* @breif Contructor
|
||||||
|
* @param rigidBody1 The first body of the joint
|
||||||
|
* @param rigidBody2 The second body of the joint
|
||||||
|
* @param initAnchorPointWorldSpace The initial anchor point of the joint in world-space coordinates
|
||||||
|
*/
|
||||||
|
FixedJointInfo(RigidBody* rigidBody1,
|
||||||
|
RigidBody* rigidBody2,
|
||||||
|
Vector3f initAnchorPointWorldSpace):
|
||||||
|
JointInfo(rigidBody1, rigidBody2, FIXEDJOINT),
|
||||||
|
this.anchorPointWorldSpace(initAnchorPointWorldSpace){
|
||||||
|
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @breif It represents a fixed joint that is used to forbid any translation or rotation
|
||||||
|
* between two bodies.
|
||||||
|
*/
|
||||||
|
class FixedJoint extends Joint {
|
||||||
|
private:
|
||||||
|
static float BETA; //!< Beta value for the bias factor of position correction
|
||||||
|
Vector3f localAnchorPointBody1; //!< Anchor point of body 1 (in local-space coordinates of body 1)
|
||||||
|
Vector3f localAnchorPointBody2; //!< Anchor point of body 2 (in local-space coordinates of body 2)
|
||||||
|
Vector3f r1World; //!< Vector from center of body 2 to anchor point in world-space
|
||||||
|
Vector3f r2World; //!< Vector from center of body 2 to anchor point in world-space
|
||||||
|
Matrix3f i1; //!< Inertia tensor of body 1 (in world-space coordinates)
|
||||||
|
Matrix3f i2; //!< Inertia tensor of body 2 (in world-space coordinates)
|
||||||
|
Vector3f impulseTranslation; //!< Accumulated impulse for the 3 translation raints
|
||||||
|
Vector3f impulseRotation; //!< Accumulate impulse for the 3 rotation raints
|
||||||
|
Matrix3f inverseMassMatrixTranslation; //!< Inverse mass matrix K=JM^-1J^-t of the 3 translation raints (3x3 matrix)
|
||||||
|
Matrix3f inverseMassMatrixRotation; //!< Inverse mass matrix K=JM^-1J^-t of the 3 rotation raints (3x3 matrix)
|
||||||
|
Vector3f biasTranslation; //!< Bias vector for the 3 translation raints
|
||||||
|
Vector3f biasRotation; //!< Bias vector for the 3 rotation raints
|
||||||
|
Quaternion initOrientationDifferenceInv; //!< Inverse of the initial orientation difference between the two bodies
|
||||||
|
|
||||||
|
long getSizeInBytes() {
|
||||||
|
return sizeof(FixedJoint);
|
||||||
|
}
|
||||||
|
void initBeforeSolve( ConstraintSolverData raintSolverData) ;
|
||||||
|
void warmstart( ConstraintSolverData raintSolverData) ;
|
||||||
|
void solveVelocityConstraint( ConstraintSolverData raintSolverData) ;
|
||||||
|
void solvePositionConstraint( ConstraintSolverData raintSolverData) ;
|
||||||
|
public:
|
||||||
|
/// Constructor
|
||||||
|
FixedJoint( FixedJointInfo jointInfo);
|
||||||
|
};
|
||||||
|
}
|
||||||
|
|
@ -23,14 +23,14 @@ HingeJoint::HingeJoint( HingeJointInfo jointInfo)
|
|||||||
this.isLowerLimitViolated(false), this.isUpperLimitViolated(false),
|
this.isLowerLimitViolated(false), this.isUpperLimitViolated(false),
|
||||||
this.motorSpeed(jointInfo.motorSpeed), this.maxMotorTorque(jointInfo.maxMotorTorque) {
|
this.motorSpeed(jointInfo.motorSpeed), this.maxMotorTorque(jointInfo.maxMotorTorque) {
|
||||||
|
|
||||||
assert(this.lowerLimit <= 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj this.lowerLimit >= -2.0 * PI);
|
assert(this.lowerLimit <= 0 && this.lowerLimit >= -2.0 * PI);
|
||||||
assert(this.upperLimit >= 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj this.upperLimit <= 2.0 * PI);
|
assert(this.upperLimit >= 0 && this.upperLimit <= 2.0 * PI);
|
||||||
|
|
||||||
// Compute the local-space anchor point for each body
|
// Compute the local-space anchor point for each body
|
||||||
etk::Transform3D transform1 = this.body1->getTransform();
|
Transform3D transform1 = this.body1.getTransform();
|
||||||
etk::Transform3D transform2 = this.body2->getTransform();
|
Transform3D transform2 = this.body2.getTransform();
|
||||||
this.localAnchorPointBody1 = transform1.getInverse() * jointInfo.this.anchorPointWorldSpace;
|
this.localAnchorPointBody1 = transform1.getInverse() * jointInfo.anchorPointWorldSpace;
|
||||||
this.localAnchorPointBody2 = transform2.getInverse() * jointInfo.this.anchorPointWorldSpace;
|
this.localAnchorPointBody2 = transform2.getInverse() * jointInfo.anchorPointWorldSpace;
|
||||||
|
|
||||||
// Compute the local-space hinge axis
|
// Compute the local-space hinge axis
|
||||||
this.hingeLocalAxisBody1 = transform1.getOrientation().getInverse() * jointInfo.rotationAxisWorld;
|
this.hingeLocalAxisBody1 = transform1.getOrientation().getInverse() * jointInfo.rotationAxisWorld;
|
||||||
@ -54,18 +54,18 @@ HingeJoint::~HingeJoint() {
|
|||||||
void HingeJoint::initBeforeSolve( ConstraintSolverData raintSolverData) {
|
void HingeJoint::initBeforeSolve( ConstraintSolverData raintSolverData) {
|
||||||
|
|
||||||
// Initialize the bodies index in the velocity array
|
// Initialize the bodies index in the velocity array
|
||||||
this.indexBody1 = raintSolverData.mapBodyToConstrainedVelocityIndex.find(this.body1)->second;
|
this.indexBody1 = raintSolverData.mapBodyToConstrainedVelocityIndex.find(this.body1).second;
|
||||||
this.indexBody2 = raintSolverData.mapBodyToConstrainedVelocityIndex.find(this.body2)->second;
|
this.indexBody2 = raintSolverData.mapBodyToConstrainedVelocityIndex.find(this.body2).second;
|
||||||
|
|
||||||
// Get the bodies positions and orientations
|
// Get the bodies positions and orientations
|
||||||
vec3 x1 = this.body1->this.centerOfMassWorld;
|
Vector3f x1 = this.body1.this.centerOfMassWorld;
|
||||||
vec3 x2 = this.body2->this.centerOfMassWorld;
|
Vector3f x2 = this.body2.this.centerOfMassWorld;
|
||||||
etk::Quaternion orientationBody1 = this.body1->getTransform().getOrientation();
|
Quaternion orientationBody1 = this.body1.getTransform().getOrientation();
|
||||||
etk::Quaternion orientationBody2 = this.body2->getTransform().getOrientation();
|
Quaternion orientationBody2 = this.body2.getTransform().getOrientation();
|
||||||
|
|
||||||
// Get the inertia tensor of bodies
|
// Get the inertia tensor of bodies
|
||||||
this.i1 = this.body1->getInertiaTensorInverseWorld();
|
this.i1 = this.body1.getInertiaTensorInverseWorld();
|
||||||
this.i2 = this.body2->getInertiaTensorInverseWorld();
|
this.i2 = this.body2.getInertiaTensorInverseWorld();
|
||||||
|
|
||||||
// Compute the vector from body center to the anchor point in world-space
|
// Compute the vector from body center to the anchor point in world-space
|
||||||
this.r1World = orientationBody1 * this.localAnchorPointBody1;
|
this.r1World = orientationBody1 * this.localAnchorPointBody1;
|
||||||
@ -90,27 +90,27 @@ void HingeJoint::initBeforeSolve( ConstraintSolverData raintSolverData) {
|
|||||||
|
|
||||||
// Compute vectors needed in the Jacobian
|
// Compute vectors needed in the Jacobian
|
||||||
mA1 = orientationBody1 * this.hingeLocalAxisBody1;
|
mA1 = orientationBody1 * this.hingeLocalAxisBody1;
|
||||||
vec3 a2 = orientationBody2 * this.hingeLocalAxisBody2;
|
Vector3f a2 = orientationBody2 * this.hingeLocalAxisBody2;
|
||||||
mA1.normalize();
|
mA1.normalize();
|
||||||
a2.normalize();
|
a2.normalize();
|
||||||
vec3 b2 = a2.getOrthoVector();
|
Vector3f b2 = a2.getOrthoVector();
|
||||||
vec3 c2 = a2.cross(b2);
|
Vector3f c2 = a2.cross(b2);
|
||||||
this.b2CrossA1 = b2.cross(mA1);
|
this.b2CrossA1 = b2.cross(mA1);
|
||||||
this.c2CrossA1 = c2.cross(mA1);
|
this.c2CrossA1 = c2.cross(mA1);
|
||||||
|
|
||||||
// Compute the corresponding skew-symmetric matrices
|
// Compute the corresponding skew-symmetric matrices
|
||||||
etk::Matrix3x3 skewSymmetricMatrixU1= etk::Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(this.r1World);
|
Matrix3f skewSymmetricMatrixU1= Matrix3f::computeSkewSymmetricMatrixForCrossProduct(this.r1World);
|
||||||
etk::Matrix3x3 skewSymmetricMatrixU2= etk::Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(this.r2World);
|
Matrix3f skewSymmetricMatrixU2= Matrix3f::computeSkewSymmetricMatrixForCrossProduct(this.r2World);
|
||||||
|
|
||||||
// Compute the inverse mass matrix K=JM^-1J^t for the 3 translation raints (3x3 matrix)
|
// Compute the inverse mass matrix K=JM^-1J^t for the 3 translation raints (3x3 matrix)
|
||||||
float inverseMassBodies = this.body1->this.massInverse + this.body2->this.massInverse;
|
float inverseMassBodies = this.body1.this.massInverse + this.body2.this.massInverse;
|
||||||
etk::Matrix3x3 massMatrix = etk::Matrix3x3(inverseMassBodies, 0, 0,
|
Matrix3f massMatrix = Matrix3f(inverseMassBodies, 0, 0,
|
||||||
0, inverseMassBodies, 0,
|
0, inverseMassBodies, 0,
|
||||||
0, 0, inverseMassBodies)
|
0, 0, inverseMassBodies)
|
||||||
+ skewSymmetricMatrixU1 * this.i1 * skewSymmetricMatrixU1.getTranspose()
|
+ skewSymmetricMatrixU1 * this.i1 * skewSymmetricMatrixU1.getTranspose()
|
||||||
+ skewSymmetricMatrixU2 * this.i2 * skewSymmetricMatrixU2.getTranspose();
|
+ skewSymmetricMatrixU2 * this.i2 * skewSymmetricMatrixU2.getTranspose();
|
||||||
this.inverseMassMatrixTranslation.setZero();
|
this.inverseMassMatrixTranslation.setZero();
|
||||||
if (this.body1->getType() == DYNAMIC || this.body2->getType() == DYNAMIC) {
|
if (this.body1.getType() == DYNAMIC || this.body2.getType() == DYNAMIC) {
|
||||||
this.inverseMassMatrixTranslation = massMatrix.getInverse();
|
this.inverseMassMatrixTranslation = massMatrix.getInverse();
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -122,10 +122,10 @@ void HingeJoint::initBeforeSolve( ConstraintSolverData raintSolverData) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
// Compute the inverse mass matrix K=JM^-1J^t for the 2 rotation raints (2x2 matrix)
|
// Compute the inverse mass matrix K=JM^-1J^t for the 2 rotation raints (2x2 matrix)
|
||||||
vec3 I1B2CrossA1 = this.i1 * this.b2CrossA1;
|
Vector3f I1B2CrossA1 = this.i1 * this.b2CrossA1;
|
||||||
vec3 I1C2CrossA1 = this.i1 * this.c2CrossA1;
|
Vector3f I1C2CrossA1 = this.i1 * this.c2CrossA1;
|
||||||
vec3 I2B2CrossA1 = this.i2 * this.b2CrossA1;
|
Vector3f I2B2CrossA1 = this.i2 * this.b2CrossA1;
|
||||||
vec3 I2C2CrossA1 = this.i2 * this.c2CrossA1;
|
Vector3f I2C2CrossA1 = this.i2 * this.c2CrossA1;
|
||||||
float el11 = this.b2CrossA1.dot(I1B2CrossA1) +
|
float el11 = this.b2CrossA1.dot(I1B2CrossA1) +
|
||||||
this.b2CrossA1.dot(I2B2CrossA1);
|
this.b2CrossA1.dot(I2B2CrossA1);
|
||||||
float el12 = this.b2CrossA1.dot(I1C2CrossA1) +
|
float el12 = this.b2CrossA1.dot(I1C2CrossA1) +
|
||||||
@ -134,9 +134,9 @@ void HingeJoint::initBeforeSolve( ConstraintSolverData raintSolverData) {
|
|||||||
this.c2CrossA1.dot(I2B2CrossA1);
|
this.c2CrossA1.dot(I2B2CrossA1);
|
||||||
float el22 = this.c2CrossA1.dot(I1C2CrossA1) +
|
float el22 = this.c2CrossA1.dot(I1C2CrossA1) +
|
||||||
this.c2CrossA1.dot(I2C2CrossA1);
|
this.c2CrossA1.dot(I2C2CrossA1);
|
||||||
etk::Matrix2x2 matrixKRotation(el11, el12, el21, el22);
|
Matrix2x2 matrixKRotation(el11, el12, el21, el22);
|
||||||
this.inverseMassMatrixRotation.setZero();
|
this.inverseMassMatrixRotation.setZero();
|
||||||
if (this.body1->getType() == DYNAMIC || this.body2->getType() == DYNAMIC) {
|
if (this.body1.getType() == DYNAMIC || this.body2.getType() == DYNAMIC) {
|
||||||
this.inverseMassMatrixRotation = matrixKRotation.getInverse();
|
this.inverseMassMatrixRotation = matrixKRotation.getInverse();
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -158,7 +158,7 @@ void HingeJoint::initBeforeSolve( ConstraintSolverData raintSolverData) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
// If the motor or limits are enabled
|
// If the motor or limits are enabled
|
||||||
if (this.isMotorEnabled || (this.isLimitEnabled hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj (this.isLowerLimitViolated || this.isUpperLimitViolated))) {
|
if (this.isMotorEnabled || (this.isLimitEnabled && (this.isLowerLimitViolated || this.isUpperLimitViolated))) {
|
||||||
|
|
||||||
// Compute the inverse of the mass matrix K=JM^-1J^t for the limits and motor (1x1 matrix)
|
// Compute the inverse of the mass matrix K=JM^-1J^t for the limits and motor (1x1 matrix)
|
||||||
this.inverseMassMatrixLimitMotor = mA1.dot(this.i1 * mA1) + mA1.dot(this.i2 * mA1);
|
this.inverseMassMatrixLimitMotor = mA1.dot(this.i1 * mA1) + mA1.dot(this.i2 * mA1);
|
||||||
@ -186,27 +186,27 @@ void HingeJoint::initBeforeSolve( ConstraintSolverData raintSolverData) {
|
|||||||
void HingeJoint::warmstart( ConstraintSolverData raintSolverData) {
|
void HingeJoint::warmstart( ConstraintSolverData raintSolverData) {
|
||||||
|
|
||||||
// Get the velocities
|
// Get the velocities
|
||||||
vec3 v1 = raintSolverData.linearVelocities[this.indexBody1];
|
Vector3f v1 = raintSolverData.linearVelocities[this.indexBody1];
|
||||||
vec3 v2 = raintSolverData.linearVelocities[this.indexBody2];
|
Vector3f v2 = raintSolverData.linearVelocities[this.indexBody2];
|
||||||
vec3 w1 = raintSolverData.angularVelocities[this.indexBody1];
|
Vector3f w1 = raintSolverData.angularVelocities[this.indexBody1];
|
||||||
vec3 w2 = raintSolverData.angularVelocities[this.indexBody2];
|
Vector3f w2 = raintSolverData.angularVelocities[this.indexBody2];
|
||||||
|
|
||||||
// Get the inverse mass and inverse inertia tensors of the bodies
|
// Get the inverse mass and inverse inertia tensors of the bodies
|
||||||
float inverseMassBody1 = this.body1->this.massInverse;
|
float inverseMassBody1 = this.body1.this.massInverse;
|
||||||
float inverseMassBody2 = this.body2->this.massInverse;
|
float inverseMassBody2 = this.body2.this.massInverse;
|
||||||
|
|
||||||
// Compute the impulse P=J^T * lambda for the 2 rotation raints
|
// Compute the impulse P=J^T * lambda for the 2 rotation raints
|
||||||
vec3 rotationImpulse = -this.b2CrossA1 * this.impulseRotation.x() - this.c2CrossA1 * this.impulseRotation.y();
|
Vector3f rotationImpulse = -this.b2CrossA1 * this.impulseRotation.x() - this.c2CrossA1 * this.impulseRotation.y();
|
||||||
|
|
||||||
// Compute the impulse P=J^T * lambda for the lower and upper limits raints
|
// Compute the impulse P=J^T * lambda for the lower and upper limits raints
|
||||||
vec3 limitsImpulse = (this.impulseUpperLimit - this.impulseLowerLimit) * mA1;
|
Vector3f limitsImpulse = (this.impulseUpperLimit - this.impulseLowerLimit) * mA1;
|
||||||
|
|
||||||
// Compute the impulse P=J^T * lambda for the motor raint
|
// Compute the impulse P=J^T * lambda for the motor raint
|
||||||
vec3 motorImpulse = -this.impulseMotor * mA1;
|
Vector3f motorImpulse = -this.impulseMotor * mA1;
|
||||||
|
|
||||||
// Compute the impulse P=J^T * lambda for the 3 translation raints of body 1
|
// Compute the impulse P=J^T * lambda for the 3 translation raints of body 1
|
||||||
vec3 linearImpulseBody1 = -this.impulseTranslation;
|
Vector3f linearImpulseBody1 = -this.impulseTranslation;
|
||||||
vec3 angularImpulseBody1 = this.impulseTranslation.cross(this.r1World);
|
Vector3f angularImpulseBody1 = this.impulseTranslation.cross(this.r1World);
|
||||||
|
|
||||||
// Compute the impulse P=J^T * lambda for the 2 rotation raints of body 1
|
// Compute the impulse P=J^T * lambda for the 2 rotation raints of body 1
|
||||||
angularImpulseBody1 += rotationImpulse;
|
angularImpulseBody1 += rotationImpulse;
|
||||||
@ -222,7 +222,7 @@ void HingeJoint::warmstart( ConstraintSolverData raintSolverData) {
|
|||||||
w1 += this.i1 * angularImpulseBody1;
|
w1 += this.i1 * angularImpulseBody1;
|
||||||
|
|
||||||
// Compute the impulse P=J^T * lambda for the 3 translation raints of body 2
|
// Compute the impulse P=J^T * lambda for the 3 translation raints of body 2
|
||||||
vec3 angularImpulseBody2 = -this.impulseTranslation.cross(this.r2World);
|
Vector3f angularImpulseBody2 = -this.impulseTranslation.cross(this.r2World);
|
||||||
|
|
||||||
// Compute the impulse P=J^T * lambda for the 2 rotation raints of body 2
|
// Compute the impulse P=J^T * lambda for the 2 rotation raints of body 2
|
||||||
angularImpulseBody2 += -rotationImpulse;
|
angularImpulseBody2 += -rotationImpulse;
|
||||||
@ -242,35 +242,35 @@ void HingeJoint::warmstart( ConstraintSolverData raintSolverData) {
|
|||||||
void HingeJoint::solveVelocityConstraint( ConstraintSolverData raintSolverData) {
|
void HingeJoint::solveVelocityConstraint( ConstraintSolverData raintSolverData) {
|
||||||
|
|
||||||
// Get the velocities
|
// Get the velocities
|
||||||
vec3 v1 = raintSolverData.linearVelocities[this.indexBody1];
|
Vector3f v1 = raintSolverData.linearVelocities[this.indexBody1];
|
||||||
vec3 v2 = raintSolverData.linearVelocities[this.indexBody2];
|
Vector3f v2 = raintSolverData.linearVelocities[this.indexBody2];
|
||||||
vec3 w1 = raintSolverData.angularVelocities[this.indexBody1];
|
Vector3f w1 = raintSolverData.angularVelocities[this.indexBody1];
|
||||||
vec3 w2 = raintSolverData.angularVelocities[this.indexBody2];
|
Vector3f w2 = raintSolverData.angularVelocities[this.indexBody2];
|
||||||
|
|
||||||
// Get the inverse mass and inverse inertia tensors of the bodies
|
// Get the inverse mass and inverse inertia tensors of the bodies
|
||||||
float inverseMassBody1 = this.body1->this.massInverse;
|
float inverseMassBody1 = this.body1.this.massInverse;
|
||||||
float inverseMassBody2 = this.body2->this.massInverse;
|
float inverseMassBody2 = this.body2.this.massInverse;
|
||||||
|
|
||||||
// --------------- Translation Constraints --------------- //
|
// --------------- Translation Constraints --------------- //
|
||||||
|
|
||||||
// Compute J*v
|
// Compute J*v
|
||||||
vec3 JvTranslation = v2 + w2.cross(this.r2World) - v1 - w1.cross(this.r1World);
|
Vector3f JvTranslation = v2 + w2.cross(this.r2World) - v1 - w1.cross(this.r1World);
|
||||||
|
|
||||||
// Compute the Lagrange multiplier lambda
|
// Compute the Lagrange multiplier lambda
|
||||||
vec3 deltaLambdaTranslation = this.inverseMassMatrixTranslation *
|
Vector3f deltaLambdaTranslation = this.inverseMassMatrixTranslation *
|
||||||
(-JvTranslation - this.bTranslation);
|
(-JvTranslation - this.bTranslation);
|
||||||
this.impulseTranslation += deltaLambdaTranslation;
|
this.impulseTranslation += deltaLambdaTranslation;
|
||||||
|
|
||||||
// Compute the impulse P=J^T * lambda of body 1
|
// Compute the impulse P=J^T * lambda of body 1
|
||||||
vec3 linearImpulseBody1 = -deltaLambdaTranslation;
|
Vector3f linearImpulseBody1 = -deltaLambdaTranslation;
|
||||||
vec3 angularImpulseBody1 = deltaLambdaTranslation.cross(this.r1World);
|
Vector3f angularImpulseBody1 = deltaLambdaTranslation.cross(this.r1World);
|
||||||
|
|
||||||
// Apply the impulse to the body 1
|
// Apply the impulse to the body 1
|
||||||
v1 += inverseMassBody1 * linearImpulseBody1;
|
v1 += inverseMassBody1 * linearImpulseBody1;
|
||||||
w1 += this.i1 * angularImpulseBody1;
|
w1 += this.i1 * angularImpulseBody1;
|
||||||
|
|
||||||
// Compute the impulse P=J^T * lambda of body 2
|
// Compute the impulse P=J^T * lambda of body 2
|
||||||
vec3 angularImpulseBody2 = -deltaLambdaTranslation.cross(this.r2World);
|
Vector3f angularImpulseBody2 = -deltaLambdaTranslation.cross(this.r2World);
|
||||||
|
|
||||||
// Apply the impulse to the body 2
|
// Apply the impulse to the body 2
|
||||||
v2 += inverseMassBody2 * deltaLambdaTranslation;
|
v2 += inverseMassBody2 * deltaLambdaTranslation;
|
||||||
@ -313,17 +313,17 @@ void HingeJoint::solveVelocityConstraint( ConstraintSolverData raintSolverData)
|
|||||||
// Compute the Lagrange multiplier lambda for the lower limit raint
|
// Compute the Lagrange multiplier lambda for the lower limit raint
|
||||||
float deltaLambdaLower = this.inverseMassMatrixLimitMotor * (-JvLowerLimit -this.bLowerLimit);
|
float deltaLambdaLower = this.inverseMassMatrixLimitMotor * (-JvLowerLimit -this.bLowerLimit);
|
||||||
float lambdaTemp = this.impulseLowerLimit;
|
float lambdaTemp = this.impulseLowerLimit;
|
||||||
this.impulseLowerLimit = etk::max(this.impulseLowerLimit + deltaLambdaLower, 0.0f);
|
this.impulseLowerLimit = max(this.impulseLowerLimit + deltaLambdaLower, 0.0f);
|
||||||
deltaLambdaLower = this.impulseLowerLimit - lambdaTemp;
|
deltaLambdaLower = this.impulseLowerLimit - lambdaTemp;
|
||||||
|
|
||||||
// Compute the impulse P=J^T * lambda for the lower limit raint of body 1
|
// Compute the impulse P=J^T * lambda for the lower limit raint of body 1
|
||||||
vec3 angularImpulseBody1 = -deltaLambdaLower * mA1;
|
Vector3f angularImpulseBody1 = -deltaLambdaLower * mA1;
|
||||||
|
|
||||||
// Apply the impulse to the body 1
|
// Apply the impulse to the body 1
|
||||||
w1 += this.i1 * angularImpulseBody1;
|
w1 += this.i1 * angularImpulseBody1;
|
||||||
|
|
||||||
// Compute the impulse P=J^T * lambda for the lower limit raint of body 2
|
// Compute the impulse P=J^T * lambda for the lower limit raint of body 2
|
||||||
vec3 angularImpulseBody2 = deltaLambdaLower * mA1;
|
Vector3f angularImpulseBody2 = deltaLambdaLower * mA1;
|
||||||
|
|
||||||
// Apply the impulse to the body 2
|
// Apply the impulse to the body 2
|
||||||
w2 += this.i2 * angularImpulseBody2;
|
w2 += this.i2 * angularImpulseBody2;
|
||||||
@ -338,17 +338,17 @@ void HingeJoint::solveVelocityConstraint( ConstraintSolverData raintSolverData)
|
|||||||
// Compute the Lagrange multiplier lambda for the upper limit raint
|
// Compute the Lagrange multiplier lambda for the upper limit raint
|
||||||
float deltaLambdaUpper = this.inverseMassMatrixLimitMotor * (-JvUpperLimit -this.bUpperLimit);
|
float deltaLambdaUpper = this.inverseMassMatrixLimitMotor * (-JvUpperLimit -this.bUpperLimit);
|
||||||
float lambdaTemp = this.impulseUpperLimit;
|
float lambdaTemp = this.impulseUpperLimit;
|
||||||
this.impulseUpperLimit = etk::max(this.impulseUpperLimit + deltaLambdaUpper, 0.0f);
|
this.impulseUpperLimit = max(this.impulseUpperLimit + deltaLambdaUpper, 0.0f);
|
||||||
deltaLambdaUpper = this.impulseUpperLimit - lambdaTemp;
|
deltaLambdaUpper = this.impulseUpperLimit - lambdaTemp;
|
||||||
|
|
||||||
// Compute the impulse P=J^T * lambda for the upper limit raint of body 1
|
// Compute the impulse P=J^T * lambda for the upper limit raint of body 1
|
||||||
vec3 angularImpulseBody1 = deltaLambdaUpper * mA1;
|
Vector3f angularImpulseBody1 = deltaLambdaUpper * mA1;
|
||||||
|
|
||||||
// Apply the impulse to the body 1
|
// Apply the impulse to the body 1
|
||||||
w1 += this.i1 * angularImpulseBody1;
|
w1 += this.i1 * angularImpulseBody1;
|
||||||
|
|
||||||
// Compute the impulse P=J^T * lambda for the upper limit raint of body 2
|
// Compute the impulse P=J^T * lambda for the upper limit raint of body 2
|
||||||
vec3 angularImpulseBody2 = -deltaLambdaUpper * mA1;
|
Vector3f angularImpulseBody2 = -deltaLambdaUpper * mA1;
|
||||||
|
|
||||||
// Apply the impulse to the body 2
|
// Apply the impulse to the body 2
|
||||||
w2 += this.i2 * angularImpulseBody2;
|
w2 += this.i2 * angularImpulseBody2;
|
||||||
@ -371,13 +371,13 @@ void HingeJoint::solveVelocityConstraint( ConstraintSolverData raintSolverData)
|
|||||||
deltaLambdaMotor = this.impulseMotor - lambdaTemp;
|
deltaLambdaMotor = this.impulseMotor - lambdaTemp;
|
||||||
|
|
||||||
// Compute the impulse P=J^T * lambda for the motor of body 1
|
// Compute the impulse P=J^T * lambda for the motor of body 1
|
||||||
vec3 angularImpulseBody1 = -deltaLambdaMotor * mA1;
|
Vector3f angularImpulseBody1 = -deltaLambdaMotor * mA1;
|
||||||
|
|
||||||
// Apply the impulse to the body 1
|
// Apply the impulse to the body 1
|
||||||
w1 += this.i1 * angularImpulseBody1;
|
w1 += this.i1 * angularImpulseBody1;
|
||||||
|
|
||||||
// Compute the impulse P=J^T * lambda for the motor of body 2
|
// Compute the impulse P=J^T * lambda for the motor of body 2
|
||||||
vec3 angularImpulseBody2 = deltaLambdaMotor * mA1;
|
Vector3f angularImpulseBody2 = deltaLambdaMotor * mA1;
|
||||||
|
|
||||||
// Apply the impulse to the body 2
|
// Apply the impulse to the body 2
|
||||||
w2 += this.i2 * angularImpulseBody2;
|
w2 += this.i2 * angularImpulseBody2;
|
||||||
@ -392,18 +392,18 @@ void HingeJoint::solvePositionConstraint( ConstraintSolverData raintSolverData)
|
|||||||
if (this.positionCorrectionTechnique != NONLINEARGAUSSSEIDEL) return;
|
if (this.positionCorrectionTechnique != NONLINEARGAUSSSEIDEL) return;
|
||||||
|
|
||||||
// Get the bodies positions and orientations
|
// Get the bodies positions and orientations
|
||||||
vec3 x1 = raintSolverData.positions[this.indexBody1];
|
Vector3f x1 = raintSolverData.positions[this.indexBody1];
|
||||||
vec3 x2 = raintSolverData.positions[this.indexBody2];
|
Vector3f x2 = raintSolverData.positions[this.indexBody2];
|
||||||
etk::Quaternion q1 = raintSolverData.orientations[this.indexBody1];
|
Quaternion q1 = raintSolverData.orientations[this.indexBody1];
|
||||||
etk::Quaternion q2 = raintSolverData.orientations[this.indexBody2];
|
Quaternion q2 = raintSolverData.orientations[this.indexBody2];
|
||||||
|
|
||||||
// Get the inverse mass and inverse inertia tensors of the bodies
|
// Get the inverse mass and inverse inertia tensors of the bodies
|
||||||
float inverseMassBody1 = this.body1->this.massInverse;
|
float inverseMassBody1 = this.body1.this.massInverse;
|
||||||
float inverseMassBody2 = this.body2->this.massInverse;
|
float inverseMassBody2 = this.body2.this.massInverse;
|
||||||
|
|
||||||
// Recompute the inverse inertia tensors
|
// Recompute the inverse inertia tensors
|
||||||
this.i1 = this.body1->getInertiaTensorInverseWorld();
|
this.i1 = this.body1.getInertiaTensorInverseWorld();
|
||||||
this.i2 = this.body2->getInertiaTensorInverseWorld();
|
this.i2 = this.body2.getInertiaTensorInverseWorld();
|
||||||
|
|
||||||
// Compute the vector from body center to the anchor point in world-space
|
// Compute the vector from body center to the anchor point in world-space
|
||||||
this.r1World = q1 * this.localAnchorPointBody1;
|
this.r1World = q1 * this.localAnchorPointBody1;
|
||||||
@ -420,70 +420,70 @@ void HingeJoint::solvePositionConstraint( ConstraintSolverData raintSolverData)
|
|||||||
|
|
||||||
// Compute vectors needed in the Jacobian
|
// Compute vectors needed in the Jacobian
|
||||||
mA1 = q1 * this.hingeLocalAxisBody1;
|
mA1 = q1 * this.hingeLocalAxisBody1;
|
||||||
vec3 a2 = q2 * this.hingeLocalAxisBody2;
|
Vector3f a2 = q2 * this.hingeLocalAxisBody2;
|
||||||
mA1.normalize();
|
mA1.normalize();
|
||||||
a2.normalize();
|
a2.normalize();
|
||||||
vec3 b2 = a2.getOrthoVector();
|
Vector3f b2 = a2.getOrthoVector();
|
||||||
vec3 c2 = a2.cross(b2);
|
Vector3f c2 = a2.cross(b2);
|
||||||
this.b2CrossA1 = b2.cross(mA1);
|
this.b2CrossA1 = b2.cross(mA1);
|
||||||
this.c2CrossA1 = c2.cross(mA1);
|
this.c2CrossA1 = c2.cross(mA1);
|
||||||
|
|
||||||
// Compute the corresponding skew-symmetric matrices
|
// Compute the corresponding skew-symmetric matrices
|
||||||
etk::Matrix3x3 skewSymmetricMatrixU1= etk::Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(this.r1World);
|
Matrix3f skewSymmetricMatrixU1= Matrix3f::computeSkewSymmetricMatrixForCrossProduct(this.r1World);
|
||||||
etk::Matrix3x3 skewSymmetricMatrixU2= etk::Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(this.r2World);
|
Matrix3f skewSymmetricMatrixU2= Matrix3f::computeSkewSymmetricMatrixForCrossProduct(this.r2World);
|
||||||
|
|
||||||
// --------------- Translation Constraints --------------- //
|
// --------------- Translation Constraints --------------- //
|
||||||
|
|
||||||
// Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation raints
|
// Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation raints
|
||||||
float inverseMassBodies = this.body1->this.massInverse + this.body2->this.massInverse;
|
float inverseMassBodies = this.body1.this.massInverse + this.body2.this.massInverse;
|
||||||
etk::Matrix3x3 massMatrix = etk::Matrix3x3(inverseMassBodies, 0, 0,
|
Matrix3f massMatrix = Matrix3f(inverseMassBodies, 0, 0,
|
||||||
0, inverseMassBodies, 0,
|
0, inverseMassBodies, 0,
|
||||||
0, 0, inverseMassBodies)
|
0, 0, inverseMassBodies)
|
||||||
+ skewSymmetricMatrixU1 * this.i1 * skewSymmetricMatrixU1.getTranspose()
|
+ skewSymmetricMatrixU1 * this.i1 * skewSymmetricMatrixU1.getTranspose()
|
||||||
+ skewSymmetricMatrixU2 * this.i2 * skewSymmetricMatrixU2.getTranspose();
|
+ skewSymmetricMatrixU2 * this.i2 * skewSymmetricMatrixU2.getTranspose();
|
||||||
this.inverseMassMatrixTranslation.setZero();
|
this.inverseMassMatrixTranslation.setZero();
|
||||||
if (this.body1->getType() == DYNAMIC || this.body2->getType() == DYNAMIC) {
|
if (this.body1.getType() == DYNAMIC || this.body2.getType() == DYNAMIC) {
|
||||||
this.inverseMassMatrixTranslation = massMatrix.getInverse();
|
this.inverseMassMatrixTranslation = massMatrix.getInverse();
|
||||||
}
|
}
|
||||||
|
|
||||||
// Compute position error for the 3 translation raints
|
// Compute position error for the 3 translation raints
|
||||||
vec3 errorTranslation = x2 + this.r2World - x1 - this.r1World;
|
Vector3f errorTranslation = x2 + this.r2World - x1 - this.r1World;
|
||||||
|
|
||||||
// Compute the Lagrange multiplier lambda
|
// Compute the Lagrange multiplier lambda
|
||||||
vec3 lambdaTranslation = this.inverseMassMatrixTranslation * (-errorTranslation);
|
Vector3f lambdaTranslation = this.inverseMassMatrixTranslation * (-errorTranslation);
|
||||||
|
|
||||||
// Compute the impulse of body 1
|
// Compute the impulse of body 1
|
||||||
vec3 linearImpulseBody1 = -lambdaTranslation;
|
Vector3f linearImpulseBody1 = -lambdaTranslation;
|
||||||
vec3 angularImpulseBody1 = lambdaTranslation.cross(this.r1World);
|
Vector3f angularImpulseBody1 = lambdaTranslation.cross(this.r1World);
|
||||||
|
|
||||||
// Compute the pseudo velocity of body 1
|
// Compute the pseudo velocity of body 1
|
||||||
vec3 v1 = inverseMassBody1 * linearImpulseBody1;
|
Vector3f v1 = inverseMassBody1 * linearImpulseBody1;
|
||||||
vec3 w1 = this.i1 * angularImpulseBody1;
|
Vector3f w1 = this.i1 * angularImpulseBody1;
|
||||||
|
|
||||||
// Update the body position/orientation of body 1
|
// Update the body position/orientation of body 1
|
||||||
x1 += v1;
|
x1 += v1;
|
||||||
q1 += etk::Quaternion(0, w1) * q1 * 0.5f;
|
q1 += Quaternion(0, w1) * q1 * 0.5f;
|
||||||
q1.normalize();
|
q1.normalize();
|
||||||
|
|
||||||
// Compute the impulse of body 2
|
// Compute the impulse of body 2
|
||||||
vec3 angularImpulseBody2 = -lambdaTranslation.cross(this.r2World);
|
Vector3f angularImpulseBody2 = -lambdaTranslation.cross(this.r2World);
|
||||||
|
|
||||||
// Compute the pseudo velocity of body 2
|
// Compute the pseudo velocity of body 2
|
||||||
vec3 v2 = inverseMassBody2 * lambdaTranslation;
|
Vector3f v2 = inverseMassBody2 * lambdaTranslation;
|
||||||
vec3 w2 = this.i2 * angularImpulseBody2;
|
Vector3f w2 = this.i2 * angularImpulseBody2;
|
||||||
|
|
||||||
// Update the body position/orientation of body 2
|
// Update the body position/orientation of body 2
|
||||||
x2 += v2;
|
x2 += v2;
|
||||||
q2 += etk::Quaternion(0, w2) * q2 * 0.5f;
|
q2 += Quaternion(0, w2) * q2 * 0.5f;
|
||||||
q2.normalize();
|
q2.normalize();
|
||||||
|
|
||||||
// --------------- Rotation Constraints --------------- //
|
// --------------- Rotation Constraints --------------- //
|
||||||
|
|
||||||
// Compute the inverse mass matrix K=JM^-1J^t for the 2 rotation raints (2x2 matrix)
|
// Compute the inverse mass matrix K=JM^-1J^t for the 2 rotation raints (2x2 matrix)
|
||||||
vec3 I1B2CrossA1 = this.i1 * this.b2CrossA1;
|
Vector3f I1B2CrossA1 = this.i1 * this.b2CrossA1;
|
||||||
vec3 I1C2CrossA1 = this.i1 * this.c2CrossA1;
|
Vector3f I1C2CrossA1 = this.i1 * this.c2CrossA1;
|
||||||
vec3 I2B2CrossA1 = this.i2 * this.b2CrossA1;
|
Vector3f I2B2CrossA1 = this.i2 * this.b2CrossA1;
|
||||||
vec3 I2C2CrossA1 = this.i2 * this.c2CrossA1;
|
Vector3f I2C2CrossA1 = this.i2 * this.c2CrossA1;
|
||||||
float el11 = this.b2CrossA1.dot(I1B2CrossA1) +
|
float el11 = this.b2CrossA1.dot(I1B2CrossA1) +
|
||||||
this.b2CrossA1.dot(I2B2CrossA1);
|
this.b2CrossA1.dot(I2B2CrossA1);
|
||||||
float el12 = this.b2CrossA1.dot(I1C2CrossA1) +
|
float el12 = this.b2CrossA1.dot(I1C2CrossA1) +
|
||||||
@ -492,9 +492,9 @@ void HingeJoint::solvePositionConstraint( ConstraintSolverData raintSolverData)
|
|||||||
this.c2CrossA1.dot(I2B2CrossA1);
|
this.c2CrossA1.dot(I2B2CrossA1);
|
||||||
float el22 = this.c2CrossA1.dot(I1C2CrossA1) +
|
float el22 = this.c2CrossA1.dot(I1C2CrossA1) +
|
||||||
this.c2CrossA1.dot(I2C2CrossA1);
|
this.c2CrossA1.dot(I2C2CrossA1);
|
||||||
etk::Matrix2x2 matrixKRotation(el11, el12, el21, el22);
|
Matrix2x2 matrixKRotation(el11, el12, el21, el22);
|
||||||
this.inverseMassMatrixRotation.setZero();
|
this.inverseMassMatrixRotation.setZero();
|
||||||
if (this.body1->getType() == DYNAMIC || this.body2->getType() == DYNAMIC) {
|
if (this.body1.getType() == DYNAMIC || this.body2.getType() == DYNAMIC) {
|
||||||
this.inverseMassMatrixRotation = matrixKRotation.getInverse();
|
this.inverseMassMatrixRotation = matrixKRotation.getInverse();
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -511,7 +511,7 @@ void HingeJoint::solvePositionConstraint( ConstraintSolverData raintSolverData)
|
|||||||
w1 = this.i1 * angularImpulseBody1;
|
w1 = this.i1 * angularImpulseBody1;
|
||||||
|
|
||||||
// Update the body position/orientation of body 1
|
// Update the body position/orientation of body 1
|
||||||
q1 += etk::Quaternion(0, w1) * q1 * 0.5f;
|
q1 += Quaternion(0, w1) * q1 * 0.5f;
|
||||||
q1.normalize();
|
q1.normalize();
|
||||||
|
|
||||||
// Compute the impulse of body 2
|
// Compute the impulse of body 2
|
||||||
@ -521,7 +521,7 @@ void HingeJoint::solvePositionConstraint( ConstraintSolverData raintSolverData)
|
|||||||
w2 = this.i2 * angularImpulseBody2;
|
w2 = this.i2 * angularImpulseBody2;
|
||||||
|
|
||||||
// Update the body position/orientation of body 2
|
// Update the body position/orientation of body 2
|
||||||
q2 += etk::Quaternion(0, w2) * q2 * 0.5f;
|
q2 += Quaternion(0, w2) * q2 * 0.5f;
|
||||||
q2.normalize();
|
q2.normalize();
|
||||||
|
|
||||||
// --------------- Limits Constraints --------------- //
|
// --------------- Limits Constraints --------------- //
|
||||||
@ -543,23 +543,23 @@ void HingeJoint::solvePositionConstraint( ConstraintSolverData raintSolverData)
|
|||||||
float lambdaLowerLimit = this.inverseMassMatrixLimitMotor * (-lowerLimitError );
|
float lambdaLowerLimit = this.inverseMassMatrixLimitMotor * (-lowerLimitError );
|
||||||
|
|
||||||
// Compute the impulse P=J^T * lambda of body 1
|
// Compute the impulse P=J^T * lambda of body 1
|
||||||
vec3 angularImpulseBody1 = -lambdaLowerLimit * mA1;
|
Vector3f angularImpulseBody1 = -lambdaLowerLimit * mA1;
|
||||||
|
|
||||||
// Compute the pseudo velocity of body 1
|
// Compute the pseudo velocity of body 1
|
||||||
vec3 w1 = this.i1 * angularImpulseBody1;
|
Vector3f w1 = this.i1 * angularImpulseBody1;
|
||||||
|
|
||||||
// Update the body position/orientation of body 1
|
// Update the body position/orientation of body 1
|
||||||
q1 += etk::Quaternion(0, w1) * q1 * 0.5f;
|
q1 += Quaternion(0, w1) * q1 * 0.5f;
|
||||||
q1.normalize();
|
q1.normalize();
|
||||||
|
|
||||||
// Compute the impulse P=J^T * lambda of body 2
|
// Compute the impulse P=J^T * lambda of body 2
|
||||||
vec3 angularImpulseBody2 = lambdaLowerLimit * mA1;
|
Vector3f angularImpulseBody2 = lambdaLowerLimit * mA1;
|
||||||
|
|
||||||
// Compute the pseudo velocity of body 2
|
// Compute the pseudo velocity of body 2
|
||||||
vec3 w2 = this.i2 * angularImpulseBody2;
|
Vector3f w2 = this.i2 * angularImpulseBody2;
|
||||||
|
|
||||||
// Update the body position/orientation of body 2
|
// Update the body position/orientation of body 2
|
||||||
q2 += etk::Quaternion(0, w2) * q2 * 0.5f;
|
q2 += Quaternion(0, w2) * q2 * 0.5f;
|
||||||
q2.normalize();
|
q2.normalize();
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -570,23 +570,23 @@ void HingeJoint::solvePositionConstraint( ConstraintSolverData raintSolverData)
|
|||||||
float lambdaUpperLimit = this.inverseMassMatrixLimitMotor * (-upperLimitError);
|
float lambdaUpperLimit = this.inverseMassMatrixLimitMotor * (-upperLimitError);
|
||||||
|
|
||||||
// Compute the impulse P=J^T * lambda of body 1
|
// Compute the impulse P=J^T * lambda of body 1
|
||||||
vec3 angularImpulseBody1 = lambdaUpperLimit * mA1;
|
Vector3f angularImpulseBody1 = lambdaUpperLimit * mA1;
|
||||||
|
|
||||||
// Compute the pseudo velocity of body 1
|
// Compute the pseudo velocity of body 1
|
||||||
vec3 w1 = this.i1 * angularImpulseBody1;
|
Vector3f w1 = this.i1 * angularImpulseBody1;
|
||||||
|
|
||||||
// Update the body position/orientation of body 1
|
// Update the body position/orientation of body 1
|
||||||
q1 += etk::Quaternion(0, w1) * q1 * 0.5f;
|
q1 += Quaternion(0, w1) * q1 * 0.5f;
|
||||||
q1.normalize();
|
q1.normalize();
|
||||||
|
|
||||||
// Compute the impulse P=J^T * lambda of body 2
|
// Compute the impulse P=J^T * lambda of body 2
|
||||||
vec3 angularImpulseBody2 = -lambdaUpperLimit * mA1;
|
Vector3f angularImpulseBody2 = -lambdaUpperLimit * mA1;
|
||||||
|
|
||||||
// Compute the pseudo velocity of body 2
|
// Compute the pseudo velocity of body 2
|
||||||
vec3 w2 = this.i2 * angularImpulseBody2;
|
Vector3f w2 = this.i2 * angularImpulseBody2;
|
||||||
|
|
||||||
// Update the body position/orientation of body 2
|
// Update the body position/orientation of body 2
|
||||||
q2 += etk::Quaternion(0, w2) * q2 * 0.5f;
|
q2 += Quaternion(0, w2) * q2 * 0.5f;
|
||||||
q2.normalize();
|
q2.normalize();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@ -620,8 +620,8 @@ void HingeJoint::enableMotor(boolean isMotorEnabled) {
|
|||||||
this.impulseMotor = 0.0;
|
this.impulseMotor = 0.0;
|
||||||
|
|
||||||
// Wake up the two bodies of the joint
|
// Wake up the two bodies of the joint
|
||||||
this.body1->setIsSleeping(false);
|
this.body1.setIsSleeping(false);
|
||||||
this.body2->setIsSleeping(false);
|
this.body2.setIsSleeping(false);
|
||||||
}
|
}
|
||||||
|
|
||||||
// Set the minimum angle limit
|
// Set the minimum angle limit
|
||||||
@ -630,7 +630,7 @@ void HingeJoint::enableMotor(boolean isMotorEnabled) {
|
|||||||
*/
|
*/
|
||||||
void HingeJoint::setMinAngleLimit(float lowerLimit) {
|
void HingeJoint::setMinAngleLimit(float lowerLimit) {
|
||||||
|
|
||||||
assert(this.lowerLimit <= 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj this.lowerLimit >= -2.0 * PI);
|
assert(this.lowerLimit <= 0 && this.lowerLimit >= -2.0 * PI);
|
||||||
|
|
||||||
if (lowerLimit != this.lowerLimit) {
|
if (lowerLimit != this.lowerLimit) {
|
||||||
|
|
||||||
@ -647,7 +647,7 @@ void HingeJoint::setMinAngleLimit(float lowerLimit) {
|
|||||||
*/
|
*/
|
||||||
void HingeJoint::setMaxAngleLimit(float upperLimit) {
|
void HingeJoint::setMaxAngleLimit(float upperLimit) {
|
||||||
|
|
||||||
assert(upperLimit >= 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj upperLimit <= 2.0 * PI);
|
assert(upperLimit >= 0 && upperLimit <= 2.0 * PI);
|
||||||
|
|
||||||
if (upperLimit != this.upperLimit) {
|
if (upperLimit != this.upperLimit) {
|
||||||
|
|
||||||
@ -666,8 +666,8 @@ void HingeJoint::resetLimits() {
|
|||||||
this.impulseUpperLimit = 0.0;
|
this.impulseUpperLimit = 0.0;
|
||||||
|
|
||||||
// Wake up the two bodies of the joint
|
// Wake up the two bodies of the joint
|
||||||
this.body1->setIsSleeping(false);
|
this.body1.setIsSleeping(false);
|
||||||
this.body2->setIsSleeping(false);
|
this.body2.setIsSleeping(false);
|
||||||
}
|
}
|
||||||
|
|
||||||
// Set the motor speed
|
// Set the motor speed
|
||||||
@ -678,8 +678,8 @@ void HingeJoint::setMotorSpeed(float motorSpeed) {
|
|||||||
this.motorSpeed = motorSpeed;
|
this.motorSpeed = motorSpeed;
|
||||||
|
|
||||||
// Wake up the two bodies of the joint
|
// Wake up the two bodies of the joint
|
||||||
this.body1->setIsSleeping(false);
|
this.body1.setIsSleeping(false);
|
||||||
this.body2->setIsSleeping(false);
|
this.body2.setIsSleeping(false);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -695,8 +695,8 @@ void HingeJoint::setMaxMotorTorque(float maxMotorTorque) {
|
|||||||
this.maxMotorTorque = maxMotorTorque;
|
this.maxMotorTorque = maxMotorTorque;
|
||||||
|
|
||||||
// Wake up the two bodies of the joint
|
// Wake up the two bodies of the joint
|
||||||
this.body1->setIsSleeping(false);
|
this.body1.setIsSleeping(false);
|
||||||
this.body2->setIsSleeping(false);
|
this.body2.setIsSleeping(false);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -742,17 +742,17 @@ float HingeJoint::computeCorrespondingAngleNearLimits(float inputAngle, float lo
|
|||||||
}
|
}
|
||||||
|
|
||||||
// Compute the current angle around the hinge axis
|
// Compute the current angle around the hinge axis
|
||||||
float HingeJoint::computeCurrentHingeAngle( etk::Quaternion orientationBody1,
|
float HingeJoint::computeCurrentHingeAngle( Quaternion orientationBody1,
|
||||||
etk::Quaternion orientationBody2) {
|
Quaternion orientationBody2) {
|
||||||
|
|
||||||
float hingeAngle;
|
float hingeAngle;
|
||||||
|
|
||||||
// Compute the current orientation difference between the two bodies
|
// Compute the current orientation difference between the two bodies
|
||||||
etk::Quaternion currentOrientationDiff = orientationBody2 * orientationBody1.getInverse();
|
Quaternion currentOrientationDiff = orientationBody2 * orientationBody1.getInverse();
|
||||||
currentOrientationDiff.normalize();
|
currentOrientationDiff.normalize();
|
||||||
|
|
||||||
// Compute the relative rotation considering the initial orientation difference
|
// Compute the relative rotation considering the initial orientation difference
|
||||||
etk::Quaternion relativeRotation = currentOrientationDiff * this.initOrientationDifferenceInv;
|
Quaternion relativeRotation = currentOrientationDiff * this.initOrientationDifferenceInv;
|
||||||
relativeRotation.normalize();
|
relativeRotation.normalize();
|
||||||
|
|
||||||
// A quaternion q = [cos(theta/2); sin(theta/2) * rotAxis] where rotAxis is a unit
|
// A quaternion q = [cos(theta/2); sin(theta/2) * rotAxis] where rotAxis is a unit
|
||||||
@ -770,10 +770,10 @@ float HingeJoint::computeCurrentHingeAngle( etk::Quaternion orientationBody1,
|
|||||||
|
|
||||||
// If the relative rotation axis and the hinge axis are pointing the same direction
|
// If the relative rotation axis and the hinge axis are pointing the same direction
|
||||||
if (dotProduct >= 0.0f) {
|
if (dotProduct >= 0.0f) {
|
||||||
hingeAngle = float(2.0) * etk::atan2(sinHalfAngleAbs, cosHalfAngle);
|
hingeAngle = float(2.0) * atan2(sinHalfAngleAbs, cosHalfAngle);
|
||||||
}
|
}
|
||||||
else {
|
else {
|
||||||
hingeAngle = float(2.0) * etk::atan2(sinHalfAngleAbs, -cosHalfAngle);
|
hingeAngle = float(2.0) * atan2(sinHalfAngleAbs, -cosHalfAngle);
|
||||||
}
|
}
|
||||||
|
|
||||||
// Convert the angle from range [-2*pi; 2*pi] into the range [-pi; pi]
|
// Convert the angle from range [-2*pi; 2*pi] into the range [-pi; pi]
|
||||||
@ -842,7 +842,7 @@ float HingeJoint::getMotorTorque(float timeStep) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
// Return the number of bytes used by the joint
|
// Return the number of bytes used by the joint
|
||||||
sizet HingeJoint::getSizeInBytes() {
|
long HingeJoint::getSizeInBytes() {
|
||||||
return sizeof(HingeJoint);
|
return sizeof(HingeJoint);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -1,25 +1,13 @@
|
|||||||
/** @file
|
package org.atriaSoft.ephysics.constraint;
|
||||||
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
|
|
||||||
* @author Daniel CHAPPUIS
|
|
||||||
* @author Edouard DUPIN
|
|
||||||
* @copyright 2010-2016, Daniel Chappuis
|
|
||||||
* @copyright 2017, Edouard DUPIN
|
|
||||||
* @license MPL v2.0 (see license file)
|
|
||||||
*/
|
|
||||||
#pragma once
|
|
||||||
|
|
||||||
#include <ephysics/raint/Joint.hpp>
|
|
||||||
#include <ephysics/mathematics/mathematics.hpp>
|
|
||||||
|
|
||||||
namespace ephysics {
|
|
||||||
/**
|
/**
|
||||||
* @brief It is used to gather the information needed to create a hinge joint.
|
* @brief It is used to gather the information needed to create a hinge joint.
|
||||||
* This structure will be used to create the actual hinge joint.
|
* This structure will be used to create the actual hinge joint.
|
||||||
*/
|
*/
|
||||||
struct HingeJointInfo : public JointInfo {
|
struct HingeJointInfo extends JointInfo {
|
||||||
public :
|
public :
|
||||||
vec3 this.anchorPointWorldSpace; //!< Anchor point (in world-space coordinates)
|
Vector3f anchorPointWorldSpace; //!< Anchor point (in world-space coordinates)
|
||||||
vec3 rotationAxisWorld; //!< Hinge rotation axis (in world-space coordinates)
|
Vector3f rotationAxisWorld; //!< Hinge rotation axis (in world-space coordinates)
|
||||||
boolean isLimitEnabled; //!< True if the hinge joint limits are enabled
|
boolean isLimitEnabled; //!< True if the hinge joint limits are enabled
|
||||||
boolean isMotorEnabled; //!< True if the hinge joint motor is enabled
|
boolean isMotorEnabled; //!< True if the hinge joint motor is enabled
|
||||||
float minAngleLimit; //!< Minimum allowed rotation angle (in radian) if limits are enabled. The angle must be in the range [-2*pi, 0]
|
float minAngleLimit; //!< Minimum allowed rotation angle (in radian) if limits are enabled. The angle must be in the range [-2*pi, 0]
|
||||||
@ -35,8 +23,8 @@ namespace ephysics {
|
|||||||
*/
|
*/
|
||||||
HingeJointInfo(RigidBody* rigidBody1,
|
HingeJointInfo(RigidBody* rigidBody1,
|
||||||
RigidBody* rigidBody2,
|
RigidBody* rigidBody2,
|
||||||
vec3 initAnchorPointWorldSpace,
|
Vector3f initAnchorPointWorldSpace,
|
||||||
vec3 initRotationAxisWorld):
|
Vector3f initRotationAxisWorld):
|
||||||
JointInfo(rigidBody1, rigidBody2, HINGEJOINT),
|
JointInfo(rigidBody1, rigidBody2, HINGEJOINT),
|
||||||
this.anchorPointWorldSpace(initAnchorPointWorldSpace),
|
this.anchorPointWorldSpace(initAnchorPointWorldSpace),
|
||||||
rotationAxisWorld(initRotationAxisWorld),
|
rotationAxisWorld(initRotationAxisWorld),
|
||||||
@ -59,8 +47,8 @@ namespace ephysics {
|
|||||||
*/
|
*/
|
||||||
HingeJointInfo(RigidBody* rigidBody1,
|
HingeJointInfo(RigidBody* rigidBody1,
|
||||||
RigidBody* rigidBody2,
|
RigidBody* rigidBody2,
|
||||||
vec3 initAnchorPointWorldSpace,
|
Vector3f initAnchorPointWorldSpace,
|
||||||
vec3 initRotationAxisWorld,
|
Vector3f initRotationAxisWorld,
|
||||||
float initMinAngleLimit,
|
float initMinAngleLimit,
|
||||||
float initMaxAngleLimit):
|
float initMaxAngleLimit):
|
||||||
JointInfo(rigidBody1, rigidBody2, HINGEJOINT),
|
JointInfo(rigidBody1, rigidBody2, HINGEJOINT),
|
||||||
@ -87,8 +75,8 @@ namespace ephysics {
|
|||||||
*/
|
*/
|
||||||
HingeJointInfo(RigidBody* rigidBody1,
|
HingeJointInfo(RigidBody* rigidBody1,
|
||||||
RigidBody* rigidBody2,
|
RigidBody* rigidBody2,
|
||||||
vec3 initAnchorPointWorldSpace,
|
Vector3f initAnchorPointWorldSpace,
|
||||||
vec3 initRotationAxisWorld,
|
Vector3f initRotationAxisWorld,
|
||||||
float initMinAngleLimit,
|
float initMinAngleLimit,
|
||||||
float initMaxAngleLimit,
|
float initMaxAngleLimit,
|
||||||
float initMotorSpeed,
|
float initMotorSpeed,
|
||||||
@ -111,46 +99,42 @@ namespace ephysics {
|
|||||||
* between two bodies around a single axis. This joint has one degree of freedom. It
|
* between two bodies around a single axis. This joint has one degree of freedom. It
|
||||||
* can be useful to simulate doors or pendulumns.
|
* can be useful to simulate doors or pendulumns.
|
||||||
*/
|
*/
|
||||||
class HingeJoint : public Joint {
|
class HingeJoint extends Joint {
|
||||||
private :
|
private :
|
||||||
static float BETA; //!< Beta value for the bias factor of position correction
|
static float BETA; //!< Beta value for the bias factor of position correction
|
||||||
vec3 this.localAnchorPointBody1; //!< Anchor point of body 1 (in local-space coordinates of body 1)
|
Vector3f localAnchorPointBody1; //!< Anchor point of body 1 (in local-space coordinates of body 1)
|
||||||
vec3 this.localAnchorPointBody2; //!< Anchor point of body 2 (in local-space coordinates of body 2)
|
Vector3f localAnchorPointBody2; //!< Anchor point of body 2 (in local-space coordinates of body 2)
|
||||||
vec3 this.hingeLocalAxisBody1; //!< Hinge rotation axis (in local-space coordinates of body 1)
|
Vector3f hingeLocalAxisBody1; //!< Hinge rotation axis (in local-space coordinates of body 1)
|
||||||
vec3 this.hingeLocalAxisBody2; //!< Hinge rotation axis (in local-space coordiantes of body 2)
|
Vector3f hingeLocalAxisBody2; //!< Hinge rotation axis (in local-space coordiantes of body 2)
|
||||||
etk::Matrix3x3 this.i1; //!< Inertia tensor of body 1 (in world-space coordinates)
|
Matrix3f i1; //!< Inertia tensor of body 1 (in world-space coordinates)
|
||||||
etk::Matrix3x3 this.i2; //!< Inertia tensor of body 2 (in world-space coordinates)
|
Matrix3f i2; //!< Inertia tensor of body 2 (in world-space coordinates)
|
||||||
vec3 mA1; //!< Hinge rotation axis (in world-space coordinates) computed from body 1
|
Vector3f mA1; //!< Hinge rotation axis (in world-space coordinates) computed from body 1
|
||||||
vec3 this.r1World; //!< Vector from center of body 2 to anchor point in world-space
|
Vector3f r1World; //!< Vector from center of body 2 to anchor point in world-space
|
||||||
vec3 this.r2World; //!< Vector from center of body 2 to anchor point in world-space
|
Vector3f r2World; //!< Vector from center of body 2 to anchor point in world-space
|
||||||
vec3 this.b2CrossA1; //!< Cross product of vector b2 and a1
|
Vector3f b2CrossA1; //!< Cross product of vector b2 and a1
|
||||||
vec3 this.c2CrossA1; //!< Cross product of vector c2 and a1;
|
Vector3f c2CrossA1; //!< Cross product of vector c2 and a1;
|
||||||
vec3 this.impulseTranslation; //!< Impulse for the 3 translation raints
|
Vector3f impulseTranslation; //!< Impulse for the 3 translation raints
|
||||||
vec2 this.impulseRotation; //!< Impulse for the 2 rotation raints
|
vec2 impulseRotation; //!< Impulse for the 2 rotation raints
|
||||||
float this.impulseLowerLimit; //!< Accumulated impulse for the lower limit raint
|
float impulseLowerLimit; //!< Accumulated impulse for the lower limit raint
|
||||||
float this.impulseUpperLimit; //!< Accumulated impulse for the upper limit raint
|
float impulseUpperLimit; //!< Accumulated impulse for the upper limit raint
|
||||||
float this.impulseMotor; //!< Accumulated impulse for the motor raint;
|
float impulseMotor; //!< Accumulated impulse for the motor raint;
|
||||||
etk::Matrix3x3 this.inverseMassMatrixTranslation; //!< Inverse mass matrix K=JM^-1J^t for the 3 translation raints
|
Matrix3f inverseMassMatrixTranslation; //!< Inverse mass matrix K=JM^-1J^t for the 3 translation raints
|
||||||
etk::Matrix2x2 this.inverseMassMatrixRotation; //!< Inverse mass matrix K=JM^-1J^t for the 2 rotation raints
|
Matrix2x2 inverseMassMatrixRotation; //!< Inverse mass matrix K=JM^-1J^t for the 2 rotation raints
|
||||||
float this.inverseMassMatrixLimitMotor; //!< Inverse of mass matrix K=JM^-1J^t for the limits and motor raints (1x1 matrix)
|
float inverseMassMatrixLimitMotor; //!< Inverse of mass matrix K=JM^-1J^t for the limits and motor raints (1x1 matrix)
|
||||||
float this.inverseMassMatrixMotor; //!< Inverse of mass matrix K=JM^-1J^t for the motor
|
float inverseMassMatrixMotor; //!< Inverse of mass matrix K=JM^-1J^t for the motor
|
||||||
vec3 this.bTranslation; //!< Bias vector for the error correction for the translation raints
|
Vector3f bTranslation; //!< Bias vector for the error correction for the translation raints
|
||||||
vec2 this.bRotation; //!< Bias vector for the error correction for the rotation raints
|
vec2 bRotation; //!< Bias vector for the error correction for the rotation raints
|
||||||
float this.bLowerLimit; //!< Bias of the lower limit raint
|
float bLowerLimit; //!< Bias of the lower limit raint
|
||||||
float this.bUpperLimit; //!< Bias of the upper limit raint
|
float bUpperLimit; //!< Bias of the upper limit raint
|
||||||
etk::Quaternion this.initOrientationDifferenceInv; //!< Inverse of the initial orientation difference between the bodies
|
Quaternion initOrientationDifferenceInv; //!< Inverse of the initial orientation difference between the bodies
|
||||||
boolean this.isLimitEnabled; //!< True if the joint limits are enabled
|
boolean isLimitEnabled; //!< True if the joint limits are enabled
|
||||||
boolean this.isMotorEnabled; //!< True if the motor of the joint in enabled
|
boolean isMotorEnabled; //!< True if the motor of the joint in enabled
|
||||||
float this.lowerLimit; //!< Lower limit (minimum allowed rotation angle in radian)
|
float lowerLimit; //!< Lower limit (minimum allowed rotation angle in radian)
|
||||||
float this.upperLimit; //!< Upper limit (maximum translation distance)
|
float upperLimit; //!< Upper limit (maximum translation distance)
|
||||||
boolean this.isLowerLimitViolated; //!< True if the lower limit is violated
|
boolean isLowerLimitViolated; //!< True if the lower limit is violated
|
||||||
boolean this.isUpperLimitViolated; //!< True if the upper limit is violated
|
boolean isUpperLimitViolated; //!< True if the upper limit is violated
|
||||||
float this.motorSpeed; //!< Motor speed (in rad/s)
|
float motorSpeed; //!< Motor speed (in rad/s)
|
||||||
float this.maxMotorTorque; //!< Maximum motor torque (in Newtons) that can be applied to reach to desired motor speed
|
float maxMotorTorque; //!< Maximum motor torque (in Newtons) that can be applied to reach to desired motor speed
|
||||||
/// Private copy-ructor
|
|
||||||
HingeJoint( HingeJoint raint);
|
|
||||||
/// Private assignment operator
|
|
||||||
HingeJoint operator=( HingeJoint raint);
|
|
||||||
/// Reset the limits
|
/// Reset the limits
|
||||||
void resetLimits();
|
void resetLimits();
|
||||||
/// Given an angle in radian, this method returns the corresponding
|
/// Given an angle in radian, this method returns the corresponding
|
||||||
@ -163,17 +147,15 @@ namespace ephysics {
|
|||||||
float lowerLimitAngle,
|
float lowerLimitAngle,
|
||||||
float upperLimitAngle) ;
|
float upperLimitAngle) ;
|
||||||
/// Compute the current angle around the hinge axis
|
/// Compute the current angle around the hinge axis
|
||||||
float computeCurrentHingeAngle( etk::Quaternion orientationBody1, etk::Quaternion orientationBody2);
|
float computeCurrentHingeAngle( Quaternion orientationBody1, Quaternion orientationBody2);
|
||||||
sizet getSizeInBytes() override;
|
long getSizeInBytes() ;
|
||||||
void initBeforeSolve( ConstraintSolverData raintSolverData) override;
|
void initBeforeSolve( ConstraintSolverData raintSolverData) ;
|
||||||
void warmstart( ConstraintSolverData raintSolverData) override;
|
void warmstart( ConstraintSolverData raintSolverData) ;
|
||||||
void solveVelocityConstraint( ConstraintSolverData raintSolverData) override;
|
void solveVelocityConstraint( ConstraintSolverData raintSolverData) ;
|
||||||
void solvePositionConstraint( ConstraintSolverData raintSolverData) override;
|
void solvePositionConstraint( ConstraintSolverData raintSolverData) ;
|
||||||
public :
|
public :
|
||||||
/// Constructor
|
/// Constructor
|
||||||
HingeJoint( HingeJointInfo jointInfo);
|
HingeJoint( HingeJointInfo jointInfo);
|
||||||
/// Destructor
|
|
||||||
virtual ~HingeJoint();
|
|
||||||
/// Return true if the limits or the joint are enabled
|
/// Return true if the limits or the joint are enabled
|
||||||
boolean isLimitEnabled() ;
|
boolean isLimitEnabled() ;
|
||||||
/// Return true if the motor of the joint is enabled
|
/// Return true if the motor of the joint is enabled
|
@ -44,7 +44,7 @@ RigidBody* Joint::getBody2() {
|
|||||||
* @return True if the joint is active
|
* @return True if the joint is active
|
||||||
*/
|
*/
|
||||||
boolean Joint::isActive() {
|
boolean Joint::isActive() {
|
||||||
return (this.body1->isActive() hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj this.body2->isActive());
|
return (this.body1.isActive() && this.body2.isActive());
|
||||||
}
|
}
|
||||||
|
|
||||||
// Return the type of the joint
|
// Return the type of the joint
|
||||||
|
@ -1,18 +1,4 @@
|
|||||||
/** @file
|
package org.atriaSoft.ephysics.constraint;
|
||||||
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
|
|
||||||
* @author Daniel CHAPPUIS
|
|
||||||
* @author Edouard DUPIN
|
|
||||||
* @copyright 2010-2016, Daniel Chappuis
|
|
||||||
* @copyright 2017, Edouard DUPIN
|
|
||||||
* @license MPL v2.0 (see license file)
|
|
||||||
*/
|
|
||||||
#pragma once
|
|
||||||
|
|
||||||
#include <ephysics/configuration.hpp>
|
|
||||||
#include <ephysics/body/RigidBody.hpp>
|
|
||||||
#include <ephysics/mathematics/mathematics.hpp>
|
|
||||||
|
|
||||||
namespace ephysics {
|
|
||||||
|
|
||||||
/// Enumeration for the type of a raint
|
/// Enumeration for the type of a raint
|
||||||
enum JointType {BALLSOCKETJOINT, SLIDERJOINT, HINGEJOINT, FIXEDJOINT};
|
enum JointType {BALLSOCKETJOINT, SLIDERJOINT, HINGEJOINT, FIXEDJOINT};
|
||||||
@ -69,7 +55,7 @@ namespace ephysics {
|
|||||||
|
|
||||||
}
|
}
|
||||||
/// Destructor
|
/// Destructor
|
||||||
virtual ~JointInfo() = default;
|
~JointInfo() = default;
|
||||||
};
|
};
|
||||||
|
|
||||||
/**
|
/**
|
||||||
@ -77,14 +63,14 @@ namespace ephysics {
|
|||||||
*/
|
*/
|
||||||
class Joint {
|
class Joint {
|
||||||
protected :
|
protected :
|
||||||
RigidBody* this.body1; //!< Pointer to the first body of the joint
|
RigidBody* body1; //!< Pointer to the first body of the joint
|
||||||
RigidBody* this.body2; //!< Pointer to the second body of the joint
|
RigidBody* body2; //!< Pointer to the second body of the joint
|
||||||
JointType this.type; //!< Type of the joint
|
JointType type; //!< Type of the joint
|
||||||
int this.indexBody1; //!< Body 1 index in the velocity array to solve the raint
|
int indexBody1; //!< Body 1 index in the velocity array to solve the raint
|
||||||
int this.indexBody2; //!< Body 2 index in the velocity array to solve the raint
|
int indexBody2; //!< Body 2 index in the velocity array to solve the raint
|
||||||
JointsPositionCorrectionTechnique this.positionCorrectionTechnique; //!< Position correction technique used for the raint (used for joints)
|
JointsPositionCorrectionTechnique positionCorrectionTechnique; //!< Position correction technique used for the raint (used for joints)
|
||||||
boolean this.isCollisionEnabled; //!< True if the two bodies of the raint are allowed to collide with each other
|
boolean isCollisionEnabled; //!< True if the two bodies of the raint are allowed to collide with each other
|
||||||
boolean this.isAlreadyInIsland; //!< True if the joint has already been added into an island
|
boolean isAlreadyInIsland; //!< True if the joint has already been added into an island
|
||||||
/// Private copy-ructor
|
/// Private copy-ructor
|
||||||
Joint( Joint raint);
|
Joint( Joint raint);
|
||||||
/// Private assignment operator
|
/// Private assignment operator
|
||||||
@ -92,20 +78,20 @@ namespace ephysics {
|
|||||||
/// Return true if the joint has already been added into an island
|
/// Return true if the joint has already been added into an island
|
||||||
boolean isAlreadyInIsland() ;
|
boolean isAlreadyInIsland() ;
|
||||||
/// Return the number of bytes used by the joint
|
/// Return the number of bytes used by the joint
|
||||||
virtual sizet getSizeInBytes() = 0;
|
long getSizeInBytes() = 0;
|
||||||
/// Initialize before solving the joint
|
/// Initialize before solving the joint
|
||||||
virtual void initBeforeSolve( ConstraintSolverData raintSolverData) = 0;
|
void initBeforeSolve( ConstraintSolverData raintSolverData) = 0;
|
||||||
/// Warm start the joint (apply the previous impulse at the beginning of the step)
|
/// Warm start the joint (apply the previous impulse at the beginning of the step)
|
||||||
virtual void warmstart( ConstraintSolverData raintSolverData) = 0;
|
void warmstart( ConstraintSolverData raintSolverData) = 0;
|
||||||
/// Solve the velocity raint
|
/// Solve the velocity raint
|
||||||
virtual void solveVelocityConstraint( ConstraintSolverData raintSolverData) = 0;
|
void solveVelocityConstraint( ConstraintSolverData raintSolverData) = 0;
|
||||||
/// Solve the position raint
|
/// Solve the position raint
|
||||||
virtual void solvePositionConstraint( ConstraintSolverData raintSolverData) = 0;
|
void solvePositionConstraint( ConstraintSolverData raintSolverData) = 0;
|
||||||
public :
|
public :
|
||||||
/// Constructor
|
/// Constructor
|
||||||
Joint( JointInfo jointInfo);
|
Joint( JointInfo jointInfo);
|
||||||
/// Destructor
|
/// Destructor
|
||||||
virtual ~Joint();
|
~Joint();
|
||||||
/// Return the reference to the body 1
|
/// Return the reference to the body 1
|
||||||
RigidBody* getBody1() ;
|
RigidBody* getBody1() ;
|
||||||
/// Return the reference to the body 2
|
/// Return the reference to the body 2
|
@ -28,10 +28,10 @@ SliderJoint::SliderJoint( SliderJointInfo jointInfo)
|
|||||||
assert(this.maxMotorForce >= 0.0);
|
assert(this.maxMotorForce >= 0.0);
|
||||||
|
|
||||||
// Compute the local-space anchor point for each body
|
// Compute the local-space anchor point for each body
|
||||||
etk::Transform3D transform1 = this.body1->getTransform();
|
Transform3D transform1 = this.body1.getTransform();
|
||||||
etk::Transform3D transform2 = this.body2->getTransform();
|
Transform3D transform2 = this.body2.getTransform();
|
||||||
this.localAnchorPointBody1 = transform1.getInverse() * jointInfo.this.anchorPointWorldSpace;
|
this.localAnchorPointBody1 = transform1.getInverse() * jointInfo.anchorPointWorldSpace;
|
||||||
this.localAnchorPointBody2 = transform2.getInverse() * jointInfo.this.anchorPointWorldSpace;
|
this.localAnchorPointBody2 = transform2.getInverse() * jointInfo.anchorPointWorldSpace;
|
||||||
|
|
||||||
// Compute the inverse of the initial orientation difference between the two bodies
|
// Compute the inverse of the initial orientation difference between the two bodies
|
||||||
this.initOrientationDifferenceInv = transform2.getOrientation() *
|
this.initOrientationDifferenceInv = transform2.getOrientation() *
|
||||||
@ -40,7 +40,7 @@ SliderJoint::SliderJoint( SliderJointInfo jointInfo)
|
|||||||
this.initOrientationDifferenceInv.inverse();
|
this.initOrientationDifferenceInv.inverse();
|
||||||
|
|
||||||
// Compute the slider axis in local-space of body 1
|
// Compute the slider axis in local-space of body 1
|
||||||
this.sliderAxisBody1 = this.body1->getTransform().getOrientation().getInverse() *
|
this.sliderAxisBody1 = this.body1.getTransform().getOrientation().getInverse() *
|
||||||
jointInfo.sliderAxisWorldSpace;
|
jointInfo.sliderAxisWorldSpace;
|
||||||
this.sliderAxisBody1.normalize();
|
this.sliderAxisBody1.normalize();
|
||||||
}
|
}
|
||||||
@ -54,25 +54,25 @@ SliderJoint::~SliderJoint() {
|
|||||||
void SliderJoint::initBeforeSolve( ConstraintSolverData raintSolverData) {
|
void SliderJoint::initBeforeSolve( ConstraintSolverData raintSolverData) {
|
||||||
|
|
||||||
// Initialize the bodies index in the veloc ity array
|
// Initialize the bodies index in the veloc ity array
|
||||||
this.indexBody1 = raintSolverData.mapBodyToConstrainedVelocityIndex.find(this.body1)->second;
|
this.indexBody1 = raintSolverData.mapBodyToConstrainedVelocityIndex.find(this.body1).second;
|
||||||
this.indexBody2 = raintSolverData.mapBodyToConstrainedVelocityIndex.find(this.body2)->second;
|
this.indexBody2 = raintSolverData.mapBodyToConstrainedVelocityIndex.find(this.body2).second;
|
||||||
|
|
||||||
// Get the bodies positions and orientations
|
// Get the bodies positions and orientations
|
||||||
vec3 x1 = this.body1->this.centerOfMassWorld;
|
Vector3f x1 = this.body1.this.centerOfMassWorld;
|
||||||
vec3 x2 = this.body2->this.centerOfMassWorld;
|
Vector3f x2 = this.body2.this.centerOfMassWorld;
|
||||||
etk::Quaternion orientationBody1 = this.body1->getTransform().getOrientation();
|
Quaternion orientationBody1 = this.body1.getTransform().getOrientation();
|
||||||
etk::Quaternion orientationBody2 = this.body2->getTransform().getOrientation();
|
Quaternion orientationBody2 = this.body2.getTransform().getOrientation();
|
||||||
|
|
||||||
// Get the inertia tensor of bodies
|
// Get the inertia tensor of bodies
|
||||||
this.i1 = this.body1->getInertiaTensorInverseWorld();
|
this.i1 = this.body1.getInertiaTensorInverseWorld();
|
||||||
this.i2 = this.body2->getInertiaTensorInverseWorld();
|
this.i2 = this.body2.getInertiaTensorInverseWorld();
|
||||||
|
|
||||||
// Vector from body center to the anchor point
|
// Vector from body center to the anchor point
|
||||||
this.R1 = orientationBody1 * this.localAnchorPointBody1;
|
this.R1 = orientationBody1 * this.localAnchorPointBody1;
|
||||||
this.R2 = orientationBody2 * this.localAnchorPointBody2;
|
this.R2 = orientationBody2 * this.localAnchorPointBody2;
|
||||||
|
|
||||||
// Compute the vector u (difference between anchor points)
|
// Compute the vector u (difference between anchor points)
|
||||||
vec3 u = x2 + this.R2 - x1 - this.R1;
|
Vector3f u = x2 + this.R2 - x1 - this.R1;
|
||||||
|
|
||||||
// Compute the two orthogonal vectors to the slider axis in world-space
|
// Compute the two orthogonal vectors to the slider axis in world-space
|
||||||
this.sliderAxisWorld = orientationBody1 * this.sliderAxisBody1;
|
this.sliderAxisWorld = orientationBody1 * this.sliderAxisBody1;
|
||||||
@ -99,18 +99,18 @@ void SliderJoint::initBeforeSolve( ConstraintSolverData raintSolverData) {
|
|||||||
this.R2CrossN1 = this.R2.cross(this.N1);
|
this.R2CrossN1 = this.R2.cross(this.N1);
|
||||||
this.R2CrossN2 = this.R2.cross(this.N2);
|
this.R2CrossN2 = this.R2.cross(this.N2);
|
||||||
this.R2CrossSliderAxis = this.R2.cross(this.sliderAxisWorld);
|
this.R2CrossSliderAxis = this.R2.cross(this.sliderAxisWorld);
|
||||||
vec3 r1PlusU = this.R1 + u;
|
Vector3f r1PlusU = this.R1 + u;
|
||||||
this.R1PlusUCrossN1 = (r1PlusU).cross(this.N1);
|
this.R1PlusUCrossN1 = (r1PlusU).cross(this.N1);
|
||||||
this.R1PlusUCrossN2 = (r1PlusU).cross(this.N2);
|
this.R1PlusUCrossN2 = (r1PlusU).cross(this.N2);
|
||||||
this.R1PlusUCrossSliderAxis = (r1PlusU).cross(this.sliderAxisWorld);
|
this.R1PlusUCrossSliderAxis = (r1PlusU).cross(this.sliderAxisWorld);
|
||||||
|
|
||||||
// Compute the inverse of the mass matrix K=JM^-1J^t for the 2 translation
|
// Compute the inverse of the mass matrix K=JM^-1J^t for the 2 translation
|
||||||
// raints (2x2 matrix)
|
// raints (2x2 matrix)
|
||||||
float sumInverseMass = this.body1->this.massInverse + this.body2->this.massInverse;
|
float sumInverseMass = this.body1.this.massInverse + this.body2.this.massInverse;
|
||||||
vec3 I1R1PlusUCrossN1 = this.i1 * this.R1PlusUCrossN1;
|
Vector3f I1R1PlusUCrossN1 = this.i1 * this.R1PlusUCrossN1;
|
||||||
vec3 I1R1PlusUCrossN2 = this.i1 * this.R1PlusUCrossN2;
|
Vector3f I1R1PlusUCrossN2 = this.i1 * this.R1PlusUCrossN2;
|
||||||
vec3 I2R2CrossN1 = this.i2 * this.R2CrossN1;
|
Vector3f I2R2CrossN1 = this.i2 * this.R2CrossN1;
|
||||||
vec3 I2R2CrossN2 = this.i2 * this.R2CrossN2;
|
Vector3f I2R2CrossN2 = this.i2 * this.R2CrossN2;
|
||||||
float el11 = sumInverseMass + this.R1PlusUCrossN1.dot(I1R1PlusUCrossN1) +
|
float el11 = sumInverseMass + this.R1PlusUCrossN1.dot(I1R1PlusUCrossN1) +
|
||||||
this.R2CrossN1.dot(I2R2CrossN1);
|
this.R2CrossN1.dot(I2R2CrossN1);
|
||||||
float el12 = this.R1PlusUCrossN1.dot(I1R1PlusUCrossN2) +
|
float el12 = this.R1PlusUCrossN1.dot(I1R1PlusUCrossN2) +
|
||||||
@ -119,9 +119,9 @@ void SliderJoint::initBeforeSolve( ConstraintSolverData raintSolverData) {
|
|||||||
this.R2CrossN2.dot(I2R2CrossN1);
|
this.R2CrossN2.dot(I2R2CrossN1);
|
||||||
float el22 = sumInverseMass + this.R1PlusUCrossN2.dot(I1R1PlusUCrossN2) +
|
float el22 = sumInverseMass + this.R1PlusUCrossN2.dot(I1R1PlusUCrossN2) +
|
||||||
this.R2CrossN2.dot(I2R2CrossN2);
|
this.R2CrossN2.dot(I2R2CrossN2);
|
||||||
etk::Matrix2x2 matrixKTranslation(el11, el12, el21, el22);
|
Matrix2x2 matrixKTranslation(el11, el12, el21, el22);
|
||||||
this.inverseMassMatrixTranslationConstraint.setZero();
|
this.inverseMassMatrixTranslationConstraint.setZero();
|
||||||
if (this.body1->getType() == DYNAMIC || this.body2->getType() == DYNAMIC) {
|
if (this.body1.getType() == DYNAMIC || this.body2.getType() == DYNAMIC) {
|
||||||
this.inverseMassMatrixTranslationConstraint = matrixKTranslation.getInverse();
|
this.inverseMassMatrixTranslationConstraint = matrixKTranslation.getInverse();
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -137,24 +137,24 @@ void SliderJoint::initBeforeSolve( ConstraintSolverData raintSolverData) {
|
|||||||
// Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation
|
// Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation
|
||||||
// contraints (3x3 matrix)
|
// contraints (3x3 matrix)
|
||||||
this.inverseMassMatrixRotationConstraint = this.i1 + this.i2;
|
this.inverseMassMatrixRotationConstraint = this.i1 + this.i2;
|
||||||
if (this.body1->getType() == DYNAMIC || this.body2->getType() == DYNAMIC) {
|
if (this.body1.getType() == DYNAMIC || this.body2.getType() == DYNAMIC) {
|
||||||
this.inverseMassMatrixRotationConstraint = this.inverseMassMatrixRotationConstraint.getInverse();
|
this.inverseMassMatrixRotationConstraint = this.inverseMassMatrixRotationConstraint.getInverse();
|
||||||
}
|
}
|
||||||
|
|
||||||
// Compute the bias "b" of the rotation raint
|
// Compute the bias "b" of the rotation raint
|
||||||
this.bRotation.setZero();
|
this.bRotation.setZero();
|
||||||
if (this.positionCorrectionTechnique == BAUMGARTEJOINTS) {
|
if (this.positionCorrectionTechnique == BAUMGARTEJOINTS) {
|
||||||
etk::Quaternion currentOrientationDifference = orientationBody2 * orientationBody1.getInverse();
|
Quaternion currentOrientationDifference = orientationBody2 * orientationBody1.getInverse();
|
||||||
currentOrientationDifference.normalize();
|
currentOrientationDifference.normalize();
|
||||||
etk::Quaternion qError = currentOrientationDifference * this.initOrientationDifferenceInv;
|
Quaternion qError = currentOrientationDifference * this.initOrientationDifferenceInv;
|
||||||
this.bRotation = biasFactor * float(2.0) * qError.getVectorV();
|
this.bRotation = biasFactor * float(2.0) * qError.getVectorV();
|
||||||
}
|
}
|
||||||
|
|
||||||
// If the limits are enabled
|
// If the limits are enabled
|
||||||
if (this.isLimitEnabled hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj (this.isLowerLimitViolated || this.isUpperLimitViolated)) {
|
if (this.isLimitEnabled && (this.isLowerLimitViolated || this.isUpperLimitViolated)) {
|
||||||
|
|
||||||
// Compute the inverse of the mass matrix K=JM^-1J^t for the limits (1x1 matrix)
|
// Compute the inverse of the mass matrix K=JM^-1J^t for the limits (1x1 matrix)
|
||||||
this.inverseMassMatrixLimit = this.body1->this.massInverse + this.body2->this.massInverse +
|
this.inverseMassMatrixLimit = this.body1.this.massInverse + this.body2.this.massInverse +
|
||||||
this.R1PlusUCrossSliderAxis.dot(this.i1 * this.R1PlusUCrossSliderAxis) +
|
this.R1PlusUCrossSliderAxis.dot(this.i1 * this.R1PlusUCrossSliderAxis) +
|
||||||
this.R2CrossSliderAxis.dot(this.i2 * this.R2CrossSliderAxis);
|
this.R2CrossSliderAxis.dot(this.i2 * this.R2CrossSliderAxis);
|
||||||
this.inverseMassMatrixLimit = (this.inverseMassMatrixLimit > 0.0) ?
|
this.inverseMassMatrixLimit = (this.inverseMassMatrixLimit > 0.0) ?
|
||||||
@ -177,7 +177,7 @@ void SliderJoint::initBeforeSolve( ConstraintSolverData raintSolverData) {
|
|||||||
if (this.isMotorEnabled) {
|
if (this.isMotorEnabled) {
|
||||||
|
|
||||||
// Compute the inverse of mass matrix K=JM^-1J^t for the motor (1x1 matrix)
|
// Compute the inverse of mass matrix K=JM^-1J^t for the motor (1x1 matrix)
|
||||||
this.inverseMassMatrixMotor = this.body1->this.massInverse + this.body2->this.massInverse;
|
this.inverseMassMatrixMotor = this.body1.this.massInverse + this.body2.this.massInverse;
|
||||||
this.inverseMassMatrixMotor = (this.inverseMassMatrixMotor > 0.0) ?
|
this.inverseMassMatrixMotor = (this.inverseMassMatrixMotor > 0.0) ?
|
||||||
1.0f / this.inverseMassMatrixMotor : 0.0f;
|
1.0f / this.inverseMassMatrixMotor : 0.0f;
|
||||||
}
|
}
|
||||||
@ -198,25 +198,25 @@ void SliderJoint::initBeforeSolve( ConstraintSolverData raintSolverData) {
|
|||||||
void SliderJoint::warmstart( ConstraintSolverData raintSolverData) {
|
void SliderJoint::warmstart( ConstraintSolverData raintSolverData) {
|
||||||
|
|
||||||
// Get the velocities
|
// Get the velocities
|
||||||
vec3 v1 = raintSolverData.linearVelocities[this.indexBody1];
|
Vector3f v1 = raintSolverData.linearVelocities[this.indexBody1];
|
||||||
vec3 v2 = raintSolverData.linearVelocities[this.indexBody2];
|
Vector3f v2 = raintSolverData.linearVelocities[this.indexBody2];
|
||||||
vec3 w1 = raintSolverData.angularVelocities[this.indexBody1];
|
Vector3f w1 = raintSolverData.angularVelocities[this.indexBody1];
|
||||||
vec3 w2 = raintSolverData.angularVelocities[this.indexBody2];
|
Vector3f w2 = raintSolverData.angularVelocities[this.indexBody2];
|
||||||
|
|
||||||
// Get the inverse mass and inverse inertia tensors of the bodies
|
// Get the inverse mass and inverse inertia tensors of the bodies
|
||||||
float inverseMassBody1 = this.body1->this.massInverse;
|
float inverseMassBody1 = this.body1.this.massInverse;
|
||||||
float inverseMassBody2 = this.body2->this.massInverse;
|
float inverseMassBody2 = this.body2.this.massInverse;
|
||||||
|
|
||||||
// Compute the impulse P=J^T * lambda for the lower and upper limits raints of body 1
|
// Compute the impulse P=J^T * lambda for the lower and upper limits raints of body 1
|
||||||
float impulseLimits = this.impulseUpperLimit - this.impulseLowerLimit;
|
float impulseLimits = this.impulseUpperLimit - this.impulseLowerLimit;
|
||||||
vec3 linearImpulseLimits = impulseLimits * this.sliderAxisWorld;
|
Vector3f linearImpulseLimits = impulseLimits * this.sliderAxisWorld;
|
||||||
|
|
||||||
// Compute the impulse P=J^T * lambda for the motor raint of body 1
|
// Compute the impulse P=J^T * lambda for the motor raint of body 1
|
||||||
vec3 impulseMotor = this.impulseMotor * this.sliderAxisWorld;
|
Vector3f impulseMotor = this.impulseMotor * this.sliderAxisWorld;
|
||||||
|
|
||||||
// Compute the impulse P=J^T * lambda for the 2 translation raints of body 1
|
// Compute the impulse P=J^T * lambda for the 2 translation raints of body 1
|
||||||
vec3 linearImpulseBody1 = -this.N1 * this.impulseTranslation.x() - this.N2 * this.impulseTranslation.y();
|
Vector3f linearImpulseBody1 = -this.N1 * this.impulseTranslation.x() - this.N2 * this.impulseTranslation.y();
|
||||||
vec3 angularImpulseBody1 = -this.R1PlusUCrossN1 * this.impulseTranslation.x() -
|
Vector3f angularImpulseBody1 = -this.R1PlusUCrossN1 * this.impulseTranslation.x() -
|
||||||
this.R1PlusUCrossN2 * this.impulseTranslation.y();
|
this.R1PlusUCrossN2 * this.impulseTranslation.y();
|
||||||
|
|
||||||
// Compute the impulse P=J^T * lambda for the 3 rotation raints of body 1
|
// Compute the impulse P=J^T * lambda for the 3 rotation raints of body 1
|
||||||
@ -234,8 +234,8 @@ void SliderJoint::warmstart( ConstraintSolverData raintSolverData) {
|
|||||||
w1 += this.i1 * angularImpulseBody1;
|
w1 += this.i1 * angularImpulseBody1;
|
||||||
|
|
||||||
// Compute the impulse P=J^T * lambda for the 2 translation raints of body 2
|
// Compute the impulse P=J^T * lambda for the 2 translation raints of body 2
|
||||||
vec3 linearImpulseBody2 = this.N1 * this.impulseTranslation.x() + this.N2 * this.impulseTranslation.y();
|
Vector3f linearImpulseBody2 = this.N1 * this.impulseTranslation.x() + this.N2 * this.impulseTranslation.y();
|
||||||
vec3 angularImpulseBody2 = this.R2CrossN1 * this.impulseTranslation.x() +
|
Vector3f angularImpulseBody2 = this.R2CrossN1 * this.impulseTranslation.x() +
|
||||||
this.R2CrossN2 * this.impulseTranslation.y();
|
this.R2CrossN2 * this.impulseTranslation.y();
|
||||||
|
|
||||||
// Compute the impulse P=J^T * lambda for the 3 rotation raints of body 2
|
// Compute the impulse P=J^T * lambda for the 3 rotation raints of body 2
|
||||||
@ -257,14 +257,14 @@ void SliderJoint::warmstart( ConstraintSolverData raintSolverData) {
|
|||||||
void SliderJoint::solveVelocityConstraint( ConstraintSolverData raintSolverData) {
|
void SliderJoint::solveVelocityConstraint( ConstraintSolverData raintSolverData) {
|
||||||
|
|
||||||
// Get the velocities
|
// Get the velocities
|
||||||
vec3 v1 = raintSolverData.linearVelocities[this.indexBody1];
|
Vector3f v1 = raintSolverData.linearVelocities[this.indexBody1];
|
||||||
vec3 v2 = raintSolverData.linearVelocities[this.indexBody2];
|
Vector3f v2 = raintSolverData.linearVelocities[this.indexBody2];
|
||||||
vec3 w1 = raintSolverData.angularVelocities[this.indexBody1];
|
Vector3f w1 = raintSolverData.angularVelocities[this.indexBody1];
|
||||||
vec3 w2 = raintSolverData.angularVelocities[this.indexBody2];
|
Vector3f w2 = raintSolverData.angularVelocities[this.indexBody2];
|
||||||
|
|
||||||
// Get the inverse mass and inverse inertia tensors of the bodies
|
// Get the inverse mass and inverse inertia tensors of the bodies
|
||||||
float inverseMassBody1 = this.body1->this.massInverse;
|
float inverseMassBody1 = this.body1.this.massInverse;
|
||||||
float inverseMassBody2 = this.body2->this.massInverse;
|
float inverseMassBody2 = this.body2.this.massInverse;
|
||||||
|
|
||||||
// --------------- Translation Constraints --------------- //
|
// --------------- Translation Constraints --------------- //
|
||||||
|
|
||||||
@ -280,8 +280,8 @@ void SliderJoint::solveVelocityConstraint( ConstraintSolverData raintSolverData)
|
|||||||
this.impulseTranslation += deltaLambda;
|
this.impulseTranslation += deltaLambda;
|
||||||
|
|
||||||
// Compute the impulse P=J^T * lambda for the 2 translation raints of body 1
|
// Compute the impulse P=J^T * lambda for the 2 translation raints of body 1
|
||||||
vec3 linearImpulseBody1 = -this.N1 * deltaLambda.x() - this.N2 * deltaLambda.y();
|
Vector3f linearImpulseBody1 = -this.N1 * deltaLambda.x() - this.N2 * deltaLambda.y();
|
||||||
vec3 angularImpulseBody1 = -this.R1PlusUCrossN1 * deltaLambda.x() -
|
Vector3f angularImpulseBody1 = -this.R1PlusUCrossN1 * deltaLambda.x() -
|
||||||
this.R1PlusUCrossN2 * deltaLambda.y();
|
this.R1PlusUCrossN2 * deltaLambda.y();
|
||||||
|
|
||||||
// Apply the impulse to the body 1
|
// Apply the impulse to the body 1
|
||||||
@ -289,8 +289,8 @@ void SliderJoint::solveVelocityConstraint( ConstraintSolverData raintSolverData)
|
|||||||
w1 += this.i1 * angularImpulseBody1;
|
w1 += this.i1 * angularImpulseBody1;
|
||||||
|
|
||||||
// Compute the impulse P=J^T * lambda for the 2 translation raints of body 2
|
// Compute the impulse P=J^T * lambda for the 2 translation raints of body 2
|
||||||
vec3 linearImpulseBody2 = this.N1 * deltaLambda.x() + this.N2 * deltaLambda.y();
|
Vector3f linearImpulseBody2 = this.N1 * deltaLambda.x() + this.N2 * deltaLambda.y();
|
||||||
vec3 angularImpulseBody2 = this.R2CrossN1 * deltaLambda.x() + this.R2CrossN2 * deltaLambda.y();
|
Vector3f angularImpulseBody2 = this.R2CrossN1 * deltaLambda.x() + this.R2CrossN2 * deltaLambda.y();
|
||||||
|
|
||||||
// Apply the impulse to the body 2
|
// Apply the impulse to the body 2
|
||||||
v2 += inverseMassBody2 * linearImpulseBody2;
|
v2 += inverseMassBody2 * linearImpulseBody2;
|
||||||
@ -299,10 +299,10 @@ void SliderJoint::solveVelocityConstraint( ConstraintSolverData raintSolverData)
|
|||||||
// --------------- Rotation Constraints --------------- //
|
// --------------- Rotation Constraints --------------- //
|
||||||
|
|
||||||
// Compute J*v for the 3 rotation raints
|
// Compute J*v for the 3 rotation raints
|
||||||
vec3 JvRotation = w2 - w1;
|
Vector3f JvRotation = w2 - w1;
|
||||||
|
|
||||||
// Compute the Lagrange multiplier lambda for the 3 rotation raints
|
// Compute the Lagrange multiplier lambda for the 3 rotation raints
|
||||||
vec3 deltaLambda2 = this.inverseMassMatrixRotationConstraint * (-JvRotation - this.bRotation);
|
Vector3f deltaLambda2 = this.inverseMassMatrixRotationConstraint * (-JvRotation - this.bRotation);
|
||||||
this.impulseRotation += deltaLambda2;
|
this.impulseRotation += deltaLambda2;
|
||||||
|
|
||||||
// Compute the impulse P=J^T * lambda for the 3 rotation raints of body 1
|
// Compute the impulse P=J^T * lambda for the 3 rotation raints of body 1
|
||||||
@ -331,20 +331,20 @@ void SliderJoint::solveVelocityConstraint( ConstraintSolverData raintSolverData)
|
|||||||
// Compute the Lagrange multiplier lambda for the lower limit raint
|
// Compute the Lagrange multiplier lambda for the lower limit raint
|
||||||
float deltaLambdaLower = this.inverseMassMatrixLimit * (-JvLowerLimit -this.bLowerLimit);
|
float deltaLambdaLower = this.inverseMassMatrixLimit * (-JvLowerLimit -this.bLowerLimit);
|
||||||
float lambdaTemp = this.impulseLowerLimit;
|
float lambdaTemp = this.impulseLowerLimit;
|
||||||
this.impulseLowerLimit = etk::max(this.impulseLowerLimit + deltaLambdaLower, 0.0f);
|
this.impulseLowerLimit = max(this.impulseLowerLimit + deltaLambdaLower, 0.0f);
|
||||||
deltaLambdaLower = this.impulseLowerLimit - lambdaTemp;
|
deltaLambdaLower = this.impulseLowerLimit - lambdaTemp;
|
||||||
|
|
||||||
// Compute the impulse P=J^T * lambda for the lower limit raint of body 1
|
// Compute the impulse P=J^T * lambda for the lower limit raint of body 1
|
||||||
vec3 linearImpulseBody1 = -deltaLambdaLower * this.sliderAxisWorld;
|
Vector3f linearImpulseBody1 = -deltaLambdaLower * this.sliderAxisWorld;
|
||||||
vec3 angularImpulseBody1 = -deltaLambdaLower * this.R1PlusUCrossSliderAxis;
|
Vector3f angularImpulseBody1 = -deltaLambdaLower * this.R1PlusUCrossSliderAxis;
|
||||||
|
|
||||||
// Apply the impulse to the body 1
|
// Apply the impulse to the body 1
|
||||||
v1 += inverseMassBody1 * linearImpulseBody1;
|
v1 += inverseMassBody1 * linearImpulseBody1;
|
||||||
w1 += this.i1 * angularImpulseBody1;
|
w1 += this.i1 * angularImpulseBody1;
|
||||||
|
|
||||||
// Compute the impulse P=J^T * lambda for the lower limit raint of body 2
|
// Compute the impulse P=J^T * lambda for the lower limit raint of body 2
|
||||||
vec3 linearImpulseBody2 = deltaLambdaLower * this.sliderAxisWorld;
|
Vector3f linearImpulseBody2 = deltaLambdaLower * this.sliderAxisWorld;
|
||||||
vec3 angularImpulseBody2 = deltaLambdaLower * this.R2CrossSliderAxis;
|
Vector3f angularImpulseBody2 = deltaLambdaLower * this.R2CrossSliderAxis;
|
||||||
|
|
||||||
// Apply the impulse to the body 2
|
// Apply the impulse to the body 2
|
||||||
v2 += inverseMassBody2 * linearImpulseBody2;
|
v2 += inverseMassBody2 * linearImpulseBody2;
|
||||||
@ -361,20 +361,20 @@ void SliderJoint::solveVelocityConstraint( ConstraintSolverData raintSolverData)
|
|||||||
// Compute the Lagrange multiplier lambda for the upper limit raint
|
// Compute the Lagrange multiplier lambda for the upper limit raint
|
||||||
float deltaLambdaUpper = this.inverseMassMatrixLimit * (-JvUpperLimit -this.bUpperLimit);
|
float deltaLambdaUpper = this.inverseMassMatrixLimit * (-JvUpperLimit -this.bUpperLimit);
|
||||||
float lambdaTemp = this.impulseUpperLimit;
|
float lambdaTemp = this.impulseUpperLimit;
|
||||||
this.impulseUpperLimit = etk::max(this.impulseUpperLimit + deltaLambdaUpper, 0.0f);
|
this.impulseUpperLimit = max(this.impulseUpperLimit + deltaLambdaUpper, 0.0f);
|
||||||
deltaLambdaUpper = this.impulseUpperLimit - lambdaTemp;
|
deltaLambdaUpper = this.impulseUpperLimit - lambdaTemp;
|
||||||
|
|
||||||
// Compute the impulse P=J^T * lambda for the upper limit raint of body 1
|
// Compute the impulse P=J^T * lambda for the upper limit raint of body 1
|
||||||
vec3 linearImpulseBody1 = deltaLambdaUpper * this.sliderAxisWorld;
|
Vector3f linearImpulseBody1 = deltaLambdaUpper * this.sliderAxisWorld;
|
||||||
vec3 angularImpulseBody1 = deltaLambdaUpper * this.R1PlusUCrossSliderAxis;
|
Vector3f angularImpulseBody1 = deltaLambdaUpper * this.R1PlusUCrossSliderAxis;
|
||||||
|
|
||||||
// Apply the impulse to the body 1
|
// Apply the impulse to the body 1
|
||||||
v1 += inverseMassBody1 * linearImpulseBody1;
|
v1 += inverseMassBody1 * linearImpulseBody1;
|
||||||
w1 += this.i1 * angularImpulseBody1;
|
w1 += this.i1 * angularImpulseBody1;
|
||||||
|
|
||||||
// Compute the impulse P=J^T * lambda for the upper limit raint of body 2
|
// Compute the impulse P=J^T * lambda for the upper limit raint of body 2
|
||||||
vec3 linearImpulseBody2 = -deltaLambdaUpper * this.sliderAxisWorld;
|
Vector3f linearImpulseBody2 = -deltaLambdaUpper * this.sliderAxisWorld;
|
||||||
vec3 angularImpulseBody2 = -deltaLambdaUpper * this.R2CrossSliderAxis;
|
Vector3f angularImpulseBody2 = -deltaLambdaUpper * this.R2CrossSliderAxis;
|
||||||
|
|
||||||
// Apply the impulse to the body 2
|
// Apply the impulse to the body 2
|
||||||
v2 += inverseMassBody2 * linearImpulseBody2;
|
v2 += inverseMassBody2 * linearImpulseBody2;
|
||||||
@ -397,13 +397,13 @@ void SliderJoint::solveVelocityConstraint( ConstraintSolverData raintSolverData)
|
|||||||
deltaLambdaMotor = this.impulseMotor - lambdaTemp;
|
deltaLambdaMotor = this.impulseMotor - lambdaTemp;
|
||||||
|
|
||||||
// Compute the impulse P=J^T * lambda for the motor of body 1
|
// Compute the impulse P=J^T * lambda for the motor of body 1
|
||||||
vec3 linearImpulseBody1 = deltaLambdaMotor * this.sliderAxisWorld;
|
Vector3f linearImpulseBody1 = deltaLambdaMotor * this.sliderAxisWorld;
|
||||||
|
|
||||||
// Apply the impulse to the body 1
|
// Apply the impulse to the body 1
|
||||||
v1 += inverseMassBody1 * linearImpulseBody1;
|
v1 += inverseMassBody1 * linearImpulseBody1;
|
||||||
|
|
||||||
// Compute the impulse P=J^T * lambda for the motor of body 2
|
// Compute the impulse P=J^T * lambda for the motor of body 2
|
||||||
vec3 linearImpulseBody2 = -deltaLambdaMotor * this.sliderAxisWorld;
|
Vector3f linearImpulseBody2 = -deltaLambdaMotor * this.sliderAxisWorld;
|
||||||
|
|
||||||
// Apply the impulse to the body 2
|
// Apply the impulse to the body 2
|
||||||
v2 += inverseMassBody2 * linearImpulseBody2;
|
v2 += inverseMassBody2 * linearImpulseBody2;
|
||||||
@ -418,25 +418,25 @@ void SliderJoint::solvePositionConstraint( ConstraintSolverData raintSolverData)
|
|||||||
if (this.positionCorrectionTechnique != NONLINEARGAUSSSEIDEL) return;
|
if (this.positionCorrectionTechnique != NONLINEARGAUSSSEIDEL) return;
|
||||||
|
|
||||||
// Get the bodies positions and orientations
|
// Get the bodies positions and orientations
|
||||||
vec3 x1 = raintSolverData.positions[this.indexBody1];
|
Vector3f x1 = raintSolverData.positions[this.indexBody1];
|
||||||
vec3 x2 = raintSolverData.positions[this.indexBody2];
|
Vector3f x2 = raintSolverData.positions[this.indexBody2];
|
||||||
etk::Quaternion q1 = raintSolverData.orientations[this.indexBody1];
|
Quaternion q1 = raintSolverData.orientations[this.indexBody1];
|
||||||
etk::Quaternion q2 = raintSolverData.orientations[this.indexBody2];
|
Quaternion q2 = raintSolverData.orientations[this.indexBody2];
|
||||||
|
|
||||||
// Get the inverse mass and inverse inertia tensors of the bodies
|
// Get the inverse mass and inverse inertia tensors of the bodies
|
||||||
float inverseMassBody1 = this.body1->this.massInverse;
|
float inverseMassBody1 = this.body1.this.massInverse;
|
||||||
float inverseMassBody2 = this.body2->this.massInverse;
|
float inverseMassBody2 = this.body2.this.massInverse;
|
||||||
|
|
||||||
// Recompute the inertia tensor of bodies
|
// Recompute the inertia tensor of bodies
|
||||||
this.i1 = this.body1->getInertiaTensorInverseWorld();
|
this.i1 = this.body1.getInertiaTensorInverseWorld();
|
||||||
this.i2 = this.body2->getInertiaTensorInverseWorld();
|
this.i2 = this.body2.getInertiaTensorInverseWorld();
|
||||||
|
|
||||||
// Vector from body center to the anchor point
|
// Vector from body center to the anchor point
|
||||||
this.R1 = q1 * this.localAnchorPointBody1;
|
this.R1 = q1 * this.localAnchorPointBody1;
|
||||||
this.R2 = q2 * this.localAnchorPointBody2;
|
this.R2 = q2 * this.localAnchorPointBody2;
|
||||||
|
|
||||||
// Compute the vector u (difference between anchor points)
|
// Compute the vector u (difference between anchor points)
|
||||||
vec3 u = x2 + this.R2 - x1 - this.R1;
|
Vector3f u = x2 + this.R2 - x1 - this.R1;
|
||||||
|
|
||||||
// Compute the two orthogonal vectors to the slider axis in world-space
|
// Compute the two orthogonal vectors to the slider axis in world-space
|
||||||
this.sliderAxisWorld = q1 * this.sliderAxisBody1;
|
this.sliderAxisWorld = q1 * this.sliderAxisBody1;
|
||||||
@ -455,7 +455,7 @@ void SliderJoint::solvePositionConstraint( ConstraintSolverData raintSolverData)
|
|||||||
this.R2CrossN1 = this.R2.cross(this.N1);
|
this.R2CrossN1 = this.R2.cross(this.N1);
|
||||||
this.R2CrossN2 = this.R2.cross(this.N2);
|
this.R2CrossN2 = this.R2.cross(this.N2);
|
||||||
this.R2CrossSliderAxis = this.R2.cross(this.sliderAxisWorld);
|
this.R2CrossSliderAxis = this.R2.cross(this.sliderAxisWorld);
|
||||||
vec3 r1PlusU = this.R1 + u;
|
Vector3f r1PlusU = this.R1 + u;
|
||||||
this.R1PlusUCrossN1 = (r1PlusU).cross(this.N1);
|
this.R1PlusUCrossN1 = (r1PlusU).cross(this.N1);
|
||||||
this.R1PlusUCrossN2 = (r1PlusU).cross(this.N2);
|
this.R1PlusUCrossN2 = (r1PlusU).cross(this.N2);
|
||||||
this.R1PlusUCrossSliderAxis = (r1PlusU).cross(this.sliderAxisWorld);
|
this.R1PlusUCrossSliderAxis = (r1PlusU).cross(this.sliderAxisWorld);
|
||||||
@ -464,11 +464,11 @@ void SliderJoint::solvePositionConstraint( ConstraintSolverData raintSolverData)
|
|||||||
|
|
||||||
// Recompute the inverse of the mass matrix K=JM^-1J^t for the 2 translation
|
// Recompute the inverse of the mass matrix K=JM^-1J^t for the 2 translation
|
||||||
// raints (2x2 matrix)
|
// raints (2x2 matrix)
|
||||||
float sumInverseMass = this.body1->this.massInverse + this.body2->this.massInverse;
|
float sumInverseMass = this.body1.this.massInverse + this.body2.this.massInverse;
|
||||||
vec3 I1R1PlusUCrossN1 = this.i1 * this.R1PlusUCrossN1;
|
Vector3f I1R1PlusUCrossN1 = this.i1 * this.R1PlusUCrossN1;
|
||||||
vec3 I1R1PlusUCrossN2 = this.i1 * this.R1PlusUCrossN2;
|
Vector3f I1R1PlusUCrossN2 = this.i1 * this.R1PlusUCrossN2;
|
||||||
vec3 I2R2CrossN1 = this.i2 * this.R2CrossN1;
|
Vector3f I2R2CrossN1 = this.i2 * this.R2CrossN1;
|
||||||
vec3 I2R2CrossN2 = this.i2 * this.R2CrossN2;
|
Vector3f I2R2CrossN2 = this.i2 * this.R2CrossN2;
|
||||||
float el11 = sumInverseMass + this.R1PlusUCrossN1.dot(I1R1PlusUCrossN1) +
|
float el11 = sumInverseMass + this.R1PlusUCrossN1.dot(I1R1PlusUCrossN1) +
|
||||||
this.R2CrossN1.dot(I2R2CrossN1);
|
this.R2CrossN1.dot(I2R2CrossN1);
|
||||||
float el12 = this.R1PlusUCrossN1.dot(I1R1PlusUCrossN2) +
|
float el12 = this.R1PlusUCrossN1.dot(I1R1PlusUCrossN2) +
|
||||||
@ -477,9 +477,9 @@ void SliderJoint::solvePositionConstraint( ConstraintSolverData raintSolverData)
|
|||||||
this.R2CrossN2.dot(I2R2CrossN1);
|
this.R2CrossN2.dot(I2R2CrossN1);
|
||||||
float el22 = sumInverseMass + this.R1PlusUCrossN2.dot(I1R1PlusUCrossN2) +
|
float el22 = sumInverseMass + this.R1PlusUCrossN2.dot(I1R1PlusUCrossN2) +
|
||||||
this.R2CrossN2.dot(I2R2CrossN2);
|
this.R2CrossN2.dot(I2R2CrossN2);
|
||||||
etk::Matrix2x2 matrixKTranslation(el11, el12, el21, el22);
|
Matrix2x2 matrixKTranslation(el11, el12, el21, el22);
|
||||||
this.inverseMassMatrixTranslationConstraint.setZero();
|
this.inverseMassMatrixTranslationConstraint.setZero();
|
||||||
if (this.body1->getType() == DYNAMIC || this.body2->getType() == DYNAMIC) {
|
if (this.body1.getType() == DYNAMIC || this.body2.getType() == DYNAMIC) {
|
||||||
this.inverseMassMatrixTranslationConstraint = matrixKTranslation.getInverse();
|
this.inverseMassMatrixTranslationConstraint = matrixKTranslation.getInverse();
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -490,31 +490,31 @@ void SliderJoint::solvePositionConstraint( ConstraintSolverData raintSolverData)
|
|||||||
vec2 lambdaTranslation = this.inverseMassMatrixTranslationConstraint * (-translationError);
|
vec2 lambdaTranslation = this.inverseMassMatrixTranslationConstraint * (-translationError);
|
||||||
|
|
||||||
// Compute the impulse P=J^T * lambda for the 2 translation raints of body 1
|
// Compute the impulse P=J^T * lambda for the 2 translation raints of body 1
|
||||||
vec3 linearImpulseBody1 = -this.N1 * lambdaTranslation.x() - this.N2 * lambdaTranslation.y();
|
Vector3f linearImpulseBody1 = -this.N1 * lambdaTranslation.x() - this.N2 * lambdaTranslation.y();
|
||||||
vec3 angularImpulseBody1 = -this.R1PlusUCrossN1 * lambdaTranslation.x() -
|
Vector3f angularImpulseBody1 = -this.R1PlusUCrossN1 * lambdaTranslation.x() -
|
||||||
this.R1PlusUCrossN2 * lambdaTranslation.y();
|
this.R1PlusUCrossN2 * lambdaTranslation.y();
|
||||||
|
|
||||||
// Apply the impulse to the body 1
|
// Apply the impulse to the body 1
|
||||||
vec3 v1 = inverseMassBody1 * linearImpulseBody1;
|
Vector3f v1 = inverseMassBody1 * linearImpulseBody1;
|
||||||
vec3 w1 = this.i1 * angularImpulseBody1;
|
Vector3f w1 = this.i1 * angularImpulseBody1;
|
||||||
|
|
||||||
// Update the body position/orientation of body 1
|
// Update the body position/orientation of body 1
|
||||||
x1 += v1;
|
x1 += v1;
|
||||||
q1 += etk::Quaternion(0, w1) * q1 * 0.5f;
|
q1 += Quaternion(0, w1) * q1 * 0.5f;
|
||||||
q1.normalize();
|
q1.normalize();
|
||||||
|
|
||||||
// Compute the impulse P=J^T * lambda for the 2 translation raints of body 2
|
// Compute the impulse P=J^T * lambda for the 2 translation raints of body 2
|
||||||
vec3 linearImpulseBody2 = this.N1 * lambdaTranslation.x() + this.N2 * lambdaTranslation.y();
|
Vector3f linearImpulseBody2 = this.N1 * lambdaTranslation.x() + this.N2 * lambdaTranslation.y();
|
||||||
vec3 angularImpulseBody2 = this.R2CrossN1 * lambdaTranslation.x() +
|
Vector3f angularImpulseBody2 = this.R2CrossN1 * lambdaTranslation.x() +
|
||||||
this.R2CrossN2 * lambdaTranslation.y();
|
this.R2CrossN2 * lambdaTranslation.y();
|
||||||
|
|
||||||
// Apply the impulse to the body 2
|
// Apply the impulse to the body 2
|
||||||
vec3 v2 = inverseMassBody2 * linearImpulseBody2;
|
Vector3f v2 = inverseMassBody2 * linearImpulseBody2;
|
||||||
vec3 w2 = this.i2 * angularImpulseBody2;
|
Vector3f w2 = this.i2 * angularImpulseBody2;
|
||||||
|
|
||||||
// Update the body position/orientation of body 2
|
// Update the body position/orientation of body 2
|
||||||
x2 += v2;
|
x2 += v2;
|
||||||
q2 += etk::Quaternion(0, w2) * q2 * 0.5f;
|
q2 += Quaternion(0, w2) * q2 * 0.5f;
|
||||||
q2.normalize();
|
q2.normalize();
|
||||||
|
|
||||||
// --------------- Rotation Constraints --------------- //
|
// --------------- Rotation Constraints --------------- //
|
||||||
@ -522,18 +522,18 @@ void SliderJoint::solvePositionConstraint( ConstraintSolverData raintSolverData)
|
|||||||
// Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation
|
// Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation
|
||||||
// contraints (3x3 matrix)
|
// contraints (3x3 matrix)
|
||||||
this.inverseMassMatrixRotationConstraint = this.i1 + this.i2;
|
this.inverseMassMatrixRotationConstraint = this.i1 + this.i2;
|
||||||
if (this.body1->getType() == DYNAMIC || this.body2->getType() == DYNAMIC) {
|
if (this.body1.getType() == DYNAMIC || this.body2.getType() == DYNAMIC) {
|
||||||
this.inverseMassMatrixRotationConstraint = this.inverseMassMatrixRotationConstraint.getInverse();
|
this.inverseMassMatrixRotationConstraint = this.inverseMassMatrixRotationConstraint.getInverse();
|
||||||
}
|
}
|
||||||
|
|
||||||
// Compute the position error for the 3 rotation raints
|
// Compute the position error for the 3 rotation raints
|
||||||
etk::Quaternion currentOrientationDifference = q2 * q1.getInverse();
|
Quaternion currentOrientationDifference = q2 * q1.getInverse();
|
||||||
currentOrientationDifference.normalize();
|
currentOrientationDifference.normalize();
|
||||||
etk::Quaternion qError = currentOrientationDifference * this.initOrientationDifferenceInv;
|
Quaternion qError = currentOrientationDifference * this.initOrientationDifferenceInv;
|
||||||
vec3 errorRotation = float(2.0) * qError.getVectorV();
|
Vector3f errorRotation = float(2.0) * qError.getVectorV();
|
||||||
|
|
||||||
// Compute the Lagrange multiplier lambda for the 3 rotation raints
|
// Compute the Lagrange multiplier lambda for the 3 rotation raints
|
||||||
vec3 lambdaRotation = this.inverseMassMatrixRotationConstraint * (-errorRotation);
|
Vector3f lambdaRotation = this.inverseMassMatrixRotationConstraint * (-errorRotation);
|
||||||
|
|
||||||
// Compute the impulse P=J^T * lambda for the 3 rotation raints of body 1
|
// Compute the impulse P=J^T * lambda for the 3 rotation raints of body 1
|
||||||
angularImpulseBody1 = -lambdaRotation;
|
angularImpulseBody1 = -lambdaRotation;
|
||||||
@ -542,7 +542,7 @@ void SliderJoint::solvePositionConstraint( ConstraintSolverData raintSolverData)
|
|||||||
w1 = this.i1 * angularImpulseBody1;
|
w1 = this.i1 * angularImpulseBody1;
|
||||||
|
|
||||||
// Update the body position/orientation of body 1
|
// Update the body position/orientation of body 1
|
||||||
q1 += etk::Quaternion(0, w1) * q1 * 0.5f;
|
q1 += Quaternion(0, w1) * q1 * 0.5f;
|
||||||
q1.normalize();
|
q1.normalize();
|
||||||
|
|
||||||
// Compute the impulse P=J^T * lambda for the 3 rotation raints of body 2
|
// Compute the impulse P=J^T * lambda for the 3 rotation raints of body 2
|
||||||
@ -552,7 +552,7 @@ void SliderJoint::solvePositionConstraint( ConstraintSolverData raintSolverData)
|
|||||||
w2 = this.i2 * angularImpulseBody2;
|
w2 = this.i2 * angularImpulseBody2;
|
||||||
|
|
||||||
// Update the body position/orientation of body 2
|
// Update the body position/orientation of body 2
|
||||||
q2 += etk::Quaternion(0, w2) * q2 * 0.5f;
|
q2 += Quaternion(0, w2) * q2 * 0.5f;
|
||||||
q2.normalize();
|
q2.normalize();
|
||||||
|
|
||||||
// --------------- Limits Constraints --------------- //
|
// --------------- Limits Constraints --------------- //
|
||||||
@ -562,7 +562,7 @@ void SliderJoint::solvePositionConstraint( ConstraintSolverData raintSolverData)
|
|||||||
if (this.isLowerLimitViolated || this.isUpperLimitViolated) {
|
if (this.isLowerLimitViolated || this.isUpperLimitViolated) {
|
||||||
|
|
||||||
// Compute the inverse of the mass matrix K=JM^-1J^t for the limits (1x1 matrix)
|
// Compute the inverse of the mass matrix K=JM^-1J^t for the limits (1x1 matrix)
|
||||||
this.inverseMassMatrixLimit = this.body1->this.massInverse + this.body2->this.massInverse +
|
this.inverseMassMatrixLimit = this.body1.this.massInverse + this.body2.this.massInverse +
|
||||||
this.R1PlusUCrossSliderAxis.dot(this.i1 * this.R1PlusUCrossSliderAxis) +
|
this.R1PlusUCrossSliderAxis.dot(this.i1 * this.R1PlusUCrossSliderAxis) +
|
||||||
this.R2CrossSliderAxis.dot(this.i2 * this.R2CrossSliderAxis);
|
this.R2CrossSliderAxis.dot(this.i2 * this.R2CrossSliderAxis);
|
||||||
this.inverseMassMatrixLimit = (this.inverseMassMatrixLimit > 0.0) ?
|
this.inverseMassMatrixLimit = (this.inverseMassMatrixLimit > 0.0) ?
|
||||||
@ -576,29 +576,29 @@ void SliderJoint::solvePositionConstraint( ConstraintSolverData raintSolverData)
|
|||||||
float lambdaLowerLimit = this.inverseMassMatrixLimit * (-lowerLimitError);
|
float lambdaLowerLimit = this.inverseMassMatrixLimit * (-lowerLimitError);
|
||||||
|
|
||||||
// Compute the impulse P=J^T * lambda for the lower limit raint of body 1
|
// Compute the impulse P=J^T * lambda for the lower limit raint of body 1
|
||||||
vec3 linearImpulseBody1 = -lambdaLowerLimit * this.sliderAxisWorld;
|
Vector3f linearImpulseBody1 = -lambdaLowerLimit * this.sliderAxisWorld;
|
||||||
vec3 angularImpulseBody1 = -lambdaLowerLimit * this.R1PlusUCrossSliderAxis;
|
Vector3f angularImpulseBody1 = -lambdaLowerLimit * this.R1PlusUCrossSliderAxis;
|
||||||
|
|
||||||
// Apply the impulse to the body 1
|
// Apply the impulse to the body 1
|
||||||
vec3 v1 = inverseMassBody1 * linearImpulseBody1;
|
Vector3f v1 = inverseMassBody1 * linearImpulseBody1;
|
||||||
vec3 w1 = this.i1 * angularImpulseBody1;
|
Vector3f w1 = this.i1 * angularImpulseBody1;
|
||||||
|
|
||||||
// Update the body position/orientation of body 1
|
// Update the body position/orientation of body 1
|
||||||
x1 += v1;
|
x1 += v1;
|
||||||
q1 += etk::Quaternion(0, w1) * q1 * 0.5f;
|
q1 += Quaternion(0, w1) * q1 * 0.5f;
|
||||||
q1.normalize();
|
q1.normalize();
|
||||||
|
|
||||||
// Compute the impulse P=J^T * lambda for the lower limit raint of body 2
|
// Compute the impulse P=J^T * lambda for the lower limit raint of body 2
|
||||||
vec3 linearImpulseBody2 = lambdaLowerLimit * this.sliderAxisWorld;
|
Vector3f linearImpulseBody2 = lambdaLowerLimit * this.sliderAxisWorld;
|
||||||
vec3 angularImpulseBody2 = lambdaLowerLimit * this.R2CrossSliderAxis;
|
Vector3f angularImpulseBody2 = lambdaLowerLimit * this.R2CrossSliderAxis;
|
||||||
|
|
||||||
// Apply the impulse to the body 2
|
// Apply the impulse to the body 2
|
||||||
vec3 v2 = inverseMassBody2 * linearImpulseBody2;
|
Vector3f v2 = inverseMassBody2 * linearImpulseBody2;
|
||||||
vec3 w2 = this.i2 * angularImpulseBody2;
|
Vector3f w2 = this.i2 * angularImpulseBody2;
|
||||||
|
|
||||||
// Update the body position/orientation of body 2
|
// Update the body position/orientation of body 2
|
||||||
x2 += v2;
|
x2 += v2;
|
||||||
q2 += etk::Quaternion(0, w2) * q2 * 0.5f;
|
q2 += Quaternion(0, w2) * q2 * 0.5f;
|
||||||
q2.normalize();
|
q2.normalize();
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -609,29 +609,29 @@ void SliderJoint::solvePositionConstraint( ConstraintSolverData raintSolverData)
|
|||||||
float lambdaUpperLimit = this.inverseMassMatrixLimit * (-upperLimitError);
|
float lambdaUpperLimit = this.inverseMassMatrixLimit * (-upperLimitError);
|
||||||
|
|
||||||
// Compute the impulse P=J^T * lambda for the upper limit raint of body 1
|
// Compute the impulse P=J^T * lambda for the upper limit raint of body 1
|
||||||
vec3 linearImpulseBody1 = lambdaUpperLimit * this.sliderAxisWorld;
|
Vector3f linearImpulseBody1 = lambdaUpperLimit * this.sliderAxisWorld;
|
||||||
vec3 angularImpulseBody1 = lambdaUpperLimit * this.R1PlusUCrossSliderAxis;
|
Vector3f angularImpulseBody1 = lambdaUpperLimit * this.R1PlusUCrossSliderAxis;
|
||||||
|
|
||||||
// Apply the impulse to the body 1
|
// Apply the impulse to the body 1
|
||||||
vec3 v1 = inverseMassBody1 * linearImpulseBody1;
|
Vector3f v1 = inverseMassBody1 * linearImpulseBody1;
|
||||||
vec3 w1 = this.i1 * angularImpulseBody1;
|
Vector3f w1 = this.i1 * angularImpulseBody1;
|
||||||
|
|
||||||
// Update the body position/orientation of body 1
|
// Update the body position/orientation of body 1
|
||||||
x1 += v1;
|
x1 += v1;
|
||||||
q1 += etk::Quaternion(0, w1) * q1 * 0.5f;
|
q1 += Quaternion(0, w1) * q1 * 0.5f;
|
||||||
q1.normalize();
|
q1.normalize();
|
||||||
|
|
||||||
// Compute the impulse P=J^T * lambda for the upper limit raint of body 2
|
// Compute the impulse P=J^T * lambda for the upper limit raint of body 2
|
||||||
vec3 linearImpulseBody2 = -lambdaUpperLimit * this.sliderAxisWorld;
|
Vector3f linearImpulseBody2 = -lambdaUpperLimit * this.sliderAxisWorld;
|
||||||
vec3 angularImpulseBody2 = -lambdaUpperLimit * this.R2CrossSliderAxis;
|
Vector3f angularImpulseBody2 = -lambdaUpperLimit * this.R2CrossSliderAxis;
|
||||||
|
|
||||||
// Apply the impulse to the body 2
|
// Apply the impulse to the body 2
|
||||||
vec3 v2 = inverseMassBody2 * linearImpulseBody2;
|
Vector3f v2 = inverseMassBody2 * linearImpulseBody2;
|
||||||
vec3 w2 = this.i2 * angularImpulseBody2;
|
Vector3f w2 = this.i2 * angularImpulseBody2;
|
||||||
|
|
||||||
// Update the body position/orientation of body 2
|
// Update the body position/orientation of body 2
|
||||||
x2 += v2;
|
x2 += v2;
|
||||||
q2 += etk::Quaternion(0, w2) * q2 * 0.5f;
|
q2 += Quaternion(0, w2) * q2 * 0.5f;
|
||||||
q2.normalize();
|
q2.normalize();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@ -664,8 +664,8 @@ void SliderJoint::enableMotor(boolean isMotorEnabled) {
|
|||||||
this.impulseMotor = 0.0;
|
this.impulseMotor = 0.0;
|
||||||
|
|
||||||
// Wake up the two bodies of the joint
|
// Wake up the two bodies of the joint
|
||||||
this.body1->setIsSleeping(false);
|
this.body1.setIsSleeping(false);
|
||||||
this.body2->setIsSleeping(false);
|
this.body2.setIsSleeping(false);
|
||||||
}
|
}
|
||||||
|
|
||||||
// Return the current translation value of the joint
|
// Return the current translation value of the joint
|
||||||
@ -677,20 +677,20 @@ float SliderJoint::getTranslation() {
|
|||||||
// TODO : Check if we need to compare rigid body position or center of mass here
|
// TODO : Check if we need to compare rigid body position or center of mass here
|
||||||
|
|
||||||
// Get the bodies positions and orientations
|
// Get the bodies positions and orientations
|
||||||
vec3 x1 = this.body1->getTransform().getPosition();
|
Vector3f x1 = this.body1.getTransform().getPosition();
|
||||||
vec3 x2 = this.body2->getTransform().getPosition();
|
Vector3f x2 = this.body2.getTransform().getPosition();
|
||||||
etk::Quaternion q1 = this.body1->getTransform().getOrientation();
|
Quaternion q1 = this.body1.getTransform().getOrientation();
|
||||||
etk::Quaternion q2 = this.body2->getTransform().getOrientation();
|
Quaternion q2 = this.body2.getTransform().getOrientation();
|
||||||
|
|
||||||
// Compute the two anchor points in world-space coordinates
|
// Compute the two anchor points in world-space coordinates
|
||||||
vec3 anchorBody1 = x1 + q1 * this.localAnchorPointBody1;
|
Vector3f anchorBody1 = x1 + q1 * this.localAnchorPointBody1;
|
||||||
vec3 anchorBody2 = x2 + q2 * this.localAnchorPointBody2;
|
Vector3f anchorBody2 = x2 + q2 * this.localAnchorPointBody2;
|
||||||
|
|
||||||
// Compute the vector u (difference between anchor points)
|
// Compute the vector u (difference between anchor points)
|
||||||
vec3 u = anchorBody2 - anchorBody1;
|
Vector3f u = anchorBody2 - anchorBody1;
|
||||||
|
|
||||||
// Compute the slider axis in world-space
|
// Compute the slider axis in world-space
|
||||||
vec3 sliderAxisWorld = q1 * this.sliderAxisBody1;
|
Vector3f sliderAxisWorld = q1 * this.sliderAxisBody1;
|
||||||
sliderAxisWorld.normalize();
|
sliderAxisWorld.normalize();
|
||||||
|
|
||||||
// Compute and return the translation value
|
// Compute and return the translation value
|
||||||
@ -739,8 +739,8 @@ void SliderJoint::resetLimits() {
|
|||||||
this.impulseUpperLimit = 0.0;
|
this.impulseUpperLimit = 0.0;
|
||||||
|
|
||||||
// Wake up the two bodies of the joint
|
// Wake up the two bodies of the joint
|
||||||
this.body1->setIsSleeping(false);
|
this.body1.setIsSleeping(false);
|
||||||
this.body2->setIsSleeping(false);
|
this.body2.setIsSleeping(false);
|
||||||
}
|
}
|
||||||
|
|
||||||
// Set the motor speed
|
// Set the motor speed
|
||||||
@ -754,8 +754,8 @@ void SliderJoint::setMotorSpeed(float motorSpeed) {
|
|||||||
this.motorSpeed = motorSpeed;
|
this.motorSpeed = motorSpeed;
|
||||||
|
|
||||||
// Wake up the two bodies of the joint
|
// Wake up the two bodies of the joint
|
||||||
this.body1->setIsSleeping(false);
|
this.body1.setIsSleeping(false);
|
||||||
this.body2->setIsSleeping(false);
|
this.body2.setIsSleeping(false);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -771,8 +771,8 @@ void SliderJoint::setMaxMotorForce(float maxMotorForce) {
|
|||||||
this.maxMotorForce = maxMotorForce;
|
this.maxMotorForce = maxMotorForce;
|
||||||
|
|
||||||
// Wake up the two bodies of the joint
|
// Wake up the two bodies of the joint
|
||||||
this.body1->setIsSleeping(false);
|
this.body1.setIsSleeping(false);
|
||||||
this.body2->setIsSleeping(false);
|
this.body2.setIsSleeping(false);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -834,7 +834,7 @@ float SliderJoint::getMotorForce(float timeStep) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
// Return the number of bytes used by the joint
|
// Return the number of bytes used by the joint
|
||||||
sizet SliderJoint::getSizeInBytes() {
|
long SliderJoint::getSizeInBytes() {
|
||||||
return sizeof(SliderJoint);
|
return sizeof(SliderJoint);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -1,26 +1,14 @@
|
|||||||
/** @file
|
package org.atriaSoft.ephysics.constraint;
|
||||||
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
|
|
||||||
* @author Daniel CHAPPUIS
|
|
||||||
* @author Edouard DUPIN
|
|
||||||
* @copyright 2010-2016, Daniel Chappuis
|
|
||||||
* @copyright 2017, Edouard DUPIN
|
|
||||||
* @license MPL v2.0 (see license file)
|
|
||||||
*/
|
|
||||||
#pragma once
|
|
||||||
|
|
||||||
#include <ephysics/mathematics/mathematics.hpp>
|
|
||||||
#include <ephysics/engine/ConstraintSolver.hpp>
|
|
||||||
|
|
||||||
namespace ephysics {
|
|
||||||
/**
|
/**
|
||||||
* This structure is used to gather the information needed to create a slider
|
* This structure is used to gather the information needed to create a slider
|
||||||
* joint. This structure will be used to create the actual slider joint.
|
* joint. This structure will be used to create the actual slider joint.
|
||||||
*/
|
*/
|
||||||
struct SliderJointInfo : public JointInfo {
|
struct SliderJointInfo extends JointInfo {
|
||||||
|
|
||||||
public :
|
public :
|
||||||
vec3 this.anchorPointWorldSpace; //!< Anchor point (in world-space coordinates)
|
Vector3f anchorPointWorldSpace; //!< Anchor point (in world-space coordinates)
|
||||||
vec3 sliderAxisWorldSpace; //!< Slider axis (in world-space coordinates)
|
Vector3f sliderAxisWorldSpace; //!< Slider axis (in world-space coordinates)
|
||||||
boolean isLimitEnabled; //!< True if the slider limits are enabled
|
boolean isLimitEnabled; //!< True if the slider limits are enabled
|
||||||
boolean isMotorEnabled; //!< True if the slider motor is enabled
|
boolean isMotorEnabled; //!< True if the slider motor is enabled
|
||||||
float minTranslationLimit; //!< Mininum allowed translation if limits are enabled
|
float minTranslationLimit; //!< Mininum allowed translation if limits are enabled
|
||||||
@ -36,8 +24,8 @@ namespace ephysics {
|
|||||||
*/
|
*/
|
||||||
SliderJointInfo(RigidBody* rigidBody1,
|
SliderJointInfo(RigidBody* rigidBody1,
|
||||||
RigidBody* rigidBody2,
|
RigidBody* rigidBody2,
|
||||||
vec3 initAnchorPointWorldSpace,
|
Vector3f initAnchorPointWorldSpace,
|
||||||
vec3 initSliderAxisWorldSpace):
|
Vector3f initSliderAxisWorldSpace):
|
||||||
JointInfo(rigidBody1, rigidBody2, SLIDERJOINT),
|
JointInfo(rigidBody1, rigidBody2, SLIDERJOINT),
|
||||||
this.anchorPointWorldSpace(initAnchorPointWorldSpace),
|
this.anchorPointWorldSpace(initAnchorPointWorldSpace),
|
||||||
sliderAxisWorldSpace(initSliderAxisWorldSpace),
|
sliderAxisWorldSpace(initSliderAxisWorldSpace),
|
||||||
@ -60,8 +48,8 @@ namespace ephysics {
|
|||||||
*/
|
*/
|
||||||
SliderJointInfo(RigidBody* rigidBody1,
|
SliderJointInfo(RigidBody* rigidBody1,
|
||||||
RigidBody* rigidBody2,
|
RigidBody* rigidBody2,
|
||||||
vec3 initAnchorPointWorldSpace,
|
Vector3f initAnchorPointWorldSpace,
|
||||||
vec3 initSliderAxisWorldSpace,
|
Vector3f initSliderAxisWorldSpace,
|
||||||
float initMinTranslationLimit,
|
float initMinTranslationLimit,
|
||||||
float initMaxTranslationLimit):
|
float initMaxTranslationLimit):
|
||||||
JointInfo(rigidBody1, rigidBody2, SLIDERJOINT),
|
JointInfo(rigidBody1, rigidBody2, SLIDERJOINT),
|
||||||
@ -88,8 +76,8 @@ namespace ephysics {
|
|||||||
*/
|
*/
|
||||||
SliderJointInfo(RigidBody* rigidBody1,
|
SliderJointInfo(RigidBody* rigidBody1,
|
||||||
RigidBody* rigidBody2,
|
RigidBody* rigidBody2,
|
||||||
vec3 initAnchorPointWorldSpace,
|
Vector3f initAnchorPointWorldSpace,
|
||||||
vec3 initSliderAxisWorldSpace,
|
Vector3f initSliderAxisWorldSpace,
|
||||||
float initMinTranslationLimit,
|
float initMinTranslationLimit,
|
||||||
float initMaxTranslationLimit,
|
float initMaxTranslationLimit,
|
||||||
float initMotorSpeed,
|
float initMotorSpeed,
|
||||||
@ -115,38 +103,38 @@ namespace ephysics {
|
|||||||
class SliderJoint: public Joint {
|
class SliderJoint: public Joint {
|
||||||
private:
|
private:
|
||||||
static float BETA; //!< Beta value for the position correction bias factor
|
static float BETA; //!< Beta value for the position correction bias factor
|
||||||
vec3 this.localAnchorPointBody1; //!< Anchor point of body 1 (in local-space coordinates of body 1)
|
Vector3f this.localAnchorPointBody1; //!< Anchor point of body 1 (in local-space coordinates of body 1)
|
||||||
vec3 this.localAnchorPointBody2; //!< Anchor point of body 2 (in local-space coordinates of body 2)
|
Vector3f this.localAnchorPointBody2; //!< Anchor point of body 2 (in local-space coordinates of body 2)
|
||||||
vec3 this.sliderAxisBody1; //!< Slider axis (in local-space coordinates of body 1)
|
Vector3f this.sliderAxisBody1; //!< Slider axis (in local-space coordinates of body 1)
|
||||||
etk::Matrix3x3 this.i1; //!< Inertia tensor of body 1 (in world-space coordinates)
|
Matrix3f this.i1; //!< Inertia tensor of body 1 (in world-space coordinates)
|
||||||
etk::Matrix3x3 this.i2; //!< Inertia tensor of body 2 (in world-space coordinates)
|
Matrix3f this.i2; //!< Inertia tensor of body 2 (in world-space coordinates)
|
||||||
etk::Quaternion this.initOrientationDifferenceInv; //!< Inverse of the initial orientation difference between the two bodies
|
Quaternion this.initOrientationDifferenceInv; //!< Inverse of the initial orientation difference between the two bodies
|
||||||
vec3 this.N1; //!< First vector orthogonal to the slider axis local-space of body 1
|
Vector3f this.N1; //!< First vector orthogonal to the slider axis local-space of body 1
|
||||||
vec3 this.N2; //!< Second vector orthogonal to the slider axis and this.N1 in local-space of body 1
|
Vector3f this.N2; //!< Second vector orthogonal to the slider axis and this.N1 in local-space of body 1
|
||||||
vec3 this.R1; //!< Vector r1 in world-space coordinates
|
Vector3f this.R1; //!< Vector r1 in world-space coordinates
|
||||||
vec3 this.R2; //!< Vector r2 in world-space coordinates
|
Vector3f this.R2; //!< Vector r2 in world-space coordinates
|
||||||
vec3 this.R2CrossN1; //!< Cross product of r2 and n1
|
Vector3f this.R2CrossN1; //!< Cross product of r2 and n1
|
||||||
vec3 this.R2CrossN2; //!< Cross product of r2 and n2
|
Vector3f this.R2CrossN2; //!< Cross product of r2 and n2
|
||||||
vec3 this.R2CrossSliderAxis; //!< Cross product of r2 and the slider axis
|
Vector3f this.R2CrossSliderAxis; //!< Cross product of r2 and the slider axis
|
||||||
vec3 this.R1PlusUCrossN1; //!< Cross product of vector (r1 + u) and n1
|
Vector3f this.R1PlusUCrossN1; //!< Cross product of vector (r1 + u) and n1
|
||||||
vec3 this.R1PlusUCrossN2; //!< Cross product of vector (r1 + u) and n2
|
Vector3f this.R1PlusUCrossN2; //!< Cross product of vector (r1 + u) and n2
|
||||||
vec3 this.R1PlusUCrossSliderAxis; //!< Cross product of vector (r1 + u) and the slider axis
|
Vector3f this.R1PlusUCrossSliderAxis; //!< Cross product of vector (r1 + u) and the slider axis
|
||||||
vec2 this.bTranslation; //!< Bias of the 2 translation raints
|
vec2 this.bTranslation; //!< Bias of the 2 translation raints
|
||||||
vec3 this.bRotation; //!< Bias of the 3 rotation raints
|
Vector3f this.bRotation; //!< Bias of the 3 rotation raints
|
||||||
float this.bLowerLimit; //!< Bias of the lower limit raint
|
float this.bLowerLimit; //!< Bias of the lower limit raint
|
||||||
float this.bUpperLimit; //!< Bias of the upper limit raint
|
float this.bUpperLimit; //!< Bias of the upper limit raint
|
||||||
etk::Matrix2x2 this.inverseMassMatrixTranslationConstraint; //!< Inverse of mass matrix K=JM^-1J^t for the translation raint (2x2 matrix)
|
Matrix2x2 this.inverseMassMatrixTranslationConstraint; //!< Inverse of mass matrix K=JM^-1J^t for the translation raint (2x2 matrix)
|
||||||
etk::Matrix3x3 this.inverseMassMatrixRotationConstraint; //!< Inverse of mass matrix K=JM^-1J^t for the rotation raint (3x3 matrix)
|
Matrix3f this.inverseMassMatrixRotationConstraint; //!< Inverse of mass matrix K=JM^-1J^t for the rotation raint (3x3 matrix)
|
||||||
float this.inverseMassMatrixLimit; //!< Inverse of mass matrix K=JM^-1J^t for the upper and lower limit raints (1x1 matrix)
|
float this.inverseMassMatrixLimit; //!< Inverse of mass matrix K=JM^-1J^t for the upper and lower limit raints (1x1 matrix)
|
||||||
float this.inverseMassMatrixMotor; //!< Inverse of mass matrix K=JM^-1J^t for the motor
|
float this.inverseMassMatrixMotor; //!< Inverse of mass matrix K=JM^-1J^t for the motor
|
||||||
vec2 this.impulseTranslation; //!< Accumulated impulse for the 2 translation raints
|
vec2 this.impulseTranslation; //!< Accumulated impulse for the 2 translation raints
|
||||||
vec3 this.impulseRotation; //!< Accumulated impulse for the 3 rotation raints
|
Vector3f this.impulseRotation; //!< Accumulated impulse for the 3 rotation raints
|
||||||
float this.impulseLowerLimit; //!< Accumulated impulse for the lower limit raint
|
float this.impulseLowerLimit; //!< Accumulated impulse for the lower limit raint
|
||||||
float this.impulseUpperLimit; //!< Accumulated impulse for the upper limit raint
|
float this.impulseUpperLimit; //!< Accumulated impulse for the upper limit raint
|
||||||
float this.impulseMotor; //!< Accumulated impulse for the motor
|
float this.impulseMotor; //!< Accumulated impulse for the motor
|
||||||
boolean this.isLimitEnabled; //!< True if the slider limits are enabled
|
boolean this.isLimitEnabled; //!< True if the slider limits are enabled
|
||||||
boolean this.isMotorEnabled; //!< True if the motor of the joint in enabled
|
boolean this.isMotorEnabled; //!< True if the motor of the joint in enabled
|
||||||
vec3 this.sliderAxisWorld; //!< Slider axis in world-space coordinates
|
Vector3f this.sliderAxisWorld; //!< Slider axis in world-space coordinates
|
||||||
float this.lowerLimit; //!< Lower limit (minimum translation distance)
|
float this.lowerLimit; //!< Lower limit (minimum translation distance)
|
||||||
float this.upperLimit; //!< Upper limit (maximum translation distance)
|
float this.upperLimit; //!< Upper limit (maximum translation distance)
|
||||||
boolean this.isLowerLimitViolated; //!< True if the lower limit is violated
|
boolean this.isLowerLimitViolated; //!< True if the lower limit is violated
|
||||||
@ -159,16 +147,16 @@ namespace ephysics {
|
|||||||
SliderJoint operator=( SliderJoint raint);
|
SliderJoint operator=( SliderJoint raint);
|
||||||
/// Reset the limits
|
/// Reset the limits
|
||||||
void resetLimits();
|
void resetLimits();
|
||||||
sizet getSizeInBytes() override;
|
long getSizeInBytes() ;
|
||||||
void initBeforeSolve( ConstraintSolverData raintSolverData) override;
|
void initBeforeSolve( ConstraintSolverData raintSolverData) ;
|
||||||
void warmstart( ConstraintSolverData raintSolverData) override;
|
void warmstart( ConstraintSolverData raintSolverData) ;
|
||||||
void solveVelocityConstraint( ConstraintSolverData raintSolverData) override;
|
void solveVelocityConstraint( ConstraintSolverData raintSolverData) ;
|
||||||
void solvePositionConstraint( ConstraintSolverData raintSolverData) override;
|
void solvePositionConstraint( ConstraintSolverData raintSolverData) ;
|
||||||
public :
|
public :
|
||||||
/// Constructor
|
/// Constructor
|
||||||
SliderJoint( SliderJointInfo jointInfo);
|
SliderJoint( SliderJointInfo jointInfo);
|
||||||
/// Destructor
|
/// Destructor
|
||||||
virtual ~SliderJoint();
|
~SliderJoint();
|
||||||
/// Return true if the limits or the joint are enabled
|
/// Return true if the limits or the joint are enabled
|
||||||
boolean isLimitEnabled() ;
|
boolean isLimitEnabled() ;
|
||||||
/// Return true if the motor of the joint is enabled
|
/// Return true if the motor of the joint is enabled
|
@ -1,12 +0,0 @@
|
|||||||
/** @file
|
|
||||||
* @author Edouard DUPIN
|
|
||||||
* @copyright 2011, Edouard DUPIN, all right reserved
|
|
||||||
* @license MPL v2.0 (see license file)
|
|
||||||
*/
|
|
||||||
|
|
||||||
#include <ephysics/debug.hpp>
|
|
||||||
|
|
||||||
int ephysic::getLogId() {
|
|
||||||
static int gval = elog::registerInstance("ephysic");
|
|
||||||
return gval;
|
|
||||||
}
|
|
@ -1,37 +0,0 @@
|
|||||||
/** @file
|
|
||||||
* @author Edouard DUPIN
|
|
||||||
* @copyright 2011, Edouard DUPIN, all right reserved
|
|
||||||
* @license MPL v2.0 (see license file)
|
|
||||||
*/
|
|
||||||
#pragma once
|
|
||||||
|
|
||||||
#include <elog/log.hpp>
|
|
||||||
|
|
||||||
namespace ephysic {
|
|
||||||
int getLogId();
|
|
||||||
};
|
|
||||||
#define EPHYBASE(info,data) ELOGBASE(ephysic::getLogId(),info,data)
|
|
||||||
|
|
||||||
#define Log.print(data) EPHYBASE(-1, data)
|
|
||||||
#define Log.critical(data) EPHYBASE(1, data)
|
|
||||||
#define Log.error(data) EPHYBASE(2, data)
|
|
||||||
#define EPHYWARNING(data) EPHYBASE(3, data)
|
|
||||||
#define Log.info(data) EPHYBASE(4, data)
|
|
||||||
#ifdef DEBUG
|
|
||||||
#define Log.debug(data) EPHYBASE(5, data)
|
|
||||||
#define Log.verbose(data) EPHYBASE(6, data)
|
|
||||||
#define EPHYTODO(data) EPHYBASE(4, "TODO : " << data)
|
|
||||||
#else
|
|
||||||
#define Log.debug(data) do { } while(false)
|
|
||||||
#define Log.verbose(data) do { } while(false)
|
|
||||||
#define EPHYTODO(data) do { } while(false)
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#define EPHYASSERT(cond,data) \
|
|
||||||
do { \
|
|
||||||
if (!(cond)) { \
|
|
||||||
Log.critical(data); \
|
|
||||||
assert(!#cond); \
|
|
||||||
} \
|
|
||||||
} while (0)
|
|
||||||
|
|
@ -26,7 +26,7 @@ CollisionWorld::~CollisionWorld() {
|
|||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
CollisionBody* CollisionWorld::createCollisionBody( etk::Transform3D transform) {
|
CollisionBody* CollisionWorld::createCollisionBody( Transform3D transform) {
|
||||||
// Get the next available body ID
|
// Get the next available body ID
|
||||||
long bodyID = computeNextAvailableBodyID();
|
long bodyID = computeNextAvailableBodyID();
|
||||||
// Largest index cannot be used (it is used for invalid index)
|
// Largest index cannot be used (it is used for invalid index)
|
||||||
@ -42,9 +42,9 @@ CollisionBody* CollisionWorld::createCollisionBody( etk::Transform3D transform)
|
|||||||
|
|
||||||
void CollisionWorld::destroyCollisionBody(CollisionBody* collisionBody) {
|
void CollisionWorld::destroyCollisionBody(CollisionBody* collisionBody) {
|
||||||
// Remove all the collision shapes of the body
|
// Remove all the collision shapes of the body
|
||||||
collisionBody->removeAllCollisionShapes();
|
collisionBody.removeAllCollisionShapes();
|
||||||
// Add the body ID to the list of free IDs
|
// Add the body ID to the list of free IDs
|
||||||
this.freeBodiesIDs.pushBack(collisionBody->getID());
|
this.freeBodiesIDs.pushBack(collisionBody.getID());
|
||||||
// Remove the collision body from the list of bodies
|
// Remove the collision body from the list of bodies
|
||||||
this.bodies.erase(this.bodies.find(collisionBody));
|
this.bodies.erase(this.bodies.find(collisionBody));
|
||||||
ETKDELETE(CollisionBody, collisionBody);
|
ETKDELETE(CollisionBody, collisionBody);
|
||||||
@ -66,21 +66,21 @@ long CollisionWorld::computeNextAvailableBodyID() {
|
|||||||
|
|
||||||
void CollisionWorld::resetContactManifoldListsOfBodies() {
|
void CollisionWorld::resetContactManifoldListsOfBodies() {
|
||||||
// For each rigid body of the world
|
// For each rigid body of the world
|
||||||
for (etk::Set<CollisionBody*>::Iterator it = this.bodies.begin(); it != this.bodies.end(); ++it) {
|
for (Set<CollisionBody*>::Iterator it = this.bodies.begin(); it != this.bodies.end(); ++it) {
|
||||||
// Reset the contact manifold list of the body
|
// Reset the contact manifold list of the body
|
||||||
(*it)->resetContactManifoldsList();
|
(*it).resetContactManifoldsList();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
boolean CollisionWorld::testAABBOverlap( CollisionBody* body1, CollisionBody* body2) {
|
boolean CollisionWorld::testAABBOverlap( CollisionBody* body1, CollisionBody* body2) {
|
||||||
// If one of the body is not active, we return no overlap
|
// If one of the body is not active, we return no overlap
|
||||||
if ( !body1->isActive()
|
if ( !body1.isActive()
|
||||||
|| !body2->isActive()) {
|
|| !body2.isActive()) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
// Compute the AABBs of both bodies
|
// Compute the AABBs of both bodies
|
||||||
AABB body1AABB = body1->getAABB();
|
AABB body1AABB = body1.getAABB();
|
||||||
AABB body2AABB = body2->getAABB();
|
AABB body2AABB = body2.getAABB();
|
||||||
// Return true if the two AABBs overlap
|
// Return true if the two AABBs overlap
|
||||||
return body1AABB.testCollision(body2AABB);
|
return body1AABB.testCollision(body2AABB);
|
||||||
}
|
}
|
||||||
@ -89,9 +89,9 @@ void CollisionWorld::testCollision( ProxyShape* shape, CollisionCallback* callba
|
|||||||
// Reset all the contact manifolds lists of each body
|
// Reset all the contact manifolds lists of each body
|
||||||
resetContactManifoldListsOfBodies();
|
resetContactManifoldListsOfBodies();
|
||||||
// Create the sets of shapes
|
// Create the sets of shapes
|
||||||
etk::Set<int> shapes;
|
Set<int> shapes;
|
||||||
shapes.add(shape->this.broadPhaseID);
|
shapes.add(shape.this.broadPhaseID);
|
||||||
etk::Set<int> emptySet;
|
Set<int> emptySet;
|
||||||
// Perform the collision detection and report contacts
|
// Perform the collision detection and report contacts
|
||||||
this.collisionDetection.testCollisionBetweenShapes(callback, shapes, emptySet);
|
this.collisionDetection.testCollisionBetweenShapes(callback, shapes, emptySet);
|
||||||
}
|
}
|
||||||
@ -100,10 +100,10 @@ void CollisionWorld::testCollision( ProxyShape* shape1, ProxyShape* shape2, Col
|
|||||||
// Reset all the contact manifolds lists of each body
|
// Reset all the contact manifolds lists of each body
|
||||||
resetContactManifoldListsOfBodies();
|
resetContactManifoldListsOfBodies();
|
||||||
// Create the sets of shapes
|
// Create the sets of shapes
|
||||||
etk::Set<int> shapes1;
|
Set<int> shapes1;
|
||||||
shapes1.add(shape1->this.broadPhaseID);
|
shapes1.add(shape1.this.broadPhaseID);
|
||||||
etk::Set<int> shapes2;
|
Set<int> shapes2;
|
||||||
shapes2.add(shape2->this.broadPhaseID);
|
shapes2.add(shape2.this.broadPhaseID);
|
||||||
// Perform the collision detection and report contacts
|
// Perform the collision detection and report contacts
|
||||||
this.collisionDetection.testCollisionBetweenShapes(callback, shapes1, shapes2);
|
this.collisionDetection.testCollisionBetweenShapes(callback, shapes1, shapes2);
|
||||||
}
|
}
|
||||||
@ -112,14 +112,14 @@ void CollisionWorld::testCollision( CollisionBody* body, CollisionCallback* call
|
|||||||
// Reset all the contact manifolds lists of each body
|
// Reset all the contact manifolds lists of each body
|
||||||
resetContactManifoldListsOfBodies();
|
resetContactManifoldListsOfBodies();
|
||||||
// Create the sets of shapes
|
// Create the sets of shapes
|
||||||
etk::Set<int> shapes1;
|
Set<int> shapes1;
|
||||||
// For each shape of the body
|
// For each shape of the body
|
||||||
for ( ProxyShape* shape = body->getProxyShapesList();
|
for ( ProxyShape* shape = body.getProxyShapesList();
|
||||||
shape != null;
|
shape != null;
|
||||||
shape = shape->getNext()) {
|
shape = shape.getNext()) {
|
||||||
shapes1.add(shape->this.broadPhaseID);
|
shapes1.add(shape.this.broadPhaseID);
|
||||||
}
|
}
|
||||||
etk::Set<int> emptySet;
|
Set<int> emptySet;
|
||||||
// Perform the collision detection and report contacts
|
// Perform the collision detection and report contacts
|
||||||
this.collisionDetection.testCollisionBetweenShapes(callback, shapes1, emptySet);
|
this.collisionDetection.testCollisionBetweenShapes(callback, shapes1, emptySet);
|
||||||
}
|
}
|
||||||
@ -128,17 +128,17 @@ void CollisionWorld::testCollision( CollisionBody* body1, CollisionBody* body2,
|
|||||||
// Reset all the contact manifolds lists of each body
|
// Reset all the contact manifolds lists of each body
|
||||||
resetContactManifoldListsOfBodies();
|
resetContactManifoldListsOfBodies();
|
||||||
// Create the sets of shapes
|
// Create the sets of shapes
|
||||||
etk::Set<int> shapes1;
|
Set<int> shapes1;
|
||||||
for ( ProxyShape* shape = body1->getProxyShapesList();
|
for ( ProxyShape* shape = body1.getProxyShapesList();
|
||||||
shape != null;
|
shape != null;
|
||||||
shape = shape->getNext()) {
|
shape = shape.getNext()) {
|
||||||
shapes1.add(shape->this.broadPhaseID);
|
shapes1.add(shape.this.broadPhaseID);
|
||||||
}
|
}
|
||||||
etk::Set<int> shapes2;
|
Set<int> shapes2;
|
||||||
for ( ProxyShape* shape = body2->getProxyShapesList();
|
for ( ProxyShape* shape = body2.getProxyShapesList();
|
||||||
shape != null;
|
shape != null;
|
||||||
shape = shape->getNext()) {
|
shape = shape.getNext()) {
|
||||||
shapes2.add(shape->this.broadPhaseID);
|
shapes2.add(shape.this.broadPhaseID);
|
||||||
}
|
}
|
||||||
// Perform the collision detection and report contacts
|
// Perform the collision detection and report contacts
|
||||||
this.collisionDetection.testCollisionBetweenShapes(callback, shapes1, shapes2);
|
this.collisionDetection.testCollisionBetweenShapes(callback, shapes1, shapes2);
|
||||||
@ -147,7 +147,7 @@ void CollisionWorld::testCollision( CollisionBody* body1, CollisionBody* body2,
|
|||||||
void CollisionWorld::testCollision(CollisionCallback* callback) {
|
void CollisionWorld::testCollision(CollisionCallback* callback) {
|
||||||
// Reset all the contact manifolds lists of each body
|
// Reset all the contact manifolds lists of each body
|
||||||
resetContactManifoldListsOfBodies();
|
resetContactManifoldListsOfBodies();
|
||||||
etk::Set<int> emptySet;
|
Set<int> emptySet;
|
||||||
// Perform the collision detection and report contacts
|
// Perform the collision detection and report contacts
|
||||||
this.collisionDetection.testCollisionBetweenShapes(callback, emptySet, emptySet);
|
this.collisionDetection.testCollisionBetweenShapes(callback, emptySet, emptySet);
|
||||||
}
|
}
|
||||||
|
Some files were not shown because too many files have changed in this diff Show More
Loading…
Reference in New Issue
Block a user