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 GetRayCastResults(OpenMetaverse.Vector3 start, OpenMetaverse.Vector3 direction,
float distance, int hitAmounts, Action <List <ContactResult> > result, PhysxScene scene)
{
int buffercount = 16;
int maxbuffercount = 1024;
PhysX.RaycastHit[] hits = null;
direction = OpenMetaverse.Vector3.Normalize(direction);
//Increase the buffer count if the call indicates overflow. Prevent infinite loops.
while (hits == null && buffercount <= maxbuffercount)
{
hits = SceneImpl.RaycastMultiple(PhysUtil.OmvVectorToPhysx(start),
PhysUtil.OmvVectorToPhysx(direction),
distance, PhysX.SceneQueryFlags.All,
buffercount,
null);
buffercount *= 2;
}
List <ContactResult> contactResults = new List <ContactResult>();
if (hits != null)
{
List <PhysX.RaycastHit> hitsSorted = new List <PhysX.RaycastHit>(hits);
hitsSorted.Sort((a, b) => a.Distance.CompareTo(b.Distance));
int count = 0;
foreach (PhysX.RaycastHit hit in hitsSorted)
{
contactResults.Add(new ContactResult()
{
Distance = hit.Distance,
FaceIndex = hit.FaceIndex,
CollisionActor = hit.Shape.Actor.UserData as PhysicsActor,
Position = PhysUtil.PhysxVectorToOmv(hit.Impact),
Normal = PhysUtil.PhysxVectorToOmv(hit.Normal),
});
if (++count >= hitAmounts)
{
break;
}
}
}
result(contactResults);
}
private void DoRotLookTarget(float timeStep, uint frameNum)
{
float strength;
float damping;
OpenMetaverse.Quaternion target;
float gtau;
lock (_properties)
{
strength = _properties.RotLookStrength;
damping = _properties.RotLookDamping;
target = _properties.RotLookTarget;
gtau = _properties.GrabTargetTau;
}
// Grab overrides rot target
if (gtau != 0)
{
return;
}
if (damping < MIN_ROTLOOK_DAMPING || strength < MIN_ROTLOOK_STRENGTH)
{
return;
}
OpenMetaverse.Quaternion normRot = OpenMetaverse.Quaternion.Normalize(_rotation);
//this is the first frame we're beginning a new rotlook force
//if we're not already at the target rotation, we need to find
//the difference between our current rotation and the desired
//rotation, then use the damping number as a tau to set the
//angular velocity
if (normRot.ApproxEquals(target, AT_ROT_TOLERANCE))
{
//nothing to do
if (_dynActor.AngularVelocity != PhysX.Math.Vector3.Zero)
{
_dynActor.AngularVelocity = PhysX.Math.Vector3.Zero;
}
return;
}
/*
* rotation velocity is vector that corresponds to axis of rotation and have length equal to rotation speed,
* for example in radians per second. It is sort of axis-angle thing.
* Let's first find quaternion q so q*q0=q1 it is q=q1/q0
*
* For unit length quaternions, you can use q=q1*Conj(q0)
*
* To find rotation velocity that turns by q during time Dt you need to convert quaternion to axis angle using something like this:
*
* double len=sqrt(q.x*q.x+q.y*q.y+q.z*q.z)
* double angle=2*atan2(len, q.w);
* vector3 axis;
* if(len>0)axis=q.xyz()/len; else axis=vector3(1,0,0);
*
* then rotation velocity w=axis*angle/dt
*
* For q1,q2 very close to eachother, xyz part of 2*q1*Conj(q0) is roughly equal to rotation velocity. (because for small angles sin(a)~=a)
*
* http://www.gamedev.net/topic/347752-quaternion-and-angular-velocity/
*/
OpenMetaverse.Quaternion q = target * OpenMetaverse.Quaternion.Conjugate(normRot);
double len = Math.Sqrt(q.X * q.X + q.Y * q.Y + q.Z * q.Z);
double angle = 2 * Math.Atan2(len, q.W);
OpenMetaverse.Vector3 axis;
if (len > 0)
{
axis.X = (float)(q.X / len);
axis.Y = (float)(q.Y / len);
axis.Z = (float)(q.Z / len);
}
else
{
axis = new OpenMetaverse.Vector3(1, 0, 0);
}
float fangle = ShortestAngle((float)angle);
OpenMetaverse.Vector3 targetAngVel = ((axis * fangle) / damping) * ROT_LOOKAT_SCALING_CONSTANT;
OpenMetaverse.Quaternion comRot = _centerOfMassLocalPose.Rotation;
OpenMetaverse.Quaternion adjRot = _rotation * comRot;
OpenMetaverse.Vector3 tensor = _massSpaceInertiaTensor;
OpenMetaverse.Vector3 t = (tensor * (targetAngVel * OpenMetaverse.Quaternion.Inverse(adjRot))) * adjRot;
OpenMetaverse.Vector3 currAngVel = PhysUtil.PhysxVectorToOmv(_dynActor.AngularVelocity);
OpenMetaverse.Vector3 velDamping = (tensor * (-currAngVel * strength * ROT_LOOKAT_SCALING_CONSTANT * OpenMetaverse.Quaternion.Inverse(adjRot))) * adjRot;
_dynActor.AddTorque(PhysUtil.OmvVectorToPhysx(t * timeStep), PhysX.ForceMode.Impulse, true);
_dynActor.AddTorque(PhysUtil.OmvVectorToPhysx(velDamping * timeStep), PhysX.ForceMode.Impulse, true);
//m_log.DebugFormat("New Ang Vel: {0}", targetAngVel);
}