public static PhysX.Math.Vector3[] OmvVectorArrayToPhysx(OpenMetaverse.Vector3[] omvArray)
{
PhysX.Math.Vector3[] physxArray = new PhysX.Math.Vector3[omvArray.Length];
for (int i = 0; i < omvArray.Length; ++i)
{
OpenMetaverse.Vector3 omvVec = omvArray[i];
physxArray[i] = new PhysX.Math.Vector3(omvVec.X, omvVec.Y, omvVec.Z);
}
return physxArray;
}
public static PhysX.Math.Vector3[] OmvVectorArrayToPhysx(OpenMetaverse.Vector3[] omvArray)
{
PhysX.Math.Vector3[] physxArray = new PhysX.Math.Vector3[omvArray.Length];
for (int i = 0; i < omvArray.Length; ++i)
{
OpenMetaverse.Vector3 omvVec = omvArray[i];
physxArray[i] = new PhysX.Math.Vector3(omvVec.X, omvVec.Y, omvVec.Z);
}
return(physxArray);
}
private void DoAxisLock(float timeStep, uint frameNum)
{
OpenMetaverse.Vector3 lockedaxis;
float gtau;
lock (_properties)
{
lockedaxis = _properties.LockedAxes;
gtau = _properties.GrabTargetTau;
}
// Grab overrides axis lock
if (gtau != 0)
{
return;
}
// Fast bypass: If all axes are unlocked, skip the math.
if (lockedaxis.X == 0 && lockedaxis.Y == 0 && lockedaxis.Z == 0)
{
return;
}
// Convert angular velocity to local.
OpenMetaverse.Vector3 localangvel = PhysUtil.PhysxVectorToOmv(_dynActor.AngularVelocity) * OpenMetaverse.Quaternion.Inverse(_rotation);
// Stop angular velocity on locked local axes (rotaxis.N == 0 means axis is locked)
if (lockedaxis.X != 0)
{
localangvel.X = 0;
}
if (lockedaxis.Y != 0)
{
localangvel.Y = 0;
}
if (lockedaxis.Z != 0)
{
localangvel.Z = 0;
}
// Convert to global angular velocity.
PhysX.Math.Vector3 angvel = PhysUtil.OmvVectorToPhysx(localangvel * _rotation);
// This is a harsh way to do this, but a locked axis must have no angular velocity whatsoever.
if (angvel != _dynActor.AngularVelocity)
{
_dynActor.ClearTorque();
_dynActor.AngularVelocity = angvel;
}
}
private void DoHover(float currentHeightAboveGround, float desiredHoverHeight, float tau, float damping, float timeStep)
{
if (_hoverDisableGravity == false)
{
_hoverDisableGravity = true;
ChangeGravityIfNeeded();
}
//if we got here, we must be outside of the target zone. if there are z forces
//headed in the wrong direction we want to cancel them right away
float diff = desiredHoverHeight - currentHeightAboveGround;
float zforce = _velocity.Z;
// Stay above PhysX NOP limits.
if (Math.Abs(zforce) < MIN_PHYSX_FORCE)
{
zforce = Math.Sign(zforce) * MIN_PHYSX_FORCE;
}
// Apply strong correction if the movement is departing from the target.
PhysX.Math.Vector3 velWithZOnly = new PhysX.Math.Vector3(0f, 0f, zforce);
if (Math.Sign(_velocity.Z) != Math.Sign(diff))
{
if (damping > 0.75f)
{
_dynActor.AddForce(-velWithZOnly, PhysX.ForceMode.VelocityChange, true);
}
else
{
_dynActor.AddForce(-velWithZOnly * _mass * timeStep, PhysX.ForceMode.Impulse, true);
}
}
// Note: the buoyancy check is here to be bug for bug compatible with standard vehicles;
// that is, unless there is some positive buoyancy, hover does not work.
if ((_properties.HoverType & PIDHoverFlag.UpOnly) == 0 || diff > 0 || _properties.Buoyancy > 0)
{
//apply enough force to get to the target in TAU, as well as applying damping
zforce = diff * (1.0f / tau) * _mass * timeStep;
// Stay above PhysX NOP limits.
if (Math.Abs(zforce) < MIN_PHYSX_FORCE)
{
zforce = Math.Sign(zforce) * MIN_PHYSX_FORCE;
}
_dynActor.AddForce(new PhysX.Math.Vector3(0f, 0f, zforce), PhysX.ForceMode.Impulse, true);
DoHoverDamping(damping, timeStep);
}
}
public static PhysX.Math.Vector3[] FloatArrayToVectorArray(float[] floatArray)
{
if (floatArray.Length % 3 != 0)
throw new InvalidOperationException("Float array size must be a multiple of 3 (X,Y,Z)");
PhysX.Math.Vector3[] ret = new PhysX.Math.Vector3[floatArray.Length / 3];
for (int i = 0, j = 0; i < floatArray.Length; i += 3, ++j)
{
ret[j] = new PhysX.Math.Vector3(floatArray[i + 0], floatArray[i + 1], floatArray[i + 2]);
}
return ret;
}
public static PhysX.Math.Vector3[] FloatArrayToVectorArray(float[] floatArray)
{
if (floatArray.Length % 3 != 0)
{
throw new InvalidOperationException("Float array size must be a multiple of 3 (X,Y,Z)");
}
PhysX.Math.Vector3[] ret = new PhysX.Math.Vector3[floatArray.Length / 3];
for (int i = 0, j = 0; i < floatArray.Length; i += 3, ++j)
{
ret[j] = new PhysX.Math.Vector3(floatArray[i + 0], floatArray[i + 1], floatArray[i + 2]);
}
return(ret);
}
public static void Scale(OpenMetaverse.Vector3 scale, HacdConvexHull[] hulls)
{
foreach (HacdConvexHull hull in hulls)
{
PhysX.Math.Vector3[] vertices = hull.Vertices;
for (int i = 0; i < vertices.Length; ++i)
{
PhysX.Math.Vector3 vert = vertices[i];
vert.X *= scale.X;
vert.Y *= scale.Y;
vert.Z *= scale.Z;
vertices[i] = vert;
}
}
}
public Tuple <PhysX.Math.Vector3[], int[]> GenerateTrimeshDataFromHeightmap(float[] heightMap)
{
const int EDGE_SIZE = 2;
const int EDGE_COLS_ROWS_ADDED = EDGE_SIZE * 2;
const int MESH_SIZE = (int)OpenSim.Framework.Constants.RegionSize + EDGE_COLS_ROWS_ADDED;
//build the trimesh points by row and column
PhysX.Math.Vector3[] points = new PhysX.Math.Vector3[MESH_SIZE * MESH_SIZE];
for (int y = 0; y < MESH_SIZE; y++)
{
for (int x = 0; x < MESH_SIZE; x++)
{
points[y * MESH_SIZE + x] = new PhysX.Math.Vector3(x - EDGE_SIZE, y - EDGE_SIZE, GetHeightAtPosition(heightMap, x - EDGE_SIZE, y - EDGE_SIZE));
}
}
//build triangle index
int regionMinusOne = (int)MESH_SIZE - 1;
int[] triangles = new int[regionMinusOne * regionMinusOne * 6];
int arrayPos = 0;
for (int y = 0; y < regionMinusOne; y++)
{
int yColStartCurrent = y * (int)MESH_SIZE;
int yColStartNext = (y + 1) * (int)MESH_SIZE;
for (int x = 0; x < regionMinusOne; x++)
{
//we build both of the triangles of the heightmap square at once
//first triangle
triangles[arrayPos++] = yColStartCurrent + x;
triangles[arrayPos++] = yColStartNext + x + 1;
triangles[arrayPos++] = yColStartNext + x;
//second triangle
triangles[arrayPos++] = yColStartCurrent + x;
triangles[arrayPos++] = yColStartCurrent + x + 1;
triangles[arrayPos++] = yColStartNext + x + 1;
}
}
return(new Tuple <PhysX.Math.Vector3[], int[]>(points, triangles));
}
private void DoHoverDamping(float hoverDamping, float timeStep)
{
if (_velocity.Z != 0)
{
PhysX.Math.Vector3 velWithZOnly = new PhysX.Math.Vector3(0f, 0f, _velocity.Z);
if (Math.Abs(velWithZOnly.Z) <= (MIN_HOVER_DAMPING_VEL_BEFORE_ZEROING * hoverDamping))
{
//_dynActor.LinearVelocity = new PhysX.Math.Vector3(_dynActor.LinearVelocity.X, _dynActor.LinearVelocity.Y, 0.0f) * hoverDamping;
_dynActor.AddForce(-velWithZOnly * hoverDamping, PhysX.ForceMode.VelocityChange, true);
}
else
{
_dynActor.AddForce(-velWithZOnly * _mass * timeStep * HOVER_DAMPING_MULTIPLIER * hoverDamping, PhysX.ForceMode.Impulse, true);
}
}
}
public Tuple<PhysX.Math.Vector3[], int[]> GenerateTrimeshDataFromHeightmap(float[] heightMap)
{
const int EDGE_SIZE = 2;
const int EDGE_COLS_ROWS_ADDED = EDGE_SIZE * 2;
const int MESH_SIZE = (int)OpenSim.Framework.Constants.RegionSize + EDGE_COLS_ROWS_ADDED;
//build the trimesh points by row and column
PhysX.Math.Vector3[] points = new PhysX.Math.Vector3[MESH_SIZE * MESH_SIZE];
for (int y = 0; y < MESH_SIZE; y++)
{
for (int x = 0; x < MESH_SIZE; x++)
{
points[y * MESH_SIZE + x] = new PhysX.Math.Vector3(x - EDGE_SIZE, y - EDGE_SIZE, GetHeightAtPosition(heightMap, x - EDGE_SIZE, y - EDGE_SIZE));
}
}
//build triangle index
int regionMinusOne = (int)MESH_SIZE - 1;
int[] triangles = new int[regionMinusOne * regionMinusOne * 6];
int arrayPos = 0;
for (int y = 0; y < regionMinusOne; y++)
{
int yColStartCurrent = y * (int)MESH_SIZE;
int yColStartNext = (y + 1) * (int)MESH_SIZE;
for (int x = 0; x < regionMinusOne; x++)
{
//we build both of the triangles of the heightmap square at once
//first triangle
triangles[arrayPos++] = yColStartCurrent + x;
triangles[arrayPos++] = yColStartNext + x + 1;
triangles[arrayPos++] = yColStartNext + x;
//second triangle
triangles[arrayPos++] = yColStartCurrent + x;
triangles[arrayPos++] = yColStartCurrent + x + 1;
triangles[arrayPos++] = yColStartNext + x + 1;
}
}
return new Tuple<PhysX.Math.Vector3[], int[]>(points, triangles);
}
private void DoGroundRepel(float currentHeightAboveGround, float desiredHoverHeight, float tau, float timeStep)
{
if (_hoverDisableGravity == false)
{
_hoverDisableGravity = true;
ChangeGravityIfNeeded();
}
//we only want to apply positive z forces for ground repel. popping up above the
//target zone is ok
float diff = desiredHoverHeight - currentHeightAboveGround;
float zforce = _velocity.Z;
// Stay above PhysX NOP limits.
if (Math.Abs(zforce) < MIN_PHYSX_FORCE)
{
zforce = Math.Sign(zforce) * MIN_PHYSX_FORCE;
}
PhysX.Math.Vector3 velWithZOnly = new PhysX.Math.Vector3(0f, 0f, zforce);
if (velWithZOnly.Z < 0.0f)
{
//cancel -Z velocity
_dynActor.AddForce(-velWithZOnly, PhysX.ForceMode.VelocityChange, true);
}
//apply enough force to get to the target in TAU, as well as applying damping
zforce = diff * (1.0f / tau) * _mass * timeStep;
// Stay above PhysX NOP limits.
if (Math.Abs(zforce) < MIN_PHYSX_FORCE)
{
zforce = Math.Sign(zforce) * MIN_PHYSX_FORCE;
}
//apply enough force to get to the target in TAU, as well as applying damping
_dynActor.AddForce(new PhysX.Math.Vector3(0f, 0f, zforce), PhysX.ForceMode.Impulse, true);
DoHoverDamping(1.0f, timeStep);
}
private bool TryGenerateFallbackHullFromHullData(List <PhysX.ConvexMeshGeometry> ret, Exception e, List <OpenMetaverse.Vector3> vertList)
{
PhysX.Math.Vector3[] verts = new PhysX.Math.Vector3[vertList.Count];
for (int i = 0; i < vertList.Count; i++)
{
verts[i] = PhysUtil.OmvVectorToPhysx(vertList[i]);
}
// fall back to basic convex hull
PhysX.ConvexMeshGeometry fallbackHull = this.GenerateBasicConvexHull(null, verts);
if (fallbackHull != null)
{
ret.Add(fallbackHull);
}
else
{
m_log.WarnFormat("[InWorldz.PhysxPhysics] Fallback hull generation failed, giving up", e);
return(false);
}
return(true);
}
public static HacdConvexHull[] CloneHullArray(HacdConvexHull[] hulls)
{
HacdConvexHull[] retHulls = new HacdConvexHull[hulls.Length];
for (int i = 0; i < hulls.Length; ++i)
{
HacdConvexHull nextHull = hulls[i];
HacdConvexHull newHull = new HacdConvexHull();
newHull.Indicies = (int[])nextHull.Indicies.Clone();
newHull.Vertices = new PhysX.Math.Vector3[nextHull.Vertices.Length];
for (int j = 0; j < nextHull.Vertices.Length; j++)
{
PhysX.Math.Vector3 vec = nextHull.Vertices[j];
newHull.Vertices[j] = new PhysX.Math.Vector3(vec.X, vec.Y, vec.Z);
}
retHulls[i] = newHull;
}
return(retHulls);
}
public HacdConvexHull[] DecomposeToConvexHulls(ulong meshHash, bool useCache, List<float3> convVertices, List<int> convIndices)
{
if (convIndices.Count % 3 != 0)
throw new InvalidOperationException("Number of indicies must be divisble by 3");
if (IsCacheCandidate(useCache, convVertices.Count))
{
//try cache
try
{
HacdConvexHull[] cachedHulls;
if (MeshingStage.HullCache.TryGetHulls(meshHash, out cachedHulls))
{
return cachedHulls;
}
}
catch (Exception e)
{
m_log.ErrorFormat("[InWorldz.PhysX.RatcliffACD] Failure retrieving HACD hulls from cache: {0}: {1}", e, e.Message);
}
}
ConvexBuilder builder = new ConvexBuilder(HullReturn);
m_hulls = new List<ConvexResult>();
DecompDesc dcomp = new DecompDesc();
dcomp.mIndices = convIndices;
dcomp.mVertices = convVertices;
builder.process(dcomp);
var retHulls = new HacdConvexHull[m_hulls.Count];
for (int i = 0; i < m_hulls.Count; i++)
{
ConvexResult hull = m_hulls[i];
float[] rawVerts = null;
if (IsCacheCandidate(useCache, convVertices.Count))
{
rawVerts = new float[hull.HullVertices.Count * 3];
}
PhysX.Math.Vector3[] hullVerts = new PhysX.Math.Vector3[hull.HullVertices.Count];
for (int j = 0; j < hull.HullVertices.Count; j++)
{
hullVerts[j] = new PhysX.Math.Vector3(hull.HullVertices[j].x, hull.HullVertices[j].y, hull.HullVertices[j].z);
if (rawVerts != null)
{
rawVerts[j * 3 + 0] = hull.HullVertices[j].x;
rawVerts[j * 3 + 1] = hull.HullVertices[j].y;
rawVerts[j * 3 + 2] = hull.HullVertices[j].z;
}
}
retHulls[i] = new HacdConvexHull {
Indicies = hull.HullIndices.ToArray(),
Vertices = hullVerts,
_rawVerts = rawVerts,
};
}
//store in cache for later
if (IsCacheCandidate(useCache, convVertices.Count))
{
try
{
MeshingStage.HullCache.CacheHulls(meshHash, retHulls);
}
catch (Exception e)
{
m_log.ErrorFormat("[InWorldz.PhysX.RatcliffACD] Failure storing HACD results in cache: {0}: {1}", e, e.Message);
}
}
return retHulls;
}
public static Vector3 AsXNA(this PhysX.Math.Vector3 vector)
{
return(new Vector3(vector.X, vector.Y, vector.Z));
}
public static OpenMetaverse.Vector3 PhysxVectorToOmv(PhysX.Math.Vector3 vec)
{
return(new OpenMetaverse.Vector3(vec.X, vec.Y, vec.Z));
}
public HacdConvexHull[] DecomposeToConvexHulls(ulong meshHash, bool useCache, List <float3> convVertices, List <int> convIndices)
{
if (convIndices.Count % 3 != 0)
{
throw new InvalidOperationException("Number of indicies must be divisble by 3");
}
if (IsCacheCandidate(useCache, convVertices.Count))
{
//try cache
try
{
HacdConvexHull[] cachedHulls;
if (MeshingStage.HullCache.TryGetHulls(meshHash, out cachedHulls))
{
return(cachedHulls);
}
}
catch (Exception e)
{
m_log.ErrorFormat("[InWorldz.PhysX.RatcliffACD] Failure retrieving HACD hulls from cache: {0}: {1}", e, e.Message);
}
}
ConvexBuilder builder = new ConvexBuilder(HullReturn);
m_hulls = new List <ConvexResult>();
DecompDesc dcomp = new DecompDesc();
dcomp.mIndices = convIndices;
dcomp.mVertices = convVertices;
builder.process(dcomp);
var retHulls = new HacdConvexHull[m_hulls.Count];
for (int i = 0; i < m_hulls.Count; i++)
{
ConvexResult hull = m_hulls[i];
float[] rawVerts = null;
if (IsCacheCandidate(useCache, convVertices.Count))
{
rawVerts = new float[hull.HullVertices.Count * 3];
}
PhysX.Math.Vector3[] hullVerts = new PhysX.Math.Vector3[hull.HullVertices.Count];
for (int j = 0; j < hull.HullVertices.Count; j++)
{
hullVerts[j] = new PhysX.Math.Vector3(hull.HullVertices[j].x, hull.HullVertices[j].y, hull.HullVertices[j].z);
if (rawVerts != null)
{
rawVerts[j * 3 + 0] = hull.HullVertices[j].x;
rawVerts[j * 3 + 1] = hull.HullVertices[j].y;
rawVerts[j * 3 + 2] = hull.HullVertices[j].z;
}
}
retHulls[i] = new HacdConvexHull {
Indicies = hull.HullIndices.ToArray(),
Vertices = hullVerts,
_rawVerts = rawVerts,
};
}
//store in cache for later
if (IsCacheCandidate(useCache, convVertices.Count))
{
try
{
MeshingStage.HullCache.CacheHulls(meshHash, retHulls);
}
catch (Exception e)
{
m_log.ErrorFormat("[InWorldz.PhysX.RatcliffACD] Failure storing HACD results in cache: {0}: {1}", e, e.Message);
}
}
return(retHulls);
}
private bool TryGenerateFallbackHullFromHullData(List<PhysX.ConvexMeshGeometry> ret, Exception e, List<OpenMetaverse.Vector3> vertList)
{
PhysX.Math.Vector3[] verts = new PhysX.Math.Vector3[vertList.Count];
for (int i = 0; i < vertList.Count; i++)
{
verts[i] = PhysUtil.OmvVectorToPhysx(vertList[i]);
}
// fall back to basic convex hull
PhysX.ConvexMeshGeometry fallbackHull = this.GenerateBasicConvexHull(null, verts);
if (fallbackHull != null)
{
ret.Add(fallbackHull);
}
else
{
m_log.WarnFormat("[InWorldz.PhysxPhysics] Fallback hull generation failed, giving up", e);
return false;
}
return true;
}
private void AddDepenetrationForce()
{
const float MIN_DEPEN_MULTIPLIER = -10.0f;
const float MAX_DEPEN_MULTIPLIER = 10.0f;
if (IsFreeDynamic)
{
PhysX.Math.Vector3 bb = _dynActor.WorldBounds.Extents * 2.0f;
float fx = (float)_PrimRand.NextDouble() * (bb.X * MAX_DEPEN_MULTIPLIER - bb.X * MIN_DEPEN_MULTIPLIER) + bb.X * MIN_DEPEN_MULTIPLIER;
float fy = (float)_PrimRand.NextDouble() * (bb.Y * MAX_DEPEN_MULTIPLIER - bb.Y * MIN_DEPEN_MULTIPLIER) + bb.Y * MIN_DEPEN_MULTIPLIER;
float fz = (float)_PrimRand.NextDouble() * (bb.Z * MAX_DEPEN_MULTIPLIER - bb.Z) + bb.Z;
PhysX.Math.Vector3 f2 = new PhysX.Math.Vector3(fx,fy,fz);
_dynActor.AddForce(f2, PhysX.ForceMode.VelocityChange, true);
}
}
public static SlimDX.Vector3 AsSlimDX(this PhysX.Math.Vector3 vector3)
{
return(new SlimDX.Vector3(vector3.X, vector3.Y, vector3.Z));
}
