Swimming and RigidBody Problem! Big Script but need help with one part!

In my water script, space is to float up, and Q is the float down. I also limit movement speed so it portrays water conditions. The problem is when i float to the surface of the water, if i move a certain way, my characters swims out of the water into the air. So to fix this, i made a variable where if he is at the surface, it locks his y position so he stays on the surface until the player presses Q to go underwater. The problem is that when i press Q again. The y constraint on the rigid body goes haywire and launches me into the air. I dont know why this happens! Please help!

I have a underwater variable and and inwater variable, inWater means i am on the surface of the water.

Scroll down to where it says: if(inWater)

Code (CSharp):

1. using UnityEngine;
2. using UnityEngine.UI;
3. using System.Collections;
4.  
5. public class MovementStamina : MonoBehaviour
6. {
7.     CharacterMotor chMotor;
8.     CharacterController controller;
9.  
10.     bool canJump = false;
11.     float jumpTimer = 1f;
12.  
13.     public Image staminaBar;
14.     GameObject StaminaStamina;
15.     staminaGUI staminaInfo;
16.  
17.     public bool inWater = true;
18.     underwater water;
19.     GameObject fps;
20.     GameObject fps1;
21.     ConstantForce force;
22.     MovementStamina movementstamina;
23.  
24.     private bool locked = false;
25.  
26.    
27.     // Use this for initialization
28.     void Start ()
29.     {
30.         chMotor = this.GetComponent<CharacterMotor> ();
31.         controller = this.GetComponent<CharacterController>();
32.  
33.         StaminaStamina = GameObject.Find ("StaminaStamina");      
34.         staminaInfo = StaminaStamina.GetComponent<staminaGUI> ();
35.  
36.         fps = GameObject.Find ("Main Camera");
37.         water = fps.GetComponent<underwater> ();
38.  
39.         fps1 = GameObject.Find ("First Person Controller");
40.  
41.     }
42.    
43.     // Update is called once per frame
44.     void Update ()
45.     {
46.  
47.         RaycastHit hit;  
48.         Vector3 down = transform.TransformDirection(Vector3.down);
49.  
50.  
51.  
52.     if(!water.isUnderwater )
53.     {
54.             inWater = false;
55.  
56.            
57.             if(Physics.Raycast(transform.position, down, out hit,6))
58.             {
59.                 if(hit.collider.gameObject.tag == "water")
60.                 {
61.    
62.                     inWater = true;
63.                     chMotor.jumping.enabled = false;
64.                    
65.                 }
66.                
67.             }
68.  
69.  
70.         if(controller.velocity.magnitude > 0 && Input.GetKey(KeyCode.LeftShift))
71.         {
72.             chMotor.movement.maxForwardSpeed = 10;
73.             chMotor.movement.maxSidewaysSpeed = 10;
74.             staminaInfo.staminaBar.fillAmount -= 0.001f;
75.  
76.         }
77.        
78.         else
79.         {
80.             chMotor.movement.maxForwardSpeed = 6;
81.             chMotor.movement.maxSidewaysSpeed = 6;
82.         }
83.  
84.  
85.         if (Input.GetKeyDown(KeyCode.Space) && canJump == true)
86.         {
87.             staminaInfo.staminaBar.fillAmount -= 0.09f;
88.             StartCoroutine ("MyMethod");
89.  
90.         }
91.  
92.         if(canJump == false)
93.         {
94.             jumpTimer -= Time.deltaTime;
95.             chMotor.jumping.enabled = false;
96.         }
97.        
98.         if(jumpTimer <= 0)
99.         {
100.  
101.             canJump = true;
102.             chMotor.jumping.enabled = true;
103.             jumpTimer = 1f;
104.  
105.         }
106.         if(staminaInfo.staminaBar.fillAmount < 0.1f)
107.         {
108.             canJump = false;
109.             chMotor.jumping.enabled = false;
110.             chMotor.movement.maxForwardSpeed = 6;
111.             chMotor.movement.maxSidewaysSpeed = 6;
112.         }
113.  
114.     }
115.         else
116.         {
117.             inWater = true;
118.  
119.             //rigidbody.isKinematic = false;
120.             chMotor.movement.gravity = 2;
121.  
122.             chMotor.movement.maxFallSpeed = 4;
123.             chMotor.movement.maxForwardSpeed = 4;
124.             chMotor.movement.maxSidewaysSpeed = 4;
125.             chMotor.movement.maxBackwardsSpeed = 4;
126.  
127.             if(controller.velocity.magnitude > 0 && Input.GetKey(KeyCode.LeftShift))
128.                 {
129.                 chMotor.movement.maxForwardSpeed = 7;
130.                 chMotor.movement.maxSidewaysSpeed = 7;
131.                 staminaInfo.staminaBar.fillAmount -= 0.001f;
132.                 }
133.                 else
134.                 {
135.                     chMotor.movement.maxForwardSpeed = 4;
136.                     chMotor.movement.maxSidewaysSpeed = 4;
137.                 }
138.         }
139.  
140.         if(inWater == true)
141.         {
142.             Debug.Log (locked);
143.             chMotor.jumping.enabled = false;
144.  
145.             if(Input.GetKey("q"))
146.             {
147.                 locked = false;
148.                 rigidbody.constraints &= ~RigidbodyConstraints.FreezePositionY;
149.                 transform.position = new Vector3(transform.position.x, transform.position.y, transform.position.z);
150.                 rigidbody.AddForce(Vector3.down * 5, ForceMode.VelocityChange);
151.                
152.                 if(Physics.Raycast(transform.position, down, out hit,3))
153.                 {
154.                     if(hit.collider.gameObject.tag == "ground")
155.                     {
156.                         rigidbody.isKinematic = true;
157.                     }
158.                    
159.                 }
160.             }
161.             else
162.             {
163.                 rigidbody.isKinematic = false;  
164.             }
165.  
166.        
167.             if(transform.position.y < 45.6)
168.             {
169.                 if(Input.GetKey("space"))
170.                 {
171.                 rigidbody.AddForce(Vector3.up * 5, ForceMode.VelocityChange);
172.                 }
173.             }
174.  
175.             if(!locked && transform.position.y > 45.6)
176.             {
177.                 locked = true;
178.             }
179.  
180.             if(locked)
181.             {
182.                 rigidbody.constraints = RigidbodyConstraints.FreezePositionY;
183.                 transform.position = new Vector3(transform.position.x, 45.6f, transform.position.z);
184.  
185.             }
186.  
187.  
188.  
189.                
190.  
191.         }
192.     }  
193.            
194.  
195.     IEnumerator MyMethod() {
196.  
197.         yield return new WaitForSeconds(.05f);
198.         canJump = false;
199.  
200.         chMotor.jumping.enabled = false;
201.     }
202.    
203. }

 

Too complicated to debug like this.

What you need is a state machine that changes the way that movement is handled entirely based on the current state. The states could be "swimming", "floating", and "walking" in this case. When you're swimming, control is passed off to some WhileSwimming function and the rest doesn't get used at all. When floating (on surface of water), it's all handled by WhileFloating. Using this, it would be much easier to narrow down your issue, and you wouldn't be so tempted to re-use physics for multiple states, each would have their own control and their own responses to those controls, and you could set up ChangeState conditions for switching between one state and another.

 

I see what you are saying thank you! That way will make the code nicer and simpler to read