private void SynchronizeNativeFramesWithAttachmentPair() { // NOTE: It's not possible to update the constraint frames given the current // transforms since the actual constraint direction will change with the // violation. //RigidBody rb1 = AttachmentPair.ReferenceObject.GetComponentInParent<RigidBody>(); //if ( rb1 == null ) // return; // //agx.Frame f1 = Native.getAttachment( 0 ).getFrame(); //f1.setLocalTranslate( AttachmentPair.ReferenceFrame.CalculateLocalPosition( rb1.gameObject ).ToHandedVec3() ); //f1.setLocalRotate( AttachmentPair.ReferenceFrame.CalculateLocalRotation( rb1.gameObject ).ToHandedQuat() ); if (ConnectedFrameNativeSyncEnabled) { RigidBody rb2 = AttachmentPair.ConnectedObject != null?AttachmentPair.ConnectedObject.GetComponentInParent <RigidBody>() : null; agx.Frame f2 = Native.getAttachment(1).getFrame(); if (rb2 != null) { f2.setLocalTranslate(AttachmentPair.ConnectedFrame.CalculateLocalPosition(rb2.gameObject).ToHandedVec3()); f2.setLocalRotate(AttachmentPair.ConnectedFrame.CalculateLocalRotation(rb2.gameObject).ToHandedQuat()); } else { f2.setLocalTranslate(AttachmentPair.ConnectedFrame.Position.ToHandedVec3()); f2.setLocalRotate(AttachmentPair.ConnectedFrame.Rotation.ToHandedQuat()); } } }
private void Gripper(bool grip) { if (GrippingBody == null || FollowBody == null || waitForKeyUp) { return; } if (grip) { GrippedBody.SyncNativeTransform(); GrippingBody.SyncNativeTransform(); // Is this needed...? if (Vector3.Distance(GrippedBody.transform.position, GrippingBody.transform.position) > MaxGripDistance) { Debug.Log("Couldn't reach to grip: " + (GrippedBody.transform.position - GrippingBody.transform.position).magnitude + ", pos1: " + GrippedBody.transform.position + ", pos2: " + GrippingBody.transform.position); return; } if (lockJoint == null) { previouslyClose = false; agx.Frame grippedFrame = new agx.Frame(); agx.Frame grippingFrame = new agx.Frame(); Vector3 offset = GrippingBody.transform.rotation * grippingPositionOffset; grippedFrame.setLocalTranslate(GrippedBody.Native.getCmLocalTranslate()); grippingFrame.setLocalTranslate(new agx.Vec3(offset.x, offset.y, offset.z)); grippedFrame.setLocalRotate(new agx.EulerAngles(grippingRotationOffset.x * Mathf.Deg2Rad, grippingRotationOffset.y * Mathf.Deg2Rad, grippingRotationOffset.z * Mathf.Deg2Rad)); lockJoint = new agx.LockJoint(GrippedBody.Native, grippedFrame, GrippingBody.Native, grippingFrame); lockJoint.setCompliance(0.00001f); lockJoint.setForceRange(new agx.RangeReal(-200, 200)); lockJoint.setDamping(1); GetSimulation().add(lockJoint); } } else { if (lockJoint != null) { lockJoint.setEnable(false); lockJoint = null; VeryCloseEvent(false); } } gripped = !gripped; }
public TemporaryNative(Type nativeType, AttachmentPair attachmentPair = null) { m_rb1 = new agx.RigidBody(); m_f1 = new agx.Frame(); m_f2 = new agx.Frame(); if (attachmentPair != null) { // Some constraints, e.g., Distance Joints depends on the constraint angle during // creation so we feed the frames with the world transform of the reference and // connecting frame. attachmentPair.Synchronize(); m_f1.setLocalTranslate(attachmentPair.ReferenceFrame.Position.ToHandedVec3()); m_f1.setLocalRotate(attachmentPair.ReferenceFrame.Rotation.ToHandedQuat()); m_f2.setLocalTranslate(attachmentPair.ConnectedFrame.Position.ToHandedVec3()); m_f2.setLocalRotate(attachmentPair.ConnectedFrame.Rotation.ToHandedQuat()); } m_native = (agx.Constraint)Activator.CreateInstance(nativeType, new object[] { m_rb1, m_f1, null, m_f2 }); }
/// <summary> /// Creates native instance and adds it to the simulation if this constraint /// is properly configured. /// </summary> /// <returns>True if successful.</returns> protected override bool Initialize() { if (AttachmentPair.ReferenceObject == null) { Debug.LogError("Unable to initialize constraint - reference object must be valid and contain a rigid body component.", this); return(false); } // Synchronize frames to make sure connected frame is up to date. AttachmentPair.Synchronize(); // TODO: Disabling rigid body game object (activeSelf == false) and will not be // able to create native body (since State == Constructed and not Awake). // Do: GetComponentInParent<RigidBody>( true <- include inactive ) and wait // for the body to become active? // E.g., rb.AwaitInitialize += ThisConstraintInitialize. RigidBody rb1 = AttachmentPair.ReferenceObject.GetInitializedComponentInParent <RigidBody>(); if (rb1 == null) { Debug.LogError("Unable to initialize constraint - reference object must contain a rigid body component.", AttachmentPair.ReferenceObject); return(false); } // Native constraint frames. agx.Frame f1 = new agx.Frame(); agx.Frame f2 = new agx.Frame(); // Note that the native constraint want 'f1' given in rigid body frame, and that // 'ReferenceFrame' may be relative to any object in the children of the body. f1.setLocalTranslate(AttachmentPair.ReferenceFrame.CalculateLocalPosition(rb1.gameObject).ToHandedVec3()); f1.setLocalRotate(AttachmentPair.ReferenceFrame.CalculateLocalRotation(rb1.gameObject).ToHandedQuat()); RigidBody rb2 = AttachmentPair.ConnectedObject != null?AttachmentPair.ConnectedObject.GetInitializedComponentInParent <RigidBody>() : null; if (rb1 == rb2) { Debug.LogError("Unable to initialize constraint - reference and connected rigid body is the same instance.", this); return(false); } if (rb2 != null) { // Note that the native constraint want 'f2' given in rigid body frame, and that // 'ReferenceFrame' may be relative to any object in the children of the body. f2.setLocalTranslate(AttachmentPair.ConnectedFrame.CalculateLocalPosition(rb2.gameObject).ToHandedVec3()); f2.setLocalRotate(AttachmentPair.ConnectedFrame.CalculateLocalRotation(rb2.gameObject).ToHandedQuat()); } else { f2.setLocalTranslate(AttachmentPair.ConnectedFrame.Position.ToHandedVec3()); f2.setLocalRotate(AttachmentPair.ConnectedFrame.Rotation.ToHandedQuat()); } try { Native = (agx.Constraint)Activator.CreateInstance(NativeType, new object[] { rb1.Native, f1, (rb2 != null ? rb2.Native : null), f2 }); // Assigning native elementary constraints to our elementary constraint instances. foreach (ElementaryConstraint ec in ElementaryConstraints) { if (!ec.OnConstraintInitialize(this)) { throw new Exception("Unable to initialize elementary constraint: " + ec.NativeName + " (not present in native constraint). ConstraintType: " + Type); } } bool added = GetSimulation().add(Native); Native.setEnable(IsEnabled); // Not possible to handle collisions if connected frame parent is null/world. if (CollisionsState != ECollisionsState.KeepExternalState && AttachmentPair.ConnectedObject != null) { string groupName = gameObject.name + "_" + gameObject.GetInstanceID().ToString(); GameObject go1 = null; GameObject go2 = null; bool propagateToChildren1 = false; bool propagateToChildren2 = false; if (CollisionsState == ECollisionsState.DisableReferenceVsConnected) { go1 = AttachmentPair.ReferenceObject; go2 = AttachmentPair.ConnectedObject; } else { go1 = rb1.gameObject; propagateToChildren1 = true; go2 = rb2 != null ? rb2.gameObject : AttachmentPair.ConnectedObject; propagateToChildren2 = true; } go1.GetOrCreateComponent <CollisionGroups>().GetInitialized <CollisionGroups>().AddGroup(groupName, propagateToChildren1); go2.GetOrCreateComponent <CollisionGroups>().GetInitialized <CollisionGroups>().AddGroup(groupName, propagateToChildren2); CollisionGroupsManager.Instance.GetInitialized <CollisionGroupsManager>().SetEnablePair(groupName, groupName, false); } Native.setName(name); bool valid = added && Native.getValid(); Simulation.Instance.StepCallbacks.PreSynchronizeTransforms += OnPreStepForwardUpdate; // It's not possible to check which properties an animator // is controlling, for now we update all properties in the // controllers if we have an animator. m_isAnimated = GetComponent <Animator>() != null; return(valid); } catch (System.Exception e) { Debug.LogException(e, gameObject); return(false); } }