public override void SetGrabSpinVelocity(OpenMetaverse.Vector3 target) { lock (_properties) { if (!_properties.BlockGrab) { _dynActor.AddTorque(PhysUtil.OmvVectorToPhysx(target), PhysX.ForceMode.VelocityChange, true); } } }
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 DoPIDMoveTarget(float timeStep, uint frameNum) { float tau; OpenMetaverse.Vector3 target; float gtau; lock (_properties) { tau = _properties.MoveTargetTau; target = _properties.MoveTarget; gtau = _properties.GrabTargetTau; } // Grab overrides move to target if (gtau != 0) { return; } if (tau > MIN_TAU) { //i had a whole elaborate setup here to do this.. turns out the linden //implementation was much simpler OpenMetaverse.Vector3 distance = target - _position; if (distance.LengthSquared() <= AT_TARGET_TOLERANCE_SQUARED) { if (_velocity != OpenMetaverse.Vector3.Zero) { StopLinearMovement(); return; } } ChangeGravityIfNeeded(); _dynActor.AddForce(PhysUtil.OmvVectorToPhysx((distance * (1.0f / tau)) - _velocity), PhysX.ForceMode.VelocityChange, true); } else // Check for stop move to target - need to turn gravity back on if buoyancy isn't enabled if ((target == OpenMetaverse.Vector3.Zero) && (tau == 0.0f)) { ChangeGravityIfNeeded(); } }
private bool DoGrabTarget(float timeStep, uint frameNum) { float tau; OpenMetaverse.Vector3 target; lock (_properties) { tau = _properties.GrabTargetTau; target = _properties.GrabTarget; } if (tau > MIN_TAU) { _dynActor.AddTorque(PhysUtil.OmvVectorToPhysx(-_angularVelocity * 0.9f), PhysX.ForceMode.VelocityChange, false); OpenMetaverse.Vector3 distance = target - _position; if (distance.LengthSquared() <= AT_TARGET_TOLERANCE_SQUARED) { if (_velocity != OpenMetaverse.Vector3.Zero) { StopLinearMovement(); return(true); } } _grabDisableGravity = true; ChangeGravityIfNeeded(); _dynActor.AddForce(PhysUtil.OmvVectorToPhysx((distance * (1.0f / tau)) - _velocity), PhysX.ForceMode.VelocityChange, true); } else { // Check for stop move to grab - need to turn gravity back on if buoyancy isn't enabled if (tau == 0.0f) { _grabDisableGravity = false; ChangeGravityIfNeeded(); return(false); } } return(true); }
private void StopLinearMovement() { _dynActor.ClearForce(); _dynActor.AddForce(PhysUtil.OmvVectorToPhysx(-_velocity), PhysX.ForceMode.VelocityChange, false); }
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); }