public override void AddRigidBody(RigidBody body)
{
if (!body.IsStaticOrKinematicObject() && 0 == (body.GetFlags() & RigidBodyFlags.BT_DISABLE_WORLD_GRAVITY))
{
body.SetGravity(ref m_gravity);
}
if (body.GetCollisionShape() != null)
{
if (!body.IsStaticObject())
{
m_nonStaticRigidBodies.Add(body);
}
else
{
body.SetActivationState(ActivationState.ISLAND_SLEEPING);
}
bool isDynamic = !(body.IsStaticObject() || body.IsKinematicObject());
CollisionFilterGroups collisionFilterGroup = isDynamic ? CollisionFilterGroups.DefaultFilter : CollisionFilterGroups.StaticFilter;
CollisionFilterGroups collisionFilterMask = isDynamic ? CollisionFilterGroups.AllFilter : (CollisionFilterGroups.AllFilter ^ CollisionFilterGroups.StaticFilter);
AddCollisionObject(body, collisionFilterGroup, collisionFilterMask);
}
}
public BroadphaseProxy(object userData, CollisionFilterGroups collisionFilterGroup, CollisionFilterGroups collisionFilterMask)
: this()
{
_clientObject = userData;
_collisionFilterGroup = collisionFilterGroup;
_collisionFilterMask = collisionFilterMask;
}
public void AddCollisionObject(CollisionObject collisionObject, CollisionFilterGroups fg, CollisionFilterGroups fm)
{
lock (addRemoveLock)
{
m_addList.Add(new ColObjectHolder(collisionObject, fg, fm));
}
}
/// <summary>
/// Since you can't have non-convex shapes (e.g. mesh's) in a compound object, this helper will generate a bunch of individual static components to attach to an entity, with each shape.
/// </summary>
/// <param name="e">Entity to add static components to</param>
/// <param name="shapes">shapes that will generate a static component for each</param>
/// <param name="offsets">optional offset for each</param>
/// <param name="rotations">optional rotation for each</param>
public static void GenerateStaticComponents(Entity e, List <IShape> shapes, List <Vector3> offsets = null, List <Quaternion> rotations = null,
CollisionFilterGroups group = CollisionFilterGroups.DefaultFilter, CollisionFilterGroupFlags collidesWith = CollisionFilterGroupFlags.AllFilter,
float FrictionCoefficient = 0.5f, float MaximumRecoverableVelocity = 2f, SpringSettings?springSettings = null)
{
for (int i = 0; i < shapes.Count; i++)
{
BepuStaticColliderComponent sc = new BepuStaticColliderComponent();
sc.ColliderShape = shapes[i];
if (offsets != null && offsets.Count > i)
{
sc.Position = offsets[i];
}
if (rotations != null && rotations.Count > i)
{
sc.Rotation = rotations[i];
}
sc.CanCollideWith = collidesWith;
sc.CollisionGroup = group;
sc.FrictionCoefficient = FrictionCoefficient;
sc.MaximumRecoveryVelocity = MaximumRecoverableVelocity;
if (springSettings.HasValue)
{
sc.SpringSettings = springSettings.Value;
}
e.Add(sc);
}
}
public BroadphaseProxy(object userData, CollisionFilterGroups collisionFilterGroup, CollisionFilterGroups collisionFilterMask)
: this()
{
_clientObject = userData;
_collisionFilterGroup = collisionFilterGroup;
_collisionFilterMask = collisionFilterMask;
}
/// <summary>
/// Counts how many objects are overlapping with a given ColliderShape. If trackObjects are true, you can
/// use OverlappedWith to check if another PhysicsComponent was part of the overlapping objects
/// </summary>
/// <param name="shape">ColliderShape to check for overlaps with</param>
/// <param name="position">Position to move the ColliderShape, in addition to its LocalOffset</param>
/// <param name="myGroup">What collision group is the ColliderShape in?</param>
/// <param name="overlapsWith">What collision groups does the ColliderShape overlap with?</param>
/// <param name="contactTest">If true, contact test overlapping objects. See ContactResults for output. Defaults to false</param>
/// <param name="stopAfterFirstContact">If contact testing, should we stop contact testing after our first contact was found?</param>
/// <returns>Number of overlapping objects</returns>
public static int PerformOverlapTest(ColliderShape shape, Xenko.Core.Mathematics.Vector3?position = null,
CollisionFilterGroups myGroup = CollisionFilterGroups.DefaultFilter,
CollisionFilterGroupFlags overlapsWith = CollisionFilterGroupFlags.AllFilter,
bool contactTest = false, bool stopAfterFirstContact = false)
{
// doesn't support multithreading
if (mySimulation.simulationLocker != null)
{
throw new InvalidOperationException("Overlap testing not supported with multithreaded physics");
}
if (ghostObject == null)
{
ghostObject = new BulletSharp.PairCachingGhostObject
{
ContactProcessingThreshold = 1e18f,
UserObject = shape
};
ghostObject.CollisionFlags |= BulletSharp.CollisionFlags.NoContactResponse;
internalResults = new OverlapCallback();
NativeOverlappingObjects = new HashSet <object>();
}
ghostObject.CollisionShape = shape.InternalShape;
ghostObject.WorldTransform = Matrix.Transformation(shape.Scaling, shape.LocalRotation, position.HasValue ? position.Value + shape.LocalOffset : shape.LocalOffset);
mySimulation.collisionWorld.AddCollisionObject(ghostObject, (BulletSharp.CollisionFilterGroups)myGroup, (BulletSharp.CollisionFilterGroups)overlapsWith);
int overlapCount = ghostObject.NumOverlappingObjects;
NativeOverlappingObjects.Clear();
for (int i = 0; i < overlapCount; i++)
{
NativeOverlappingObjects.Add(ghostObject.OverlappingPairs[i]);
}
if (contactTest)
{
internalResults.Clear();
internalResults.CollisionFilterGroup = (int)myGroup;
internalResults.CollisionFilterMask = (int)overlapsWith;
foreach (object nativeobj in NativeOverlappingObjects)
{
mySimulation.collisionWorld.ContactPairTest(ghostObject, (BulletSharp.CollisionObject)nativeobj, internalResults);
if (stopAfterFirstContact && internalResults.Contacts.Count > 0)
{
break;
}
}
}
mySimulation.collisionWorld.RemoveCollisionObject(ghostObject);
return(overlapCount);
}
public BroadphaseProxy(ref Vector3 aabbMin, ref Vector3 aabbMax, Object userPtr, CollisionFilterGroups collisionFilterGroup, CollisionFilterGroups collisionFilterMask, Object multiSapParentProxy)
{
m_clientObject = userPtr;
m_collisionFilterGroup = collisionFilterGroup;
m_collisionFilterMask = collisionFilterMask;
m_aabbMin = aabbMin;
m_aabbMax = aabbMax;
m_multiSapParentProxy = multiSapParentProxy;
}
public BulletXNAObject(Game game, string modelAssetName, float mass, DefaultMotionState motionState, CollisionShape collisionShape, Vector3 localInertia, bool ghost, CollisionFilterGroups collisionGroup, CollisionFilterGroups collisionMask)
: base(game, modelAssetName)
{
m_motionState = motionState;
CollisionGroups = collisionGroup;
CollisionMask = collisionMask;
IsGhost = ghost;
SetUpBulletPhysicsBody(mass, motionState, collisionShape, localInertia);
}
/* void* m_userPtr;
short int m_collisionFilterGroup;
short int m_collisionFilterMask;
*/
public MultiSapProxy(ref Vector3 aabbMin, ref Vector3 aabbMax, BroadphaseNativeTypes shapeType,
Object userPtr, CollisionFilterGroups collisionFilterGroup, CollisionFilterGroups collisionFilterMask)
: base(ref aabbMin, ref aabbMax, userPtr, collisionFilterGroup, collisionFilterMask, null)
{
m_aabbMin = aabbMin;
m_aabbMax = aabbMax;
m_shapeType = shapeType;
m_multiSapParentProxy =this;
}
public BroadphaseProxy(ref IndexedVector3 aabbMin, ref IndexedVector3 aabbMax, Object userPtr, CollisionFilterGroups collisionFilterGroup, CollisionFilterGroups collisionFilterMask, Object multiSapParentProxy)
{
m_clientObject = userPtr;
m_collisionFilterGroup = collisionFilterGroup;
m_collisionFilterMask = collisionFilterMask;
m_aabbMin = aabbMin;
m_aabbMax = aabbMax;
m_multiSapParentProxy = multiSapParentProxy;
}
/// <summary>
/// Adds the rigid body to the engine processing pipeline.
/// </summary>
/// <param name="rigidBody">The rigid body.</param>
/// <param name="group">The group.</param>
/// <param name="mask">The mask.</param>
/// <exception cref="System.Exception">Cannot perform this action when the physics engine is set to CollisionsOnly</exception>
public void AddRigidBody(RigidBody rigidBody, CollisionFilterGroups group, CollisionFilterGroups mask)
{
if (discreteDynamicsWorld == null)
{
throw new Exception("Cannot perform this action when the physics engine is set to CollisionsOnly");
}
discreteDynamicsWorld.AddRigidBody(rigidBody.InternalRigidBody, (short)group, (short)mask);
}
public 物理演算を超越した特性(
bool 物理演算の影響を受けないKinematic剛体である = false,
CollisionFilterGroups 自身の衝突グループ番号 = CollisionFilterGroups.DefaultFilter,
CollisionFilterGroups 自身と衝突する他の衝突グループ番号 = CollisionFilterGroups.AllFilter)
{
this.物理演算の影響を受けないKinematic剛体である = 物理演算の影響を受けないKinematic剛体である;
this.自身の衝突グループ番号 = 自身の衝突グループ番号;
this.自身と衝突する他の衝突グループ番号 = 自身と衝突する他の衝突グループ番号;
}
/* void* m_userPtr;
* short int m_collisionFilterGroup;
* short int m_collisionFilterMask;
*/
public MultiSapProxy(ref IndexedVector3 aabbMin, ref IndexedVector3 aabbMax, BroadphaseNativeTypes shapeType,
Object userPtr, CollisionFilterGroups collisionFilterGroup, CollisionFilterGroups collisionFilterMask)
: base(ref aabbMin, ref aabbMax, userPtr, collisionFilterGroup, collisionFilterMask, null)
{
m_aabbMin = aabbMin;
m_aabbMax = aabbMax;
m_shapeType = shapeType;
m_multiSapParentProxy = this;
}
public override void AddRigidBody(RigidBody body, CollisionFilterGroups group, CollisionFilterGroups mask)
{
body.SetGravity(ref m_gravity);
if (body.GetCollisionShape() != null)
{
AddCollisionObject(body, group, mask);
}
}
public BulletPhysicObject(CollisionShape CollisionShape, Vector3 translation, Matrix rotation, Vector3 Scale, float mass, CollisionFilterGroups CollisionFilterGroup = CollisionFilterGroups.DefaultFilter,CollisionFilterGroups collisionFilterMask = CollisionFilterGroups.DefaultFilter)
{
shape = CollisionShape;
this.scale = Scale;
this.mass = mass;
shape.LocalScaling = Scale;
obj = LocalCreateRigidBody(mass, Matrix.CreateTranslation(translation) * rotation, CollisionShape);
}
public virtual BroadphaseProxy CreateProxy(ref Vector3 aabbMin, ref Vector3 aabbMax, BroadphaseNativeTypes shapeType, Object userPtr, CollisionFilterGroups collisionFilterGroup, CollisionFilterGroups collisionFilterMask, IDispatcher dispatcher, Object multiSapProxy)
{
//void* ignoreMe -> we could think of recursive multi-sap, if someone is interested
MultiSapProxy proxy = new MultiSapProxy(ref aabbMin, ref aabbMax,shapeType,userPtr, collisionFilterGroup,collisionFilterMask);
m_multiSapProxies.Add(proxy);
///this should deal with inserting/removal into child broadphases
SetAabb(proxy,ref aabbMin,ref aabbMax,dispatcher);
return proxy;
}
/// <summary>
/// Raycast between the camera and its target. The script assumes the camera is a child entity of its target.
/// </summary>
private void UpdateCameraRaycast()
{
var maxLength = DefaultDistance;
var cameraVector = new Vector3(0, 0, DefaultDistance);
Entity.GetParent().Transform.Rotation.Rotate(ref cameraVector);
if (ConeRadius <= 0)
{
// If the cone radius
var raycastStart = Entity.GetParent().Transform.WorldMatrix.TranslationVector;
var hitResult = this.GetSimulation().Raycast(raycastStart, raycastStart + cameraVector);
if (hitResult.Succeeded)
{
maxLength = Math.Min(DefaultDistance, (raycastStart - hitResult.Point).Length());
}
}
else
{
// If the cone radius is > 0 we will sweep an actual cone and see where it collides
var fromMatrix = Matrix.Translation(0, 0, -DefaultDistance * 0.5f)
* Entity.GetParent().Transform.WorldMatrix;
var toMatrix = Matrix.Translation(0, 0, DefaultDistance * 0.5f)
* Entity.GetParent().Transform.WorldMatrix;
resultsOutput.Clear();
const CollisionFilterGroups cfg = CollisionFilterGroups.DefaultFilter;
const CollisionFilterGroupFlags cfgf = CollisionFilterGroupFlags.DefaultFilter; // Intentionally ignoring the CollisionFilterGroupFlags.StaticFilter; to avoid collision with poles
this.GetSimulation().ShapeSweepPenetrating(coneShape, fromMatrix, toMatrix, resultsOutput, cfg, cfgf);
foreach (var result in resultsOutput)
{
if (result.Succeeded)
{
var signedVector = result.Point - Entity.GetParent().Transform.WorldMatrix.TranslationVector;
var signedDistance = Vector3.Dot(cameraVector, signedVector);
var currentLength = DefaultDistance * result.HitFraction;
if (signedDistance > 0 && currentLength < maxLength)
{
maxLength = currentLength;
}
}
}
}
if (maxLength < MinimumDistance)
{
maxLength = MinimumDistance;
}
Entity.Transform.Position.Z = maxLength;
}
/// <summary>
/// Adds the character to the engine processing pipeline.
/// </summary>
/// <param name="character">The character.</param>
/// <param name="group">The group.</param>
/// <param name="mask">The mask.</param>
/// <exception cref="System.Exception">Cannot perform this action when the physics engine is set to CollisionsOnly</exception>
public void AddCharacter(Character character, CollisionFilterGroups group, CollisionFilterGroups mask)
{
if (discreteDynamicsWorld == null)
{
throw new Exception("Cannot perform this action when the physics engine is set to CollisionsOnly");
}
var collider = character.InternalCollider;
var action = character.KinematicCharacter;
discreteDynamicsWorld.AddCollisionObject(collider, (BulletSharp.CollisionFilterGroups)group, (BulletSharp.CollisionFilterGroups)mask);
discreteDynamicsWorld.AddCharacter(action);
}
internal static RigidBody CreateRigidBody(float mass, OpenTK.Matrix4 transform, CollisionShape shape,
CollisionFilterGroups group = CollisionFilterGroups.DefaultFilter,
CollisionFilterGroups mask = CollisionFilterGroups.AllFilter)
{
if (shape == null)
{
shape = CreateBoxShape(Vector3.Zero);
}
RigidBody body = new RigidBody(shape, mass, transform);
world.AddRigidBody(body.BulletRigidBody, group, mask);
rigidBodies.Add(body);
return(body);
}
/// Perform a ray-hit test with the specified collision model.
public override bool RayHit(ChVector from,
ChVector to,
ChCollisionModel model,
ref ChRayhitResult mresult,
CollisionFilterGroups filter_group,
CollisionFilterGroups filter_mask)
{
IndexedVector3 btfrom = new IndexedVector3((float)from.x, (float)from.y, (float)from.z);
IndexedVector3 btto = new IndexedVector3((float)to.x, (float)to.y, (float)to.z);
BulletXNA.BulletCollision.AllHitsRayResultCallback rayCallback = new AllHitsRayResultCallback(btfrom, btto);
rayCallback.m_collisionFilterGroup = filter_group;
rayCallback.m_collisionFilterMask = filter_mask;
this.bt_collision_world.rayTest(btfrom, btto, rayCallback);
// Find the closest hit result on the specified model (if any)
int hit = -1;
float fraction = 1;
for (int i = 0; i < rayCallback.m_collisionObjects.Count; ++i)
{
if (rayCallback.m_collisionObjects[i].GetUserPointer() == model && rayCallback.m_hitFractions[i] < fraction)
{
hit = i;
fraction = rayCallback.m_hitFractions[i];
}
}
// Ray does not hit specified model
if (hit == -1)
{
mresult.hit = false;
return(false);
}
// Return the closest hit on the specified model
mresult.hit = true;
mresult.hitModel = (ChCollisionModel)(rayCallback.m_collisionObjects[hit].GetUserPointer());
mresult.abs_hitPoint.Set(rayCallback.m_hitPointWorld[hit].X, rayCallback.m_hitPointWorld[hit].Y,
rayCallback.m_hitPointWorld[hit].Z);
mresult.abs_hitNormal.Set(rayCallback.m_hitNormalWorld[hit].X, rayCallback.m_hitNormalWorld[hit].Y,
rayCallback.m_hitNormalWorld[hit].Z);
mresult.abs_hitNormal.Normalize();
mresult.dist_factor = fraction;
mresult.abs_hitPoint = mresult.abs_hitPoint - mresult.abs_hitNormal * mresult.hitModel.GetEnvelope();
return(true);
}
internal static CollisionObject CreateCollisionObject(
CollisionShape shape,
CollisionFilterGroups group = CollisionFilterGroups.DefaultFilter,
CollisionFilterGroups mask = CollisionFilterGroups.AllFilter)
{
if (shape == null)
{
return(null);
}
var o = new CollisionObject(shape);
collisionObjects.Add(o);
world.AddCollisionObject(o.BulletCollisionObject, group, mask);
return(o);
}
public override void AddRigidBody(RigidBody body, CollisionFilterGroups group, CollisionFilterGroups mask)
{
if (!body.IsStaticOrKinematicObject() && 0 == (body.GetFlags() & RigidBodyFlags.BT_DISABLE_WORLD_GRAVITY))
{
body.SetGravity(ref m_gravity);
}
if (body.GetCollisionShape() != null)
{
if (!body.IsStaticObject())
{
if (!m_nonStaticRigidBodies.Contains(body))
{
m_nonStaticRigidBodies.Add(body);
}
}
else
{
body.SetActivationState(ActivationState.ISLAND_SLEEPING);
}
AddCollisionObject(body, group, mask);
}
}
public void BuildCollisionData(Color[] map, int width, int height, Vector3 bottomLeft)
{
Vector2 dim = Vector2.One;
Vector3 scale = new Vector3(dim, 1);
BoxShape collisionBoxShape = new BoxShape(new IndexedVector3(0.5f));
for (int x = 0; x < width; x++)
{
for (int y = 0; y < height; y++)
{
int currentIndex = x + y * width;
if (map[currentIndex] == Color.White)
{
float yOffset = -dim.Y;//0f;// -(dim.Y / 2f);
Vector3 position = new Vector3(x - bottomLeft.X, (bottomLeft.Y - y) + yOffset, bottomLeft.Z);
m_waterLocations.Add(position);
}
if (map[currentIndex] == Color.White)
{
// check the 4 ordinals , if we're surrounded by other solid blocks
// then we don't need a block here.
bool upSet = false;
bool downSet = false;
bool leftSet = false;
bool rightSet = false;
if (x >= 1 && x < width - 1)
{
if (map[currentIndex - 1] == Color.White)
{
leftSet = true;
}
if (map[currentIndex + 1] == Color.White)
{
rightSet = true;
}
}
if (y >= 1 && y < height - 1)
{
if (map[currentIndex - height] == Color.White)
{
upSet = true;
}
if (map[currentIndex + height] == Color.White)
{
downSet = true;
}
}
// if we're not surrounded by blocks then add in.
if (!(upSet && downSet && leftSet && rightSet))
{
Object rigifdBody;
float yOffset = -dim.Y;//0f;// -(dim.Y / 2f);
Vector3 position = new Vector3(x - bottomLeft.X, (bottomLeft.Y - y) + yOffset, bottomLeft.Z);
RigidBodyConstructionInfo constructionInfo = new BulletXNA.BulletDynamics.RigidBodyConstructionInfo(0f, null, (BulletXNA.BulletCollision.CollisionShape)collisionBoxShape);
RigidBody rigidBody = new BulletXNA.BulletDynamics.RigidBody(constructionInfo);
Matrix bsm = Matrix.CreateTranslation(position);
rigidBody.SetWorldTransform(bsm);
// FIXME MAN - setup some collision flags on these bodies...
BulletXNA.BulletCollision.CollisionFilterGroups flags = (BulletXNA.BulletCollision.CollisionFilterGroups)(1 << 8);
BulletXNA.BulletCollision.CollisionFilterGroups mask = (BulletXNA.BulletCollision.CollisionFilterGroups)(1 << 9);
//rigidBody.CollisionFlags |= (BulletSharp.CollisionFlags)CollisionObjectType.Ground;
m_dynamicsWorld.AddRigidBody(rigidBody, flags, mask);
}
}
// Build water ghost objects.
foreach (Vector3 pos in m_waterLocations)
{
GhostObject ghostObject = new GhostObject();
ghostObject.SetCollisionShape((BulletXNA.BulletCollision.CollisionShape)collisionBoxShape);
CollisionFilterGroups flags = (CollisionFilterGroups)(1 << 10);
CollisionFilterGroups mask = (CollisionFilterGroups)(1 << 9);
ghostObject.SetCollisionFlags(CollisionFlags.CF_NO_CONTACT_RESPONSE | CollisionFlags.CF_STATIC_OBJECT); // We can choose to make it "solid" if we want...
ghostObject.SetWorldTransform(BulletXNA.LinearMath.IndexedMatrix.CreateTranslation(pos));
m_dynamicsWorld.AddCollisionObject(ghostObject, flags, mask);
break;
}
}
}
}
ushort AddHandle(ref Vector3 aabbMin, ref Vector3 aabbMax, IntPtr owner, CollisionFilterGroups collisionFilterGroup,
CollisionFilterGroups collisionFilterMask, Dispatcher dispatcher, IntPtr multiSapProxy)
{
return AddHandle(ref aabbMin, ref aabbMax, owner, collisionFilterGroup, collisionFilterMask, dispatcher, multiSapProxy);
}
public virtual BroadphaseProxy CreateProxy(ref Vector3 aabbMin, ref Vector3 aabbMax, BroadphaseNativeTypes shapeType, Object userPtr, CollisionFilterGroups collisionFilterGroup, CollisionFilterGroups collisionFilterMask, IDispatcher dispatcher, Object multiSapProxy)
{
ushort handleId = AddHandle(ref aabbMin,ref aabbMax, userPtr,collisionFilterGroup,collisionFilterMask,dispatcher,multiSapProxy);
Handle handle = GetHandle(handleId);
if (m_raycastAccelerator != null)
{
BroadphaseProxy rayProxy = m_raycastAccelerator.CreateProxy(ref aabbMin,ref aabbMax,shapeType,userPtr,collisionFilterGroup,collisionFilterMask,dispatcher,0);
handle.m_dbvtProxy = rayProxy;
}
return handle;
}
public ushort AddHandle(ref Vector3 aabbMin, ref Vector3 aabbMax, Object pOwner, CollisionFilterGroups collisionFilterGroup, CollisionFilterGroups collisionFilterMask, IDispatcher dispatcher, Object multiSapProxy)
{
// quantize the bounds
ushort[] min = new ushort[3], max = new ushort[3];
Quantize(min, ref aabbMin, 0);
Quantize(max, ref aabbMax, 1);
// allocate a handle
ushort handle = AllocHandle();
Handle pHandle = GetHandle(handle);
pHandle.m_uniqueId = handle;
//pHandle.m_pOverlaps = 0;
pHandle.m_clientObject = pOwner;
pHandle.m_collisionFilterGroup = collisionFilterGroup;
pHandle.m_collisionFilterMask = collisionFilterMask;
pHandle.m_multiSapParentProxy = multiSapProxy;
// compute current limit of edge arrays
ushort limit = (ushort)(m_numHandles * 2);
// insert new edges just inside the max boundary edge
for (int axis = 0; axis < 3; axis++)
{
m_pHandles[0].m_maxEdges[axis] += 2;
m_pEdges[axis,limit + 1].Copy(m_pEdges[axis,limit - 1]);
m_pEdges[axis,limit - 1].m_pos = min[axis];
m_pEdges[axis,limit - 1].m_handle = handle;
m_pEdges[axis,limit].m_pos = max[axis];
m_pEdges[axis,limit].m_handle = handle;
Edge limitEdge = m_pEdges[axis, limit];
Edge limitEdgeMinus = m_pEdges[axis, limit-1];
Edge limitEdgePlus = m_pEdges[axis, limit+1];
pHandle.m_minEdges[axis] = (ushort)(limit - 1);
pHandle.m_maxEdges[axis] = limit;
}
// now sort the new edges to their correct position
SortMinDown(0, pHandle.m_minEdges[0], dispatcher,false);
SortMaxDown(0, pHandle.m_maxEdges[0], dispatcher,false);
SortMinDown(1, pHandle.m_minEdges[1], dispatcher,false);
SortMaxDown(1, pHandle.m_maxEdges[1], dispatcher,false);
SortMinDown(2, pHandle.m_minEdges[2], dispatcher,true);
SortMaxDown(2, pHandle.m_maxEdges[2], dispatcher,true);
return handle;
}
/// <summary>
/// Performs a sweep test using a collider shape and never stops until "to"
/// </summary>
/// <param name="shape">The shape.</param>
/// <param name="from">From.</param>
/// <param name="to">To.</param>
/// <param name="resultsOutput">The list to fill with results.</param>
/// <param name="collisionFilterGroups">The collision group of this shape sweep</param>
/// <param name="collisionFilterGroupFlags">The collision group that this shape sweep can collide with</param>
/// <exception cref="System.Exception">This kind of shape cannot be used for a ShapeSweep.</exception>
public void ShapeSweepPenetrating(ColliderShape shape, Matrix from, Matrix to, IList<HitResult> resultsOutput, CollisionFilterGroups collisionFilterGroups, CollisionFilterGroupFlags collisionFilterGroupFlags)
{
var sh = shape.InternalShape as BulletSharp.ConvexShape;
if (sh == null) throw new Exception("This kind of shape cannot be used for a ShapeSweep.");
using (var rcb = new XenkoAllHitsConvexResultCallback(resultsOutput)
{
CollisionFilterGroup = (BulletSharp.CollisionFilterGroups)collisionFilterGroups,
CollisionFilterMask = (BulletSharp.CollisionFilterGroups)collisionFilterGroupFlags
})
{
collisionWorld.ConvexSweepTest(sh, from, to, rcb);
}
}
public virtual BroadphaseProxy CreateProxy(ref IndexedVector3 aabbMin, ref IndexedVector3 aabbMax, BroadphaseNativeTypes shapeType, Object userPtr, CollisionFilterGroups collisionFilterGroup, CollisionFilterGroups collisionFilterMask, IDispatcher dispatcher, Object multiSapProxy)
{
if (m_numHandles >= m_maxHandles)
{
Debug.Assert(false);
return(null); //should never happen, but don't let the game crash ;-)
}
int position = AllocHandle();
m_pHandles[position] = new SimpleBroadphaseProxy(position, ref aabbMin, ref aabbMax, shapeType, userPtr, collisionFilterGroup, collisionFilterMask, multiSapProxy);;
m_pHandles[position].m_uniqueId = ++m_proxyCounter;
return(m_pHandles[position]);
}
/// <summary>
/// Raycasts penetrating any shape the ray encounters.
/// Filtering by CollisionGroup
/// </summary>
/// <param name="simulation">Physics simulation.</param>
/// <param name="raySegment">Ray.</param>
/// <param name="resultsOutput">The list to fill with results.</param>
/// <param name="collisionFilterGroups">The collision group of this shape sweep</param>
/// <param name="collisionFilterGroupFlags">The collision group that this shape sweep can collide with</param>
/// <exception cref="ArgumentNullException">If the simulation argument is null.</exception>
public static void RaycastPenetrating(this Simulation simulation, RaySegment raySegment, IList <HitResult> resultsOutput, CollisionFilterGroups collisionFilterGroups, CollisionFilterGroupFlags collisionFilterGroupFlags)
{
if (simulation == null)
{
throw new ArgumentNullException(nameof(simulation));
}
simulation.RaycastPenetrating(raySegment.Start, raySegment.End, resultsOutput, collisionFilterGroups, collisionFilterGroupFlags);
}
public void AddCollisionObject(CollisionObject collisionObject, CollisionFilterGroups collisionFilterGroup,
CollisionFilterGroups collisionFilterMask)
{
_collisionObjectArray.Add(collisionObject, (short)collisionFilterGroup, (short)collisionFilterMask);
}
public void Initialize( ref btVector3 aabbMin, ref btVector3 aabbMax
, btCollisionObject userPtr, CollisionFilterGroups collisionFilterGroup
, CollisionFilterGroups collisionFilterMask )
{
//m_uniqueId = 0;
m_clientObject = userPtr;
m_collisionFilterGroup = collisionFilterGroup;
m_collisionFilterMask = collisionFilterMask;
m_aabbMax = aabbMax;
m_aabbMin = aabbMin;
m_multiSapParentProxy = null;
}
public SimpleBroadphaseProxy(int position, ref IndexedVector3 minpt, ref IndexedVector3 maxpt, BroadphaseNativeTypes shapeType, Object userPtr, CollisionFilterGroups collisionFilterGroup, CollisionFilterGroups collisionFilterMask, Object multiSapProxy)
: base(ref minpt, ref maxpt, userPtr, collisionFilterGroup, collisionFilterMask, multiSapProxy)
{
// bit of a cheat/hack - store the position in the array as we can't find it for freehandle otherwise.
m_position = position;
}
public virtual BroadphaseProxy CreateProxy(ref IndexedVector3 aabbMin, ref IndexedVector3 aabbMax, BroadphaseNativeTypes shapeType, Object userPtr, CollisionFilterGroups collisionFilterGroup, CollisionFilterGroups collisionFilterMask, IDispatcher dispatcher, Object multiSapProxy)
{
if (m_numHandles >= m_maxHandles)
{
Debug.Assert(false);
return null; //should never happen, but don't let the game crash ;-)
}
int position = AllocHandle();
m_pHandles[position] = new SimpleBroadphaseProxy(position, ref aabbMin, ref aabbMax, shapeType, userPtr, collisionFilterGroup, collisionFilterMask, multiSapProxy); ;
m_pHandles[position].m_uniqueId = ++m_proxyCounter;
return m_pHandles[position];
}
public abstract void AddRigidBody(RigidBody body, CollisionFilterGroups group, CollisionFilterGroups mask);
/// Perform a ray-hit test with the specified collision model.
public abstract bool RayHit(ChVector from,
ChVector to,
ChCollisionModel model,
ref ChRayhitResult mresult,
CollisionFilterGroups filter_group,
CollisionFilterGroups filter_mask);
public void AddRigidBody(RigidBody body, CollisionFilterGroups group, CollisionFilterGroups mask)
{
_collisionObjectArray.Add(body, (short)group, (short)mask);
}
public virtual void AddRigidBody(RigidBody body, CollisionFilterGroups group, CollisionFilterGroups mask)
{
if (!body.IsStaticOrKinematicObject() && 0 == (body.GetFlags() & RigidBodyFlags.BT_DISABLE_WORLD_GRAVITY))
body.Gravity = m_gravity;
if (body.GetCollisionShape() != null)
{
if (!body.IsStaticObject())
{
m_nonStaticRigidBodies.Add(body);
}
else
{
body.SetActivationState(ActivationState.ISLAND_SLEEPING);
}
AddCollisionObject(body, group, mask);
}
}
/// <summary>
/// Performs a sweep test using a collider shape and never stops until "to"
/// </summary>
/// <param name="shape">The shape.</param>
/// <param name="from">From.</param>
/// <param name="to">To.</param>
/// <param name="collisionFilterGroups">The collision group of this shape sweep</param>
/// <param name="collisionFilterGroupFlags">The collision group that this shape sweep can collide with</param>
/// <returns>The list with hit results.</returns>
/// <exception cref="System.Exception">This kind of shape cannot be used for a ShapeSweep.</exception>
public FastList<HitResult> ShapeSweepPenetrating(ColliderShape shape, Matrix from, Matrix to, CollisionFilterGroups collisionFilterGroups, CollisionFilterGroupFlags collisionFilterGroupFlags)
{
var results = new FastList<HitResult>();
ShapeSweepPenetrating(shape, from, to, results, collisionFilterGroups, collisionFilterGroupFlags);
return results;
}
public virtual BroadphaseProxy CreateProxy(IndexedVector3 aabbMin, IndexedVector3 aabbMax, BroadphaseNativeTypes shapeType, Object userPtr, CollisionFilterGroups collisionFilterGroup, CollisionFilterGroups collisionFilterMask, IDispatcher dispatcher, Object multiSapProxy)
{
return(CreateProxy(ref aabbMin, ref aabbMax, shapeType, userPtr, collisionFilterGroup, collisionFilterMask, dispatcher, multiSapProxy));
}
public ContactResultCallback()
{
m_collisionFilterGroup = CollisionFilterGroups.BDefaultGroup;
m_collisionFilterMask = CollisionFilterGroups.BAllGroup;
}
public SimpleBroadphaseProxy(int position, ref IndexedVector3 minpt, ref IndexedVector3 maxpt, BroadphaseNativeTypes shapeType, Object userPtr, CollisionFilterGroups collisionFilterGroup, CollisionFilterGroups collisionFilterMask, Object multiSapProxy)
: base(ref minpt, ref maxpt, userPtr, collisionFilterGroup, collisionFilterMask, multiSapProxy)
{
// bit of a cheat/hack - store the position in the array as we can't find it for freehandle otherwise.
m_position = position;
}
public void AddMultiBody(MultiBody body, CollisionFilterGroups group, CollisionFilterGroups mask)
{
btMultiBodyDynamicsWorld_addMultiBody3(_native, body._native, (short)group, (short)mask);
_bodies.Add(body);
}
public void AddSoftBody(SoftBody body, CollisionFilterGroups collisionFilterGroup, CollisionFilterGroups collisionFilterMask)
{
body.SoftBodySolver = _softBodySolver;
_collisionObjectArray.Add(body, (short)collisionFilterGroup, (short)collisionFilterMask);
}
public override void AddCollisionObject(CollisionObject collisionObject, CollisionFilterGroups collisionFilterGroup, CollisionFilterGroups collisionFilterMask)
{
base.AddCollisionObject(collisionObject, collisionFilterGroup, collisionFilterMask);
}
//Broadphase Interface
public virtual BroadphaseProxy CreateProxy(Vector3 aabbMin, Vector3 aabbMax, BroadphaseNativeTypes shapeType, Object userPtr, CollisionFilterGroups collisionFilterGroup, CollisionFilterGroups collisionFilterMask, IDispatcher dispatcher, Object multiSapProxy)
{
return CreateProxy(ref aabbMin, ref aabbMax, shapeType, userPtr, collisionFilterGroup, collisionFilterMask, dispatcher, multiSapProxy);
}
public virtual BroadphaseProxy CreateProxy(ref IndexedVector3 aabbMin, ref IndexedVector3 aabbMax, BroadphaseNativeTypes shapeType, Object userPtr, CollisionFilterGroups collisionFilterGroup, CollisionFilterGroups collisionFilterMask, IDispatcher dispatcher, Object multiSapProxy)
{
DbvtProxy proxy = new DbvtProxy(ref aabbMin, ref aabbMax, userPtr, collisionFilterGroup, collisionFilterMask);
DbvtAabbMm aabb = DbvtAabbMm.FromMM(ref aabbMin, ref aabbMax);
//bproxy.aabb = btDbvtAabbMm::FromMM(aabbMin,aabbMax);
proxy.stage = m_stageCurrent;
proxy.m_uniqueId = ++m_gid;
proxy.leaf = m_sets[0].Insert(ref aabb, proxy);
ListAppend(proxy, ref m_stageRoots[m_stageCurrent]);
if (!m_deferedcollide)
{
DbvtTreeCollider collider = new DbvtTreeCollider();
collider.Initialize(this);
collider.proxy = proxy;
Dbvt.CollideTV(m_sets[0].m_root, ref aabb, collider, m_sets[0].CollideTVStack, ref m_sets[0].CollideTVCount);
Dbvt.CollideTV(m_sets[1].m_root, ref aabb, collider, m_sets[1].CollideTVStack, ref m_sets[1].CollideTVCount);
}
return (proxy);
}
/// <summary>
/// Performs a sweep test using a collider shape and stops at the first hit
/// </summary>
/// <param name="shape">The shape.</param>
/// <param name="from">From.</param>
/// <param name="to">To.</param>
/// <param name="collisionFilterGroups">The collision group of this shape sweep</param>
/// <param name="collisionFilterGroupFlags">The collision group that this shape sweep can collide with</param>
/// <returns></returns>
/// <exception cref="System.Exception">This kind of shape cannot be used for a ShapeSweep.</exception>
public HitResult ShapeSweep(ColliderShape shape, Matrix from, Matrix to, CollisionFilterGroups collisionFilterGroups, CollisionFilterGroupFlags collisionFilterGroupFlags)
{
var sh = shape.InternalShape as BulletSharp.ConvexShape;
if (sh == null) throw new Exception("This kind of shape cannot be used for a ShapeSweep.");
var result = new HitResult(); //result.Succeeded is false by default
using (var rcb = new BulletSharp.ClosestConvexResultCallback(from.TranslationVector, to.TranslationVector)
{
CollisionFilterGroup = (BulletSharp.CollisionFilterGroups)collisionFilterGroups,
CollisionFilterMask = (BulletSharp.CollisionFilterGroups)collisionFilterGroupFlags
})
{
collisionWorld.ConvexSweepTest(sh, from, to, rcb);
if (rcb.HitCollisionObject == null) return result;
result.Succeeded = true;
result.Collider = (PhysicsComponent)rcb.HitCollisionObject.UserObject;
result.Normal = rcb.HitNormalWorld;
result.Point = rcb.HitPointWorld;
result.HitFraction = rcb.ClosestHitFraction;
}
return result;
}
/* ctor */
public DbvtProxy(ref IndexedVector3 aabbMin, ref IndexedVector3 aabbMax, Object userPtr, CollisionFilterGroups collisionFilterGroup, CollisionFilterGroups collisionFilterMask) :
base(ref aabbMin, ref aabbMax, userPtr, collisionFilterGroup, collisionFilterMask, null)
{
links[0] = links[1] = null;
}
btBroadphaseProxy( ref btVector3 aabbMin, ref btVector3 aabbMax
, btCollisionObject userPtr, CollisionFilterGroups collisionFilterGroup
, CollisionFilterGroups collisionFilterMask, object multiSapParentProxy )
{
//m_uniqueId = 0;
m_clientObject = userPtr;
m_collisionFilterGroup = collisionFilterGroup;
m_collisionFilterMask = collisionFilterMask;
m_aabbMax = aabbMax;
m_aabbMin = aabbMin;
m_multiSapParentProxy = multiSapParentProxy;
}
/// <summary>
/// Raycasts penetrating any shape the ray encounters.
/// </summary>
/// <param name="simulation">Physics simulation.</param>
/// <param name="raySegment">Ray.</param>
/// <param name="collisionFilterGroups">The collision group of this shape sweep</param>
/// <param name="collisionFilterGroupFlags">The collision group that this shape sweep can collide with</param>
/// <returns>The list with hit results.</returns>
/// <exception cref="ArgumentNullException">If the simulation argument is null.</exception>
public static FastList <HitResult> RaycastPenetrating(this Simulation simulation, RaySegment raySegment, CollisionFilterGroups collisionFilterGroups, CollisionFilterGroupFlags collisionFilterGroupFlags)
{
if (simulation == null)
{
throw new ArgumentNullException(nameof(simulation));
}
var result = new FastList <HitResult>();
simulation.RaycastPenetrating(raySegment.Start, raySegment.End, result, collisionFilterGroups, collisionFilterGroupFlags);
return(result);
}
public void AddSoftBody(SoftBody body, CollisionFilterGroups collisionFilterGroup, CollisionFilterGroups collisionFilterMask)
{
body.SoftBodySolver = _softBodySolver;
_collisionObjectArray.Add(body, (short)collisionFilterGroup, (short)collisionFilterMask);
}
/// <summary>
/// Raycasts and stops at the first hit.
/// </summary>
/// <param name="simulation">Physics simulation.</param>
/// <param name="raySegment">Ray.</param>
/// <param name="collisionFilterGroups">The collision group of this shape sweep</param>
/// <param name="collisionFilterGroupFlags">The collision group that this shape sweep can collide with</param>
/// <returns>The hit results.</returns>
/// <exception cref="ArgumentNullException">If the simulation argument is null.</exception>
public static HitResult Raycast(this Simulation simulation, RaySegment raySegment, CollisionFilterGroups collisionFilterGroups, CollisionFilterGroupFlags collisionFilterGroupFlags)
{
if (simulation == null)
{
throw new ArgumentNullException(nameof(simulation));
}
return(simulation.Raycast(raySegment.Start, raySegment.End, collisionFilterGroups, collisionFilterGroupFlags));
}
protected override void OnInitializePhysics()
{
// collision configuration contains default setup for memory, collision setup
CollisionConf = new DefaultCollisionConfiguration();
Dispatcher = new CollisionDispatcher(CollisionConf);
Broadphase = new DbvtBroadphase();
Solver = new MultiBodyConstraintSolver();
World = new MultiBodyDynamicsWorld(Dispatcher, Broadphase, Solver as MultiBodyConstraintSolver, CollisionConf);
World.Gravity = new Vector3(0, -9.81f, 0);
const bool floating = false;
const bool gyro = false;
const int numLinks = 1;
const bool canSleep = false;
const bool selfCollide = false;
Vector3 linkHalfExtents = new Vector3(0.05f, 0.5f, 0.1f);
//Vector3 baseHalfExtents = new Vector3(0.05f, 0.5f, 0.1f);
Vector3 baseInertiaDiag = Vector3.Zero;
const float baseMass = 0;
multiBody = new MultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep);
//multiBody.UseRK4Integration = true;
//multiBody.BaseWorldTransform = Matrix.Identity;
//init the links
Vector3 hingeJointAxis = new Vector3(1, 0, 0);
//y-axis assumed up
Vector3 parentComToCurrentCom = new Vector3(0, -linkHalfExtents[1], 0);
Vector3 currentPivotToCurrentCom = new Vector3(0, -linkHalfExtents[1], 0);
Vector3 parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom;
for (int i = 0; i < numLinks; i++)
{
const float linkMass = 10;
Vector3 linkInertiaDiag = Vector3.Zero;
using (var shape = new SphereShape(radius))
{
shape.CalculateLocalInertia(linkMass, out linkInertiaDiag);
}
multiBody.SetupRevolute(i, linkMass, linkInertiaDiag, i - 1, Quaternion.Identity,
hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, false);
}
multiBody.FinalizeMultiDof();
(World as MultiBodyDynamicsWorld).AddMultiBody(multiBody);
multiBody.CanSleep = canSleep;
multiBody.HasSelfCollision = selfCollide;
multiBody.UseGyroTerm = gyro;
#if PENDULUM_DAMPING
multiBody.LinearDamping = 0.1f;
multiBody.AngularDamping = 0.9f;
#else
multiBody.LinearDamping = 0;
multiBody.AngularDamping = 0;
#endif
for (int i = 0; i < numLinks; i++)
{
var shape = new SphereShape(radius);
CollisionShapes.Add(shape);
var col = new MultiBodyLinkCollider(multiBody, i);
col.CollisionShape = shape;
//const bool isDynamic = true;
CollisionFilterGroups collisionFilterGroup = CollisionFilterGroups.DefaultFilter; // : CollisionFilterGroups.StaticFilter;
CollisionFilterGroups collisionFilterMask = CollisionFilterGroups.AllFilter; // : CollisionFilterGroups.AllFilter & ~CollisionFilterGroups.StaticFilter;
World.AddCollisionObject(col, collisionFilterGroup, collisionFilterMask);
multiBody.GetLink(i).Collider = col;
}
}
public SimpleBroadphaseProxy(Vector3 minPoint, Vector3 maxPoint, BroadphaseNativeTypes shapeType, object userData, CollisionFilterGroups collisionFilterGroup, CollisionFilterGroups collisionFilterMask)
: base(userData, collisionFilterGroup, collisionFilterMask)
{
_min = minPoint;
_max = maxPoint;
}
public virtual void AddCollisionObject(CollisionObject collisionObject, CollisionFilterGroups collisionFilterGroup, CollisionFilterGroups collisionFilterMask)
{
//check that the object isn't already added
//btAssert( m_collisionObjects.findLinearSearch(collisionObject) == m_collisionObjects.size());
Debug.Assert(collisionObject != null);
//Debug.Assert(!m_collisionObjects.Contains(collisionObject));
if (m_collisionObjects.Contains(collisionObject))
{
return;
}
m_collisionObjects.Add(collisionObject);
//calculate new AABB
IndexedMatrix trans = collisionObject.GetWorldTransform();
IndexedVector3 minAabb;
IndexedVector3 maxAabb;
collisionObject.GetCollisionShape().GetAabb(ref trans, out minAabb, out maxAabb);
BroadphaseNativeTypes type = collisionObject.GetCollisionShape().GetShapeType();
collisionObject.SetBroadphaseHandle(GetBroadphase().CreateProxy(
ref minAabb,
ref maxAabb,
type,
collisionObject,
collisionFilterGroup,
collisionFilterMask,
m_dispatcher1, 0
));
}
public void AddRigidBody(RigidBody body, CollisionFilterGroups group, CollisionFilterGroups mask)
{
_collisionObjectArray.Add(body, (short)group, (short)mask);
}
public void AddMultiBody(MultiBody body, CollisionFilterGroups group, CollisionFilterGroups mask)
{
btMultiBodyDynamicsWorld_addMultiBody3(_native, body._native, (short)group,
(short)mask);
_bodies.Add(body);
}
public override void AddRigidBody(RigidBody body, CollisionFilterGroups group, CollisionFilterGroups mask)
{
body.SetGravity(ref m_gravity);
if (body.GetCollisionShape() != null)
{
AddCollisionObject(body, group, mask);
}
}
ushort AddHandle(ref Vector3 aabbMin, ref Vector3 aabbMax, IntPtr owner, CollisionFilterGroups collisionFilterGroup,
CollisionFilterGroups collisionFilterMask, Dispatcher dispatcher, IntPtr multiSapProxy)
{
return(AddHandle(ref aabbMin, ref aabbMax, owner, collisionFilterGroup, collisionFilterMask, dispatcher, multiSapProxy));
}
public ContactResultCallback()
{
m_collisionFilterGroup = CollisionFilterGroups.DefaultFilter;
m_collisionFilterMask = CollisionFilterGroups.AllFilter;
}
